1. 源由

惯性传感器是飞控关于姿态最重要的一个传感器。从复杂度角度看，除了数据融合外，算是逻辑上最为复杂的。

本章我们将重点了解下ArduPilot是如何来获取该传感器的数据，并如何处理的。

2. 设计

惯性传感器遵循Sensor Drivers的front-end/back-end分层设计。

鉴于传感器硬件种类很多，因此在该现实情况下，惯性传感器对back-end进一步抽象了AP_InertialSensor_Backend类。

从该AP_InertialSensor_Backend类模版上，可以主要关注以下几个方法：

• virtual void start() { }
• virtual bool update() = 0; /* front end */
• device driver IDs

class AP_InertialSensor_Backend
{
public:AP_InertialSensor_Backend(AP_InertialSensor &imu);AP_InertialSensor_Backend(const AP_InertialSensor_Backend &that) = delete;// we declare a virtual destructor so that drivers can// override with a custom destructor if need be.virtual ~AP_InertialSensor_Backend(void) {}/** Update the sensor data. Called by the frontend to transfer* accumulated sensor readings to the frontend structure via the* _publish_gyro() and _publish_accel() functions*/virtual bool update() = 0; /* front end *//** optional function to accumulate more samples. This is needed for drivers that don't use a timer to gather samples*/virtual void accumulate() {}/** Configure and start all sensors. The empty implementation allows* subclasses to already start the sensors when it's detected*/virtual void start() { }/** Return an AuxiliaryBus if backend has another bus it is able to export*/virtual AuxiliaryBus *get_auxiliary_bus() { return nullptr; }/** Return the unique identifier for this backend: it's the same for* several sensors if the backend registers more gyros/accels*/int16_t get_id() const { return _id; }//Returns the Clip Limitfloat get_clip_limit() const { return _clip_limit; }// get a startup banner to output to the GCSvirtual bool get_output_banner(char* banner, uint8_t banner_len) { return false; }#if HAL_EXTERNAL_AHRS_ENABLEDvirtual void handle_external(const AP_ExternalAHRS::ins_data_message_t &pkt) {}
#endif/*device driver IDs. These are used to fill in the devtype fieldof the device ID, which shows up as INS*ID* parameters tousers. The values are chosen for compatibility with existing PX4drivers.If a change is made to a driver that would make existingcalibration values invalid then this number must be changed.*/enum DevTypes {DEVTYPE_BMI160       = 0x09,DEVTYPE_L3G4200D     = 0x10,DEVTYPE_ACC_LSM303D  = 0x11,DEVTYPE_ACC_BMA180   = 0x12,DEVTYPE_ACC_MPU6000  = 0x13,DEVTYPE_ACC_MPU9250  = 0x16,DEVTYPE_ACC_IIS328DQ = 0x17,DEVTYPE_ACC_LSM9DS1  = 0x18,DEVTYPE_GYR_MPU6000  = 0x21,DEVTYPE_GYR_L3GD20   = 0x22,DEVTYPE_GYR_MPU9250  = 0x24,DEVTYPE_GYR_I3G4250D = 0x25,DEVTYPE_GYR_LSM9DS1  = 0x26,DEVTYPE_INS_ICM20789 = 0x27,DEVTYPE_INS_ICM20689 = 0x28,DEVTYPE_INS_BMI055   = 0x29,DEVTYPE_SITL         = 0x2A,DEVTYPE_INS_BMI088   = 0x2B,DEVTYPE_INS_ICM20948 = 0x2C,DEVTYPE_INS_ICM20648 = 0x2D,DEVTYPE_INS_ICM20649 = 0x2E,DEVTYPE_INS_ICM20602 = 0x2F,DEVTYPE_INS_ICM20601 = 0x30,DEVTYPE_INS_ADIS1647X = 0x31,DEVTYPE_SERIAL       = 0x32,DEVTYPE_INS_ICM40609 = 0x33,DEVTYPE_INS_ICM42688 = 0x34,DEVTYPE_INS_ICM42605 = 0x35,DEVTYPE_INS_ICM40605 = 0x36,DEVTYPE_INS_IIM42652 = 0x37,DEVTYPE_BMI270       = 0x38,DEVTYPE_INS_BMI085   = 0x39,DEVTYPE_INS_ICM42670 = 0x3A,DEVTYPE_INS_ICM45686 = 0x3B,DEVTYPE_INS_SCHA63T  = 0x3C,};protected:// access to frontendAP_InertialSensor &_imu;// semaphore for access to shared frontend dataHAL_Semaphore _sem;//Default Clip Limitfloat _clip_limit = 15.5f * GRAVITY_MSS;void _rotate_and_correct_accel(uint8_t instance, Vector3f &accel) __RAMFUNC__;void _rotate_and_correct_gyro(uint8_t instance, Vector3f &gyro) __RAMFUNC__;// rotate gyro vector, offset and publishvoid _publish_gyro(uint8_t instance, const Vector3f &gyro) __RAMFUNC__; /* front end */// apply notch and lowpass gyro filters and sample for FFTvoid apply_gyro_filters(const uint8_t instance, const Vector3f &gyro);void save_gyro_window(const uint8_t instance, const Vector3f &gyro, uint8_t phase);// this should be called every time a new gyro raw sample is// available - be it published or not the sample is raw in the// sense that it's not filtered yet, but it must be rotated and// corrected (_rotate_and_correct_gyro)// The sample_us value must be provided for non-FIFO based// sensors, and should be set to zero for FIFO based sensorsvoid _notify_new_gyro_raw_sample(uint8_t instance, const Vector3f &accel, uint64_t sample_us=0) __RAMFUNC__;// alternative interface using delta-angles. Rotation and correction is handled inside this functionvoid _notify_new_delta_angle(uint8_t instance, const Vector3f &dangle);// rotate accel vector, scale, offset and publishvoid _publish_accel(uint8_t instance, const Vector3f &accel) __RAMFUNC__; /* front end */// this should be called every time a new accel raw sample is available -// be it published or not// the sample is raw in the sense that it's not filtered yet, but it must// be rotated and corrected (_rotate_and_correct_accel)// The sample_us value must be provided for non-FIFO based// sensors, and should be set to zero for FIFO based sensorsvoid _notify_new_accel_raw_sample(uint8_t instance, const Vector3f &accel, uint64_t sample_us=0, bool fsync_set=false) __RAMFUNC__;// alternative interface using delta-velocities. Rotation and correction is handled inside this functionvoid _notify_new_delta_velocity(uint8_t instance, const Vector3f &dvelocity);// set the amount of oversamping a accel is doingvoid _set_accel_oversampling(uint8_t instance, uint8_t n);// set the amount of oversamping a gyro is doingvoid _set_gyro_oversampling(uint8_t instance, uint8_t n);// indicate the backend is doing sensor-rate sampling for this accelvoid _set_accel_sensor_rate_sampling_enabled(uint8_t instance, bool value) {const uint8_t bit = (1<<instance);if (value) {_imu._accel_sensor_rate_sampling_enabled |= bit;} else {_imu._accel_sensor_rate_sampling_enabled &= ~bit;}}void _set_gyro_sensor_rate_sampling_enabled(uint8_t instance, bool value) {const uint8_t bit = (1<<instance);if (value) {_imu._gyro_sensor_rate_sampling_enabled |= bit;} else {_imu._gyro_sensor_rate_sampling_enabled &= ~bit;}}void _set_raw_sample_accel_multiplier(uint8_t instance, uint16_t mul) {_imu._accel_raw_sampling_multiplier[instance] = mul;}void _set_raw_sample_gyro_multiplier(uint8_t instance, uint16_t mul) {_imu._gyro_raw_sampling_multiplier[instance] = mul;}// update the sensor rate for FIFO sensorsvoid _update_sensor_rate(uint16_t &count, uint32_t &start_us, float &rate_hz) const __RAMFUNC__;// return true if the sensors are still converging and sampling rates could change significantlybool sensors_converging() const { return AP_HAL::millis() < HAL_INS_CONVERGANCE_MS; }// set accelerometer max absolute offset for calibrationvoid _set_accel_max_abs_offset(uint8_t instance, float offset);// get accelerometer raw sample rate.float _accel_raw_sample_rate(uint8_t instance) const {return _imu._accel_raw_sample_rates[instance];}// set accelerometer raw sample rate;  note that the storage type// is actually float!void _set_accel_raw_sample_rate(uint8_t instance, uint16_t rate_hz) {_imu._accel_raw_sample_rates[instance] = rate_hz;}// get gyroscope raw sample ratefloat _gyro_raw_sample_rate(uint8_t instance) const {return _imu._gyro_raw_sample_rates[instance];}// set gyro raw sample rate; note that the storage type is// actually float!void _set_gyro_raw_sample_rate(uint8_t instance, uint16_t rate_hz) {_imu._gyro_raw_sample_rates[instance] = rate_hz;}// publish a temperature valuevoid _publish_temperature(uint8_t instance, float temperature); /* front end */// increment accelerometer error_countvoid _inc_accel_error_count(uint8_t instance) __RAMFUNC__;// increment gyro error_countvoid _inc_gyro_error_count(uint8_t instance) __RAMFUNC__;// backend unique identifier or -1 if backend doesn't identify itselfint16_t _id = -1;// return the default filter frequency in Hz for the sample rateuint16_t _accel_filter_cutoff(void) const { return _imu._accel_filter_cutoff; }// return the default filter frequency in Hz for the sample rateuint16_t _gyro_filter_cutoff(void) const { return _imu._gyro_filter_cutoff; }// return the requested loop rate at which samples will be made available in Hzuint16_t get_loop_rate_hz(void) const {// enum can be directly cast to Hzreturn (uint16_t)_imu._loop_rate;}// common gyro update function for all backendsvoid update_gyro(uint8_t instance) __RAMFUNC__; /* front end */// common accel update function for all backendsvoid update_accel(uint8_t instance) __RAMFUNC__; /* front end */// support for updating filter at runtimeuint16_t _last_accel_filter_hz;uint16_t _last_gyro_filter_hz;void set_gyro_orientation(uint8_t instance, enum Rotation rotation) {_imu._gyro_orientation[instance] = rotation;}void set_accel_orientation(uint8_t instance, enum Rotation rotation) {_imu._accel_orientation[instance] = rotation;}// increment clipping counted. Used by drivers that do decimation before supplying// samples to the frontendvoid increment_clip_count(uint8_t instance) {_imu._accel_clip_count[instance]++;}// should fast sampling be enabled on this IMU?bool enable_fast_sampling(uint8_t instance) {return (_imu._fast_sampling_mask & (1U<<instance)) != 0;}// if fast sampling is enabled, the rate to use in kHzuint8_t get_fast_sampling_rate() const {return (1 << uint8_t(_imu._fast_sampling_rate));}// called by subclass when data is received from the sensor, thus// at the 'sensor rate'void _notify_new_accel_sensor_rate_sample(uint8_t instance, const Vector3f &accel) __RAMFUNC__;void _notify_new_gyro_sensor_rate_sample(uint8_t instance, const Vector3f &gyro) __RAMFUNC__;/*notify of a FIFO reset so we don't use bad data to update observed sensor rate*/void notify_accel_fifo_reset(uint8_t instance) __RAMFUNC__;void notify_gyro_fifo_reset(uint8_t instance) __RAMFUNC__;// log an unexpected change in a register for an IMUvoid log_register_change(uint32_t bus_id, const AP_HAL::Device::checkreg &reg) __RAMFUNC__;// note that each backend is also expected to have a static detect()// function which instantiates an instance of the backend sensor// driver if the sensor is availableprivate:bool should_log_imu_raw() const ;void log_accel_raw(uint8_t instance, const uint64_t sample_us, const Vector3f &accel) __RAMFUNC__;void log_gyro_raw(uint8_t instance, const uint64_t sample_us, const Vector3f &gryo) __RAMFUNC__;// loggingvoid Write_ACC(const uint8_t instance, const uint64_t sample_us, const Vector3f &accel) const __RAMFUNC__; // Write ACC data packet: raw accel datavoid Write_GYR(const uint8_t instance, const uint64_t sample_us, const Vector3f &gyro) const __RAMFUNC__;  // Write GYR data packet: raw gyro data};

注：back-end实际上可以看做是驱动类，针对不同硬件其名称略有差异。这里以手头BMI270硬件为参考。

3. 实例BMI270

从类抽象的角度，上面给出了概括性的back-end抽象类，以及对front-end的接口函数；但是不知道为什么该抽象类并未从框架代码角度进行抽象。

这里所说的框架代码的意思更多侧重代码流程上的API接口。
通常我们会将框架和业务通过一些标准的API接口来进行定义（入参，出参），对具体器件尤其硬件寄存器等操作进行抽象，具体放在后面继承类进行进一步的实现。

可能是历史或者其他的原因，从上述AP_InertialSensor_Backend抽象看，并未做到上述抽象，不过front-end/back-end之间的区分是有明确给出的，同时针对具有共性的API进行了实现。

鉴于上述原因，后面将根据BMI270具体实现进行讨论。

3.1 AP_InertialSensor_BMI270

延续前面我们关注的实现方法，将对以下方法进行展开：

• init //硬件初始化
• start //硬件启动
• read_fifo //获取原始数据
• update //front-end数据更新API

class AP_InertialSensor_BMI270 : public AP_InertialSensor_Backend {
public:static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu,AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev,enum Rotation rotation=BMI270_DEFAULT_ROTATION);static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu,AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,enum Rotation rotation=BMI270_DEFAULT_ROTATION);/*** Configure the sensors and start reading routine.*/void start() override;bool update() override;private:AP_InertialSensor_BMI270(AP_InertialSensor &imu,AP_HAL::OwnPtr<AP_HAL::Device> dev,enum Rotation rotation);bool read_registers(uint8_t reg, uint8_t *data, uint8_t len);bool write_register(uint8_t reg, uint8_t v);/*** If the macro BMI270_DEBUG is defined, check if there are errors reported* on the device's error register and panic if so. The implementation is* empty if the BMI270_DEBUG isn't defined.*/void check_err_reg();/*** Try to perform initialization of the BMI270 device.** The device semaphore must be taken and released by the caller.** @return true on success, false otherwise.*/bool hardware_init();/*** Try to initialize this driver.** Do sensor and other required initializations.** @return true on success, false otherwise.*/bool init();/*** Configure accelerometer sensor. The device semaphore must already be* taken before calling this function.** @return true on success, false otherwise.*/void configure_accel();/*** Configure gyroscope sensor. The device semaphore must already be* taken before calling this function.** @return true on success, false otherwise.*/void configure_gyro();/*** Reset FIFO.*/void fifo_reset();/*** Configure FIFO.*/void configure_fifo();/*** Read samples from fifo.*/void read_fifo();void parse_accel_frame(const uint8_t* d);void parse_gyro_frame(const uint8_t* d);AP_HAL::OwnPtr<AP_HAL::Device> _dev;AP_HAL::Device::PeriodicHandle periodic_handle;enum Rotation _rotation;uint8_t _accel_instance;uint8_t _gyro_instance;uint8_t temperature_counter;static const uint8_t maximum_fifo_config_file[];
};

