六轴传感器（ICM42688）驱动及姿态估计

2022-07-28

Word count: 8.3k | Reading time≈ 35 min

前言：打算给小巡洋舰搞点花活，顺便做个飞控，记录一下其中最为关键的惯性导航（IMU）。本次项目中使用的是TDK出的最新MEMS传感器以提高项目的生命周期，因此本文将先简单介绍ICM42688这个片子，接着探讨SPI驱动方法，最后记录六轴MEMS传感器的工作原理。

ICM42688简介

ICM42688芯片

这款6轴MEMS运动跟踪传感器内有一个三轴陀螺仪和一个三轴加速度计，通信方面支持I3C接口、常规的IIC、SPI通信和2kB的FIFO，并且按照TDK的老传统这个片子里也是带数字处理单元的。这个片子目前算是消费电子里的顶流了，参数如下：

陀螺仪噪声密度：$0.0028°/s/\sqrt{Hz}$
加速度计噪声密度：$70\mu g/\sqrt{Hz}$
8档陀螺仪量程：$\pm15.6 \sim 2000dps$
4档加速度计量程：2/4/8/16g
前端ADC精度：16bits
SPI通信速率达24MHz
最后，温漂参数也是非常不错：
- 陀螺仪SSF-Temp为$\pm 0.005 % /° C$，ZRO-Temp为$\pm 0.005 °/s/° C$
- 加速度计SSF-Temp为$\pm 0.005 % /°C$，ZGO-Temp为$\pm 0.15 mg/° C$

至于内置的APEX数字运动处理器也是非常优秀了，可以说是针对消费电子设计的，支持检测拿起、记录步数、倾斜测试等。这些功能在机器人应用上虽然犹如鸡肋，但是应该可以做一些花活。

SPI总线

SPI总线在高速板载外设的通信中，极为常见。标准的4线SPI的信号线有：

片选信号线NSS（或CS、CSB）：SPI标准允许一套总线上挂载多个设备，但是通信时只能有一个设备与主机通信，也就是片选被使能（拉低）的外设；
同步时钟线SCLK（或SCK）：在下面将会看到，数据线上的信号将在时钟线的上下边沿被采集或放置，多个从机时该线共享；
主机收从机发数据线MISO（或SDO）：全称Master Input Slave Output，作用很显然了，串行发至主机的数据线，多个从机时该线共享；
主机发从机收数据线MOSI（或SDI）：全称Master Output Slave Input，类似的，串行发至从机的数据线，多个从机时该线共享；

⚠️SPI总线属于有时钟的同步总线，因此在布线时应该严格按等长线设计；但一般无需做阻抗匹配

SPI总线

在STM32等MCU上，SPI外设的配置十分灵活，如上所示可以配置出最少4种类型的SPI通信方式，并由参数CPOL和CPHA配置：

CPOL(Clock Polarity)表示时钟线SCLK在空闲状态时的信号电平。
CPHA(Clock Phase)表示数据线MOSI和MISO在哪个时钟沿捕获数据。如上图所示，当该参数为1Edge时，设备在第一个时钟跳变沿采样数据；当该参数为2Edge时，设备在第二个时钟跳变沿采样数据。

ICM42688的SPI标准

外设SPI时序

上图为截取自42688数据手册的SPI时序图，显然片选信号CS和时钟信号SCLK在空闲时都为高电平，数据线SDI和SDO在每个周期的第一个边沿放置数据及第二个边沿采样数据。因此CPOL=High(1)，CPHA=2Edge(1)。

底层驱动代码

ICM42688原理图

本文接下来开发的驱动程序都是基于上原理图实现的硬件进行配置，为FOSH计划中的GUAV通用无人空中飞行器项目。采样标准4线SPI通信的方式，并使用1个中断线和一个外部32.768kHz的标准时钟（来自MCU的内部时钟共享，以提高数据同步性），另外添加PWM控制的加热恒温功能以防止低温环境带来的误差。

驱动特性

在ICM42688P的数据手册上，我们看到该片的SPI使用时为：

先发高位（MSB first）
上升沿采样（与上面SPI小节中传输时序一致）
读写时都是主机先发（MOSI）的第一个字节为地址信息，且该字节的最高位表示读写指令（读是1，写是0）

根据这些信息来配置MCU，笔者使用的为STM32H750VBT6，使用官方工具CubeMX配置SPI参数具体如下：

STM32配置

在CubeMX生成的项目中，可以编写如下代码读写ICM42688P的寄存器，并测试连续读写功能：

基于该测试代码和上面的原理图（等长布线），经实际上板测试，在225kHz~22.5MHz的时钟（波特率）范围内都可以稳定通信（SPI官方最高速率指标为24MHz）

#include "bsp.h"

#include "string.h"

#define ICM42xxx_HSPI hspi2
#define ICM42xxx_BUFFER_LEN 16
#define ICM42xxx_SPI_NSS_HIGH()  HAL_GPIO_WritePin(SPI2_NSS4IMU_GPIO_Port, SPI2_NSS4IMU_Pin, GPIO_PIN_SET)
#define ICM42xxx_SPI_NSS_LOW()   HAL_GPIO_WritePin(SPI2_NSS4IMU_GPIO_Port, SPI2_NSS4IMU_Pin, GPIO_PIN_RESET)

uint8_t icm42xxx_spi_tx [ICM42xxx_BUFFER_LEN] ={0xff};
uint8_t icm42xxx_spi_rx [ICM42xxx_BUFFER_LEN] ={0xff};

uint8_t testCache[4];

/*****************************************************
以下为寄存器读写接口
*****************************************************/

/**
  * @brief  向指定寄存器写入数据
  * @note		这个芯片只能单次写，不支持突发写
  * @param  reg_addr:  写入寄存器地址
  *          value: 写入的数据
  * @retval 0正常；及其他错误代码
  */
uint32_t icm42xxx_spi_master_write_register(uint8_t reg_addr, uint8_t value)
{
	ICM42xxx_SPI_NSS_LOW();	// 软件使能
	icm42xxx_spi_tx[0] = reg_addr & 0x7F;	// 写入时，寄存器地址字节的第一位为 0；0x7f <-> 01111111
	icm42xxx_spi_tx[1] = value;	// MOSI上第二个字节为写入的数据
	
	HAL_SPI_TransmitReceive(&ICM42xxx_HSPI,
	                       &icm42xxx_spi_tx[0],
	                       &icm42xxx_spi_rx[0], 2,
	                       0XFF);
	ICM42xxx_SPI_NSS_HIGH();

	return 0;
}

/**
  * @brief  读取指定寄存器or一堆寄存器数据
  * @note   支持突发（连续）读
	* @param  reg_addr:  读取数据的起始地址
  *          pData:     返回的数据
  *          len:    读取的数字长度
  * @retval 0正常；及其他错误代码
  */
