4.1 这个完，全得完！——陀螺仪、加速度计

4.1 这个完，全得完！——陀螺仪、加速度计

4.1.1 ICM42688简介

ICM42688是一款六轴姿态传感器（三轴陀螺仪+三轴加速度计），相比于其它常见的MPU6050、ICM20602，其具有更高的精度、刷新频率以及更小的零漂，适合于无人机复杂飞行的要求。

（1）ICM42688参数

ICM42688支持I2C与SPI通信，其重要的参数如下：

参数	参数值
I2C最大通信频率	1 MHz
SPI最大通信频率	24MHz
VDD与VDDIO供电	1.7~3.6V（其中VDDIO表示与这通信的单片机IO口的电压 ）
额定电流	0.88mA
陀螺仪噪声	2.8 mdps/√Hz
加速度计噪声	70 µg/√Hz
使用温度	−40°C to +85°C
陀螺仪满量程（dps，度/秒）	± 15.6/31.2/62.5/125/250/500/1000/2000
加速度计满量程（g）	± 2/4/8/16
中断引脚	可通过寄存器配置数据更新时其电平变化
CS引脚	低电平允许SPI通信使能

这里关于陀螺仪噪声与加速度计噪声的单位“/√Hz”是一个功率谱密度相关的单位，是与传感器精度有关的参数，越小精度越高。比如ICM20602的陀螺仪噪声为4 mdps/√Hz，MPU6050的是5 mdps/√Hz，QMI8658A是13 mdps/√Hz，都比ICM42688的精度低

详细参考：https://www.zhihu.com/question/320561241/answer/2573062407

（2）ICM42688使用

单片机可以通过SPI与ICM42688进行通信，但更具体的数据交换方式其实是间片机操作ICM42688内部的寄存器，进而实现设置ICM42688量程等参数、读取角速度等数据。其实不只是六轴芯片，其他类型的传感器芯片（如气压计DPS310、磁力计HMC5883等）、Flash芯片，反正只要使用I2C、SPI等通信的，基本也都遵循类似的逻辑。

因为种种芯片终归与单片机寄存器有着类似的结构，而单片机内部与和寄存器连接的APB、AHB总线等，估摸着也是使用与SPI什么类似的通信（只是猜测）。说不定之后便会将六轴、气压计内置到单片机中，于是间片机手册中除了SPI、UART寄存器外又多了GYRO、ACC、BARO、MAG、图传寄存器，便叫作“AT32专业级AIO飞行控制单片机”！

ICM42688寄存器的分布，可以在“4、硬件资料\芯片数据手册\C1850418_姿态传感器-陀螺仪_ICM-42688-P_规格书_TDK+INVENSENSE(应美盛)姿态传感器_陀螺仪规格书.PDF”
中查看，第60页的13小节“13 Register Map”展示了所有寄存器的列表，总共有4个寄存器组，Bank 0 到 Bank 4（很奇怪没有Bank 3），第4页的目录如下图：

图4.1.1.1

这里我们只用到了Bank 0下面的寄存器。

（3）ICM42688寄存器

这里列出之后将使用到的ICM42688相关的寄存器。

• 14.58 WHO_AM_I（我是谁寄存器）
  Reset value: 0x47

  • 7:0 WHOAMI
    保存有设备ID值，默认是0x47

• 14.59 REG_BANK_SEL（寄存器组选择寄存器）

  • 2:0 BANK_SEL
    寄存器组选择
    000: Bank 0 (default)
    001: Bank 1
    010: Bank 2
    011: Bank 3
    100: Bank 4
    101: Reserved
    110: Reserved
    111: Reserved

    第次上电后都可以通过这个寄存器选择要操作的寄存器组，因为四个寄存器组中各寄存器的地址是重复的（范围0~127）。

• 14.1 DEVICE_CONFIG（设备设置寄存器）

  • 0 SOFT_RESET_CONFIG
    软件 复位设置。
    这个位用于软件复位，写入1后ICM42688会进入复位状态，复位后需要等待至少1ms才能对ICM42688的其他寄存器进行操作。

• 15.17 INTF_CONFIG4（连接设置4寄存器）

  • 1 SPI_AP_4WIRE
    0: 使用3线SPI模式
    1: 使用4线SPI模式 (default)
    这是一个属于寄存器组1（Bank 1）的寄存器

• 14.37 GYRO_CONFIG0
  陀螺仪设置寄存器

  • 位7:5 GYRO_FS_SEL
    陀螺仪满量程选择，单位dps（即°/s，第秒转动的角度）
    000: ±2000dps (default)
    001: ±1000dps
    010: ±500dps
    011: ±250dps
    100: ±125dps
    101: ±62.5dps
    110: ±31.25dps
    111: ±15.625dps
    由于接收到的每个轴的陀螺仪数据是int16类型的，极限值是±32768（准确来说是-32769与32768），所以如果想将原始数据转化为标准单位的值，需要除以 （32768/|满量程|），即有公式：

  标准值 = 原始值/(32768/|满量程|)  
  
  如GYRO_FS_SEL设置为001，即 满量程=±1000dps  
  
  则：标准值 = 原始值/(32768/1000)  
  或：标准值 = 原始值*(1000/32768)
  

  • 位3:0 GYRO_ODR
    陀螺仪输出速率选择
    0000: Reserved
    0001: 32kHz
    0010: 16kHz
    0011: 8kHz
    0100: 4kHz
    0101: 2kHz
    0110: 1kHz (default)
    0111: 200Hz
    1000: 100Hz
    1001: 50Hz
    1010: 25Hz
    1011: 12.5Hz
    1100: Reserved
    1101: Reserved
    1110: Reserved
    1111: 500Hz