3.2 probe

IMU硬件是如何上电后自动检测到的。

注：AP_Vehicle::setup如何被调用，详见ArduPilot飞控启动&运行过程简介。

AP_Vehicle::setup└──> init_ardupilot└──> startup_INS_ground└──> ins.init(scheduler.get_loop_rate_hz());└──> _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))

IMU有两种硬件总线可供使用:I2C和SPI，现在慢慢倾向于使用SPI总线，尤其是小四轴航模(Betaflight/iNav等)。

当probe失败，可能存在以下几种情况：

1. 非法入参，dev空指针；
2. 内存空间不够，无法new对象；
3. probe发现设设备失败（总线异常/上电异常/芯片不存在）；

注：上述情况需要逐一排查，以便找到rootcasue。

AP_InertialSensor_Backend *
AP_InertialSensor_BMI270::probe(AP_InertialSensor &imu,AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev,enum Rotation rotation)
{if (!dev) {return nullptr;}auto sensor = new AP_InertialSensor_BMI270(imu, std::move(dev), rotation);if (!sensor) {return nullptr;}if (!sensor->init()) {delete sensor;return nullptr;}return sensor;
}AP_InertialSensor_Backend *
AP_InertialSensor_BMI270::probe(AP_InertialSensor &imu,AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,enum Rotation rotation)
{if (!dev) {return nullptr;}auto sensor = new AP_InertialSensor_BMI270(imu, std::move(dev), rotation);if (!sensor) {return nullptr;}if (!sensor->init()) {delete sensor;return nullptr;}return sensor;
}