uint32_t icm42xxx_spi_master_read_register(uint8_t reg_addr, uint8_t* pData, uint8_t len)
{
	if(len>ICM42xxx_BUFFER_LEN-1)
		return 1;	// 超出缓存空间！
	
	ICM42xxx_SPI_NSS_LOW();
	icm42xxx_spi_tx[0] = reg_addr | 0x80;	// 读取时，寄存器地址字节的第一位为 1
	memset(icm42xxx_spi_tx+1,0x00,len);	// 接收数据时，注意MOSI上不能再发地址，否则会破坏突发读的地址连续性

	if( HAL_SPI_TransmitReceive(&ICM42xxx_HSPI,
	                             &icm42xxx_spi_tx[0],
	                             &icm42xxx_spi_rx[0], len+1,
	                             0XFF)
		 != HAL_OK) //因为先发地址后读数据，因此这里长度是len+1
	{
		return 2;	// SPI读取操作出错
	}
	ICM42xxx_SPI_NSS_HIGH();
	memcpy(pData,icm42xxx_spi_rx+1,len);
	
	return 0;
}

/**
  * @brief  通信测试
  * @note   测试单次读取芯片ID，和连续读
  * @param  
  * @retval 0测试通过；及其他错误代码
  */
uint32_t icm42xxx_spi_master_communicate_test(void)
{
	#define ICM42XXX_REGADDR_WHO_AM_I (0x75)	// 不同芯片读出来的ID值不一样，42688P是0x47
	#define ICM42XXX_REGVAL_WHO_AM_I (0x47)
	#define ICM42XXX_REGADDR_ACCEL_WOM_Y_THR (0x4B)
	
	
	icm42xxx_spi_master_read_register(ICM42XXX_REGADDR_WHO_AM_I, testCache, 1);	// 读取身份信息寄存器
	
	if(testCache[0] != ICM42XXX_REGVAL_WHO_AM_I)	// 身份核验
		return 1;	// 表示读取身份错误
	
	icm42xxx_spi_master_write_register(0x11,0x01);	// 软件复位
	HAL_Delay(2);	// 要等待最少1ms
	icm42xxx_spi_master_read_register(ICM42XXX_REGADDR_ACCEL_WOM_Y_THR, testCache, 2);		// 连读WOM_Y_TH、WOM_Z_TH
	/********************************
	BUG REPORT:
	调试过程中，发现不能连续操作3个字节
	目前还没找到原因！
	可能原因：
	1. 这几个寄存器之间不支持连读写
	   需要连续原厂才有答案
	2. 硬件时序偏移，使用高精度DLA可知
	********************************/
	for(int i = 0; i < 2;i++)
	{
		testCache[i] += 0x15;	// 随机变个数据
		HAL_Delay(1);
		icm42xxx_spi_master_write_register((ICM42XXX_REGADDR_ACCEL_WOM_Y_THR + i), testCache[i]);	// 写入数据
	}
	
	HAL_Delay(1);
	icm42xxx_spi_master_read_register(ICM42XXX_REGADDR_ACCEL_WOM_Y_THR, testCache+2, 2);	// 再读一次
	
	for(int i = 0; i < 2;i++)
	{
		if(testCache[i] != testCache[i+2])	// 检验连续读写的正确性
			return 2;	// 表示连续读写错误
	}
	
	return 0;	// 表示测试通过
	
}

姿态解算算法

一般用直观的欧拉角来表示机体姿态，但是解算过程中，由于欧拉角的万向锁问题，人们使用四元数作为主要数学工具。

四元数

这是类似于复数的工具，但虚部是$\mathbb R^3$空间向量$\mathbf q_v=[q_1,q_2,q_3]^T$，一般写为：

$$
\mathbf q \triangleq \begin{bmatrix}
q_0 \\
\mathbf q_v
\end{bmatrix}
$$

具有性质：

加减法（显然满足交换律）
$$
\mathbf{p} \pm \mathbf{q} =
\begin{bmatrix}
p_0 \\ \mathbf p_v
\end{bmatrix} \pm
\begin{bmatrix}
q_0 \\ \mathbf q_v
\end{bmatrix} =
\begin{bmatrix}
p_0\pm q_0 \\ \mathbf p_v\pm \mathbf q_v
\end{bmatrix}
$$
乘法（不能交换次序，但有分配律和结合律）
$$
\mathbf p\otimes \mathbf q =
\begin{bmatrix}
p_0 \\ \mathbf p_v
\end{bmatrix} \otimes
\begin{bmatrix}
q_0 \\ \mathbf q_v
\end{bmatrix} =
\begin{bmatrix}
p_0q_0-\mathbf q_v^T \mathbf p_v \\ \mathbf p_v\times \mathbf q_v + p_0 \mathbf q_v + q_0 \mathbf p_v
\end{bmatrix}
\\
\mathbf q \otimes (\mathbf r+\mathbf m) = \mathbf q\otimes \mathbf r + \mathbf q \otimes \mathbf m
\\
\mathbf q \otimes \mathbf r \otimes \mathbf m = (\mathbf q \otimes \mathbf r) \otimes \mathbf m =\mathbf q \otimes (\mathbf r \otimes \mathbf m )
$$
数乘
$$
s\mathbf q = \mathbf q s = \begin{bmatrix} sq_0 \\ s\mathbf q_v \end{bmatrix}
$$
共轭（和复数类似，都是虚部反）
$$
\mathbf q^* = \begin{bmatrix} q_0 \\ -\mathbf q_v \end{bmatrix}
\\
(\mathbf q \otimes \mathbf p)^* = \mathbf p^* \otimes \mathbf q^*
$$
范数
$$
\Vert \mathbf q \Vert = \sqrt{q_0^2 + \mathbf q_v^T\mathbf q_v} = \sqrt{\Vert \mathbf q \otimes \mathbf q^*\Vert}
$$
逆
$$
\mathbf q \otimes \mathbf q^{-1} = \begin{bmatrix}
1 \\
\mathbf 0_{3\times 1}
\end{bmatrix} \\
\mathbf q^{-1} = \frac{\mathbf q^*}{\Vert \mathbf q \Vert}
$$

以上就是四元数的基本性质，接下来我们将这个工具往需要的姿态解算上带。首先，从四元数的范数计算，我们类似单位向量一般的，定义单位四元数$\Vert \mathbf q\Vert = 1$，这是因为单位化之后的四元数们具有优秀的计算性质：

$$
\forall . \Vert \mathbf q \Vert =\Vert \mathbf p \Vert =1
\\
\Rightarrow \Vert \mathbf q \otimes \mathbf p \Vert = 1
\\
\mathbf q^{-1} = \mathbf q^{*}
$$

然后我们观察到四元数$\mathbf q$是由一个标量$q_0$和一个空间向量$\mathbf q_v$组成，如果把一个旋转过程记录为绕$\mathbf v$轴（右手螺旋系）旋转$\theta_0$角度，那么看起来恰好可以用四元数来表示🧐但是人们发现直接这样带进去的话，算起来还是不好用！然后就有人发现，可以把旋转过程和四元数对应为：

