六轴传感器icm20602的自检和校准学习

       六轴传感器在当今智能穿戴和定位导航产品中被广泛应用，而六轴传感器中做的最好的要属InvenSense公司的产品了，本文结合官方提供的mpu6500驱动程序和icm20602驱动程序，讲解icm20602的自检和校准流程，并将其移植到icm20602上。

首先来张icm20602芯片性能介绍图：

        集成3轴加速度计和3轴陀螺仪，陀螺仪量程范围可选+/-250dps,+/-500dps,+/-1000dps和+/-2000dps.加速度计量程范围可选+/-2g,+/-4g,+/-8g和+/-16g。相较mpu6500在性能误差上有很大提升，InvenSense推荐在新的设计中使用ICM系列。工作电压1.71V到3.45V，可以设计工作电压1.8V，2.5V或者3.3V。

       相较MPU6500，关于ICM20602的开发资料网上很少，详细的应用讲解就更找不到几篇了。在使用上MPU6500和ICM20602相类似，所以可以参考MPU6500来使用ICM20602，但要注意两者之间有个重大的不同：ICM20602没有集成MPU！所以想要计算欧拉角，需要得到原始加计和陀螺仪数据，利用InvenSense提供的算法库来实现四元数的计算，从而解算出欧拉角。

    在四元数解算前，首先要进行芯片自检和校准：自检的目的是验证芯片的机械和电气特性是否正常；校准是为了获得满足当前设计的参考系。自检流程如下：

1、芯片处于no motion状态，读取加速度计和陀螺仪输出数据，计算平均值；

2、GYRO_CONFIG寄存器和ACCEL_CONFIG寄存器使能自检，此时芯片内部会自动模拟外力施加给加速度计和陀螺仪，此时输出的加速度计和陀螺仪数据相较不使能自检状态会有所变化，计算平均值；

3、读取自检寄存器输出数据，获得厂家出厂时固化的OTP；

4、最后利用公式计算自检是否通过，芯片手册中给出的公式如下：

实际上我们并不需要直接使用该公式，计算太复杂了。可以直接使用驱动程序中给出的转化表来计算：

具体使用方法见下文。通过以上步骤，就实现了芯片的自检，接下来进行校准：

在使用六轴传感器时通常都有一个初始状态，有的是芯片平放，有的是芯片竖立，芯片摆放的不同初始加速度是不同的，而芯片接下来的运动是相对初始状态而言。举个例子：如果六轴传感器在一辆行驶的骑车上，那么它的初始状态的加速度和汽车一致，如果选择的参考系是大地，那么初始加速度就是0，如果选择汽车作为参考系，那么初始加速度就是汽车的运行加速度，六轴传感器需要以该加速度作为初始值，所以就需要将此时的加速度值写入偏移寄存器。加速度计的输出值都是相对偏移寄存器而言的。总结如下：厂家出厂时会像XA_OFFS寄存器中预先写入校准值，所以读取初始状态下加速度计输出值，计算平均值后，需要读取出厂时的校准值，减去平均值后写入XA_OFFS寄存器。

对于陀螺计的校准，MPU6500的DMP提供了一个特性：选择DMP_FEATURE_GYRO_CAL后，芯片在8s内保持静止，陀螺仪自动完成校准。还有一种方式是和加速度计一样，将初始no motion状态读取到的陀螺仪数据求平均，用出厂时的校准值相减后写入XG_OFFS偏移寄存器。ICM20602只能选择后一种方式。

以上是流程介绍，接下来看驱动程序的具体实现：

1、自检程序实现流程：

run_self_test()函数就实现了自检和校准的全过程。

 在run_self_test()函数中有如上代码，mpu_run_6500_self_test用于实现芯片自检；

 在mpu_run_6500_self_test中，首先调用get_st_6500_biases()函数来计算加计和陀螺仪不使能自检情况下输出的平均值。

 get_st_6500_biases()函数中，首先读取packet_count个数据，将每个轴的数据各自相加，保存在accel和gyro数组中。

 

 求取每个轴的平均值，由于得到的是浮点数，所以放大65536倍变成整数。

然后再在mpu_run_6500_self_test中再次调用get_st_6500_biases函数，获得使能自检后的加计和陀螺仪输出，保存在gyro_st和accel_st数组中。

在mpu_run_6500_self_test中调用accel_6500_self_test和gyro_6500_self_test函数，实现加计和陀螺仪自检。

首先来看一下mpu_run_6500_self_test函数，读取XA_ST_DATA寄存器的值，获得厂家固化的OTP。

利用转换好的mpu_6500_st_tb参数来代替公式计算，mpu_6500_st_tb的值和上文截图的数组值相同。

将使能自检时得到的加速度计值和不使能自检时得到的加速度计值相减，保存到st_shift_cust数组中，利用上文得到的ct_shift_prod，计算st_shift_ratio，取绝对值和max_accel_var相比，小于则自检通过，大于则自检不通过。例程中max_accel_var取值0.14.

陀螺仪的自检过程与此相似，不再赘述。至此，加速度计和陀螺仪的自检完成，返回到run_self_test(）函数，继续查看校准的过程：

gyro和accel寄存器中存储着之前得到的加计和陀螺仪输出数据的平均值。mpu_set_gyro_bias_reg和mpu_set_accel_bias_6500_reg函数完成校准工作。

以陀螺仪校准为例，首先读取偏移寄存器中厂家出厂时写入的校准值。用偏移寄存器中的值和之前得到的陀螺仪输出数据相减，写入到偏移寄存器中，校准完成。加速度计的校准过程与此相同，在此不再赘述。

ICM20602的自检和校准过程与此类似，绝大多数都相同，只是更换了函数名和变量名。

inv_icm20602_run_selftest函数用来实现加速度计和陀螺仪的自检和校准。ICM20602关于四元数的解算，InvenSense提供了集成库libAlgoInvn.a，后续博客会详细讲解四元数的解算流程。