• 14.38 ACCEL_CONFIG0
  加速度计设置寄存器

  • 位7:5 ACCEL_FS_SEL
    满量程设置
    000: ±16g (default)
    001: ±8g
    010: ±4g
    011: ±2g
    100: Reserved
    101: Reserved
    110: Reserved
    111: Reserved
  • 位3:0 ACCEL_ODR
    输出速率设置
    0000: Reserved
    0001: 32kHz (LN mode)
    0010: 16kHz (LN mode)
    0011: 8kHz (LN mode)
    0100: 4kHz (LN mode)
    0101: 2kHz (LN mode)
    0110: 1kHz (LN mode) (default)
    0111: 200Hz (LP or LN mode)
    1000: 100Hz (LP or LN mode)
    1001: 50Hz (LP or LN mode)
    1010: 25Hz (LP or LN mode)
    1011: 12.5Hz (LP or LN mode)
    1100: 6.25Hz (LP mode)
    1101: 3.125Hz (LP mode)
    1110: 1.5625Hz (LP mode)
    1111: 500Hz (LP or LN mode)
    这里的“LN mode”指低噪声模式，在下面的寄存器中将提到

• 14.36 PWR_MGMT0（电源管理寄存器0）

  • 5 TEMP_DIS
    0: 使能温度传感器 (default)
    1: 失能温度传感器
  • 3:2 GYRO_MODE
    陀螺仪模式设置位
    00: 关闭模式 (default)
    01: 待机模式
    10: Reserved
    11: 低噪声模式 (LN, Low Noise)
  • 1:0 ACCEL_MODE
    加速度计模式设置位
    00: 关闭模式 (default)
    01: 关闭模式
    10: 低功耗模式 (LP)
    11: 低噪声模式 (LN, Low Noise)

这里将陀螺仪、加速度计都设置为低噪声模式（LN），设置后要延时最少200us等待设置完成

4.1.2 SPI简介

（1）SPI引脚

SPI相信在学习stm32的时候都已经了解过，这里简单回顾一下。

• CS
  从设备片选使能引脚，如果从设备是低电平使能，当拉低这个引脚后从设备就会被选中，主机和这个被选中的从机进行通信

• SCLK
  时钟引脚，由主机产生

• MOSI
  主机给从机发送指令或者数据的通道

• MISO
  主机读取从机的状态或者数据的通道

  另有3线与2线形式的SPI，3线就是MOSI、MISO合在一起了，但是只能半双工通信，2线就是再去掉片选引脚。

（2）SPI参数

设置SPI通信时有两个很重要的参数，CPOL与CPHA

• CPOL（Clock Polarity，即时钟极性）
  即空闲时刻SCLK引脚的电平状态。CPOL=0，当不通信时SCLK引脚为低电平，通信时SCLK电平不断变化。CPOL=1，同理

• CPHA（Clock Phase，即时钟相位）
  不论是MISO还是MOSI，数据总是在SCLK引脚电平变化的边沿进行数据采集或数据二进制位变化。而一个时钟周期包含了一个上升沿与一个下降沿，第个电平沿都与数据引脚对应一各操作，数据采集与数据变化，而两者的对应关系由CPOL、CPHA共同决定。其中CPHA决定的就是采样电平沿是通信开始时的第一个电平沿还是第二个电平沿。

综上所述，两个参数共能决定4种SPI通信时序，具体如下图：

图4.1.2.1 SPI通信时序

（3）与ICM42688进行SPI通信的格式

在“4、硬件资料\芯片数据手册\……ICM-42688-P……规格书.PDF”的第52页“9.6 SPI INTERFACE”小节中详细介绍了使用SPI与ICM42688通信的方式。

关于CS片选引脚，规格书中写到“CS goes low (active) at the start of transmission and goes back high (inactive) at the end。”翻译过来就是CS引脚在SPI通信开始前要变为低电平，通信停止后变为高电平。

与ICM42688进行SPI通信还要遵循以下格式（其实许多IMU都要求这种格式）：

1. 传输数据要首先发送MSB（二进制的最高有效位），最后发送LSB（二进制的最低有效位）。
2. ICM42688在SCLK引脚的上升沿锁存数据（读取数据），在单片机中要进行对应的设置，CPOL=0、CPHA=0或CPOL=1、CPHA=1。
3. 数据应该在SCLK的下降沿上进行转换
4. SCLK的最大频率为24mhz
5. SPI读取操作在16个或更多的时钟周期(两个或更多字节)完成。第一个字节包含SPI地址，之后的字节包含SPI数据。第一个字节的第一个位包含的读/写位，表示接下来要进行的操作，读或写。下图表示了SPI地址字节与数据字节的格式，其中数据可能有两个或更多的字节数:

图4.1.2.6

4.1.3 硬件设计

本小节例程功能：使用ANO匿名上位机显示ICM42688陀螺仪、加速度计、温度计数据