$$
\mathbf q = \begin{bmatrix}
\cos\frac \theta 2 \\
\mathbf v \sin\frac \theta 2
\end{bmatrix},
\Vert \mathbf v \Vert =1
$$

首先，这样映射的话就成了单位四元数啦，求逆很方便！然后将空间方向向量$\mathbf v_1$用一个四元数表示：
$$
\mathbf p = \begin{bmatrix}
0 \\
\mathbf v_1
\end{bmatrix}
$$

则该空间向量经过一个$\mathbf q$的旋转过程（绕$\mathbf v$转$\theta$角），经证明，可采用下式计算旋转后的空间姿态$\mathbf v_1’$：

$$
\begin{bmatrix}
0 \\ \mathbf v_1’
\end{bmatrix} = \mathbf q \otimes \mathbf p \otimes \mathbf q^{-1} =
\begin{bmatrix}
\cos\frac \theta 2 \\
\mathbf v \sin\frac \theta 2
\end{bmatrix} \otimes
\begin{bmatrix}
0 \\
\mathbf v_1
\end{bmatrix} \otimes
\begin{bmatrix}
\cos\frac \theta 2 \\
-\mathbf v \sin\frac \theta 2
\end{bmatrix}
$$

读者可以直接记下该重要公式，有余力可结合文末的推荐参考文献自行推导具体过程。

四元数旋转计算过程矩阵化

除了四元数外，这里也介绍一下它的好兄弟——旋转矩阵：

$$
\mathbf v_e = \mathbf R ^e_b \cdot \mathbf v_b
\\
\mathbf R^e_b \in SO(3),SO(3) = { \mathbf R | \mathbf R^T \mathbf R = \mathbf I_3, det(\mathbf R) = 1,\mathbf R \in \mathbb R_{3\times 3} }
$$

虽然用不上，但是这里为啥要介绍旋转矩阵呢，因为优秀的先人就是从这个旋转矩阵的变换中，观察到四元数空间旋转计算过程和旋转矩阵计算极为像！前提是我们要把四元数旋转计算过程进行矩阵化（毕竟不矩阵化，计算机也算不了这玩意），四元数乘法计算可以转为：

$$
\mathbf p\otimes \mathbf q = \mathbf p^+ \cdot \mathbf q = \mathbf q^- \cdot \mathbf p
$$

上式矩阵乘法中，将一个四元数直接作为$\mathbb R_{4}$向量，另一个四元数与四元数乘法符号一起转换为一个$\mathbb R_{4\times 4}$等效矩阵：

$$
\mathbf p^{+} = p_0 \mathbf I_4 +
\begin{bmatrix}
0 & -\mathbf p_v^T
\\
\mathbf p_v & [\mathbf p_v]_\times
\end{bmatrix}
$$
$$
\mathbf q^{-} = q_0 \mathbf I_4 +
\begin{bmatrix}
0 & -\mathbf q_v^T
\\
\mathbf q_v & - [\mathbf q_v]_\times
\end{bmatrix}
$$

其中，$[\mathbf p_v]_\times$是$\mathbf p_v$叉乘计算的矩阵点乘表示：

$$
\mathbf p_v \times \mathbf a = [\mathbf p_v]_\times \mathbf a,因此
[\mathbf p_v]_\times =
\begin{bmatrix}
0 & -p_3 & p_2
\\
p_3 & 0 & -p_1
\\
-p_2 & p_1 & 0
\end{bmatrix}
$$

因此四元数旋转计算过程可以矩阵化为：

$$
\begin{bmatrix}
0 \\ \mathbf v_{1}’
\end{bmatrix} = \mathbf q \otimes
\begin{bmatrix}
0 \\
\mathbf v_{1}
\end{bmatrix} \otimes \mathbf q^* =
\mathbf q^{+} \cdot \mathbf q^{* -} \cdot
\begin{bmatrix}
0 \\
\mathbf v_{1}
\end{bmatrix}
$$
$$
\Rightarrow
\begin{bmatrix}
0 \\ \mathbf v_1’
\end{bmatrix} =
\begin{bmatrix}
q_0 & -\mathbf q_v^T
\\
\mathbf q_v & q_0\mathbf I_3 + [\mathbf q_v]_\times
\end{bmatrix}
\begin{bmatrix}
q_0 & +\mathbf q_v^T
\\
-\mathbf q_v & q_0\mathbf I_3 + [\mathbf q_v]_\times
\end{bmatrix}
\begin{bmatrix}
0 \\
\mathbf v_1
\end{bmatrix}
$$

在这个过程中，由于变换对象和结果对象四元数的实数部分都是0，所以如下我们可以把与0操作的部分直接去掉，则得到如下类似旋转矩阵的变换形式：

$$
\mathbf v_1’ =
\begin{bmatrix}
q_0^2+q_1^2-q_2^2-q_3^2 & 2(q_1q_2-q_0q_3) & 2(q_1q_3+q_0q_2)
\\
2(q_1q_2+q_0q_3) & q_0^2-q_1^2+q_2^2-q_3^2 &2(q_2q_3-q_0q_1)
\\
2(q_1q_3-q_0q_2) & 2(q_2q_3+q_0q_1) & q_0^2-q_1^2-q_2^2+q_3^2
\end{bmatrix} \cdot
\mathbf v_1
$$

六轴数据→四元数→姿态估计

姿态估计算法的最终目标通常就是欧拉角，因为欧拉角的物理概念很直观很好理解，方便设计多旋翼等类型的控制系统。而中间计算过程，上面通过对四元数的介绍，我们知道肯定是用这种优秀的工具。当机体停在平地上开机，接着开始运动，我们可以分别从3轴陀螺仪、3轴加速度计获得两套姿态：

陀螺仪数据获得姿态

这里我们将传感器返回的3轴陀螺仪数据记为：
$$
\omega =
\begin{bmatrix}
\omega_x \\
\omega_y \\
\omega_z
\end{bmatrix}
$$

显然，对角速度进行积分就可以直接得到姿态，但从长远来看，我们需要把积分过程用四元数来算才行。从四元数的性质推导得到四元数的微分方程：

$$
\dot{\mathbf q} = \frac 12 \mathbf q \otimes
\begin{bmatrix}
0 \\
\omega
\end{bmatrix}
$$

所以陀螺仪估计姿态，在MCU中的离散式计算方程为：

$$
\mathbf q_t = \mathbf q_{t-1} + \frac 12 \mathbf q_{t-1} \otimes
\begin{bmatrix}
0 \\
\omega
\end{bmatrix} \Delta t
$$

式中$\Delta t$为定采样/计算迭代的周期时长。一个纯积分的系统，都必然是会发散的！这也是用陀螺仪测量的问题，如果一直迭代上式，就算是静止放着，角速度测量时的噪声、误差等都会不断累积，也就是大家常说的“漂移”。
加速度计数据获得姿态

类似的，我们把传感器得到的3轴加速度计数据记为：

$$
\mathbf a =
\begin{bmatrix}
a_x \\
a_y \\
a_z
\end{bmatrix}
$$

