PX4加速度计传感器校准原理

理想情况下，传感器的读数与实际物理量相同，但由于一些随机因素的影响，传感器测量到的数据与实际数据存在偏差，且偏差不停变化。偏差大体分为两种，零位偏差和标度偏差。对于一个传感器来说，实际物理量=K*传感器度数+B。理想状态下，K=1,B=0,但是实际中K与B的值会不停的随机变化，K的改变引起的偏差叫标度偏差，B的改变引起的偏差叫零位偏差。传感器校准，就是通过对已知物理量的测量，求出K与B的值，并将这两个值传入传感器驱动，使得传感器驱动可以解算出准确的物理量。

“六面较准法”：Pixhawk的加速度计是3轴的，也就是在x,y,z三个方向上测得三个加速度，所以存在三个y=kx+b的公式，根据两点确定一条直线的原则，每个轴都要测量到两个不同的已知值，所以将每个轴垂直向下采集一个负G数据，垂直向上采集一个正G数据，从而算出每个轴的k和b值。

入口函数do_accel_calibration

设置驱动中的校准参数为默认值，”px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);“的含义是将校准参数的默认值写入加速度计驱动，默认三个scale是1，三个offset是0。

Calibration_routines.cpp文件中的749行：do_accel_calibration_measurements -> calibrate_from_orientation -> detect_orientation函数的返回值就是机头朝向，六个朝向都会进行一次校准数据采集。calibration_worker函数指针指向accel_calibration_worker函数accel_calibration_worker -> read_accelerometer_avg函数负责采集某个机头朝向的加速度计值，三个轴各取3000个数据平均，然后将平均数从传感器坐标系转换到机体坐标系，将转换后的结果存放到worker_data -> accel_ref[3][6][3]。

零偏计算

六个方向的数据采集完毕后，由calculate_calibration_values函数完成校准结果的计算，offset的计算就是取同一个轴上的最大最小两个数据的平均值。

比例计算

 将机体系x轴朝上，y轴朝上，z轴朝上的数据减去各个轴的offset后存放到3*3矩阵mat_A的三个行中，mat_A是机体坐标系的值，mat_A的三行代表三个不同的朝向，mat_A的三列代表在某个朝向减去offset后的X、Y、Z值。

 有未知3*3矩阵Accel_T:

      mat_A * Accel_T  = [g      0      0]  //X轴朝上

                                      |0     g       0 | //y轴朝上

                                      [0      0      g]  //z轴朝上

 mat_invert3函数用伴随矩阵法求出mat_A的逆阵，等式左右两边同时左乘mat_A^-1解出Accel_T。如果飞机与加速度计的坐标系重合，则Accel_T中的主对角线的三个元素就是加速度计三个轴的scale信息。

==================================================================================================================================================

IMU与MAG的校准步骤：

1.校准数据的设置：
commander（地面站）经过计算之后通过param_set()来设置校准数据，路径为src/modules/commander下的各个calibration文件，上电后rcS脚本执行sensors start之后会执行commder start来启动commander.cpp，commander.cpp进而调用各个calibration文件，进而进行校准设置。

2.校准数据的获取：
在sensors.cpp中的task_main()循环中还有一个poll函数是parameter_update_poll()，这个poll函数获取了各个sensor的offset和scale来供各个sensor驱动中measure校准时使用，它使用param_get()来获得。

二.陀螺仪的校准：
1.do_gyro_calibration()：
commander.cpp中commander_main()中执行do_gyro_calibration()。

2.初始化：
gyro_calibration.cpp中do_gyro_calibration()初始化gyro_scale_zero中各个offset赋值为0，各个scale赋值为1，并将gyro_scale_zero各个值通过param_set()赋值给对应参数值。

3.gyro_calibration_worker()：
gyro_calibration.cpp中do_gyro_calibration()执行gyro_calibration_worker()。

4.校准：
gyro_calibration.cpp中gyro_calibration_worker()中：
（1）循环执行5000次。
（2）每次循环执行px4_poll()进行延时处理。
（3）每次循环均依次对各个gyro进行如下校准。
（3）orb_check()检查更新。
（4）orb_copy()接受数据赋值相应坐标轴offset，将校准变量gyro_scale[s]加等gyro测量值gyro_report。
（5）相应坐标轴offset分别除以校准计数值5000。

5.赋值：
gyro_calibration.cpp中执行param_set_no_notification()将gyro_scale[s]相应坐标轴的offset赋值给对应参数值。

三.加速度计的校准：
1.do_accel_calibration()：
commander.cpp中commander_main()中执行do_accel_calibration()。

2.初始化：
accelerometer_calibration.cpp中do_accel_calibration()初始化accel_scale中各个offset赋值为0，各个scale赋值为1，并将accel_scale各个值通过param_set()赋值给对应参数值。

3.校准：
accelerometer_calibration.cpp中do_accel_calibration()执行do_accel_calibration_measurements。
（1）执行calibrate_from_orientation()进行六个方向测量accel的offset值并赋值accel_ref[max_accel_sens][detect_orientation_side_count][3]中：
 [1] 根据方向的转换，循环六次。
 [2] 每次循环执行accel_calibration_worker()。
 [3] accel_calibration_worker()执行read_accelerometer_avg()。
 [4] read_accelerometer_avg函数中：
   a：循环执行750次。
   b：每次循环执行px4_poll()进行延时处理。
   c：并对各个accel进行如下校准。
   d：orb_check()检查更新。
   e：orb_copy()接受数据赋值相应坐标轴offset，将校准变量accel_sum[s]加等accel测量值arp。
   f：相应坐标轴offset分别除以校准计数值750赋值给accel_ref。
（2）执行calculate_calibration_values()，根据accel_ref计算偏移offsets和转移矩阵transform matrix，分别赋值accel_offs和accel_T。

4.计算offset：
根据accel_offs和accel_T值结合board_rotation_t计算accel_scale[s]相应坐标轴的offset。

5.赋值：
accelerometer_calibration.cpp中执行param_set_no_notification()将accel_scale[s]相应坐标轴的offset赋值给对应参数值。

四.地磁的校准：
1.do_mag_calibration()：
commander.cpp中commander_main()中执行do_mag_calibration()。

2.校准：
mag_calibration.cpp的do_mag_calibration()中
（1）初始化mscale_null中各个offset赋值为0，各个scale赋值为1。
（2）执行mag_calibrate_all(mavlink_log_pub)同时对所有mag校准。
（3）所有测量点x，y，z轴数据初始化。
（4）订阅mag消息。
（5）限制点数据测量间隔时间。
（6）进行mag点数据进行采集，执行calibrate_from_orientation，赋值worker_data。
 [1] 对六个面分别进行检测，检测过程如下：
 [2] 执行calibration_worker指针函数，即mag_calibration_worker函数：
   a：旋转检测：任意方向旋转即可，仅需要大致相交的两个轴，通过对陀螺数据积分检测是否旋转。
   b：对该面进行采集calibration_points_perside个mag数据存储至worker_data.x，worker_data.y，worker_data.z中。
（7）球体拟合最小二乘mag数据，调用sphere_fit_least_squares()得到mag偏移sphere_x，sphere_y，sphere_z，sphere_radius。
（8）sphere_x，sphere_y，sphere_z分别赋值mscale.x_offset，mscale.y_offset，mscale.z_offset。
（9）通过mscale.x_offset，mscale.y_offset，mscale.z_offset对偏移参数赋值。