用到的硬件资源有：
（1）SPI1
SPI1_CS_PA4
SPI1_SCK_PA5
SPI1_MISO_PA6
SPI1_MOSI_PA7

（2）板载ICM42688P传感器

图4.1.3.1

图4.1.3.2

4.1.4 下载验证

进入“2、飞控例程\4.1 ICM42688”，打开工程并编译。

按住BOOT按键，给飞控上电，进入Bootloader（同时进入USB的DFU模式）。编译例程，双击“Download_tool_ISP”文件夹下的“1_Download_by_usb.bat”，开始下载程序。

连接好蓝牙模块，打开ANO软件及其中的波形显示界面，这里将通道0_{2设置为了x/y/z轴陀螺仪的数值，3}5设置为了x/y/z轴加速度计的值，6为测得的温度计的值。摇晃一下无人机，会发现波形发生变化，如下图：

图4.1.4.1

4.1.5 SPI相关寄存器与库函数

（1）SPI相关的寄存器

• 13.4.1 SPI控制寄存器1（SPI_CTRL1）

  • 位 15 SLBEN
    单线双向半双工模式使能
    0：关闭；
    1：开启。

  • 位 14 SLBTD
    单线双向半双工模式传输方向
    和 SLBEN 位一起决定在“单线双向半双工”模式下数据的传输方向
    0：接收模式；
    1：发送模式。

  • 位 11 FBN
    帧位个数（frame bit num）
    该位配置发送/接收时数据帧位个数。
    0：8 位；
    1：16 位。

  • 位 10 ORA
    仅接收有效
    在“双线单向”模式时，该位置起表示只有接收有效，发
    送被禁止。
    0：发送和接收；
    1：仅接收。

  这里要将SPI设置为双线全双工通信模式，故上述三个为都设置为0

  • 位 9 SWCSEN
    软件 CS 模式使能（Software CS enable）
    当该位被置起时，CS 管脚上的电平由 SWCSIL 位的值决
    定，此时在 CS 管脚上的 I/O 电平状态无效。
    0：关闭；

  • 位 8 SWCSIL
    软件 CS 内部电平（Software CS internal level）
    该位只在 SWCSEN 位置起时有意义，它决定了 CS 上的
    内部电平状态。
    做主设备时，该位必须设置置起。
    0：低电平；
    1：高电平。

  • 位 7 LTF
    LSB 先传输
    该位用于选择数据先传输 MSB 还是 LSB。
    0：MSB；
    1：LSB。

  • 位 6 SPIEN
    SPI 使能
    0：关闭；
    1：开启。

  • 位 5：3 MDIV
    主模式时钟频率分频系数
    作主模式时，分频系数对外设时钟进行分频作为 SPI 时
    钟，MDIV其实有4位，但此处只有3位，另一位MDIV[3]位在 SPI_CTRL2 寄存器中，MDIV[3：0]：
    0000：2 分频
    0001：4 分频
    0010：8 分频
    0011：16 分频
    0100：32 分频
    0101：64 分频
    0110：128 分频
    0111：256 分频
    1000：512 分频
    1001：1024 分频

  • 位 2 MSTEN
    主模式使能（Master enable）
    0：关闭（从设备）；
    1：开启（主设备）。

  • 位 1 CLKPOL
    时钟极性（Clock polarity）
    空闲时时钟输出的极性。
    0：低电平；
    1：高电平。

  • 位 0 CLKPHA
    时钟相位（Clock phase）
    0：第一个边沿进行数据采集；
    1：第二个边沿进行数据采集。

• 13.4.1 SPI控制寄存器1（SPI_CTRL1）

  • 位 9 MDIV3EN
    主模式时钟频率三分频使能
    0：关闭；
    1：开启。
    注：该位开启时，MDIV[3：0]无效，SPI 时钟被强制为
    PCLK 三分频。
  • 位 8 MDIV[3]
    主模式时钟频率分频系数
    详见SPI_CTRL1的MDIV位，MDIV[2：0]在 SPI_CTRL1 寄存器

• 13.4.3 SPI状态寄存器（SPI_STS）

  • 位 1 TDBE
    发送数据缓冲器空
    0：非空；
    1：空。
  • 位 0 RDBF
    接收数据缓冲器满
    0：未满；
    1：满。

• 13.4.4 SPI数据寄存器（SPI_DT）

  • 位 15：0
    数据值
    该寄存器包含读和写的功能，当数据位配置为 8 位时，该
    寄存器只有低 8 位[7：0]有效。

（1）SPI相关的库函数

• 5.21.2 函数 spi_default_para_init

  • 函数原型
    void spi_default_para_init(spi_init_type* spi_init_struct);
  • 功能描述
    给 SPI 初始化结构体赋初值
  • 输入参数 1
    spi_init_struct：指向 spi_init_type 类型的指针

• 5.21.3 函数 spi_init

  • 函数原型
    void spi_init(spi_type* spi_x, spi_init_type* spi_init_struct);
  • 功能描述
    SPI 初始化
  • 输入参数 1
    spi_x：所选择的 SPI 外设
    该参数可以选取自其中之一 ： SPI1，SPI2，SPI3，SPI4.
  • 输入参数 2
    spi_init_struct：指向 spi_initspi_init_type 类型的指针

