基于 RT-Thread 的 RoboMaster 电控框架（三）

基于 RT-Thread 的 RoboMaster 电控框架（三）

由于 RT-Thread 稳定高效的内核，丰富的文档教程，积极活跃的社区氛围，以及设备驱动框架、Kconfig、Scons、日志系统、海量的软件包…很难不选择 RT-Thread 进行项目开发。但也正是因为这些优点的覆盖面较广，很多初学者会觉得无从下手，但只要步入 RT-Thread 的大门，你就发现她的美好。这系列文档将作为本人基于 RT-Thread 开发 RoboMaster 电控框架的记录与分享，希望能帮助到更多初识 RT-Thread 的小伙伴，也欢迎大家交流分享，指正不足，共同进步。

背景

使用的开发板为大疆的 RoboMaster-C 型开发板，基础工程为 rt-thread>bsp>stm32f407-robomaster-c

BMI088模块开发

BMI088 为 robomaster-c 开发板上集成的6轴imu，在此为提高速度陀螺仪和加速度计均使用使用 SPI 通讯方式，驱动主要参考 Firmament Autopilot Embedded System 该飞控开源项目中的驱动程序在此基础上进行调整。

添加 SPI 通信 API

首先将飞控程序中针对 RT-Thread 的 SPI 设备驱动封装的 SPI 读写函数借鉴过来：

#define SPI_DIR_READ  0x80
#define SPI_DIR_WRITE 0x00

/**
 * This function write a 8 bit reg.
 *
 * @param device the SPI device attached to SPI bus
 * 
 * @param reg Register address
 * 
 * @param val The value to be written
 *
 * @return RT_EOK if write successfully.
 */
rt_inline rt_err_t spi_write_reg8(rt_device_t spi_device, uint8_t reg, uint8_t val)
{
    uint8_t buffer[2];
    rt_size_t w_byte;

    buffer[0] = SPI_DIR_WRITE | reg;
    buffer[1] = val;

    w_byte = rt_spi_transfer((struct rt_spi_device*)spi_device, buffer, NULL, 2);

    return (w_byte == 2) ? RT_EOK : RT_ERROR;
}

/**
 * This function read a 8 bit reg.
 *
 * @param device the SPI device attached to SPI bus
 * 
 * @param reg Register address
 * 
 * @param buffer Buffer of read data
 *
 * @return RT_EOK if read successfully.
 */
rt_inline rt_err_t spi_read_reg8(rt_device_t spi_device, uint8_t reg, uint8_t* buffer)
{
    uint8_t reg_addr;

    reg_addr = SPI_DIR_READ | reg;

    return rt_spi_send_then_recv((struct rt_spi_device*)spi_device, (void*)&reg_addr, 1, (void*)buffer, 1);
}

/**
 * This function read multiple contiguous 8 bit regs.
 *
 * @param device the SPI device attached to SPI bus
 * 
 * @param reg Start register address
 * 
 * @param buffer Buffer of read data
 *
 * @param len The number of read registers
 * 
 * @return RT_EOK if read successfully.
 */
rt_inline rt_err_t spi_read_multi_reg8(rt_device_t spi_device, uint8_t reg, uint8_t* buffer, uint8_t len)
{
    uint8_t reg_addr;

    reg_addr = SPI_DIR_READ | reg;

    return rt_spi_send_then_recv((struct rt_spi_device*)spi_device, (void*)&reg_addr, 1, (void*)buffer, len);
}

因为C板上 STM32 与 BMI088 是通过 SPI1 相连接，Kconfig 文件中添加 SPI1 部分并使能，并且需要进入到CubeMX 中使能 SPI1，这一步最重要的是选取引脚，这样 RTT 中的 SPI1 设备驱动才能使用：

BMI088 驱动

主要就是先对 BMI088 上的陀螺仪和加速度计分别进行初始化，设置相关采样参数，需要注意的一点是加速度计上电后默认是 I2C 模式，需要其片选引脚上检测到电平后才会切换为 SPI 模式并保持，如果没有正确处理这一步可能会导致这个现象：上电后读不出来加速度计，reset 后又能读取。

