Ardupilot开源飞控之Invensense IMUs

1. 源由

• 在ArduPilot开源代码之AP_InertialSensor_Backend用了BMI270作为实例进行了介绍。
• 在APM/PX4/betaflight/inav开源飞控之IMU方向对于IMU方向有点歧义。

本文主要针对Invensense系列的IMU，进行驱动方面的研读，并期望解读IMU方向歧义问题。

2. Invensense IMUs

Invensense IMU类定义和相关重要API。

2.1 AP_InertialSensor_Invensense

• 【front-end】update
• 【back-end】probe //SPI and I2C
• 【back-end】start
• accumulate
• get_auxiliary_bus
• get_output_banner

    enum Invensense_Type {
        Invensense_MPU6000=0,
        Invensense_MPU6500,
        Invensense_MPU9250,
        Invensense_ICM20608,
        Invensense_ICM20602,
        Invensense_ICM20601,
        Invensense_ICM20789,
        Invensense_ICM20689,
    };

2.2 AP_InertialSensor_Invensensev2

• 【front-end】update
• 【back-end】probe //SPI and I2C
• 【back-end】start
• accumulate
• get_auxiliary_bus
• get_output_banner

    enum Invensensev2_Type {
        Invensensev2_ICM20948 = 0,
        Invensensev2_ICM20648,
        Invensensev2_ICM20649
    };

2.3 AP_InertialSensor_Invensensev3

• 【front-end】update
• 【back-end】probe //SPI only
• 【back-end】start
• accumulate
• get_output_banner

    enum class Invensensev3_Type : uint8_t {
        ICM40609 = 0, // No HiRes
        ICM42688, // HiRes 19bit
        ICM42605, // No HiRes
        ICM40605, // No HiRes
        IIM42652, // HiRes 19bit
        ICM42670, // HiRes 19bit
        ICM45686  // HiRes 20bit
    };

3.【back-end】硬件初始化

调用关系可参考：ArduPilot开源代码之AP_InertialSensor_Backend

3.1 AP_InertialSensor_Invensense

AP_InertialSensor_Invensense::probe
 └──> AP_InertialSensor_Invensense //构造函数
     └──> AP_InertialSensor_Invensense::_init
         └──> AP_InertialSensor_Invensense::_hardware_init

3.2 AP_InertialSensor_Invensensev2

AP_InertialSensor_Invensensev2::probe
 └──> AP_InertialSensor_Invensensev2 //构造函数
      └──> AP_InertialSensor_Invensensev2::_init
          └──> AP_InertialSensor_Invensensev2::_hardware_init

3.3 AP_InertialSensor_Invensensev3

AP_InertialSensor_Invensensev3::probe
 └──> AP_InertialSensor_Invensensev3 //构造函数
      └──> AP_InertialSensor_Invensensev3::hardware_init

4.【back-end】硬件启动

4.1 AP_InertialSensor_Invensense

初始化芯片，并挂上AP_InertialSensor_Invensense::_poll_data钩子，定期获取数据。