• 5.21.14 函数 spi_enable

  • 函数原型
    void spi_enable(spi_type* spi_x, confirm_state new_state);
  • 功能描述
    SPI 使能
  • 输入参数 1
    spi_x：所选择的 SPI 外设
    该参数可以选取自其中之一 ： SPI1，SPI2，SPI3，SPI4.
  • 输入参数 2
    new_state：使能或关闭
    该参数可以选取自其中之一 ：FALSE，TRUE

• 5.21.23 函数 spi_i2s_flag_get

  • 函数原型
    flag_status spi_i2s_flag_get(spi_type* spi_x, uint32_t spi_i2s_flag);
  • 功能描述
    读取选定的 SPI/I2S 标志
  • 输入参数 1
    spi_x：所选择的 SPI 外设
    该参数可以选取自其中之一 ： SPI1，SPI2，SPI3，SPI4，I2S2EXT，I2S3EXT
  • 输入参数 2
    spi_i2s_flag：需要获取状态的标志选择
    这里用到了如下两个状态标志：
    SPI_I2S_RDBF_FLAG： SPI/I2S 接收数据缓冲器满标志
    SPI_I2S_TDBE_FLAG： SPI/I2S 发送数据缓冲器空标志

• 5.21.21 函数 spi_i2s_data_transmit

  • 函数原型
    void spi_i2s_data_transmit(spi_type* spi_x, uint16_t tx_data);
  • 功能描述
    SPI 发送一次数据
  • 输入参数 1
    spi_x：所选择的 SPI 外设
    该参数可以选取自其中之一 ： SPI1，SPI2，SPI3，SPI4，I2S2EXT，I2S3EXT
  • 输入参数 2
    tx_data：待发送的数据
    取值范围（帧位个数为 8bit 时）：0x00~0xFF
    取值范围（帧位个数为 16bit 时）：0x0000~0xFFFF

• 5.21.20 函数 spi_i2s_data_receive

  • 函数原型
    uint16_t spi_i2s_data_receive(spi_type* spi_x);
  • 功能描述
    SPI 接收一笔数据
  • 输入参数 1
    spi_x：所选择的 SPI 外设
    该参数可以选取自其中之一 ： SPI1，SPI2，SPI3，SPI4，I2S2EXT，I2S3EXT.
  • 输入参数 2
    rx_data：接收到的数据
    参数范围（帧位个数为 8bit 时）：0x00~0xFF
    参数范围（帧位个数为 16bit 时）：0x0000~0xFFFF

4.1.6 软件设计

（1）ICM42688初始化

在例程main.c中开始任务如下：

void START_task_function(void *pvParameters)		//开始任务
{
	
	/****************************************** 进入临界区，原子操作 ***************************************/
	taskENTER_CRITICAL();
	
	/****************************************** 地面站串口初始化 *****************************************/
	printfSerialInit();
	ANO_init_usart(230400);
	
	/****************************************** LED初始化 ************************************************/
	My_LED_init();
	
	/****************************************** 遥控器初始化 ************************************************/
	Remoter_init();
	
	/****************************************** 航模遥控器初始化 ************************************/
	Motor_init();
	
	/****************************************** 六轴传感器初始化 ****************************************/
	bsp_Icm42688Init();
	
	
#ifdef QY_BY_CORE_BOARD			//AT32核心板
	
#else							//AT32飞控
	
#endif
	
	
	/* 创建其他任务 */
	qy_tasks_creat();	

	
	vTaskDelete(START_handler);		//删除开始任务
	/* 退出临界区 */
	taskEXIT_CRITICAL();
}

行21：
通过调用bsp_Icm42688Init()函数进行SPI与ICM42688的初始化。

bsp_Icm42688Init()函数定义在“qy_drivers\icm42688.c”中，原型为：

int8_t bsp_Icm42688Init(void)
{
	icm42688_spi_init();

    return(bsp_Icm42688RegCfg());
}

可以看到该函数通过调用icm42688_spi_init()函数、bsp_Icm42688RegCfg()函数分别对SPI、ICM42688进行初始化。

icm42688_spi_init()函数原型为：

