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

背景

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

BMI088模块开发

BMI088 为 robomaster-c 开发板上集成的6轴imu，在此为提高速度陀螺仪和加速度计均使用使用 SPI 通讯方式，

添加 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*)®_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*)®_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模块获取传感器原始数据啦。

存在问题及优化方向

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