void AP_InertialSensor_Invensense::start()
{
    // pre-fetch instance numbers for checking fast sampling settings
    if (!_imu.get_gyro_instance(_gyro_instance) || !_imu.get_accel_instance(_accel_instance)) {
        return;
    }

    WITH_SEMAPHORE(_dev->get_semaphore());

    // initially run the bus at low speed
    _dev->set_speed(AP_HAL::Device::SPEED_LOW);

    // only used for wake-up in accelerometer only low power mode
    _register_write(MPUREG_PWR_MGMT_2, 0x00);
    hal.scheduler->delay(1);

    // always use FIFO
    _fifo_reset(false);

    // grab the used instances
    enum DevTypes gdev, adev;
    switch (_mpu_type) {
    case Invensense_MPU9250:
        gdev = DEVTYPE_GYR_MPU9250;
        adev = DEVTYPE_ACC_MPU9250;
        _enable_offset_checking = true;
        break;
    case Invensense_ICM20602:
        gdev = DEVTYPE_INS_ICM20602;
        adev = DEVTYPE_INS_ICM20602;
        // ICM20602 has a bug where sometimes the data gets a huge offset
        // this seems to be fixed by doing a quick FIFO reset via USR_CTRL
        // reg
        _enable_fast_fifo_reset = true;
        _enable_offset_checking = true;
        break;
    case Invensense_ICM20601:
        gdev = DEVTYPE_INS_ICM20601;
        adev = DEVTYPE_INS_ICM20601;
        _enable_offset_checking = true;
        break;
    case Invensense_ICM20608:
        // unfortunately we don't have a separate ID for 20608, and we
        // can't change this without breaking existing calibrations
        gdev = DEVTYPE_GYR_MPU6000;
        adev = DEVTYPE_ACC_MPU6000;
        _enable_offset_checking = true;
        break;
    case Invensense_ICM20789:
        gdev = DEVTYPE_INS_ICM20789;
        adev = DEVTYPE_INS_ICM20789;
        _enable_offset_checking = true;
        break;
    case Invensense_ICM20689:
        gdev = DEVTYPE_INS_ICM20689;
        adev = DEVTYPE_INS_ICM20689;
        _enable_offset_checking = true;
        break;
    case Invensense_MPU6000:
    case Invensense_MPU6500:
    default:
        gdev = DEVTYPE_GYR_MPU6000;
        adev = DEVTYPE_ACC_MPU6000;
        break;
    }

    /*
      setup temperature sensitivity and offset. This varies
      considerably between parts
     */
    switch (_mpu_type) {
    case Invensense_MPU9250:
        temp_zero = 21.0f;
        temp_sensitivity = 1.0f/340;
        break;

    case Invensense_MPU6000:
    case Invensense_MPU6500:
        temp_zero = 36.53f;
        temp_sensitivity = 1.0f/340;
        break;

    case Invensense_ICM20608:
    case Invensense_ICM20602:
    case Invensense_ICM20601:
        temp_zero = 25.0f;
        temp_sensitivity = 1.0f/326.8f;
        break;

    case Invensense_ICM20789:
        temp_zero = 25.0f;
        temp_sensitivity = 0.003f;
        break;
    case Invensense_ICM20689:
        temp_zero = 25.0f;
        temp_sensitivity = 0.003f;
        break;
    }

    if (!_imu.register_gyro(_gyro_instance, 1000, _dev->get_bus_id_devtype(gdev)) ||
        !_imu.register_accel(_accel_instance, 1000, _dev->get_bus_id_devtype(adev))) {
        return;
    }

    // setup ODR and on-sensor filtering
    _set_filter_register();

    // update backend sample rate
    _set_accel_raw_sample_rate(_accel_instance, _accel_backend_rate_hz);
    _set_gyro_raw_sample_rate(_gyro_instance, _gyro_backend_rate_hz);

    // indicate what multiplier is appropriate for the sensors'
    // readings to fit them into an int16_t:
    _set_raw_sample_accel_multiplier(_accel_instance, multiplier_accel);

    // set sample rate to 1000Hz and apply a software filter
    // In this configuration, the gyro sample rate is 8kHz
    _register_write(MPUREG_SMPLRT_DIV, 0, true);
    hal.scheduler->delay(1);

    // Gyro scale 2000º/s
    _register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS, true);
    hal.scheduler->delay(1);

    // read the product ID rev c has 1/2 the sensitivity of rev d
    uint8_t product_id = _register_read(MPUREG_PRODUCT_ID);

    if (_mpu_type == Invensense_MPU6000 &&
        ((product_id == MPU6000ES_REV_C4) ||
         (product_id == MPU6000ES_REV_C5) ||
         (product_id == MPU6000_REV_C4)   ||
         (product_id == MPU6000_REV_C5))) {
        // Accel scale 8g (4096 LSB/g)
        // Rev C has different scaling than rev D
        _register_write(MPUREG_ACCEL_CONFIG,1<<3, true);
        _accel_scale = GRAVITY_MSS / 4096.f;
        _gyro_scale = (radians(1) / 16.4f);
    } else if (_mpu_type == Invensense_ICM20601) {
        // Accel scale 32g (4096 LSB/g)
        _register_write(MPUREG_ACCEL_CONFIG,1<<3, true);
        _accel_scale = GRAVITY_MSS / 4096.f;
        _gyro_scale = (radians(1) / 8.2f);
        _clip_limit = 29.5f * GRAVITY_MSS;
    } else {
        // Accel scale 16g (2048 LSB/g)
        _register_write(MPUREG_ACCEL_CONFIG,3<<3, true);
        _accel_scale = GRAVITY_MSS / 2048.f;
        _gyro_scale = (radians(1) / 16.4f);
    }
    hal.scheduler->delay(1);

    if (_mpu_type == Invensense_ICM20608 ||
        _mpu_type == Invensense_ICM20602 ||
        _mpu_type == Invensense_ICM20601) {
        // this avoids a sensor bug, see description above
        _register_write(MPUREG_ICM_UNDOC1, MPUREG_ICM_UNDOC1_VALUE, true);
    }
    
    // configure interrupt to fire when new data arrives
    _register_write(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN);
    hal.scheduler->delay(1);

    // clear interrupt on any read, and hold the data ready pin high
    // until we clear the interrupt. We don't do this for the 20789 as
    // that sensor has already setup the appropriate config inside the
    // baro driver.
    if (_mpu_type != Invensense_ICM20789) {    
        uint8_t v = _register_read(MPUREG_INT_PIN_CFG) | BIT_INT_RD_CLEAR | BIT_LATCH_INT_EN;
        v &= BIT_BYPASS_EN;
        _register_write(MPUREG_INT_PIN_CFG, v);
    }

    if (_enable_offset_checking) {
        /*
          there is a bug in at least the ICM-20602 where the
          MPUREG_ACC_OFF_Y_H changes at runtime, which adds an offset
          on the Y accelerometer. To prevent this we read the factory
          cal values of the sensor at startup and write them back as
          checked register values. Then we rely on the register
          checking code to detect the change and fix it
         */
        uint8_t regs[] = { MPUREG_ACC_OFF_X_H, MPUREG_ACC_OFF_X_L,
                           MPUREG_ACC_OFF_Y_H, MPUREG_ACC_OFF_Y_L,
                           MPUREG_ACC_OFF_Z_H, MPUREG_ACC_OFF_Z_L };
        for (uint8_t i=0; i<ARRAY_SIZE(regs); i++) {
            _register_write(regs[i], _register_read(regs[i]), true);
        }
    }


    if (_mpu_type == Invensense_ICM20602) {
        /*
          save y offset high separately for ICM20602 to quickly
          recover from a change in this register. The ICM20602 has a
          bug where every few hours it can change the factory Y offset
          register, which leads to a sudden change in Y accelerometer
          output
         */
        _saved_y_ofs_high = _register_read(MPUREG_ACC_OFF_Y_H);
    }

    // now that we have initialised, we set the bus speed to high
    _dev->set_speed(AP_HAL::Device::SPEED_HIGH);

    // setup sensor rotations from probe()
    set_gyro_orientation(_gyro_instance, _rotation);
    set_accel_orientation(_accel_instance, _rotation);

    // setup scale factors for fifo data after downsampling
    _fifo_accel_scale = _accel_scale / _accel_fifo_downsample_rate;
    _fifo_gyro_scale = _gyro_scale / _gyro_fifo_downsample_rate;
    
    // allocate fifo buffer
    _fifo_buffer = (uint8_t *)hal.util->malloc_type(MPU_FIFO_BUFFER_LEN * MPU_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE);
    if (_fifo_buffer == nullptr) {
        AP_HAL::panic("Invensense: Unable to allocate FIFO buffer");
    }

    // start the timer process to read samples, using the fastest rate avilable
    _dev->register_periodic_callback(1000000UL / _gyro_backend_rate_hz, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_Invensense::_poll_data, void));
}

