基于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 gnormmax)
gnormmax = gnormtemp;
if (gnormtemp < gnormmin)
gnormmin = gnormtemp;
for (uint8_t j = 0; j gyromax[j])
gyromax[j] = gyro[j];
if (gyro[j] 1.0f ||
gyrodiff[1] > 1.0f ||
gyrodiff[2] > 1.0f)
break;
log_i(gyrodiff: %f,gnormdiff);
for(uint8_t j = 0; j 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；
需要注意陀螺仪校准处理部分。
无界全面屏轻薄本 华为MateBook D 14锐龙版种草众多学生党
两种端到端的自动驾驶系统算法架构
壹沓科技提供物流自动化解决方案，助力「外运集运」数智供应链建设
欧菲光AR-HUD，打造沉浸式互联智驾体验
新一代“指纹-身份识别”AI系统问世
基于RT-Thread的RoboMaster电控框架（三）
回顾第四届电磁兼容及天线技术会的内容分析和介绍
三星第四季将售出6000万台智能手机
直驱变频和变频哪个更耐用 ocdc电机与bldc的区别
TLC549程序
AI-2000警用机器人上岗，配置先进感知设备按路线进行定速巡逻
面板价格回落趋缓，明年有望转势
中国IC设计现状， RISC-V IP核异军突起
努比亚Z17今晚发布会：价格突破3500元，你能接受嘛？
二次电池的爆炸原因
以AI技术为导向的嵌入式应用现在发展如何
光伏发电谐波怎么解决
利用网络实现开关量信号的采集与控制,支持modbus TCP协议
电机轴承位磨损如何判定
谷歌将投资10亿美元在伦敦郊外建设数据中心
使用的开发板为大疆的 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 gnormmax)
gnormmax = gnormtemp;
if (gnormtemp < gnormmin)
gnormmin = gnormtemp;
for (uint8_t j = 0; j gyromax[j])
gyromax[j] = gyro[j];
if (gyro[j] 1.0f ||
gyrodiff[1] > 1.0f ||
gyrodiff[2] > 1.0f)
break;
log_i(gyrodiff: %f,gnormdiff);
for(uint8_t j = 0; j 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；
需要注意陀螺仪校准处理部分。
无界全面屏轻薄本 华为MateBook D 14锐龙版种草众多学生党
两种端到端的自动驾驶系统算法架构
壹沓科技提供物流自动化解决方案，助力「外运集运」数智供应链建设
欧菲光AR-HUD，打造沉浸式互联智驾体验
新一代“指纹-身份识别”AI系统问世
基于RT-Thread的RoboMaster电控框架（三）
回顾第四届电磁兼容及天线技术会的内容分析和介绍
三星第四季将售出6000万台智能手机
直驱变频和变频哪个更耐用 ocdc电机与bldc的区别
TLC549程序
AI-2000警用机器人上岗，配置先进感知设备按路线进行定速巡逻
面板价格回落趋缓，明年有望转势
中国IC设计现状， RISC-V IP核异军突起
努比亚Z17今晚发布会：价格突破3500元，你能接受嘛？
二次电池的爆炸原因
以AI技术为导向的嵌入式应用现在发展如何
光伏发电谐波怎么解决
利用网络实现开关量信号的采集与控制,支持modbus TCP协议
电机轴承位磨损如何判定
谷歌将投资10亿美元在伦敦郊外建设数据中心