3.3 init

init方法主要是对硬件进行初始化配置，详见hardware_init。

bool AP_InertialSensor_BMI270::init()
{_dev->set_read_flag(0x80);return hardware_init();
}

硬件由于上电以及内部芯片初始化，可能存在时序上的一些同步问题，因此这里做了BMI270_HARDWARE_INIT_MAX_TRIES动作。

bool AP_InertialSensor_BMI270::hardware_init()
{bool init = false;bool read_chip_id = false;hal.scheduler->delay(BMI270_POWERUP_DELAY_MSEC);WITH_SEMAPHORE(_dev->get_semaphore());_dev->set_speed(AP_HAL::Device::SPEED_LOW);for (unsigned i = 0; i < BMI270_HARDWARE_INIT_MAX_TRIES; i++) {uint8_t chip_id = 0;/* If CSB sees a rising edge after power-up, the device interface switches to SPI* after 200 μs until a reset or the next power-up occurs therefore it is recommended* to perform a SPI single read of register CHIP_ID (the obtained value will be invalid)* before the actual communication start, in order to use the SPI interface */read_registers(BMI270_REG_CHIP_ID, &chip_id, 1);hal.scheduler->delay(1);write_register(BMI270_REG_CMD, BMI270_CMD_SOFTRESET);hal.scheduler->delay(BMI270_POWERUP_DELAY_MSEC);    // power on and soft reset time is 2ms// switch to SPI mode againread_registers(BMI270_REG_CHIP_ID, &chip_id, 1);hal.scheduler->delay(1);read_registers(BMI270_REG_CHIP_ID, &chip_id, 1);if (chip_id != BMI270_CHIP_ID) {continue;}// successfully identified the chip, proceed with initialisationread_chip_id = true;// disable power savewrite_register(BMI270_REG_PWR_CONF, 0x00);hal.scheduler->delay(1); // needs to be at least 450us// upload configwrite_register(BMI270_REG_INIT_CTRL, 0x00);// Transfer the config file, data packet includes INIT_DATA_dev->transfer(maximum_fifo_config_file, sizeof(maximum_fifo_config_file), nullptr, 0);// config is donewrite_register(BMI270_REG_INIT_CTRL, 1);// check the resultshal.scheduler->delay(20);uint8_t status = 0;read_registers(BMI270_REG_INTERNAL_STATUS, &status, 1);if ((status & 1) == 1) {init = true;DEV_PRINTF("BMI270 initialized after %d retries\n", i+1);break;}}_dev->set_speed(AP_HAL::Device::SPEED_HIGH);if (read_chip_id && !init) {DEV_PRINTF("BMI270: failed to init\n");}return init;
}

3.4 start

ACC和GYRO可以配置不同的采样频率，但是要注意的是如果采样频率不同，对应的read_fifo也要调整，确保数据的有效性以及减少无意义的pulling检查。

注：Interrupt的方式要比pulling的方法节省资源。

AP_Vehicle::setup└──> init_ardupilot└──> startup_INS_ground└──> ins.init(scheduler.get_loop_rate_hz());└──> _start_backends├──> detect_backends└──> _backends[i]->start()

当硬件芯片检测到之后，就开始启动并配置芯片功能。

void AP_InertialSensor_BMI270::start()
{_dev->get_semaphore()->take_blocking();configure_accel();configure_gyro();configure_fifo();_dev->get_semaphore()->give();if (!_imu.register_accel(_accel_instance, BMI270_BACKEND_SAMPLE_RATE, _dev->get_bus_id_devtype(DEVTYPE_BMI270)) ||!_imu.register_gyro(_gyro_instance, BMI270_BACKEND_SAMPLE_RATE, _dev->get_bus_id_devtype(DEVTYPE_BMI270))) {return;}// setup sensor rotations from probe()set_gyro_orientation(_gyro_instance, _rotation);set_accel_orientation(_accel_instance, _rotation);/* Call read_fifo() at 1600Hz */periodic_handle = _dev->register_periodic_callback(BACKEND_PERIOD_US, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_BMI270::read_fifo, void));
}

3.5 read_fifo

通过fifo数据获取传感数据

1. 检查fifo状态
2. 检查fifo内容长度
3. 检查fifo空字节
4. 获取一次传感数据集
5. 获取传感数据时间同步
6. 获取传感数据
7. 获取IMU器件温度

注：在start中注册了定时回调的钩子函数&AP_InertialSensor_BMI270::read_fifo，因此会被定时同步IMU数据。

void AP_InertialSensor_BMI270::read_fifo(void)
{// check for FIFO errors/overflow //fifo异常检查uint8_t err = 0;read_registers(BMI270_REG_ERR_REG, &err, 1);if ((err>>6 & 1) == 1) {fifo_reset();return;}uint8_t len[2];if (!read_registers(BMI270_REG_FIFO_LENGTH_LSB, len, 2)) {  //获取fifo长度_inc_accel_error_count(_accel_instance);_inc_gyro_error_count(_gyro_instance);return;}uint16_t fifo_length = len[0] + (len[1]<<8);  //检查fifo空字节if (fifo_length & 0x8000) {// emptyreturn;}// don't read more than 8 frames at a timeif (fifo_length > BMI270_MAX_FIFO_SAMPLES*13) {  //fifo存在超过一次的传感数据集fifo_length = BMI270_MAX_FIFO_SAMPLES*13;}if (fifo_length == 0) {return;}uint8_t data[fifo_length];if (!read_registers(BMI270_REG_FIFO_DATA, data, fifo_length)) {  //获取fifo传感数据_inc_accel_error_count(_accel_instance);_inc_gyro_error_count(_gyro_instance);return;}// adjust the periodic callback to be synchronous with the incoming data// this means that we rarely run read_fifo() without updating the sensor data_dev->adjust_periodic_callback(periodic_handle, BACKEND_PERIOD_US);  //数据更新时间同步const uint8_t *p = &data[0];while (fifo_length >= 12) {/*the fifo frames are variable length, with the frame type in the first byte*/uint8_t frame_len = 2;switch (p[0] & 0xFC) {case 0x84: // accelframe_len = 7;parse_accel_frame(p+1);break;case 0x88: // gyroframe_len = 7;parse_gyro_frame(p+1);break;case 0x8C: // accel + gyroframe_len = 13;parse_gyro_frame(p+1);parse_accel_frame(p+7);break;case 0x40:// skip frameframe_len = 2;break;case 0x44:// sensortime frameframe_len = 4;break;case 0x48:// fifo config frameframe_len = 5;break;case 0x50:// sample drop frameframe_len = 2;break;case 0x80:// invalid framefifo_reset();return;}p += frame_len;fifo_length -= frame_len;}// temperature sensor updated every 10msif (temperature_counter++ == 100) {temperature_counter = 0;uint8_t tbuf[2];if (!read_registers(BMI270_REG_TEMPERATURE_LSB, tbuf, 2)) {_inc_accel_error_count(_accel_instance);_inc_gyro_error_count(_gyro_instance);} else {uint16_t tval = tbuf[0] | (tbuf[1] << 8);if (tval != 0x8000) {   // 0x8000 is invalidint16_t klsb = static_cast<int16_t>(tval);float temp_degc = klsb * 0.002f + 23.0f;_publish_temperature(_accel_instance, temp_degc);}}}
}

3.6 update

front-end更新ACC和GYRO传感数据，是back-end和front-end之间的接口。

bool AP_InertialSensor_BMI270::update()
{update_accel(_accel_instance);update_gyro(_gyro_instance);return true;
}

ACC数据更新

void AP_InertialSensor_Backend::update_accel(uint8_t instance) /* front end */
{    WITH_SEMAPHORE(_sem);if ((1U<<instance) & _imu.imu_kill_mask) {  //IMU硬件和实例不一致，直接返回return;}if (_imu._new_accel_data[instance]) {  //有ACC新数据_publish_accel(instance, _imu._accel_filtered[instance]);_imu._new_accel_data[instance] = false;}// possibly update filter frequencyif (_last_accel_filter_hz != _accel_filter_cutoff()) {_imu._accel_filter[instance].set_cutoff_frequency(_accel_raw_sample_rate(instance), _accel_filter_cutoff());_last_accel_filter_hz = _accel_filter_cutoff();}
}

GYRO数据更新

void AP_InertialSensor_Backend::update_gyro(uint8_t instance) /* front end */
{    WITH_SEMAPHORE(_sem);if ((1U<<instance) & _imu.imu_kill_mask) {  //IMU硬件和实例不一致，直接返回return;}if (_imu._new_gyro_data[instance]) {  //有GYRO新数据_publish_gyro(instance, _imu._gyro_filtered[instance]);
#if HAL_GYROFFT_ENABLED// copy the gyro samples from the backend to the frontend window for FFTs sampling at less than IMU rate_imu._gyro_for_fft[instance] = _imu._last_gyro_for_fft[instance];
#endif_imu._new_gyro_data[instance] = false;}// possibly update filter frequencyconst float gyro_rate = _gyro_raw_sample_rate(instance);if (_last_gyro_filter_hz != _gyro_filter_cutoff() || sensors_converging()) {_imu._gyro_filter[instance].set_cutoff_frequency(gyro_rate, _gyro_filter_cutoff());
#if HAL_GYROFFT_ENABLED_imu._post_filter_gyro_filter[instance].set_cutoff_frequency(gyro_rate, _gyro_filter_cutoff());
#endif_last_gyro_filter_hz = _gyro_filter_cutoff();}for (auto &notch : _imu.harmonic_notches) {if (notch.params.enabled()) {notch.update_params(instance, sensors_converging(), gyro_rate);}}
}

4. 总结

整个AP_InertialSensor_XXX驱动，可以理解为以下三部分：

1. 初始化：probe/init/start (被AP_Vehicle::setup调用)
2. 数据更新：read_fifo (被start中定时函数调用)
3. API接口：update (被AP_InertialSensor::update调用)

注：关于SPI的DMA优化在ChibiOS操作系统中进行BSP的支持。换句话说AP中更多的是High Level Design。

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