4.2 AP_InertialSensor_Invensensev2

初始化芯片，并挂上AP_InertialSensor_Invensensev2::_poll_data钩子，定期获取数据。

void AP_InertialSensor_Invensensev2::start()
{
    // pre-fetch instance numbers for checking fast sampling settings
    if (!_imu.get_gyro_instance(_gyro_instance) || !_imu.get_accel_instance(_accel_instance)) {
        return;
    }

    WITH_SEMAPHORE(_dev->get_semaphore());

    // initially run the bus at low speed
    _dev->set_speed(AP_HAL::Device::SPEED_LOW);

    // only used for wake-up in accelerometer only low power mode
    _register_write(INV2REG_PWR_MGMT_2, 0x00);
    hal.scheduler->delay(1);

    // always use FIFO
    _fifo_reset();

    // grab the used instances
    enum DevTypes gdev, adev;
    switch (_inv2_type) {
    case Invensensev2_ICM20648:
        gdev = DEVTYPE_INS_ICM20648;
        adev = DEVTYPE_INS_ICM20648;
        // using 16g full range, 2048 LSB/g
        _accel_scale = (GRAVITY_MSS / 2048);
        break;
    case Invensensev2_ICM20649:
        // 20649 is setup for 30g full scale, 1024 LSB/g
        gdev = DEVTYPE_INS_ICM20649;
        adev = DEVTYPE_INS_ICM20649;
        _accel_scale = (GRAVITY_MSS / 1024);
        break;
    case Invensensev2_ICM20948:
    default:
        gdev = DEVTYPE_INS_ICM20948;
        adev = DEVTYPE_INS_ICM20948;
        // using 16g full range, 2048 LSB/g
        _accel_scale = (GRAVITY_MSS / 2048);
        break;
    }

    if (!_imu.register_gyro(_gyro_instance, 1125, _dev->get_bus_id_devtype(gdev)) ||
        !_imu.register_accel(_accel_instance, 1125, _dev->get_bus_id_devtype(adev))) {
        return;
    }


    // setup on-sensor filtering and scaling
    _set_filter_and_scaling();
#if INVENSENSE_EXT_SYNC_ENABLE
    _register_write(INV2REG_FSYNC_CONFIG, FSYNC_CONFIG_EXT_SYNC_AZ, true);
#endif
    // update backend sample rate
    _set_accel_raw_sample_rate(_accel_instance, _accel_backend_rate_hz);
    _set_gyro_raw_sample_rate(_gyro_instance, _gyro_backend_rate_hz);

    // indicate what multiplier is appropriate for the sensors'
    // readings to fit them into an int16_t:
    _set_raw_sample_accel_multiplier(_accel_instance, multiplier_accel);

    // set sample rate to 1.125KHz
    _register_write(INV2REG_GYRO_SMPLRT_DIV, 0, true);
    hal.scheduler->delay(1);
    
    // configure interrupt to fire when new data arrives
    _register_write(INV2REG_INT_ENABLE_1, 0x01);
    hal.scheduler->delay(1);

    // now that we have initialised, we set the bus speed to high
    _dev->set_speed(AP_HAL::Device::SPEED_HIGH);

    // setup sensor rotations from probe()
    set_gyro_orientation(_gyro_instance, _rotation);
    set_accel_orientation(_accel_instance, _rotation);

    // setup scale factors for fifo data after downsampling
    _fifo_accel_scale = _accel_scale / _accel_fifo_downsample_rate;
    _fifo_gyro_scale = GYRO_SCALE / _gyro_fifo_downsample_rate;
    
    // allocate fifo buffer
    _fifo_buffer = (uint8_t *)hal.util->malloc_type(INV2_FIFO_BUFFER_LEN * INV2_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE);
    if (_fifo_buffer == nullptr) {
        AP_HAL::panic("Invensense: Unable to allocate FIFO buffer");
    }

    // start the timer process to read samples
    _dev->register_periodic_callback(1265625UL / _gyro_backend_rate_hz, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_Invensensev2::_poll_data, void));
}