static rt_err_t accelerometer_init(void)
{
    uint8_t accel_id;

    /* init spi bus */
    rt_device_open(accel_spi_dev, RT_DEVICE_OFLAG_RDWR);

    /* dummy read to let accel enter SPI mode */
    spi_read_reg8(accel_spi_dev, BMI088_ACC_BGW_CHIPID, &accel_id);
    rt_hw_us_delay(1000);
    spi_read_reg8(accel_spi_dev, BMI088_ACC_BGW_CHIPID, &accel_id);

    /* read accel id */
    spi_read_reg8(accel_spi_dev, BMI088_ACC_BGW_CHIPID, &accel_id);
    if (accel_id != BMI088_ACC_BGW_CHIPID_VALUE) {
        LOG_W("Warning: not found BMI088 accel id: %02x", accel_id);
        return RT_ERROR;
    }

    /* soft reset */
    spi_write_reg8(accel_spi_dev, BMI088_ACC_SOFTRESET, 0xB6);
    rt_hw_us_delay(2000);
    /* dummy read to let accel enter SPI mode */
    spi_read_reg8(accel_spi_dev, BMI088_ACC_BGW_CHIPID, &accel_id);
    /* enter normal mode */
    spi_write_reg8(accel_spi_dev, BMI088_ACC_PWR_CTRL, 0x04);
    rt_hw_us_delay(55000);

    /* set default range and bandwidth */
    accel_set_range(6);          /* 6g */
    accel_set_sample_rate(800);  /* 800Hz sample rate */
    accel_set_bwp_odr(280);      /* Normal BW */

    /* enter active mode */
    spi_write_reg8(accel_spi_dev, BMI088_ACC_PWR_CONF, 0x00);
    rt_hw_us_delay(1000);

    return RT_EOK;
}

static rt_err_t gyroscope_init(void)
{
    uint8_t gyro_id;

    /* init spi bus */
    rt_device_open(gyro_spi_dev, RT_DEVICE_OFLAG_RDWR);

    spi_read_reg8(gyro_spi_dev, BMI088_CHIP_ID_ADDR, &gyro_id);
    if (gyro_id != BMI088_GRRO_CHIP_ID) {
        LOG_W("Warning: not found BMI088 gyro id: %02x", gyro_id);
        return RT_ERROR;
    }

    /* soft reset */
    spi_write_reg8(gyro_spi_dev, BMI088_BGW_SOFT_RST_ADDR, 0xB6);
    rt_hw_us_delay(35000); // > 30ms delay

    gyro_set_range(2000);       /* 2000dps */
    gyro_set_sample_rate(2000); /* OSR 2000KHz, Filter BW: 230Hz */

    /* enable gyroscope */
    __modify_reg(gyro_spi_dev, BMI088_MODE_LPM1_ADDR, REG_VAL(0, BIT(7) | BIT(5))); /* {0; 0}  NORMAL mode */
    rt_hw_us_delay(1000);

    return RT_EOK;
}

这里为了减小陀螺仪的零飘影响，可以对陀螺仪进行校准，得出补偿的值。