void icm42688_spi_init(void)
{
  gpio_init_type gpio_initstructure;
  spi_init_type spi_init_struct;

  crm_periph_clock_enable(CRM_GPIOA_PERIPH_CLOCK, TRUE);
  crm_periph_clock_enable(CRM_DMA1_PERIPH_CLOCK, TRUE);
  /* software cs, pa4 as a general io to control flash cs */
  gpio_initstructure.gpio_out_type       = GPIO_OUTPUT_PUSH_PULL;		//推挽输出
  gpio_initstructure.gpio_pull           = GPIO_PULL_UP;				//上拉
  gpio_initstructure.gpio_mode           = GPIO_MODE_OUTPUT;			//引脚输出模式
  gpio_initstructure.gpio_drive_strength = GPIO_DRIVE_STRENGTH_STRONGER;	//强驱动能力
  gpio_initstructure.gpio_pins           = GPIO_PINS_4;
  gpio_init(GPIOA, &gpio_initstructure);

  /* sck */
  gpio_initstructure.gpio_pull           = GPIO_PULL_UP;
  gpio_initstructure.gpio_mode           = GPIO_MODE_MUX;				//引脚复用模式
  gpio_initstructure.gpio_pins           = GPIO_PINS_5;
  gpio_init(GPIOA, &gpio_initstructure);
  gpio_pin_mux_config(GPIOA, GPIO_PINS_SOURCE5, GPIO_MUX_5);			//复用MUX6

  /* miso */
  gpio_initstructure.gpio_pull           = GPIO_PULL_UP;
  gpio_initstructure.gpio_pins           = GPIO_PINS_6;
  gpio_init(GPIOA, &gpio_initstructure);
  gpio_pin_mux_config(GPIOA, GPIO_PINS_SOURCE6, GPIO_MUX_5);

  /* mosi */
  gpio_initstructure.gpio_pull           = GPIO_PULL_UP;
  gpio_initstructure.gpio_pins           = GPIO_PINS_7;
  gpio_init(GPIOA, &gpio_initstructure);
  gpio_pin_mux_config(GPIOA, GPIO_PINS_SOURCE7, GPIO_MUX_5);

  crm_periph_clock_enable(CRM_SPI1_PERIPH_CLOCK, TRUE);			//使能SPI1时钟
  spi_default_para_init(&spi_init_struct);
  spi_init_struct.transmission_mode = SPI_TRANSMIT_FULL_DUPLEX;	//双线全双工通信
  spi_init_struct.master_slave_mode = SPI_MODE_MASTER;			//主机模式
  spi_init_struct.mclk_freq_division = SPI_MCLK_DIV_16;			//APB1时钟16分频作为时钟源(即144/16 = 9 M)
  spi_init_struct.first_bit_transmission = SPI_FIRST_BIT_MSB;	//先发送高位
  spi_init_struct.frame_bit_num = SPI_FRAME_8BIT;				//一帧包含 8bit 数据
  spi_init_struct.clock_polarity = SPI_CLOCK_POLARITY_LOW;		//空闲时，时钟输出低电平
  spi_init_struct.clock_phase = SPI_CLOCK_PHASE_1EDGE;			//SPI 第一个时钟沿进行数据采样
  spi_init_struct.cs_mode_selection = SPI_CS_SOFTWARE_MODE;		//软件 CS 模式
  spi_init(SPI1, &spi_init_struct);		//SPI初始化
  spi_enable(SPI1, TRUE);				//SPI使能 

}  

行3~35：
对时钟、GPIO等进行初始化

行37~45：
设置了关于SPI的一系列参数设置，并通过 spi_init（）函数来实现这些设置。如行40，先发送MSB、行42，CPOL=0、行43，CPHA=0，等与之前所讲ICM42688要求的SPI通信方式相对应。

行46：
使能SPI

bsp_Icm42688RegCfg()函数的原型则为：

int8_t bsp_Icm42688RegCfg(void)
{
    uint8_t reg_val = 0;
	
    /* 读取 who am i 寄存器 */
	icm42688_readReg(ICM42688_WHO_AM_I, &reg_val);
	
    icm42688_writeReg(ICM42688_REG_BANK_SEL,  0); 	 //设置bank 0区域寄存器
    icm42688_writeReg(ICM42688_DEVICE_CONFIG, 0x01); //软复位传感器（此后需要至少等待1ms）
    ICM42688DelayMs(3);


    if(reg_val == ICM42688_ID)
    {
        icm42688_writeReg(ICM42688_REG_BANK_SEL, 1); 	//设置bank 1区域寄存器
        icm42688_writeReg(ICM42688_INTF_CONFIG4, 0x02); //设置为4线SPI通信

        icm42688_writeReg(ICM42688_REG_BANK_SEL, 0); 	//设置bank 0区域寄存器
        icm42688_writeReg(ICM42688_FIFO_CONFIG, 0x40);  //Stream-to-FIFO Mode(page63)


		icm42688_readReg(ICM42688_INT_SOURCE0, &reg_val);
        icm42688_writeReg(ICM42688_INT_SOURCE0, 0x00);
        icm42688_writeReg(ICM42688_FIFO_CONFIG2, 0x00); // watermark
        icm42688_writeReg(ICM42688_FIFO_CONFIG3, 0x02); // watermark
        icm42688_writeReg(ICM42688_INT_SOURCE0, reg_val);
        icm42688_writeReg(ICM42688_FIFO_CONFIG1, 0x63); // Enable the accel and gyro to the FIFO

        icm42688_writeReg(ICM42688_REG_BANK_SEL, 0x00);
        icm42688_writeReg(ICM42688_INT_CONFIG, 0x36);

        icm42688_writeReg(ICM42688_REG_BANK_SEL, 0x00);
		icm42688_readReg(ICM42688_INT_SOURCE0, &reg_val);
        reg_val |= (1 << 2); //FIFO_THS_INT1_ENABLE
        icm42688_writeReg(ICM42688_INT_SOURCE0, reg_val);

        bsp_Icm42688GetAres(AFS_8G);						//计算加速度计分辨率
        icm42688_writeReg(ICM42688_REG_BANK_SEL, 0x00);
		icm42688_readReg(ICM42688_ACCEL_CONFIG0, &reg_val);
        reg_val |= (AFS_8G << 5);   						//量程 ±8g
        reg_val |= (AODR_1000Hz);     						//输出速率 1000HZ
        icm42688_writeReg(ICM42688_ACCEL_CONFIG0, reg_val);

        bsp_Icm42688GetGres(GFS_1000DPS);					//计算陀螺仪分辨率
        icm42688_writeReg(ICM42688_REG_BANK_SEL, 0x00);
		icm42688_readReg(ICM42688_GYRO_CONFIG0, &reg_val);
        reg_val |= (GFS_1000DPS << 5);   					//量程 ±1000dps
        reg_val |= (GODR_1000Hz);     						//输出速率 1000HZ
        icm42688_writeReg(ICM42688_GYRO_CONFIG0, reg_val);

        icm42688_writeReg(ICM42688_REG_BANK_SEL, 0x00);
		icm42688_readReg(ICM42688_PWR_MGMT0, &reg_val);
        reg_val &= ~(1 << 5);								//使能温度测量
        reg_val |= ((3) << 2);								//设置GYRO_MODE  0:关闭 1:待机 2:预留 3:低噪声
        reg_val |= (3);										//设置ACCEL_MODE 0:关闭 1:关闭 2:低功耗 3:低噪声
        icm42688_writeReg(ICM42688_PWR_MGMT0, reg_val);
        ICM42688DelayMs(1); 								//操作完PWR—MGMT0寄存器后 200us内不能有任何读写寄存器的操作

        return 0;
    }
    return -1;
}