4.3 AP_InertialSensor_Invensensev3

初始化芯片，并挂上AP_InertialSensor_Invensensev3::read_fifo钩子，定期获取数据。

void AP_InertialSensor_Invensensev3::start()
{
    // pre-fetch instance numbers for checking fast sampling settings
    if (!_imu.get_gyro_instance(gyro_instance) || !_imu.get_accel_instance(accel_instance)) {
        return;
    }
    WITH_SEMAPHORE(dev->get_semaphore());

    // initially run the bus at low speed
    dev->set_speed(AP_HAL::Device::SPEED_LOW);

    enum DevTypes devtype = DEVTYPE_INS_ICM42688;
    fifo_config1 = 0x07;

    switch (inv3_type) {
    case Invensensev3_Type::IIM42652:
        devtype = DEVTYPE_INS_IIM42652;
        temp_sensitivity = 1.0 / 2.07;
        break;
    case Invensensev3_Type::ICM42688:
        devtype = DEVTYPE_INS_ICM42688;
        temp_sensitivity = 1.0 / 2.07;
        break;
    case Invensensev3_Type::ICM42605:
        devtype = DEVTYPE_INS_ICM42605;
        temp_sensitivity = 1.0 / 2.07;
        break;
    case Invensensev3_Type::ICM40605:
        devtype = DEVTYPE_INS_ICM40605;
        fifo_config1 = 0x0F;
        temp_sensitivity = 1.0 * 128 / 115.49;
        break;
    case Invensensev3_Type::ICM42670:
        devtype = DEVTYPE_INS_ICM42670;
        temp_sensitivity = 1.0 / 2.0;
        break;
    case Invensensev3_Type::ICM45686:
        devtype = DEVTYPE_INS_ICM45686;
        temp_sensitivity = 1.0 / 2.0;
        gyro_scale = GYRO_SCALE_4000DPS;
        accel_scale = ACCEL_SCALE_32G;
        break;
    case Invensensev3_Type::ICM40609:
        devtype = DEVTYPE_INS_ICM40609;
        temp_sensitivity = 1.0 / 2.07;
        accel_scale = ACCEL_SCALE_32G;
        break;
    }

#if HAL_INS_HIGHRES_SAMPLE
    // now we know who we are, other things can be checked for
    if (enable_highres_sampling(accel_instance)) {
        switch (inv3_type) {
            case Invensensev3_Type::ICM42688: // HiRes 19bit
            case Invensensev3_Type::IIM42652: // HiRes 19bit
            case Invensensev3_Type::ICM42670: // HiRes 19bit
            case Invensensev3_Type::ICM45686: // HiRes 20bit
                highres_sampling = dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI;
                break;
            case Invensensev3_Type::ICM40609: // No HiRes
            case Invensensev3_Type::ICM42605:
            case Invensensev3_Type::ICM40605:
                break;
        }
    }

    // optionally enable high resolution mode
    if (highres_sampling) {
        fifo_config1 |= (1U<<4);  // FIFO_HIRES_EN
        gyro_scale = GYRO_SCALE_HIGHRES_2000DPS;
        accel_scale = ACCEL_SCALE_HIGHRES_16G;
        temp_sensitivity = 1.0 / 132.48;
        if (inv3_type == Invensensev3_Type::ICM45686) {
            temp_sensitivity = 1.0 / 128.0;
            accel_scale = ACCEL_SCALE_HIGHRES_32G;
            gyro_scale = GYRO_SCALE_HIGHRES_4000DPS;
        } else if (inv3_type == Invensensev3_Type::ICM42670) {
            temp_sensitivity = 1.0 / 128.0;
        }
    }
#endif
    // always use FIFO
    fifo_reset();

    // setup on-sensor filtering and scaling and backend rate
    if (inv3_type == Invensensev3_Type::ICM42670) {
        set_filter_and_scaling_icm42670();
    } else if (inv3_type == Invensensev3_Type::ICM45686) {
        set_filter_and_scaling_icm456xy();
    } else {
        set_filter_and_scaling();
    }

    // pre-calculate backend period
    backend_period_us = 1000000UL / backend_rate_hz;

    if (!_imu.register_gyro(gyro_instance, backend_rate_hz, dev->get_bus_id_devtype(devtype)) ||
        !_imu.register_accel(accel_instance, backend_rate_hz, dev->get_bus_id_devtype(devtype))) {
        return;
    }

    // update backend sample rate
    _set_accel_raw_sample_rate(accel_instance, backend_rate_hz);
    _set_gyro_raw_sample_rate(gyro_instance, backend_rate_hz);

    // indicate what multiplier is appropriate for the sensors'
    // readings to fit them into an int16_t:
    _set_raw_sample_accel_multiplier(accel_instance, multiplier_accel);

    // now that we have initialised, we set the bus speed to high
    dev->set_speed(AP_HAL::Device::SPEED_HIGH);

    // setup sensor rotations from probe()
    set_gyro_orientation(gyro_instance, rotation);
    set_accel_orientation(accel_instance, rotation);

    // allocate fifo buffer
#if HAL_INS_HIGHRES_SAMPLE
    if (highres_sampling) {
        fifo_buffer = hal.util->malloc_type(INV3_FIFO_BUFFER_LEN * INV3_HIGHRES_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE);
    } else
#endif
    fifo_buffer = hal.util->malloc_type(INV3_FIFO_BUFFER_LEN * INV3_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE);

    if (fifo_buffer == nullptr) {
        AP_HAL::panic("Invensensev3: Unable to allocate FIFO buffer");
    }

    // start the timer process to read samples, using the fastest rate avilable
    periodic_handle = dev->register_periodic_callback(backend_period_us, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_Invensensev3::read_fifo, void));
}

