1. 源由

前面研读了IMU如何通过front-end/back-end获取传感数据。ArduPilot在对模型IMU最初的校准就是在传感数据获取的基础上，通过AP_InertialSensor应用类完成的。

在此，对该类的实际原理和设计，以及代码进行深入的了解。

2. AP_InertialSensor类

该应用类，重要的入口函数主要如下：

init：初始化
periodic：日志记录
update：front-end数据更新

2.1 init

针对硬件进行probe和初始化，详见：ArduPilot开源代码之AP_InertialSensor_Backend。

AP_Vehicle::setup
 └──> init_ardupilot
     └──> Copter::startup_INS_ground
         └──> ins.init
             ├──> _start_backends
             │   └──> detect_backends
             │       └──> HAL_INS_PROBE_LIST --> HAL_INS_PROBE1;HAL_INS_PROBE2
             │           ├──> ADD_BACKEND(AP_InertialSensor_BMI270::probe(*this,hal.spi->get_device("bmi270_1"),ROTATION_ROLL_180_YAW_90))
             │           └──> ADD_BACKEND(AP_InertialSensor_BMI270::probe(*this,hal.spi->get_device("bmi270_2"),ROTATION_PITCH_180))
             └──> batchsampler.init

2.2 periodic

通过SCHED_TASK_CLASS进行ArduPilot任务调度。

SCHED_TASK_CLASS(AP_InertialSensor,    &copter.ins,                 periodic,       400,  50, 123),

AP_InertialSensor::periodic
 └──> <AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED> batchsampler.periodic
     └──> <_sensor_mask == 0> return  //没有IMU，也就不需要记录日志数据了
         └──> push_data_to_log

2.3 update

通过FAST_TASK_CLASS进行ArduPilot任务调度。

// update INS immediately to get current gyro data populated
FAST_TASK_CLASS(AP_InertialSensor, &copter.ins, update),

AP_InertialSensor::update
 ├──> wait_for_sample
 ├──> [mark sensors unhealthy and let update() in each backend mark them healthy via _publish_gyro() and _publish_accel()]
 ├──> _backends[i]->update() //更新IMU数据
 ├──> [记录2秒_accel_startup_error_count/_gyro_startup_error_count]
 ├──> [更新have_zero_accel_error_count/have_zero_gyro_error_count]
 ├──> [更新_gyro_healthy/_accel_healthy]
 ├──> [set primary to first healthy accel and gyro; _primary_gyro/_primary_accel]
 └──> _last_update_usec = AP_HAL::micros()

3. 重要应用方法

3.1 BatchSampler::push_data_to_log

AP_InertialSensor::BatchSampler::push_data_to_log
 ├──> <!initialised> return
 ├──> <_sensor_mask == 0> return
 ├──> <data_write_offset - data_read_offset < samples_per_msg> return
 ├──> <AP_HAL::millis() - last_sent_ms < (uint16_t)push_interval_ms> return
 ├──> <(AP_Logger *logger = AP_Logger::get_singleton()) == nullptr> return
 ├──> [send isb header] Write_ISBH
 ├──> [send a data packet] Write_ISBD
 ├──> data_read_offset += samples_per_msg
 ├──> <data_read_offset >= _real_required_count> 
 └──> [rotate to next instance] rotate_to_next_sensor

3.2 wait_for_sample

AP_InertialSensor::wait_for_sample
 ├──> <_have_sample> return
 ├──> <第一次进入等待采样> 直接跳转到[开始检查采样数据]
 ├──> <接近下次采样时间> 等待一段时差；更新下次采样时间；然后进入[开始检查采样数据]
 ├──> <overshot小于 1/8 loop时间> 更新下次采样时间；直接进入[开始检查采样数据]
 ├──> <overshot大于 1/8 loop时间> 以当前时刻为基点更新下次采样时间；直接进入[开始检查采样数据]
 └──> [开始检查采样数据]
     ├──> [标记需要需要获取采样数据对应的gyro bit位]
     ├──> [标记需要需要获取采样数据对应的acc bit位]```
     ├──> <在1/3 loop时间内等待到了有效数据> 结束本次采样等待
     ├──> <1/3 loop时间超时，不再等待其他有效数据> 结束本次采样等待
     └──> 更新_delta_time/_last_sample_usec时间

3.2 calibrate_gyros

除了上面能够显而易见看到的几个功能以外，还有一个GCS出发的IMU校准功能。

注：具体6面校准原理和逻辑我们后续研读，这里先把出发校准的API接口暴露出来。有兴趣的朋友可以先看看整个逻辑实现

AP_InertialSensor::calibrate_gyros
 ├──> init_gyro
 ├──> <!gyro_calibrated_ok_all()> return false
 └──> AP::ahrs().reset_gyro_drift()

下面是GCS_MAVLINK应用类实现的出发校准。
在这里插入图片描述

4. 总结

AP_InertialSensor应用类，总的来说完成以下功能：

IMU自动芯片侦测&初始化
IMU数据日志记录
IMU的font-end数据更新
IMU校准

本文主要针对每个功能的入口以及AP_InertialSensor应用类进行的介绍，比较具体的功能代码细节，如有时间将和大家一起研读。

5. 参考资料

【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码框架
【3】ArduPilot飞控之ubuntu22.04-SITL安装
【4】ArduPilot飞控之ubuntu22.04-Gazebo模拟
【5】ArduPilot飞控之Mission Planner模拟
【6】ArduPilot飞控AOCODARC-H7DUAL固件编译
【7】ArduPilot之开源代码Library&Sketches设计
【8】ArduPilot之开源代码Sensor Drivers设计
【9】ArduPilot之开源代码基础知识&Threading概念
【10】ArduPilot之开源代码UARTs and the Console使用
【11】ArduPilot飞控启动&运行过程简介
【11】ArduPilot之开源代码Task介绍
【12】ArduPilot开源代码之AP_Param
【13】ArduPilot开源代码之AP_Scheduler
【14】ArduPilot开源代码之AP_VideoTX
【15】ArduPilot开源代码之AP_InertialSensor_Backend

ArduPilot开源代码之AP_InertialSensor