行37~49：
设置陀螺仪、加速度计的量程与输出频率。

关于SPI的通信格式，这里通过icm42688_writeReg（）、icm42688_readReg（）、icm42688_readBytes（）三个函数实现。其中涉及到TDBE、RDBF两个标志的问题。
TDBE，发送缓冲区空标志，E即empty，空。
RDBF，接收缓冲区满标志，F即finish，完成。

icm42688_writeReg（）函数原型为：

/* 
* 功能  icm42688写寄存器 
* 参数  addr	寄存器地址
*		dat		需要写入的数据
* 返回  无
*/
static void icm42688_writeReg(uint8_t addr, uint8_t dat)
{
	uint8_t cmd;
	
	ICM24688_CS(0);
	cmd = addr | ICM24688_SPI_W; 		/* 注：此处的运算实质上起到了NSS引脚拉低后的2ns延时作用，因此不可移除 */
	while(SET == spi_i2s_flag_get(ICM_SPI, SPI_I2S_RDBF_FLAG))
		spi_i2s_data_receive(ICM_SPI);	/* 读取读SPI_DATA寄存器中的数据以复位RBNE标志位 */
	while(RESET == spi_i2s_flag_get(ICM_SPI, SPI_I2S_TDBE_FLAG));/* 等待发送缓冲区清空 */
	
	while(SET == spi_i2s_flag_get(ICM_SPI, SPI_I2S_TDBE_FLAG))
		spi_i2s_data_transmit(ICM_SPI, cmd);/* 发送地址与写指令 */
	while(RESET == spi_i2s_flag_get(ICM_SPI, SPI_I2S_TDBE_FLAG));/* 等待发送缓冲区清空 */	
	
	while(SET == spi_i2s_flag_get(ICM_SPI, SPI_I2S_TDBE_FLAG))
		spi_i2s_data_transmit(ICM_SPI, dat);/* 发送需要写入寄存器的值 */
	while(SET == spi_i2s_flag_get(ICM_SPI, SPI_I2S_BF_FLAG)); /* 等待数据传输完毕 */
	
	while(SET == spi_i2s_flag_get(ICM_SPI, SPI_I2S_RDBF_FLAG))
		spi_i2s_data_receive(ICM_SPI);/* 读取读SPI_DATA寄存器中的数据以复位RBNE标志位 */
	while(RESET == spi_i2s_flag_get(ICM_SPI, SPI_I2S_TDBE_FLAG));/* 等待发送缓冲区清空 */
	
	ICM24688_CS(1);
}

行13~15：
由于TDBE、RDBF两个标志一个表示空一个表示满while循环看着有点别扭，但两个都是在等待缓冲区清空。其中发送缓冲区清空较简单，一直等就好，发送完成缓冲区自动就会清空；接收则要读取才会将缓冲区清空了。

行17~23：
发送SPI地址字节与SPI数据字节。

行25~27：
与行13~15内容相同，其实去掉也没问题。

icm42688_readReg（）函数