5.【back-end】获取数据

5.1 AP_InertialSensor_Invensense

Vector3f物理坐标系与系统坐标系转换逻辑：

1. 物理坐标Y轴 【变更为】 系统坐标X轴
2. 物理坐标X轴 【变更为】 系统坐标Y轴
3. 物理坐标Z轴 【变更为】 系统坐标Z轴反向

上述关系仍然满足右手螺旋关系，但当芯片物理Y轴朝向机头时，系统坐标系与机体坐标系重合，旋转ROTATION_NONE。

AP_InertialSensor_Invensense::_poll_data
 └──> AP_InertialSensor_Invensense::_read_fifo
     └──> AP_InertialSensor_Invensense::_accumulate/_accumulate_sensor_rate_sampling

bool AP_InertialSensor_Invensense::_accumulate(uint8_t *samples, uint8_t n_samples)
{
    for (uint8_t i = 0; i < n_samples; i++) {
        const uint8_t *data = samples + MPU_SAMPLE_SIZE * i;
        Vector3f accel, gyro;
        bool fsync_set = false;

#if INVENSENSE_EXT_SYNC_ENABLE
        fsync_set = (int16_val(data, 2) & 1U) != 0;
#endif
        
        accel = Vector3f(int16_val(data, 1),
                         int16_val(data, 0),
                         -int16_val(data, 2));
        accel *= _accel_scale;

        int16_t t2 = int16_val(data, 3);
        if (!_check_raw_temp(t2)) {
            if (_enable_fast_fifo_reset) {
                _fast_fifo_reset();
                return false;
            } else {
                if (!hal.scheduler->in_expected_delay()) {
                    debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2);
                }
                _fifo_reset(true);
                return false;
            }
        }
        float temp = t2 * temp_sensitivity + temp_zero;
        
        gyro = Vector3f(int16_val(data, 5),
                        int16_val(data, 4),
                        -int16_val(data, 6));
        gyro *= _gyro_scale;

        _rotate_and_correct_accel(_accel_instance, accel);
        _rotate_and_correct_gyro(_gyro_instance, gyro);

        _notify_new_accel_raw_sample(_accel_instance, accel, 0, fsync_set);
        _notify_new_gyro_raw_sample(_gyro_instance, gyro);

        _temp_filtered = _temp_filter.apply(temp);
    }
    return true;
}

/*
  when doing fast sampling the sensor gives us 8k samples/second. Every 2nd accel sample is a duplicate.

  To filter this we first apply a 1p low pass filter at 188Hz, then we
  average over 8 samples to bring the data rate down to 1kHz. This
  gives very good aliasing rejection at frequencies well above what
  can be handled with 1kHz sample rates.
 */