static void bmi088_calibrate(void){
    static float start_time;
    static uint16_t cali_times = 5000;   // 需要足够多的数据才能得到有效陀螺仪零偏校准结果
    float accel[3], gyro[3];
    float gyroMax[3], gyroMin[3];
    float gNormTemp, gNormMax, gNormMin;
    static float gyroDiff[3], gNormDiff;
    int16_t acc_raw[3];

    start_time = dwt_get_time_s();
    do
    {
        if (dwt_get_time_s() - start_time > 20)
        {
            // 校准超时
            gyro_offset[0] = GxOFFSET;
            gyro_offset[1] = GyOFFSET;
            gyro_offset[2] = GzOFFSET;
            bmi088_g_norm = gNORM;
            break;
        }

        dwt_delay_s(0.005);
        // 开始时先置零，避免对数据读取造成影响
        bmi088_g_norm = 0;
        gyro_offset[0] = 0;
        gyro_offset[1] = 0;
        gyro_offset[2] = 0;

        for (uint16_t i = 0; i < cali_times; i++)
        {
            accel_read_raw(acc_raw);
            accel[0] = accel_range_scale * acc_raw[0];
            accel[1] = accel_range_scale * acc_raw[1];
            accel[2] = accel_range_scale * acc_raw[2];
            gNormTemp = sqrtf(accel[0] * accel[0] +
                              accel[1] * accel[1] +
                              accel[2] * accel[2]);
            bmi088_g_norm += gNormTemp;

            gyro_read_rad(gyro);
            for(uint8_t j = 0; j < 3; j++){
                gyro_offset[j] += gyro[j];
            }

            // 记录数据极差
            if (i == 0)
            {
                gNormMax = gNormTemp;
                gNormMin = gNormTemp;
                for (uint8_t j = 0; j < 3; j++)
                {
                    gyroMax[j] = gyro[j];
                    gyroMin[j] = gyro[j];
                }
            }
            else
            {
                if (gNormTemp > gNormMax)
                    gNormMax = gNormTemp;
                if (gNormTemp < gNormMin)
                    gNormMin = gNormTemp;
                for (uint8_t j = 0; j < 3; j++)
                {
                    if (gyro[j] > gyroMax[j])
                        gyroMax[j] = gyro[j];
                    if (gyro[j] < gyroMin[j])
                        gyroMin[j] = gyro[j];
                }
            }

            // 数据差异过大认为收到外界干扰，需重新校准
            gNormDiff = gNormMax - gNormMin;
            for (uint8_t j = 0; j < 3; j++)
                gyroDiff[j] = gyroMax[j] - gyroMin[j];
            if (gNormDiff > 0.6f ||
                gyroDiff[0] > 1.0f ||
                gyroDiff[1] > 1.0f ||
                gyroDiff[2] > 1.0f)
                break;
            LOG_I("gyroDiff: %f",gNormDiff);
            for(uint8_t j = 0; j < 3; j++){
                LOG_D("gyroDiff%d: %f",j ,gyroDiff[j]);
            }
            dwt_delay_s(0.0005);
        }
        // 取平均值得到标定结果
        bmi088_g_norm /= (float)cali_times;
        LOG_W("bmi088_g_norm: %f",bmi088_g_norm);
        for (uint8_t i = 0; i < 3; i++)
        {
            gyro_offset[i] /= (float)cali_times;
            LOG_W("gyro_offset: %f",gyro_offset[i]);
        }

        cali_count++;
    } while (gNormDiff > 0.3f ||
             fabsf(bmi088_g_norm - 9.8f) > 0.5f ||
             gyroDiff[0] > 1.0f ||
             gyroDiff[1] > 1.0f ||
             gyroDiff[2] > 1.0f ||
             fabsf(gyro_offset[0]) > 0.01f ||
             fabsf(gyro_offset[1]) > 0.01f ||
             fabsf(gyro_offset[2]) > 0.01f);
    // 根据标定结果校准加速度计标度因数
    accel_scale = 9.81f / bmi088_g_norm;
}

由于校准时间较长，并且需要处于稳定的环境下，定期或更换开发板时进行一次校准即可,校准成功后手动修改 GxOFFSET 等宏；通过在 menuconfig 中使能 BSP_BMI088_CALI 进行校准；在串口终端可以查看校准进度,如多次校准失败,适当调大误差范围。

抽象设备

为提高程序的模块化，选用不同传感器时的灵活性，将 bmi088 抽象为 imu一类设备，抽象出 imu_init 、 gyro_read 、 gyro_config、accel_read、accel_config、temp_read 6个操作方法。

struct imu_ops{
    rt_err_t (*imu_init)(void);
    rt_err_t (*gyro_read)(float data[3]);
    rt_err_t (*gyro_config)(struct gyro_configure cfg);
    rt_err_t (*accel_read)(float data[3]);
    rt_err_t (*accel_config)(struct accel_configure cfg);
    float (*temp_read)(void);
};

项目选用不同的磁力计传感器时，对接这些个接口即可，以 bmi088 为例：

struct imu_ops imu_ops = {
    .imu_init = bmi088_init,
    .gyro_read = bim088_gyro_read,
    .gyro_config = bim088_gyro_config,
    .accel_read = bim088_accel_read,
    .accel_config = bim088_accel_config,
    .temp_read = bmi088_temp_read,
};

应用层需要使用磁力计时，调用 imu_ops 中的操作方法即可：

/* read data */
float gyro[3],acc[3],temp;

imu_ops.gyro_read(gyro);
imu_ops.accel_read(acc);
temp = imu_ops.temp_read();

/* config */
struct gyro_configure usr_conf_g = GYRO_CONFIG_DEFAULT;
struct acc_configure usr_conf_a = ACCEL_CONFIG_DEFAULT;

到此就可以使用imu模块获取传感器原始数据啦

仓库地址放这里了 HNU_RM_SHARK_C ，觉得不错可以点个 Star ！

感谢阿木实验室开源的基于 RT-Thread 的飞控程序！

存在问题及优化方向

1. 目前为了提高性能，imu设备的注册对接形式是比较简陋的；
2. 目前对 imu 设备的抽象只考虑到6轴 imu；
3. 需要注意陀螺仪校准处理部分。