一.综述：
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对偏移参数赋值。
网友评论