bool AP_InertialSensor_Invensense::_accumulate_sensor_rate_sampling(uint8_t *samples, uint8_t n_samples)
{
    int32_t tsum = 0;
    const int32_t unscaled_clip_limit = _clip_limit / _accel_scale;
    bool clipped = false;
    bool ret = true;
    
    for (uint8_t i = 0; i < n_samples; i++) {
        const uint8_t *data = samples + MPU_SAMPLE_SIZE * i;

        // use temperature to detect FIFO corruption
        int16_t t2 = int16_val(data, 3);
        if (!_check_raw_temp(t2)) {
            if (_enable_fast_fifo_reset) {
                _fast_fifo_reset();
                ret = false;
            } else {
                if (!hal.scheduler->in_expected_delay()) {
                    debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2);
                }
                _fifo_reset(true);
                ret = false;
            }
            break;
        }
        tsum += t2;

        if (_accum.gyro_count % _gyro_to_accel_sample_ratio == 0) {
            // accel data is at 4kHz or 1kHz
            Vector3f a(int16_val(data, 1),
                       int16_val(data, 0),
                       -int16_val(data, 2));
            if (fabsf(a.x) > unscaled_clip_limit ||
                fabsf(a.y) > unscaled_clip_limit ||
                fabsf(a.z) > unscaled_clip_limit) {
                clipped = true;
            }
            _accum.accel += _accum.accel_filter.apply(a);

            Vector3f a2 = a * _accel_scale;
            _notify_new_accel_sensor_rate_sample(_accel_instance, a2);

            _accum.accel_count++;

            if (_accum.accel_count % _accel_fifo_downsample_rate == 0) {
                _accum.accel *= _fifo_accel_scale;
                _rotate_and_correct_accel(_accel_instance, _accum.accel);
                _notify_new_accel_raw_sample(_accel_instance, _accum.accel, 0, false);
                _accum.accel.zero();
                _accum.accel_count = 0;
                // we assume that the gyro rate is always >= and a multiple of the accel rate
                _accum.gyro_count = 0;
            }
        }

        _accum.gyro_count++;

        Vector3f g(int16_val(data, 5),
                   int16_val(data, 4),
                   -int16_val(data, 6));

        Vector3f g2 = g * _gyro_scale;
        _notify_new_gyro_sensor_rate_sample(_gyro_instance, g2);

        _accum.gyro += g;

        if (_accum.gyro_count % _gyro_fifo_downsample_rate == 0) {
            _accum.gyro *= _fifo_gyro_scale;
            _rotate_and_correct_gyro(_gyro_instance, _accum.gyro);
            _notify_new_gyro_raw_sample(_gyro_instance, _accum.gyro);
            _accum.gyro.zero();
        }
    }

    if (clipped) {
        increment_clip_count(_accel_instance);
    }

    if (ret) {
        float temp = (static_cast<float>(tsum)/n_samples)*temp_sensitivity + temp_zero;
        _temp_filtered = _temp_filter.apply(temp);
    }
    
    return ret;
}

5.2 AP_InertialSensor_Invensensev2

Vector3f物理坐标系与系统坐标系转换逻辑：

1. 物理坐标Y轴 【变更为】 系统坐标X轴
2. 物理坐标X轴 【变更为】 系统坐标Y轴
3. 物理坐标Z轴 【变更为】 系统坐标Z轴反向

上述关系仍然满足右手螺旋关系，但当芯片物理Y轴朝向机头时，系统坐标系与机体坐标系重合，旋转ROTATION_NONE。

AP_InertialSensor_Invensensev2::_poll_data
 └──> AP_InertialSensor_Invensensev2::_read_fifo
     └──> AP_InertialSensor_Invensensev2::_accumulate/_accumulate_sensor_rate_sampling

bool AP_InertialSensor_Invensensev2::_accumulate(uint8_t *samples, uint8_t n_samples)
{
    for (uint8_t i = 0; i < n_samples; i++) {
        const uint8_t *data = samples + INV2_SAMPLE_SIZE * i;
        Vector3f accel, gyro;
        bool fsync_set = false;

#if INVENSENSE_EXT_SYNC_ENABLE
        fsync_set = (int16_val(data, 2) & 1U) != 0;
#endif
        
        accel = Vector3f(int16_val(data, 1),
                         int16_val(data, 0),
                         -int16_val(data, 2));
        accel *= _accel_scale;

        int16_t t2 = int16_val(data, 6);
        if (!_check_raw_temp(t2)) {
            if (!hal.scheduler->in_expected_delay()) {
                debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2);
            }
            _fifo_reset();
            return false;
        }
        float temp = t2 * temp_sensitivity + temp_zero;
        
        gyro = Vector3f(int16_val(data, 4),
                        int16_val(data, 3),
                        -int16_val(data, 5));
        gyro *= GYRO_SCALE;

        _rotate_and_correct_accel(_accel_instance, accel);
        _rotate_and_correct_gyro(_gyro_instance, gyro);

        _notify_new_accel_raw_sample(_accel_instance, accel, 0, fsync_set);
        _notify_new_gyro_raw_sample(_gyro_instance, gyro);

        _temp_filtered = _temp_filter.apply(temp);
    }
    return true;
}

/*
  when doing fast sampling the sensor gives us 9k samples/second. Every 2nd accel sample is a duplicate.

  To filter this we first apply a 1p low pass filter at 188Hz, then we
  average over 8 samples to bring the data rate down to 1kHz. This
  gives very good aliasing rejection at frequencies well above what
  can be handled with 1kHz sample rates.
 */
