PX4：【传感器校准】

sensor的校准

校准步骤：

1. 首先通过地面站QGC进行校准，QGC将校准参数设置到sh文件中。
2. 此后再基于QGC的校准参数，不断获取校准数据的更新。

文件目录：

在src/commander文件夹下
包含了 accelerometer_calibration.cpp/.h 多个传感器的校正
airspeed_calibration.cpp/.h
gyro_calibration.cpp/.h
mag_calibration.cpp/.h

代码入口：

Commader.cpp

• Commander::run（）
  — commander_low_prio_loop（）
  【 订阅了vehicle_command 】根据cmd各param值，运行相应的算法：
  (cmd.param1) == 1 ——> calib_ret = do_gyro_calibration(&mavlink_log_pub);
  (cmd.param2) == 1 ——> calib_ret = do_mag_calibration(&mavlink_log_pub);
  (cmd.param5) == 1 ——> calib_ret = do_accel_calibration(&mavlink_log_pub);
  (cmd.param5) == 2 ——> calib_ret = do_level_calibration(&mavlink_log_pub);
  …

求解模型

accelerometer_calibration.cpp中进行旋转的校正并求得加速度三轴偏移量

• accel_corr_ref[6][3] =
  [ g 0 0 ] // nose up
  | -g 0 0 | // nose down
  | 0 g 0 | // left side down
  | 0 -g 0 | // right side down
  | 0 0 g | // on back
  [ 0 0 -g ] // level

• accel_raw_ref [ 6] [ 3]

• accel_corr_ref[i] = accel_T * (accel_raw_ref[i] - accel_offs), i = 0…5

计算公式

在accelerometer_calibration.cpp的 calculate_calibration_values() 函数中完成加速度计旋转矩阵与偏移量的计算
先求accel_offs

• accel_offs[i] = (accel_raw_ref [ i2][ i] + accel_raw_ref[i2+1][i]) / 2 , i = 0…2

再求accel_T

• 9 未知量

• 需要使用 6个测量方程中的3个 measurements -> 3 * 3 = 9 equations

• accel_corr_ref[i2] = accel_T * (accel_raw_ref[i2] - accel_offs), i = 0…2
  accel_corr_ref[j2][i] = accel_T[i] * (accel_raw_ref[j2] - accel_offs), j = 0…2

A * x = b

• x =
  [ accel_T[0][i] ]
  | accel_T[1][i] |
  [ accel_T[2][i] ]

• b =
  [ accel_corr_ref[0][i] ]
  | accel_corr_ref[2][i] |
  [ accel_corr_ref[4][i] ]

• a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0;2;4, j = 0…2

• A =
  [ a[0][0] a[0][1] a[0][2] ]
  | a[2][0] a[2][1] a[2][2] |
  [ a[4][0] a[4][1] a[4][2] ]

• accel_T = A^-1 * g

• g = 9.80665