当我们把传感器静止放在一个水平面上时，显然受到重力的作用，测量的结果应该是当地的重力加速度$\mathbf g = [0,0,g]^T$；而在其他姿态静止情况下，我们通过勾股定理可以得到：

$$
a_x = -g \sin \theta \\
a_y = g \cos \theta \sin \phi \\
a_z = g \cos \theta \cos \phi
$$

多么美妙方便呀，可以直接得到两个姿态角$\theta = \arcsin(\frac {a_x}{-g})$和$\phi = \arctan(\frac{a_y}{a_z})$。是的，非常遗憾，得不到偏航角！并且还有其他问题，比如载体运动时测量结果带有运动加速度、加速度噪声等。

通过上面直接用原始数据求解姿态，我们就得到了一句流传于世的经典名言：陀螺仪测量结果高频特性好，加速度计测量结果低频特性好。展开来说呢，就是陀螺仪因为积分漂移的问题，看总体的话测量结果会越来越差，但是局部测量的姿态（角速度）可靠性高；而加速度恰恰相反，没有漂移问题，但是局部测量结果估计的姿态存在较大干扰和噪声。所以，工程师们达成了一个共识，陀螺仪和加速计配合起来用就叫做一个完美！（当然，还有个缺陷就是偏航角测量问题，得加地磁计）那么就要设计一套多传感器数据融合估计姿态算法了，接下来开始介绍一套常用的6轴传感器估计姿态算法。

这里接下来讨论的其实是Mahony算法！还有很多其他优秀的姿态估计算法，只是本文写不下而已！

Mahony算法框图

图中$_b \vec a$和$_b\vec \omega$分别表示传感器测量得到的加速度和角速度，由于传感器是安装在机体上的，所以这里使用前下角标“$_b$”进行注明，并考虑图片分辨率问题使用头戴箭头的方式替代本文中的粗体来表示矢量。

这个算法融合的原理简单来说，就是用PI控制器去调整四元数离散微分方程中的角速度，补偿的依据（误差）是加速度计测量得到的向量与估计姿态下的重力方向做叉乘，这个误差的定义也就是这个算法的精髓。可以看出算法框图中是一个闭环结构，所以最后算法收敛的目标就是陀螺仪估计得到的姿态应该接近/等于加速度计估计的姿态。

Mahony姿态估计实现代码

#define sampleFreq	1000.0f			// 采样率（ Hz ）
#define twoKpDef	(2.0f * 0.5f)	// 2 * proportional gain
#define twoKiDef	(2.0f * 0.0f)	// 2 * integral gain

volatile float twoKp = twoKpDef;											// 2 * proportional gain (Kp)
volatile float twoKi = twoKiDef;											// 2 * integral gain (Ki)
volatile float integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f;	// 积分误差

/******************************************************************
**  算法实现
*******************************************************************/

/**
  * @brief  快速计算反平方根
  * @note   1/sqrt(x)
  * @param  x : 输入参数
  * @retval y : 反平方根结果
  */
float inv_sqrt(float x) 
{
	float halfx = 0.5f * x;
	float y     = x;
	long  i     = *(long*)&y;
	
	i = 0x5f3759df - (i >> 1);
	y = *(float*)&i;
	y = y * (1.5f - (halfx * y * y));
	
	return y;
}

/**
  * @brief  Mahony融合算法（6轴版）
  * @note   只负责迭代环路，初始化另外提前做
  * @param  q : 四元数（数组）
  * @param  gx,gy,gz : 陀螺仪数据（标准dps单位）
  * @param  ax,ay,az : 加速度数据（标准g单位）
  * @retval None
  */
void MahonyFilter_6Axis(float q[4], 
	                       float gx, float gy, float gz,
												 float ax, float ay, float az)
{
	float normFactor;
	float halfvx, halfvy, halfvz;	// 估计的重力加速度矢量，half表示半值
	float halfex, halfey, halfez; // 误差向量
	float q0 = q[0],q1 = q[1], q2 = q[2], q3 = q[3];
	
	float q0q0 = q[0]*q[0];
	float q0q1 = q[0]*q[1];
	float q0q2 = q[0]*q[2];
	float q0q3 = q[0]*q[3];
	float q1q1 = q[1]*q[1];
	float q1q2 = q[1]*q[2];
	float q1q3 = q[1]*q[3];
	float q2q2 = q[2]*q[2];   
	float q2q3 = q[2]*q[3];
	float q3q3 = q[3]*q[3];  

	// 只在加速度计数据有效时才进行运算
	if((ax != 0.0f) || (ay != 0.0f) || (az != 0.0f)) 
	{
		// 将加速度计得到的实际重力加速度向量v单位化
		normFactor = inv_sqrt(ax * ax + ay * ay + az * az); //向量模长倒数
		ax *= normFactor;
		ay *= normFactor;
		az *= normFactor;        

		// 通过四元数（旋转矩阵）得到理论重力加速度在机体坐标系下的向量值 
		// 注意，这里实际上是矩阵第三列*1/2，在开头对Kp Ki的宏定义均为2*增益，这样处理目的是减少乘法运算量
		halfvx = q1q3 - q0q2;
		halfvy = q0q1 + q2q3;
		halfvz = q0q0 - 0.5f + q3q3;	// q0q0 - q1q1 - q2q2 + q3q3 的优化版（|q| =1）
	
		// 求误差：实际重力加速度向量（测量值）与理论重力加速度向量（估计值）做外积
		halfex = (ay * halfvz - az * halfvy);
		halfey = (az * halfvx - ax * halfvz);
		halfez = (ax * halfvy - ay * halfvx);

		if(twoKi > 0.0f) 
		{
			// 积分过程
			integralFBx += twoKi * halfex * (1.0f / sampleFreq);	
			integralFBy += twoKi * halfey * (1.0f / sampleFreq);
			integralFBz += twoKi * halfez * (1.0f / sampleFreq);

			// 积分项
			gx += integralFBx;
			gy += integralFBy;
			gz += integralFBz;
		}
		else {
			integralFBx = 0.0f;	
			integralFBy = 0.0f;
			integralFBz = 0.0f;
		}

		// 比例项
		gx += twoKp * halfex;
		gy += twoKp * halfey;
		gz += twoKp * halfez;
	}
	
	// 四元数 微分方程
	gx *= (0.5f * (1.0f / sampleFreq));
	gy *= (0.5f * (1.0f / sampleFreq));
	gz *= (0.5f * (1.0f / sampleFreq));
	
	q[0] += (-q1 * gx - q2 * gy - q3 * gz);
	q[1] += (q0 * gx + q2 * gz - q3 * gy);
	q[2] += (q0 * gy - q1 * gz + q3 * gx);
	q[3] += (q0 * gz + q1 * gy - q2 * gx); 
	
	// 单位化四元数
	normFactor = inv_sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
	q[0] *= normFactor;
	q[1] *= normFactor;
	q[2] *= normFactor;
	q[3] *= normFactor;
    
}