bool AP_InertialSensor_Invensensev2::_accumulate_sensor_rate_sampling(uint8_t *samples, uint8_t n_samples)
{
    int32_t tsum = 0;
    int32_t unscaled_clip_limit = _clip_limit / _accel_scale;
    bool clipped = false;
    bool ret = true;

    for (uint8_t i = 0; i < n_samples; i++) {
        const uint8_t *data = samples + INV2_SAMPLE_SIZE * i;

        // use temperature to detect FIFO corruption
        int16_t t2 = int16_val(data, 6);
        if (!_check_raw_temp(t2)) {
            if (!hal.scheduler->in_expected_delay()) {
                debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2);
            }
            _fifo_reset();
            ret = false;
            break;
        }
        tsum += t2;
        if (_accum.gyro_count % 2 == 0) {
            // accel data is at 4kHz or 1kHz
            Vector3f a(int16_val(data, 1),
                       int16_val(data, 0),
                       -int16_val(data, 2));
            if (fabsf(a.x) > unscaled_clip_limit ||
                fabsf(a.y) > unscaled_clip_limit ||
                fabsf(a.z) > unscaled_clip_limit) {
                clipped = true;
            }
            _accum.accel += _accum.accel_filter.apply(a);

            Vector3f a2 = a * _accel_scale;
            _notify_new_accel_sensor_rate_sample(_accel_instance, a2);

            _accum.accel_count++;

            if (_accum.accel_count % _accel_fifo_downsample_rate == 0) {
                _accum.accel *= _fifo_accel_scale;
                _rotate_and_correct_accel(_accel_instance, _accum.accel);
                _notify_new_accel_raw_sample(_accel_instance, _accum.accel, 0, false);
                _accum.accel.zero();
                _accum.accel_count = 0;
                // we assume that the gyro rate is always >= and a multiple of the accel rate
                _accum.gyro_count = 0;
            }
        }

        _accum.gyro_count++;

        Vector3f g(int16_val(data, 4),
                   int16_val(data, 3),
                   -int16_val(data, 5));

        Vector3f g2 = g * GYRO_SCALE;
        _notify_new_gyro_sensor_rate_sample(_gyro_instance, g2);

        _accum.gyro += g;

        if (_accum.gyro_count % _gyro_fifo_downsample_rate == 0) {
            _accum.gyro *= _fifo_gyro_scale;
            _rotate_and_correct_gyro(_gyro_instance, _accum.gyro);
            _notify_new_gyro_raw_sample(_gyro_instance, _accum.gyro);
            _accum.gyro.zero();
        }
    }

    if (clipped) {
        increment_clip_count(_accel_instance);
    }

    if (ret) {
        float temp = (static_cast<float>(tsum)/n_samples)*temp_sensitivity + temp_zero;
        _temp_filtered = _temp_filter.apply(temp);
    }
    
    return ret;
}

5.3 AP_InertialSensor_Invensensev3

Vector3f物理坐标系与系统坐标系转换逻辑：

1. 物理坐标Y轴 【与】 系统坐标X轴 【保持一致】
2. 物理坐标X轴 【与】 系统坐标Y轴 【保持一致】
3. 物理坐标Z轴 【与】 系统坐标Z轴 【保持一致】

上述关系仍然满足右手螺旋关系，当芯片物理Y轴朝向机头时，系统坐标系需旋转ROTATION_ROLL_180_YAW_90与机体坐标系重合。

AP_InertialSensor_Invensensev3::read_fifo
 └──> AP_InertialSensor_Invensensev3::accumulate_samples/accumulate_highres_samples

bool AP_InertialSensor_Invensensev3::accumulate_samples(const FIFOData *data, uint8_t n_samples)
{
#if INV3_ENABLE_FIFO_LOGGING
    const uint64_t tstart = AP_HAL::micros64();
#endif
    for (uint8_t i = 0; i < n_samples; i++) {
        const FIFOData &d = data[i];

        // we have a header to confirm we don't have FIFO corruption! no more mucking
        // about with the temperature registers
        // ICM45686 - TMST_FIELD_EN bit 3 : 1
        // ICM42688 - HEADER_TIMESTAMP_FSYNC bit 2-3 : 10
        if ((d.header & 0xFC) != 0x68) { // ACCEL_EN | GYRO_EN | TMST_FIELD_EN
            // no or bad data
            return false;
        }

        Vector3f accel{float(d.accel[0]), float(d.accel[1]), float(d.accel[2])};
        Vector3f gyro{float(d.gyro[0]), float(d.gyro[1]), float(d.gyro[2])};

        accel *= accel_scale;
        gyro *= gyro_scale;

#if INV3_ENABLE_FIFO_LOGGING
        Write_GYR(gyro_instance, tstart+(i*backend_period_us), gyro, true);
#endif

        const float temp = d.temperature * temp_sensitivity + temp_zero;

        // these four calls are about 40us
        _rotate_and_correct_accel(accel_instance, accel);
        _rotate_and_correct_gyro(gyro_instance, gyro);

        _notify_new_accel_raw_sample(accel_instance, accel, 0);
        _notify_new_gyro_raw_sample(gyro_instance, gyro);

        temp_filtered = temp_filter.apply(temp);
    }
    return true;
}