/* 
* 功能  icm42688读寄存器 
* 参数  addr	寄存器地址
*		dat		读取得到的数据
* 返回  无
*/
void icm42688_readReg(uint8_t addr, uint8_t *dat)
{
	uint8_t cmd;
	uint8_t res;
	
	ICM24688_CS(0);
	cmd = addr | ICM24688_SPI_R; /* 注：此处的运算实质上起到了NSS引脚拉低后的2ns延时作用，因此不可移除 */
	
	while(SET == spi_i2s_flag_get(ICM_SPI, SPI_I2S_RDBF_FLAG))
		res = spi_i2s_data_receive(ICM_SPI);/* 读取读SPI_DATA寄存器中的数据以复位RBNE标志位 */
	while(RESET == spi_i2s_flag_get(ICM_SPI, SPI_I2S_TDBE_FLAG));/* 等待发送缓冲区清空 */
	
	while(SET == spi_i2s_flag_get(ICM_SPI, SPI_I2S_TDBE_FLAG))
		spi_i2s_data_transmit(ICM_SPI, cmd);/* 发送地址与读指令 */
	while(SET == spi_i2s_flag_get(ICM_SPI, SPI_I2S_BF_FLAG));/* 等待数据传输完毕 */	
	while(SET == spi_i2s_flag_get(ICM_SPI, SPI_I2S_RDBF_FLAG))
		spi_i2s_data_receive(ICM_SPI);/* 本周期（以缓冲区内数据发送完成为结束）内收到的数据不能使用 */
	while(RESET == spi_i2s_flag_get(ICM_SPI, SPI_I2S_TDBE_FLAG));/* 等待发送缓冲区清空 */
 
	while(SET == spi_i2s_flag_get(ICM_SPI, SPI_I2S_TDBE_FLAG))
		spi_i2s_data_transmit(ICM_SPI, 0x00);/* 传输数据以进行接收 */
	while(SET == spi_i2s_flag_get(ICM_SPI, SPI_I2S_BF_FLAG));/* 等待数据传输完毕 */	
	while(SET == spi_i2s_flag_get(ICM_SPI, SPI_I2S_RDBF_FLAG))
		res = spi_i2s_data_receive(ICM_SPI);/* 这个周期收到的才是传感器返回的数据 */
	while(RESET == spi_i2s_flag_get(ICM_SPI, SPI_I2S_TDBE_FLAG));/* 等待发送缓冲区清空 */
	ICM24688_CS(1);
	
	*dat = res;
}

行15~17：
接收与发送待缓冲区清空。

行19~24：
发送SPI地址字节，并等待两缓冲区清空。同时发送SPI地址字节时ICM42688不没有接收到完整的地址字节，而发送完成后到真正接收到ICM42688的数据应该还有些时间间隙，则单片机在此前接收到的SPI数据是不能用的，故加入了22~24行。同时21行判断了SPI_I2S_BF_FLAG传输忙标志，该标志猜测是TDBE与RDBF标志的融合。

行26~31：
为接收ICM42688发送的数据，接收数据的同时还要发送一个字节的数据，在在“4、硬件资料\芯片数据手册\……ICM-42688-P……规格书.PDF”的第52页“9.6 SPI INTERFACE”小节中有这样的描述：“In cases of multiple-byte Reads, data is two or
more bytes:” ，即“当读取多个字节时，SPI数据字节是两个或多个”。根据此描述，读取ICM42688寄存器值时主机还是要发送SPI数据字节的，但发送内容不限，这里设置发送0，第读取一个寄存器的值后，寄存器地址递增。

icm42688_readBytes（）函数
为同时读取多个寄存器的数据，原型为：

/* 
* 功能  icm42688读取多字节数据 
* 参数  addr	寄存器地址
*		dat		读取得到的数据
*		len		需要读取的数据长度
* 返回  无
*/
void icm42688_readBytes(uint8_t addr, uint8_t *dat_array, uint8_t len)
{
	uint8_t cmd;
	uint8_t *dat_ptr = dat_array;
	uint8_t i;
	
	ICM24688_CS(0);
	cmd = addr | ICM24688_SPI_R; /* 注：此处的运算实质上起到了NSS引脚拉低后的2ns延时作用，因此不可移除 */
	while(SET == spi_i2s_flag_get(ICM_SPI, SPI_I2S_RDBF_FLAG))
		spi_i2s_data_receive(ICM_SPI);/* 读取读SPI_DATA寄存器中的数据以复位RBNE标志位 */
	while(RESET == spi_i2s_flag_get(ICM_SPI, SPI_I2S_TDBE_FLAG));/* 等待发送缓冲区清空 */
	
	while(SET == spi_i2s_flag_get(ICM_SPI, SPI_I2S_TDBE_FLAG))
		spi_i2s_data_transmit(ICM_SPI, cmd);/* 发送地址与读指令 */
	while(SET == spi_i2s_flag_get(ICM_SPI, SPI_I2S_BF_FLAG));/* 等待数据传输完毕 */	
	while(SET == spi_i2s_flag_get(ICM_SPI, SPI_I2S_RDBF_FLAG))
		spi_i2s_data_receive(ICM_SPI);/* 本周期（以缓冲区内数据发送完成为结束）内收到的数据不能使用 */
	while(RESET == spi_i2s_flag_get(ICM_SPI, SPI_I2S_TDBE_FLAG));/* 等待发送缓冲区清空 */
	
	for(i = 0; i < len; i++)
	{
		while(SET == spi_i2s_flag_get(ICM_SPI, SPI_I2S_TDBE_FLAG))
			spi_i2s_data_transmit(ICM_SPI, 0);/* 传输数据以进行接收 */
		while(SET == spi_i2s_flag_get(ICM_SPI, SPI_I2S_BF_FLAG));/* 等待数据传输完毕 */
		while(SET == spi_i2s_flag_get(ICM_SPI, SPI_I2S_RDBF_FLAG))		
			*dat_ptr = spi_i2s_data_receive(ICM_SPI);/* 这个周期收到的才是传感器返回的数据 */
		while(RESET == spi_i2s_flag_get(ICM_SPI, SPI_I2S_TDBE_FLAG));/* 等待发送缓冲区清空 */
		dat_ptr++;
	}
	ICM24688_CS(1);
}