迭代得到的四元数，可以需要时用来解欧拉角：

/**
  * @brief  四元数转欧拉角
  * @note   函数内不校验四元数的准确性
  * @param  pIMU : 标准单位制的陀螺仪数据区
  * @retval None
  */
void trans_quaternionToEulerAngle(imu_rawData_t* pIMU)
{
	float q[4];
	for(int i =0; i <4; i++)
	{
		q[i] = pIMU->quaternion_body2Earth[i];
	}
	/* yaw    -pi----pi */
	pIMU->yaw = -atan2(2*q[1]*q[2] + 2*q[0]*q[3], -2*q[2]*q[2] - 2*q[3]*q[3] + 1)* 57.3; 
	/* pitch  -pi/2----pi/2 */
	pIMU->pitch = -asin(-2*q[1]*q[3] + 2*q[0]*q[2])* 57.3;   
	/* roll   -pi----pi  */	
	pIMU->roll =  atan2(2*q[2]*q[3] + 2*q[0]*q[1], -2*q[1]*q[1] - 2*q[2]*q[2] + 1)* 57.3;
}

MEMS陀螺仪工作原理

科里奥利力

首先介绍物理学中的科里奥利力（Coriolis force），是对旋转体系中进行直线运动的质点由于惯性相对于旋转体系产生的直线运动的偏移的一种描述。科里奥利力来自于物体运动所具有的惯性。说白了，其实就是射击游戏里的打提前量，但是目标是旋转的。这玩意的发现来源于炮弹射不准问题，因为地球是有自转的（旋转系），所以在某个时刻瞄准目标打炮时，炮弹只会直着飞出去（相对于宇宙这个静止惯性系），然而在飞的过程中目标跟着地球转走了，就打不准；从地面上受害者的角度来看，就是炮弹自己慢慢拐弯走了？？？🤣聪明的炮兵就因此就请出了科里奥利力来算射击提前量：

$$
\vec F_c = -2m(\vec \omega \times \vec v)
$$
式中，$m$是炮弹的质量，$\vec v$是炮弹的飞行速度，$\vec \omega$是受害者所在旋转系的角速度。

MEMS陀螺仪原理

MEMS陀螺仪也是这么个道理，内部有一个微机电（Micro-Electro-Mechanical）谐振腔，腔内有一个小振子受到交替电压驱动作振动，当外壳带着谐振腔旋转时，里面的振子因为科里奥利力导致振动方向偏移，测量这个方向偏移就可以得到旋转速度了。

MEMS加速度计原理

加速度计的不同类型原理有：

压电式：里面有个质量块以及用来测量的压电元件，加减速时由于压力不同导致压电元件产生的电压不同：
$$
F = ma \\
V = kF
$$
电容式：内部一样有一个质量块，加减速时拉动电容的极板，通过测量电容量即可
热感式：没有任何质量块，它的中央有一个加热体，周边是温度传感器，里面是密闭的气腔，工作时在加热体的作用下，气体在内部形成一个热气团，热气团的比重和周围的冷气是有差异的，通过惯性热气团的移动形成的热场变化让感应器感应到加速度值

初始化（信号链配置）详解

在上面我们给出了使用ICM42688芯片的应用硬件电路和高速SPI驱动的相关工作，以及将陀螺仪数据与加速度计数据进行融合的入门级Mahony算法。在最后补充对ICM42688的初始化过程，在上电/复位后需要依次初始化的有：

软件复位
设置外部时钟源与PLL
配置陀螺仪信号链
配置加速度计信号链
配置中断
配置FIFO
使能供电

所有功能的描述在数据手册上都能找到，这里着重讲解信号链（下图）的配置，因为这关系到采集数据的有效性和可靠性问题。陀螺仪由于上方讨论的振子结构问题，所以输出信号的噪声比较大；因此相比于加速度计的信号链，陀螺仪加入了一级“限波器”（Notch Filter），用来消除振子的工作频率噪声。随后是两级通用的滤波器：抗混叠滤波器（Anti-Alias Filter）和用户接口滤波器（UI Filter），中间夹着一个硬件的偏置补偿。

ICM426XX信号链结构

UIF是由输出数据率（ODR：output data rate）、阶数（Order）、带宽（Bandwidth）或群延迟（Group Delay）三个指标进行配置，主要作用是输出系统需要的数据到寄存器中；因为MEMS传感器的带宽非常大，所以根据奈归斯特采样定理，需要将进入UIF的信号带宽限制为ODR的一半，因此AAF的作用就是这个了。

下图是使用AAF前后的信号波形情况，可以看出对于陀螺仪来说效果非常明显，而加速度计本身噪声就比较少。

启用AAF前后对比

以下是初始化的实现代码：

/**
  * @brief  ICM426XX设备初始化
  *         软复位、配置外部时钟和RTC、配置信号链参数、
  *         加载预准数据、配置中断、配置FIFO
  * @note   需要预先配置好处理器的SPI接口、中断引脚、RTC时钟输出
  * @param  None
  * @retval None
  */
