ArduPilot开源代码之AP_InertialSensor_Backend

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 Limit
    float get_clip_limit() const { return _clip_limit; }

    // get a startup banner to output to the GCS
    virtual bool get_output_banner(char* banner, uint8_t banner_len) { return false; }

#if HAL_EXTERNAL_AHRS_ENABLED
    virtual void handle_external(const AP_ExternalAHRS::ins_data_message_t &pkt) {}
#endif

    /*
      device driver IDs. These are used to fill in the devtype field
      of the device ID, which shows up as INS*ID* parameters to
      users. The values are chosen for compatibility with existing PX4
      drivers.
      If a change is made to a driver that would make existing
      calibration 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 frontend
    AP_InertialSensor &_imu;

    // semaphore for access to shared frontend data
    HAL_Semaphore _sem;

    //Default Clip Limit
    float _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 publish
    void _publish_gyro(uint8_t instance, const Vector3f &gyro) __RAMFUNC__; /* front end */

    // apply notch and lowpass gyro filters and sample for FFT
    void 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 sensors
    void _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 function
    void _notify_new_delta_angle(uint8_t instance, const Vector3f &dangle);
    
    // rotate accel vector, scale, offset and publish
    void _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 sensors
    void _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 function
    void _notify_new_delta_velocity(uint8_t instance, const Vector3f &dvelocity);
    
    // set the amount of oversamping a accel is doing
    void _set_accel_oversampling(uint8_t instance, uint8_t n);

    // set the amount of oversamping a gyro is doing
    void _set_gyro_oversampling(uint8_t instance, uint8_t n);

    // indicate the backend is doing sensor-rate sampling for this accel
    void _set_accel_sensor_rate_sampling_enabled(uint8_t instance, bool value) {
        const uint8_t bit = (1<

注：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 dev,
                                            enum Rotation rotation=BMI270_DEFAULT_ROTATION);

    static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu,
                                            AP_HAL::OwnPtr 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 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 _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 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 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 again
        read_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 initialisation
        read_chip_id = true;

        // disable power save
        write_register(BMI270_REG_PWR_CONF, 0x00);
        hal.scheduler->delay(1); // needs to be at least 450us

        // upload config
        write_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 done
        write_register(BMI270_REG_INIT_CTRL, 1);

        // check the results
        hal.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) {
        // empty
        return;
    }

    // don't read more than 8 frames at a time
    if (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: // accel
            frame_len = 7;
            parse_accel_frame(p+1);
            break;
        case 0x88: // gyro
            frame_len = 7;
            parse_gyro_frame(p+1);
            break;
        case 0x8C: // accel + gyro
            frame_len = 13;
            parse_gyro_frame(p+1);
            parse_accel_frame(p+7);
            break;
        case 0x40:
            // skip frame
            frame_len = 2;
            break;
        case 0x44:
            // sensortime frame
            frame_len = 4;
            break;
        case 0x48:
            // fifo config frame
            frame_len = 5;
            break;
        case 0x50:
            // sample drop frame
            frame_len = 2;
            break;
        case 0x80:
            // invalid frame
            fifo_reset();
            return;
        }
        p += frame_len;
        fifo_length -= frame_len;
    }

    // temperature sensor updated every 10ms
    if (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 invalid
                int16_t klsb = static_cast(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<

GYRO数据更新

void AP_InertialSensor_Backend::update_gyro(uint8_t instance) /* front end */
{    
    WITH_SEMAPHORE(_sem);

    if ((1U<

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