// high-resolution packets are always 20-bits, but not always 20-bits of data.
// Scale factors account for the useless bits
static inline float uint20_to_float(uint8_t msb, uint8_t bits, uint8_t lsb)
{
    uint32_t value20bit = uint32_t(msb) << 12U | uint32_t(bits) << 4U | lsb;
    int32_t value32bit;
    // Check the sign bit (MSB)
    if (value20bit & 0x80000) { // MSB is set (negative value)
        // Extend the sign bit to the upper 12 bits of the 32-bit integer
        value32bit = (int32_t)(value20bit | 0xFFF00000);
    } else { // MSB is not set (positive value)
        // Zero-fill the upper 12 bits of the 32-bit integer
        value32bit = value20bit;
    }

    return float(value32bit);
}


bool AP_InertialSensor_Invensensev3::accumulate_highres_samples(const FIFODataHighRes *data, uint8_t n_samples)
{
    for (uint8_t i = 0; i < n_samples; i++) {
        const FIFODataHighRes &d = data[i];

        // we have a header to confirm we don't have FIFO corruption! no more mucking
        // about with the temperature registers
        if ((d.header & 0xFC) != 0x78) { // ACCEL_EN | GYRO_EN | HIRES_EN | TMST_FIELD_EN
            // no or bad data
            return false;
        }

        Vector3f accel{uint20_to_float(d.accel[1], d.accel[0], d.ax),
            uint20_to_float(d.accel[3], d.accel[2], d.ay),
            uint20_to_float(d.accel[5], d.accel[4], d.az)};
        Vector3f gyro{uint20_to_float(d.gyro[1], d.gyro[0], d.gx),
            uint20_to_float(d.gyro[3], d.gyro[2], d.gy),
            uint20_to_float(d.gyro[5], d.gyro[4], d.gz)};

        accel *= accel_scale;
        gyro *= gyro_scale;

        const float temp = d.temperature * temp_sensitivity + temp_zero;

        // these four calls are about 40us
        _rotate_and_correct_accel(accel_instance, accel);
        _rotate_and_correct_gyro(gyro_instance, gyro);

        _notify_new_accel_raw_sample(accel_instance, accel, 0);
        _notify_new_gyro_raw_sample(gyro_instance, gyro);

        temp_filtered = temp_filter.apply(temp);
    }
    return true;
}

6.【front-end】数据更新

6.1 AP_InertialSensor_Invensense

/*
  publish any pending data
 */
bool AP_InertialSensor_Invensense::update() /* front end */
{
    update_accel(_accel_instance);
    update_gyro(_gyro_instance);

    _publish_temperature(_accel_instance, _temp_filtered);

    if (fast_reset_count) {
        // check if we have reported in the last 1 seconds or
        // fast_reset_count changed
#if HAL_GCS_ENABLED && BOARD_FLASH_SIZE > 1024
        const uint32_t now = AP_HAL::millis();
        if (now - last_fast_reset_count_report_ms > 5000U) {
            last_fast_reset_count_report_ms = now;
            char param_name[sizeof("IMUxx_RST")];
            snprintf(param_name, sizeof(param_name), "IMU%u_RST", MIN(_gyro_instance,9));
            gcs().send_named_float(param_name, fast_reset_count);
        }
#endif
#if HAL_LOGGING_ENABLED
        if (last_fast_reset_count != fast_reset_count) {
            AP::logger().Write_MessageF("IMU%u fast fifo reset %u", _gyro_instance, fast_reset_count);
            last_fast_reset_count = fast_reset_count;
        }
#endif
    }

    return true;
}

6.2 AP_InertialSensor_Invensensev2

/*
  publish any pending data
 */
bool AP_InertialSensor_Invensensev2::update()
{
    update_accel(_accel_instance);
    update_gyro(_gyro_instance);

    _publish_temperature(_accel_instance, _temp_filtered);

    return true;
}

6.3 AP_InertialSensor_Invensensev3

/*
  publish any pending data
 */
bool AP_InertialSensor_Invensensev3::update()
{
    update_accel(accel_instance);
    update_gyro(gyro_instance);
    _publish_temperature(accel_instance, temp_filtered);

    return true;
}

7. 总结

根据ArduPilot开源代码之AP_InertialSensor_Backend分析了Invensense系列IMU的驱动代码。

从逻辑上进一步梳理了IMU6000和ICM42688P芯片物理坐标系一致，但由于Ardupilot驱动将Invensense和Invensensev2系列传感数据坐标进行了方向调整，导致了IMU对齐方式约Invensensev3系列传感器不一致的问题。

8. 参考资料

【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码框架
【3】ArduPilot之开源代码Library&Sketches设计
【4】ArduPilot之开源代码Sensor Drivers设计
【5】ArduPilot开源代码之AP_InertialSensor_Backend
【6】APM/PX4/betaflight/inav开源飞控之IMU方向