void icm426xx_deviceInit(void)
{
	#ifndef _RAWSIGNAL_ANALYSIS_ICM426XX_
	// 陀螺仪抗混叠滤波器（AAF）带宽，更改配置需查手册中的参数表
	uint16_t gyro_aaf_delta = 2;		/* 84 Hz（3dB） */
	uint16_t gyro_aaf_deltasqr = gyro_aaf_delta*gyro_aaf_delta;
	uint8_t gyro_aaf_bitshift = 13;
	// 加速度计抗混叠滤波器（AAF）带宽
	uint16_t accel_aaf_delta = 2;		/* 84 Hz（3dB） */
	uint16_t accel_aaf_deltasqr = gyro_aaf_delta*gyro_aaf_delta;
	uint8_t accel_aaf_bitshift = 13;
	#endif
	
	imuStatus = ICM426XX_STATUS_NOREADY;	// 标志设备还没准备好
	
	imu.quaternion_body2Earth[0] = 1;	// 初始化四元数
	imu.quaternion_body2Earth[1] = 0;
	imu.quaternion_body2Earth[2] = 0;
	imu.quaternion_body2Earth[3] = 0;
	
	// - 等待设备上电安全完成
	HAL_Delay(3);	// 3ms
	
	// - 复位设备--------------------------------------------------------------------------
	icm426xx_spi_master_write_register(MPUREG_REG_BANK_SEL, 0);	// 切换至寄存器BANK 0
	icm426xx_spi_master_write_register(MPUREG_DEVICE_CONFIG, ICM426XX_DEVICE_CONFIG_RESET_EN);	// 软件复位
	HAL_Delay(2);	// 等待内部复位操作完成，最少1ms
	
	// - 设置外部时钟源（32.768kHz）和RTC时间戳---------------------------------------------
	
	icm426xx_spi_master_write_register(MPUREG_REG_BANK_SEL,1);	// 切换至寄存器BANK 1
	icm426xx_spi_master_write_register(MPUREG_INTF_CONFIG5_B1,
	                                   ICM426XX_INTF_CONFIG5_PIN9_asCLKIN
	                                   ); // 配置9脚为 外部时钟输入模式
	
	icm426xx_spi_master_write_register(MPUREG_REG_BANK_SEL,0);	// 切换至寄存器BANK 0
	// 低功耗模式下加速度计使用内部RC震荡器；使能PLL 和  RTC
	icm426xx_spi_master_write_register(MPUREG_INTF_CONFIG1,
	                                   (ICM426XX_INTF_CONFIG1_ACCEL_LP_CLK_RCOSC | \
																		 ICM426XX_INTF_CONFIG1_RTC_MODE_EN | \
																		 0x01)
	                                   ); 
	// 时间戳设置：时间戳导入可读寄存器、精度1个标准RTC周期、帧同步使能、使能时间戳
	icm426xx_spi_master_write_register(MPUREG_TMST_CONFIG,
	                                   (ICM426XX_TMST_CONFIG_TMST_TO_REGS_EN | \
																		  ICM426XX_TMST_CONFIG_RESOL_16us       | \
																			ICM426XX_TMST_CONFIG_TMST_FSYNC_EN   | \
																			ICM426XX_TMST_CONFIG_TMST_EN
																			)
																		);
	
	// - 设置陀螺仪的信号链参数------------------------------------------------------------
	
	// -- 陀螺仪的陷波器（NotchFilter）参数配置，根据实际情况选用
	//icm426xx_Gyro_ConfigNotchFilter(2,ICM426XX_GYRO_NF_BW_680Hz);
	
	// -- 陀螺仪的抗混叠滤波器（Anti-Alias Filter）配置
	icm426xx_spi_master_write_register(MPUREG_REG_BANK_SEL,1);	// 切换至寄存器BANK 1
	
	#ifndef _RAWSIGNAL_ANALYSIS_ICM426XX_
		icm426xx_spi_master_write_register(MPUREG_GYRO_CONFIG_STATIC3_B1,
																			 (gyro_aaf_delta & 0x3f)
																			);
		icm426xx_spi_master_write_register(MPUREG_GYRO_CONFIG_STATIC4_B1,
																			 (gyro_aaf_deltasqr & 0xff)
																			);
		icm426xx_spi_master_write_register(MPUREG_GYRO_CONFIG_STATIC5_B1,
																			 ( ((gyro_aaf_deltasqr>>8) & 0x0f) | (gyro_aaf_bitshift<<4) )
																			);
																		
		#ifdef _DONT_USE_NOTCHFILTER_ICM426XX_
			// ---- 使能AAF，不使用NF
			icm426xx_spi_master_write_register(MPUREG_GYRO_CONFIG_STATIC2_B1,
																				 (ICM426XX_GYRO_AAF_EN | ICM426XX_GYRO_NF_DIS)
																				);
		#else
			// ---- 使能AAF和NF
			icm426xx_spi_master_write_register(MPUREG_GYRO_CONFIG_STATIC2_B1,
																				 (ICM426XX_GYRO_AAF_EN | ICM426XX_GYRO_NF_EN)
																				);
		#endif
	#else
		// ---- 关闭AAF和NF，从而收集原始数据
		icm426xx_spi_master_write_register(MPUREG_GYRO_CONFIG_STATIC2_B1,
																			 (ICM426XX_GYRO_AAF_DIS | ICM426XX_GYRO_NF_DIS)
																			);
	#endif
																		
	// -- 设置用户接口滤波器（UI Filter）和量程（FSR）
	icm426xx_spi_master_write_register(MPUREG_REG_BANK_SEL,  0);	// 切换至寄存器BANK 0
	// 3阶，200Hz 数据率（ODR），带宽104.8Hz（群延时3.8ms）；量程500dps
	icm426xx_spi_master_write_register(MPUREG_GYRO_CONFIG0,
	                                   (_USER_GYRO_FULLSCALE_ | ICM426XX_GYRO_CONFIG0_ODR_200_HZ)
																		 ); // FSR 和 ODR
	icm426xx_spi_master_write_register(MPUREG_GYRO_CONFIG1,
	                                   (ICM426XX_GYRO_CONFIG_GYRO_UI_FILT_ORD_3RD_ORDER | (2<<1))
																		 );	// 阶数
	icm426xx_spi_master_write_register(MPUREG_ACCEL_GYRO_CONFIG0,
	                                   (ICM426XX_GYRO_ACCEL_CONFIG0_ACCEL_FILT_BW_14 | ICM426XX_GYRO_ACCEL_CONFIG0_GYRO_FILT_BW_14)
																		 ); // 带宽（加速度计和陀螺仪）
				 
	
	// - 设置加速度计的信号链--------------------------------------------------------------
	
	icm426xx_spi_master_write_register(MPUREG_REG_BANK_SEL,2);	// 切换至寄存器BANK2
	// -- 加速度计的抗混叠滤波器（Anti-Alias Filter）配置
	#ifndef _RAWSIGNAL_ANALYSIS_ICM426XX_
		icm426xx_spi_master_write_register(MPUREG_ACCEL_CONFIG_STATIC2_B2,
																			 ((accel_aaf_delta << BIT_ACCEL_AAF_DELT_POS) | \
																			  ICM426XX_ACCEL_AAF_EN)
																			);	// 这里顺带使能了AAF
		icm426xx_spi_master_write_register(MPUREG_ACCEL_CONFIG_STATIC3_B2,
																			 (accel_aaf_deltasqr & 0xff)
																			);
		icm426xx_spi_master_write_register(MPUREG_ACCEL_CONFIG_STATIC4_B2,
																			 ( ((accel_aaf_deltasqr>>8) & 0x0f) | (accel_aaf_bitshift<<4) )
																			);
	#else
		// 关闭AAF，从而收集原始数据
		icm426xx_spi_master_write_register(MPUREG_ACCEL_CONFIG_STATIC2_B2,
																			 ICM426XX_ACCEL_AAF_DIS);
	#endif
																		
	// -- 设置用户接口滤波器（UI Filter）和量程（FSR）
	icm426xx_spi_master_write_register(MPUREG_REG_BANK_SEL,  0);	// 切换至寄存器BANK 0

	// 3阶，200Hz 数据率（ODR），带宽104.8Hz（群延时3.8ms）；量程500dps	
	icm426xx_spi_master_write_register(MPUREG_ACCEL_CONFIG0,
	                                   (_USER_ACCL_FULLSCALE_ | ICM426XX_ACCEL_CONFIG0_ODR_200_HZ)
																		 ); // FSR 和 ODR
	icm426xx_spi_master_write_register(MPUREG_ACCEL_CONFIG1,
	                                   (ICM426XX_ACCEL_CONFIG_ACCEL_UI_FILT_ORD_3RD_ORDER | (2<<1))
																		 );	// 阶数
																		 // NOTE: 这里不操作带宽，因为在陀螺仪配置时一起配了
	
	// - 加载预先校准好的6轴偏置数据-------------------------------------------------------
		// 外部加载！
	
	// - 配置中断 并启动中断输出-----------------------------------------------------------
	
	icm426xx_spi_master_write_register(MPUREG_REG_BANK_SEL,  0);	// 切换至寄存器BANK 0
	// 配置INT1的输出模式：推挽（Push Pull），高电平有效，脉冲型中断输出
	icm426xx_spi_master_write_register(MPUREG_INT_CONFIG,
	                                    (ICM426XX_INT_CONFIG_INT1_DRIVE_CIRCUIT_PP | \
																			 ICM426XX_INT_CONFIG_INT1_POLARITY_HIGH)
																		);
	// 配置中断源
	#ifdef _USER_USE_ICM426XX_FIFO_
	// 可触发的中断源：FIFO阈值、FIFO满
	icm426xx_spi_master_write_register(MPUREG_INT_SOURCE0,
	                                   (BIT_INT_SOURCE0_FIFO_THS_INT1_EN  | \
																		BIT_INT_SOURCE0_FIFO_FULL_INT1_EN
																			 )
																		);
	#else
	// 可触发的中断源：UI滤波器 数据可读
	icm426xx_spi_master_write_register(MPUREG_INT_SOURCE0,
	                                   BIT_INT_SOURCE0_UI_DRDY_INT1_EN
																		);
	#endif
	// 配置清除中断方式
	icm426xx_spi_master_write_register(MPUREG_INT_CONFIG0,
	                                    (ICM426XX_UI_DRDY_INT_CLEAR_ON_STATUS_READ | \
																			 ICM426XX_FIFO_THS_INT_CLEAR_ON_STATUS_READ | \
																			 ICM426XX_FIFO_FULL_INT_CLEAR_ON_STATUS_READ
																			)
																		);
	// 配置中断信号的脉冲宽度：100us宽度脉冲，使能中断有效抑制；最后异步复位中断脚
	icm426xx_spi_master_write_register(MPUREG_INT_CONFIG1,
	                                    (ICM426XX_INT_TPULSE_DURATION_100_US | \
																			 ICM426XX_INT_TDEASSERT_ENABLED | \
																			 ICM426XX_INT_CONFIG1_ASY_RST_ENABLED)
																		);
																			 
	// - 配置FIFO的数据格式 并启动FIFO-----------------------------------------------------
	
	icm426xx_spi_master_write_register(MPUREG_REG_BANK_SEL,  0);	// 切换至寄存器BANK 0
	
	//复位FIFO
	icm426xx_spi_master_write_register(MPUREG_FIFO_CONFIG,
	                                   ICM426XX_FIFO_CONFIG_MODE_BYPASS
																		);
	
	#ifdef _USER_USE_ICM426XX_FIFO_
	//配置FIFO的无效数据报告方式、计数方式、数据字端
	//// -32768或-32767表示无效数据； 按包计数； FIFO计数器和传感器数据包按“小字端”组织
	icm426xx_spi_master_write_register(MPUREG_INTF_CONFIG0,
	                                    (ICM426XX_INTF_CONFIG0_FIFO_SREG_INVALID_IND_EN | \
																			 ICM426XX_INTF_CONFIG0_FIFO_COUNT_REC_RECORD | \
																			 ICM426XX_INTF_CONFIG0_FIFO_COUNT_LITTLE_ENDIAN | \
																			 ICM426XX_INTF_CONFIG0_DATA_LITTLE_ENDIAN
																			 )
																		);
	
	//配置FIFO包格式为 “Packet 3”：Header（1Byte）+Accel（6Byte）+Gyro（6Byte）+Temp（1Byte）+Time（2Byte）
	/* 允许断包读取、使能包数量过多中断、不用高精度扩展、使能时间戳、使能温度数据、使能陀螺仪、使能加速度计*/
	icm426xx_spi_master_write_register(MPUREG_FIFO_CONFIG1,
	                                    (ICM426XX_FIFO_CONFIG1_RESUME_PARTIAL_RD_EN | \
																			 ICM426XX_FIFO_CONFIG1_WM_GT_TH_EN           | \
																			 ICM426XX_FIFO_CONFIG1_HIRES_DIS             | \
																			 ICM426XX_FIFO_CONFIG1_TMST_FSYNC_EN         | \
																			 ICM426XX_FIFO_CONFIG1_TEMP_EN               | \
																			 ICM426XX_FIFO_CONFIG1_GYRO_EN               | \
																			 ICM426XX_FIFO_CONFIG1_ACCEL_EN) 
																		 );
	// FIFO计数报警值（WaterMark）为4个 包！
	icm426xx_spi_master_write_register(MPUREG_FIFO_CONFIG2,   4);
	icm426xx_spi_master_write_register(MPUREG_FIFO_CONFIG2+1, 0);
	
	//配置FIFO为连续数据流模式，（满时新数据会覆盖旧数据）
	icm426xx_spi_master_write_register(MPUREG_FIFO_CONFIG,
	                                   ICM426XX_FIFO_CONFIG_MODE_STOP_ON_FULL
																		);
	#endif
	
	// - 电源管理配置（给陀螺仪和加速度计上电）--------------------------------------------
	
	icm426xx_spi_master_write_register(MPUREG_REG_BANK_SEL,  0);	// 切换至寄存器BANK 0
	icm426xx_spi_master_write_register(MPUREG_PWR_MGMT_0,
	                                    (ICM426XX_PWR_MGMT_0_TEMP_EN | \
																			 ICM426XX_PWR_MGMT_0_IDLE_EN | \
																			 ICM426XX_PWR_MGMT_0_GYRO_MODE_LN | \
																			 ICM426XX_PWR_MGMT_0_ACCEL_MODE_LN
																			 )
																		);
	HAL_Delay(50);	// 官方要求陀螺仪切换模式后，必须等最少45ms，这里保险起见50ms
	
	
	// - 配置完成------------------------------------------------------------------------

	icm426xx_spi_master_write_register(MPUREG_REG_BANK_SEL,  0);	// 切换至寄存器BANK 0
	imuStatus = ICM426XX_STATUS_NORMAL;
	return;
	 
}