行27~36：
与icm42688_readReg（）函数的的不同就要这，发送多个SPI数据字节也接收多个SPI数据，第读取一个ICM42688寄存器值后，寄存器地址都递增（如果设置为8位SPI，变为读取下一个8位的寄存器）。

（2）ICM42688调用

本小节新建了qy_Ati_pos_solve_task.c与qy_Ati_pos_solve_task.h文件，用于存放Ati_pos_solve_task姿位解算任务相关的代码。原型为：

void Ati_pos_solve_task_function(void *pvParameters)
{
	
	while(1)
	{	
		icm42688_get_gyro();
		icm42688_get_acc();
		icm42688_get_temp();
		
		//数据标准化，并调整传感器方向，和机体坐标一致
		Ati_pos_post.gyro.x = icm42688_gyro_transition(icm42688_gyro_x);	// 单位为°/s
		Ati_pos_post.gyro.y = icm42688_gyro_transition(icm42688_gyro_y);
		Ati_pos_post.gyro.z = icm42688_gyro_transition(icm42688_gyro_z);

		Ati_pos_post.acce.x = icm42688_acc_transition(icm42688_acc_x);		// 单位为 g(m/s^2)
		Ati_pos_post.acce.y = icm42688_acc_transition(icm42688_acc_y);	
		Ati_pos_post.acce.z = icm42688_acc_transition(icm42688_acc_z);

		vTaskDelay(5);
	}
}

这里要谈一下坐标系相关的问题：
航空航天领域常使用如下形式的机体姿态角：

图4.1.6.1 机体姿态角
Roll：横滚角，Pitch:俯仰角，Yaw:偏航角

使用如下形式的机体坐标：

图4.1.6.2 机体坐标
机头是x正方向，z正方向与所受地球引力方向重合。

而ICM42688的坐标的在芯片手册53页的“10.1 ORIENTATION OF AXES”小节有讲到，如图：

图4.1.6.3 ICM42688的坐标

在使用时要保证六轴（或称作IMU）的加速计方向要与机体坐标的xyz方向对应，保证陀螺仪各轴的旋转方向与机体姿态角的旋转方向对应。幸好机体坐标、姿态角与IMU的坐标在设计时应该就考虑到了两者间的吻合问题（也不知道是谁迁就的谁）。也就是说当IMU的加速度计方向与机体坐标重合时，机体姿态角与陀螺仪的旋转方向也自然的吻合的，反过来也是一样。
但还要考虑到IMU安装是可能与机体坐标、姿态角不吻合的情况，这就需要软件模拟改变IMU安装角度的问题了。比如在使用Betaflight等开源飞控时便有设置传感器安装方向的功能，比如“CW 90度”表示IMU绕z轴旋转90度，“CW 90度 flip”表示将IMU翻转到背面再绕z轴旋转90度，如下图：

图4.1.6.4

但IMU模拟旋转就要涉及到旋转矩阵之类的东西了，这里就用更简单直接赋值来进行类似的操作了，如Ati_pos_solve_task_function（）函数的10~17行，这里Ati_pos_post.gyro.x等代表的就是与机体坐标与机体姿态角吻合的数据，由于飞控设计时已经考虑到这点，只要AT32飞控上面的白色箭头指向机头方向这里就可以直接使用IMU的实际坐标了。另外icm42688_gyro_transition（）函数等是单位化函数，用于将IMU原始值，转化为“°/s”、“g”等标准单位的数据。

3408718859)]
图4.1.6.2 机体坐标
机头是x正方向，z正方向与所受地球引力方向重合。

而ICM42688的坐标的在芯片手册53页的“10.1 ORIENTATION OF AXES”小节有讲到，如图：
[外链图片转存中…(img-7yfeCh0w-1703408718859)]
图4.1.6.3 ICM42688的坐标

在使用时要保证六轴（或称作IMU）的加速计方向要与机体坐标的xyz方向对应，保证陀螺仪各轴的旋转方向与机体姿态角的旋转方向对应。幸好机体坐标、姿态角与IMU的坐标在设计时应该就考虑到了两者间的吻合问题（也不知道是谁迁就的谁）。也就是说当IMU的加速度计方向与机体坐标重合时，机体姿态角与陀螺仪的旋转方向也自然的吻合的，反过来也是一样。
但还要考虑到IMU安装是可能与机体坐标、姿态角不吻合的情况，这就需要软件模拟改变IMU安装角度的问题了。比如在使用Betaflight等开源飞控时便有设置传感器安装方向的功能，比如“CW 90度”表示IMU绕z轴旋转90度，“CW 90度 flip”表示将IMU翻转到背面再绕z轴旋转90度，如下图：
[外链图片转存中…(img-rCEG9cBv-1703408718859)]
图4.1.6.4

但IMU模拟旋转就要涉及到旋转矩阵之类的东西了，这里就用更简单直接赋值来进行类似的操作了，如Ati_pos_solve_task_function（）函数的10~17行，这里Ati_pos_post.gyro.x等代表的就是与机体坐标与机体姿态角吻合的数据，由于飞控设计时已经考虑到这点，只要AT32飞控上面的白色箭头指向机头方向这里就可以直接使用IMU的实际坐标了。另外icm42688_gyro_transition（）函数等是单位化函数，用于将IMU原始值，转化为“°/s”、“g”等标准单位的数据。