PX4原始碼分析8_PX4的sensor校準

一.綜述：

sensor的校準過程分為兩部分，首先需要先通過地面站進行校準設定，然後通過校準資料的更新來獲取最新的校準資料。

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對偏移引數賦值。