考虑到系统在工作的过程中，环境因素的变化，因此将加载偏置的接口单独放在初始化接口的外部：

/**
  * @brief  加载各轴偏置量
  * @note   偏置量在复杂环境下要不断调整
  * @param  pOffset : 加载的偏置量数据区指针（遵照ICM426XX寄存器格式）
  * @retval None
  */
void icm426xx_load6AxisOffset(uint8_t pOffset[9])
{
	icm426xx_spi_master_write_register(MPUREG_REG_BANK_SEL, 4);	// 切换至寄存器BANK 0
	
	for(int addr=0; addr < 9; addr ++)
	{
		icm426xx_spi_master_write_register(MPUREG_OFFSET_USER_0_B4 + addr,
		                                   pOffset[addr]);
	}
	
	icm426xx_spi_master_write_register(MPUREG_REG_BANK_SEL, 0);	// 切换至寄存器BANK 0
}

/**
  * @brief  求解各轴偏置量（平均法）
  * @note   在接收数据中断中调用
  * @param  pOffset : 存储解算出的偏置数据寄存器格式
  * @retval None
  */
void icm426xx_getAverageOffset(uint8_t pOffset[9])
{
	for (int addr =0; addr < 3; addr++)
	{
		// 求平均量
		gyroAxisSum[addr] = gyroAxisSum[addr] >>9 ;	// log2(512) = 9
		accelAxisSum[addr] = accelAxisSum[addr] >>9;
		
		// NOTE:陀螺仪偏置精度固定为 (1/32dps)/LSB（±64dps补偿范围），这里做比例变换
		gyroAxisSum[addr] = -(gyroAxisSum[addr] * 500) >> (6+4);	// log2(64)=6是目标FSR，另外的4是16位精度到12位精度的损失
		
	}
	
	// NOTE:加速度计偏置精度为：(0.5mg)/LSB （±1g补偿范围）
	accelAxisSum[0] = -(accelAxisSum[0] * 4) >> 4;		// X轴补偿为 0
	accelAxisSum[1] = -(accelAxisSum[1] * 4) >> 4;		// Y轴补偿为 0
	accelAxisSum[2] = -((accelAxisSum[2] * 4) >> 4) + 1000;		// Z轴补偿为 1g

	
	pOffset[0] = gyroAxisSum[0]  & 0x00ff;	// X轴陀螺仪低位
	pOffset[1] = ((gyroAxisSum[1]& 0x0f00) >> 4) | ((gyroAxisSum[0]&0x0f00) >> 8);	// Y和X的高位
	pOffset[2] = gyroAxisSum[1]  & 0x00ff;	// Y轴陀螺仪低位
	pOffset[3] = gyroAxisSum[2]  & 0x00ff;	// Z轴陀螺仪低位
	
	pOffset[4] = ((accelAxisSum[0]& 0x0f00) >> 4) | \
	             ((gyroAxisSum[2] & 0x0f00) >> 8);	// X轴加速度计高位，Z轴陀螺仪高位
	
	pOffset[5] = accelAxisSum[0]  & 0x00ff;	// X轴加速度计低位
	pOffset[6] = accelAxisSum[1]  & 0x00ff;	// Y轴加速度计低位
	pOffset[7] = ((accelAxisSum[2]& 0x0f00) >> 4) | ((accelAxisSum[1]&0x0f00) >> 8);	// Z和Y的高位
	pOffset[8] = accelAxisSum[2]  & 0x00ff;	// Z轴加速度计低位
	
}

若使用了中断引脚，则可以按如下进行中断处理：

/**
  * @brief  中断解析回调函数
  * @note   在MCU中断引脚对应的EXTI函数中调用
  *         只需调用 icm426xx_deviceInit() 接口该函数即可自动触发
  * @param  None
  * @retval None
  */
void icm426xx_callback_interrupt1(void)
{
	uint8_t regVal_int_status;
	
	// - 检验芯片状态
	if(imuStatus != ICM426XX_STATUS_NORMAL)
		return;	// 芯片不工作退出
	
	// - 读取中断状态/标志寄存器
	icm426xx_spi_master_read_register(MPUREG_INT_STATUS,
	                                 &regVal_int_status, 1);
	
	#ifdef _USER_USE_ICM426XX_FIFO_		// 仅在使用FIFO的情况下，使用这两个中断
	if(regVal_int_status & BIT_INT_STATUS_FIFO_FULL)
	{
		/* 中断优先级最高：处理FIFO满状态 */
		icm426xx_readFIFOPacket(NULL);	// 按需要调用 包处理回调函数
	}
	else if(regVal_int_status & BIT_INT_STATUS_FIFO_THS)
	{
		/* 处理FIFO达到阈值状态 */
		icm426xx_readFIFOPacket(NULL);	// 按需要调用 包处理回调函数
	}
	else
	#endif
		if(regVal_int_status & BIT_INT_STATUS_DRDY)
	{
		/* 数据准备好了 */
		icm426xx_getRawData(&imuData);	// 读取原始数据
		
		if (sumNum < 512)	// 存储采样数据
		{
			// --- 陀螺仪采样数据 累加
			transCache.byte[0] = imuData.gyro_x_L;   transCache.byte[1] = imuData.gyro_x_H;
			gyroAxisSum[0] += transCache.getSign;
			
			transCache.byte[0] = imuData.gyro_y_L;   transCache.byte[1] = imuData.gyro_y_H;
			gyroAxisSum[1] += transCache.getSign;
			
			transCache.byte[0] = imuData.gyro_z_L;   transCache.byte[1] = imuData.gyro_z_H;
			gyroAxisSum[2] += transCache.getSign;
			
			// --- 加速度计采样数据 累加
			transCache.byte[0] = imuData.accel_x_L;   transCache.byte[1] = imuData.accel_x_H;
			accelAxisSum[0] += transCache.getSign;
			
			transCache.byte[0] = imuData.accel_y_L;   transCache.byte[1] = imuData.accel_y_H;
			accelAxisSum[1] += transCache.getSign;
			
			transCache.byte[0] = imuData.accel_z_L;   transCache.byte[1] = imuData.accel_z_H;
			accelAxisSum[2] += transCache.getSign;
			
			sumNum ++;
			if (sumNum >= 512)
			{
				icm426xx_getAverageOffset(Offset6Axis);	// 求解偏置
				icm426xx_load6AxisOffset(Offset6Axis);	// 加载偏置
			}
		}
		else{
			icm426xx_trans_RawDataToStandardData(&imuData, &imu);	// 解析为标准单位
//			print_IMU_data(&imu);
			
			// Mahony滤波器
			MahonyFilter_6Axis(imu.quaternion_body2Earth,
			                   imu.gyro_x, imu.gyro_y, imu.gyro_z,
			                   imu.accel_x, imu.accel_y, imu.accel_z);
			trans_quaternionToEulerAngle(&imu);	// 把四元数转为欧拉角
			
			// 打印数据
			print_AHRS_data(&imu);
		}
		
		#ifdef _RAWSIGNAL_ANALYSIS_ICM426XX_
		/*记录数据*/
		// 注意：测量时应该把设备放在水平、静止的台上
		#endif
		
	}
	
	if(regVal_int_status & BIT_INT_STATUS_RESET_DONE)
	{
		/* 复位完成 */
	}
	if(regVal_int_status & BIT_INT_STATUS_PLL_RDY)
	{
		/* PLL准备完成 */
	}
	if(regVal_int_status & BIT_INT_STATUS_AGC_RDY)
	{
		/* AGC控制器准备完成 */
	}
	
}

参考文献

Mahony滤波算法原论文
AHRS项目：一个包含了常用定位、姿态算法的Python开源工具包
全权.多旋翼飞行器设计与控制[M]，电子工业出版社，2018

Donate

Copyright： Copyright is owned by the author. For commercial reprints, please contact the author for authorization. For non-commercial reprints, please indicate the source.