AimBot/Gimbal
14
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

第一次提交

Browse Source
This commit is contained in:
李傲琦
2026-05-29 17:36:34 +08:00
commit 5b3333125d
1318 changed files with 722705 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
modules/BMI088/bmi088-datasheet.pdf
BIN
View File
Binary file not shown.
modules/BMI088/bmi088.c
+475
View File
@@ -0,0 +1,475 @@
#include "bmi088_regNdef.h"
#include "bmi088.h"
#include "user_lib.h"
#include "daemon.h"
static DaemonInstance *bmi088_daemon_instance;
// ---------------------------以下私有函数,用于读写BMI088寄存器封装,blocking--------------------------------//
/**
* @brief 读取BMI088寄存器Accel. BMI088要求在不释放CS的情况下连续读取
*
* @param bmi088 待读取的BMI088实例
* @param reg 待读取的寄存器地址
* @param dataptr 读取到的数据存放的指针
* @param len 读取长度
*/
static void BMI088AccelRead(BMI088Instance *bmi088, uint8_t reg, uint8_t *dataptr, uint8_t len)
{
if (len > 6)
while (1)
;
// 一次读取最多6个字节,加上两个dummy data 第一个字节的第一个位是读写位,1为读,0为写,1-7bit是寄存器地址
static uint8_t tx[8]; // 读取,第一个字节为0x80|reg ,第二个是dummy data,后面的没用都是dummy write
static uint8_t rx[8]; // 前两个字节是dummy data,第三个开始是真正的数据
tx[0] = 0x80 | reg; // 静态变量每次进来还是上次的值,所以要每次都要给tx[0]赋值0x80
SPITransRecv(bmi088->spi_acc, rx, tx, len + 2);
memcpy(dataptr, rx + 2, len); // @todo : memcpy有额外开销,后续可以考虑优化,在SPI中加入接口或模式,使得在一次传输结束后不释放CS,直接接着传输
}
/**
* @brief 读取BMI088寄存器Gyro, BMI088要求在不释放CS的情况下连续读取
*
* @param bmi088 待读取的BMI088实例
* @param reg 待读取的寄存器地址
* @param dataptr 读取到的数据存放的指针
* @param len 读取长度
*/
static void BMI088GyroRead(BMI088Instance *bmi088, uint8_t reg, uint8_t *dataptr, uint8_t len)
{
if (len > 6)
while (1)
;
// 一次读取最多6个字节,加上一个dummy data ,第一个字节的第一个位是读写位,1为读,0为写,1-7bit是寄存器地址
static uint8_t tx[7] = {0x80}; // 读取,第一个字节为0x80 | reg ,之后是dummy data
static uint8_t rx[7]; // 第一个是dummy data,第三个开始是真正的数据
tx[0] = 0x80 | reg;
SPITransRecv(bmi088->spi_gyro, rx, tx, len + 1);
memcpy(dataptr, rx + 1, len); // @todo : memcpy有额外开销,后续可以考虑优化,在SPI中加入接口或模式,使得在一次传输结束后不释放CS,直接接着传输
}
/**
* @brief 写accel寄存器.对spitransmit形式上的封装
* @attention 只会向目标reg写入一个字节,因为只有1个字节所以直接传值(指针是32位反而浪费)
*
* @param bmi088 待写入的BMI088实例
* @param reg 待写入的寄存器地址
* @param data 待写入的数据(注意不是指针)
*/
static void BMI088AccelWriteSingleReg(BMI088Instance *bmi088, uint8_t reg, uint8_t data)
{
uint8_t tx[2] = {reg, data};
SPITransmit(bmi088->spi_acc, tx, 2);
}
/**
* @brief 写gyro寄存器.形式上的封装
* @attention 只会向目标reg写入一个字节,因为只有1个字节所以直接传值(指针是32位反而浪费)
*
* @param bmi088 待写入的BMI088实例
* @param reg 待写入的寄存器地址
* @param data 待写入的数据(注意不是指针)
*/
static void BMI088GyroWriteSingleReg(BMI088Instance *bmi088, uint8_t reg, uint8_t data)
{
uint8_t tx[2] = {reg, data};
SPITransmit(bmi088->spi_gyro, tx, 2);
}
// -------------------------以上为私有函数,封装了BMI088寄存器读写函数,blocking--------------------------------//
// -------------------------以下为私有函数,用于初始化BMI088acc和gyro的硬件和配置--------------------------------//
#define BMI088REG 0
#define BMI088DATA 1
#define BMI088ERROR 2
// BMI088初始化配置数组for accel,第一列为reg地址,第二列为写入的配置值,第三列为错误码(如果出错)
static uint8_t BMI088_Accel_Init_Table[BMI088_WRITE_ACCEL_REG_NUM][3] =
{
{BMI088_ACC_PWR_CTRL, BMI088_ACC_ENABLE_ACC_ON, BMI088_ACC_PWR_CTRL_ERROR},
{BMI088_ACC_PWR_CONF, BMI088_ACC_PWR_ACTIVE_MODE, BMI088_ACC_PWR_CONF_ERROR},
{BMI088_ACC_CONF, BMI088_ACC_NORMAL | BMI088_ACC_800_HZ | BMI088_ACC_CONF_MUST_Set, BMI088_ACC_CONF_ERROR},
{BMI088_ACC_RANGE, BMI088_ACC_RANGE_6G, BMI088_ACC_RANGE_ERROR},
{BMI088_INT1_IO_CTRL, BMI088_ACC_INT1_IO_ENABLE | BMI088_ACC_INT1_GPIO_PP | BMI088_ACC_INT1_GPIO_LOW, BMI088_INT1_IO_CTRL_ERROR},
{BMI088_INT_MAP_DATA, BMI088_ACC_INT1_DRDY_INTERRUPT, BMI088_INT_MAP_DATA_ERROR}};
// BMI088初始化配置数组for gyro,第一列为reg地址,第二列为写入的配置值,第三列为错误码(如果出错)
static uint8_t BMI088_Gyro_Init_Table[BMI088_WRITE_GYRO_REG_NUM][3] =
{
{BMI088_GYRO_RANGE, BMI088_GYRO_2000, BMI088_GYRO_RANGE_ERROR},
{BMI088_GYRO_BANDWIDTH, BMI088_GYRO_1000_116_HZ | BMI088_GYRO_BANDWIDTH_MUST_Set, BMI088_GYRO_BANDWIDTH_ERROR},
{BMI088_GYRO_LPM1, BMI088_GYRO_NORMAL_MODE, BMI088_GYRO_LPM1_ERROR},
{BMI088_GYRO_CTRL, BMI088_DRDY_ON, BMI088_GYRO_CTRL_ERROR},
{BMI088_GYRO_INT3_INT4_IO_CONF, BMI088_GYRO_INT3_GPIO_PP | BMI088_GYRO_INT3_GPIO_LOW, BMI088_GYRO_INT3_INT4_IO_CONF_ERROR},
{BMI088_GYRO_INT3_INT4_IO_MAP, BMI088_GYRO_DRDY_IO_INT3, BMI088_GYRO_INT3_INT4_IO_MAP_ERROR}};
// @attention : 以上两个数组配合各自的初始化函数使用. 若要修改请参照BMI088 datasheet
/**
* @brief 初始化BMI088加速度计,提高可读性分拆功能
*
* @param bmi088 待初始化的BMI088实例
* @return uint8_t BMI088ERROR CODE if any problems here
*/
static uint8_t BMI088AccelInit(BMI088Instance *bmi088)
{
uint8_t whoami_check = 0;
// 加速度计以I2C模式启动,需要一次上升沿来切换到SPI模式,因此进行一次fake write
BMI088AccelRead(bmi088, BMI088_ACC_CHIP_ID, &whoami_check, 1);
DWT_Delay(0.001);
BMI088AccelWriteSingleReg(bmi088, BMI088_ACC_SOFTRESET, BMI088_ACC_SOFTRESET_VALUE); // 软复位
DWT_Delay(BMI088_COM_WAIT_SENSOR_TIME / 1000);
// 检查ID,如果不是0x1E(bmi088 whoami寄存器值),则返回错误
BMI088AccelRead(bmi088, BMI088_ACC_CHIP_ID, &whoami_check, 1);
if (whoami_check != BMI088_ACC_CHIP_ID_VALUE)
return BMI088_NO_SENSOR;
DWT_Delay(0.001);
// 初始化寄存器,提高可读性
uint8_t reg = 0, data = 0;
BMI088_ERORR_CODE_e error = 0;
// 使用sizeof而不是magic number,这样如果修改了数组大小,不用修改这里的代码;或者使用宏定义
for (uint8_t i = 0; i < sizeof(BMI088_Accel_Init_Table) / sizeof(BMI088_Accel_Init_Table[0]); i++)
{
reg = BMI088_Accel_Init_Table[i][BMI088REG];
data = BMI088_Accel_Init_Table[i][BMI088DATA];
BMI088AccelWriteSingleReg(bmi088, reg, data); // 写入寄存器
DWT_Delay(0.01);
BMI088AccelRead(bmi088, reg, &data, 1); // 写完之后立刻读回检查
DWT_Delay(0.01);
if (data != BMI088_Accel_Init_Table[i][BMI088DATA])
error |= BMI088_Accel_Init_Table[i][BMI088ERROR];
//{i--;} 可以设置retry次数,如果retry次数用完了,则返回error
}
return error;
}
/**
* @brief 初始化BMI088陀螺仪,提高可读性分拆功能
*
* @param bmi088 待初始化的BMI088实例
* @return uint8_t BMI088ERROR CODE
*/
static uint8_t BMI088GyroInit(BMI088Instance *bmi088)
{
// 后续添加reset和通信检查?
// code to go here ...
BMI088GyroWriteSingleReg(bmi088, BMI088_GYRO_SOFTRESET, BMI088_GYRO_SOFTRESET_VALUE); // 软复位
DWT_Delay(0.08);
// 检查ID,如果不是0x0F(bmi088 whoami寄存器值),则返回错误
uint8_t whoami_check = 0;
BMI088GyroRead(bmi088, BMI088_GYRO_CHIP_ID, &whoami_check, 1);
if (whoami_check != BMI088_GYRO_CHIP_ID_VALUE)
return BMI088_NO_SENSOR;
DWT_Delay(0.001);
// 初始化寄存器,提高可读性
uint8_t reg = 0, data = 0;
BMI088_ERORR_CODE_e error = 0;
// 使用sizeof而不是magic number,这样如果修改了数组大小,不用修改这里的代码;或者使用宏定义
for (uint8_t i = 0; i < sizeof(BMI088_Gyro_Init_Table) / sizeof(BMI088_Gyro_Init_Table[0]); i++)
{
reg = BMI088_Gyro_Init_Table[i][BMI088REG];
data = BMI088_Gyro_Init_Table[i][BMI088DATA];
BMI088GyroWriteSingleReg(bmi088, reg, data); // 写入寄存器
DWT_Delay(0.001);
BMI088GyroRead(bmi088, reg, &data, 1); // 写完之后立刻读回对应寄存器检查是否写入成功
DWT_Delay(0.001);
if (data != BMI088_Gyro_Init_Table[i][BMI088DATA])
error |= BMI088_Gyro_Init_Table[i][BMI088ERROR];
//{i--;} 可以设置retry次数,尝试重新写入.如果retry次数用完了,则返回error
}
return error;
}
// -------------------------以上为私有函数,用于初始化BMI088acc和gyro的硬件和配置--------------------------------//
// -------------------------以下为私有函数,private用于IT模式下的中断处理---------------------------------//
/**
* @brief 待编写,很快!
*
*/
static void BMI088AccSPIFinishCallback(SPIInstance *spi)
{
// static BMI088Instance *bmi088;
// bmi088 = (BMI088Instance *)(spi->id);
// 若第一次读取加速度,则在这里启动温度读取
// 如果使用异步姿态更新,此处唤醒量测更新的任务
}
static void BMI088GyroSPIFinishCallback(SPIInstance *spi)
{
// static BMI088Instance *bmi088;
// bmi088 = (BMI088Instance *)(spi->id);
// 若不是异步,啥也不做;否则启动姿态的预测步(propagation)
}
static void BMI088AccINTCallback(GPIOInstance *gpio)
{
static BMI088Instance *bmi088;
static uint8_t buf[6] = {0}; // 最多读取6个byte(gyro/acc,temp是2)
bmi088 = (BMI088Instance *)(gpio->id);
bmi088->update_flag.imu_ready = 1;
bmi088->update_flag.acc = 1;
BMI088AccelRead(bmi088, BMI088_ACCEL_XOUT_L, buf, 6);
for (uint8_t i = 0; i < 3; i++)
bmi088->acc[i] = bmi088->acc_coef * (float)(int16_t)(((buf[2 * i + 1]) << 8) | buf[2 * i]);
// 启动加速度计数据读取(和温度读取,如果有必要),并转换为实际值
// 读取完毕会调用BMI088AccSPIFinishCallback
}
static void BMI088GyroINTCallback(GPIOInstance *gpio)
{
static BMI088Instance *bmi088;
static uint8_t buf[6] = {0}; // 最多读取6个byte(gyro/acc,temp是2)
bmi088 = (BMI088Instance *)(gpio->id);
bmi088->update_flag.imu_ready = 1;
bmi088->update_flag.gyro = 1;
uint8_t whoami_check = 0;
//do{BMI088GyroRead(bmi088, BMI088_GYRO_CHIP_ID, &whoami_check, 1);}while(whoami_check != BMI088_GYRO_CHIP_ID_VALUE);
BMI088GyroRead(bmi088, BMI088_GYRO_X_L, buf, 6);
for (uint8_t i = 0; i < 3; i++)
bmi088->gyro[i] = bmi088->BMI088_GYRO_SEN * (float)(int16_t)(((buf[2 * i + 1]) << 8) | buf[2 * i]);
// 启动陀螺仪数据读取,并转换为实际值
// 读取完毕会调用BMI088GyroSPIFinishCallback
}
// -------------------------以上为私有函数,private用于IT模式下的中断处理---------------------------------//
// -------------------------以下为公有函数,用于注册BMI088,标定和数据读取--------------------------------//
/**
* @brief
* @todo 现在要考虑一下数据返回的方式,指针还是结构体?
*
* @param bmi088
* @return BMI088_Data_t
*/
uint8_t BMI088Acquire(BMI088Instance *bmi088, BMI088_Data_t *data_store)
{
// 如果是blocking模式,则主动触发一次读取并返回数据
if (bmi088->work_mode == BMI088_BLOCK_PERIODIC_MODE)
{
static uint8_t buf[6] = {0}; // 最多读取6个byte(gyro/acc,temp是2)
// 读取accel的x轴数据首地址,bmi088内部自增读取地址 // 3* sizeof(int16_t)
BMI088AccelRead(bmi088, BMI088_ACCEL_XOUT_L, buf, 6);
for (uint8_t i = 0; i < 3; i++)
data_store->acc[i] = bmi088->acc_coef * (float)(int16_t)(((buf[2 * i + 1]) << 8) | buf[2 * i]);
BMI088GyroRead(bmi088, BMI088_GYRO_X_L, buf, 6); // 连续读取3个(3*2=6)轴的角速度
for (uint8_t i = 0; i < 3; i++)
data_store->gyro[i] = bmi088->BMI088_GYRO_SEN * (float)(int16_t)(((buf[2 * i + 1]) << 8) | buf[2 * i]);
BMI088AccelRead(bmi088, BMI088_TEMP_M, buf, 2); // 读温度,温度传感器在accel上
data_store->temperature = (float)(int16_t)(((buf[0] << 3) | (buf[1] >> 5))) * BMI088_TEMP_FACTOR + BMI088_TEMP_OFFSET;
return 1;
}
// 如果是IT模式,则检查标志位.当传感器数据准备好会触发外部中断,中断服务函数会将标志位置1
if (bmi088->work_mode == BMI088_BLOCK_TRIGGER_MODE && bmi088->update_flag.imu_ready == 1)
{
if(bmi088->update_flag.acc == 1)
{
memcpy(data_store->acc,bmi088->acc,3*sizeof(float));
bmi088->update_flag.acc = 0;
bmi088->update_flag.imu_ready = 0;
}
if(bmi088->update_flag.gyro == 1)
{
memcpy(data_store->gyro,bmi088->gyro,3*sizeof(float));
bmi088->update_flag.gyro = 0;
bmi088->update_flag.imu_ready = 0;
}
return 1;
}
// 如果数据还没准备好,则返回空数据?或者返回上一次的数据?或者返回错误码? @todo
if (bmi088->update_flag.imu_ready == 0)
return 0;
return 1;
}
/* pre calibrate parameter to go here */
#pragma message "REMEMBER TO SET PRE CALIBRATE PARAMETER IF YOU CHOOSE NOT TO CALIBRATE"
#define BMI088_PRE_CALI_ACC_X_OFFSET 0.0f
#define BMI088_PRE_CALI_ACC_Y_OFFSET 0.0f
#define BMI088_PRE_CALI_ACC_Z_OFFSET 0.0f
#define BMI088_PRE_CALI_G_NORM 9.805f
/**
* @brief BMI088 acc gyro 标定
* @note 标定后的数据存储在bmi088->bias和gNorm中,用于后续数据消噪和单位转换归一化
* @attention 不管工作模式是blocking还是IT,标定时都是blocking模式,所以不用担心中断关闭后无法标定(RobotInit关闭了全局中断)
* @attention 标定精度和等待时间有关,目前使用线性回归.后续考虑引入非线性回归
* @todo 将标定次数(等待时间)变为参数供设定
* @section 整体流程为1.累加加速度数据计算gNrom() 2.累加陀螺仪数据计算零飘
* 3. 如果标定过程运动幅度过大,重新标定 4.保存标定参数
*
* @param _bmi088 待标定的BMI088实例
*/
void BMI088CalibrateIMU(BMI088Instance *_bmi088)
{
if (_bmi088->cali_mode == BMI088_CALIBRATE_ONLINE_MODE) // 性感bmi088在线标定,耗时6s
{
_bmi088->acc_coef = BMI088_ACCEL_6G_SEN; // 标定完后要乘以9.805/gNorm
_bmi088->BMI088_GYRO_SEN = BMI088_GYRO_2000_SEN; // 后续改为从initTable中获取
// 一次性参数用完就丢,不用static
float startTime; // 开始标定时间,用于确定是否超时
uint16_t CaliTimes = 6000; // 标定次数(6s)
float gyroMax[3], gyroMin[3]; // 保存标定过程中读取到的数据最大值判断是否满足标定环境
float gNormTemp, gNormMax, gNormMin; // 同上,计算矢量范数(模长)
float gyroDiff[3], gNormDiff; // 每个轴的最大角速度跨度及其模长
BMI088_Data_t raw_data;
startTime = DWT_GetTimeline_s();
// 循环继续的条件为标定环境不满足
do // 用do while至少执行一次,省得对上面的参数进行初始化
{ // 标定超时,直接使用预标定参数(如果有)
if (DWT_GetTimeline_s() - startTime > 12.01)
{ // 两次都没有成功就切换标定模式,丢给下一个if处理,使用预标定参数
_bmi088->cali_mode = BMI088_LOAD_PRE_CALI_MODE;
break;
}
DWT_Delay(0.0005);
_bmi088->gNorm = 0;
for (uint8_t i = 0; i < 3; i++) // 重置gNorm和零飘
_bmi088->gyro_offset[i] = 0;
// @todo : 这里也有获取bmi088数据的操作,后续与BMI088Acquire合并.注意标定时的工作模式是阻塞,且offset和acc_coef要初始化成0和1,标定完成后再设定为标定值
for (uint16_t i = 0; i < CaliTimes; ++i) // 提前计算,优化
{
BMI088Acquire(_bmi088, &raw_data);
gNormTemp = NormOf3d(raw_data.acc);
_bmi088->gNorm += gNormTemp; // 计算范数并累加,最后除以calib times获取单次值
for (uint8_t ii = 0; ii < 3; ii++)
_bmi088->gyro_offset[ii] += raw_data.gyro[ii]; // 因为标定时传感器静止,所以采集到的值就是漂移,累加当前值,最后除以calib times获得零飘
if (i == 0) // 避免未定义的行为(else中)
{
gNormMax = gNormMin = gNormTemp; // 初始化成当前的重力加速度模长
for (uint8_t j = 0; j < 3; ++j)
{
gyroMax[j] = raw_data.gyro[j];
gyroMin[j] = raw_data.gyro[j];
}
}
else // 更新gNorm的Min Max和gyro的minmax
{
gNormMax = gNormMax > gNormTemp ? gNormMax : gNormTemp;
gNormMin = gNormMin < gNormTemp ? gNormMin : gNormTemp;
for (uint8_t j = 0; j < 3; ++j)
{
gyroMax[j] = gyroMax[j] > _bmi088->gyro[j] ? gyroMax[j] : _bmi088->gyro[j];
gyroMin[j] = gyroMin[j] < _bmi088->gyro[j] ? gyroMin[j] : _bmi088->gyro[j];
}
}
gNormDiff = gNormMax - gNormMin; // 最大值和最小值的差
for (uint8_t j = 0; j < 3; ++j)
gyroDiff[j] = gyroMax[j] - gyroMin[j]; // 分别计算三轴
if (gNormDiff > 0.5f ||
gyroDiff[0] > 0.15f ||
gyroDiff[1] > 0.15f ||
gyroDiff[2] > 0.15f)
break; // 超出范围了,重开! remake到while循环,外面还有一层
DWT_Delay(0.0005); // 休息一会再开始下一轮数据获取,IMU准备数据需要时间
}
_bmi088->gNorm /= (float)CaliTimes; // 加速度范数重力
for (uint8_t i = 0; i < 3; ++i)
_bmi088->gyro_offset[i] /= (float)CaliTimes; // 三轴零飘
// 这里直接存到temperature,可以另外增加BMI088Instance的成员变量TempWhenCalib
_bmi088->temperature = raw_data.temperature * BMI088_TEMP_FACTOR + BMI088_TEMP_OFFSET; // 保存标定时的温度,如果已知温度和零飘的关系
// caliTryOutCount++; 保存已经尝试的标定次数?由你.
} while (gNormDiff > 0.5f ||
fabsf(_bmi088->gNorm - 9.8f) > 0.5f ||
gyroDiff[0] > 0.15f ||
gyroDiff[1] > 0.15f ||
gyroDiff[2] > 0.15f ||
fabsf(_bmi088->gyro_offset[0]) > 0.01f ||
fabsf(_bmi088->gyro_offset[1]) > 0.01f ||
fabsf(_bmi088->gyro_offset[2]) > 0.01f); // 满足条件说明标定环境不好
}
// 离线标定
if (_bmi088->cali_mode == BMI088_LOAD_PRE_CALI_MODE) // 如果标定失败也会进来,直接使用离线数据
{
_bmi088->gyro_offset[0] = BMI088_PRE_CALI_ACC_X_OFFSET;
_bmi088->gyro_offset[1] = BMI088_PRE_CALI_ACC_Y_OFFSET;
_bmi088->gyro_offset[2] = BMI088_PRE_CALI_ACC_Z_OFFSET;
_bmi088->gNorm = BMI088_PRE_CALI_G_NORM;
}
_bmi088->acc_coef *= 9.805 / _bmi088->gNorm;
}
// 考虑阻塞模式和非阻塞模式的兼容性,通过条件编译(则需要在编译前修改宏定义)或runtime参数判断
// runtime的开销不大(一次性判断),但是需要修改函数原型,增加参数,代码长度增加(但不多)
// runtime如何修改callback?根据参数选择是否给spi传入callback,如果是阻塞模式,则不传入callback,如果是非阻塞模式,则传入callback(bsp会检查是否NULL)
// 条件编译的开销小,但是需要修改宏定义,增加编译时间,同时人力介入
// 根据实际情况选择(说了和没说一样!)
BMI088Instance *BMI088Register(BMI088_Init_Config_s *config)
{
// 申请内存
BMI088Instance *bmi088_instance = (BMI088Instance *)zmalloc(sizeof(BMI088Instance));
// 从右向左赋值,让bsp instance保存指向bmi088_instance的指针(父指针),便于在底层中断中访问bmi088_instance
config->acc_int_config.id =
config->gyro_int_config.id =
config->spi_acc_config.id =
config->spi_gyro_config.id =
config->heat_pwm_config.id = bmi088_instance;
// @todo:
// 目前只实现了!!!阻塞读取模式!!!.如果需要使用IT模式,则需要修改这里的代码,为spi和gpio注册callback(默认为NULL)
// 还需要设置SPI的传输模式为DMA模式或IT模式(默认为blocking)
// 可以通过conditional compilation或者runtime参数判断
// code to go here ...
// INT_ACC EXTI CALLBACK: 检查是否有传输正在进行,如果没有则开启SPI DMA传输,有则置位wait标志位;
// 第一次是加速度计,第二次是温度.
// INT_GYRO EXTI CALLBACK: 开启SPI DMA传输,不会出现等待传输的情况
// SPI_GYRO DMA CALLBACK: 解算陀螺仪数据,
// SPI_ACC DMA CALLBACK: 解算加速度计数据,清除温度wait标志位并启动温度传输,第二次进入中断时解算温度数据
// 还有其他方案可用,比如阻塞等待传输完成,但是比较笨.
config->spi_acc_config.spi_work_mode = SPI_BLOCK_MODE;
config->spi_gyro_config.spi_work_mode = SPI_BLOCK_MODE;
// 根据参数选择工作模式
bmi088_instance->spi_acc = SPIRegister(&config->spi_acc_config);
bmi088_instance->spi_gyro = SPIRegister(&config->spi_gyro_config);
bmi088_instance->heat_pwm = PWMRegister(&config->heat_pwm_config);
PIDInit(&bmi088_instance->heat_pid, &config->heat_pid_config);
// 初始化acc和gyro
BMI088_ERORR_CODE_e error = BMI088_NO_ERROR;
do
{
error = BMI088_NO_ERROR;
error |= BMI088AccelInit(bmi088_instance);
error |= BMI088GyroInit(bmi088_instance);
// 可以增加try out times,超出次数则返回错误
} while (error != 0);
bmi088_instance->work_mode = BMI088_BLOCK_PERIODIC_MODE; // 临时设置为阻塞模式
BMI088CalibrateIMU(bmi088_instance); // 标定acc和gyro
bmi088_instance->work_mode = config->work_mode; // 恢复工作模式
if (config->work_mode == BMI088_BLOCK_TRIGGER_MODE)
{
bmi088_instance->spi_acc->spi_work_mode = SPI_DMA_MODE;
bmi088_instance->spi_gyro->spi_work_mode = SPI_DMA_MODE;
// 设置回调函数
bmi088_instance->spi_acc->callback = BMI088AccSPIFinishCallback;
bmi088_instance->spi_gyro->callback = BMI088GyroSPIFinishCallback;
bmi088_instance->acc_int = GPIORegister(&config->acc_int_config); // 只有在非阻塞模式下才需要注册中断
bmi088_instance->gyro_int = GPIORegister(&config->gyro_int_config);
bmi088_instance->acc_int->gpio_model_callback = BMI088AccINTCallback;
bmi088_instance->gyro_int->gpio_model_callback = BMI088GyroINTCallback;
} // 注册实例
return bmi088_instance;
}
modules/BMI088/bmi088.h
+116
View File
@@ -0,0 +1,116 @@
#ifndef __BMI088_H__ // 防止重复包含
#define __BMI088_H__
#include "bsp_spi.h"
#include "bsp_gpio.h"
#include "controller.h"
#include "bsp_pwm.h"
#include "stdint.h"
// bmi088工作模式枚举
typedef enum
{
BMI088_BLOCK_PERIODIC_MODE = 0, // 阻塞模式,周期性读取
BMI088_BLOCK_TRIGGER_MODE, // 阻塞模式,触发读取(中断)
} BMI088_Work_Mode_e;
// bmi088标定方式枚举,若使用预设标定参数,注意修改预设参数
typedef enum
{
BMI088_CALIBRATE_ONLINE_MODE = 0, // 初始化时进行标定
BMI088_LOAD_PRE_CALI_MODE, // 使用预设标定参数,
} BMI088_Calibrate_Mode_e;
#pragma pack(1) // 1字节对齐
/* BMI088数据*/
typedef struct
{
float gyro[3]; // 陀螺仪数据,xyz
float acc[3]; // 加速度计数据,xyz
float temperature; // 温度
// float timestamp; // 时间戳,单位为ms,用于计算两次采样的时间间隔,同时给视觉提供timeline
// uint32_t count; // 第count次采样,用于对齐时间戳
} BMI088_Data_t;
#pragma pack() // 恢复默认对齐,需要传输的结构体务必开启1字节对齐
/* BMI088实例结构体定义 */
typedef struct
{
// 传输模式和工作模式控制
BMI088_Work_Mode_e work_mode;
BMI088_Calibrate_Mode_e cali_mode;
// SPI接口
SPIInstance *spi_gyro; // 注意,SPIInstnace内部也有一个GPIOInstance,用于控制片选CS
SPIInstance *spi_acc; // 注意,SPIInstnace内部也有一个GPIOInstance,用于控制片选CS
// EXTI GPIO,如果BMI088工作在中断模式,则需要配置中断引脚(有数据产生时触发解算)
GPIOInstance *gyro_int;
GPIOInstance *acc_int;
// 温度控制
PIDInstance heat_pid; // 恒温PID
PWMInstance *heat_pwm; // 加热PWM
// IMU数据
float gyro[3]; // 陀螺仪数据,xyz
float acc[3]; // 加速度计数据,xyz
float temperature; // 温度
// 标定数据
float gyro_offset[3]; // 陀螺仪零偏
float gNorm; // 重力加速度模长,从标定获取
float acc_coef; // 加速度计原始数据转换系数
// 传感器灵敏度,用于计算实际值(regNdef.h中定义)
float BMI088_ACCEL_SEN;
float BMI088_GYRO_SEN;
// 用于计算两次采样的时间间隔
uint32_t bias_dwt_cnt;
// 数据更新标志位
struct // 位域,节省空间提高可读性
{
uint8_t gyro : 1; // 1:有新数据,0:无新数据
uint8_t acc : 1;
uint8_t temp : 1;
uint8_t gyro_overrun : 1; // 1:数据溢出,0:无溢出
uint8_t acc_overrun : 1;
uint8_t temp_overrun : 1;
uint8_t imu_ready : 1; // 1:IMU数据准备好,0:IMU数据未准备好(gyro+acc)
// 后续可添加其他标志位,不够用可以扩充16or32,太多可以删
} update_flag;
} BMI088Instance;
/* BMI088初始化配置 */
typedef struct
{
BMI088_Work_Mode_e work_mode;
BMI088_Calibrate_Mode_e cali_mode;
SPI_Init_Config_s spi_gyro_config;
SPI_Init_Config_s spi_acc_config;
GPIO_Init_Config_s gyro_int_config;
GPIO_Init_Config_s acc_int_config;
PID_Init_Config_s heat_pid_config;
PWM_Init_Config_s heat_pwm_config;
} BMI088_Init_Config_s;
/**
* @brief 初始化BMI088,返回BMI088实例指针
* @note 一般一个开发板只有一个BMI088,所以这里就叫BMI088Init而不是Register
*
* @param config bmi088初始化配置
* @return BMI088Instance* 实例指针
*/
BMI088Instance *BMI088Register(BMI088_Init_Config_s *config);
/**
* @brief 读取BMI088数据
* @param bmi088 BMI088实例指针
* @return BMI088_Data_t 读取到的数据
*/
uint8_t BMI088Acquire(BMI088Instance *bmi088,BMI088_Data_t* data_store);
/**
* @brief 标定传感器.BMI088在初始化的时候会调用此函数. 提供接口方便标定离线数据
* @attention @todo 注意,当操作系统开始运行后,此函数会和INS_Task冲突.目前不允许在运行时调用此函数,后续加入标志位判断以提供运行时重新的标定功能
*
* @param _bmi088 待标定的实例
*/
void BMI088CalibrateIMU(BMI088Instance *_bmi088);
#endif // !__BMI088_H__
modules/BMI088/bmi088.md
+98
View File
@@ -0,0 +1,98 @@
# BMI088
## 示例
```c
BMI088_Init_Config_s imu_config = {
.spi_acc_config={
.GPIOx=GPIOA,
.GPIOx=GPIO_PIN_4,
.spi_handle=&hspi1,
},
.spi_gyro_config={
.GPIOx=GPIOB,
.GPIOx=GPIO_PIN_0,
.spi_handle=&hspi1,
},
.acc_int_config={
.exti_mode=EXTI_TRIGGER_FALLING,
.GPIO_Pin=GPIO_PIN_4,
.GPIOx=GPIOC,
},
.gyro_int_config={
.exti_mode=EXTI_TRIGGER_FALLING,
.GPIO_Pin=GPIO_PIN_5,
.GPIOx=GPIOC,
},
.heat_pid_config={
.Kp=0.0f,
.Kd=0.0f,
.Ki=0.0f,
.MaxOut=0.0f,
.DeadBand=0.0f,
},
.heat_pwm_config={
.channel=TIM_CHANNEL_1,
.htim=&htim1,
},
.cali_mode=BMI088_CALIBRATE_ONLINE_MODE,
.work_mode=BMI088_BLOCK_PERIODIC_MODE,
};
BMI088Instance* imu=BMI088Register(&imu_config);
```
## IT非阻塞模式下BMI088读取流程
数据准备完成标志位:`uint8_t BMI088_READY_FLAG` 总共8个位 也可以用位域可读性更高
1. 当accel int中断发生,开启DMA SPI传输,完成后将acc ready置位
2. 当gyro int中断发生,开启DMA SPI传输,完成后将gyro ready置位
3. 当温度数据中断(温度传感器在accel内部,也是accel int)发生,开启DMA传输,完成后将温度标志位置位
> 由于DMA传输非阻塞,启动传输后只有到传输完成时才会拉高片选结束SPI transfer,因此需要在callback中加入标志位置位的操作.
这可以通过条件编译完成.
温度数据不需要和accel和gyro同步,它不参与姿态解算,可以不用管.
当加速度数据和陀螺仪数据都准备完成之后,唤醒姿态解算任务INs_taSk,进行一次解算.唤醒可以通过软件开启EXTI中断,在中断中调用实时系统的vTaskNotifyFromISR()完成,也可以将任务ready标志位置位,当运行到对应位置时检查标志位判断是否要进行任务.时间间隔不是大问题,inS_TAsK中有dwt用于计算两次任务执行的间隔,它将会自动处理好bias的大小
`__HAL_GPIO_EXTI_GENERATE_SWIT()` `HAL_EXTI_GENERATE_SWI()` 可以触发软件中断
of course,两者的数据更新实际上可以异步进行,这里为了方便起见当两者数据都准备好以后再行融合
## 数据读写规则(so called 16-bit protocol)
加速度计读取read:
1. bit 0 :1 bit 1-7: reg address
2. dummy read,加速度计此时返回的数据无效
3. 真正的数据从第三个字节开始.
通过SPI传输三个字节(分三次)
byte1: 1(读)+7位寄存器地址
byte2: 没用
byte3: 读取到的数据
write写入:
1. bit 0: 0 bit1-7: reg address
2. 要写入寄存器的数据(注意没有dummy byte)
---
**注意,陀螺仪和加速度计的读取不同**
陀螺仪gyro读取read:
1. bit 0 :1 bit1-7: reg address
2. 读回的数据
通过SPI传输两个字节(分两次)
byte1: 1(读)+7位寄存器地址
byte2: 读取到的数据
write写入:
1. bit0 : 0 bit1-7 : reg address
2. 写入的数据
modules/BMI088/bmi088_regNdef.h
+234
View File
@@ -0,0 +1,234 @@
#ifndef BMI088REG_H
#define BMI088REG_H
/*------- BMI088寄存器地址及其存放的内容-------*/
#define BMI088_ACC_CHIP_ID 0x00 // the register is " Who am I "
#define BMI088_ACC_CHIP_ID_VALUE 0x1E
#define BMI088_ACC_ERR_REG 0x02
#define BMI088_ACCEL_CONGIF_ERROR_SHFITS 0x2
#define BMI088_ACCEL_CONGIF_ERROR (1 << BMI088_ACCEL_CONGIF_ERROR_SHFITS)
#define BMI088_FATAL_ERROR_SHFITS 0x0
#define BMI088_FATAL_ERROR (1 << BMI088_FATAL_ERROR)
#define BMI088_ACC_STATUS 0x03
#define BMI088_ACCEL_DRDY_SHFITS 0x7
#define BMI088_ACCEL_DRDY (1 << BMI088_ACCEL_DRDY_SHFITS)
#define BMI088_ACCEL_XOUT_L 0x12
#define BMI088_ACCEL_XOUT_M 0x13
#define BMI088_ACCEL_YOUT_L 0x14
#define BMI088_ACCEL_YOUT_M 0x15
#define BMI088_ACCEL_ZOUT_L 0x16
#define BMI088_ACCEL_ZOUT_M 0x17
#define BMI088_SENSORTIME_DATA_L 0x18
#define BMI088_SENSORTIME_DATA_M 0x19
#define BMI088_SENSORTIME_DATA_H 0x1A
#define BMI088_ACC_INT_STAT_1 0x1D
#define BMI088_ACCEL_DRDY_INTERRUPT_SHFITS 0x7
#define BMI088_ACCEL_DRDY_INTERRUPT (1 << BMI088_ACCEL_DRDY_INTERRUPT_SHFITS)
#define BMI088_TEMP_M 0x22
#define BMI088_TEMP_L 0x23
#define BMI088_ACC_CONF 0x40
#define BMI088_ACC_CONF_MUST_Set 0x80
#define BMI088_ACC_BWP_SHFITS 0x4
#define BMI088_ACC_OSR4 (0x0 << BMI088_ACC_BWP_SHFITS)
#define BMI088_ACC_OSR2 (0x1 << BMI088_ACC_BWP_SHFITS)
#define BMI088_ACC_NORMAL (0x2 << BMI088_ACC_BWP_SHFITS)
#define BMI088_ACC_ODR_SHFITS 0x0
#define BMI088_ACC_12_5_HZ (0x5 << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_25_HZ (0x6 << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_50_HZ (0x7 << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_100_HZ (0x8 << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_200_HZ (0x9 << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_400_HZ (0xA << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_800_HZ (0xB << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_1600_HZ (0xC << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_RANGE 0x41
#define BMI088_ACC_RANGE_SHFITS 0x0
#define BMI088_ACC_RANGE_3G (0x0 << BMI088_ACC_RANGE_SHFITS)
#define BMI088_ACC_RANGE_6G (0x1 << BMI088_ACC_RANGE_SHFITS)
#define BMI088_ACC_RANGE_12G (0x2 << BMI088_ACC_RANGE_SHFITS)
#define BMI088_ACC_RANGE_24G (0x3 << BMI088_ACC_RANGE_SHFITS)
#define BMI088_INT1_IO_CTRL 0x53
#define BMI088_ACC_INT1_IO_ENABLE_SHFITS 0x3
#define BMI088_ACC_INT1_IO_ENABLE (0x1 << BMI088_ACC_INT1_IO_ENABLE_SHFITS)
#define BMI088_ACC_INT1_GPIO_MODE_SHFITS 0x2
#define BMI088_ACC_INT1_GPIO_PP (0x0 << BMI088_ACC_INT1_GPIO_MODE_SHFITS)
#define BMI088_ACC_INT1_GPIO_OD (0x1 << BMI088_ACC_INT1_GPIO_MODE_SHFITS)
#define BMI088_ACC_INT1_GPIO_LVL_SHFITS 0x1
#define BMI088_ACC_INT1_GPIO_LOW (0x0 << BMI088_ACC_INT1_GPIO_LVL_SHFITS)
#define BMI088_ACC_INT1_GPIO_HIGH (0x1 << BMI088_ACC_INT1_GPIO_LVL_SHFITS)
#define BMI088_INT2_IO_CTRL 0x54
#define BMI088_ACC_INT2_IO_ENABLE_SHFITS 0x3
#define BMI088_ACC_INT2_IO_ENABLE (0x1 << BMI088_ACC_INT2_IO_ENABLE_SHFITS)
#define BMI088_ACC_INT2_GPIO_MODE_SHFITS 0x2
#define BMI088_ACC_INT2_GPIO_PP (0x0 << BMI088_ACC_INT2_GPIO_MODE_SHFITS)
#define BMI088_ACC_INT2_GPIO_OD (0x1 << BMI088_ACC_INT2_GPIO_MODE_SHFITS)
#define BMI088_ACC_INT2_GPIO_LVL_SHFITS 0x1
#define BMI088_ACC_INT2_GPIO_LOW (0x0 << BMI088_ACC_INT2_GPIO_LVL_SHFITS)
#define BMI088_ACC_INT2_GPIO_HIGH (0x1 << BMI088_ACC_INT2_GPIO_LVL_SHFITS)
#define BMI088_INT_MAP_DATA 0x58
#define BMI088_ACC_INT2_DRDY_INTERRUPT_SHFITS 0x6
#define BMI088_ACC_INT2_DRDY_INTERRUPT (0x1 << BMI088_ACC_INT2_DRDY_INTERRUPT_SHFITS)
#define BMI088_ACC_INT1_DRDY_INTERRUPT_SHFITS 0x2
#define BMI088_ACC_INT1_DRDY_INTERRUPT (0x1 << BMI088_ACC_INT1_DRDY_INTERRUPT_SHFITS)
#define BMI088_ACC_SELF_TEST 0x6D
#define BMI088_ACC_SELF_TEST_OFF 0x00
#define BMI088_ACC_SELF_TEST_POSITIVE_SIGNAL 0x0D
#define BMI088_ACC_SELF_TEST_NEGATIVE_SIGNAL 0x09
#define BMI088_ACC_PWR_CONF 0x7C
#define BMI088_ACC_PWR_SUSPEND_MODE 0x03
#define BMI088_ACC_PWR_ACTIVE_MODE 0x00
#define BMI088_ACC_PWR_CTRL 0x7D
#define BMI088_ACC_ENABLE_ACC_OFF 0x00
#define BMI088_ACC_ENABLE_ACC_ON 0x04
#define BMI088_ACC_SOFTRESET 0x7E
#define BMI088_ACC_SOFTRESET_VALUE 0xB6
#define BMI088_GYRO_CHIP_ID 0x00
#define BMI088_GYRO_CHIP_ID_VALUE 0x0F
#define BMI088_GYRO_X_L 0x02
#define BMI088_GYRO_X_H 0x03
#define BMI088_GYRO_Y_L 0x04
#define BMI088_GYRO_Y_H 0x05
#define BMI088_GYRO_Z_L 0x06
#define BMI088_GYRO_Z_H 0x07
#define BMI088_GYRO_INT_STAT_1 0x0A
#define BMI088_GYRO_DYDR_SHFITS 0x7
#define BMI088_GYRO_DYDR (0x1 << BMI088_GYRO_DYDR_SHFITS)
#define BMI088_GYRO_RANGE 0x0F
#define BMI088_GYRO_RANGE_SHFITS 0x0
#define BMI088_GYRO_2000 (0x0 << BMI088_GYRO_RANGE_SHFITS)
#define BMI088_GYRO_1000 (0x1 << BMI088_GYRO_RANGE_SHFITS)
#define BMI088_GYRO_500 (0x2 << BMI088_GYRO_RANGE_SHFITS)
#define BMI088_GYRO_250 (0x3 << BMI088_GYRO_RANGE_SHFITS)
#define BMI088_GYRO_125 (0x4 << BMI088_GYRO_RANGE_SHFITS)
#define BMI088_GYRO_BANDWIDTH 0x10
// the first num means Output data rate, the second num means bandwidth
#define BMI088_GYRO_BANDWIDTH_MUST_Set 0x80
#define BMI088_GYRO_2000_532_HZ 0x00
#define BMI088_GYRO_2000_230_HZ 0x01
#define BMI088_GYRO_1000_116_HZ 0x02
#define BMI088_GYRO_400_47_HZ 0x03
#define BMI088_GYRO_200_23_HZ 0x04
#define BMI088_GYRO_100_12_HZ 0x05
#define BMI088_GYRO_200_64_HZ 0x06
#define BMI088_GYRO_100_32_HZ 0x07
#define BMI088_GYRO_LPM1 0x11
#define BMI088_GYRO_NORMAL_MODE 0x00
#define BMI088_GYRO_SUSPEND_MODE 0x80
#define BMI088_GYRO_DEEP_SUSPEND_MODE 0x20
#define BMI088_GYRO_SOFTRESET 0x14
#define BMI088_GYRO_SOFTRESET_VALUE 0xB6
#define BMI088_GYRO_CTRL 0x15
#define BMI088_DRDY_OFF 0x00
#define BMI088_DRDY_ON 0x80
#define BMI088_GYRO_INT3_INT4_IO_CONF 0x16
#define BMI088_GYRO_INT4_GPIO_MODE_SHFITS 0x3
#define BMI088_GYRO_INT4_GPIO_PP (0x0 << BMI088_GYRO_INT4_GPIO_MODE_SHFITS)
#define BMI088_GYRO_INT4_GPIO_OD (0x1 << BMI088_GYRO_INT4_GPIO_MODE_SHFITS)
#define BMI088_GYRO_INT4_GPIO_LVL_SHFITS 0x2
#define BMI088_GYRO_INT4_GPIO_LOW (0x0 << BMI088_GYRO_INT4_GPIO_LVL_SHFITS)
#define BMI088_GYRO_INT4_GPIO_HIGH (0x1 << BMI088_GYRO_INT4_GPIO_LVL_SHFITS)
#define BMI088_GYRO_INT3_GPIO_MODE_SHFITS 0x1
#define BMI088_GYRO_INT3_GPIO_PP (0x0 << BMI088_GYRO_INT3_GPIO_MODE_SHFITS)
#define BMI088_GYRO_INT3_GPIO_OD (0x1 << BMI088_GYRO_INT3_GPIO_MODE_SHFITS)
#define BMI088_GYRO_INT3_GPIO_LVL_SHFITS 0x0
#define BMI088_GYRO_INT3_GPIO_LOW (0x0 << BMI088_GYRO_INT3_GPIO_LVL_SHFITS)
#define BMI088_GYRO_INT3_GPIO_HIGH (0x1 << BMI088_GYRO_INT3_GPIO_LVL_SHFITS)
#define BMI088_GYRO_INT3_INT4_IO_MAP 0x18
#define BMI088_GYRO_DRDY_IO_OFF 0x00
#define BMI088_GYRO_DRDY_IO_INT3 0x01
#define BMI088_GYRO_DRDY_IO_INT4 0x80
#define BMI088_GYRO_DRDY_IO_BOTH (BMI088_GYRO_DRDY_IO_INT3 | BMI088_GYRO_DRDY_IO_INT4)
#define BMI088_GYRO_SELF_TEST 0x3C
#define BMI088_GYRO_RATE_OK_SHFITS 0x4
#define BMI088_GYRO_RATE_OK (0x1 << BMI088_GYRO_RATE_OK_SHFITS)
#define BMI088_GYRO_BIST_FAIL_SHFITS 0x2
#define BMI088_GYRO_BIST_FAIL (0x1 << BMI088_GYRO_BIST_FAIL_SHFITS)
#define BMI088_GYRO_BIST_RDY_SHFITS 0x1
#define BMI088_GYRO_BIST_RDY (0x1 << BMI088_GYRO_BIST_RDY_SHFITS)
#define BMI088_GYRO_TRIG_BIST_SHFITS 0x0
#define BMI088_GYRO_TRIG_BIST (0x1 << BMI088_GYRO_TRIG_BIST_SHFITS)
/* -------BMI088 配置和传感器灵敏度换算系数等------- */
#define BMI088_TEMP_FACTOR 0.125f
#define BMI088_TEMP_OFFSET 23.0f
#define BMI088_WRITE_ACCEL_REG_NUM 6
#define BMI088_WRITE_GYRO_REG_NUM 6
#define BMI088_GYRO_DATA_READY_BIT 0
#define BMI088_ACCEL_DATA_READY_BIT 1
#define BMI088_ACCEL_TEMP_DATA_READY_BIT 2
#define BMI088_LONG_DELAY_TIME 80
#define BMI088_COM_WAIT_SENSOR_TIME 150
#define BMI088_ACCEL_IIC_ADDRESSE (0x18 << 1)
#define BMI088_GYRO_IIC_ADDRESSE (0x68 << 1)
#define BMI088_ACCEL_3G_SEN 0.0008974358974f
#define BMI088_ACCEL_6G_SEN 0.00179443359375f
#define BMI088_ACCEL_12G_SEN 0.0035888671875f
#define BMI088_ACCEL_24G_SEN 0.007177734375f
#define BMI088_GYRO_2000_SEN 0.00106526443603169529841533860381f
#define BMI088_GYRO_1000_SEN 0.00053263221801584764920766930190693f
#define BMI088_GYRO_500_SEN 0.00026631610900792382460383465095346f
#define BMI088_GYRO_250_SEN 0.00013315805450396191230191732547673f
#define BMI088_GYRO_125_SEN 0.000066579027251980956150958662738366f
/* BMI088错误码枚举 */
typedef enum
{
BMI088_NO_ERROR = 0x00,
BMI088_ACC_PWR_CTRL_ERROR = 0x01,
BMI088_ACC_PWR_CONF_ERROR = 0x02,
BMI088_ACC_CONF_ERROR = 0x03,
BMI088_ACC_SELF_TEST_ERROR = 0x04,
BMI088_ACC_RANGE_ERROR = 0x05,
BMI088_INT1_IO_CTRL_ERROR = 0x06,
BMI088_INT_MAP_DATA_ERROR = 0x07,
BMI088_GYRO_RANGE_ERROR = 0x08,
BMI088_GYRO_BANDWIDTH_ERROR = 0x09,
BMI088_GYRO_LPM1_ERROR = 0x0A,
BMI088_GYRO_CTRL_ERROR = 0x0B,
BMI088_GYRO_INT3_INT4_IO_CONF_ERROR = 0x0C,
BMI088_GYRO_INT3_INT4_IO_MAP_ERROR = 0x0D,
BMI088_SELF_TEST_ACCEL_ERROR = 0x80,
BMI088_SELF_TEST_GYRO_ERROR = 0x40,
BMI088_NO_SENSOR = 0xFF,
} BMI088_ERORR_CODE_e;
#endif
modules/TFminiPlus/tfminiplus.c
+30
View File
@@ -0,0 +1,30 @@
#include "tfminiplus.h"
#include "stdlib.h"
#include "bsp_dwt.h"
static TFMiniInstance *tfmini;
TFMiniInstance *TFMiniRegister(I2C_HandleTypeDef *hi2c)
{
tfmini = (TFMiniInstance *)malloc(sizeof(TFMiniInstance));
IIC_Init_Config_s conf = {
.handle = hi2c,
.dev_address = 0x10,
.id = tfmini,
.work_mode = IIC_BLOCK_MODE,
};
tfmini->iic = IICRegister(&conf);
DWT_Delay(0.5);
return tfmini;
}
float GetDistance(TFMiniInstance *tfmini)
{
uint8_t cmd_buf[5] = {0x5A, 0x05, 0x00, 0x06, 0x65};
IICTransmit(tfmini->iic, cmd_buf, 5, IIC_SEQ_RELEASE);
IICReceive(tfmini->iic, tfmini->buf, 11, IIC_SEQ_RELEASE);
tfmini->Mode = tfmini->buf[6];
tfmini->Dist = (uint16_t)tfmini->buf[2] + (((uint16_t)tfmini->buf[3]) << 8);
tfmini->Strength = tfmini->buf[4] | (tfmini->buf[5] << 8);
return tfmini->Dist;
}
modules/TFminiPlus/tfminiplus.h
+20
View File
@@ -0,0 +1,20 @@
#ifndef __TFMINIPLUS_H__
#define __TFMINIPLUS_H__
#include "stdint.h"
#include "bsp_iic.h"
typedef struct
{
IICInstance *iic;
uint8_t Mode; //= buf[6];
uint16_t Dist; //= buf[2] | (buf[3] << 8);
uint32_t Strength; //= buf[4] | (buf[5] << 8);
uint8_t buf[9];
} TFMiniInstance;
TFMiniInstance *TFMiniRegister(I2C_HandleTypeDef *hi2c);
float GetDistance(TFMiniInstance *tfmini);
#endif
modules/TFminiPlus/tfminiplus.md
+3
View File
@@ -0,0 +1,3 @@
# tfminiplus
北醒激光单点激光雷达模块的简单实现。目前使用iic阻塞通信耗时和速度都太慢，需修改为中断读取，bsp_iic已经提供相应的接口。
modules/alarm/buzzer.c
+93
View File
@@ -0,0 +1,93 @@
#include "bsp_pwm.h"
#include "buzzer.h"
#include "bsp_dwt.h"
#include "string.h"
static PWMInstance *buzzer;
// static uint8_t idx;
static BuzzzerInstance *buzzer_list[BUZZER_DEVICE_CNT] = {0};
/**
* @brief 蜂鸣器初始化
*
*/
void BuzzerInit()
{
PWM_Init_Config_s buzzer_config = {
.htim = &htim12,
.channel = TIM_CHANNEL_2,
.dutyratio = 0,
.period = 0.001,
};
buzzer = PWMRegister(&buzzer_config);
}
BuzzzerInstance *BuzzerRegister(Buzzer_config_s *config)
{
if (config->alarm_level > BUZZER_DEVICE_CNT) // 超过最大实例数,考虑增加或查看是否有内存泄漏
while (1)
;
BuzzzerInstance *buzzer_temp = (BuzzzerInstance *)malloc(sizeof(BuzzzerInstance));
memset(buzzer_temp, 0, sizeof(BuzzzerInstance));
buzzer_temp->alarm_level = config->alarm_level;
buzzer_temp->loudness = config->loudness;
buzzer_temp->octave = config->octave;
buzzer_temp->alarm_state = ALARM_OFF;
buzzer_list[config->alarm_level] = buzzer_temp;
return buzzer_temp;
}
void AlarmSetStatus(BuzzzerInstance *buzzer, AlarmState_e state)
{
buzzer->alarm_state = state;
}
void BuzzerTask()
{
BuzzzerInstance *buzz;
for (size_t i = 0; i < BUZZER_DEVICE_CNT; ++i)
{
buzz = buzzer_list[i];
if (buzz->alarm_level > ALARM_LEVEL_LOW)
{
continue;
}
if (buzz->alarm_state == ALARM_OFF)
{
PWMSetDutyRatio(buzzer, 0);
}
else
{
PWMSetDutyRatio(buzzer, buzz->loudness);
switch (buzz->octave)
{
case OCTAVE_1:
PWMSetPeriod(buzzer, (float)1 / DoFreq);
break;
case OCTAVE_2:
PWMSetPeriod(buzzer, (float)1 / ReFreq);
break;
case OCTAVE_3:
PWMSetPeriod(buzzer, (float)1 / MiFreq);
break;
case OCTAVE_4:
PWMSetPeriod(buzzer, (float)1 / FaFreq);
break;
case OCTAVE_5:
PWMSetPeriod(buzzer, (float)1 / SoFreq);
break;
case OCTAVE_6:
PWMSetPeriod(buzzer, (float)1 / LaFreq);
break;
case OCTAVE_7:
PWMSetPeriod(buzzer, (float)1 / SiFreq);
break;
default:
break;
}
break;
}
}
}
modules/alarm/buzzer.h
+60
View File
@@ -0,0 +1,60 @@
#ifndef BUZZER_H
#define BUZZER_H
#include "bsp_pwm.h"
#define BUZZER_DEVICE_CNT 5
#define DoFreq 523
#define ReFreq 587
#define MiFreq 659
#define FaFreq 698
#define SoFreq 784
#define LaFreq 880
#define SiFreq 988
typedef enum
{
OCTAVE_1 = 0,
OCTAVE_2,
OCTAVE_3,
OCTAVE_4,
OCTAVE_5,
OCTAVE_6,
OCTAVE_7,
OCTAVE_8,
}octave_e;
typedef enum
{
ALARM_LEVEL_HIGH = 0,
ALARM_LEVEL_ABOVE_MEDIUM,
ALARM_LEVEL_MEDIUM,
ALARM_LEVEL_BELOW_MEDIUM,
ALARM_LEVEL_LOW,
}AlarmLevel_e;
typedef enum
{
ALARM_OFF = 0,
ALARM_ON,
}AlarmState_e;
typedef struct
{
AlarmLevel_e alarm_level;
octave_e octave;
float loudness;
}Buzzer_config_s;
typedef struct
{
float loudness;
octave_e octave;
AlarmLevel_e alarm_level;
AlarmState_e alarm_state;
}BuzzzerInstance;
void BuzzerInit();
void BuzzerTask();
BuzzzerInstance *BuzzerRegister(Buzzer_config_s *config);
void AlarmSetStatus(BuzzzerInstance *buzzer, AlarmState_e state);
#endif // !BUZZER_H
modules/alarm/buzzer.md
+20
View File
@@ -0,0 +1,20 @@
# buzzer
用于拉响蜂鸣器警报
## 使用范例
```c
Buzzer_config_s buzzer_config ={
.alarm_level = ALARM_LEVEL_HIGH, //设置警报等级 同一状态下 高等级的响应
.loudness= 0.4, //设置响度
.octave= OCTAVE_1, // 设置音阶
};
robocmd_alarm = BuzzerRegister(&buzzer_config);
AlarmSetStatus(robocmd_alarm, ALARM_ON);
AlarmSetStatus(robocmd_alarm, ALARM_OFF);
```
@todo: 将音阶改为可供选择的搭配 如 Do Re Mi Fa So La Si 自由组合 用户只需输入字符串"DoReMi"即可
modules/algorithm/QuaternionEKF.c
+489
View File
@@ -0,0 +1,489 @@
/**
******************************************************************************
* @file QuaternionEKF.c
* @author Wang Hongxi
* @version V1.2.0
* @date 2022/3/8
* @brief attitude update with gyro bias estimate and chi-square test
******************************************************************************
* @attention
* 1st order LPF transfer function:
* 1
* ———————
* as + 1
*
******************************************************************************
*/
#include "QuaternionEKF.h"
QEKF_INS_t QEKF_INS;
const float IMU_QuaternionEKF_F[36] = {1, 0, 0, 0, 0, 0,
0, 1, 0, 0, 0, 0,
0, 0, 1, 0, 0, 0,
0, 0, 0, 1, 0, 0,
0, 0, 0, 0, 1, 0,
0, 0, 0, 0, 0, 1};
float IMU_QuaternionEKF_P[36] = {100000, 0.1, 0.1, 0.1, 0.1, 0.1,
0.1, 100000, 0.1, 0.1, 0.1, 0.1,
0.1, 0.1, 100000, 0.1, 0.1, 0.1,
0.1, 0.1, 0.1, 100000, 0.1, 0.1,
0.1, 0.1, 0.1, 0.1, 100, 0.1,
0.1, 0.1, 0.1, 0.1, 0.1, 100};
float IMU_QuaternionEKF_K[18];
float IMU_QuaternionEKF_H[18];
static float invSqrt(float x);
static void IMU_QuaternionEKF_Observe(KalmanFilter_t *kf);
static void IMU_QuaternionEKF_F_Linearization_P_Fading(KalmanFilter_t *kf);
static void IMU_QuaternionEKF_SetH(KalmanFilter_t *kf);
static void IMU_QuaternionEKF_xhatUpdate(KalmanFilter_t *kf);
/**
* @brief Quaternion EKF initialization and some reference value
* @param[in] process_noise1 quaternion process noise 10
* @param[in] process_noise2 gyro bias process noise 0.001
* @param[in] measure_noise accel measure noise 1000000
* @param[in] lambda fading coefficient 0.9996
* @param[in] lpf lowpass filter coefficient 0
*/
void IMU_QuaternionEKF_Init(float* init_quaternion,float process_noise1, float process_noise2, float measure_noise, float lambda, float lpf)
{
QEKF_INS.Initialized = 1;
QEKF_INS.Q1 = process_noise1;
QEKF_INS.Q2 = process_noise2;
QEKF_INS.R = measure_noise;
QEKF_INS.ChiSquareTestThreshold = 1e-8;
QEKF_INS.ConvergeFlag = 0;
QEKF_INS.ErrorCount = 0;
QEKF_INS.UpdateCount = 0;
if (lambda > 1)
{
lambda = 1;
}
QEKF_INS.lambda = lambda;
QEKF_INS.accLPFcoef = lpf;
// 初始化矩阵维度信息
Kalman_Filter_Init(&QEKF_INS.IMU_QuaternionEKF, 6, 0, 3);
Matrix_Init(&QEKF_INS.ChiSquare, 1, 1, (float *)QEKF_INS.ChiSquare_Data);
// 姿态初始化
for(int i = 0; i < 4; i++)
{
QEKF_INS.IMU_QuaternionEKF.xhat_data[i] = init_quaternion[i];
}
// 自定义函数初始化,用于扩展或增加kf的基础功能
QEKF_INS.IMU_QuaternionEKF.User_Func0_f = IMU_QuaternionEKF_Observe;
QEKF_INS.IMU_QuaternionEKF.User_Func1_f = IMU_QuaternionEKF_F_Linearization_P_Fading;
QEKF_INS.IMU_QuaternionEKF.User_Func2_f = IMU_QuaternionEKF_SetH;
QEKF_INS.IMU_QuaternionEKF.User_Func3_f = IMU_QuaternionEKF_xhatUpdate;
// 设定标志位,用自定函数替换kf标准步骤中的SetK(计算增益)以及xhatupdate(后验估计/融合)
QEKF_INS.IMU_QuaternionEKF.SkipEq3 = TRUE;
QEKF_INS.IMU_QuaternionEKF.SkipEq4 = TRUE;
memcpy(QEKF_INS.IMU_QuaternionEKF.F_data, IMU_QuaternionEKF_F, sizeof(IMU_QuaternionEKF_F));
memcpy(QEKF_INS.IMU_QuaternionEKF.P_data, IMU_QuaternionEKF_P, sizeof(IMU_QuaternionEKF_P));
}
/**
* @brief Quaternion EKF update
* @param[in] gyro x y z in rad/s
* @param[in] accel x y z in m/s²
* @param[in] update period in s
*/
void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay, float az, float dt)
{
// 0.5(Ohm-Ohm^bias)*deltaT,用于更新工作点处的状态转移F矩阵
static float halfgxdt, halfgydt, halfgzdt;
static float accelInvNorm;
/* F, number with * represent vals to be set
0 1* 2* 3* 4 5
6* 7 8* 9* 10 11
12* 13* 14 15* 16 17
18* 19* 20* 21 22 23
24 25 26 27 28 29
30 31 32 33 34 35
*/
QEKF_INS.dt = dt;
QEKF_INS.Gyro[0] = gx - QEKF_INS.GyroBias[0];
QEKF_INS.Gyro[1] = gy - QEKF_INS.GyroBias[1];
QEKF_INS.Gyro[2] = gz - QEKF_INS.GyroBias[2];
// set F
halfgxdt = 0.5f * QEKF_INS.Gyro[0] * dt;
halfgydt = 0.5f * QEKF_INS.Gyro[1] * dt;
halfgzdt = 0.5f * QEKF_INS.Gyro[2] * dt;
// 此部分设定状态转移矩阵F的左上角部分 4x4子矩阵,即0.5(Ohm-Ohm^bias)*deltaT,右下角有一个2x2单位阵已经初始化好了
// 注意在predict步F的右上角是4x2的零矩阵,因此每次predict的时候都会调用memcpy用单位阵覆盖前一轮线性化后的矩阵
memcpy(QEKF_INS.IMU_QuaternionEKF.F_data, IMU_QuaternionEKF_F, sizeof(IMU_QuaternionEKF_F));
QEKF_INS.IMU_QuaternionEKF.F_data[1] = -halfgxdt;
QEKF_INS.IMU_QuaternionEKF.F_data[2] = -halfgydt;
QEKF_INS.IMU_QuaternionEKF.F_data[3] = -halfgzdt;
QEKF_INS.IMU_QuaternionEKF.F_data[6] = halfgxdt;
QEKF_INS.IMU_QuaternionEKF.F_data[8] = halfgzdt;
QEKF_INS.IMU_QuaternionEKF.F_data[9] = -halfgydt;
QEKF_INS.IMU_QuaternionEKF.F_data[12] = halfgydt;
QEKF_INS.IMU_QuaternionEKF.F_data[13] = -halfgzdt;
QEKF_INS.IMU_QuaternionEKF.F_data[15] = halfgxdt;
QEKF_INS.IMU_QuaternionEKF.F_data[18] = halfgzdt;
QEKF_INS.IMU_QuaternionEKF.F_data[19] = halfgydt;
QEKF_INS.IMU_QuaternionEKF.F_data[20] = -halfgxdt;
// accel low pass filter,加速度过一下低通滤波平滑数据,降低撞击和异常的影响
if (QEKF_INS.UpdateCount == 0) // 如果是第一次进入,需要初始化低通滤波
{
QEKF_INS.Accel[0] = ax;
QEKF_INS.Accel[1] = ay;
QEKF_INS.Accel[2] = az;
}
QEKF_INS.Accel[0] = QEKF_INS.Accel[0] * QEKF_INS.accLPFcoef / (QEKF_INS.dt + QEKF_INS.accLPFcoef) + ax * QEKF_INS.dt / (QEKF_INS.dt + QEKF_INS.accLPFcoef);
QEKF_INS.Accel[1] = QEKF_INS.Accel[1] * QEKF_INS.accLPFcoef / (QEKF_INS.dt + QEKF_INS.accLPFcoef) + ay * QEKF_INS.dt / (QEKF_INS.dt + QEKF_INS.accLPFcoef);
QEKF_INS.Accel[2] = QEKF_INS.Accel[2] * QEKF_INS.accLPFcoef / (QEKF_INS.dt + QEKF_INS.accLPFcoef) + az * QEKF_INS.dt / (QEKF_INS.dt + QEKF_INS.accLPFcoef);
// set z,单位化重力加速度向量
accelInvNorm = invSqrt(QEKF_INS.Accel[0] * QEKF_INS.Accel[0] + QEKF_INS.Accel[1] * QEKF_INS.Accel[1] + QEKF_INS.Accel[2] * QEKF_INS.Accel[2]);
for (uint8_t i = 0; i < 3; ++i)
{
QEKF_INS.IMU_QuaternionEKF.MeasuredVector[i] = QEKF_INS.Accel[i] * accelInvNorm; // 用加速度向量更新量测值
}
// get body state
QEKF_INS.gyro_norm = 1.0f / invSqrt(QEKF_INS.Gyro[0] * QEKF_INS.Gyro[0] +
QEKF_INS.Gyro[1] * QEKF_INS.Gyro[1] +
QEKF_INS.Gyro[2] * QEKF_INS.Gyro[2]);
QEKF_INS.accl_norm = 1.0f / accelInvNorm;
// 如果角速度小于阈值且加速度处于设定范围内,认为运动稳定,加速度可以用于修正角速度
// 稍后在最后的姿态更新部分会利用StableFlag来确定
if (QEKF_INS.gyro_norm < 0.3f && QEKF_INS.accl_norm > 9.8f - 0.5f && QEKF_INS.accl_norm < 9.8f + 0.5f)
{
QEKF_INS.StableFlag = 1;
}
else
{
QEKF_INS.StableFlag = 0;
}
// set Q R,过程噪声和观测噪声矩阵
QEKF_INS.IMU_QuaternionEKF.Q_data[0] = QEKF_INS.Q1 * QEKF_INS.dt;
QEKF_INS.IMU_QuaternionEKF.Q_data[7] = QEKF_INS.Q1 * QEKF_INS.dt;
QEKF_INS.IMU_QuaternionEKF.Q_data[14] = QEKF_INS.Q1 * QEKF_INS.dt;
QEKF_INS.IMU_QuaternionEKF.Q_data[21] = QEKF_INS.Q1 * QEKF_INS.dt;
QEKF_INS.IMU_QuaternionEKF.Q_data[28] = QEKF_INS.Q2 * QEKF_INS.dt;
QEKF_INS.IMU_QuaternionEKF.Q_data[35] = QEKF_INS.Q2 * QEKF_INS.dt;
QEKF_INS.IMU_QuaternionEKF.R_data[0] = QEKF_INS.R;
QEKF_INS.IMU_QuaternionEKF.R_data[4] = QEKF_INS.R;
QEKF_INS.IMU_QuaternionEKF.R_data[8] = QEKF_INS.R;
// 调用kalman_filter.c封装好的函数,注意几个User_Funcx_f的调用
Kalman_Filter_Update(&QEKF_INS.IMU_QuaternionEKF);
// 获取融合后的数据,包括四元数和xy零飘值
QEKF_INS.q[0] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[0];
QEKF_INS.q[1] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[1];
QEKF_INS.q[2] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[2];
QEKF_INS.q[3] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[3];
QEKF_INS.GyroBias[0] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[4];
QEKF_INS.GyroBias[1] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[5];
QEKF_INS.GyroBias[2] = 0; // 大部分时候z轴通天,无法观测yaw的漂移
// 利用四元数反解欧拉角
QEKF_INS.Yaw = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[3] + QEKF_INS.q[1] * QEKF_INS.q[2]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[1] * QEKF_INS.q[1]) - 1.0f) * 57.295779513f;
QEKF_INS.Pitch = atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[1] + QEKF_INS.q[2] * QEKF_INS.q[3]), 2.0f * (QEKF_INS.q[0] * QEKF_INS.q[0] + QEKF_INS.q[3] * QEKF_INS.q[3]) - 1.0f) * 57.295779513f;
QEKF_INS.Roll = asinf(-2.0f * (QEKF_INS.q[1] * QEKF_INS.q[3] - QEKF_INS.q[0] * QEKF_INS.q[2])) * 57.295779513f;
// get Yaw total, yaw数据可能会超过360,处理一下方便其他功能使用(如小陀螺)
if (QEKF_INS.Yaw - QEKF_INS.YawAngleLast > 180.0f)
{
QEKF_INS.YawRoundCount--;
}
else if (QEKF_INS.Yaw - QEKF_INS.YawAngleLast < -180.0f)
{
QEKF_INS.YawRoundCount++;
}
QEKF_INS.YawTotalAngle = 360.0f * QEKF_INS.YawRoundCount + QEKF_INS.Yaw;
QEKF_INS.YawAngleLast = QEKF_INS.Yaw;
QEKF_INS.UpdateCount++; // 初始化低通滤波用,计数测试用
}
/**
* @brief 用于更新线性化后的状态转移矩阵F右上角的一个4x2分块矩阵,稍后用于协方差矩阵P的更新;
* 并对零漂的方差进行限制,防止过度收敛并限幅防止发散
*
* @param kf
*/
static void IMU_QuaternionEKF_F_Linearization_P_Fading(KalmanFilter_t *kf)
{
static float q0, q1, q2, q3;
static float qInvNorm;
q0 = kf->xhatminus_data[0];
q1 = kf->xhatminus_data[1];
q2 = kf->xhatminus_data[2];
q3 = kf->xhatminus_data[3];
// quaternion normalize
qInvNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
for (uint8_t i = 0; i < 4; ++i)
{
kf->xhatminus_data[i] *= qInvNorm;
}
/* F, number with * represent vals to be set
0 1 2 3 4* 5*
6 7 8 9 10* 11*
12 13 14 15 16* 17*
18 19 20 21 22* 23*
24 25 26 27 28 29
30 31 32 33 34 35
*/
// set F
kf->F_data[4] = q1 * QEKF_INS.dt / 2;
kf->F_data[5] = q2 * QEKF_INS.dt / 2;
kf->F_data[10] = -q0 * QEKF_INS.dt / 2;
kf->F_data[11] = q3 * QEKF_INS.dt / 2;
kf->F_data[16] = -q3 * QEKF_INS.dt / 2;
kf->F_data[17] = -q0 * QEKF_INS.dt / 2;
kf->F_data[22] = q2 * QEKF_INS.dt / 2;
kf->F_data[23] = -q1 * QEKF_INS.dt / 2;
// fading filter,防止零飘参数过度收敛
kf->P_data[28] /= QEKF_INS.lambda;
kf->P_data[35] /= QEKF_INS.lambda;
// 限幅,防止发散
if (kf->P_data[28] > 10000)
{
kf->P_data[28] = 10000;
}
if (kf->P_data[35] > 10000)
{
kf->P_data[35] = 10000;
}
}
/**
* @brief 在工作点处计算观测函数h(x)的Jacobi矩阵H
*
* @param kf
*/
static void IMU_QuaternionEKF_SetH(KalmanFilter_t *kf)
{
static float doubleq0, doubleq1, doubleq2, doubleq3;
/* H
0 1 2 3 4 5
6 7 8 9 10 11
12 13 14 15 16 17
last two cols are zero
*/
// set H
doubleq0 = 2 * kf->xhatminus_data[0];
doubleq1 = 2 * kf->xhatminus_data[1];
doubleq2 = 2 * kf->xhatminus_data[2];
doubleq3 = 2 * kf->xhatminus_data[3];
memset(kf->H_data, 0, sizeof_float * kf->zSize * kf->xhatSize);
kf->H_data[0] = -doubleq2;
kf->H_data[1] = doubleq3;
kf->H_data[2] = -doubleq0;
kf->H_data[3] = doubleq1;
kf->H_data[6] = doubleq1;
kf->H_data[7] = doubleq0;
kf->H_data[8] = doubleq3;
kf->H_data[9] = doubleq2;
kf->H_data[12] = doubleq0;
kf->H_data[13] = -doubleq1;
kf->H_data[14] = -doubleq2;
kf->H_data[15] = doubleq3;
}
/**
* @brief 利用观测值和先验估计得到最优的后验估计
* 加入了卡方检验以判断融合加速度的条件是否满足
* 同时引入发散保护保证恶劣工况下的必要量测更新
*
* @param kf
*/
static void IMU_QuaternionEKF_xhatUpdate(KalmanFilter_t *kf)
{
static float q0, q1, q2, q3;
kf->MatStatus = Matrix_Transpose(&kf->H, &kf->HT); // z|x => x|z
kf->temp_matrix.numRows = kf->H.numRows;
kf->temp_matrix.numCols = kf->Pminus.numCols;
kf->MatStatus = Matrix_Multiply(&kf->H, &kf->Pminus, &kf->temp_matrix); // temp_matrix = H·P'(k)
kf->temp_matrix1.numRows = kf->temp_matrix.numRows;
kf->temp_matrix1.numCols = kf->HT.numCols;
kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->HT, &kf->temp_matrix1); // temp_matrix1 = H·P'(k)·HT
kf->S.numRows = kf->R.numRows;
kf->S.numCols = kf->R.numCols;
kf->MatStatus = Matrix_Add(&kf->temp_matrix1, &kf->R, &kf->S); // S = H P'(k) HT + R
kf->MatStatus = Matrix_Inverse(&kf->S, &kf->temp_matrix1); // temp_matrix1 = inv(H·P'(k)·HT + R)
q0 = kf->xhatminus_data[0];
q1 = kf->xhatminus_data[1];
q2 = kf->xhatminus_data[2];
q3 = kf->xhatminus_data[3];
kf->temp_vector.numRows = kf->H.numRows;
kf->temp_vector.numCols = 1;
// 计算预测得到的重力加速度方向(通过姿态获取的)
kf->temp_vector_data[0] = 2 * (q1 * q3 - q0 * q2);
kf->temp_vector_data[1] = 2 * (q0 * q1 + q2 * q3);
kf->temp_vector_data[2] = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3; // temp_vector = h(xhat'(k))
// 计算预测值和各个轴的方向余弦
for (uint8_t i = 0; i < 3; ++i)
{
QEKF_INS.OrientationCosine[i] = acosf(fabsf(kf->temp_vector_data[i]));
}
// 利用加速度计数据修正
kf->temp_vector1.numRows = kf->z.numRows;
kf->temp_vector1.numCols = 1;
kf->MatStatus = Matrix_Subtract(&kf->z, &kf->temp_vector, &kf->temp_vector1); // temp_vector1 = z(k) - h(xhat'(k))
// chi-square test,卡方检验
kf->temp_matrix.numRows = kf->temp_vector1.numRows;
kf->temp_matrix.numCols = 1;
kf->MatStatus = Matrix_Multiply(&kf->temp_matrix1, &kf->temp_vector1, &kf->temp_matrix); // temp_matrix = inv(H·P'(k)·HT + R)·(z(k) - h(xhat'(k)))
kf->temp_vector.numRows = 1;
kf->temp_vector.numCols = kf->temp_vector1.numRows;
kf->MatStatus = Matrix_Transpose(&kf->temp_vector1, &kf->temp_vector); // temp_vector = z(k) - h(xhat'(k))'
kf->MatStatus = Matrix_Multiply(&kf->temp_vector, &kf->temp_matrix, &QEKF_INS.ChiSquare);
// rk is small,filter converged/converging
if (QEKF_INS.ChiSquare_Data[0] < 0.5f * QEKF_INS.ChiSquareTestThreshold)
{
QEKF_INS.ConvergeFlag = 1;
}
// rk is bigger than thre but once converged
if (QEKF_INS.ChiSquare_Data[0] > QEKF_INS.ChiSquareTestThreshold && QEKF_INS.ConvergeFlag)
{
if (QEKF_INS.StableFlag)
{
QEKF_INS.ErrorCount++; // 载体静止时仍无法通过卡方检验
}
else
{
QEKF_INS.ErrorCount = 0;
}
if (QEKF_INS.ErrorCount > 50)
{
// 滤波器发散
QEKF_INS.ConvergeFlag = 0;
kf->SkipEq5 = FALSE; // step-5 is cov mat P updating
}
else
{
// 残差未通过卡方检验 仅预测
// xhat(k) = xhat'(k)
// P(k) = P'(k)
memcpy(kf->xhat_data, kf->xhatminus_data, sizeof_float * kf->xhatSize);
memcpy(kf->P_data, kf->Pminus_data, sizeof_float * kf->xhatSize * kf->xhatSize);
kf->SkipEq5 = TRUE; // part5 is P updating
return;
}
}
else // if divergent or rk is not that big/acceptable,use adaptive gain
{
// scale adaptive,rk越小则增益越大,否则更相信预测值
if (QEKF_INS.ChiSquare_Data[0] > 0.1f * QEKF_INS.ChiSquareTestThreshold && QEKF_INS.ConvergeFlag)
{
QEKF_INS.AdaptiveGainScale = (QEKF_INS.ChiSquareTestThreshold - QEKF_INS.ChiSquare_Data[0]) / (0.9f * QEKF_INS.ChiSquareTestThreshold);
}
else
{
QEKF_INS.AdaptiveGainScale = 1;
}
QEKF_INS.ErrorCount = 0;
kf->SkipEq5 = FALSE;
}
// cal kf-gain K
kf->temp_matrix.numRows = kf->Pminus.numRows;
kf->temp_matrix.numCols = kf->HT.numCols;
kf->MatStatus = Matrix_Multiply(&kf->Pminus, &kf->HT, &kf->temp_matrix); // temp_matrix = P'(k)·HT
kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->temp_matrix1, &kf->K);
// implement adaptive
for (uint8_t i = 0; i < kf->K.numRows * kf->K.numCols; ++i)
{
kf->K_data[i] *= QEKF_INS.AdaptiveGainScale;
}
for (uint8_t i = 4; i < 6; ++i)
{
for (uint8_t j = 0; j < 3; ++j)
{
kf->K_data[i * 3 + j] *= QEKF_INS.OrientationCosine[i - 4] / 1.5707963f; // 1 rad
}
}
kf->temp_vector.numRows = kf->K.numRows;
kf->temp_vector.numCols = 1;
kf->MatStatus = Matrix_Multiply(&kf->K, &kf->temp_vector1, &kf->temp_vector); // temp_vector = K(k)·(z(k) - H·xhat'(k))
// 零漂修正限幅,一般不会有过大的漂移
if (QEKF_INS.ConvergeFlag)
{
for (uint8_t i = 4; i < 6; ++i)
{
if (kf->temp_vector.pData[i] > 1e-2f * QEKF_INS.dt)
{
kf->temp_vector.pData[i] = 1e-2f * QEKF_INS.dt;
}
if (kf->temp_vector.pData[i] < -1e-2f * QEKF_INS.dt)
{
kf->temp_vector.pData[i] = -1e-2f * QEKF_INS.dt;
}
}
}
// 不修正yaw轴数据
kf->temp_vector.pData[3] = 0;
kf->MatStatus = Matrix_Add(&kf->xhatminus, &kf->temp_vector, &kf->xhat);
}
/**
* @brief EKF观测环节,其实就是把数据复制一下
*
* @param kf kf类型定义
*/
static void IMU_QuaternionEKF_Observe(KalmanFilter_t *kf)
{
memcpy(IMU_QuaternionEKF_P, kf->P_data, sizeof(IMU_QuaternionEKF_P));
memcpy(IMU_QuaternionEKF_K, kf->K_data, sizeof(IMU_QuaternionEKF_K));
memcpy(IMU_QuaternionEKF_H, kf->H_data, sizeof(IMU_QuaternionEKF_H));
}
/**
* @brief 自定义1/sqrt(x),速度更快
*
* @param x x
* @return float
*/
static float invSqrt(float x)
{
float halfx = 0.5f * x;
float y = x;
long i = *(long *)&y;
i = 0x5f375a86 - (i >> 1);
y = *(float *)&i;
y = y * (1.5f - (halfx * y * y));
return y;
}
modules/algorithm/QuaternionEKF.h
+75
View File
@@ -0,0 +1,75 @@
/**
******************************************************************************
* @file QuaternionEKF.h
* @author Wang Hongxi
* @version V1.2.0
* @date 2022/3/8
* @brief attitude update with gyro bias estimate and chi-square test
******************************************************************************
* @attention
*
******************************************************************************
*/
#ifndef _QUAT_EKF_H
#define _QUAT_EKF_H
#include "kalman_filter.h"
/* boolean type definitions */
#ifndef TRUE
#define TRUE 1 /**< boolean true */
#endif
#ifndef FALSE
#define FALSE 0 /**< boolean fails */
#endif
typedef struct
{
uint8_t Initialized;
KalmanFilter_t IMU_QuaternionEKF;
uint8_t ConvergeFlag;
uint8_t StableFlag;
uint64_t ErrorCount;
uint64_t UpdateCount;
float q[4]; // 四元数估计值
float GyroBias[3]; // 陀螺仪零偏估计值
float Gyro[3];
float Accel[3];
float OrientationCosine[3];
float accLPFcoef;
float gyro_norm;
float accl_norm;
float AdaptiveGainScale;
float Roll;
float Pitch;
float Yaw;
float YawTotalAngle;
float Q1; // 四元数更新过程噪声
float Q2; // 陀螺仪零偏过程噪声
float R; // 加速度计量测噪声
float dt; // 姿态更新周期
mat ChiSquare;
float ChiSquare_Data[1]; // 卡方检验检测函数
float ChiSquareTestThreshold; // 卡方检验阈值
float lambda; // 渐消因子
int16_t YawRoundCount;
float YawAngleLast;
} QEKF_INS_t;
extern QEKF_INS_t QEKF_INS;
extern float chiSquare;
extern float ChiSquareTestThreshold;
void IMU_QuaternionEKF_Init(float* init_quaternion,float process_noise1, float process_noise2, float measure_noise, float lambda, float lpf);
void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay, float az, float dt);
#endif
modules/algorithm/algorithm.md
+29
View File
@@ -0,0 +1,29 @@
# algorithms
<p align='right'>neozng1@hnu.edu.cn</p>
> TODO:
> 1. 实现麦轮和全向轮的速度解算
> 2. 实现一些通用的滤波器，如指数平均，窗平均，低通等
## 总览和使用
module层的algorithm提供了一些供其他模块以及app的应用层使用的算法，包括：
1. PID控制器`controller.h`
2. crc8 crc16循环冗余校验
3. 卡尔曼滤波器`kalman_filter.h`，可以通过用户自定义函数配置为扩展卡尔曼滤波
4. `LQR.h`，线性二次型调节器
5. `QuaterninoEKF.h`，用于`ins_task`的四元数姿态解算和扩展卡尔曼滤波融合
6. `user_lib.h`，一些通用的函数，包括限幅、数据类型转换、角度弧度转换、快速符号判断以及优化开方等功能。多个模块都会使用的、不好区分的函数可以放置于此
## 代码结构
.c 为算法的实现，.h为算法对外接口的头文件
在编写应用的时候，你基本不会使用这里的函数，或是修改其实现。
若发现bug或需要增加功能，联系组长讨论。
modules/algorithm/controller.c
+211
View File
@@ -0,0 +1,211 @@
/**
* @file controller.c
* @author wanghongxi
* @author modified by neozng
* @brief PID控制器定义
* @version beta
* @date 2022-11-01
*
* @copyrightCopyright (c) 2022 HNU YueLu EC all rights reserved
*/
#include "controller.h"
#include "memory.h"
/* ----------------------------下面是pid优化环节的实现---------------------------- */
// 梯形积分
static void f_Trapezoid_Intergral(PIDInstance *pid)
{
// 计算梯形的面积,(上底+下底)*高/2
pid->ITerm = pid->Ki * ((pid->Err + pid->Last_Err) / 2) * pid->dt;
}
// 变速积分(误差小时积分作用更强)
static void f_Changing_Integration_Rate(PIDInstance *pid)
{
if (pid->Err * pid->Iout > 0)
{
// 积分呈累积趋势
if (abs(pid->Err) <= pid->CoefB)
return; // Full integral
if (abs(pid->Err) <= (pid->CoefA + pid->CoefB))
pid->ITerm *= (pid->CoefA - abs(pid->Err) + pid->CoefB) / pid->CoefA;
else // 最大阈值,不使用积分
pid->ITerm = 0;
}
}
static void f_Integral_Limit(PIDInstance *pid)
{
static float temp_Output, temp_Iout;
temp_Iout = pid->Iout + pid->ITerm;
temp_Output = pid->Pout + pid->Iout + pid->Dout;
if (abs(temp_Output) > pid->MaxOut)
{
if (pid->Err * pid->Iout > 0) // 积分却还在累积
{
pid->ITerm = 0; // 当前积分项置零
}
}
if (temp_Iout > pid->IntegralLimit)
{
pid->ITerm = 0;
pid->Iout = pid->IntegralLimit;
}
if (temp_Iout < -pid->IntegralLimit)
{
pid->ITerm = 0;
pid->Iout = -pid->IntegralLimit;
}
}
// 微分先行(仅使用反馈值而不计参考输入的微分)
static void f_Derivative_On_Measurement(PIDInstance *pid)
{
pid->Dout = pid->Kd * (pid->Last_Measure - pid->Measure) / pid->dt;
}
// 微分滤波(采集微分时,滤除高频噪声)
static void f_Derivative_Filter(PIDInstance *pid)
{
pid->Dout = pid->Dout * pid->dt / (pid->Derivative_LPF_RC + pid->dt) +
pid->Last_Dout * pid->Derivative_LPF_RC / (pid->Derivative_LPF_RC + pid->dt);
}
// 输出滤波
static void f_Output_Filter(PIDInstance *pid)
{
pid->Output = pid->Output * pid->dt / (pid->Output_LPF_RC + pid->dt) +
pid->Last_Output * pid->Output_LPF_RC / (pid->Output_LPF_RC + pid->dt);
}
// 输出限幅
static void f_Output_Limit(PIDInstance *pid)
{
if (pid->Output > pid->MaxOut)
{
pid->Output = pid->MaxOut;
}
if (pid->Output < -(pid->MaxOut))
{
pid->Output = -(pid->MaxOut);
}
}
// 电机堵转检测
static void f_PID_ErrorHandle(PIDInstance *pid)
{
/*Motor Blocked Handle*/
if (fabsf(pid->Output) < pid->MaxOut * 0.001f || fabsf(pid->Ref) < 0.0001f)
return;
if ((fabsf(pid->Ref - pid->Measure) / fabsf(pid->Ref)) > 0.95f)
{
// Motor blocked counting
pid->ERRORHandler.ERRORCount++;
}
else
{
pid->ERRORHandler.ERRORCount = 0;
}
if (pid->ERRORHandler.ERRORCount > 500)
{
// Motor blocked over 1000times
pid->ERRORHandler.ERRORType = PID_MOTOR_BLOCKED_ERROR;
}
}
/* ---------------------------下面是PID的外部算法接口--------------------------- */
/**
* @brief 初始化PID,设置参数和启用的优化环节,将其他数据置零
*
* @param pid PID实例
* @param config PID初始化设置
*/
void PIDInit(PIDInstance *pid, PID_Init_Config_s *config)
{
// config的数据和pid的部分数据是连续且相同的的,所以可以直接用memcpy
// @todo: 不建议这样做,可扩展性差,不知道的开发者可能会误以为pid和config是同一个结构体
// 后续修改为逐个赋值
memset(pid, 0, sizeof(PIDInstance));
// utilize the quality of struct that its memeory is continuous
memcpy(pid, config, sizeof(PID_Init_Config_s));
// set rest of memory to 0
DWT_GetDeltaT(&pid->DWT_CNT);
}
/**
* @brief PID计算
* @param[in] PID结构体
* @param[in] 测量值
* @param[in] 期望值
* @retval 返回空
*/
float PIDCalculate(PIDInstance *pid, float measure, float ref)
{
// 堵转检测
if (pid->Improve & PID_ErrorHandle)
f_PID_ErrorHandle(pid);
pid->dt = DWT_GetDeltaT(&pid->DWT_CNT); // 获取两次pid计算的时间间隔,用于积分和微分
if (pid->dt < 0.000001f) // 避免除零
{
return 0.0f;
}
// 保存上次的测量值和误差,计算当前error
pid->Measure = measure;
pid->Ref = ref;
pid->Err = pid->Ref - pid->Measure;
// 如果在死区外,则计算PID
if (abs(pid->Err) > pid->DeadBand)
{
// 基本的pid计算,使用位置式
pid->Pout = pid->Kp * pid->Err;
pid->ITerm = pid->Ki * pid->Err * pid->dt;
pid->Dout = pid->Kd * (pid->Err - pid->Last_Err) / pid->dt;
// 梯形积分
if (pid->Improve & PID_Trapezoid_Intergral)
f_Trapezoid_Intergral(pid);
// 变速积分
if (pid->Improve & PID_ChangingIntegrationRate)
f_Changing_Integration_Rate(pid);
// 微分先行
if (pid->Improve & PID_Derivative_On_Measurement)
f_Derivative_On_Measurement(pid);
// 微分滤波器
if (pid->Improve & PID_DerivativeFilter)
f_Derivative_Filter(pid);
// 积分限幅
if (pid->Improve & PID_Integral_Limit)
f_Integral_Limit(pid);
pid->Iout += pid->ITerm; // 累加积分
pid->Output = pid->Pout + pid->Iout + pid->Dout; // 计算输出
// 输出滤波
if (pid->Improve & PID_OutputFilter)
f_Output_Filter(pid);
// 输出限幅
f_Output_Limit(pid);
}
else // 进入死区, 则清空积分和输出
{
pid->Output = 0;
pid->ITerm = 0;
}
// 保存当前数据,用于下次计算
pid->Last_Measure = pid->Measure;
pid->Last_Output = pid->Output;
pid->Last_Dout = pid->Dout;
pid->Last_Err = pid->Err;
pid->Last_ITerm = pid->ITerm;
return pid->Output;
}
modules/algorithm/controller.h
+136
View File
@@ -0,0 +1,136 @@
/**
******************************************************************************
* @file controller.h
* @author Wang Hongxi
* @version V1.1.3
* @date 2021/7/3
* @brief
******************************************************************************
* @attention
*
******************************************************************************
*/
#ifndef _CONTROLLER_H
#define _CONTROLLER_H
#include "main.h"
#include "stdint.h"
#include "memory.h"
#include "stdlib.h"
#include "bsp_dwt.h"
#include "arm_math.h"
#include <math.h>
#ifndef abs
#define abs(x) ((x > 0) ? x : -x)
#endif
// PID 优化环节使能标志位,通过位与可以判断启用的优化环节;也可以改成位域的形式
typedef enum
{
PID_IMPROVE_NONE = 0b00000000, // 0000 0000
PID_Integral_Limit = 0b00000001, // 0000 0001
PID_Derivative_On_Measurement = 0b00000010, // 0000 0010
PID_Trapezoid_Intergral = 0b00000100, // 0000 0100
PID_Proportional_On_Measurement = 0b00001000, // 0000 1000
PID_OutputFilter = 0b00010000, // 0001 0000
PID_ChangingIntegrationRate = 0b00100000, // 0010 0000
PID_DerivativeFilter = 0b01000000, // 0100 0000
PID_ErrorHandle = 0b10000000, // 1000 0000
} PID_Improvement_e;
/* PID 报错类型枚举*/
typedef enum errorType_e
{
PID_ERROR_NONE = 0x00U,
PID_MOTOR_BLOCKED_ERROR = 0x01U
} ErrorType_e;
typedef struct
{
uint64_t ERRORCount;
ErrorType_e ERRORType;
} PID_ErrorHandler_t;
/* PID结构体 */
typedef struct
{
//---------------------------------- init config block
// config parameter
float Kp;
float Ki;
float Kd;
float MaxOut;
float DeadBand;
// improve parameter
PID_Improvement_e Improve;
float IntegralLimit; // 积分限幅
float CoefA; // 变速积分 For Changing Integral
float CoefB; // 变速积分 ITerm = Err*((A-abs(err)+B)/A) when B<|err|<A+B
float Output_LPF_RC; // 输出滤波器 RC = 1/omegac
float Derivative_LPF_RC; // 微分滤波器系数
//-----------------------------------
// for calculating
float Measure;
float Last_Measure;
float Err;
float Last_Err;
float Last_ITerm;
float Pout;
float Iout;
float Dout;
float ITerm;
float Output;
float Last_Output;
float Last_Dout;
float Ref;
uint32_t DWT_CNT;
float dt;
PID_ErrorHandler_t ERRORHandler;
} PIDInstance;
/* 用于PID初始化的结构体*/
typedef struct // config parameter
{
// basic parameter
float Kp;
float Ki;
float Kd;
float MaxOut; // 输出限幅
float DeadBand; // 死区
// improve parameter
PID_Improvement_e Improve;
float IntegralLimit; // 积分限幅
float CoefA; // AB为变速积分参数,变速积分实际上就引入了积分分离
float CoefB; // ITerm = Err*((A-abs(err)+B)/A) when B<|err|<A+B
float Output_LPF_RC; // RC = 1/omegac
float Derivative_LPF_RC;
} PID_Init_Config_s;
/**
* @brief 初始化PID实例
* @todo 待修改为统一的PIDRegister风格
* @param pid PID实例指针
* @param config PID初始化配置
*/
void PIDInit(PIDInstance *pid, PID_Init_Config_s *config);
/**
* @brief 计算PID输出
*
* @param pid PID实例指针
* @param measure 反馈值
* @param ref 设定值
* @return float PID计算输出
*/
float PIDCalculate(PIDInstance *pid, float measure, float ref);
#endif
modules/algorithm/crc16.c
+101
View File
@@ -0,0 +1,101 @@
#include "crc16.h"
static uint8_t crc_tab16_init = 0;
static uint16_t crc_tab16[256];
/*
* uint16_t crc_16( const unsigned char *input_str, size_t num_bytes );
*
*函数crc_16()一次计算一个字节的16位CRC16
*其开头已传递给函数的字符串。的数量
*要检查的字节也是一个参数。字符串中的字节数为
*受恒定大小最大值的限制。
*/
uint16_t crc_16(const uint8_t *input_str, uint16_t num_bytes)
{
uint16_t crc;
const uint8_t *ptr;
uint16_t a;
if (!crc_tab16_init)
init_crc16_tab();
crc = CRC_START_16;
ptr = input_str;
if (ptr != NULL)
for (a = 0; a < num_bytes; a++)
{
crc = (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t)*ptr++) & 0x00FF];
}
return crc;
}
/*
* uint16_t crc_modbus( const unsigned char *input_str, size_t num_bytes );
*
*函数crc_modbus()一次计算16位modbus循环冗余校验
*一个字节字符串，其开头已被传递给函数。这
*要检查的字节数也是一个参数。
*/
uint16_t crc_modbus(const uint8_t *input_str, uint16_t num_bytes)
{
uint16_t crc;
const uint8_t *ptr;
uint16_t a;
if (!crc_tab16_init)
init_crc16_tab();
crc = CRC_START_MODBUS;
ptr = input_str;
if (ptr != NULL)
for (a = 0; a < num_bytes; a++)
{
crc = (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t)*ptr++) & 0x00FF];
}
return crc;
}
/*
* uint16_t update_crc_16( uint16_t crc, unsigned char c );
*
*函数update_crc_16()根据
*前一个循环冗余校验值和下一个要检查的数据字节。
*/
uint16_t update_crc_16(uint16_t crc, uint8_t c)
{
if (!crc_tab16_init)
init_crc16_tab();
return (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t)c) & 0x00FF];
}
/*
* static void init_crc16_tab( void );
*
*为了获得最佳性能，使用CRC16例程查找带有值的表
*可以直接在异或算法中使用的算法。
*查找表首次由init_crc16_tab()例程计算
*调用循环冗余校验函数。
*/
void init_crc16_tab(void)
{
uint16_t i;
uint16_t j;
uint16_t crc;
uint16_t c;
for (i = 0; i < 256; i++)
{
crc = 0;
c = i;
for (j = 0; j < 8; j++)
{
if ((crc ^ c) & 0x0001)
crc = (crc >> 1) ^ CRC_POLY_16;
else
crc = crc >> 1;
c = c >> 1;
}
crc_tab16[i] = crc;
}
crc_tab16_init = 1;
}
modules/algorithm/crc16.h
+14
View File
@@ -0,0 +1,14 @@
#ifndef __CRC16_H
#define __CRC16_H
#include "main.h"
#define CRC_START_16 0xFFFF
#define CRC_START_MODBUS 0xFFFF
#define CRC_POLY_16 0xA001
uint16_t crc_16(const uint8_t *input_str, uint16_t num_bytes);
uint16_t crc_modbus(const uint8_t *input_str, uint16_t num_bytes);
uint16_t update_crc_16(uint16_t crc, uint8_t c);
void init_crc16_tab(void);
#endif
modules/algorithm/crc8.c
+61
View File
@@ -0,0 +1,61 @@
#include "crc8.h"
/*
* static uint8_t sht75_crc_table[];
*
* The SHT75 humidity sensor is capable of calculating an 8 bit CRC checksum to
* ensure data integrity. The lookup table crc_table[] is used to recalculate
* the CRC.
*/
static uint8_t sht75_crc_table[] =
{
0, 49, 98, 83, 196, 245, 166, 151, 185, 136, 219, 234, 125, 76, 31, 46,
67, 114, 33, 16, 135, 182, 229, 212, 250, 203, 152, 169, 62, 15, 92, 109,
134, 183, 228, 213, 66, 115, 32, 17, 63, 14, 93, 108, 251, 202, 153, 168,
197, 244, 167, 150, 1, 48, 99, 82, 124, 77, 30, 47, 184, 137, 218, 235,
61, 12, 95, 110, 249, 200, 155, 170, 132, 181, 230, 215, 64, 113, 34, 19,
126, 79, 28, 45, 186, 139, 216, 233, 199, 246, 165, 148, 3, 50, 97, 80,
187, 138, 217, 232, 127, 78, 29, 44, 2, 51, 96, 81, 198, 247, 164, 149,
248, 201, 154, 171, 60, 13, 94, 111, 65, 112, 35, 18, 133, 180, 231, 214,
122, 75, 24, 41, 190, 143, 220, 237, 195, 242, 161, 144, 7, 54, 101, 84,
57, 8, 91, 106, 253, 204, 159, 174, 128, 177, 226, 211, 68, 117, 38, 23,
252, 205, 158, 175, 56, 9, 90, 107, 69, 116, 39, 22, 129, 176, 227, 210,
191, 142, 221, 236, 123, 74, 25, 40, 6, 55, 100, 85, 194, 243, 160, 145,
71, 118, 37, 20, 131, 178, 225, 208, 254, 207, 156, 173, 58, 11, 88, 105,
4, 53, 102, 87, 192, 241, 162, 147, 189, 140, 223, 238, 121, 72, 27, 42,
193, 240, 163, 146, 5, 52, 103, 86, 120, 73, 26, 43, 188, 141, 222, 239,
130, 179, 224, 209, 70, 119, 36, 21, 59, 10, 89, 104, 255, 206, 157, 172};
/*
* uint8_t crc_8( const unsigned char *input_str, size_t num_bytes );
*
*函数crc_8()计算输入字符串的8位宽crc
*给定长度。
*/
uint8_t crc_8(const uint8_t *input_str, uint16_t num_bytes)
{
uint16_t a;
uint8_t crc;
const uint8_t *ptr;
crc = CRC_START_8;
ptr = input_str;
if (ptr != NULL)
for (a = 0; a < num_bytes; a++)
{
crc = sht75_crc_table[(*ptr++) ^ crc];
}
return crc;
}
/*
* uint8_t update_crc_8( unsigned char crc, unsigned char val );
*
*给定一个数据字节和循环冗余校验值的前一个值
*update_crc_8()计算并返回数据的新的实际crc值
*/
uint8_t update_crc_8(uint8_t crc, uint8_t val)
{
return sht75_crc_table[val ^ crc];
}
modules/algorithm/crc8.h
+11
View File
@@ -0,0 +1,11 @@
#ifndef __CRC8_H
#define __CRC8_H
#include "main.h"
#define CRC_START_8 0x00
uint8_t crc_8(const uint8_t *input_str, uint16_t num_bytes);
uint8_t update_crc_8(uint8_t crc, uint8_t val);
#endif
modules/algorithm/kalman_filter.c
+480
View File
@@ -0,0 +1,480 @@
/**
******************************************************************************
* @file kalman filter.c
* @author Wang Hongxi
* @version V1.2.2
* @date 2022/1/8
* @brief C implementation of kalman filter
******************************************************************************
* @attention
* 该卡尔曼滤波器可以在传感器采样频率不同的情况下，动态调整矩阵H R和K的维数与数值。
* This implementation of kalman filter can dynamically adjust dimension and
* value of matrix H R and K according to the measurement validity under any
* circumstance that the sampling rate of component sensors are different.
*
* 因此矩阵H和R的初始化会与矩阵P A和Q有所不同。另外的，在初始化量测向量z时需要额外写
* 入传感器量测所对应的状态与这个量测的方式，详情请见例程
* Therefore, the initialization of matrix P, F, and Q is sometimes different
* from that of matrices H R. when initialization. Additionally, the corresponding
* state and the method of the measurement should be provided when initializing
* measurement vector z. For more details, please see the example.
*
* 若不需要动态调整量测向量z，可简单将结构体中的Use_Auto_Adjustment初始化为0，并像初
* 始化矩阵P那样用常规方式初始化z H R即可。
* If automatic adjustment is not required, assign zero to the UseAutoAdjustment
* and initialize z H R in the normal way as matrix P.
*
* 要求量测向量z与控制向量u在传感器回调函数中更新。整数0意味着量测无效，即自上次卡尔曼
* 滤波更新后无传感器数据更新。因此量测向量z与控制向量u会在卡尔曼滤波更新过程中被清零
* MeasuredVector and ControlVector are required to be updated in the sensor
* callback function. Integer 0 in measurement vector z indicates the invalidity
* of current measurement, so MeasuredVector and ControlVector will be reset
* (to 0) during each update.
*
* 此外，矩阵P过度收敛后滤波器将难以再适应状态的缓慢变化，从而产生滤波估计偏差。该算法
* 通过限制矩阵P最小值的方法，可有效抑制滤波器的过度收敛，详情请见例程。
* Additionally, the excessive convergence of matrix P will make filter incapable
* of adopting the slowly changing state. This implementation can effectively
* suppress filter excessive convergence through boundary limiting for matrix P.
* For more details, please see the example.
*
* @example:
* x =
* | height |
* | velocity |
* |acceleration|
*
* KalmanFilter_t Height_KF;
*
* void INS_Task_Init(void)
* {
* static float P_Init[9] =
* {
* 10, 0, 0,
* 0, 30, 0,
* 0, 0, 10,
* };
* static float F_Init[9] =
* {
* 1, dt, 0.5*dt*dt,
* 0, 1, dt,
* 0, 0, 1,
* };
* static float Q_Init[9] =
* {
* 0.25*dt*dt*dt*dt, 0.5*dt*dt*dt, 0.5*dt*dt,
* 0.5*dt*dt*dt, dt*dt, dt,
* 0.5*dt*dt, dt, 1,
* };
*
* // 设置最小方差
* static float state_min_variance[3] = {0.03, 0.005, 0.1};
*
* // 开启自动调整
* Height_KF.UseAutoAdjustment = 1;
*
* // 气压测得高度 GPS测得高度 加速度计测得z轴运动加速度
* static uint8_t measurement_reference[3] = {1, 1, 3}
*
* static float measurement_degree[3] = {1, 1, 1}
* // 根据measurement_reference与measurement_degree生成H矩阵如下（在当前周期全部测量数据有效情况下）
* |1 0 0|
* |1 0 0|
* |0 0 1|
*
* static float mat_R_diagonal_elements = {30, 25, 35}
* //根据mat_R_diagonal_elements生成R矩阵如下（在当前周期全部测量数据有效情况下）
* |30 0 0|
* | 0 25 0|
* | 0 0 35|
*
* Kalman_Filter_Init(&Height_KF, 3, 0, 3);
*
* // 设置矩阵值
* memcpy(Height_KF.P_data, P_Init, sizeof(P_Init));
* memcpy(Height_KF.F_data, F_Init, sizeof(F_Init));
* memcpy(Height_KF.Q_data, Q_Init, sizeof(Q_Init));
* memcpy(Height_KF.MeasurementMap, measurement_reference, sizeof(measurement_reference));
* memcpy(Height_KF.MeasurementDegree, measurement_degree, sizeof(measurement_degree));
* memcpy(Height_KF.MatR_DiagonalElements, mat_R_diagonal_elements, sizeof(mat_R_diagonal_elements));
* memcpy(Height_KF.StateMinVariance, state_min_variance, sizeof(state_min_variance));
* }
*
* void INS_Task(void const *pvParameters)
* {
* // 循环更新
* Kalman_Filter_Update(&Height_KF);
* vTaskDelay(ts);
* }
*
* // 测量数据更新应按照以下形式 即向MeasuredVector赋值
* void Barometer_Read_Over(void)
* {
* ......
* INS_KF.MeasuredVector[0] = baro_height;
* }
* void GPS_Read_Over(void)
* {
* ......
* INS_KF.MeasuredVector[1] = GPS_height;
* }
* void Acc_Data_Process(void)
* {
* ......
* INS_KF.MeasuredVector[2] = acc.z;
* }
******************************************************************************
*/
#include "kalman_filter.h"
uint16_t sizeof_float, sizeof_double;
static void H_K_R_Adjustment(KalmanFilter_t *kf);
/**
* @brief 初始化矩阵维度信息并为矩阵分配空间
*
* @param kf kf类型定义
* @param xhatSize 状态变量维度
* @param uSize 控制变量维度
* @param zSize 观测量维度
*/
void Kalman_Filter_Init(KalmanFilter_t *kf, uint8_t xhatSize, uint8_t uSize, uint8_t zSize)
{
sizeof_float = sizeof(float);
sizeof_double = sizeof(double);
kf->xhatSize = xhatSize;
kf->uSize = uSize;
kf->zSize = zSize;
kf->MeasurementValidNum = 0;
// measurement flags
kf->MeasurementMap = (uint8_t *)user_malloc(sizeof(uint8_t) * zSize);
memset(kf->MeasurementMap, 0, sizeof(uint8_t) * zSize);
kf->MeasurementDegree = (float *)user_malloc(sizeof_float * zSize);
memset(kf->MeasurementDegree, 0, sizeof_float * zSize);
kf->MatR_DiagonalElements = (float *)user_malloc(sizeof_float * zSize);
memset(kf->MatR_DiagonalElements, 0, sizeof_float * zSize);
kf->StateMinVariance = (float *)user_malloc(sizeof_float * xhatSize);
memset(kf->StateMinVariance, 0, sizeof_float * xhatSize);
kf->temp = (uint8_t *)user_malloc(sizeof(uint8_t) * zSize);
memset(kf->temp, 0, sizeof(uint8_t) * zSize);
// filter data
kf->FilteredValue = (float *)user_malloc(sizeof_float * xhatSize);
memset(kf->FilteredValue, 0, sizeof_float * xhatSize);
kf->MeasuredVector = (float *)user_malloc(sizeof_float * zSize);
memset(kf->MeasuredVector, 0, sizeof_float * zSize);
kf->ControlVector = (float *)user_malloc(sizeof_float * uSize);
memset(kf->ControlVector, 0, sizeof_float * uSize);
// xhat x(k|k)
kf->xhat_data = (float *)user_malloc(sizeof_float * xhatSize);
memset(kf->xhat_data, 0, sizeof_float * xhatSize);
Matrix_Init(&kf->xhat, kf->xhatSize, 1, (float *)kf->xhat_data);
// xhatminus x(k|k-1)
kf->xhatminus_data = (float *)user_malloc(sizeof_float * xhatSize);
memset(kf->xhatminus_data, 0, sizeof_float * xhatSize);
Matrix_Init(&kf->xhatminus, kf->xhatSize, 1, (float *)kf->xhatminus_data);
if (uSize != 0)
{
// control vector u
kf->u_data = (float *)user_malloc(sizeof_float * uSize);
memset(kf->u_data, 0, sizeof_float * uSize);
Matrix_Init(&kf->u, kf->uSize, 1, (float *)kf->u_data);
}
// measurement vector z
kf->z_data = (float *)user_malloc(sizeof_float * zSize);
memset(kf->z_data, 0, sizeof_float * zSize);
Matrix_Init(&kf->z, kf->zSize, 1, (float *)kf->z_data);
// covariance matrix P(k|k)
kf->P_data = (float *)user_malloc(sizeof_float * xhatSize * xhatSize);
memset(kf->P_data, 0, sizeof_float * xhatSize * xhatSize);
Matrix_Init(&kf->P, kf->xhatSize, kf->xhatSize, (float *)kf->P_data);
// create covariance matrix P(k|k-1)
kf->Pminus_data = (float *)user_malloc(sizeof_float * xhatSize * xhatSize);
memset(kf->Pminus_data, 0, sizeof_float * xhatSize * xhatSize);
Matrix_Init(&kf->Pminus, kf->xhatSize, kf->xhatSize, (float *)kf->Pminus_data);
// state transition matrix F FT
kf->F_data = (float *)user_malloc(sizeof_float * xhatSize * xhatSize);
kf->FT_data = (float *)user_malloc(sizeof_float * xhatSize * xhatSize);
memset(kf->F_data, 0, sizeof_float * xhatSize * xhatSize);
memset(kf->FT_data, 0, sizeof_float * xhatSize * xhatSize);
Matrix_Init(&kf->F, kf->xhatSize, kf->xhatSize, (float *)kf->F_data);
Matrix_Init(&kf->FT, kf->xhatSize, kf->xhatSize, (float *)kf->FT_data);
if (uSize != 0)
{
// control matrix B
kf->B_data = (float *)user_malloc(sizeof_float * xhatSize * uSize);
memset(kf->B_data, 0, sizeof_float * xhatSize * uSize);
Matrix_Init(&kf->B, kf->xhatSize, kf->uSize, (float *)kf->B_data);
}
// measurement matrix H
kf->H_data = (float *)user_malloc(sizeof_float * zSize * xhatSize);
kf->HT_data = (float *)user_malloc(sizeof_float * xhatSize * zSize);
memset(kf->H_data, 0, sizeof_float * zSize * xhatSize);
memset(kf->HT_data, 0, sizeof_float * xhatSize * zSize);
Matrix_Init(&kf->H, kf->zSize, kf->xhatSize, (float *)kf->H_data);
Matrix_Init(&kf->HT, kf->xhatSize, kf->zSize, (float *)kf->HT_data);
// process noise covariance matrix Q
kf->Q_data = (float *)user_malloc(sizeof_float * xhatSize * xhatSize);
memset(kf->Q_data, 0, sizeof_float * xhatSize * xhatSize);
Matrix_Init(&kf->Q, kf->xhatSize, kf->xhatSize, (float *)kf->Q_data);
// measurement noise covariance matrix R
kf->R_data = (float *)user_malloc(sizeof_float * zSize * zSize);
memset(kf->R_data, 0, sizeof_float * zSize * zSize);
Matrix_Init(&kf->R, kf->zSize, kf->zSize, (float *)kf->R_data);
// kalman gain K
kf->K_data = (float *)user_malloc(sizeof_float * xhatSize * zSize);
memset(kf->K_data, 0, sizeof_float * xhatSize * zSize);
Matrix_Init(&kf->K, kf->xhatSize, kf->zSize, (float *)kf->K_data);
kf->S_data = (float *)user_malloc(sizeof_float * kf->xhatSize * kf->xhatSize);
kf->temp_matrix_data = (float *)user_malloc(sizeof_float * kf->xhatSize * kf->xhatSize);
kf->temp_matrix_data1 = (float *)user_malloc(sizeof_float * kf->xhatSize * kf->xhatSize);
kf->temp_vector_data = (float *)user_malloc(sizeof_float * kf->xhatSize);
kf->temp_vector_data1 = (float *)user_malloc(sizeof_float * kf->xhatSize);
Matrix_Init(&kf->S, kf->xhatSize, kf->xhatSize, (float *)kf->S_data);
Matrix_Init(&kf->temp_matrix, kf->xhatSize, kf->xhatSize, (float *)kf->temp_matrix_data);
Matrix_Init(&kf->temp_matrix1, kf->xhatSize, kf->xhatSize, (float *)kf->temp_matrix_data1);
Matrix_Init(&kf->temp_vector, kf->xhatSize, 1, (float *)kf->temp_vector_data);
Matrix_Init(&kf->temp_vector1, kf->xhatSize, 1, (float *)kf->temp_vector_data1);
kf->SkipEq1 = 0;
kf->SkipEq2 = 0;
kf->SkipEq3 = 0;
kf->SkipEq4 = 0;
kf->SkipEq5 = 0;
}
void Kalman_Filter_Measure(KalmanFilter_t *kf)
{
// 矩阵H K R根据量测情况自动调整
// matrix H K R auto adjustment
if (kf->UseAutoAdjustment != 0)
H_K_R_Adjustment(kf);
else
{
memcpy(kf->z_data, kf->MeasuredVector, sizeof_float * kf->zSize);
memset(kf->MeasuredVector, 0, sizeof_float * kf->zSize);
}
memcpy(kf->u_data, kf->ControlVector, sizeof_float * kf->uSize);
}
void Kalman_Filter_xhatMinusUpdate(KalmanFilter_t *kf)
{
if (!kf->SkipEq1)
{
if (kf->uSize > 0)
{
kf->temp_vector.numRows = kf->xhatSize;
kf->temp_vector.numCols = 1;
kf->MatStatus = Matrix_Multiply(&kf->F, &kf->xhat, &kf->temp_vector);
kf->temp_vector1.numRows = kf->xhatSize;
kf->temp_vector1.numCols = 1;
kf->MatStatus = Matrix_Multiply(&kf->B, &kf->u, &kf->temp_vector1);
kf->MatStatus = Matrix_Add(&kf->temp_vector, &kf->temp_vector1, &kf->xhatminus);
}
else
{
kf->MatStatus = Matrix_Multiply(&kf->F, &kf->xhat, &kf->xhatminus);
}
}
}
void Kalman_Filter_PminusUpdate(KalmanFilter_t *kf)
{
if (!kf->SkipEq2)
{
kf->MatStatus = Matrix_Transpose(&kf->F, &kf->FT);
kf->MatStatus = Matrix_Multiply(&kf->F, &kf->P, &kf->Pminus);
kf->temp_matrix.numRows = kf->Pminus.numRows;
kf->temp_matrix.numCols = kf->FT.numCols;
kf->MatStatus = Matrix_Multiply(&kf->Pminus, &kf->FT, &kf->temp_matrix); // temp_matrix = F P(k-1) FT
kf->MatStatus = Matrix_Add(&kf->temp_matrix, &kf->Q, &kf->Pminus);
}
}
void Kalman_Filter_SetK(KalmanFilter_t *kf)
{
if (!kf->SkipEq3)
{
kf->MatStatus = Matrix_Transpose(&kf->H, &kf->HT); // z|x => x|z
kf->temp_matrix.numRows = kf->H.numRows;
kf->temp_matrix.numCols = kf->Pminus.numCols;
kf->MatStatus = Matrix_Multiply(&kf->H, &kf->Pminus, &kf->temp_matrix); // temp_matrix = H·P'(k)
kf->temp_matrix1.numRows = kf->temp_matrix.numRows;
kf->temp_matrix1.numCols = kf->HT.numCols;
kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->HT, &kf->temp_matrix1); // temp_matrix1 = H·P'(k)·HT
kf->S.numRows = kf->R.numRows;
kf->S.numCols = kf->R.numCols;
kf->MatStatus = Matrix_Add(&kf->temp_matrix1, &kf->R, &kf->S); // S = H P'(k) HT + R
kf->MatStatus = Matrix_Inverse(&kf->S, &kf->temp_matrix1); // temp_matrix1 = inv(H·P'(k)·HT + R)
kf->temp_matrix.numRows = kf->Pminus.numRows;
kf->temp_matrix.numCols = kf->HT.numCols;
kf->MatStatus = Matrix_Multiply(&kf->Pminus, &kf->HT, &kf->temp_matrix); // temp_matrix = P'(k)·HT
kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->temp_matrix1, &kf->K);
}
}
void Kalman_Filter_xhatUpdate(KalmanFilter_t *kf)
{
if (!kf->SkipEq4)
{
kf->temp_vector.numRows = kf->H.numRows;
kf->temp_vector.numCols = 1;
kf->MatStatus = Matrix_Multiply(&kf->H, &kf->xhatminus, &kf->temp_vector); // temp_vector = H xhat'(k)
kf->temp_vector1.numRows = kf->z.numRows;
kf->temp_vector1.numCols = 1;
kf->MatStatus = Matrix_Subtract(&kf->z, &kf->temp_vector, &kf->temp_vector1); // temp_vector1 = z(k) - H·xhat'(k)
kf->temp_vector.numRows = kf->K.numRows;
kf->temp_vector.numCols = 1;
kf->MatStatus = Matrix_Multiply(&kf->K, &kf->temp_vector1, &kf->temp_vector); // temp_vector = K(k)·(z(k) - H·xhat'(k))
kf->MatStatus = Matrix_Add(&kf->xhatminus, &kf->temp_vector, &kf->xhat);
}
}
void Kalman_Filter_P_Update(KalmanFilter_t *kf)
{
if (!kf->SkipEq5)
{
kf->temp_matrix.numRows = kf->K.numRows;
kf->temp_matrix.numCols = kf->H.numCols;
kf->temp_matrix1.numRows = kf->temp_matrix.numRows;
kf->temp_matrix1.numCols = kf->Pminus.numCols;
kf->MatStatus = Matrix_Multiply(&kf->K, &kf->H, &kf->temp_matrix); // temp_matrix = K(k)·H
kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->Pminus, &kf->temp_matrix1); // temp_matrix1 = K(k)·H·P'(k)
kf->MatStatus = Matrix_Subtract(&kf->Pminus, &kf->temp_matrix1, &kf->P);
}
}
/**
* @brief 执行卡尔曼滤波黄金五式,提供了用户定义函数,可以替代五个中的任意一个环节,方便自行扩展为EKF/UKF/ESKF/AUKF等
*
* @param kf kf类型定义
* @return float* 返回滤波值
*/
float *Kalman_Filter_Update(KalmanFilter_t *kf)
{
// 0. 获取量测信息
Kalman_Filter_Measure(kf);
if (kf->User_Func0_f != NULL)
kf->User_Func0_f(kf);
// 先验估计
// 1. xhat'(k)= A·xhat(k-1) + B·u
Kalman_Filter_xhatMinusUpdate(kf);
if (kf->User_Func1_f != NULL)
kf->User_Func1_f(kf);
// 预测更新
// 2. P'(k) = A·P(k-1)·AT + Q
Kalman_Filter_PminusUpdate(kf);
if (kf->User_Func2_f != NULL)
kf->User_Func2_f(kf);
if (kf->MeasurementValidNum != 0 || kf->UseAutoAdjustment == 0)
{
// 量测更新
// 3. K(k) = P'(k)·HT / (H·P'(k)·HT + R)
Kalman_Filter_SetK(kf);
if (kf->User_Func3_f != NULL)
kf->User_Func3_f(kf);
// 融合
// 4. xhat(k) = xhat'(k) + K(k)·(z(k) - H·xhat'(k))
Kalman_Filter_xhatUpdate(kf);
if (kf->User_Func4_f != NULL)
kf->User_Func4_f(kf);
// 修正方差
// 5. P(k) = (1-K(k)·H)·P'(k) ==> P(k) = P'(k)-K(k)·H·P'(k)
Kalman_Filter_P_Update(kf);
}
else
{
// 无有效量测,仅预测
// xhat(k) = xhat'(k)
// P(k) = P'(k)
memcpy(kf->xhat_data, kf->xhatminus_data, sizeof_float * kf->xhatSize);
memcpy(kf->P_data, kf->Pminus_data, sizeof_float * kf->xhatSize * kf->xhatSize);
}
// 自定义函数,可以提供后处理等
if (kf->User_Func5_f != NULL)
kf->User_Func5_f(kf);
// 避免滤波器过度收敛
// suppress filter excessive convergence
for (uint8_t i = 0; i < kf->xhatSize; ++i)
{
if (kf->P_data[i * kf->xhatSize + i] < kf->StateMinVariance[i])
kf->P_data[i * kf->xhatSize + i] = kf->StateMinVariance[i];
}
memcpy(kf->FilteredValue, kf->xhat_data, sizeof_float * kf->xhatSize);
if (kf->User_Func6_f != NULL)
kf->User_Func6_f(kf);
return kf->FilteredValue;
}
static void H_K_R_Adjustment(KalmanFilter_t *kf)
{
kf->MeasurementValidNum = 0;
memcpy(kf->z_data, kf->MeasuredVector, sizeof_float * kf->zSize);
memset(kf->MeasuredVector, 0, sizeof_float * kf->zSize);
// 识别量测数据有效性并调整矩阵H R K
// recognize measurement validity and adjust matrices H R K
memset(kf->R_data, 0, sizeof_float * kf->zSize * kf->zSize);
memset(kf->H_data, 0, sizeof_float * kf->xhatSize * kf->zSize);
for (uint8_t i = 0; i < kf->zSize; ++i)
{
if (kf->z_data[i] != 0)
{
// 重构向量z
// rebuild vector z
kf->z_data[kf->MeasurementValidNum] = kf->z_data[i];
kf->temp[kf->MeasurementValidNum] = i;
// 重构矩阵H
// rebuild matrix H
kf->H_data[kf->xhatSize * kf->MeasurementValidNum + kf->MeasurementMap[i] - 1] = kf->MeasurementDegree[i];
kf->MeasurementValidNum++;
}
}
for (uint8_t i = 0; i < kf->MeasurementValidNum; ++i)
{
// 重构矩阵R
// rebuild matrix R
kf->R_data[i * kf->MeasurementValidNum + i] = kf->MatR_DiagonalElements[kf->temp[i]];
}
// 调整矩阵维数
// adjust the dimensions of system matrices
kf->H.numRows = kf->MeasurementValidNum;
kf->H.numCols = kf->xhatSize;
kf->HT.numRows = kf->xhatSize;
kf->HT.numCols = kf->MeasurementValidNum;
kf->R.numRows = kf->MeasurementValidNum;
kf->R.numCols = kf->MeasurementValidNum;
kf->K.numRows = kf->xhatSize;
kf->K.numCols = kf->MeasurementValidNum;
kf->z.numRows = kf->MeasurementValidNum;
}
modules/algorithm/kalman_filter.h
+122
View File
@@ -0,0 +1,122 @@
/**
******************************************************************************
* @file kalman filter.h
* @author Wang Hongxi
* @version V1.2.2
* @date 2022/1/8
* @brief
******************************************************************************
* @attention
*
******************************************************************************
*/
#ifndef __KALMAN_FILTER_H
#define __KALMAN_FILTER_H
// cortex-m4 DSP lib
/*
#define __CC_ARM // Keil
#define ARM_MATH_CM4
#define ARM_MATH_MATRIX_CHECK
#define ARM_MATH_ROUNDING
#define ARM_MATH_DSP // define in arm_math.h
*/
//#include "stm32f407xx.h"
#include "arm_math.h"
//#include "dsp/matrix_functions.h"
#include "math.h"
#include "stdint.h"
#include "stdlib.h"
#ifndef user_malloc
#ifdef _CMSIS_OS_H
#define user_malloc pvPortMalloc
#else
#define user_malloc malloc
#endif
#endif
// 若运算速度不够,可以使用q31代替f32,但是精度会降低
#define mat arm_matrix_instance_f32
#define Matrix_Init arm_mat_init_f32
#define Matrix_Add arm_mat_add_f32
#define Matrix_Subtract arm_mat_sub_f32
#define Matrix_Multiply arm_mat_mult_f32
#define Matrix_Transpose arm_mat_trans_f32
#define Matrix_Inverse arm_mat_inverse_f32
typedef struct kf_t
{
float *FilteredValue;
float *MeasuredVector;
float *ControlVector;
uint8_t xhatSize;
uint8_t uSize;
uint8_t zSize;
uint8_t UseAutoAdjustment;
uint8_t MeasurementValidNum;
uint8_t *MeasurementMap; // 量测与状态的关系 how measurement relates to the state
float *MeasurementDegree; // 测量值对应H矩阵元素值 elements of each measurement in H
float *MatR_DiagonalElements; // 量测方差 variance for each measurement
float *StateMinVariance; // 最小方差 避免方差过度收敛 suppress filter excessive convergence
uint8_t *temp;
// 配合用户定义函数使用,作为标志位用于判断是否要跳过标准KF中五个环节中的任意一个
uint8_t SkipEq1, SkipEq2, SkipEq3, SkipEq4, SkipEq5;
// definiion of struct mat: rows & cols & pointer to vars
mat xhat; // x(k|k)
mat xhatminus; // x(k|k-1)
mat u; // control vector u
mat z; // measurement vector z
mat P; // covariance matrix P(k|k)
mat Pminus; // covariance matrix P(k|k-1)
mat F, FT; // state transition matrix F FT
mat B; // control matrix B
mat H, HT; // measurement matrix H
mat Q; // process noise covariance matrix Q
mat R; // measurement noise covariance matrix R
mat K; // kalman gain K
mat S, temp_matrix, temp_matrix1, temp_vector, temp_vector1;
int8_t MatStatus;
// 用户定义函数,可以替换或扩展基准KF的功能
void (*User_Func0_f)(struct kf_t *kf);
void (*User_Func1_f)(struct kf_t *kf);
void (*User_Func2_f)(struct kf_t *kf);
void (*User_Func3_f)(struct kf_t *kf);
void (*User_Func4_f)(struct kf_t *kf);
void (*User_Func5_f)(struct kf_t *kf);
void (*User_Func6_f)(struct kf_t *kf);
// 矩阵存储空间指针
float *xhat_data, *xhatminus_data;
float *u_data;
float *z_data;
float *P_data, *Pminus_data;
float *F_data, *FT_data;
float *B_data;
float *H_data, *HT_data;
float *Q_data;
float *R_data;
float *K_data;
float *S_data, *temp_matrix_data, *temp_matrix_data1, *temp_vector_data, *temp_vector_data1;
} KalmanFilter_t;
extern uint16_t sizeof_float, sizeof_double;
void Kalman_Filter_Init(KalmanFilter_t *kf, uint8_t xhatSize, uint8_t uSize, uint8_t zSize);
void Kalman_Filter_Measure(KalmanFilter_t *kf);
void Kalman_Filter_xhatMinusUpdate(KalmanFilter_t *kf);
void Kalman_Filter_PminusUpdate(KalmanFilter_t *kf);
void Kalman_Filter_SetK(KalmanFilter_t *kf);
void Kalman_Filter_xhatUpdate(KalmanFilter_t *kf);
void Kalman_Filter_P_Update(KalmanFilter_t *kf);
float *Kalman_Filter_Update(KalmanFilter_t *kf);
#endif //__KALMAN_FILTER_H
modules/algorithm/user_lib.c
+214
View File
@@ -0,0 +1,214 @@
/**
******************************************************************************
* @file user_lib.c
* @author Wang Hongxi
* @author modified by neozng
* @version 0.2 beta
* @date 2021/2/18
* @brief
******************************************************************************
* @attention
*
******************************************************************************
*/
#include "stdlib.h"
#include "memory.h"
#include "user_lib.h"
#include "math.h"
#include "main.h"
#ifdef _CMSIS_OS_H
#define user_malloc pvPortMalloc
#else
#define user_malloc malloc
#endif
void *zmalloc(size_t size)
{
void *ptr = malloc(size);
memset(ptr, 0, size);
return ptr;
}
// 快速开方
float Sqrt(float x)
{
float y;
float delta;
float maxError;
if (x <= 0)
{
return 0;
}
// initial guess
y = x / 2;
// refine
maxError = x * 0.001f;
do
{
delta = (y * y) - x;
y -= delta / (2 * y);
} while (delta > maxError || delta < -maxError);
return y;
}
// 绝对值限制
float abs_limit(float num, float Limit)
{
if (num > Limit)
{
num = Limit;
}
else if (num < -Limit)
{
num = -Limit;
}
return num;
}
// 判断符号位
float sign(float value)
{
if (value >= 0.0f)
{
return 1.0f;
}
else
{
return -1.0f;
}
}
// 浮点死区
float float_deadband(float Value, float minValue, float maxValue)
{
if (Value < maxValue && Value > minValue)
{
Value = 0.0f;
}
return Value;
}
// 限幅函数
float float_constrain(float Value, float minValue, float maxValue)
{
if (Value < minValue)
return minValue;
else if (Value > maxValue)
return maxValue;
else
return Value;
}
// 限幅函数
int16_t int16_constrain(int16_t Value, int16_t minValue, int16_t maxValue)
{
if (Value < minValue)
return minValue;
else if (Value > maxValue)
return maxValue;
else
return Value;
}
// 循环限幅函数
float loop_float_constrain(float Input, float minValue, float maxValue)
{
if (maxValue < minValue)
{
return Input;
}
if (Input > maxValue)
{
float len = maxValue - minValue;
while (Input > maxValue)
{
Input -= len;
}
}
else if (Input < minValue)
{
float len = maxValue - minValue;
while (Input < minValue)
{
Input += len;
}
}
return Input;
}
// 弧度格式化为-PI~PI
// 角度格式化为-180~180
float theta_format(float Ang)
{
return loop_float_constrain(Ang, -180.0f, 180.0f);
}
int float_rounding(float raw)
{
static int integer;
static float decimal;
integer = (int)raw;
decimal = raw - integer;
if (decimal > 0.5f)
integer++;
return integer;
}
// 三维向量归一化
float *Norm3d(float *v)
{
float len = Sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
v[0] /= len;
v[1] /= len;
v[2] /= len;
return v;
}
// 计算模长
float NormOf3d(float *v)
{
return Sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
}
// 三维向量叉乘v1 x v2
void Cross3d(float *v1, float *v2, float *res)
{
res[0] = v1[1] * v2[2] - v1[2] * v2[1];
res[1] = v1[2] * v2[0] - v1[0] * v2[2];
res[2] = v1[0] * v2[1] - v1[1] * v2[0];
}
// 三维向量点乘
float Dot3d(float *v1, float *v2)
{
return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
}
// 均值滤波,删除buffer中的最后一个元素,填入新的元素并求平均值
float AverageFilter(float new_data, float *buf, uint8_t len)
{
float sum = 0;
for (uint8_t i = 0; i < len - 1; i++)
{
buf[i] = buf[i + 1];
sum += buf[i];
}
buf[len - 1] = new_data;
sum += new_data;
return sum / len;
}
void MatInit(mat *m, uint8_t row, uint8_t col)
{
m->numCols = col;
m->numRows = row;
m->pData = (float *)zmalloc(row * col * sizeof(float));
}
modules/algorithm/user_lib.h
+126
View File
@@ -0,0 +1,126 @@
/**
******************************************************************************
* @file user_lib.h
* @author Wang Hongxi
* @version V1.0.0
* @date 2021/2/18
* @brief
******************************************************************************
* @attention
*
******************************************************************************
*/
#ifndef _USER_LIB_H
#define _USER_LIB_H
#include "stdint.h"
#include "main.h"
#include "cmsis_os.h"
//#include "stm32f407xx.h"
#include "arm_math.h"
#ifndef user_malloc
#ifdef _CMSIS_OS_H
#define user_malloc pvPortMalloc
#else
#define user_malloc malloc
#endif
#endif
#define msin(x) (arm_sin_f32(x))
#define mcos(x) (arm_cos_f32(x))
typedef arm_matrix_instance_f32 mat;
// 若运算速度不够,可以使用q31代替f32,但是精度会降低
#define MatAdd arm_mat_add_f32
#define MatSubtract arm_mat_sub_f32
#define MatMultiply arm_mat_mult_f32
#define MatTranspose arm_mat_trans_f32
#define MatInverse arm_mat_inverse_f32
void MatInit(mat *m, uint8_t row, uint8_t col);
/* boolean type definitions */
#ifndef TRUE
#define TRUE 1 /**< boolean true */
#endif
#ifndef FALSE
#define FALSE 0 /**< boolean fails */
#endif
/* circumference ratio */
#ifndef PI
#define PI 3.14159265354f
#endif
#define VAL_LIMIT(val, min, max) \
do \
{ \
if ((val) <= (min)) \
{ \
(val) = (min); \
} \
else if ((val) >= (max)) \
{ \
(val) = (max); \
} \
} while (0)
#define ANGLE_LIMIT_360(val, angle) \
do \
{ \
(val) = (angle) - (int)(angle); \
(val) += (int)(angle) % 360; \
} while (0)
#define ANGLE_LIMIT_360_TO_180(val) \
do \
{ \
if ((val) > 180) \
(val) -= 360; \
} while (0)
#define VAL_MIN(a, b) ((a) < (b) ? (a) : (b))
#define VAL_MAX(a, b) ((a) > (b) ? (a) : (b))
/**
* @brief 返回一块干净的内�?,不过仍然需要强制转�?为你需要的类型
*
* @param size 分配大小
* @return void*
*/
void *zmalloc(size_t size);
// ���ٿ���
float Sqrt(float x);
// ��������
float abs_limit(float num, float Limit);
// �жϷ���λ
float sign(float value);
// ��������
float float_deadband(float Value, float minValue, float maxValue);
// �޷�����
float float_constrain(float Value, float minValue, float maxValue);
// �޷�����
int16_t int16_constrain(int16_t Value, int16_t minValue, int16_t maxValue);
// ѭ���޷�����
float loop_float_constrain(float Input, float minValue, float maxValue);
// �Ƕ� ���޷� 180 ~ -180
float theta_format(float Ang);
int float_rounding(float raw);
float *Norm3d(float *v);
float NormOf3d(float *v);
void Cross3d(float *v1, float *v2, float *res);
float Dot3d(float *v1, float *v2);
float AverageFilter(float new_data, float *buf, uint8_t len);
#define rad_format(Ang) loop_float_constrain((Ang), -PI, PI)
#endif
modules/bluetooth/HC05.c
+58
View File
@@ -0,0 +1,58 @@
#include "HC05.h"
#include "bsp_usart.h"
#define HC05_BUFFERSIZE HC05_DATASIZE+2 // HC05发送和接收数据buffer大小，不得大于256
#define FRAME_HEAD 0XAA // 帧头
#define FRAME_END 0X55 // 帧尾
static HC05 hc05_msg; // HC05通信数据
static USARTInstance *hc05_usart_instance; // HC05串口通信实例
static uint8_t hc05_init_flag = 0; // HC05初始化标志位
// *hc05_usart_instance串口回调函数
static void HC05RxCallback()
{
uint8_t *rxbuff;
rxbuff = hc05_usart_instance->recv_buff;
// 帧头帧尾判断
if(rxbuff[0] == FRAME_HEAD && rxbuff[HC05_BUFFERSIZE - 1] == FRAME_END)
{
for(int i = 0; i < HC05_DATASIZE; i++)
hc05_msg.recv_data[i] = (uint8_t)rxbuff[i+1];
}
return;
}
// HC05串口接收初始化
HC05 *HC05Init(UART_HandleTypeDef *hc05_usart_handle)
{
USART_Init_Config_s conf;
conf.module_callback = HC05RxCallback;
conf.usart_handle = hc05_usart_handle;
conf.recv_buff_size = HC05_BUFFERSIZE;
hc05_usart_instance = USARTRegister(&conf);
hc05_init_flag = 1;
return (HC05*)&hc05_msg;
}
// HC05串口发送函数，一次最多发送HC05_DATASIZE个数据
void HC05_SendData(uint8_t *data, uint8_t data_num)
{
// 发送数据中加入帧头和帧尾
hc05_msg.send_data[0] = FRAME_HEAD;
for (int i = 0; i < data_num; i++)
hc05_msg.send_data[i+1] = data[i];
hc05_msg.send_data[HC05_BUFFERSIZE - 1] = FRAME_END;
// 发送数据
USARTSend(hc05_usart_instance, hc05_msg.send_data, data_num+2, USART_TRANSFER_IT);
}
modules/bluetooth/HC05.h
+23
View File
@@ -0,0 +1,23 @@
#ifndef HC05_H
#define HC05_H
#include <stdint.h>
#include "main.h"
#include "usart.h"
#define HC05_DATASIZE 4 // HC05接收和发送数据大小，根据需要修改
// HC05通信数据结构体，后续根据需要添加和修改
typedef struct
{
uint8_t send_data[HC05_DATASIZE + 2]; // 发送数据，加上帧头和帧尾
uint8_t recv_data[HC05_DATASIZE]; // 接收数据
}HC05;
// HC05串口接收初始化
HC05 *HC05Init(UART_HandleTypeDef *hc05_usart_handle);
// HC05串口发送函数，一次最多发送HC05_DATASIZE个数据
void HC05_SendData( uint8_t *data, uint8_t data_num);
#endif
modules/can_comm/can_comm.c
+134
View File
@@ -0,0 +1,134 @@
#include "can_comm.h"
#include "memory.h"
#include "stdlib.h"
#include "crc8.h"
#include "bsp_dwt.h"
#include "bsp_log.h"
/**
* @brief 重置CAN comm的接收状态和buffer
*
* @param ins 需要重置的实例
*/
static void CANCommResetRx(CANCommInstance *ins)
{
// 当前已经收到的buffer清零
memset(ins->raw_recvbuf, 0, ins->cur_recv_len);
ins->recv_state = 0; // 接收状态重置
ins->cur_recv_len = 0; // 当前已经收到的长度重置
}
/**
* @brief cancomm的接收回调函数
*
* @param _instance
*/
static void CANCommRxCallback(CANInstance *_instance)
{
CANCommInstance *comm = (CANCommInstance *)_instance->id; // 注意写法,将can instance的id强制转换为CANCommInstance*类型
/* 当前接收状态判断 */
if (_instance->rx_buff[0] == CAN_COMM_HEADER && comm->recv_state == 0) // 之前尚未开始接收且此次包里第一个位置是帧头
{
if (_instance->rx_buff[1] == comm->recv_data_len) // 如果这一包里的datalen也等于我们设定接收长度(这是因为暂时不支持动态包长)
{
comm->recv_state = 1; // 设置接收状态为1,说明已经开始接收
}
else
return; // 直接跳过即可
}
if (comm->recv_state) // 已经收到过帧头
{
// 如果已经接收到的长度加上当前一包的长度大于总buf len,说明接收错误
if (comm->cur_recv_len + _instance->rx_len > comm->recv_buf_len)
{
CANCommResetRx(comm);
return; // 重置状态然后返回
}
// 直接把当前接收到的数据接到buffer后面
memcpy(comm->raw_recvbuf + comm->cur_recv_len, _instance->rx_buff, _instance->rx_len);
comm->cur_recv_len += _instance->rx_len;
// 收完这一包以后刚好等于总buf len,说明已经收完了
if (comm->cur_recv_len == comm->recv_buf_len)
{
// 如果buff里本tail的位置等于CAN_COMM_TAIL
if (comm->raw_recvbuf[comm->recv_buf_len - 1] == CAN_COMM_TAIL)
{ // 通过校验,复制数据到unpack_data中
if (comm->raw_recvbuf[comm->recv_buf_len - 2] == crc_8(comm->raw_recvbuf + 2, comm->recv_data_len))
{ // 数据量大的话考虑使用DMA
memcpy(comm->unpacked_recv_data, comm->raw_recvbuf + 2, comm->recv_data_len);
comm->update_flag = 1; // 数据更新flag置为1
DaemonReload(comm->comm_daemon); // 重载daemon,避免数据更新后一直不被读取而导致数据更新不及时
}
}
CANCommResetRx(comm);
return; // 重置状态然后返回
}
}
}
static void CANCommLostCallback(void *cancomm)
{
CANCommInstance *comm = (CANCommInstance *)cancomm;
CANCommResetRx(comm);
LOGWARNING("[can_comm] can comm rx[%d] lost, reset rx state.", &comm->can_ins->rx_id);
}
CANCommInstance *CANCommInit(CANComm_Init_Config_s *comm_config)
{
CANCommInstance *ins = (CANCommInstance *)malloc(sizeof(CANCommInstance));
memset(ins, 0, sizeof(CANCommInstance));
ins->recv_data_len = comm_config->recv_data_len;
ins->recv_buf_len = comm_config->recv_data_len + CAN_COMM_OFFSET_BYTES; // head + datalen + crc8 + tail
ins->send_data_len = comm_config->send_data_len;
ins->send_buf_len = comm_config->send_data_len + CAN_COMM_OFFSET_BYTES;
ins->raw_sendbuf[0] = CAN_COMM_HEADER; // head,直接设置避免每次发送都要重新赋值,下面的tail同理
ins->raw_sendbuf[1] = comm_config->send_data_len; // datalen
ins->raw_sendbuf[comm_config->send_data_len + CAN_COMM_OFFSET_BYTES - 1] = CAN_COMM_TAIL;
// can instance的设置
comm_config->can_config.id = ins; // CANComm的实例指针作为CANInstance的id,回调函数中会用到
comm_config->can_config.can_module_callback = CANCommRxCallback;
ins->can_ins = CANRegister(&comm_config->can_config);
Daemon_Init_Config_s daemon_config = {
.callback = CANCommLostCallback,
.owner_id = (void *)ins,
.reload_count = comm_config->daemon_count,
};
ins->comm_daemon = DaemonRegister(&daemon_config);
return ins;
}
void CANCommSend(CANCommInstance *instance, uint8_t *data)
{
static uint8_t crc8;
static uint8_t send_len;
// 将data copy到raw_sendbuf中,计算crc8
memcpy(instance->raw_sendbuf + 2, data, instance->send_data_len);
crc8 = crc_8(data, instance->send_data_len);
instance->raw_sendbuf[2 + instance->send_data_len] = crc8;
// CAN单次发送最大为8字节,如果超过8字节,需要分包发送
for (size_t i = 0; i < instance->send_buf_len; i += 8)
{ // 如果是最后一包,send len将会小于8,要修改CAN的txconf中的DLC位,调用bsp_can提供的接口即可
send_len = instance->send_buf_len - i >= 8 ? 8 : instance->send_buf_len - i;
CANSetDLC(instance->can_ins, send_len);
memcpy(instance->can_ins->tx_buff, instance->raw_sendbuf + i, send_len);
CANTransmit(instance->can_ins, 1);
}
}
void *CANCommGet(CANCommInstance *instance)
{
instance->update_flag = 0; // 读取后将更新flag置为0
return instance->unpacked_recv_data;
}
uint8_t CANCommIsOnline(CANCommInstance *instance)
{
return DaemonIsOnline(instance->comm_daemon);
}
modules/can_comm/can_comm.h
+92
View File
@@ -0,0 +1,92 @@
/**
* @file can_comm.h
* @author Neo neozng1@hnu.edu.cn
* @brief 用于多机CAN通信的收发模块
* @version 0.1
* @date 2022-11-27
*
* @copyright Copyright (c) 2022 HNUYueLu EC all rights reserved
*
*/
#ifndef CAN_COMM_H
#define CAN_COMM_H
#include "bsp_can.h"
#include "daemon.h"
#define MX_CAN_COMM_COUNT 4 // 注意均衡负载,一条总线上不要挂载过多的外设
#define CAN_COMM_MAX_BUFFSIZE 60 // 最大发送/接收字节数,如果不够可以增加此数值
#define CAN_COMM_HEADER 's' // 帧头
#define CAN_COMM_TAIL 'e' // 帧尾
#define CAN_COMM_OFFSET_BYTES 4 // 's'+ datalen + 'e' + crc8
#pragma pack(1)
/* CAN comm 结构体, 拥有CAN comm的app应该包含一个CAN comm指针 */
typedef struct
{
CANInstance *can_ins;
/* 发送部分 */
uint8_t send_data_len; // 发送数据长度
uint8_t send_buf_len; // 发送缓冲区长度,为发送数据长度+帧头单包数据长度帧尾以及校验和(4)
uint8_t raw_sendbuf[CAN_COMM_MAX_BUFFSIZE + CAN_COMM_OFFSET_BYTES]; // 额外4个bytes保存帧头帧尾和校验和
/* 接收部分 */
uint8_t recv_data_len; // 接收数据长度
uint8_t recv_buf_len; // 接收缓冲区长度,为接收数据长度+帧头单包数据长度帧尾以及校验和(4)
uint8_t raw_recvbuf[CAN_COMM_MAX_BUFFSIZE + CAN_COMM_OFFSET_BYTES]; // 额外4个bytes保存帧头帧尾和校验和
uint8_t unpacked_recv_data[CAN_COMM_MAX_BUFFSIZE]; // 解包后的数据,调用CANCommGet()后cast成对应的类型通过指针读取即可
/* 接收和更新标志位*/
uint8_t recv_state; // 接收状态,
uint8_t cur_recv_len; // 当前已经接收到的数据长度(包括帧头帧尾datalen和校验和)
uint8_t update_flag; // 数据更新标志位,当接收到新数据时,会将此标志位置1,调用CANCommGet()后会将此标志位置0
DaemonInstance* comm_daemon;
} CANCommInstance;
#pragma pack()
/* CAN comm 初始化结构体 */
typedef struct
{
CAN_Init_Config_s can_config; // CAN初始化结构体
uint8_t send_data_len; // 发送数据长度
uint8_t recv_data_len; // 接收数据长度
uint16_t daemon_count; // 守护进程计数,用于初始化守护进程
} CANComm_Init_Config_s;
/**
* @brief 初始化CANComm
*
* @param config CANComm初始化结构体
* @return CANCommInstance*
*/
CANCommInstance *CANCommInit(CANComm_Init_Config_s *comm_config);
/**
* @brief 通过CANComm发送数据
*
* @param instance cancomm实例
* @param data 注意此地址的有效数据长度需要和初始化时传入的datalen相同
*/
void CANCommSend(CANCommInstance *instance, uint8_t *data);
/**
* @brief 获取CANComm接收的数据,需要自己使用强制类型转换将返回的void指针转换成指定类型
*
* @return void* 返回的数据指针
* @attention 注意如果希望直接通过转换指针访问数据,如果数据是union或struct,要检查是否使用了pack(n)
* CANComm接收到的数据可以看作是pack(1)之后的,是连续存放的.
* 如果使用了pack(n)可能会导致数据错乱,并且无法使用强制类型转换通过memcpy直接访问,转而需要手动解包.
* 强烈建议通过CANComm传输的数据使用pack(1)
*/
void *CANCommGet(CANCommInstance *instance);
/**
* @brief 检查CANComm是否在线
*
* @param instance
* @return uint8_t
*/
uint8_t CANCommIsOnline(CANCommInstance *instance);
#endif // !CAN_COMM_H
modules/can_comm/can_comm.md
+140
View File
@@ -0,0 +1,140 @@
# can_comm
<p align='right'>neozng1@hnu.edu.cn</p>
> TODO:
>
> 1. 对`CANCommGet()`进行修改，使得其可以返回数据是否更新的相关信息。
## 重要提醒
如果传输过程中出现多次丢包或长度校验不通过，尤其是传输长度较大的时候，请开启CAN的Auto Retransmission，并尝试修改CANComm实例的发送和接受ID（以提高在总线仲裁中的优先级）。
## 总览和封装说明
CAN comm是用于CAN多机通信的模块。你不需要关心实现的协议，**只需要传入你想发送/接收的结构体的长度**（通过`sizeof()`获取）即可。单帧发送长度目前限制为60字节，如果需要加长可以在`can_comm.h`中修改`CAN_COMM_MAX_BUFFSIZE`的定义值。注意CAN的负载以及使用此模块的发送频率，否则可能造成堵塞。
## 代码结构
.h中放置的是数据定义和外部接口，以及协议的定义和宏，.c中包含一些私有函数。
## 外部接口
```c
CANCommInstance *CANCommInit(CANComm_Init_Config_s* comm_config);
void CANCommSend(CANCommInstance *instance, uint8_t *data);
void *CANCommGet(CANCommInstance *instance);
uint8_t CANCommIsOnline(CANCommInstance *instance);
```
第一个函数将会初始化一个CANComm实例，返回其指针。使用CANComm进行通信的应用应该保存返回的指针。初始化需要传入一个初始化结构体。请在应用初始化的时候调用该函数。推荐的结构体配置方式如下：
```c
/* 初始化结构体的定义 */
typedef struct
{
can_instance_config_s can_config;
uint8_t send_data_len; //发送数据长度
uint8_t recv_data_len; //接收数据长度,两者请用sizeof获取
} CANComm_Init_Config_s;
CANComm_Init_Config_s cconfig = {
.can_config = {
.can_handle=&hcan1,
.tx_id=0x02,
.rx_id=0x03},
.send_data_len = sizeof(your_data_structure),
.recv_data_len = sizeof(recv_data_structure)
};
```
第二个函数将会通过CAN发送一帧数据。这里的“帧”不是CAN的一帧报文，而是你使用的数据。在CANComm初始化的时候，使用sizeof获得你要发送的数据的长度。调用此函数时，将你的数据结构或数据cast成`u8`类型的指针传入即可。==**特别注意，你的结构体需要使用预编译指令`#pragma pack(1)`以及`#pragma pack()`包裹**==，从而进行字节压缩而不进行字节对齐，如：
```c
#pragma pack(1)
typedef struct
{
uint8_t aa;
float bb;
double cc;
uint16_t dd;
} some_struct;
#pragma pack()
```
只有这样，这个结构体才不会进行padding扩充字节实现字节对齐。你可以尝试一下，如果不使用pack处理，上面的结构体将会占据18个字节以上的空间；开启pack后，会降低至15。更多关于字节对齐的内容，自行查询。
> 后期可能更新字节对齐和内存访问相关的教程。
`CANCommGet()`是获取来自CANComm接收到的数据的接口，返回值为一个void类型指针，你需要通过**强制类型转换**将其变为你设定的接收类型指针，然后就可以访问其数据了。
## 私有函数和变量
```c
static CANCommInstance *can_comm_instance[MX_CAN_COMM_COUNT] = {NULL};
static uint8_t idx;
static void CANCommResetRx(CANCommInstance *ins);
static void CANCommRxCallback(can_instance *_instance);
```
第一个指针数组保存所有的can comm实例，从而在callback中区分不同实例。idx是初始化使用的索引，用于确定每个can comm实例指针存放的位置。
`CANCommResetRx()`在`CANCommRxCallback()`中多次被调用，用于清空接收缓冲区。
`CANCommRxCallback()`是CAN comm初始化can实例时的回调函数，用于can接收中断，进行协议解析。
## 使用范例
例如，这里要发送的数据是一个float，接收的数据是如下的`struct`，**==注意要使用pack==**：
```c
#pragma pack(1)
struct test
{
float aa;
float bb;
float cc;
uint16_t dd;
};
#pragma pack()
```
初始化时设置如下：
```c
CANComm_Init_Config_s cconfig = {
.can_config = {
.can_handle=&hcan1,
.tx_id=0x02,
.rx_id=0x03},
.send_data_len = sizeof(float),
.recv_data_len = sizeof(struct test)
};
CANCommInstance* ins = CANCommInit(&cconfig);
```
通过`CANCommGet()`并使用强制类型转换获得接收到的数据指针：
```c
struct test* data_ptr=(struct test*)CANCommGet(ins)
```
发送通过`CANCommSend()`，建议使用强制类型转换：
```c
float tx=114.514;
CANCommSend(ins, (uint8_t*)&tx);
```
## 接收解析流程
CAN comm的通信协议如下：
| 帧头 | 数据长度 | 数据 | crc8校验 | 帧尾 |
| ----------- | -------- | ---------------- | -------- | ----------- |
| 's'（0x73） | 0~255 | 最大60（可修改） | 校验码 | 'e'（0x65） |
| 1-byte | 1-byte | n-byte | 1-byte | 1-byte |
接收的流程见代码注释。
流程图如下：![未命名文件](../../.assets/CANcomm.png)
modules/daemon/daemon.c
+56
View File
@@ -0,0 +1,56 @@
#include "daemon.h"
#include "bsp_dwt.h" // 后续通过定时器来计时?
#include "stdlib.h"
#include "memory.h"
#include "buzzer.h"
// 用于保存所有的daemon instance
static DaemonInstance *daemon_instances[DAEMON_MX_CNT] = {NULL};
static uint8_t idx; // 用于记录当前的daemon instance数量,配合回调使用
DaemonInstance *DaemonRegister(Daemon_Init_Config_s *config)
{
DaemonInstance *instance = (DaemonInstance *)malloc(sizeof(DaemonInstance));
memset(instance, 0, sizeof(DaemonInstance));
instance->owner_id = config->owner_id;
instance->reload_count = config->reload_count == 0 ? 100 : config->reload_count; // 默认值为100
instance->callback = config->callback;
instance->temp_count = config->init_count == 0 ? 100 : config->init_count; // 默认值为100,初始计数
instance->temp_count = config->reload_count;
daemon_instances[idx++] = instance;
return instance;
}
/* "喂狗"函数 */
void DaemonReload(DaemonInstance *instance)
{
instance->temp_count = instance->reload_count;
}
uint8_t DaemonIsOnline(DaemonInstance *instance)
{
return instance->temp_count > 0;
}
void DaemonTask()
{
DaemonInstance *dins; // 提高可读性同时降低访存开销
for (size_t i = 0; i < idx; ++i)
{
dins = daemon_instances[i];
if (dins->temp_count > 0) // 如果计数器还有值,说明上一次喂狗后还没有超时,则计数器减一
dins->temp_count--;
else if (dins->callback) // 等于零说明超时了,调用回调函数(如果有的话)
{
dins->callback(dins->owner_id); // module内可以将owner_id强制类型转换成自身类型从而调用特定module的offline callback
// @todo 为蜂鸣器/led等增加离线报警的功能,非常关键!
}
}
}
// (需要id的原因是什么?) 下面是copilot的回答!
// 需要id的原因是因为有些module可能有多个实例,而我们需要知道具体是哪个实例offline
// 如果只有一个实例,则可以不用id,直接调用callback即可
// 比如: 有一个module叫做"电机",它有两个实例,分别是"电机1"和"电机2",那么我们调用电机的离线处理函数时就需要知道是哪个电机offline
modules/daemon/daemon.h
+62
View File
@@ -0,0 +1,62 @@
#ifndef MONITOR_H
#define MONITOR_H
#include "stdint.h"
#include "string.h"
#define DAEMON_MX_CNT 64
/* 模块离线处理函数指针 */
typedef void (*offline_callback)(void *);
/* daemon结构体定义 */
typedef struct daemon_ins
{
uint16_t reload_count; // 重载值
offline_callback callback; // 异常处理函数,当模块发生异常时会被调用
uint16_t temp_count; // 当前值,减为零说明模块离线或异常
void *owner_id; // daemon实例的地址,初始化的时候填入
} DaemonInstance;
/* daemon初始化配置 */
typedef struct
{
uint16_t reload_count; // 实际上这是app唯一需要设置的值?
uint16_t init_count; // 上线等待时间,有些模块需要收到主控的指令才会反馈报文,或pc等需要开机时间
offline_callback callback; // 异常处理函数,当模块发生异常时会被调用
void *owner_id; // id取拥有daemon的实例的地址,如DJIMotorInstance*,cast成void*类型
} Daemon_Init_Config_s;
/**
* @brief 注册一个daemon实例
*
* @param config 初始化配置
* @return DaemonInstance* 返回实例指针
*/
DaemonInstance *DaemonRegister(Daemon_Init_Config_s *config);
/**
* @brief 当模块收到新的数据或进行其他动作时,调用该函数重载temp_count,相当于"喂狗"
*
* @param instance daemon实例指针
*/
void DaemonReload(DaemonInstance *instance);
/**
* @brief 确认模块是否离线
*
* @param instance daemon实例指针
* @return uint8_t 若在线且工作正常,返回1;否则返回零. 后续根据异常类型和离线状态等进行优化.
*/
uint8_t DaemonIsOnline(DaemonInstance *instance);
/**
* @brief 放入rtos中,会给每个daemon实例的temp_count按频率进行递减操作.
* 模块成功接受数据或成功操作则会重载temp_count的值为reload_count.
*
*/
void DaemonTask();
#endif // !MONITOR_H
modules/daemon/daemon.md
+36
View File
@@ -0,0 +1,36 @@
# daemon
<p align='right'>neozng1@hnu.edu.cn</p>
用于监测模块和应用运行情况的module(和官方代码中的deteck task作用相同)
## 使用范例
要使用该module，则包含`daemon.h`的头文件，并在使用daemon的文件中保留一个daemon的指针。
初始化的时候，要传入以下参数：
```c
typedef struct
{
uint16_t reload_count; // 实际上这是app唯一需要设置的值?
offline_callback callback; // 异常处理函数,当模块发生异常时会被调用
void *owner_id; // id取拥有daemon的实例的地址,如DJIMotorInstance*,cast成void*类型
} Daemon_Init_Config_s;
```
`reload_count`是”喂狗“时的重载值，一般根据你希望的离线容许时间和模块接收数据/访问数据的频率确定。daemonTask递减计数器的频率是100hz(在HAL_N_Middlewares/Src/freertos.c中查看任务),你可以据此以及模块收到数据/操作的频率设置reload_count。
`daemon_task()`会在实时系统中以1kHz的频率运行，每次运行该任务，都会将所有daemon实例当前的count进行自减操作，当count减为零，则说明模块已经很久没有上线（处于deactivated状态，即没有收到数据，也没有进行其他读写操作）。
`offline_callback`是模块离线的回调函数，即当包含daemon的模块发生离线情况时，该函数会被调用以应对离线情况，如重启模块，重新初始化等。如果没有则传入`NULL`即可。
`owner_id`即模块取自身地址并通过强制类型转换化为`void*`类型，用于拥有多个实例的模块在`offline_callback`中区分自身。如多个电机都使用一个相同的`offline_callback`，那么在调用回调函数的时候就可以通过该指针来访问某个特定的电机。
> 这种方法也称作“parent pointer”，即**保存拥有指向自身的指针对象的地址**。这样就可以在特定的情况下通过自身来访问自己的父对象。
## 具体实现
即`DaemonTask()`，在操作系统中以1kHz运行。注释详细，请参见`daemon.c`。
modules/encoder/encoder.md
+6
View File
@@ -0,0 +1,6 @@
# encoder
提供对光电和霍尔磁编码器的支持.
可能需要新增bsp_tim以提供period lapse和tim编码器计数的功能.
modules/general_def.h
+17
View File
@@ -0,0 +1,17 @@
#ifndef GENERAL_DEF_H
#define GENERAL_DEF_H
// 一些module的通用数值型定义,注意条件macro兼容,一些宏可能在math.h中已经定义过了
#ifndef PI
#define PI 3.1415926535f
#endif
#define PI2 (PI * 2.0f) // 2 pi
#define RAD_2_DEGREE 57.2957795f // 180/pi
#define DEGREE_2_RAD 0.01745329252f // pi/180
#define RPM_2_ANGLE_PER_SEC 6.0f // ×360°/60sec
#define RPM_2_RAD_PER_SEC 0.104719755f // ×2pi/60sec
#endif // !GENERAL_DEF_H
modules/imu/BMI088Middleware.c
+29
View File
@@ -0,0 +1,29 @@
#include "BMI088Middleware.h"
#include "main.h"
SPI_HandleTypeDef *BMI088_SPI;
void BMI088_ACCEL_NS_L(void)
{
HAL_GPIO_WritePin(CS2_ACCEL_GPIO_Port, CS2_ACCEL_Pin, GPIO_PIN_RESET);
}
void BMI088_ACCEL_NS_H(void)
{
HAL_GPIO_WritePin(CS2_ACCEL_GPIO_Port, CS2_ACCEL_Pin, GPIO_PIN_SET);
}
void BMI088_GYRO_NS_L(void)
{
HAL_GPIO_WritePin(CS2_GYRO_GPIO_Port, CS2_GYRO_Pin, GPIO_PIN_RESET);
}
void BMI088_GYRO_NS_H(void)
{
HAL_GPIO_WritePin(CS2_GYRO_GPIO_Port, CS2_GYRO_Pin, GPIO_PIN_SET);
}
uint8_t BMI088_read_write_byte(uint8_t txdata)
{
uint8_t rx_data;
HAL_SPI_TransmitReceive(BMI088_SPI, &txdata, &rx_data, 1, 1000);
return rx_data;
}
modules/imu/BMI088Middleware.h
+32
View File
@@ -0,0 +1,32 @@
#ifndef BMI088MIDDLEWARE_H
#define BMI088MIDDLEWARE_H
#include "main.h"
#define BMI088_USE_SPI
//#define BMI088_USE_IIC
/*
#define CS1_ACCEL_GPIO_Port ACCEL_NSS_GPIO_Port
#define CS1_ACCEL_Pin ACCEL_NSS_Pin
#define CS1_GYRO_GPIO_Port GYRO_NSS_GPIO_Port
#define CS1_GYRO_Pin GYRO_NSS_Pin
*/
#if defined(BMI088_USE_SPI)
extern void BMI088_ACCEL_NS_L(void);
extern void BMI088_ACCEL_NS_H(void);
extern void BMI088_GYRO_NS_L(void);
extern void BMI088_GYRO_NS_H(void);
extern uint8_t BMI088_read_write_byte(uint8_t reg);
extern SPI_HandleTypeDef *BMI088_SPI;
#elif defined(BMI088_USE_IIC)
#endif
#endif
modules/imu/BMI088driver.c
+405
View File
@@ -0,0 +1,405 @@
#include "BMI088driver.h"
#include "BMI088reg.h"
#include "BMI088Middleware.h"
#include "bsp_dwt.h"
#include "bsp_log.h"
#include <math.h>
#pragma message "this is a legacy support. test the new BMI088 module as soon as possible."
float BMI088_ACCEL_SEN = BMI088_ACCEL_6G_SEN;
float BMI088_GYRO_SEN = BMI088_GYRO_2000_SEN;
static uint8_t res = 0;
static uint8_t write_reg_num = 0;
static uint8_t error = BMI088_NO_ERROR;
float gyroDiff[3], gNormDiff;
uint8_t caliOffset = 1;
int16_t caliCount = 0;
IMU_Data_t BMI088;
#if defined(BMI088_USE_SPI)
#define BMI088_accel_write_single_reg(reg, data) \
{ \
BMI088_ACCEL_NS_L(); \
BMI088_write_single_reg((reg), (data)); \
BMI088_ACCEL_NS_H(); \
}
#define BMI088_accel_read_single_reg(reg, data) \
{ \
BMI088_ACCEL_NS_L(); \
BMI088_read_write_byte((reg) | 0x80); \
BMI088_read_write_byte(0x55); \
(data) = BMI088_read_write_byte(0x55); \
BMI088_ACCEL_NS_H(); \
}
#define BMI088_accel_read_muli_reg(reg, data, len) \
{ \
BMI088_ACCEL_NS_L(); \
BMI088_read_write_byte((reg) | 0x80); \
BMI088_read_muli_reg(reg, data, len); \
BMI088_ACCEL_NS_H(); \
}
#define BMI088_gyro_write_single_reg(reg, data) \
{ \
BMI088_GYRO_NS_L(); \
BMI088_write_single_reg((reg), (data)); \
BMI088_GYRO_NS_H(); \
}
#define BMI088_gyro_read_single_reg(reg, data) \
{ \
BMI088_GYRO_NS_L(); \
BMI088_read_single_reg((reg), &(data)); \
BMI088_GYRO_NS_H(); \
}
#define BMI088_gyro_read_muli_reg(reg, data, len) \
{ \
BMI088_GYRO_NS_L(); \
BMI088_read_muli_reg((reg), (data), (len)); \
BMI088_GYRO_NS_H(); \
}
static void BMI088_write_single_reg(uint8_t reg, uint8_t data);
static void BMI088_read_single_reg(uint8_t reg, uint8_t *return_data);
static void BMI088_read_muli_reg(uint8_t reg, uint8_t *buf, uint8_t len);
#elif defined(BMI088_USE_IIC)
#endif
static uint8_t BMI088_Accel_Init_Table[BMI088_WRITE_ACCEL_REG_NUM][3] =
{
{BMI088_ACC_PWR_CTRL, BMI088_ACC_ENABLE_ACC_ON, BMI088_ACC_PWR_CTRL_ERROR},
{BMI088_ACC_PWR_CONF, BMI088_ACC_PWR_ACTIVE_MODE, BMI088_ACC_PWR_CONF_ERROR},
{BMI088_ACC_CONF, BMI088_ACC_NORMAL | BMI088_ACC_800_HZ | BMI088_ACC_CONF_MUST_Set, BMI088_ACC_CONF_ERROR},
{BMI088_ACC_RANGE, BMI088_ACC_RANGE_6G, BMI088_ACC_RANGE_ERROR},
{BMI088_INT1_IO_CTRL, BMI088_ACC_INT1_IO_ENABLE | BMI088_ACC_INT1_GPIO_PP | BMI088_ACC_INT1_GPIO_LOW, BMI088_INT1_IO_CTRL_ERROR},
{BMI088_INT_MAP_DATA, BMI088_ACC_INT1_DRDY_INTERRUPT, BMI088_INT_MAP_DATA_ERROR}
};
static uint8_t BMI088_Gyro_Init_Table[BMI088_WRITE_GYRO_REG_NUM][3] =
{
{BMI088_GYRO_RANGE, BMI088_GYRO_2000, BMI088_GYRO_RANGE_ERROR},
{BMI088_GYRO_BANDWIDTH, BMI088_GYRO_2000_230_HZ | BMI088_GYRO_BANDWIDTH_MUST_Set, BMI088_GYRO_BANDWIDTH_ERROR},
{BMI088_GYRO_LPM1, BMI088_GYRO_NORMAL_MODE, BMI088_GYRO_LPM1_ERROR},
{BMI088_GYRO_CTRL, BMI088_DRDY_ON, BMI088_GYRO_CTRL_ERROR},
{BMI088_GYRO_INT3_INT4_IO_CONF, BMI088_GYRO_INT3_GPIO_PP | BMI088_GYRO_INT3_GPIO_LOW, BMI088_GYRO_INT3_INT4_IO_CONF_ERROR},
{BMI088_GYRO_INT3_INT4_IO_MAP, BMI088_GYRO_DRDY_IO_INT3, BMI088_GYRO_INT3_INT4_IO_MAP_ERROR}
};
static void Calibrate_MPU_Offset(IMU_Data_t *bmi088);
uint8_t BMI088Init(SPI_HandleTypeDef *bmi088_SPI, uint8_t calibrate)
{
BMI088_SPI = bmi088_SPI;
error = BMI088_NO_ERROR;
error |= bmi088_accel_init();
error |= bmi088_gyro_init();
if (calibrate)
Calibrate_MPU_Offset(&BMI088);
else
{
BMI088.GyroOffset[0] = GxOFFSET;
BMI088.GyroOffset[1] = GyOFFSET;
BMI088.GyroOffset[2] = GzOFFSET;
BMI088.gNorm = gNORM;
BMI088.AccelScale = 9.81f / BMI088.gNorm;
BMI088.TempWhenCali = 40;
}
return error;
}
void Calibrate_MPU_Offset(IMU_Data_t *bmi088)
{
static float startTime;
static uint16_t CaliTimes = 6000;
uint8_t buf[8] = {0, 0, 0, 0, 0, 0};
int16_t bmi088_raw_temp;
float gyroMax[3], gyroMin[3];
float gNormTemp = 0.0f, gNormMax = 0.0f, gNormMin = 0.0f;
startTime = DWT_GetTimeline_s();
do
{
if (DWT_GetTimeline_s() - startTime > 12)
{
// ��????
bmi088->GyroOffset[0] = GxOFFSET;
bmi088->GyroOffset[1] = GyOFFSET;
bmi088->GyroOffset[2] = GzOFFSET;
bmi088->gNorm = gNORM;
bmi088->TempWhenCali = 40;
LOGERROR("[BMI088] Calibrate Failed! Use offline params");
break;
}
DWT_Delay(0.005);
bmi088->gNorm = 0;
bmi088->GyroOffset[0] = 0;
bmi088->GyroOffset[1] = 0;
bmi088->GyroOffset[2] = 0;
for (uint16_t i = 0; i < CaliTimes; ++i)
{
BMI088_accel_read_muli_reg(BMI088_ACCEL_XOUT_L, buf, 6);
bmi088_raw_temp = (int16_t)((buf[1]) << 8) | buf[0];
bmi088->Accel[0] = bmi088_raw_temp * BMI088_ACCEL_SEN;
bmi088_raw_temp = (int16_t)((buf[3]) << 8) | buf[2];
bmi088->Accel[1] = bmi088_raw_temp * BMI088_ACCEL_SEN;
bmi088_raw_temp = (int16_t)((buf[5]) << 8) | buf[4];
bmi088->Accel[2] = bmi088_raw_temp * BMI088_ACCEL_SEN;
gNormTemp = sqrtf(bmi088->Accel[0] * bmi088->Accel[0] +
bmi088->Accel[1] * bmi088->Accel[1] +
bmi088->Accel[2] * bmi088->Accel[2]);
bmi088->gNorm += gNormTemp;
BMI088_gyro_read_muli_reg(BMI088_GYRO_CHIP_ID, buf, 8);
if (buf[0] == BMI088_GYRO_CHIP_ID_VALUE)
{
bmi088_raw_temp = (int16_t)((buf[3]) << 8) | buf[2];
bmi088->Gyro[0] = bmi088_raw_temp * BMI088_GYRO_SEN;
bmi088->GyroOffset[0] += bmi088->Gyro[0];
bmi088_raw_temp = (int16_t)((buf[5]) << 8) | buf[4];
bmi088->Gyro[1] = bmi088_raw_temp * BMI088_GYRO_SEN;
bmi088->GyroOffset[1] += bmi088->Gyro[1];
bmi088_raw_temp = (int16_t)((buf[7]) << 8) | buf[6];
bmi088->Gyro[2] = bmi088_raw_temp * BMI088_GYRO_SEN;
bmi088->GyroOffset[2] += bmi088->Gyro[2];
}
if (i == 0)
{
gNormMax = gNormTemp;
gNormMin = gNormTemp;
for (uint8_t j = 0; j < 3; ++j)
{
gyroMax[j] = bmi088->Gyro[j];
gyroMin[j] = bmi088->Gyro[j];
}
}
else
{
if (gNormTemp > gNormMax)
gNormMax = gNormTemp;
if (gNormTemp < gNormMin)
gNormMin = gNormTemp;
for (uint8_t j = 0; j < 3; ++j)
{
if (bmi088->Gyro[j] > gyroMax[j])
gyroMax[j] = bmi088->Gyro[j];
if (bmi088->Gyro[j] < gyroMin[j])
gyroMin[j] = bmi088->Gyro[j];
}
}
gNormDiff = gNormMax - gNormMin;
for (uint8_t j = 0; j < 3; ++j)
gyroDiff[j] = gyroMax[j] - gyroMin[j];
if (gNormDiff > 0.5f ||
gyroDiff[0] > 0.15f ||
gyroDiff[1] > 0.15f ||
gyroDiff[2] > 0.15f)
{
LOGWARNING("[bmi088] calibration was interrupted\n");
break;
}
DWT_Delay(0.0005);
}
bmi088->gNorm /= (float)CaliTimes;
for (uint8_t i = 0; i < 3; ++i)
bmi088->GyroOffset[i] /= (float)CaliTimes;
BMI088_accel_read_muli_reg(BMI088_TEMP_M, buf, 2);
bmi088_raw_temp = (int16_t)((buf[0] << 3) | (buf[1] >> 5));
if (bmi088_raw_temp > 1023)
bmi088_raw_temp -= 2048;
bmi088->TempWhenCali = bmi088_raw_temp * BMI088_TEMP_FACTOR + BMI088_TEMP_OFFSET;
caliCount++;
} while (gNormDiff > 0.5f ||
fabsf(bmi088->gNorm - 9.8f) > 0.5f ||
gyroDiff[0] > 0.15f ||
gyroDiff[1] > 0.15f ||
gyroDiff[2] > 0.15f ||
fabsf(bmi088->GyroOffset[0]) > 0.01f ||
fabsf(bmi088->GyroOffset[1]) > 0.01f ||
fabsf(bmi088->GyroOffset[2]) > 0.01f);
bmi088->AccelScale = 9.81f / bmi088->gNorm;
}
uint8_t bmi088_accel_init(void)
{
// check commiunication
BMI088_accel_read_single_reg(BMI088_ACC_CHIP_ID, res);
DWT_Delay(0.001);
BMI088_accel_read_single_reg(BMI088_ACC_CHIP_ID, res);
DWT_Delay(0.001);
// accel software reset
BMI088_accel_write_single_reg(BMI088_ACC_SOFTRESET, BMI088_ACC_SOFTRESET_VALUE);
// HAL_Delay(BMI088_LONG_DELAY_TIME);
DWT_Delay(0.08);
// check commiunication is normal after reset
BMI088_accel_read_single_reg(BMI088_ACC_CHIP_ID, res);
DWT_Delay(0.001);
BMI088_accel_read_single_reg(BMI088_ACC_CHIP_ID, res);
DWT_Delay(0.001);
// check the "who am I"
if (res != BMI088_ACC_CHIP_ID_VALUE)
{
LOGERROR("[bmi088] Can not read bmi088 acc chip id");
return BMI088_NO_SENSOR;
}
// set accel sonsor config and check
for (write_reg_num = 0; write_reg_num < BMI088_WRITE_ACCEL_REG_NUM; write_reg_num++)
{
BMI088_accel_write_single_reg(BMI088_Accel_Init_Table[write_reg_num][0], BMI088_Accel_Init_Table[write_reg_num][1]);
DWT_Delay(0.001);
BMI088_accel_read_single_reg(BMI088_Accel_Init_Table[write_reg_num][0], res);
DWT_Delay(0.001);
if (res != BMI088_Accel_Init_Table[write_reg_num][1])
{
// write_reg_num--;
// return BMI088_Accel_Init_Table[write_reg_num][2];
error |= BMI088_Accel_Init_Table[write_reg_num][2];
}
}
return BMI088_NO_ERROR;
}
uint8_t bmi088_gyro_init(void)
{
// check commiunication
BMI088_gyro_read_single_reg(BMI088_GYRO_CHIP_ID, res);
DWT_Delay(0.001);
BMI088_gyro_read_single_reg(BMI088_GYRO_CHIP_ID, res);
DWT_Delay(0.001);
// reset the gyro sensor
BMI088_gyro_write_single_reg(BMI088_GYRO_SOFTRESET, BMI088_GYRO_SOFTRESET_VALUE);
// HAL_Delay(BMI088_LONG_DELAY_TIME);
DWT_Delay(0.08);
// check commiunication is normal after reset
BMI088_gyro_read_single_reg(BMI088_GYRO_CHIP_ID, res);
DWT_Delay(0.001);
BMI088_gyro_read_single_reg(BMI088_GYRO_CHIP_ID, res);
DWT_Delay(0.001);
// check the "who am I"
if (res != BMI088_GYRO_CHIP_ID_VALUE)
{
LOGERROR("[bmi088] Can not read bmi088 gyro chip id");
return BMI088_NO_SENSOR;
}
// set gyro sonsor config and check
for (write_reg_num = 0; write_reg_num < BMI088_WRITE_GYRO_REG_NUM; write_reg_num++)
{
BMI088_gyro_write_single_reg(BMI088_Gyro_Init_Table[write_reg_num][0], BMI088_Gyro_Init_Table[write_reg_num][1]);
DWT_Delay(0.001);
BMI088_gyro_read_single_reg(BMI088_Gyro_Init_Table[write_reg_num][0], res);
DWT_Delay(0.001);
if (res != BMI088_Gyro_Init_Table[write_reg_num][1])
{
write_reg_num--;
// return BMI088_Gyro_Init_Table[write_reg_num][2];
error |= BMI088_Accel_Init_Table[write_reg_num][2];
}
}
return BMI088_NO_ERROR;
}
void BMI088_Read(IMU_Data_t *bmi088)
{
static uint8_t buf[8] = {0};
static int16_t bmi088_raw_temp;
BMI088_accel_read_muli_reg(BMI088_ACCEL_XOUT_L, buf, 6);
bmi088_raw_temp = (int16_t)((buf[1]) << 8) | buf[0];
bmi088->Accel[0] = bmi088_raw_temp * BMI088_ACCEL_SEN * bmi088->AccelScale;
bmi088_raw_temp = (int16_t)((buf[3]) << 8) | buf[2];
bmi088->Accel[1] = bmi088_raw_temp * BMI088_ACCEL_SEN * bmi088->AccelScale;
bmi088_raw_temp = (int16_t)((buf[5]) << 8) | buf[4];
bmi088->Accel[2] = bmi088_raw_temp * BMI088_ACCEL_SEN * bmi088->AccelScale;
BMI088_gyro_read_muli_reg(BMI088_GYRO_CHIP_ID, buf, 8);
if (buf[0] == BMI088_GYRO_CHIP_ID_VALUE)
{
if (caliOffset)
{
bmi088_raw_temp = (int16_t)((buf[3]) << 8) | buf[2];
bmi088->Gyro[0] = bmi088_raw_temp * BMI088_GYRO_SEN - bmi088->GyroOffset[0];
bmi088_raw_temp = (int16_t)((buf[5]) << 8) | buf[4];
bmi088->Gyro[1] = bmi088_raw_temp * BMI088_GYRO_SEN - bmi088->GyroOffset[1];
bmi088_raw_temp = (int16_t)((buf[7]) << 8) | buf[6];
bmi088->Gyro[2] = bmi088_raw_temp * BMI088_GYRO_SEN - bmi088->GyroOffset[2];
}
else
{
bmi088_raw_temp = (int16_t)((buf[3]) << 8) | buf[2];
bmi088->Gyro[0] = bmi088_raw_temp * BMI088_GYRO_SEN;
bmi088_raw_temp = (int16_t)((buf[5]) << 8) | buf[4];
bmi088->Gyro[1] = bmi088_raw_temp * BMI088_GYRO_SEN;
bmi088_raw_temp = (int16_t)((buf[7]) << 8) | buf[6];
bmi088->Gyro[2] = bmi088_raw_temp * BMI088_GYRO_SEN;
}
}
BMI088_accel_read_muli_reg(BMI088_TEMP_M, buf, 2);
bmi088_raw_temp = (int16_t)((buf[0] << 3) | (buf[1] >> 5));
if (bmi088_raw_temp > 1023)
{
bmi088_raw_temp -= 2048;
}
bmi088->Temperature = bmi088_raw_temp * BMI088_TEMP_FACTOR + BMI088_TEMP_OFFSET;
}
#if defined(BMI088_USE_SPI)
static void BMI088_write_single_reg(uint8_t reg, uint8_t data)
{
BMI088_read_write_byte(reg);
BMI088_read_write_byte(data);
}
static void BMI088_read_single_reg(uint8_t reg, uint8_t *return_data)
{
BMI088_read_write_byte(reg | 0x80);
*return_data = BMI088_read_write_byte(0x55);
}
static void BMI088_read_muli_reg(uint8_t reg, uint8_t *buf, uint8_t len)
{
BMI088_read_write_byte(reg | 0x80);
while (len != 0)
{
*buf = BMI088_read_write_byte(0x55);
buf++;
len--;
}
}
#elif defined(BMI088_USE_IIC)
#endif
modules/imu/BMI088driver.h
+133
View File
@@ -0,0 +1,133 @@
#ifndef BMI088DRIVER_H
#define BMI088DRIVER_H
#include "stdint.h"
#include "main.h"
#define BMI088_TEMP_FACTOR 0.125f
#define BMI088_TEMP_OFFSET 23.0f
#define BMI088_WRITE_ACCEL_REG_NUM 6
#define BMI088_WRITE_GYRO_REG_NUM 6
#define BMI088_GYRO_DATA_READY_BIT 0
#define BMI088_ACCEL_DATA_READY_BIT 1
#define BMI088_ACCEL_TEMP_DATA_READY_BIT 2
#define BMI088_LONG_DELAY_TIME 80
#define BMI088_COM_WAIT_SENSOR_TIME 150
#define BMI088_ACCEL_IIC_ADDRESSE (0x18 << 1)
#define BMI088_GYRO_IIC_ADDRESSE (0x68 << 1)
#define BMI088_ACCEL_3G_SEN 0.0008974358974f
#define BMI088_ACCEL_6G_SEN 0.00179443359375f
#define BMI088_ACCEL_12G_SEN 0.0035888671875f
#define BMI088_ACCEL_24G_SEN 0.007177734375f
#define BMI088_GYRO_2000_SEN 0.00106526443603169529841533860381f
#define BMI088_GYRO_1000_SEN 0.00053263221801584764920766930190693f
#define BMI088_GYRO_500_SEN 0.00026631610900792382460383465095346f
#define BMI088_GYRO_250_SEN 0.00013315805450396191230191732547673f
#define BMI088_GYRO_125_SEN 0.000066579027251980956150958662738366f
// ���ֶ��޸�
#if INFANTRY_ID == 0
#define GxOFFSET 0.00247530174f
#define GyOFFSET 0.000393082853f
#define GzOFFSET 0.000393082853f
#define gNORM 9.69293118f
#elif INFANTRY_ID == 1
#define GxOFFSET 0.0007222f
#define GyOFFSET -0.001786f
#define GzOFFSET 0.0004346f
#define gNORM 9.876785f
#elif INFANTRY_ID == 2
#define GxOFFSET 0.0007222f
#define GyOFFSET -0.001786f
#define GzOFFSET 0.0004346f
#define gNORM 9.876785f
#elif INFANTRY_ID == 3
#define GxOFFSET 0.00270364084f
#define GyOFFSET -0.000532632112f
#define GzOFFSET 0.00478090625f
#define gNORM 9.73574924f
#elif INFANTRY_ID == 4
#define GxOFFSET 0.0007222f
#define GyOFFSET -0.001786f
#define GzOFFSET 0.0004346f
#define gNORM 9.876785f
#endif
/* IMU数据结构体 */
typedef struct
{
float Accel[3];
float Gyro[3];
float TempWhenCali;
float Temperature;
float AccelScale;
float GyroOffset[3];
float gNorm;
} IMU_Data_t;
/* BMI088错误码枚举 */
enum
{
BMI088_NO_ERROR = 0x00,
BMI088_ACC_PWR_CTRL_ERROR = 0x01,
BMI088_ACC_PWR_CONF_ERROR = 0x02,
BMI088_ACC_CONF_ERROR = 0x03,
BMI088_ACC_SELF_TEST_ERROR = 0x04,
BMI088_ACC_RANGE_ERROR = 0x05,
BMI088_INT1_IO_CTRL_ERROR = 0x06,
BMI088_INT_MAP_DATA_ERROR = 0x07,
BMI088_GYRO_RANGE_ERROR = 0x08,
BMI088_GYRO_BANDWIDTH_ERROR = 0x09,
BMI088_GYRO_LPM1_ERROR = 0x0A,
BMI088_GYRO_CTRL_ERROR = 0x0B,
BMI088_GYRO_INT3_INT4_IO_CONF_ERROR = 0x0C,
BMI088_GYRO_INT3_INT4_IO_MAP_ERROR = 0x0D,
BMI088_SELF_TEST_ACCEL_ERROR = 0x80,
BMI088_SELF_TEST_GYRO_ERROR = 0x40,
BMI088_NO_SENSOR = 0xFF,
};
extern IMU_Data_t BMI088;
/**
* @brief 初始化BMI088,传入连接的SPI总线handle,以及是否进行在线标定
*
* @param bmi088_SPI handle
* @param calibrate 1为进行在线标定,0使用离线数据
* @return uint8_t 成功则返回BMI088_NO_ERROR
*/
extern uint8_t BMI088Init(SPI_HandleTypeDef *bmi088_SPI, uint8_t calibrate);
/**
* @brief 加速计初始化
*
* @return uint8_t
*/
extern uint8_t bmi088_accel_init(void);
/**
* @brief 陀螺仪初始化
*
* @return uint8_t
*/
extern uint8_t bmi088_gyro_init(void);
/**
* @brief 读取一次BMI088的数据,包括gyro和accel
*
* @param bmi088 传入BMI088实例(结构体)
*/
extern void BMI088_Read(IMU_Data_t *bmi088);
#endif
modules/imu/BMI088reg.h
+180
View File
@@ -0,0 +1,180 @@
#ifndef BMI088REG_H
#define BMI088REG_H
#define BMI088_ACC_CHIP_ID 0x00 // the register is " Who am I "
#define BMI088_ACC_CHIP_ID_VALUE 0x1E
#define BMI088_ACC_ERR_REG 0x02
#define BMI088_ACCEL_CONGIF_ERROR_SHFITS 0x2
#define BMI088_ACCEL_CONGIF_ERROR (1 << BMI088_ACCEL_CONGIF_ERROR_SHFITS)
#define BMI088_FATAL_ERROR_SHFITS 0x0
#define BMI088_FATAL_ERROR (1 << BMI088_FATAL_ERROR)
#define BMI088_ACC_STATUS 0x03
#define BMI088_ACCEL_DRDY_SHFITS 0x7
#define BMI088_ACCEL_DRDY (1 << BMI088_ACCEL_DRDY_SHFITS)
#define BMI088_ACCEL_XOUT_L 0x12
#define BMI088_ACCEL_XOUT_M 0x13
#define BMI088_ACCEL_YOUT_L 0x14
#define BMI088_ACCEL_YOUT_M 0x15
#define BMI088_ACCEL_ZOUT_L 0x16
#define BMI088_ACCEL_ZOUT_M 0x17
#define BMI088_SENSORTIME_DATA_L 0x18
#define BMI088_SENSORTIME_DATA_M 0x19
#define BMI088_SENSORTIME_DATA_H 0x1A
#define BMI088_ACC_INT_STAT_1 0x1D
#define BMI088_ACCEL_DRDY_INTERRUPT_SHFITS 0x7
#define BMI088_ACCEL_DRDY_INTERRUPT (1 << BMI088_ACCEL_DRDY_INTERRUPT_SHFITS)
#define BMI088_TEMP_M 0x22
#define BMI088_TEMP_L 0x23
#define BMI088_ACC_CONF 0x40
#define BMI088_ACC_CONF_MUST_Set 0x80
#define BMI088_ACC_BWP_SHFITS 0x4
#define BMI088_ACC_OSR4 (0x0 << BMI088_ACC_BWP_SHFITS)
#define BMI088_ACC_OSR2 (0x1 << BMI088_ACC_BWP_SHFITS)
#define BMI088_ACC_NORMAL (0x2 << BMI088_ACC_BWP_SHFITS)
#define BMI088_ACC_ODR_SHFITS 0x0
#define BMI088_ACC_12_5_HZ (0x5 << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_25_HZ (0x6 << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_50_HZ (0x7 << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_100_HZ (0x8 << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_200_HZ (0x9 << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_400_HZ (0xA << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_800_HZ (0xB << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_1600_HZ (0xC << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_RANGE 0x41
#define BMI088_ACC_RANGE_SHFITS 0x0
#define BMI088_ACC_RANGE_3G (0x0 << BMI088_ACC_RANGE_SHFITS)
#define BMI088_ACC_RANGE_6G (0x1 << BMI088_ACC_RANGE_SHFITS)
#define BMI088_ACC_RANGE_12G (0x2 << BMI088_ACC_RANGE_SHFITS)
#define BMI088_ACC_RANGE_24G (0x3 << BMI088_ACC_RANGE_SHFITS)
#define BMI088_INT1_IO_CTRL 0x53
#define BMI088_ACC_INT1_IO_ENABLE_SHFITS 0x3
#define BMI088_ACC_INT1_IO_ENABLE (0x1 << BMI088_ACC_INT1_IO_ENABLE_SHFITS)
#define BMI088_ACC_INT1_GPIO_MODE_SHFITS 0x2
#define BMI088_ACC_INT1_GPIO_PP (0x0 << BMI088_ACC_INT1_GPIO_MODE_SHFITS)
#define BMI088_ACC_INT1_GPIO_OD (0x1 << BMI088_ACC_INT1_GPIO_MODE_SHFITS)
#define BMI088_ACC_INT1_GPIO_LVL_SHFITS 0x1
#define BMI088_ACC_INT1_GPIO_LOW (0x0 << BMI088_ACC_INT1_GPIO_LVL_SHFITS)
#define BMI088_ACC_INT1_GPIO_HIGH (0x1 << BMI088_ACC_INT1_GPIO_LVL_SHFITS)
#define BMI088_INT2_IO_CTRL 0x54
#define BMI088_ACC_INT2_IO_ENABLE_SHFITS 0x3
#define BMI088_ACC_INT2_IO_ENABLE (0x1 << BMI088_ACC_INT2_IO_ENABLE_SHFITS)
#define BMI088_ACC_INT2_GPIO_MODE_SHFITS 0x2
#define BMI088_ACC_INT2_GPIO_PP (0x0 << BMI088_ACC_INT2_GPIO_MODE_SHFITS)
#define BMI088_ACC_INT2_GPIO_OD (0x1 << BMI088_ACC_INT2_GPIO_MODE_SHFITS)
#define BMI088_ACC_INT2_GPIO_LVL_SHFITS 0x1
#define BMI088_ACC_INT2_GPIO_LOW (0x0 << BMI088_ACC_INT2_GPIO_LVL_SHFITS)
#define BMI088_ACC_INT2_GPIO_HIGH (0x1 << BMI088_ACC_INT2_GPIO_LVL_SHFITS)
#define BMI088_INT_MAP_DATA 0x58
#define BMI088_ACC_INT2_DRDY_INTERRUPT_SHFITS 0x6
#define BMI088_ACC_INT2_DRDY_INTERRUPT (0x1 << BMI088_ACC_INT2_DRDY_INTERRUPT_SHFITS)
#define BMI088_ACC_INT1_DRDY_INTERRUPT_SHFITS 0x2
#define BMI088_ACC_INT1_DRDY_INTERRUPT (0x1 << BMI088_ACC_INT1_DRDY_INTERRUPT_SHFITS)
#define BMI088_ACC_SELF_TEST 0x6D
#define BMI088_ACC_SELF_TEST_OFF 0x00
#define BMI088_ACC_SELF_TEST_POSITIVE_SIGNAL 0x0D
#define BMI088_ACC_SELF_TEST_NEGATIVE_SIGNAL 0x09
#define BMI088_ACC_PWR_CONF 0x7C
#define BMI088_ACC_PWR_SUSPEND_MODE 0x03
#define BMI088_ACC_PWR_ACTIVE_MODE 0x00
#define BMI088_ACC_PWR_CTRL 0x7D
#define BMI088_ACC_ENABLE_ACC_OFF 0x00
#define BMI088_ACC_ENABLE_ACC_ON 0x04
#define BMI088_ACC_SOFTRESET 0x7E
#define BMI088_ACC_SOFTRESET_VALUE 0xB6
#define BMI088_GYRO_CHIP_ID 0x00
#define BMI088_GYRO_CHIP_ID_VALUE 0x0F
#define BMI088_GYRO_X_L 0x02
#define BMI088_GYRO_X_H 0x03
#define BMI088_GYRO_Y_L 0x04
#define BMI088_GYRO_Y_H 0x05
#define BMI088_GYRO_Z_L 0x06
#define BMI088_GYRO_Z_H 0x07
#define BMI088_GYRO_INT_STAT_1 0x0A
#define BMI088_GYRO_DYDR_SHFITS 0x7
#define BMI088_GYRO_DYDR (0x1 << BMI088_GYRO_DYDR_SHFITS)
#define BMI088_GYRO_RANGE 0x0F
#define BMI088_GYRO_RANGE_SHFITS 0x0
#define BMI088_GYRO_2000 (0x0 << BMI088_GYRO_RANGE_SHFITS)
#define BMI088_GYRO_1000 (0x1 << BMI088_GYRO_RANGE_SHFITS)
#define BMI088_GYRO_500 (0x2 << BMI088_GYRO_RANGE_SHFITS)
#define BMI088_GYRO_250 (0x3 << BMI088_GYRO_RANGE_SHFITS)
#define BMI088_GYRO_125 (0x4 << BMI088_GYRO_RANGE_SHFITS)
#define BMI088_GYRO_BANDWIDTH 0x10
// the first num means Output data rate, the second num means bandwidth
#define BMI088_GYRO_BANDWIDTH_MUST_Set 0x80
#define BMI088_GYRO_2000_532_HZ 0x00
#define BMI088_GYRO_2000_230_HZ 0x01
#define BMI088_GYRO_1000_116_HZ 0x02
#define BMI088_GYRO_400_47_HZ 0x03
#define BMI088_GYRO_200_23_HZ 0x04
#define BMI088_GYRO_100_12_HZ 0x05
#define BMI088_GYRO_200_64_HZ 0x06
#define BMI088_GYRO_100_32_HZ 0x07
#define BMI088_GYRO_LPM1 0x11
#define BMI088_GYRO_NORMAL_MODE 0x00
#define BMI088_GYRO_SUSPEND_MODE 0x80
#define BMI088_GYRO_DEEP_SUSPEND_MODE 0x20
#define BMI088_GYRO_SOFTRESET 0x14
#define BMI088_GYRO_SOFTRESET_VALUE 0xB6
#define BMI088_GYRO_CTRL 0x15
#define BMI088_DRDY_OFF 0x00
#define BMI088_DRDY_ON 0x80
#define BMI088_GYRO_INT3_INT4_IO_CONF 0x16
#define BMI088_GYRO_INT4_GPIO_MODE_SHFITS 0x3
#define BMI088_GYRO_INT4_GPIO_PP (0x0 << BMI088_GYRO_INT4_GPIO_MODE_SHFITS)
#define BMI088_GYRO_INT4_GPIO_OD (0x1 << BMI088_GYRO_INT4_GPIO_MODE_SHFITS)
#define BMI088_GYRO_INT4_GPIO_LVL_SHFITS 0x2
#define BMI088_GYRO_INT4_GPIO_LOW (0x0 << BMI088_GYRO_INT4_GPIO_LVL_SHFITS)
#define BMI088_GYRO_INT4_GPIO_HIGH (0x1 << BMI088_GYRO_INT4_GPIO_LVL_SHFITS)
#define BMI088_GYRO_INT3_GPIO_MODE_SHFITS 0x1
#define BMI088_GYRO_INT3_GPIO_PP (0x0 << BMI088_GYRO_INT3_GPIO_MODE_SHFITS)
#define BMI088_GYRO_INT3_GPIO_OD (0x1 << BMI088_GYRO_INT3_GPIO_MODE_SHFITS)
#define BMI088_GYRO_INT3_GPIO_LVL_SHFITS 0x0
#define BMI088_GYRO_INT3_GPIO_LOW (0x0 << BMI088_GYRO_INT3_GPIO_LVL_SHFITS)
#define BMI088_GYRO_INT3_GPIO_HIGH (0x1 << BMI088_GYRO_INT3_GPIO_LVL_SHFITS)
#define BMI088_GYRO_INT3_INT4_IO_MAP 0x18
#define BMI088_GYRO_DRDY_IO_OFF 0x00
#define BMI088_GYRO_DRDY_IO_INT3 0x01
#define BMI088_GYRO_DRDY_IO_INT4 0x80
#define BMI088_GYRO_DRDY_IO_BOTH (BMI088_GYRO_DRDY_IO_INT3 | BMI088_GYRO_DRDY_IO_INT4)
#define BMI088_GYRO_SELF_TEST 0x3C
#define BMI088_GYRO_RATE_OK_SHFITS 0x4
#define BMI088_GYRO_RATE_OK (0x1 << BMI088_GYRO_RATE_OK_SHFITS)
#define BMI088_GYRO_BIST_FAIL_SHFITS 0x2
#define BMI088_GYRO_BIST_FAIL (0x1 << BMI088_GYRO_BIST_FAIL_SHFITS)
#define BMI088_GYRO_BIST_RDY_SHFITS 0x1
#define BMI088_GYRO_BIST_RDY (0x1 << BMI088_GYRO_BIST_RDY_SHFITS)
#define BMI088_GYRO_TRIG_BIST_SHFITS 0x0
#define BMI088_GYRO_TRIG_BIST (0x1 << BMI088_GYRO_TRIG_BIST_SHFITS)
#endif
modules/imu/ins_task.c
+351
View File
@@ -0,0 +1,351 @@
/**
******************************************************************************
* @file ins_task.c
* @author Wang Hongxi
* @author annotation and modificaiton by neozng
* @version V2.0.0
* @date 2022/2/23
* @brief
******************************************************************************
* @attention
*
******************************************************************************
*/
#include "ins_task.h"
#include "controller.h"
#include "QuaternionEKF.h"
#include "spi.h"
#include "tim.h"
#include "user_lib.h"
#include "general_def.h"
#include "master_process.h"
#include "arm_math.h"
static INS_t INS;
static IMU_Param_t IMU_Param;
static PIDInstance TempCtrl = {0};
const float xb[3] = {1, 0, 0};
const float yb[3] = {0, 1, 0};
const float zb[3] = {0, 0, 1};
// 用于获取两次采样之间的时间间隔
static uint32_t INS_DWT_Count = 0;
static float dt = 0, t = 0;
static float RefTemp = 40; // 恒温设定温度
static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3]);
static void IMUPWMSet(uint16_t pwm)
{
__HAL_TIM_SetCompare(&htim3, TIM_CHANNEL_4, pwm);
}
/**
* @brief 温度控制
*
*/
static void IMU_Temperature_Ctrl(void)
{
PIDCalculate(&TempCtrl, BMI088.Temperature, RefTemp);
IMUPWMSet(float_constrain(float_rounding(TempCtrl.Output), 0, UINT32_MAX));
}
// 使用加速度计的数据初始化Roll和Pitch,而Yaw置0,这样可以避免在初始时候的姿态估计误差
static void InitQuaternion(float *init_q4)
{
float acc_init[3] = {0};
float gravity_norm[3] = {0, 0, 1}; // 导航系重力加速度矢量,归一化后为(0,0,1)
float axis_rot[3] = {0}; // 旋转轴
// 读取100次加速度计数据,取平均值作为初始值
for (uint8_t i = 0; i < 100; ++i)
{
BMI088_Read(&BMI088);
acc_init[X] += BMI088.Accel[X];
acc_init[Y] += BMI088.Accel[Y];
acc_init[Z] += BMI088.Accel[Z];
DWT_Delay(0.001);
}
for (uint8_t i = 0; i < 3; ++i)
acc_init[i] /= 100;
Norm3d(acc_init);
// 计算原始加速度矢量和导航系重力加速度矢量的夹角
float angle = acosf(Dot3d(acc_init, gravity_norm));
Cross3d(acc_init, gravity_norm, axis_rot);
Norm3d(axis_rot);
init_q4[0] = cosf(angle / 2.0f);
for (uint8_t i = 0; i < 2; ++i)
init_q4[i + 1] = axis_rot[i] * sinf(angle / 2.0f); // 轴角公式,第三轴为0(没有z轴分量)
}
attitude_t *INS_Init(void)
{
if (!INS.init)
INS.init = 1;
else
return (attitude_t *)&INS.Gyro;
HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_4);
while (BMI088Init(&hspi2, 1) != BMI088_NO_ERROR)
;
IMU_Param.scale[X] = 1;
IMU_Param.scale[Y] = 1;
IMU_Param.scale[Z] = 1;
IMU_Param.Yaw = 0;
IMU_Param.Pitch = 0;
IMU_Param.Roll = 0;
IMU_Param.flag = 1;
float init_quaternion[4] = {0};
InitQuaternion(init_quaternion);
IMU_QuaternionEKF_Init(init_quaternion, 10, 0.001, 1000000, 1, 0);
// imu heat init
PID_Init_Config_s config = {.MaxOut = 800,
.IntegralLimit = 80,
.DeadBand = 0,
.Kp = 400,
.Ki = 5,
.Kd = 0,
.Improve = 0x01}; // enable integratiaon limit
PIDInit(&TempCtrl, &config);
// noise of accel is relatively big and of high freq,thus lpf is used
INS.AccelLPF = 0.0085;
DWT_GetDeltaT(&INS_DWT_Count);
return (attitude_t *)&INS.Gyro; // @todo: 这里偷懒了,不要这样做! 修改INT_t结构体可能会导致异常,待修复.
}
/* 注意以1kHz的频率运行此任务 */
void INS_Task(void)
{
static uint32_t count = 0;
const float gravity[3] = {0, 0, 9.81f};
dt = DWT_GetDeltaT(&INS_DWT_Count);
t += dt;
// ins update
if ((count % 1) == 0)
{
BMI088_Read(&BMI088);
INS.Accel[X] = BMI088.Accel[X];
INS.Accel[Y] = BMI088.Accel[Y];
INS.Accel[Z] = BMI088.Accel[Z];
INS.Gyro[X] = BMI088.Gyro[X];
INS.Gyro[Y] = BMI088.Gyro[Y];
INS.Gyro[Z] = BMI088.Gyro[Z];
// demo function,用于修正安装误差,可以不管,本demo暂时没用
IMU_Param_Correction(&IMU_Param, INS.Gyro, INS.Accel);
// 计算重力加速度矢量和b系的XY两轴的夹角,可用作功能扩展,本demo暂时没用
// INS.atanxz = -atan2f(INS.Accel[X], INS.Accel[Z]) * 180 / PI;
// INS.atanyz = atan2f(INS.Accel[Y], INS.Accel[Z]) * 180 / PI;
// 核心函数,EKF更新四元数
IMU_QuaternionEKF_Update(INS.Gyro[X], INS.Gyro[Y], INS.Gyro[Z], INS.Accel[X], INS.Accel[Y], INS.Accel[Z], dt);
memcpy(INS.q, QEKF_INS.q, sizeof(QEKF_INS.q));
// 机体系基向量转换到导航坐标系，本例选取惯性系为导航系
BodyFrameToEarthFrame(xb, INS.xn, INS.q);
BodyFrameToEarthFrame(yb, INS.yn, INS.q);
BodyFrameToEarthFrame(zb, INS.zn, INS.q);
// 将重力从导航坐标系n转换到机体系b,随后根据加速度计数据计算运动加速度
float gravity_b[3];
EarthFrameToBodyFrame(gravity, gravity_b, INS.q);
for (uint8_t i = 0; i < 3; ++i) // 同样过一个低通滤波
{
INS.MotionAccel_b[i] = (INS.Accel[i] - gravity_b[i]) * dt / (INS.AccelLPF + dt) + INS.MotionAccel_b[i] * INS.AccelLPF / (INS.AccelLPF + dt);
}
BodyFrameToEarthFrame(INS.MotionAccel_b, INS.MotionAccel_n, INS.q); // 转换回导航系n
INS.Yaw = QEKF_INS.Yaw;
INS.Pitch = QEKF_INS.Pitch;
INS.Roll = QEKF_INS.Roll;
INS.YawTotalAngle = QEKF_INS.YawTotalAngle;
VisionSetAltitude(INS.Yaw, INS.Pitch, INS.Roll);
}
// temperature control
if ((count % 2) == 0)
{
// 500hz
IMU_Temperature_Ctrl();
}
if ((count++ % 1000) == 0)
{
// 1Hz 可以加入monitor函数,检查IMU是否正常运行/离线
}
}
/**
* @brief Transform 3dvector from BodyFrame to EarthFrame
* @param[1] vector in BodyFrame
* @param[2] vector in EarthFrame
* @param[3] quaternion
*/
void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q)
{
vecEF[0] = 2.0f * ((0.5f - q[2] * q[2] - q[3] * q[3]) * vecBF[0] +
(q[1] * q[2] - q[0] * q[3]) * vecBF[1] +
(q[1] * q[3] + q[0] * q[2]) * vecBF[2]);
vecEF[1] = 2.0f * ((q[1] * q[2] + q[0] * q[3]) * vecBF[0] +
(0.5f - q[1] * q[1] - q[3] * q[3]) * vecBF[1] +
(q[2] * q[3] - q[0] * q[1]) * vecBF[2]);
vecEF[2] = 2.0f * ((q[1] * q[3] - q[0] * q[2]) * vecBF[0] +
(q[2] * q[3] + q[0] * q[1]) * vecBF[1] +
(0.5f - q[1] * q[1] - q[2] * q[2]) * vecBF[2]);
}
/**
* @brief Transform 3dvector from EarthFrame to BodyFrame
* @param[1] vector in EarthFrame
* @param[2] vector in BodyFrame
* @param[3] quaternion
*/
void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q)
{
vecBF[0] = 2.0f * ((0.5f - q[2] * q[2] - q[3] * q[3]) * vecEF[0] +
(q[1] * q[2] + q[0] * q[3]) * vecEF[1] +
(q[1] * q[3] - q[0] * q[2]) * vecEF[2]);
vecBF[1] = 2.0f * ((q[1] * q[2] - q[0] * q[3]) * vecEF[0] +
(0.5f - q[1] * q[1] - q[3] * q[3]) * vecEF[1] +
(q[2] * q[3] + q[0] * q[1]) * vecEF[2]);
vecBF[2] = 2.0f * ((q[1] * q[3] + q[0] * q[2]) * vecEF[0] +
(q[2] * q[3] - q[0] * q[1]) * vecEF[1] +
(0.5f - q[1] * q[1] - q[2] * q[2]) * vecEF[2]);
}
/**
* @brief reserved.用于修正IMU安装误差与标度因数误差,即陀螺仪轴和云台轴的安装偏移
*
*
* @param param IMU参数
* @param gyro 角速度
* @param accel 加速度
*/
static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3])
{
static float lastYawOffset, lastPitchOffset, lastRollOffset;
static float c_11, c_12, c_13, c_21, c_22, c_23, c_31, c_32, c_33;
float cosPitch, cosYaw, cosRoll, sinPitch, sinYaw, sinRoll;
if (fabsf(param->Yaw - lastYawOffset) > 0.001f ||
fabsf(param->Pitch - lastPitchOffset) > 0.001f ||
fabsf(param->Roll - lastRollOffset) > 0.001f || param->flag)
{
cosYaw = arm_cos_f32(param->Yaw / 57.295779513f);
cosPitch = arm_cos_f32(param->Pitch / 57.295779513f);
cosRoll = arm_cos_f32(param->Roll / 57.295779513f);
sinYaw = arm_sin_f32(param->Yaw / 57.295779513f);
sinPitch = arm_sin_f32(param->Pitch / 57.295779513f);
sinRoll = arm_sin_f32(param->Roll / 57.295779513f);
// 1.yaw(alpha) 2.pitch(beta) 3.roll(gamma)
c_11 = cosYaw * cosRoll + sinYaw * sinPitch * sinRoll;
c_12 = cosPitch * sinYaw;
c_13 = cosYaw * sinRoll - cosRoll * sinYaw * sinPitch;
c_21 = cosYaw * sinPitch * sinRoll - cosRoll * sinYaw;
c_22 = cosYaw * cosPitch;
c_23 = -sinYaw * sinRoll - cosYaw * cosRoll * sinPitch;
c_31 = -cosPitch * sinRoll;
c_32 = sinPitch;
c_33 = cosPitch * cosRoll;
param->flag = 0;
}
float gyro_temp[3];
for (uint8_t i = 0; i < 3; ++i)
gyro_temp[i] = gyro[i] * param->scale[i];
gyro[X] = c_11 * gyro_temp[X] +
c_12 * gyro_temp[Y] +
c_13 * gyro_temp[Z];
gyro[Y] = c_21 * gyro_temp[X] +
c_22 * gyro_temp[Y] +
c_23 * gyro_temp[Z];
gyro[Z] = c_31 * gyro_temp[X] +
c_32 * gyro_temp[Y] +
c_33 * gyro_temp[Z];
float accel_temp[3];
for (uint8_t i = 0; i < 3; ++i)
accel_temp[i] = accel[i];
accel[X] = c_11 * accel_temp[X] +
c_12 * accel_temp[Y] +
c_13 * accel_temp[Z];
accel[Y] = c_21 * accel_temp[X] +
c_22 * accel_temp[Y] +
c_23 * accel_temp[Z];
accel[Z] = c_31 * accel_temp[X] +
c_32 * accel_temp[Y] +
c_33 * accel_temp[Z];
lastYawOffset = param->Yaw;
lastPitchOffset = param->Pitch;
lastRollOffset = param->Roll;
}
//------------------------------------functions below are not used in this demo-------------------------------------------------
//----------------------------------you can read them for learning or programming-----------------------------------------------
//----------------------------------they could also be helpful for further design-----------------------------------------------
/**
* @brief Update quaternion
*/
void QuaternionUpdate(float *q, float gx, float gy, float gz, float dt)
{
float qa, qb, qc;
gx *= 0.5f * dt;
gy *= 0.5f * dt;
gz *= 0.5f * dt;
qa = q[0];
qb = q[1];
qc = q[2];
q[0] += (-qb * gx - qc * gy - q[3] * gz);
q[1] += (qa * gx + qc * gz - q[3] * gy);
q[2] += (qa * gy - qb * gz + q[3] * gx);
q[3] += (qa * gz + qb * gy - qc * gx);
}
/**
* @brief Convert quaternion to eular angle
*/
void QuaternionToEularAngle(float *q, float *Yaw, float *Pitch, float *Roll)
{
*Yaw = atan2f(2.0f * (q[0] * q[3] + q[1] * q[2]), 2.0f * (q[0] * q[0] + q[1] * q[1]) - 1.0f) * 57.295779513f;
*Pitch = atan2f(2.0f * (q[0] * q[1] + q[2] * q[3]), 2.0f * (q[0] * q[0] + q[3] * q[3]) - 1.0f) * 57.295779513f;
*Roll = asinf(2.0f * (q[0] * q[2] - q[1] * q[3])) * 57.295779513f;
}
/**
* @brief Convert eular angle to quaternion
*/
void EularAngleToQuaternion(float Yaw, float Pitch, float Roll, float *q)
{
float cosPitch, cosYaw, cosRoll, sinPitch, sinYaw, sinRoll;
Yaw /= 57.295779513f;
Pitch /= 57.295779513f;
Roll /= 57.295779513f;
cosPitch = arm_cos_f32(Pitch / 2);
cosYaw = arm_cos_f32(Yaw / 2);
cosRoll = arm_cos_f32(Roll / 2);
sinPitch = arm_sin_f32(Pitch / 2);
sinYaw = arm_sin_f32(Yaw / 2);
sinRoll = arm_sin_f32(Roll / 2);
q[0] = cosPitch * cosRoll * cosYaw + sinPitch * sinRoll * sinYaw;
q[1] = sinPitch * cosRoll * cosYaw - cosPitch * sinRoll * sinYaw;
q[2] = sinPitch * cosRoll * sinYaw + cosPitch * sinRoll * cosYaw;
q[3] = cosPitch * cosRoll * sinYaw - sinPitch * sinRoll * cosYaw;
}
modules/imu/ins_task.h
+143
View File
@@ -0,0 +1,143 @@
/**
******************************************************************************
* @file ins_task.h
* @author Wang Hongxi
* @author annotation and modification by NeoZeng
* @version V2.0.0
* @date 2022/2/23
* @brief
******************************************************************************
* @attention INS任务的初始化不要放入实时系统!应该由application拥有实例,随后在
* 应用层调用初始化函数.
*
******************************************************************************
*/
#ifndef __INS_TASK_H
#define __INS_TASK_H
#include "stdint.h"
#include "BMI088driver.h"
#include "QuaternionEKF.h"
#define X 0
#define Y 1
#define Z 2
#define INS_TASK_PERIOD 1
typedef struct
{
float Gyro[3]; // 角速度
float Accel[3]; // 加速度
// 还需要增加角速度数据
float Roll;
float Pitch;
float Yaw;
float YawTotalAngle;
} attitude_t; // 最终解算得到的角度,以及yaw转动的总角度(方便多圈控制)
typedef struct
{
float q[4]; // 四元数估计值
float MotionAccel_b[3]; // 机体坐标加速度
float MotionAccel_n[3]; // 绝对系加速度
float AccelLPF; // 加速度低通滤波系数
// bodyframe在绝对系的向量表示
float xn[3];
float yn[3];
float zn[3];
// 加速度在机体系和XY两轴的夹角
// float atanxz;
// float atanyz;
// IMU量测值
float Gyro[3]; // 角速度
float Accel[3]; // 加速度
// 位姿
float Roll;
float Pitch;
float Yaw;
float YawTotalAngle;
uint8_t init;
} INS_t;
/* 用于修正安装误差的参数 */
typedef struct
{
uint8_t flag;
float scale[3];
float Yaw;
float Pitch;
float Roll;
} IMU_Param_t;
/**
* @brief 初始化惯导解算系统
*
*/
attitude_t *INS_Init(void);
/**
* @brief 此函数放入实时系统中,以1kHz频率运行
* p.s. osDelay(1);
*
*/
void INS_Task(void);
/**
* @brief 四元数更新函数,即实现dq/dt=0.5Ωq
*
* @param q 四元数
* @param gx
* @param gy
* @param gz
* @param dt 距离上次调用的时间间隔
*/
void QuaternionUpdate(float *q, float gx, float gy, float gz, float dt);
/**
* @brief 四元数转换成欧拉角 ZYX
*
* @param q
* @param Yaw
* @param Pitch
* @param Roll
*/
void QuaternionToEularAngle(float *q, float *Yaw, float *Pitch, float *Roll);
/**
* @brief ZYX欧拉角转换为四元数
*
* @param Yaw
* @param Pitch
* @param Roll
* @param q
*/
void EularAngleToQuaternion(float Yaw, float Pitch, float Roll, float *q);
/**
* @brief 机体系到惯性系的变换函数
*
* @param vecBF body frame
* @param vecEF earth frame
* @param q
*/
void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q);
/**
* @brief 惯性系转换到机体系
*
* @param vecEF
* @param vecBF
* @param q
*/
void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q);
#endif
modules/imu/ins_task.md
+18
View File
@@ -0,0 +1,18 @@
# ins_task
<p align='right'>neozng1@hnu.edu.cn</p>
## 硬触发流程
![image-20221113212706633](.assets\image-20221113212706633.png)
`times%10` 是固定相机的采集频率为100hz，请根据视觉算法实际能达到的最大帧率调整。
## 算法解析
介绍EKF四元数姿态解算的教程在:[四元数EKF姿态更新算法](https://zhuanlan.zhihu.com/p/454155643)
## 模块移植
由于历史遗留问题,IMU模块耦合程度高.后续实现BSP_SPI,将bmi088 middleware删除.仅保留BMI088读取的协议和寄存器定义等,单独实现IMU模块.
> 移植已经完成,请转而使用module/BMI088的模块. 当前文件夹将在beta1.2停止支持, 1.5之后删除. INS_Task届时会被放到algorithm中,以提供对不同IMU的兼容.
modules/master_machine/master_process.c
+157
View File
@@ -0,0 +1,157 @@
/**
* @file master_process.c
* @author neozng
* @brief module for recv&send vision data
* @version beta
* @date 2022-11-03
* @todo 增加对串口调试助手协议的支持,包括vofa和serial debug
* @copyright Copyright (c) 2022
*
*/
#include "master_process.h"
#include "seasky_protocol.h"
#include "daemon.h"
#include "bsp_log.h"
#include "robot_def.h"
static USARTInstance *vision_usart_instance;
static Vision_Recv_s recv_data;
static Vision_Send_s send_data;
static DaemonInstance *vision_daemon_instance;
void VisionSetFlag(Enemy_Color_e enemy_color, Work_Mode_e work_mode, Bullet_Speed_e bullet_speed)
{
send_data.enemy_color = enemy_color;
send_data.work_mode = work_mode;
send_data.bullet_speed = bullet_speed;
}
void VisionSetAltitude(float yaw, float pitch, float roll)
{
send_data.yaw = yaw;
send_data.pitch = pitch;
send_data.roll = roll;
}
/**
* @brief 离线回调函数,将在daemon.c中被daemon task调用
* @attention 由于HAL库的设计问题,串口开启DMA接收之后同时发送有概率出现__HAL_LOCK()导致的死锁,使得无法
* 进入接收中断.通过daemon判断数据更新,重新调用服务启动函数以解决此问题.
*
* @param id vision_usart_instance的地址,此处没用.
*/
static void VisionOfflineCallback(void *id)
{
#ifdef VISION_USE_UART
USARTServiceInit(vision_usart_instance);
#endif // !VISION_USE_UART
LOGWARNING("[vision] vision offline, restart communication.");
}
#ifdef VISION_USE_UART
#include "bsp_usart.h"
/**
* @brief 接收解包回调函数,将在bsp_usart.c中被usart rx callback调用
* @todo 1.提高可读性,将get_protocol_info的第四个参数增加一个float类型buffer
* 2.添加标志位解码
*/
static void DecodeVision()
{
uint16_t flag_register;
DaemonReload(vision_daemon_instance); // 喂狗
get_protocol_info(vision_usart_instance->recv_buff, &flag_register, (uint8_t *)&recv_data.pitch);
// TODO: code to resolve flag_register;
}
Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle)
{
USART_Init_Config_s conf;
conf.module_callback = DecodeVision;
conf.recv_buff_size = VISION_RECV_SIZE;
conf.usart_handle = _handle;
vision_usart_instance = USARTRegister(&conf);
// 为master process注册daemon,用于判断视觉通信是否离线
Daemon_Init_Config_s daemon_conf = {
.callback = VisionOfflineCallback, // 离线时调用的回调函数,会重启串口接收
.owner_id = vision_usart_instance,
.reload_count = 10,
};
vision_daemon_instance = DaemonRegister(&daemon_conf);
return &recv_data;
}
/**
* @brief 发送函数
*
* @param send 待发送数据
*
*/
void VisionSend()
{
// buff和txlen必须为static,才能保证在函数退出后不被释放,使得DMA正确完成发送
// 析构后的陷阱需要特别注意!
static uint16_t flag_register;
static uint8_t send_buff[VISION_SEND_SIZE];
static uint16_t tx_len;
// TODO: code to set flag_register
flag_register = 30 << 8 | 0b00000001;
// 将数据转化为seasky协议的数据包
get_protocol_send_data(0x02, flag_register, &send_data.yaw, 3, send_buff, &tx_len);
USARTSend(vision_usart_instance, send_buff, tx_len, USART_TRANSFER_DMA); // 和视觉通信使用IT,防止和接收使用的DMA冲突
// 此处为HAL设计的缺陷,DMASTOP会停止发送和接收,导致再也无法进入接收中断.
// 也可在发送完成中断中重新启动DMA接收,但较为复杂.因此,此处使用IT发送.
// 若使用了daemon,则也可以使用DMA发送.
}
#endif // VISION_USE_UART
#ifdef VISION_USE_VCP
#include "bsp_usb.h"
static uint8_t *vis_recv_buff;
static void DecodeVision(uint16_t recv_len)
{
uint16_t flag_register;
get_protocol_info(vis_recv_buff, &flag_register, (uint8_t *)&recv_data.pitch);
// TODO: code to resolve flag_register;
}
/* 视觉通信初始化 */
Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle)
{
UNUSED(_handle); // 仅为了消除警告
USB_Init_Config_s conf = {.rx_cbk = DecodeVision};
vis_recv_buff = USBInit(conf);
// 为master process注册daemon,用于判断视觉通信是否离线
Daemon_Init_Config_s daemon_conf = {
.callback = VisionOfflineCallback, // 离线时调用的回调函数,会重启串口接收
.owner_id = NULL,
.reload_count = 5, // 50ms
};
vision_daemon_instance = DaemonRegister(&daemon_conf);
return &recv_data;
}
void VisionSend()
{
static uint16_t flag_register;
static uint8_t send_buff[VISION_SEND_SIZE];
static uint16_t tx_len;
// TODO: code to set flag_register
flag_register = 30 << 8 | 0b00000001;
// 将数据转化为seasky协议的数据包
get_protocol_send_data(0x02, flag_register, &send_data.yaw, 3, send_buff, &tx_len);
USBTransmit(send_buff, tx_len);
}
#endif // VISION_USE_VCP
modules/master_machine/master_process.h
+114
View File
@@ -0,0 +1,114 @@
#ifndef MASTER_PROCESS_H
#define MASTER_PROCESS_H
#include "bsp_usart.h"
#include "seasky_protocol.h"
#define VISION_RECV_SIZE 18u // 当前为固定值,36字节
#define VISION_SEND_SIZE 36u
#pragma pack(1)
typedef enum
{
NO_FIRE = 0,
AUTO_FIRE = 1,
AUTO_AIM = 2
} Fire_Mode_e;
typedef enum
{
NO_TARGET = 0,
TARGET_CONVERGING = 1,
READY_TO_FIRE = 2
} Target_State_e;
typedef enum
{
NO_TARGET_NUM = 0,
HERO1 = 1,
ENGINEER2 = 2,
INFANTRY3 = 3,
INFANTRY4 = 4,
INFANTRY5 = 5,
OUTPOST = 6,
SENTRY = 7,
BASE = 8
} Target_Type_e;
typedef struct
{
Fire_Mode_e fire_mode;
Target_State_e target_state;
Target_Type_e target_type;
float pitch;
float yaw;
} Vision_Recv_s;
typedef enum
{
COLOR_NONE = 0,
COLOR_BLUE = 1,
COLOR_RED = 2,
} Enemy_Color_e;
typedef enum
{
VISION_MODE_AIM = 0,
VISION_MODE_SMALL_BUFF = 1,
VISION_MODE_BIG_BUFF = 2
} Work_Mode_e;
typedef enum
{
BULLET_SPEED_NONE = 0,
BIG_AMU_10 = 10,
SMALL_AMU_15 = 15,
BIG_AMU_16 = 16,
SMALL_AMU_18 = 18,
SMALL_AMU_30 = 30,
} Bullet_Speed_e;
typedef struct
{
Enemy_Color_e enemy_color;
Work_Mode_e work_mode;
Bullet_Speed_e bullet_speed;
float yaw;
float pitch;
float roll;
} Vision_Send_s;
#pragma pack()
/**
* @brief 调用此函数初始化和视觉的串口通信
*
* @param handle 用于和视觉通信的串口handle(C板上一般为USART1,丝印为USART2,4pin)
*/
Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle);
/**
* @brief 发送视觉数据
*
*/
void VisionSend();
/**
* @brief 设置视觉发送标志位
*
* @param enemy_color
* @param work_mode
* @param bullet_speed
*/
void VisionSetFlag(Enemy_Color_e enemy_color, Work_Mode_e work_mode, Bullet_Speed_e bullet_speed);
/**
* @brief 设置发送数据的姿态部分
*
* @param yaw
* @param pitch
*/
void VisionSetAltitude(float yaw, float pitch, float roll);
#endif // !MASTER_PROCESS_H
modules/master_machine/master_process.md
+133
View File
@@ -0,0 +1,133 @@
# master_process
<p align='right'>neozng1@hnu.edu.cn</p>
> TODO:
>
> 1. 补全标志位解析和发送设置的代码
> 2. 增加发送给视觉数据的时间戳用于数据对齐
## 总览和封装说明
模块包含了和视觉通信的初始化、向上位机发送信息的接口和模块的串口的回调处理。接口的定义统一，可以方便的替换成其他通信方式，如CAN。
## 代码结构
.h文件内包括了外部接口和与**视觉上位机通信的数据结构定义**，以及模块对应的宏。c文件内为私有函数和外部接口的定义。
本模块主要是对协议解析的处理和协议发送的封装，实际内容不多。协议相关内容都在`seasky_protocol.h`中。
## 类型定义
和视觉通信所必须的标志位和数据。包括开火模式，目标状态，目标类型，接收/发送数据结构体。
```c
typedef enum
{
NO_FIRE = 0,
AUTO_FIRE = 1,
AUTO_AIM = 2
} Fire_Mode_e;
typedef enum
{
NO_TARGET = 0,
TARGET_CONVERGING = 1,
READY_TO_FIRE = 2
} Target_State_e;
typedef enum
{
NO_TARGET_NUM = 0,
HERO1 = 1,
ENGINEER2 = 2,
INFANTRY3 = 3,
INFANTRY4 = 4,
INFANTRY5 = 5,
OUTPOST = 6,
SENTRY = 7,
BASE = 8
} Target_Type_e;
typedef struct
{
Fire_Mode_e fire_mode;
Target_State_e target_state;
Target_Type_e target_type;
float pitch;
float yaw;
} Vision_Recv_s;
typedef enum
{
BLUE = 0,
RED = 1
} Enemy_Color_e;
typedef enum
{
MODE_AIM = 0,
MODE_SMALL_BUFF = 1,
MODE_BIG_BUFF = 2
} Work_Mode_e;
typedef enum
{
BIG_AMU_10 = 10,
SMALL_AMU_15 = 15,
BIG_AMU_16 = 16,
SMALL_AMU_18 = 18,
SMALL_AMU_30 = 30,
} Bullet_Speed_e;
typedef struct
{
Enemy_Color_e enemy_color;
Work_Mode_e work_mode;
Bullet_Speed_e bullet_speed;
float yaw;
float pitch;
float roll;
} Vision_Send_s;
```
## 外部接口
```c
Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle);
void VisionSend(Vision_Send_s *send);
```
给`VisionInit()`传入串口handle，将初始化一个视觉通信模块，返回值是接收数据的结构体指针。拥有视觉模块的应用应该在初始化中调用此函数，并保存返回值的指针。
调用`VisionSend`并传入填好发送数据的结构体，会通过底层的通信模块向视觉发送一帧报文。
## 私有函数和变量
```c
static Vision_Recv_s recv_data;
static usart_instance *vision_usart_instance;
static void DecodeVision()
{
static uint16_t flag_register;
get_protocol_info(vision_usart_instance->recv_buff, &flag_register, (uint8_t*)&recv_data.pitch);
// TODO: code to resolve flag_register;
}
```
第一个是保存接收数据的结构体，其指针将会在初始化的时候返回给拥有者。目前最多只能配置一个视觉模块。
第二个是该模块拥有的串口实例指针，用于调度其底层的发送和接收。如果要换成CAN/SPI等，替换成对应实例，并修改初始化和发送的实现即可。
`DecodeVision()`是解析视觉接收数据的回调函数，会在串口接收回调中被调用。如果修改通信协议，只需要更改
`get_protocol_info()`。
modules/master_machine/seasky_protocol.c
+133
View File
@@ -0,0 +1,133 @@
/**
* @file seasky_protocol.c
* @author Liu Wei
* @author modified by Neozng
* @brief 湖南大学RoBoMatster串口通信协议
* @version 0.1
* @date 2022-11-03
*
* @copyright Copyright (c) 2022
*
*/
#include "seasky_protocol.h"
#include "crc8.h"
#include "crc16.h"
#include "memory.h"
/*获取CRC8校验码*/
uint8_t Get_CRC8_Check(uint8_t *pchMessage,uint16_t dwLength)
{
return crc_8(pchMessage,dwLength);
}
/*检验CRC8数据段*/
static uint8_t CRC8_Check_Sum(uint8_t *pchMessage, uint16_t dwLength)
{
uint8_t ucExpected = 0;
if ((pchMessage == 0) || (dwLength <= 2))
return 0;
ucExpected = crc_8(pchMessage, dwLength - 1);
return (ucExpected == pchMessage[dwLength - 1]);
}
/*获取CRC16校验码*/
uint16_t Get_CRC16_Check(uint8_t *pchMessage,uint32_t dwLength)
{
return crc_16(pchMessage,dwLength);
}
/*检验CRC16数据段*/
static uint16_t CRC16_Check_Sum(uint8_t *pchMessage, uint32_t dwLength)
{
uint16_t wExpected = 0;
if ((pchMessage == 0) || (dwLength <= 2))
{
return 0;
}
wExpected = crc_16(pchMessage, dwLength - 2);
return (((wExpected & 0xff) == pchMessage[dwLength - 2]) && (((wExpected >> 8) & 0xff) == pchMessage[dwLength - 1]));
}
/*检验数据帧头*/
static uint8_t protocol_heade_Check(protocol_rm_struct *pro, uint8_t *rx_buf)
{
if (rx_buf[0] == PROTOCOL_CMD_ID)
{
pro->header.sof = rx_buf[0];
if (CRC8_Check_Sum(&rx_buf[0], 4))
{
pro->header.data_length = (rx_buf[2] << 8) | rx_buf[1];
pro->header.crc_check = rx_buf[3];
pro->cmd_id = (rx_buf[5] << 8) | rx_buf[4];
return 1;
}
}
return 0;
}
/*
此函数根据待发送的数据更新数据帧格式以及内容，实现数据的打包操作
后续调用通信接口的发送函数发送tx_buf中的对应数据
*/
void get_protocol_send_data(uint16_t send_id, // 信号id
uint16_t flags_register, // 16位寄存器
float *tx_data, // 待发送的float数据
uint8_t float_length, // float的数据长度
uint8_t *tx_buf, // 待发送的数据帧
uint16_t *tx_buf_len) // 待发送的数据帧长度
{
static uint16_t crc16;
static uint16_t data_len;
data_len = float_length * 4 + 2;
/*帧头部分*/
tx_buf[0] = PROTOCOL_CMD_ID;
tx_buf[1] = data_len & 0xff; // 低位在前
tx_buf[2] = (data_len >> 8) & 0xff; // 低位在前
tx_buf[3] = crc_8(&tx_buf[0], 3); // 获取CRC8校验位
/*数据的信号id*/
tx_buf[4] = send_id & 0xff;
tx_buf[5] = (send_id >> 8) & 0xff;
/*建立16位寄存器*/
tx_buf[6] = flags_register & 0xff;
tx_buf[7] = (flags_register >> 8) & 0xff;
/*float数据段*/
for (int i = 0; i < 4 * float_length; i++)
{
tx_buf[i + 8] = ((uint8_t *)(&tx_data[i / 4]))[i % 4];
}
/*整包校验*/
crc16 = crc_16(&tx_buf[0], data_len + 6);
tx_buf[data_len + 6] = crc16 & 0xff;
tx_buf[data_len + 7] = (crc16 >> 8) & 0xff;
*tx_buf_len = data_len + 8;
}
/*
此函数用于处理接收数据，
返回数据内容的id
*/
uint16_t get_protocol_info(uint8_t *rx_buf, // 接收到的原始数据
uint16_t *flags_register, // 接收数据的16位寄存器地址
uint8_t *rx_data) // 接收的float数据存储地址
{
// 放在静态区,避免反复申请栈上空间
static protocol_rm_struct pro;
static uint16_t date_length;
if (protocol_heade_Check(&pro, rx_buf))
{
date_length = OFFSET_BYTE + pro.header.data_length;
if (CRC16_Check_Sum(&rx_buf[0], date_length))
{
*flags_register = (rx_buf[7] << 8) | rx_buf[6];
memcpy(rx_data, rx_buf + 8, pro.header.data_length - 2);
return pro.cmd_id;
}
}
return 0;
}
modules/master_machine/seasky_protocol.h
+36
View File
@@ -0,0 +1,36 @@
#ifndef __SEASKY_PROTOCOL_H
#define __SEASKY_PROTOCOL_H
#include <stdio.h>
#include <stdint.h>
#define PROTOCOL_CMD_ID 0XA5
#define OFFSET_BYTE 8 // 出数据段外，其他部分所占字节数
typedef struct
{
struct
{
uint8_t sof;
uint16_t data_length;
uint8_t crc_check; // 帧头CRC校验
} header; // 数据帧头
uint16_t cmd_id; // 数据ID
uint16_t frame_tail; // 帧尾CRC校验
} protocol_rm_struct;
/*更新发送数据帧，并计算发送数据帧长度*/
void get_protocol_send_data(uint16_t send_id, // 信号id
uint16_t flags_register, // 16位寄存器
float *tx_data, // 待发送的float数据
uint8_t float_length, // float的数据长度
uint8_t *tx_buf, // 待发送的数据帧
uint16_t *tx_buf_len); // 待发送的数据帧长度
/*接收数据处理*/
uint16_t get_protocol_info(uint8_t *rx_buf, // 接收到的原始数据
uint16_t *flags_register, // 接收数据的16位寄存器地址
uint8_t *rx_data); // 接收的float数据存储地址
#endif
modules/master_machine/湖南大学RoboMaster电控组通信协议.md
+73
View File
@@ -0,0 +1,73 @@
# 湖南大学RoboMaster电控组通信协议
<p align='right'>Seasky LIUWei</p>
<p align='right'>modified by neozng1@hnu.edu.cn</p>
可用于视觉，以及其他自研模块（仅限串口通信）
> TODO:
>
> 1. 利用F4自带的硬件CRC模块计算校验码，提高速度
> 2. 增加更多的数据类型支持，使得数据类型也为可配置的
## 一、串口配置
通信方式是串口，配置为波特率 921600，8 位数据位，1 位停止位，无硬件流控，无校验位。
## 二、数据帧说明
以下所有低位在前发送，数据长度可变。
1. 通信协议格式
| 帧头 | 数据ID | 数据 | 帧尾 |
| ----------------------- | -------------- | ------------- | :---------------------------------- |
| protocol_header(4-byte) | cmd_id(2-byte) | data (n-byte) | frame_tail(2-byte，CRC16，整包校验) |
2. 帧头详细定义
| 帧头 | 偏移位置 | 字节大小 | 内容 |
| ----------- | -------- | -------- | ----------------------------- |
| sof(CMD) | 0 | 1 | 数据帧起始字节，固定值为 0xA5 |
| data_length | 1 | 2 | 数据帧中 data 的长度 |
| crc_check | 3 | 1 | 帧头CRC校验 |
3. cmd_id 命令码 ID 说明 **(字节偏移 4，字节大小 2)**
| 命令码 | 数据段长度 | 功能说明 |
| -------------- | --------------- | -------- |
| 0x0001(可修改) | 2 byte (16-bit) | 视觉数据 |
| ....... | | |
4. 数据段data (n-byte)
| 数据 | 偏移位置 | 字节大小 | 内容 |
| -------------- | -------- | --------------- | ------------------------------------- |
| flags_register | 6 | 2 | 16位标志置位寄存器 |
| float_data | 8 | 4 * data_length | float数据内容（4 * data_length-byte） |
5. frame_tail(CRC16，整包校验)
## 三、协议接口说明
```c
/*更新发送数据帧，并计算发送数据帧长度*/
void get_protocol_send_data(uint16_t send_id, //信号id
uint16_t flags_register, // 16位寄存器
float *tx_data, //待发送的float数据
uint8_t float_length, // float的数据长度
uint8_t *tx_buf, //待发送的数据帧
uint16_t *tx_buf_len); //待发送的数据帧长度
```
将float_data字段准备好放入`tx_data[]`并设置好标志位`flags_register`之后，调用此函数。它将计算循环冗余校验码，并把数据转换成串口发送的格式，放在`tx_buf[]`中。
```c
/*接收数据处理*/
uint16_t get_protocol_info(uint8_t *rx_buf, //接收到的原始数据
uint16_t *flags_register, //接收数据的16位寄存器地址
float *rx_data); //接收的float数据存储地址
```
将收到的一包原始数据buff地址传入，若校验通过，会把收到的标志位和float数据解析出来，保存在`flags_register*`和 `rx_data[]`中。
modules/message_center/message_center.c
+117
View File
@@ -0,0 +1,117 @@
#include "message_center.h"
#include "stdlib.h"
#include "string.h"
#include "bsp_log.h"
/* message_center是fake head node,是方便链表编写的技巧,这样就不需要处理链表头的特殊情况 */
static Publisher_t message_center = {
.topic_name = "Message_Manager",
.first_subs = NULL,
.next_topic_node = NULL};
static void CheckName(char *name)
{
if (strnlen(name, MAX_TOPIC_NAME_LEN + 1) >= MAX_TOPIC_NAME_LEN)
{
LOGERROR("EVENT NAME TOO LONG:%s", name);
while (1)
; // 进入这里说明话题名超出长度限制
}
}
static void CheckLen(uint8_t len1, uint8_t len2)
{
if (len1 != len2)
{
LOGERROR("EVENT LEN NOT SAME:%d,%d", len1, len2);
while (1)
; // 进入这里说明相同话题的消息长度却不同
}
}
Publisher_t *PubRegister(char *name, uint8_t data_len)
{
CheckName(name);
Publisher_t *node = &message_center;
while (node->next_topic_node) // message_center会直接跳过,不需要特殊处理,这被称作dumb_head(编程技巧)
{
node = node->next_topic_node; // 切换到下一个发布者(话题)结点
if (strcmp(node->topic_name, name) == 0) // 如果已经注册了相同的话题,直接返回结点指针
{
CheckLen(data_len, node->data_len);
node->pub_registered_flag = 1;
return node;
}
} // 遍历完发现尚未创建name对应的话题
// 在链表尾部创建新的话题并初始化
node->next_topic_node = (Publisher_t *)malloc(sizeof(Publisher_t));
memset(node->next_topic_node, 0, sizeof(Publisher_t));
node->next_topic_node->data_len = data_len;
strcpy(node->next_topic_node->topic_name, name);
node->pub_registered_flag = 1;
return node->next_topic_node;
}
Subscriber_t *SubRegister(char *name, uint8_t data_len)
{
Publisher_t *pub = PubRegister(name, data_len); // 查找或创建该话题的发布者
// 创建新的订阅者结点,申请内存,注意要memset因为新空间不一定是空的,可能有之前留存的垃圾值
Subscriber_t *ret = (Subscriber_t *)malloc(sizeof(Subscriber_t));
memset(ret, 0, sizeof(Subscriber_t));
// 对新建的Subscriber进行初始化
ret->data_len = data_len; // 设定数据长度
for (size_t i = 0; i < QUEUE_SIZE; ++i)
{ // 给消息队列的每一个元素分配空间,queue里保存的实际上是数据执指针,这样可以兼容不同的数据长度
ret->queue[i] = malloc(data_len);
}
// 如果是第一个订阅者,特殊处理一下,将first_subs指针指向新建的订阅者(详见文档)
if (pub->first_subs == NULL)
{
pub->first_subs = ret;
return ret;
}
// 若该话题已经有订阅者, 遍历订阅者链表,直到到达尾部
Subscriber_t *sub = pub->first_subs; // 作为iterator
while (sub->next_subs_queue) // 遍历订阅了该话题的订阅者链表
{
sub = sub->next_subs_queue; // 移动到下一个订阅者,遇到空指针停下,说明到了链表尾部
}
sub->next_subs_queue = ret; // 把刚刚创建的订阅者接上
return ret;
}
/* 如果队列为空,会返回0;成功获取数据,返回1;后续可以做更多的修改,比如剩余消息数目等 */
uint8_t SubGetMessage(Subscriber_t *sub, void *data_ptr)
{
if (sub->temp_size == 0)
{
return 0;
}
memcpy(data_ptr, sub->queue[sub->front_idx], sub->data_len);
sub->front_idx = (sub->front_idx++) % QUEUE_SIZE; // 队列头索引增加
sub->temp_size--; // pop一个数据,长度减1
return 1;
}
uint8_t PubPushMessage(Publisher_t *pub, void *data_ptr)
{
static Subscriber_t *iter;
iter = pub->first_subs; // iter作为订阅者指针,遍历订阅该话题的所有订阅者;如果为空说明遍历结束
// 遍历订阅了当前话题的所有订阅者,依次填入最新消息
while (iter)
{
if (iter->temp_size == QUEUE_SIZE) // 如果队列已满,则需要删除最老的数据(头部),再填入
{
// 队列头索引前移动,相当于抛弃前一个位置的数据,被抛弃的位置稍后会被写入新的数据
iter->front_idx = (iter->front_idx + 1) % QUEUE_SIZE;
iter->temp_size--; // 相当于出队,size-1
}
// 将Pub的数据复制到队列的尾部(最新)
memcpy(iter->queue[iter->back_idx], data_ptr, pub->data_len);
iter->back_idx = (iter->back_idx + 1) % QUEUE_SIZE; // 队列尾部前移
iter->temp_size++; // 入队,size+1
iter = iter->next_subs_queue; // 访问下一个订阅者
}
return 1;
}
modules/message_center/message_center.h
+85
View File
@@ -0,0 +1,85 @@
/**
* @file message_center.h
* @author NeoZeng neozng1@hnu.edu.cn
* @brief 这是一个伪pubsub机制,仅对应用之间的通信进行了隔离
* @version 0.1
* @date 2022-11-30
*
* @copyright Copyright (c) 2022
*
*/
#ifndef PUBSUB_H
#define PUBSUB_H
#include "stdint.h"
#define MAX_TOPIC_NAME_LEN 32 // 最大的话题名长度,每个话题都有字符串来命名
#define MAX_TOPIC_COUNT 12 // 最多支持的话题数量
#define QUEUE_SIZE 1
typedef struct mqt
{
/* 用数组模拟FIFO队列 */
void *queue[QUEUE_SIZE];
uint8_t data_len;
uint8_t front_idx;
uint8_t back_idx;
uint8_t temp_size; // 当前队列长度
/* 指向下一个订阅了相同的话题的订阅者的指针 */
struct mqt *next_subs_queue; // 使得发布者可以通过链表访问所有订阅了相同话题的订阅者
} Subscriber_t;
/**
* @brief 发布者类型.每个发布者拥有发布者实例,并且可以通过链表访问所有订阅了自己发布的话题的订阅者
*
*/
typedef struct ent
{
/* 话题名称 */
char topic_name[MAX_TOPIC_NAME_LEN + 1]; // 1个字节用于存放字符串结束符 '\0'
uint8_t data_len; // 该话题的数据长度
/* 指向第一个订阅了该话题的订阅者,通过链表访问所有订阅者 */
Subscriber_t *first_subs;
/* 指向下一个Publisher的指针 */
struct ent *next_topic_node;
uint8_t pub_registered_flag; // 用于标记该发布者是否已经注册
} Publisher_t;
/**
* @brief 订阅name的话题消息
*
* @param name 话题名称
* @param data_len 消息长度,通过sizeof()获取
* @return Subscriber_t* 返回订阅者实例
*/
Subscriber_t *SubRegister(char *name, uint8_t data_len);
/**
* @brief 注册成为消息发布者
*
* @param name 发布者发布的话题名称(话题)
* @return Publisher_t* 返回发布者实例
*/
Publisher_t *PubRegister(char *name, uint8_t data_len);
/**
* @brief 获取消息
*
* @param sub 订阅者实例指针
* @param data_ptr 数据指针,接收的消息将会放到此处
* @return uint8_t 返回值为0说明没有新的消息(消息队列为空),为1说明获取到了新的消息
*/
uint8_t SubGetMessage(Subscriber_t *sub, void *data_ptr);
/**
* @brief 发布者给所有订阅了话题的订阅者推送消息
*
* @param pub 发布者实例指针
* @param data_ptr 指向要发布的数据的指针
* @return uint8_t 新消息成功推送给几个订阅者
*/
uint8_t PubPushMessage(Publisher_t *pub, void *data_ptr);
#endif // !PUBSUB_H
modules/message_center/message_center.md
+180
View File
@@ -0,0 +1,180 @@
# message_center
<p align='right'>neozng1@hnu.edu.cn</p>
> TODO:
>
> 支持自定义队列长度，使得订阅者可以自行确定需要的队列长度，适应不同的需求
## 总览和封装说明
**重要定义：**
- 发布者：发布消息的对象。发布者会将自己的消息推送给所有订阅了某个特定**话题**的订阅者。
- 订阅者：获取消息的对象。订阅者在订阅了某个话题之后，可以通过接口获得该话题的消息。
- 话题（topic）：用于区分消息来源的对象。可以将一个话题看作一刊杂志，不同的发布者会将文章汇集到杂志上，而订阅者选择订阅一种杂志，然后就可以获取所有写在杂志上的文章。
Message Center不同应用间进行消息传递的中介，它的存在可以在相当大的程度上解耦不同的app，使得不同的应用之间**不存在包含关系**，让代码的自由度更大，将不同模块之间的关系降为**松耦合**。在以往，如果一个.c文件中的数据需要被其他任务/源文件共享，那么其他模块应该要包含前者的头文件，且头文件中应当存在获取该模块数据的接口（即函数，一般是返回数据指针或直接返回数据，**强烈建议不要使用全局变量**）；但现在，不同的应用之间完全隔离，他们不需要了解彼此的存在，而是只能看见一个**消息中心**以及一些**话题**。
需要被共享的消息，将会被**发布者**（publisher）发送到消息中心；要获取消息，则由**订阅者**（subscriber）从消息中心根据订阅的话题获取。在这之前，发布者要在消息中心完成**注册**，将自己要发布的消息类型和话题名称提交到消息中心；订阅者同样要先在消息中心完成订阅，将自己要接收的消息类型和话题名称提交到订阅中心。消息中心会根据**话题名称**，把订阅者绑定到发布相同名称的发布者上。
> 为了节省空间，数据结构上采用了链表+循环数组模拟队列的方式。C没有哈希表，因此让发布者保存所有订阅者的地址（实际上只保存首地址，然后通过链表访问所有订阅者）。
Message Center对外提供了四个接口，所有原本要进行信息交互的应用都应该包含`message_center.h`，并在初始化的时候进行注册。
## 代码结构
.h 文件中包含了外部接口和类型定义，.c中包含了各个接口的具体实现。
## 外部接口
**在代码实现上，话题名实际上就是通过一个字符串体现的。**
```c
Subscriber_t* SubRegister(char* name,uint8_t data_len);
Publisher_t* PubRegister(char* name,uint8_t data_len);
uint8_t SubGetMessage(Subscriber_t* sub,void* data_ptr);
void PubPushMessage(Publisher_t* pub,void* data_ptr);
```
### 订阅者
订阅者应该保存一个订阅者类型的指针`Subscriber_t*`，在初始化的时候调用`SubRegister()`并传入要订阅的话题名和该话题对应消息的长度，可以直接输入字符串，示例如下，将从event_name订阅float数据：
```c
Subscriber_t* my_sub;
my_sub=SubRegister("event_name",sizeof(float));
```
订阅完毕后，在应用中通过`SubGetMessage()`获取消息，调用时传入订阅时获得的指针，以及要存放数据的指针。在使用的时候，建议使用强制类型转换将`data_ptr` cast成void*类型（好习惯）。
如果消息队列中有消息，返回值为1；否则，返回值为0，说明没有新的消息可用。
### 发布者
发布者应该保存一个发布者类型的指针，在初始化的时候传入要发布的话题名和该话题对应的消息长度。
完成注册后，通过`PubPushMessage()`发布新的消息。所有订阅了该话题的订阅者都会收到新的消息推送。
### 可修改的宏
```c
#define MAX_EVENT_NAME_LEN 32 //最大的话题名长度,每个话题都由字符串来命名
#define QUEUE_SIZE 1 //消息队列的长度
```
修改第一个可以扩大话题名长度，第二个确定消息队列的长度，数量越大可以保存的消息越多。
## 私有函数和定义
```c
static Publisher_t message_center = {
.event_name = "Message_Manager",
.first_subs = NULL,
.next_event_node = NULL};
static void CheckName(char* name)
{
if(strnlen(name,MAX_EVENT_NAME_LEN+1)>=MAX_EVENT_NAME_LEN)
{
while (1); // 进入这里说明话题名超出长度限制
}
}
```
`message_center`内部保存了指向第一个发布者的指针，可以看作整个消息中心的抽象。通过这个变量，可以访问所有发布者和订阅者。它将会在各个函数中作为dumb_head（哑结点）以简化逻辑，这样不需要对链表头进行特殊处理。
`CheckName()`在发布者/订阅者注册的时候被调用，用于检查话题名是否超过长度限制。超长后会进入死循环，方便开发者检查。
> 四个外部接口的实现都有详细的注释，有兴趣的同学可以自行阅读。下方也提供了流程图。
## 注册、发布、获取消息流程
包含一个结构图和四个流程图。
### Message Center的结构![image-20221201150945052](../../.assets/image-20221201150945052.png)
<center>建议打开原图查看</center>
**多个publisher可以绑定同一个话题，往该话题推送消息。但一个subscriber只能订阅一个话题，如果应用需要订阅多个话题，则要创建对应数量的订阅者。**
> 对于电控程序目前的情况，不存在多个publisher向同一个话题推送消息的情况。
**对于相同话题，其消息长度必须相同**。发布者和订阅者在注册时都会传入消息长度，用`sizof(your_data)`获取。应当保证不同的模块在进行交互式，使用相同的数据长度。
### 发布者和订阅者注册的流程
- **发布者：**
遍历发布者的话题结点，如果发现相同的话题，直接返回指针即可；遍历完成后发现尚未创建则创建新的话题。
<img src="../../.assets/image-20221201152530558.png" alt="image-20221201152530558" style="zoom: 80%;" />
- **订阅者：**
需要注意，由于不同应用/模块的初始化顺序不同，可能出现订阅者先于发布者订阅某一消息的情况，所以要进行发布者链表的遍历，判断是否已经存在相同话题名的发布者，不存在则要先创建发布者结点再将新建订阅者结点并挂载到前者上。
<img src="../../.assets/image-20221201152904044.png" alt="image-20221201152904044" style="zoom:80%;" />
### 推送/获取消息的流程
- **数组+头尾索引模拟队列**
<img src="../../.assets/image-20221201155228196.png" alt="image-20221201155228196" style="zoom: 71%;" />
front指向队列头，即最早入队的数据；back指向队列尾，即最新的数据。队列是first in first out（FIFO，先进先出）的结构。back指向的位置是入队数据被写入的位置，front指向的是读取时会出队的位置。当有数据入队，back++；出队则front++。若碰到数组边界，则返回数组头，可以通过取模实现：
```C
idx=(idx+1)%SIZE_OF_ARRAY; //SIZE_OF_ARRAY是数组大小
```
我们还需要一个变量用于保存当前队列的元素个数，如果在写入时，队列长度等于上限，应该先将最老的数据出队，再写入新的数据，即：
```c
back=(back+1)%SIZE_OF_ARRAY;
size--;
queue[front]=new_data;
front=(front+1)%SIZE_OF_ARRAY;
```
- **发布者推送消息到指定话题**
通过发布者指针，将订阅了该话题的所有订阅者遍历，将新数据入队。
- **订阅者获取消息**
从订阅者指针访问消息队列，取出最先进入队列的数据。注意判断队列是否为空，如果为空则返回0。
## 示例代码
```c
typedef struct
{
float a;
uint8_t b;
uint32_t c;
}good;
good g1;
good g2;
good pub_data={.a=1,.b=2,.c=3};
// 一个发布者,两个订阅者
Subscriber_t* s=SubRegister("test",sizeof(good));
Subscriber_t* ss=SubRegister("test",sizeof(good));
Publisher_t* p=PubRegister("test",sizeof(good));
// 推送消息
PubPushMessage(p,&pub_data);
pub_data.a=4;
pub_data.b=5;
pub_data.c=6;
// 推送新消息
PubPushMessage(p,&pub_data);
volatile uint8_t d= 0; // 确定收到的消息是否有效,可以根据d的值进一步处理
d=SubGetMessage(s,&g1);
d=SubGetMessage(s,&g1);
d=SubGetMessage(s,&g1); // 此时d等于0
d=SubGetMessage(ss,&g2);
```
modules/module.md
+3
View File
@@ -0,0 +1,3 @@
# module
这是module层的说明。
modules/motor/DJImotor/dji_motor.c
+416
View File
@@ -0,0 +1,416 @@
#include "dji_motor.h"
#include "general_def.h"
#include "bsp_dwt.h"
#include "bsp_log.h"
static uint8_t idx = 0; // register idx,是该文件的全局电机索引,在注册时使用
/* DJI电机的实例,此处仅保存指针,内存的分配将通过电机实例初始化时通过malloc()进行 */
static DJIMotorInstance *dji_motor_instance[DJI_MOTOR_CNT] = {NULL}; // 会在control任务中遍历该指针数组进行pid计算
#ifdef FDCAN
static CANInstance sender_assignment[15] = {
[0] = {.can_handle = &hfdcan1, .txconf.Identifier = 0x1ff, .txconf.IdType = FDCAN_STANDARD_ID, .txconf.TxFrameType = FDCAN_DATA_FRAME, .txconf.DataLength = FDCAN_DLC_BYTES_8, .txconf.FDFormat = FDCAN_CLASSIC_CAN,.txconf.BitRateSwitch = FDCAN_BRS_OFF, .tx_buff = {0}},
[1] = {.can_handle = &hfdcan1, .txconf.Identifier = 0x200, .txconf.IdType = FDCAN_STANDARD_ID, .txconf.TxFrameType = FDCAN_DATA_FRAME, .txconf.DataLength = FDCAN_DLC_BYTES_8, .txconf.FDFormat = FDCAN_CLASSIC_CAN,.txconf.BitRateSwitch = FDCAN_BRS_OFF, .tx_buff = {0}},
[2] = {.can_handle = &hfdcan1, .txconf.Identifier = 0x2ff, .txconf.IdType = FDCAN_STANDARD_ID, .txconf.TxFrameType = FDCAN_DATA_FRAME, .txconf.DataLength = FDCAN_DLC_BYTES_8, .txconf.FDFormat = FDCAN_CLASSIC_CAN,.txconf.BitRateSwitch = FDCAN_BRS_OFF, .tx_buff = {0}},
[3] = {.can_handle = &hfdcan2, .txconf.Identifier = 0x1ff, .txconf.IdType = FDCAN_STANDARD_ID, .txconf.TxFrameType = FDCAN_DATA_FRAME, .txconf.DataLength = FDCAN_DLC_BYTES_8, .txconf.FDFormat = FDCAN_CLASSIC_CAN,.txconf.BitRateSwitch = FDCAN_BRS_OFF, .tx_buff = {0}},
[4] = {.can_handle = &hfdcan2, .txconf.Identifier = 0x200, .txconf.IdType = FDCAN_STANDARD_ID, .txconf.TxFrameType = FDCAN_DATA_FRAME, .txconf.DataLength = FDCAN_DLC_BYTES_8, .txconf.FDFormat = FDCAN_CLASSIC_CAN,.txconf.BitRateSwitch = FDCAN_BRS_OFF, .tx_buff = {0}},
[5] = {.can_handle = &hfdcan2, .txconf.Identifier = 0x2ff, .txconf.IdType = FDCAN_STANDARD_ID, .txconf.TxFrameType = FDCAN_DATA_FRAME, .txconf.DataLength = FDCAN_DLC_BYTES_8, .txconf.FDFormat = FDCAN_CLASSIC_CAN,.txconf.BitRateSwitch = FDCAN_BRS_OFF, .tx_buff = {0}},
[6] = {.can_handle = &hfdcan3, .txconf.Identifier = 0x1ff, .txconf.IdType = FDCAN_STANDARD_ID, .txconf.TxFrameType = FDCAN_DATA_FRAME, .txconf.DataLength = FDCAN_DLC_BYTES_8, .txconf.FDFormat = FDCAN_CLASSIC_CAN,.txconf.BitRateSwitch = FDCAN_BRS_OFF, .tx_buff = {0}},
[7] = {.can_handle = &hfdcan3, .txconf.Identifier = 0x200, .txconf.IdType = FDCAN_STANDARD_ID, .txconf.TxFrameType = FDCAN_DATA_FRAME, .txconf.DataLength = FDCAN_DLC_BYTES_8, .txconf.FDFormat = FDCAN_CLASSIC_CAN,.txconf.BitRateSwitch = FDCAN_BRS_OFF, .tx_buff = {0}},
[8] = {.can_handle = &hfdcan3, .txconf.Identifier = 0x2ff, .txconf.IdType = FDCAN_STANDARD_ID, .txconf.TxFrameType = FDCAN_DATA_FRAME, .txconf.DataLength = FDCAN_DLC_BYTES_8, .txconf.FDFormat = FDCAN_CLASSIC_CAN,.txconf.BitRateSwitch = FDCAN_BRS_OFF, .tx_buff = {0}},
[9] = {.can_handle = &hfdcan1, .txconf.Identifier = 0x1fe, .txconf.IdType = FDCAN_STANDARD_ID, .txconf.TxFrameType = FDCAN_DATA_FRAME, .txconf.DataLength = FDCAN_DLC_BYTES_8, .txconf.FDFormat = FDCAN_CLASSIC_CAN,.txconf.BitRateSwitch = FDCAN_BRS_OFF, .tx_buff = {0}},
[10] = {.can_handle = &hfdcan1, .txconf.Identifier = 0x2fe, .txconf.IdType = FDCAN_STANDARD_ID, .txconf.TxFrameType = FDCAN_DATA_FRAME, .txconf.DataLength = FDCAN_DLC_BYTES_8, .txconf.FDFormat = FDCAN_CLASSIC_CAN,.txconf.BitRateSwitch = FDCAN_BRS_OFF, .tx_buff = {0}},
[11] = {.can_handle = &hfdcan2, .txconf.Identifier = 0x1fe, .txconf.IdType = FDCAN_STANDARD_ID, .txconf.TxFrameType = FDCAN_DATA_FRAME, .txconf.DataLength = FDCAN_DLC_BYTES_8, .txconf.FDFormat = FDCAN_CLASSIC_CAN,.txconf.BitRateSwitch = FDCAN_BRS_OFF, .tx_buff = {0}},
[12] = {.can_handle = &hfdcan2, .txconf.Identifier = 0x2fe, .txconf.IdType = FDCAN_STANDARD_ID, .txconf.TxFrameType = FDCAN_DATA_FRAME, .txconf.DataLength = FDCAN_DLC_BYTES_8, .txconf.FDFormat = FDCAN_CLASSIC_CAN,.txconf.BitRateSwitch = FDCAN_BRS_OFF, .tx_buff = {0}},
[13] = {.can_handle = &hfdcan3, .txconf.Identifier = 0x1fe, .txconf.IdType = FDCAN_STANDARD_ID, .txconf.TxFrameType = FDCAN_DATA_FRAME, .txconf.DataLength = FDCAN_DLC_BYTES_8, .txconf.FDFormat = FDCAN_CLASSIC_CAN,.txconf.BitRateSwitch = FDCAN_BRS_OFF, .tx_buff = {0}},
[14] = {.can_handle = &hfdcan3, .txconf.Identifier = 0x2fe, .txconf.IdType = FDCAN_STANDARD_ID, .txconf.TxFrameType = FDCAN_DATA_FRAME, .txconf.DataLength = FDCAN_DLC_BYTES_8, .txconf.FDFormat = FDCAN_CLASSIC_CAN,.txconf.BitRateSwitch = FDCAN_BRS_OFF, .tx_buff = {0}},
};
#else
/**
* @brief 由于DJI电机发送以四个一组的形式进行,故对其进行特殊处理,用6个(2can*3group)can_instance专门负责发送
* 该变量将在 DJIMotorControl() 中使用,分组在 MotorSenderGrouping()中进行
*
* @note 因为只用于发送,所以不需要在bsp_can中注册
*
* C610(m2006)/C620(m3508):0x1ff,0x200;
* GM6020:0x1ff,0x2ff
* 反馈(rx_id): GM6020: 0x204+id ; C610/C620: 0x200+id
* can1: [0]:0x1FF,[1]:0x200,[2]:0x2FF
* can2: [3]:0x1FF,[4]:0x200,[5]:0x2FF
*/
static CANInstance sender_assignment[6] = {
[0] = {.can_handle = &hcan1, .txconf.StdId = 0x1ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[1] = {.can_handle = &hcan1, .txconf.StdId = 0x200, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[2] = {.can_handle = &hcan1, .txconf.StdId = 0x2ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[3] = {.can_handle = &hcan2, .txconf.StdId = 0x1ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[4] = {.can_handle = &hcan2, .txconf.StdId = 0x200, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[5] = {.can_handle = &hcan2, .txconf.StdId = 0x2ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
};
#endif
/**
* @brief 6个用于确认是否有电机注册到sender_assignment中的标志位,防止发送空帧,此变量将在DJIMotorControl()使用
* flag的初始化在 MotorSenderGrouping()中进行
*/
static uint8_t sender_enable_flag[15] = {0};
/**
* @brief 根据电调/拨码开关上的ID,根据说明书的默认id分配方式计算发送ID和接收ID,
* 并对电机进行分组以便处理多电机控制命令
*/
static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *config)
{
uint8_t motor_id = config->tx_id - 1; // 下标从零开始,先减一方便赋值
uint8_t motor_send_num;
uint8_t motor_grouping;
uint8_t grouping_offset;
//通过CAN计算分组偏移量
if(config->can_handle == &hcan1)
{
grouping_offset=0;
}
else if(config->can_handle == &hcan2)
{
grouping_offset=3;
}
else
{
grouping_offset=6;
}
switch (motor->motor_type)
{
case M2006:
case M3508:
if (motor_id < 4) // 根据ID分组
{
motor_send_num = motor_id;
motor_grouping = grouping_offset + 1;
}
else
{
motor_send_num = motor_id - 4;
motor_grouping = grouping_offset + 0;
}
// 计算接收id并设置分组发送id
config->rx_id = 0x200 + motor_id + 1; // 把ID+1,进行分组设置
sender_enable_flag[motor_grouping] = 1; // 设置发送标志位,防止发送空帧
motor->message_num = motor_send_num;
motor->sender_group = motor_grouping;
// 检查是否发生id冲突
for (size_t i = 0; i < idx; ++i)
{
if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id)
{
LOGERROR("[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.");
uint16_t can_bus = config->can_handle == &hcan1 ? 1 : 2;
while (1) // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是)
LOGERROR("[dji_motor] id [%d], can_bus [%d]", config->rx_id, can_bus);
}
}
break;
case GM6020:
if (motor_id < 4)
{
motor_send_num = motor_id;
motor_grouping = grouping_offset + 0;
}
else
{
motor_send_num = motor_id - 4;
motor_grouping = grouping_offset + 2;
}
config->rx_id = 0x204 + motor_id + 1; // 把ID+1,进行分组设置
sender_enable_flag[motor_grouping] = 1; // 只要有电机注册到这个分组,置为1;在发送函数中会通过此标志判断是否有电机注册
motor->message_num = motor_send_num;
motor->sender_group = motor_grouping;
for (size_t i = 0; i < idx; ++i)
{
if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id)
{
LOGERROR("[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.");
uint16_t can_bus = config->can_handle == &hcan1 ? 1 : 2;
while (1) // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是)
LOGERROR("[dji_motor] id [%d], can_bus [%d]", config->rx_id, can_bus);
}
}
break;
case GM6020_NEW:
if(config->can_handle == &hcan1)
{
grouping_offset=9;
}
else if(config->can_handle == &hcan2)
{
grouping_offset=11;
}
else
{
grouping_offset=13;
}
if (motor_id < 4)
{
motor_send_num = motor_id;
motor_grouping = grouping_offset + 0;
}
else
{
motor_send_num = motor_id - 4;
motor_grouping = grouping_offset + 1;
}
config->rx_id = 0x204 + motor_id + 1; // 把ID+1,进行分组设置
sender_enable_flag[motor_grouping] = 1; // 只要有电机注册到这个分组,置为1;在发送函数中会通过此标志判断是否有电机注册
motor->message_num = motor_send_num;
motor->sender_group = motor_grouping;
for (size_t i = 0; i < idx; ++i)
{
if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id)
{
LOGERROR("[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.");
uint16_t can_bus = config->can_handle == &hcan1 ? 1 : 2;
while (1) // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是)
LOGERROR("[dji_motor] id [%d], can_bus [%d]", config->rx_id, can_bus);
}
}
break;
default: // other motors should not be registered here
while (1)
LOGERROR("[dji_motor]You must not register other motors using the API of DJI motor."); // 其他电机不应该在这里注册
}
}
/**
* @todo 是否可以简化多圈角度的计算？
* @brief 根据返回的can_instance对反馈报文进行解析
*
* @param _instance 收到数据的instance,通过遍历与所有电机进行对比以选择正确的实例
*/
static void DecodeDJIMotor(CANInstance *_instance)
{
// 这里对can instance的id进行了强制转换,从而获得电机的instance实例地址
// _instance指针指向的id是对应电机instance的地址,通过强制转换为电机instance的指针,再通过->运算符访问电机的成员motor_measure,最后取地址获得指针
uint8_t *rxbuff = _instance->rx_buff;
DJIMotorInstance *motor = (DJIMotorInstance *)_instance->id;
DJI_Motor_Measure_s *measure = &motor->measure; // measure要多次使用,保存指针减小访存开销
DaemonReload(motor->daemon);
motor->dt = DWT_GetDeltaT(&motor->feed_cnt);
// 解析数据并对电流和速度进行滤波,电机的反馈报文具体格式见电机说明手册
measure->last_ecd = measure->ecd;
measure->ecd = ((uint16_t)rxbuff[0]) << 8 | rxbuff[1];
measure->angle_single_round = ECD_ANGLE_COEF_DJI * (float)measure->ecd;
measure->speed_aps = (1.0f - SPEED_SMOOTH_COEF) * measure->speed_aps +
RPM_2_ANGLE_PER_SEC * SPEED_SMOOTH_COEF * (float)((int16_t)(rxbuff[2] << 8 | rxbuff[3]));
measure->real_current = (1.0f - CURRENT_SMOOTH_COEF) * measure->real_current +
CURRENT_SMOOTH_COEF * (float)((int16_t)(rxbuff[4] << 8 | rxbuff[5]));
measure->temperature = rxbuff[6];
// 多圈角度计算,前提是假设两次采样间电机转过的角度小于180°,自己画个图就清楚计算过程了
if (measure->ecd - measure->last_ecd > 4096)
measure->total_round--;
else if (measure->ecd - measure->last_ecd < -4096)
measure->total_round++;
measure->total_angle = measure->total_round * 360 + measure->angle_single_round;
}
static void DJIMotorLostCallback(void *motor_ptr)
{
DJIMotorInstance *motor = (DJIMotorInstance *)motor_ptr;
uint16_t can_bus = motor->motor_can_instance->can_handle == &hcan1 ? 1 : 2;
LOGWARNING("[dji_motor] Motor lost, can bus [%d] , id [%d]", can_bus, motor->motor_can_instance->tx_id);
}
// 电机初始化,返回一个电机实例
DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config)
{
DJIMotorInstance *instance = (DJIMotorInstance *)malloc(sizeof(DJIMotorInstance));
memset(instance, 0, sizeof(DJIMotorInstance));
// motor basic setting 电机基本设置
instance->motor_type = config->motor_type; // 6020 or 2006 or 3508
instance->motor_settings = config->controller_setting_init_config; // 正反转,闭环类型等
// motor controller init 电机控制器初始化
PIDInit(&instance->motor_controller.current_PID, &config->controller_param_init_config.current_PID);
PIDInit(&instance->motor_controller.speed_PID, &config->controller_param_init_config.speed_PID);
PIDInit(&instance->motor_controller.angle_PID, &config->controller_param_init_config.angle_PID);
instance->motor_controller.other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
instance->motor_controller.other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
instance->motor_controller.current_feedforward_ptr = config->controller_param_init_config.current_feedforward_ptr;
instance->motor_controller.speed_feedforward_ptr = config->controller_param_init_config.speed_feedforward_ptr;
// 后续增加电机前馈控制器(速度和电流)
// 电机分组,因为至多4个电机可以共用一帧CAN控制报文
MotorSenderGrouping(instance, &config->can_init_config);
// 注册电机到CAN总线
config->can_init_config.can_module_callback = DecodeDJIMotor; // set callback
config->can_init_config.id = instance; // set id,eq to address(it is identity)
instance->motor_can_instance = CANRegister(&config->can_init_config);
// 注册守护线程
Daemon_Init_Config_s daemon_config = {
.callback = DJIMotorLostCallback,
.owner_id = instance,
.reload_count = 2, // 20ms未收到数据则丢失
};
instance->daemon = DaemonRegister(&daemon_config);
DJIMotorEnable(instance);
dji_motor_instance[idx++] = instance;
return instance;
}
/* 电流只能通过电机自带传感器监测,后续考虑加入力矩传感器应变片等 */
void DJIMotorChangeFeed(DJIMotorInstance *motor, Closeloop_Type_e loop, Feedback_Source_e type)
{
if (loop == ANGLE_LOOP)
motor->motor_settings.angle_feedback_source = type;
else if (loop == SPEED_LOOP)
motor->motor_settings.speed_feedback_source = type;
else
LOGERROR("[dji_motor] loop type error, check memory access and func param"); // 检查是否传入了正确的LOOP类型,或发生了指针越界
}
void DJIMotorStop(DJIMotorInstance *motor)
{
motor->motor_controller.speed_PID.Iout = 0; // 停止时清除速度环积分项
motor->motor_controller.angle_PID.Iout = 0; // 停止时清除位置环积分项
motor->motor_controller.current_PID.Iout = 0; // 停止时清
motor->stop_flag = MOTOR_STOP;
}
void DJIMotorEnable(DJIMotorInstance *motor)
{
motor->stop_flag = MOTOR_ENALBED;
}
/* 修改电机的实际闭环对象 */
void DJIMotorOuterLoop(DJIMotorInstance *motor, Closeloop_Type_e outer_loop)
{
motor->motor_settings.outer_loop_type = outer_loop;
}
// 设置参考值
void DJIMotorSetRef(DJIMotorInstance *motor, float ref)
{
motor->motor_controller.pid_ref = ref;
}
void DJIMotorSetExternal(DJIMotorInstance *motor, float external)
{
motor->motor_controller.output_external = external;
}
// 为所有电机实例计算三环PID,发送控制报文
void DJIMotorControl()
{
// 直接保存一次指针引用从而减小访存的开销,同样可以提高可读性
uint8_t group, num; // 电机组号和组内编号
int16_t set; // 电机控制CAN发送设定值
DJIMotorInstance *motor;
Motor_Control_Setting_s *motor_setting; // 电机控制参数
Motor_Controller_s *motor_controller; // 电机控制器
DJI_Motor_Measure_s *measure; // 电机测量值
float pid_measure, pid_ref, output_external; // 电机PID测量值和设定值
// 遍历所有电机实例,进行串级PID的计算并设置发送报文的值
for (size_t i = 0; i < idx; ++i)
{ // 减小访存开销,先保存指针引用
motor = dji_motor_instance[i];
motor_setting = &motor->motor_settings;
motor_controller = &motor->motor_controller;
measure = &motor->measure;
pid_ref = motor_controller->pid_ref; // 保存设定值,防止motor_controller->pid_ref在计算过程中被修改
output_external = motor_controller->output_external;
if (motor_setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
pid_ref *= -1; // 设置反转
// pid_ref会顺次通过被启用的闭环充当数据的载体
// 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出
if ((motor_setting->close_loop_type & ANGLE_LOOP) && motor_setting->outer_loop_type == ANGLE_LOOP)
{
if (motor_setting->angle_feedback_source == OTHER_FEED)
pid_measure = *motor_controller->other_angle_feedback_ptr;
else
pid_measure = measure->total_angle; // MOTOR_FEED,对total angle闭环,防止在边界处出现突跃
// 更新pid_ref进入下一个环
pid_ref = PIDCalculate(&motor_controller->angle_PID, pid_measure, pid_ref);
}
// 计算速度环,(外层闭环为速度或位置)且(启用速度环)时会计算速度环
if ((motor_setting->close_loop_type & SPEED_LOOP) && (motor_setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP)))
{
if (motor_setting->feedforward_flag & SPEED_FEEDFORWARD)
pid_ref += *motor_controller->speed_feedforward_ptr;
if (motor_setting->speed_feedback_source == OTHER_FEED)
pid_measure = *motor_controller->other_speed_feedback_ptr;
else // MOTOR_FEED
pid_measure = measure->speed_aps;
// 更新pid_ref进入下一个环
pid_ref = PIDCalculate(&motor_controller->speed_PID, pid_measure, pid_ref);
}
// 计算电流环,目前只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身传感器的反馈
if (motor_setting->feedforward_flag & CURRENT_FEEDFORWARD)
pid_ref += *motor_controller->current_feedforward_ptr;
if (motor_setting->close_loop_type & CURRENT_LOOP)
{
pid_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref);
}
if (motor_setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE)
pid_ref *= -1;
// 获取最终输出
if (motor->motor_settings.send_mode == SEND_MODE_NORMAL)
{
set = (int16_t)pid_ref;
}
else
{
set = (int16_t)output_external;
}
// 分组填入发送数据
group = motor->sender_group;
num = motor->message_num;
sender_assignment[group].tx_buff[2 * num] = (uint8_t)(set >> 8); // 低八位
sender_assignment[group].tx_buff[2 * num + 1] = (uint8_t)(set & 0x00ff); // 高八位
// 若该电机处于停止状态,直接将buff置零
if (motor->stop_flag == MOTOR_STOP)
memset(sender_assignment[group].tx_buff + 2 * num, 0, 16u);
}
// 遍历flag,检查是否要发送这一帧报文
#ifdef FDCAN
for (size_t i = 0; i < 15; ++i)
#else
for (size_t i = 0; i < 6; ++i)
#endif
{
if (sender_enable_flag[i])
{
CANTransmit(&sender_assignment[i], 1);
}
}
}
modules/motor/DJImotor/dji_motor.h
+133
View File
@@ -0,0 +1,133 @@
/**
* @file dji_motor.h
* @author neozng
* @brief DJI智能电机头文件
* @version 0.2
* @date 2022-11-01
*
* @todo 1. 给不同的电机设置不同的低通滤波器惯性系数而不是统一使用宏
2. 为M2006和M3508增加开环的零位校准函数,并在初始化时调用(根据用户配置决定是否调用)
* @copyright Copyright (c) 2022 HNU YueLu EC all rights reserved
*
*/
#ifndef DJI_MOTOR_H
#define DJI_MOTOR_H
#include "bsp_can.h"
#include "controller.h"
#include "motor_def.h"
#include "stdint.h"
#include "daemon.h"
#define DJI_MOTOR_CNT 12
/* 滤波系数设置为1的时候即关闭滤波 */
#define SPEED_SMOOTH_COEF 0.85f // 最好大于0.85
#define CURRENT_SMOOTH_COEF 0.9f // 必须大于0.9
#define ECD_ANGLE_COEF_DJI 0.043945f // (360/8192),将编码器值转化为角度制
/* DJI电机CAN反馈信息*/
typedef struct
{
uint16_t last_ecd; // 上一次读取的编码器值
uint16_t ecd; // 0-8191,刻度总共有8192格
float angle_single_round; // 单圈角度
float speed_aps; // 角速度,单位为:度/秒
int16_t real_current; // 实际电流
uint8_t temperature; // 温度 Celsius
float total_angle; // 总角度,注意方向
int32_t total_round; // 总圈数,注意方向
} DJI_Motor_Measure_s;
/**
* @brief DJI intelligent motor typedef
*
*/
typedef struct
{
DJI_Motor_Measure_s measure; // 电机测量值
Motor_Control_Setting_s motor_settings; // 电机设置
Motor_Controller_s motor_controller; // 电机控制器
CANInstance *motor_can_instance; // 电机CAN实例
// 分组发送设置
uint8_t sender_group;
uint8_t message_num;
Motor_Type_e motor_type; // 电机类型
Motor_Working_Type_e stop_flag; // 启停标志
DaemonInstance* daemon;
uint32_t feed_cnt;
float dt;
} DJIMotorInstance;
/**
* @brief 调用此函数注册一个DJI智能电机,需要传递较多的初始化参数,请在application初始化的时候调用此函数
* 推荐传参时像标准库一样构造initStructure然后传入此函数.
* recommend: type xxxinitStructure = {.member1=xx,
* .member2=xx,
* ....};
* 请注意不要在一条总线上挂载过多的电机(超过6个),若一定要这么做,请降低每个电机的反馈频率(设为500Hz),
* 并减小DJIMotorControl()任务的运行频率.
*
* @attention M3508和M2006的反馈报文都是0x200+id,而GM6020的反馈是0x204+id,请注意前两者和后者的id不要冲突.
* 如果产生冲突,在初始化电机的时候会进入IDcrash_Handler(),可以通过debug来判断是否出现冲突.
*
* @param config 电机初始化结构体,包含了电机控制设置,电机PID参数设置,电机类型以及电机挂载的CAN设置
*
* @return DJIMotorInstance*
*/
DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config);
/**
* @brief 被application层的应用调用,给电机设定参考值.
* 对于应用,可以将电机视为传递函数为1的设备,不需要关心底层的闭环
*
* @param motor 要设置的电机
* @param ref 设定参考值
*/
void DJIMotorSetRef(DJIMotorInstance *motor, float ref);
/**
* @brief 切换反馈的目标来源,如将角速度和角度的来源换为IMU(小陀螺模式常用)
*
* @param motor 要切换反馈数据来源的电机
* @param loop 要切换反馈数据来源的控制闭环
* @param type 目标反馈模式
*/
void DJIMotorChangeFeed(DJIMotorInstance *motor, Closeloop_Type_e loop, Feedback_Source_e type);
/**
* @brief 该函数被motor_task调用运行在rtos上,motor_stask内通过osDelay()确定控制频率
*/
void DJIMotorControl();
/**
* @brief 停止电机,注意不是将设定值设为零,而是直接给电机发送的电流值置零
*
*/
void DJIMotorStop(DJIMotorInstance *motor);
/**
* @brief 启动电机,此时电机会响应设定值
* 初始化时不需要此函数,因为stop_flag的默认值为0
*
*/
void DJIMotorEnable(DJIMotorInstance *motor);
/**
* @brief 修改电机闭环目标(外层闭环)
*
* @param motor 要修改的电机实例指针
* @param outer_loop 外层闭环类型
*/
void DJIMotorOuterLoop(DJIMotorInstance *motor, Closeloop_Type_e outer_loop);
void DJIMotorSetExternal(DJIMotorInstance *motor, float external);
#endif // !DJI_MOTOR_H
modules/motor/DJImotor/dji_motor.md
+470
View File
@@ -0,0 +1,470 @@
# dji_motor
<p align='right'>neozng1@hnu.edu.cn</p>
> TODO:
>
> 1. 给不同的电机设置不同的低通滤波器惯性系数而不是统一使用宏
> 2. 为M2006和M3508增加开环的零位校准函数
---
> 建议将电机的反馈频率通过RoboMaster Assistant统一设置为500Hz。当前默认的`MotorTask()`执行频率为500Hz，若不修改电机反馈频率可能导致单条总线挂载的电机数量有限，且容易出现帧错误和仲裁失败的情况。
## 总览和封装说明
> 如果你不需要理解该模块的工作原理，你只需要查看这一小节。
dji_motor模块对DJI智能电机，包括M2006，M3508以及GM6020进行了详尽的封装。你不再需要关心PID的计算以及CAN报文的发送和接收解析，你只需要专注于根据应用层的需求，设定合理的期望值，并通过`DJIMotorSetRef()`设置对应电机的输入参考即可。
**==设定值的单位==**
1. ==位置环为**角度制**（0-360，total_angle可以为任意值）==
2. ==速度环为角速度，单位为**度/每秒**（deg/sec）==
3. ==电流环为A==
4. ==GM6020的输入设定为**力矩**，待测量（-30000~30000）==
==M3508的输入设定为-20A~20A （-16384~16384）==
==M2006的输入设定为-10A~10A （-10000~10000）==
如果你希望更改电机的反馈来源，比如进入小陀螺模式/视觉模式（这时候你想要云台保持静止，使用IMU的yaw角度值作为反馈来源），只需要调用`DJIMotorChangeFeed()`，电机便可立刻切换反馈数据来源至IMU。
要获得一个电机，请通过`DJIMotorInit()`并传入一些参数，他就会返回一个电机的指针。你也不再需要查看这些电机和电调的说明书，**只需要设置其电机id**（6020为拨码开关值，2006和3508为电调的闪动次数），该模块会自动为你计算CAN发送和接收ID并搞定所有硬件层的琐事。
初始化电机时，你需要传入的参数包括：
- **电机挂载的CAN总线设置**，CAN1 or CAN2，以及电机的id，使用`can_instance_config_s`封装，只需要设置这两个参数:
```c
CAN_HandleTypeDef *can_handle;
uint32_t tx_id; // tx_id设置为电机id,不需要查说明书计算，直接为电调的闪动次数或拨码开关值，为1-8
```
- **电机类型**，使用`Motor_Type_e`：
```c
GM6020 = 0
M3508 = 1
M2006 = 2
```
- **电机控制设置**
- 闭环类型
```c
OPEN_LOOP
CURRENT_LOOP
SPEED_LOOP
ANGLE_LOOP
CURRENT_LOOP | SPEED_LOOP // 同时对电流和速度闭环
SPEED_LOOP | ANGLE_LOOP // 同时对速度和位置闭环
CURRENT_LOOP | SPEED_LOOP |ANGLE_LOOP // 三环全开
```
- 是否反转
```c
MOTOR_DIRECTION_NORMAL
MOTOR_DIRECTION_REVERSE
```
- 是否其他反馈来源，以及他们对应的数据指针（如果有的话）
```c
MOTOR_FEED = 0
OTHER_FEED = 1
---
// 电流只能从电机传感器获得所以无法设置其他来源
```
- 每个环的PID参数以及是否使用改进功能，以及其他反馈来源指针（如果在上一步启用了其他数据来源）
```c
typedef struct // config parameter
{
float Kp;
float Ki;
float Kd;
float MaxOut; // 输出限幅
// 以下是优化参数
float IntegralLimit; // 积分限幅
float DeadBand; // 死区
float CoefA; // For Changing Integral
float CoefB; // ITerm = Err*((A-abs(err)+B)/A) when B<|err|<A+B
float Output_LPF_RC; // RC = 1/omegac
float Derivative_LPF_RC;
PID_Improvement_e Improve; // 优化环节，定义在下一个代码块
} PIDInit_config_s;
// 只有当你设启用了对应的优化环节，优化参数才会生效
```
```c
typedef enum
{
NONE = 0b00000000,
Integral_Limit = 0b00000001,
Derivative_On_Measurement = 0b00000010,
Trapezoid_Intergral = 0b00000100,
Proportional_On_Measurement = 0b00001000,
OutputFilter = 0b00010000,
ChangingIntegrationRate = 0b00100000,
DerivativeFilter = 0b01000000,
ErrorHandle = 0b10000000,
} PID_Improvement_e;
// 若希望使用多个环节的优化，这样就行：Integral_Limit |Trapezoid_Intergral|...|...
```
```c
float *other_angle_feedback_ptr
float *other_speed_feedback_ptr
```
---
推荐的初始化参数编写格式如下：
```c
Motor_Init_Config_s config = {
.motor_type = M3508, // 要注册的电机为3508电机
.can_init_config = {.can_handle = &hcan1, // 挂载在CAN1
.tx_id = 1}, // C620每隔一段时间闪动1次,设置为1
// 采用电机编码器角度与速度反馈,启用速度环和电流环,不反转,最外层闭环为速度环
.controller_setting_init_config = {.angle_feedback_source = MOTOR_FEED,
.outer_loop_type = SPEED_LOOP,
.close_loop_type = SPEED_LOOP | CURRENT_LOOP,
.speed_feedback_source = MOTOR_FEED,
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL},
// 电流环和速度环PID参数的设置,不采用计算优化则不需要传入Improve参数
// 不使用其他数据来源(如IMU),不需要传入反馈数据变量指针
.controller_param_init_config = {.current_PID = {.Improve = 0,
.Kp = 1,
.Ki = 0,
.Kd = 0,
.DeadBand = 0,
.MaxOut = 4000},
.speed_PID = {.Improve = 0,
.Kp = 1,
.Ki = 0,
.Kd = 0,
.DeadBand = 0,
.MaxOut = 4000}}};
dji_motor_instance *djimotor = DJIMotorInit(config); // 设置好参数后进行初始化并保留返回的指针
```
---
要控制一个DJI电机，我们提供了2个接口：
```c
void DJIMotorSetRef(dji_motor_instance *motor, float ref);
void DJIMotorChangeFeed(dji_motor_instance *motor,
Closeloop_Type_e loop,
Feedback_Source_e type);
```
调用第一个并传入设定值，它会自动根据你设定的PID参数进行动作。 如果对不同闭环都有参考输入,则设置最外层的闭环(通过此函数)并将剩下的参考输入通过前馈数据指针进行设定
调用第二个并设定要修改的反馈环节和反馈类型，它会将反馈数据指针切换到你设定好的变量（需要在初始化的时候设置反馈指针）。
**如果需要获取电机的反馈数据**（如小陀螺模式需要根据麦克纳姆轮逆运动学解算底盘速度），直接通过你拥有的`dji_motor_instance`访问成员变量：
```c
// LeftForwardMotor是一个dji_motor_instance实例
float speed=LeftForwardMotor->motor_measure->speed_rpm;
...
```
***现在，忘记PID的计算和发送、接收以及协议解析，专注于模块之间的逻辑交互吧。***
---
## 代码结构
.h文件内包括了外部接口和类型定义,以及模块对应的宏。c文件内为私有函数和外部接口的定义。
motor_def.h内包含了一些电机通用的定义。
## 类型定义
```c
#define DJI_MOTOR_CNT 12
#define SPEED_SMOOTH_COEF 0.9f // better to be greater than 0.85
#define CURRENT_SMOOTH_COEF 0.98f // this coef must be greater than 0.95
typedef struct /* DJI电机CAN反馈信息*/
{
uint16_t ecd;
uint16_t last_ecd;
int16_t speed_rpm;
int16_t given_current;
uint8_t temperate;
int16_t total_round;
int32_t total_angle;
} dji_motor_measure;
typedef struct
{
/* motor measurement recv from CAN feedback */
dji_motor_measure motor_measure;
/* basic config of a motor*/
Motor_Control_Setting_s motor_settings;
/* controller used in the motor (3 loops)*/
Motor_Controller_s motor_controller;
/* the CAN instance own by motor instance*/
can_instance motor_can_instance;
/* sender assigment*/
uint8_t sender_group;
uint8_t message_num;
uint8_t stop_flag;
Motor_Type_e motor_type;
} dji_motor_instance;
```
- `DJI_MOTOR_CNT`是允许的最大DJI电机数量，根据经验，暂定为每个CAN6个，防止出现拥塞。
- `SPEED_SMOOTH_COEF`和`CURRENT_SMOOTH_COEF`是电机反馈的电流和速度数据低通滤波器惯性系数，数值越小平滑效果越大，但滞后也越大。设定时不应当低于推荐值。
- `dji_motor_measure`是DJI电机的反馈信息，包括当前编码器值、上次测量编码器值、速度、电流、温度、总圈数和单圈角度。
- `Motor_Control_Setting_s`的定义在`motor_def.h`之中，它和`Motor_Controller_s`都是所有电机通用的组件（如M3508，LK9025，HT04，MT6023等），其包含内容如下：
```c
typedef struct /* 电机控制配置 */
{
Closeloop_Type_e outer_loop_type;
Closeloop_Type_e close_loop_type;
Motor_Reverse_Flag_e motor_reverse_flag;
Feedback_Source_e angle_feedback_source;
Feedback_Source_e speed_feedback_source;
} Motor_Control_Setting_s;
```
`Motor_Control_Setting_s`里包含了电机的闭环类型，反转标志以及额外的反馈来源标志。
- 闭环类型指示该电机使用的控制器配置，其枚举定义如下：
```c
typedef enum
{
CURRENT_LOOP = 0b0001,
SPEED_LOOP = 0b0010,
ANGLE_LOOP = 0b0100,
_ = 0b0011,
__ = 0b0110,
___ = 0b0111
} Closeloop_Type_e;
```
以M3508为例，假设需要进行**速度闭环**和**电流闭环**，那么在初始化时就将这个变量的值设为`CURRENT_LOOP | SPEED_LOOP`。在`DJIMotorControl()`中，函数将会根据此标志位判断设定的参考值需要经过那些控制器的计算。
另外,你还需要设置当前电机的最外层闭环，即电机的闭环目标为什么类型的值。初始化时需要设置`outer_loop_type`。以M2006作为拨盘电机时为例，你希望它在单发/双发等固定发射数量的模式下对位置进行闭环（拨盘转过一定角度对应拨出一颗弹丸），但你也有可能希望在连发的时候让拨盘连续的转动，以一定的频率发射弹丸。我们提供了`DJIMotorOuterLoop()`用于修改电机的外层闭环，改变电机的闭环对象。
> 注意，务必分清串级控制（多环）和外层闭环的区别。前者是为了提高内环的性能，使得其能更好地跟随外环参考值；而后者描述的是系统真实的控制目标（闭环目标）。如3508，没有电流环仍然可以对速度完成闭环，对于高层的应用来说，它们本质上不关心电机内部是否还有电流环，它们只把外层闭环为速度的电机当作一个**速度伺服执行器**，**外层闭环**描述的就是真正的闭环目标。
- 为了避开恼人的正负号，提高代码的可维护性，在初始化电机时设定`motor_reverse_flag`使得所有电机都按照你想要的方向旋转，其定义如下：
```c
typedef enum
{
MOTOR_DIRECTION_NORMAL = 0,
MOTOR_DIRECTION_REVERSE = 1
} Motor_Reverse_Flag_e;
```
- `speed_feedback_source`以及`angle_feedback_source`是指示电机反馈来源的标志位。一般情况下电机使用自身的编码器作为控制反馈量。但在某些时候，如小陀螺模式，云台电机会使用IMU的姿态数据作为反馈数据来源。其定义如下：
```c
typedef enum
{
MOTOR_FEED = 0,
OTHER_FEED = 1
} Feedback_Source_e;
```
**注意，如果启用其他数据来源，你需要在电机的控制器配置`Motor_Controller_s`下的`other_xxx_feedback_ptr`中指定其他数据来源。**
你可以在`DJIMotorChangeFeed()`中修改电机的数据来源。
- `Motor_Controller_s`的定义也在`motor_def.h`之中：
```c
/* 电机控制器,包括其他来源的反馈数据指针,3环控制器和电机的参考输入*/
typedef struct
{
float *other_angle_feedback_ptr;
float *other_speed_feedback_ptr;
PID_t current_PID;
PID_t speed_PID;
PID_t angle_PID;
float pid_ref; // 将会作为每个环的输入和输出顺次通过串级闭环
} Motor_Controller_s;
```
两个`float*`指针应当指向其他反馈来源数据（如果有的话，需要在`motor_settings`中设定）。
三个PID分别为三个控制闭环所用，在`DJIMotorControl()`中，该函数会根据`close_loop_type`的设定计算对应的闭环。
**`pid_ref`是控制的设定值，app层的应用想要更改电机的输出，就要调用`DJIMotorSetRef()`更改此值。**
- `dji_motor_instance`是一个DJI电机实例。一个电机实例内包含电机的反馈信息，电机的控制设置，电机控制器，电机对应的CAN实例以及电机的类型；由于DJI电机支持**一帧报文控制至多4个电机**，该结构体还包含了用于给电机分组发送进行特殊处理的`sender_group`和`message_num`（具体实现细节参考`MotorSenderGrouping()`函数）。
## 外部接口
```c
dji_motor_instance *DJIMotorInit(can_instance_config config,
Motor_Control_Setting_s motor_setting,
Motor_Controller_Init_s controller_init,
Motor_Type_e type);
void DJIMotorSetRef(dji_motor_instance *motor, float ref);
void DJIMotorChangeFeed(dji_motor_instance *motor,
Closeloop_Type_e loop,
Feedback_Source_e type);
void DJIMotorControl();
void DJIMotorStop(dji_motor_instance *motor);
void DJIMotorEnable(dji_motor_instance *motor);
void DJIMotorOuterLoop(dji_motor_instance *motor);
```
- `DJIMotorInit()`是用于初始化电机对象的接口，传入包括电机can配置、电机控制配置、电机控制器配置以及电机类型在内的初始化参数。**它将会返回一个电机实例指针**，你应当在应用层保存这个指针，这样才能操控这个电机。
- `DJIMotorSetRef()`是设定电机输出的接口，**在调用这个函数的时候，你可以认为你的设定值会直接转变为电机的输出**。`DJIMotorControl()`会帮你完成闭环计算，不用担心PID。
- `DJIMotorChangeFeed()`一般在更改云台或底盘的运动模式的时候被调用，传入要修改反馈来源的电机实例指针、要修改的闭环以及反馈来源类型。如希望切换到IMU的yaw值作为云台设定值，传入yaw轴电机实例和`ANGLE_LOOP`（位置环）、`OTHER_FEED`（启用其他数据来源）即可。当然，你需要在初始化的时候设定`motor_controller`中的 `other_angle_feedback_ptr`，使其指向yaw值的变量。
- `DJIMotorControl()`是根据电机的配置计算控制值的函数。该函数在`motor_task.c`中被调用，应当在freeRTOS中以一定频率运行。此函数为PID的计算进行了彻底的封装，要修改电机的参考输入，请在app层的应用中调用`DJIMotorSetRef()`。
该函数的具体实现请参照代码，注释已经较为清晰。流程大致为：
1. 根据电机的初始化控制配置，计算各个控制闭环
2. 根据反转标志位，确定是否将输出反转
3. 根据每个电机的发送分组，将最终输出值填入对应的分组buff
4. 检查每一个分组，若该分组有电机，发送报文
- `DJIMotorStop()`和`DJIMotorEnable()`用于控制电机的启动和停止。当电机被设为stop的时候，不会响应任何的参考输入。
- `DJIMotorOuterLoop()`用于修改电机的外部闭环类型，即电机的真实闭环目标。
## 私有函数和变量
在.c文件内设为static的函数和变量
```c
static uint8_t idx = 0; // register idx,是该文件的全局电机索引,在注册时使用
static dji_motor_instance *dji_motor_info[DJI_MOTOR_CNT] = {NULL};
```
这是管理所有电机实例的入口。idx用于电机初始化。
```c
#define PI2 (3.141592f * 2)
#define ECD_ANGLE_COEF_DJI 3.835e-4 // ecd/8192*pi
```
这两个宏用于在电机反馈信息中的多圈角度计算，将编码器的0~8192转化为角度表示。
```c
/* @brief 由于DJI电机发送以四个一组的形式进行,故对其进行特殊处理,用6个(2can*3group)can_instance专门负责发送
* 该变量将在 DJIMotorControl() 中使用,分组在 MotorSenderGrouping()中进行
*
* can1: [0]:0x1FF,[1]:0x200,[2]:0x2FF
* can2: [0]:0x1FF,[1]:0x200,[2]:0x2FF */
static can_instance sender_assignment[6] =
{
[0] = {.can_handle = &hcan1, .txconf.StdId = 0x1ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
...
...
};
static uint8_t sender_enable_flag[6] = {0};
```
- 这些是电机分组发送所需的变量。注册电机时，会根据挂载的总线以及发送id，将电机分组。在CAN发送电机控制信息的时候，根据`sender_assignment[]`保存的分组进行发送，而不会使用电机实例自带的`can_instance`。
- DJI电机共有3种分组，分别为0x1FF,0x200,0x2FF。注册电机的时候，`MotorSenderGrouping()`函数会根据发送id计算出CAN的`tx_id`（即上述三个中的一个）和`rx_id`。然后为电机实例分配用于指示其在`sender_assignment[]`中的编号的 `sender_group`和其在该发送组中的位置`message_num`（一帧报文可以发送四条控制指令，`message_num`会指定电机是这四个中的哪一个）。具体的分配请查看`MotorSenderGrouping()`的定义。
- 当某一个分组有电机注册时，该分组的索引将会在`sender_enable_flag`[]中被置1，这样，就可以避免发送没有电机注册的报文，防止总线拥塞。具体的，在`DecodeDJIMotor()`中，该函数会查看`sender_enable_flag[]`的每一个位置，确定这一组是否有电机被注册，若有则发送`sender_assignment[]`中对应位置的`tx_buff`。
```c
static void IDcrash_Handler(uint8_t conflict_motor_idx, uint8_t temp_motor_idx)
static void MotorSenderGrouping(can_instance_config *config)
static void DecodeDJIMotor(can_instance *_instance)
```
- `IDcrash_Handler()`在电机id发生冲突的时候会被`MotorSenderGrouping()`调用，陷入死循环之中，并把冲突的id保存在函数里。这样就可以通过debug确定是否发生冲突以及冲突的编号。
- `MotorSenderGrouping()`被`DJIMotorInit()`调用，他将会根据电机id计算出CAN的发送和接收ID，并根据发送ID对电机进行分组。
- `DecodeDJIMotor()`是解析电机反馈报文的函数，在`DJIMotorInit()`中会将其注册到该电机实例对应的`can_instance`中（即`can_instance`的`can_module_callback()`）。这样，当该电机的反馈报文到达时，`bsp_can.c`中的回调函数会调用解包函数进行反馈数据解析。
该函数还会对电流和速度反馈值进行滤波，消除高频噪声；同时计算多圈角度和单圈绝对角度。
**电机反馈的电流值为说明书中的映射值，需转换为实际值。**
**反馈的速度单位是rpm（转每分钟），转换为角度每秒。**
**反馈的位置是编码器值（0~8191），转换为角度。**
## 使用范例
```c
//初始化设置
Motor_Init_Config_s config = {
.motor_type = GM6020,
.can_init_config = {
.can_handle = &hcan1,
.tx_id = 6
},
.controller_setting_init_config = {
.angle_feedback_source = MOTOR_FEED,
.outer_loop_type = SPEED_LOOP,
.close_loop_type = SPEED_LOOP | ANGLE_LOOP,
.speed_feedback_source = MOTOR_FEED,
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL
},
.controller_param_init_config = {
.angle_PID = {
.Improve = 0,
.Kp = 1,
.Ki = 0,
.Kd = 0,
.DeadBand = 0,
.MaxOut = 4000},
.speed_PID = {
.Improve = 0,
.Kp = 1,
.Ki = 0,
.Kd = 0,
.DeadBand = 0,
.MaxOut = 4000
}
}
};
//注册电机并保存实例指针
dji_motor_instance *djimotor = DJIMotorInit(&config);
```
然后在任务中修改电机设定值即可实现控制：
```
DJIMotorSetRef(djimotor, 10);
```
前提是已经将`DJIMotorControl()`放入实时系统任务当中或以一定d。你也可以单独执行`DJIMotorControl()`。
modules/motor/DMmotor/dmmotor.c
+232
View File
@@ -0,0 +1,232 @@
#include "dmmotor.h"
#include "memory.h"
#include "general_def.h"
#include "user_lib.h"
#include "cmsis_os.h"
#include "string.h"
#include "daemon.h"
#include "stdlib.h"
#include "bsp_log.h"
static uint8_t idx;
static DMMotorInstance *dm_motor_instance[DM_MOTOR_CNT];
static osThreadId dm_task_handle[DM_MOTOR_CNT];
/* 两个用于将uint值和float值进行映射的函数,在设定发送值和解析反馈值时使用 */
static uint16_t float_to_uint(float x, float x_min, float x_max, uint8_t bits)
{
float span = x_max - x_min;
float offset = x_min;
return (uint16_t)((x - offset) * ((float)((1 << bits) - 1)) / span);
}
static float uint_to_float(int x_int, float x_min, float x_max, int bits)
{
float span = x_max - x_min;
float offset = x_min;
return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset;
}
void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor)
{
memset(motor->motor_can_instace->tx_buff, 0xff, 7); // 发送电机指令的时候前面7bytes都是0xff
motor->motor_can_instace->tx_buff[7] = (uint8_t)cmd; // 最后一位是命令id
CANTransmit(motor->motor_can_instace, 1);
}
static void DMMotorDecode(CANInstance *motor_can)
{
uint16_t tmp; // 用于暂存解析值,稍后转换成float数据,避免多次创建临时变量
uint8_t *rxbuff = motor_can->rx_buff;
DMMotorInstance *motor = (DMMotorInstance *)motor_can->id;
DM_Motor_Measure_s *measure = &(motor->measure); // 将can实例中保存的id转换成电机实例的指针
DaemonReload(motor->motor_daemon);
measure->id = rxbuff[0] & 0x0F; // 低 4 位是控制器 ID
measure->state = (rxbuff[0] >> 4) & 0x0F; // ERR 存储在 state 字段
measure->last_position = measure->position;
tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]);
measure->position = uint_to_float(tmp, DM_P_MIN, DM_P_MAX, 16);
tmp = (uint16_t)((rxbuff[3] << 4) | rxbuff[4] >> 4);
measure->velocity = uint_to_float(tmp, DM_V_MIN, DM_V_MAX, 12);
tmp = (uint16_t)(((rxbuff[4] & 0x0f) << 8) | rxbuff[5]);
measure->torque = uint_to_float(tmp, DM_T_MIN, DM_T_MAX, 12);
measure->T_Mos = (float)rxbuff[6];
measure->T_Rotor = (float)rxbuff[7];
// 多圈角度计算
if (measure->position - measure->last_position > 12.5)
measure->total_round--;
else if (measure->position - measure->last_position < -12.5)
measure->total_round++;
measure->total_angle = measure->position + measure->total_round * 25;
measure->total_angle = measure->total_angle * RAD_2_DEGREE;
measure->velocity = measure->velocity * RAD_2_DEGREE;
}
static void DMMotorLostCallback(void *motor_ptr)
{
DMMotorInstance *motor = (DMMotorInstance *)motor_ptr;
uint16_t can_bus = motor->motor_can_instace->can_handle == &hcan1 ? 1 : 2;
LOGWARNING("[DMmotor] Motor lost, can bus [%d] , id [%d]", can_bus, motor->motor_can_instace->tx_id);
}
void DMMotorCaliEncoder(DMMotorInstance *motor)
{
DMMotorSetMode(DM_CMD_ZERO_POSITION, motor);
DWT_Delay(0.1);
}
DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config)
{
DMMotorInstance *motor = (DMMotorInstance *)malloc(sizeof(DMMotorInstance));
memset(motor, 0, sizeof(DMMotorInstance));
motor->motor_settings = config->controller_setting_init_config;
PIDInit(&motor->current_PID, &config->controller_param_init_config.current_PID);
PIDInit(&motor->speed_PID, &config->controller_param_init_config.speed_PID);
PIDInit(&motor->angle_PID, &config->controller_param_init_config.angle_PID);
motor->other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
motor->other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
config->can_init_config.can_module_callback = DMMotorDecode;
config->can_init_config.id = motor;
motor->motor_can_instace = CANRegister(&config->can_init_config);
Daemon_Init_Config_s conf = {
.callback = DMMotorLostCallback,
.owner_id = motor,
.reload_count = 10,
};
motor->motor_daemon = DaemonRegister(&conf);
DMMotorEnable(motor);
DMMotorSetMode(DM_CMD_MOTOR_MODE, motor);
// DWT_Delay(0.1);
// DMMotorCaliEncoder(motor);
// DWT_Delay(0.1);
dm_motor_instance[idx++] = motor;
return motor;
}
void DMMotorSetRef(DMMotorInstance *motor, float ref)
{
motor->pid_ref = ref;
}
void DMMotorEnable(DMMotorInstance *motor)
{
motor->stop_flag = MOTOR_ENALBED;
}
void DMMotorStop(DMMotorInstance *motor) // 不使用使能模式是因为需要收到反馈
{
motor->stop_flag = MOTOR_STOP;
}
void DMMotorOuterLoop(DMMotorInstance *motor, Closeloop_Type_e type)
{
motor->motor_settings.outer_loop_type = type;
}
//@Todo: 目前只实现了力控，更多位控PID等请自行添加
void DMMotorTask(void const *argument)
{
float pid_ref, set;
DMMotorInstance *motor = (DMMotorInstance *)argument;
DM_Motor_Measure_s *measure = &motor->measure;
Motor_Control_Setting_s *setting = &motor->motor_settings;
// CANInstance *motor_can = motor->motor_can_instace;
// uint16_t tmp;
DMMotor_Send_s motor_send_mailbox;
float pid_measure; // 电机PID测量值和设定值
while (1)
{
if(motor->measure.state == 0x00)
{
DMMotorSetMode(DM_CMD_MOTOR_MODE, motor);
osDelay(20);
}
if(motor->measure.state != 0x00&&motor->measure.state != 0x01)
{
DMMotorSetMode(DM_CMD_CLEAR_ERROR, motor);
osDelay(20);
DMMotorSetMode(DM_CMD_MOTOR_MODE, motor);
osDelay(20);
}
pid_ref = motor->pid_ref;
if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
pid_ref *= -1; // 设置反转
if ((setting->close_loop_type & ANGLE_LOOP) && setting->outer_loop_type == ANGLE_LOOP)
{
if (setting->angle_feedback_source == OTHER_FEED)
{
pid_measure = *motor->other_angle_feedback_ptr;
// char buff[20];
// Float2Str(buff, pid_measure);
// LOGINFO("pid_measure: %s", buff);
}
else
pid_measure = measure->total_angle;
// 更新pid_ref进入下一个环
pid_ref = PIDCalculate(&motor->angle_PID, pid_measure, pid_ref);
}
if ((setting->close_loop_type & SPEED_LOOP) && (setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP)))
{
if (setting->feedforward_flag & SPEED_FEEDFORWARD)
pid_ref += *motor->speed_feedforward_ptr;
if (setting->speed_feedback_source == OTHER_FEED)
pid_measure = *motor->other_speed_feedback_ptr;
else // MOTOR_FEED
pid_measure = measure->velocity;
// 更新pid_ref进入下一个环
pid_ref = PIDCalculate(&motor->speed_PID, pid_measure, pid_ref);
}
if (setting->feedforward_flag == 1)
pid_ref += 0.8;
if (setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE)
pid_ref *= -1;
set = pid_ref;
LIMIT_MIN_MAX(set, DM_T_MIN, DM_T_MAX);
motor_send_mailbox.position_des = float_to_uint(0, DM_P_MIN, DM_P_MAX, 16);
motor_send_mailbox.velocity_des = float_to_uint(0, DM_V_MIN, DM_V_MAX, 12);
motor_send_mailbox.torque_des = float_to_uint(set, DM_T_MIN, DM_T_MAX, 12);
motor_send_mailbox.Kp = 0;
motor_send_mailbox.Kd = 0;
if (motor->stop_flag == MOTOR_STOP)
motor_send_mailbox.torque_des = float_to_uint(0, DM_T_MIN, DM_T_MAX, 12);
motor->motor_can_instace->tx_buff[0] = (uint8_t)(motor_send_mailbox.position_des >> 8);
motor->motor_can_instace->tx_buff[1] = (uint8_t)(motor_send_mailbox.position_des);
motor->motor_can_instace->tx_buff[2] = (uint8_t)(motor_send_mailbox.velocity_des >> 4);
motor->motor_can_instace->tx_buff[3] = (uint8_t)(((motor_send_mailbox.velocity_des & 0xF) << 4) | (motor_send_mailbox.Kp >> 8));
motor->motor_can_instace->tx_buff[4] = (uint8_t)(motor_send_mailbox.Kp);
motor->motor_can_instace->tx_buff[5] = (uint8_t)(motor_send_mailbox.Kd >> 4);
motor->motor_can_instace->tx_buff[6] = (uint8_t)(((motor_send_mailbox.Kd & 0xF) << 4) | (motor_send_mailbox.torque_des >> 8));
motor->motor_can_instace->tx_buff[7] = (uint8_t)(motor_send_mailbox.torque_des);
CANTransmit(motor->motor_can_instace, 1);
osDelay(1);
}
}
void DMMotorControlInit()
{
char dm_task_name[5] = "dm";
// 遍历所有电机实例,创建任务
if (!idx)
return;
for (size_t i = 0; i < idx; i++)
{
char dm_id_buff[2] = {0};
__itoa(i, dm_id_buff, 10);
strcat(dm_task_name, dm_id_buff);
osThreadDef(dm_task_name, DMMotorTask, osPriorityNormal, 0, 128);
dm_task_handle[i] = osThreadCreate(osThread(dm_task_name), dm_motor_instance[i]);
}
}
modules/motor/DMmotor/dmmotor.h
+80
View File
@@ -0,0 +1,80 @@
#ifndef DMMOTOR_H
#define DMMOTOR_H
#include <stdint.h>
#include "bsp_can.h"
#include "controller.h"
#include "motor_def.h"
#include "daemon.h"
#define DM_MOTOR_CNT 4
#define DM_P_MIN (-12.56637f)
#define DM_P_MAX 12.56637f
#define DM_V_MIN (-45.0f)
#define DM_V_MAX 45.0f
#define DM_T_MIN (-18.0f)
#define DM_T_MAX 18.0f
typedef struct
{
uint8_t id;
uint8_t state;
float velocity;
float last_position;
float position;
float torque;
float T_Mos;
float T_Rotor;
int32_t total_round;
float total_angle;
}DM_Motor_Measure_s;
typedef struct
{
uint16_t position_des;
uint16_t velocity_des;
uint16_t torque_des;
uint16_t Kp;
uint16_t Kd;
}DMMotor_Send_s;
typedef struct
{
DM_Motor_Measure_s measure;
Motor_Control_Setting_s motor_settings;
PIDInstance current_PID;
PIDInstance speed_PID;
PIDInstance angle_PID;
float *other_angle_feedback_ptr;
float *other_speed_feedback_ptr;
float *speed_feedforward_ptr;
float *current_feedforward_ptr;
float pid_ref;
Motor_Working_Type_e stop_flag;
CANInstance *motor_can_instace;
DaemonInstance* motor_daemon;
uint32_t lost_cnt;
uint8_t lost_status;
uint8_t last_lost_status;
}DMMotorInstance;
typedef enum
{
DM_CMD_MOTOR_MODE = 0xfc, // 使能,会响应指令
DM_CMD_RESET_MODE = 0xfd, // 停止
DM_CMD_ZERO_POSITION = 0xfe, // 将当前的位置设置为编码器零位
DM_CMD_CLEAR_ERROR = 0xfb // 清除电机过热错误
}DMMotor_Mode_e;
DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config);
void DMMotorSetRef(DMMotorInstance *motor, float ref);
void DMMotorOuterLoop(DMMotorInstance *motor,Closeloop_Type_e closeloop_type);
void DMMotorEnable(DMMotorInstance *motor);
void DMMotorStop(DMMotorInstance *motor);
void DMMotorCaliEncoder(DMMotorInstance *motor);
void DMMotorControlInit();
void DMMotorSetMode(DMMotor_Mode_e mode, DMMotorInstance *motor);
#endif // !DMMOTOR
modules/motor/HTmotor/HT04.c
+235
View File
@@ -0,0 +1,235 @@
#include "HT04.h"
#include "memory.h"
#include "general_def.h"
#include "user_lib.h"
#include "cmsis_os.h"
#include "string.h"
#include "daemon.h"
#include "stdlib.h"
#include "bsp_log.h"
static uint8_t idx;
static HTMotorInstance *ht_motor_instance[HT_MOTOR_CNT];
static osThreadId ht_task_handle[HT_MOTOR_CNT];
static uint8_t zero_buff[6] = {0};
/**
* @brief 设置电机模式,报文内容[0xff,0xff,0xff,0xff,0xff,0xff,0xff,cmd]
*
* @param cmd
* @param motor
*/
static void HTMotorSetMode(HTMotor_Mode_t cmd, HTMotorInstance *motor)
{
memset(motor->motor_can_instace->tx_buff, 0xff, 7); // 发送电机指令的时候前面7bytes都是0xff
motor->motor_can_instace->tx_buff[7] = (uint8_t)cmd; // 最后一位是命令id
CANTransmit(motor->motor_can_instace, 1);
memcpy(motor->motor_can_instace->tx_buff, zero_buff, 6);
}
/* 两个用于将uint值和float值进行映射的函数,在设定发送值和解析反馈值时使用 */
static uint16_t float_to_uint(float x, float x_min, float x_max, uint8_t bits)
{
float span = x_max - x_min;
float offset = x_min;
return (uint16_t)((x - offset) * ((float)((1 << bits) - 1)) / span);
}
static float uint_to_float(int x_int, float x_min, float x_max, int bits)
{
float span = x_max - x_min;
float offset = x_min;
return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset;
}
/**
* @brief 解析电机反馈值
*
* @param motor_can 收到
*/
static void HTMotorDecode(CANInstance *motor_can)
{
uint16_t tmp; // 用于暂存解析值,稍后转换成float数据,避免多次创建临时变量
uint8_t const *rxbuff = motor_can->rx_buff;
HTMotorInstance *motor = (HTMotorInstance *)motor_can->id;
HTMotor_Measure_t *measure = &(motor->measure); // 将can实例中保存的id转换成电机实例的指针
DaemonReload(motor->motor_daemon);
measure->feed_dt = DWT_GetDeltaT(&measure->feed_cnt);
measure->last_angle = measure->total_angle;
tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]);
measure->total_angle = uint_to_float(tmp, P_MIN, P_MAX, 16);
tmp = (uint16_t)(rxbuff[3] << 4) | (rxbuff[4] >> 4);
measure->speed_rads = AverageFilter((uint_to_float(tmp, V_MIN, V_MAX, 12) - HT_SPEED_BIAS), measure->speed_buff, SPEED_BUFFER_SIZE);
tmp = (uint16_t)(((rxbuff[4] & 0x0f) << 8) | rxbuff[5]);
measure->real_current = CURRENT_SMOOTH_COEF * uint_to_float(tmp, T_MIN, T_MAX, 12) +
(1 - CURRENT_SMOOTH_COEF) * measure->real_current;
}
static void HTMotorLostCallback(void *motor_ptr)
{
HTMotorInstance *motor = (HTMotorInstance *)motor_ptr;
LOGWARNING("[ht_motor] motor %d lost\n", motor->motor_can_instace->tx_id);
if (++motor->lost_cnt % 10 != 0)
HTMotorSetMode(CMD_MOTOR_MODE, motor); // 尝试重新让电机进入控制模式
}
/* 海泰电机一生黑,什么垃圾协议! */
void HTMotorCalibEncoder(HTMotorInstance *motor)
{
uint16_t p, v, kp, kd, t;
p = float_to_uint(0, P_MIN, P_MAX, 16);
v = float_to_uint(0, V_MIN, V_MAX, 12);
kp = float_to_uint(0, KP_MIN, KP_MAX, 12);
kd = float_to_uint(0, KD_MIN, KD_MAX, 12);
t = float_to_uint(0, T_MIN, T_MAX, 12);
uint8_t *buf = motor->motor_can_instace->tx_buff;
buf[0] = p >> 8;
buf[1] = p & 0xFF;
buf[2] = v >> 4;
buf[3] = ((v & 0xF) << 4) | (kp >> 8);
buf[4] = kp & 0xFF;
buf[5] = kd >> 4;
buf[6] = ((kd & 0xF) << 4) | (t >> 8);
buf[7] = t & 0xff;
memcpy(zero_buff, buf, 6); // 初始化的时候至少调用一次,故将其他指令为0时发送的报文保存一下,详见ht04电机说明
CANTransmit(motor->motor_can_instace, 1);
DWT_Delay(0.005);
HTMotorSetMode(CMD_ZERO_POSITION, motor); // sb 玩意校准完了编码器也不为0
DWT_Delay(0.005);
// HTMotorSetMode(CMD_MOTOR_MODE, motor);
}
HTMotorInstance *HTMotorInit(Motor_Init_Config_s *config)
{
HTMotorInstance *motor = (HTMotorInstance *)malloc(sizeof(HTMotorInstance));
memset(motor, 0, sizeof(HTMotorInstance));
motor->motor_settings = config->controller_setting_init_config;
PIDInit(&motor->current_PID, &config->controller_param_init_config.current_PID);
PIDInit(&motor->speed_PID, &config->controller_param_init_config.speed_PID);
PIDInit(&motor->angle_PID, &config->controller_param_init_config.angle_PID);
motor->other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
motor->other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
config->can_init_config.can_module_callback = HTMotorDecode;
config->can_init_config.id = motor;
motor->motor_can_instace = CANRegister(&config->can_init_config);
Daemon_Init_Config_s conf = {
.callback = HTMotorLostCallback,
.owner_id = motor,
.reload_count = 5, // 20ms
};
motor->motor_daemon = DaemonRegister(&conf);
HTMotorEnable(motor);
HTMotorSetMode(CMD_MOTOR_MODE, motor); // 确保电机已经上电并执行电机模式
DWT_Delay(0.05f);
HTMotorCalibEncoder(motor); // 将当前编码器位置作为零位
DWT_Delay(0.05f); // 保证下一个电机发送时CAN是空闲的,注意应用在初始化模块的时候不应该进入中断
ht_motor_instance[idx++] = motor;
return motor;
}
void HTMotorSetRef(HTMotorInstance *motor, float ref)
{
motor->pid_ref = ref;
}
/**
* @brief 为了避免总线堵塞,为每个电机创建一个发送任务
* @param argument 传入的电机指针
*/
__attribute__((noreturn)) void HTMotorTask(void const *argument)
{
float set, pid_measure, pid_ref;
HTMotorInstance *motor = (HTMotorInstance *)argument;
HTMotor_Measure_t *measure = &motor->measure;
Motor_Control_Setting_s *setting = &motor->motor_settings;
CANInstance *motor_can = motor->motor_can_instace;
uint16_t tmp;
while (1)
{
pid_ref = motor->pid_ref;
if ((setting->close_loop_type & ANGLE_LOOP) && setting->outer_loop_type == ANGLE_LOOP)
{
if (setting->angle_feedback_source == OTHER_FEED)
pid_measure = *motor->other_angle_feedback_ptr;
else
pid_measure = measure->total_angle;
// measure单位是rad,ref是角度,统一到angle下计算,方便建模
pid_ref = PIDCalculate(&motor->angle_PID, pid_measure * RAD_2_DEGREE, pid_ref);
}
if ((setting->close_loop_type & SPEED_LOOP) && setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP))
{
if (setting->feedforward_flag & SPEED_FEEDFORWARD)
pid_ref += *motor->speed_feedforward_ptr;
if (setting->angle_feedback_source == OTHER_FEED)
pid_measure = *motor->other_speed_feedback_ptr;
else
pid_measure = measure->speed_rads;
// measure单位是rad / s ,ref是angle per sec,统一到angle下计算
pid_ref = PIDCalculate(&motor->speed_PID, pid_measure * RAD_2_DEGREE, pid_ref);
}
if (setting->feedforward_flag & CURRENT_FEEDFORWARD)
pid_ref += *motor->current_feedforward_ptr;
if (setting->close_loop_type & CURRENT_LOOP)
{
pid_ref = PIDCalculate(&motor->current_PID, measure->real_current, pid_ref);
}
set = pid_ref;
if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
set *= -1;
LIMIT_MIN_MAX(set, T_MIN, T_MAX);
tmp = float_to_uint(set, T_MIN, T_MAX, 12);
if (motor->stop_flag == MOTOR_STOP)
tmp = float_to_uint(0, T_MIN, T_MAX, 12);
motor_can->tx_buff[6] = (tmp >> 8);
motor_can->tx_buff[7] = tmp & 0xff;
CANTransmit(motor_can, 0.5);
osDelay(1);
}
}
void HTMotorControlInit()
{
char ht_task_name[5] = "ht";
// 遍历所有电机实例,创建任务
if (!idx)
return;
for (size_t i = 0; i < idx; i++)
{
char ht_id_buff[2] = {0};
__itoa(i, ht_id_buff, 10);
strcat(ht_task_name, ht_id_buff); // 似乎没什么吊用,osthreaddef会把第一个变量当作宏字符串传入,作为任务名
// @todo 还需要一个更优雅的方案来区分不同的电机任务
osThreadDef(ht_task_name, HTMotorTask, osPriorityNormal, 0, 128);
ht_task_handle[i] = osThreadCreate(osThread(ht_task_name), ht_motor_instance[i]);
}
}
void HTMotorStop(HTMotorInstance *motor)
{
motor->stop_flag = MOTOR_STOP;
}
void HTMotorEnable(HTMotorInstance *motor)
{
motor->stop_flag = MOTOR_ENALBED;
}
void HTMotorOuterLoop(HTMotorInstance *motor, Closeloop_Type_e type)
{
motor->motor_settings.outer_loop_type = type;
}
modules/motor/HTmotor/HT04.h
+123
View File
@@ -0,0 +1,123 @@
#ifndef HT04_H
#define HT04_H
#include <stdint.h>
#include "bsp_can.h"
#include "controller.h"
#include "motor_def.h"
#include "daemon.h"
#define HT_MOTOR_CNT 4
#define CURRENT_SMOOTH_COEF 0.9f
#define SPEED_BUFFER_SIZE 5
#define HT_SPEED_BIAS -0.0109901428f // 电机速度偏差,单位rad/s
#define P_MIN -95.5f // Radians
#define P_MAX 95.5f
#define V_MIN -45.0f // Rad/s
#define V_MAX 45.0f
#define T_MIN -18.0f // N·m
#define T_MAX 18.0f
#define KP_MIN 0.0f // N-m/rad
#define KP_MAX 500.0f
#define KD_MIN 0.0f // N-m/rad/s
#define KD_MAX 5.0f
typedef struct // HT04
{
float total_angle; // 角度为多圈角度,范围是-95.5~95.5,单位为rad
float last_angle;
float speed_rads;
float speed_buff[SPEED_BUFFER_SIZE];
float real_current;
float feed_dt;
uint32_t feed_cnt;
} HTMotor_Measure_t;
/* HT电机类型定义*/
typedef struct
{
HTMotor_Measure_t measure;
Motor_Control_Setting_s motor_settings;
PIDInstance current_PID;
PIDInstance speed_PID;
PIDInstance angle_PID;
float *other_angle_feedback_ptr;
float *other_speed_feedback_ptr;
float *speed_feedforward_ptr;
float *current_feedforward_ptr;
float pid_ref;
Motor_Working_Type_e stop_flag; // 启停标志
CANInstance *motor_can_instace;
DaemonInstance *motor_daemon;
uint32_t lost_cnt;
} HTMotorInstance;
/* HT电机模式,初始化时自动进入CMD_MOTOR_MODE*/
typedef enum
{
CMD_MOTOR_MODE = 0xfc, // 使能,会响应指令
CMD_RESET_MODE = 0xfd, // 停止
CMD_ZERO_POSITION = 0xfe // 将当前的位置设置为编码器零位
} HTMotor_Mode_t;
/**
* @brief
*
* @param config
* @return HTMotorInstance*
*/
HTMotorInstance *HTMotorInit(Motor_Init_Config_s *config);
/**
* @brief 设定电机的参考值
*
* @param motor 要设定的电机
* @param current 设定值
*/
void HTMotorSetRef(HTMotorInstance *motor, float ref);
/**
* @brief 初始化电机任务,若要使用,需要在motortask的死循环前调用
*
*/
void HTMotorControlInit();
/**
* @brief 停止电机,之后电机不会响应HTMotorSetRef设定的值
*
* @param motor
*/
void HTMotorStop(HTMotorInstance *motor);
/**
* @brief 启动电机
*
* @param motor 要启动的电机
*/
void HTMotorEnable(HTMotorInstance *motor);
/**
* @brief 修改电机闭环对象
*
* @param motor
* @param type
*/
void HTMotorOuterLoop(HTMotorInstance *motor, Closeloop_Type_e type);
/**
* @brief 将当前编码器位置置零
*
* @param motor
*/
void HTMotorCalibEncoder(HTMotorInstance *motor);
#endif // !HT04_H#define HT04_H
modules/motor/HTmotor/HT04.md
+44
View File
@@ -0,0 +1,44 @@
- 在通过串口设置电机的时候，注意发送指令不要追加回车\n，否则电机端不响应。可以使用sscom，其会将你的按键直接当作ascii字符发送出去。
- **此电机的CAN线序和RoboMaster开发板相反，注意单独制作CAN线**
- 电机控制和反馈报文：
![控制报文](%E6%8E%A7%E5%88%B6%E6%8A%A5%E6%96%87.png)
注意，我们的代码实现不使用其电调协议的位置PD算法；自行实现了三环，并在最后给出电流参考值，发送给电调。因此command packed structure中的**pos cmd & vel cmd & Kp & Kd均为零**。报文均为小端，**低位在前**。
请注意发送和反馈数据的**单位**。
> ~~HT的电机协议做的真不行，纯纯直接抄mit还抄不明白，之后全部换成LK！~~
另一种上电时校准编码器的方法:
```c
static void HTMotorDecode(CANInstance *motor_can)
{
uint16_t tmp; // 用于暂存解析值,稍后转换成float数据,避免多次创建临时变量
uint8_t *rxbuff = motor_can->rx_buff;
HTMotor_Measure_t *measure = &((HTMotorInstance *)motor_can->id)->measure; // 将can实例中保存的id转换成电机实例的指针
measure->last_angle = measure->total_angle;
tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]);
measure->total_angle = uint_to_float(tmp, P_MIN, P_MAX, 16) - measure->angle_bias;
tmp = (uint16_t)(rxbuff[3] << 4) | (rxbuff[4] >> 4);
measure->speed_rads = AverageFilter((uint_to_float(tmp, V_MIN, V_MAX, 12) - HT_SPEED_BIAS), measure->speed_buff, SPEED_BUFFER_SIZE);
tmp = (uint16_t)(((rxbuff[4] & 0x0f) << 8) | rxbuff[5]);
measure->real_current = CURRENT_SMOOTH_COEF * uint_to_float(tmp, T_MIN, T_MAX, 12) +
(1 - CURRENT_SMOOTH_COEF) * measure->real_current;
if (!measure->first_feedback_flag) // 初始化的时候设置偏置
{
measure->first_feedback_flag = 1;
measure->angle_bias = measure->total_angle;
measure->total_angle = 0;
measure->speed_rads = 0;
}
}
```
第一次收到数据时默认电机处于限位处,将速度和角度都设置为零,记录当前的编码器数据,之后每次收到都减去该值.
modules/motor/HTmotor/控制报文.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 103 KiB

modules/motor/HTmotor/驱动器硬件说明.pdf
BIN
View File
Binary file not shown.
modules/motor/LKmotor/LK-TECH电机CAN协议说明V2_3.pdf
BIN
View File
Binary file not shown.
modules/motor/LKmotor/LK9025.c
+169
View File
@@ -0,0 +1,169 @@
#include "LK9025.h"
#include "stdlib.h"
#include "general_def.h"
#include "daemon.h"
#include "bsp_dwt.h"
#include "bsp_log.h"
static uint8_t idx;
static LKMotorInstance *lkmotor_instance[LK_MOTOR_MX_CNT] = {NULL};
static CANInstance *sender_instance; // 多电机发送时使用的caninstance(当前保存的是注册的第一个电机的caninstance)
// 后续考虑兼容单电机和多电机指令.
/**
* @brief 电机反馈报文解析
*
* @param _instance 发生中断的caninstance
*/
static void LKMotorDecode(CANInstance *_instance)
{
LKMotorInstance *motor = (LKMotorInstance *)_instance->id; // 通过caninstance保存的father id获取对应的motorinstance
LKMotor_Measure_t *measure = &motor->measure;
uint8_t *rx_buff = _instance->rx_buff;
DaemonReload(motor->daemon); // 喂狗
measure->feed_dt = DWT_GetDeltaT(&measure->feed_dwt_cnt);
measure->last_ecd = measure->ecd;
measure->ecd = (uint16_t)((rx_buff[7] << 8) | rx_buff[6]);
measure->angle_single_round = ECD_ANGLE_COEF_LK * measure->ecd;
measure->speed_rads = (1 - SPEED_SMOOTH_COEF) * measure->speed_rads +
DEGREE_2_RAD * SPEED_SMOOTH_COEF * (float)((int16_t)(rx_buff[5] << 8 | rx_buff[4]));
measure->real_current = (1 - CURRENT_SMOOTH_COEF) * measure->real_current +
CURRENT_SMOOTH_COEF * (float)((int16_t)(rx_buff[3] << 8 | rx_buff[2]));
measure->temperature = rx_buff[1];
if (measure->ecd - measure->last_ecd > 32768)
measure->total_round--;
else if (measure->ecd - measure->last_ecd < -32768)
measure->total_round++;
measure->total_angle = measure->total_round * 360 + measure->angle_single_round;
}
static void LKMotorLostCallback(void *motor_ptr)
{
LKMotorInstance *motor = (LKMotorInstance *)motor_ptr;
LOGWARNING("[LKMotor] motor lost, id: %d", motor->motor_can_ins->tx_id);
}
LKMotorInstance *LKMotorInit(Motor_Init_Config_s *config)
{
LKMotorInstance *motor = (LKMotorInstance *)malloc(sizeof(LKMotorInstance));
motor = (LKMotorInstance *)malloc(sizeof(LKMotorInstance));
memset(motor, 0, sizeof(LKMotorInstance));
motor->motor_settings = config->controller_setting_init_config;
PIDInit(&motor->current_PID, &config->controller_param_init_config.current_PID);
PIDInit(&motor->speed_PID, &config->controller_param_init_config.speed_PID);
PIDInit(&motor->angle_PID, &config->controller_param_init_config.angle_PID);
motor->other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
motor->other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
config->can_init_config.id = motor;
config->can_init_config.can_module_callback = LKMotorDecode;
config->can_init_config.rx_id = 0x140 + config->can_init_config.tx_id;
config->can_init_config.tx_id = config->can_init_config.tx_id + 0x280 - 1; // 这样在发送写入buffer的时候更方便,因为下标从0开始,LK多电机发送id为0x280
motor->motor_can_ins = CANRegister(&config->can_init_config);
if (idx == 0) // 用第一个电机的can instance发送数据
{
sender_instance = motor->motor_can_ins;
sender_instance->tx_id = 0x280; // 修改tx_id为0x280,用于多电机发送,不用管其他LKMotorInstance的tx_id,它们仅作初始化用
}
LKMotorEnable(motor);
DWT_GetDeltaT(&motor->measure.feed_dwt_cnt);
lkmotor_instance[idx++] = motor;
Daemon_Init_Config_s daemon_config = {
.callback = LKMotorLostCallback,
.owner_id = motor,
.reload_count = 5, // 50ms
};
motor->daemon = DaemonRegister(&daemon_config);
return motor;
}
/* 第一个电机的can instance用于发送数据,向其tx_buff填充数据 */
void LKMotorControl()
{
float pid_measure, pid_ref;
int16_t set;
LKMotorInstance *motor;
LKMotor_Measure_t *measure;
Motor_Control_Setting_s *setting;
for (size_t i = 0; i < idx; ++i)
{
motor = lkmotor_instance[i];
measure = &motor->measure;
setting = &motor->motor_settings;
pid_ref = motor->pid_ref;
if ((setting->close_loop_type & ANGLE_LOOP) && setting->outer_loop_type == ANGLE_LOOP)
{
if (setting->angle_feedback_source == OTHER_FEED)
pid_measure = *motor->other_angle_feedback_ptr;
else
pid_measure = measure->real_current;
pid_ref = PIDCalculate(&motor->angle_PID, pid_measure, pid_ref);
if (setting->feedforward_flag & SPEED_FEEDFORWARD)
pid_ref += *motor->speed_feedforward_ptr;
}
if ((setting->close_loop_type & SPEED_LOOP) && setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP))
{
if (setting->angle_feedback_source == OTHER_FEED)
pid_measure = *motor->other_speed_feedback_ptr;
else
pid_measure = measure->speed_rads;
pid_ref = PIDCalculate(&motor->angle_PID, pid_measure, pid_ref);
if (setting->feedforward_flag & CURRENT_FEEDFORWARD)
pid_ref += *motor->current_feedforward_ptr;
}
if (setting->close_loop_type & CURRENT_LOOP)
{
pid_ref = PIDCalculate(&motor->current_PID, measure->real_current, pid_ref);
}
set = pid_ref;
if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
set *= -1;
// 这里随便写的,为了兼容多电机命令.后续应该将tx_id以更好的方式表达电机id,单独使用一个CANInstance,而不是用第一个电机的CANInstance
memcpy(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280) * 2, &set, sizeof(uint16_t));
if (motor->stop_flag == MOTOR_STOP)
{ // 若该电机处于停止状态,直接将发送buff置零
memset(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280) * 2, 0, sizeof(uint16_t));
}
}
if (idx) // 如果有电机注册了
CANTransmit(sender_instance, 0.2);
}
void LKMotorStop(LKMotorInstance *motor)
{
motor->stop_flag = MOTOR_STOP;
}
void LKMotorEnable(LKMotorInstance *motor)
{
motor->stop_flag = MOTOR_ENALBED;
}
void LKMotorSetRef(LKMotorInstance *motor, float ref)
{
motor->pid_ref = ref;
}
uint8_t LKMotorIsOnline(LKMotorInstance *motor)
{
return DaemonIsOnline(motor->daemon);
}
modules/motor/LKmotor/LK9025.h
+98
View File
@@ -0,0 +1,98 @@
#ifndef LK9025_H
#define LK9025_H
#include "stdint.h"
#include "bsp_can.h"
#include "controller.h"
#include "motor_def.h"
#include "daemon.h"
#define LK_MOTOR_MX_CNT 4 // 最多允许4个LK电机使用多电机指令,挂载在一条总线上
#define I_MIN -2000
#define I_MAX 2000
#define CURRENT_SMOOTH_COEF 0.9f
#define SPEED_SMOOTH_COEF 0.85f
#define REDUCTION_RATIO_DRIVEN 1
#define ECD_ANGLE_COEF_LK (360.0f / 65536.0f)
#define CURRENT_TORQUE_COEF_LK 0.003645f // 电流设定值转换成扭矩的系数,算出来的设定值除以这个系数就是扭矩值
typedef struct // 9025
{
uint16_t last_ecd; // 上一次读取的编码器值
uint16_t ecd; // 当前编码器值
float angle_single_round; // 单圈角度
float speed_rads; // speed rad/s
int16_t real_current; // 实际电流
uint8_t temperature; // 温度,C°
float total_angle; // 总角度
int32_t total_round; // 总圈数
float feed_dt;
uint32_t feed_dwt_cnt;
} LKMotor_Measure_t;
typedef struct
{
LKMotor_Measure_t measure;
Motor_Control_Setting_s motor_settings;
float *other_angle_feedback_ptr; // 其他反馈来源的反馈数据指针
float *other_speed_feedback_ptr;
float *speed_feedforward_ptr; // 速度前馈数据指针,可以通过此指针设置速度前馈值,或LQR等时作为速度状态变量的输入
float *current_feedforward_ptr; // 电流前馈指针
PIDInstance current_PID;
PIDInstance speed_PID;
PIDInstance angle_PID;
float pid_ref;
Motor_Working_Type_e stop_flag; // 启停标志
CANInstance *motor_can_ins;
DaemonInstance *daemon;
} LKMotorInstance;
/**
* @brief 初始化LK电机
*
* @param config 电机配置
* @return LKMotorInstance* 返回实例指针
*/
LKMotorInstance *LKMotorInit(Motor_Init_Config_s *config);
/**
* @brief 设置参考值
* @attention 注意此函数设定的ref是最外层闭环的输入,若要设定内层闭环的值请通过前馈数据指针设置
*
* @param motor 要设置的电机
* @param ref 设定值
*/
void LKMotorSetRef(LKMotorInstance *motor, float ref);
/**
* @brief 为所有LK电机计算pid/反转/模式控制,并通过bspcan发送电流值(发送CAN报文)
*
*/
void LKMotorControl();
/**
* @brief 停止LK电机,之后电机不会响应任何指令
*
* @param motor
*/
void LKMotorStop(LKMotorInstance *motor);
/**
* @brief 启动LK电机
*
* @param motor
*/
void LKMotorEnable(LKMotorInstance *motor);
uint8_t LKMotorIsOnline(LKMotorInstance *motor);
#endif // LK9025_H
modules/motor/LKmotor/LK_motor.md
+12
View File
@@ -0,0 +1,12 @@
# LK motor
这是瓴控电机的模块封装说明文档。关于LK电机的控制报文和反馈报文值，详见LK电机的说明文档，由于电机实例已经自带三环PID计算，一般来说**我们能用到的只有单电机的力矩指令和多电机指令。**
注意LK电机在使用多电机发送的时候，只支持一条总线上至多4个电机，多电机模式下LK仅支持发送id 0x280为接收ID为0x140+id.
要设置为多电机模式，请通过串口连接电机，并使用该文件夹下的LK motor tool.exe进行配置。
## LK的其他电机
若使用其他LK电机，唯一需要修改的是确定编码器的精度，即LKMotorDecode()部分的速度反馈和编码器反馈解析。
modules/motor/LKmotor/反馈报文.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 163 KiB

modules/motor/LKmotor/报文格式.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 276 KiB

modules/motor/motor_def.h
+149
View File
@@ -0,0 +1,149 @@
/**
* @file motor_def.h
* @author neozng
* @brief 电机通用的数据结构定义
* @version beta
* @date 2022-11-01
*
* @copyright Copyright (c) 2022 HNU YueLu EC all rights reserved
*
*/
#ifndef MOTOR_DEF_H
#define MOTOR_DEF_H
#include "controller.h"
#include "stdint.h"
#define LIMIT_MIN_MAX(x, min, max) (x) = (((x) <= (min)) ? (min) : (((x) >= (max)) ? (max) : (x)))
/**
* @brief 闭环类型,如果需要多个闭环,则使用或运算
* 例如需要速度环和电流环: CURRENT_LOOP|SPEED_LOOP
*/
typedef enum
{
OPEN_LOOP = 0b0000,
CURRENT_LOOP = 0b0001,
SPEED_LOOP = 0b0010,
ANGLE_LOOP = 0b0100,
// only for checking
SPEED_AND_CURRENT_LOOP = 0b0011,
ANGLE_AND_SPEED_LOOP = 0b0110,
ALL_THREE_LOOP = 0b0111,
} Closeloop_Type_e;
typedef enum
{
FEEDFORWARD_NONE = 0b00,
CURRENT_FEEDFORWARD = 0b01,
SPEED_FEEDFORWARD = 0b10,
CURRENT_AND_SPEED_FEEDFORWARD = CURRENT_FEEDFORWARD | SPEED_FEEDFORWARD,
} Feedfoward_Type_e;
/* 反馈来源设定,若设为OTHER_FEED则需要指定数据来源指针,详见Motor_Controller_s*/
typedef enum
{
MOTOR_FEED = 0,
OTHER_FEED,
} Feedback_Source_e;
/* 电机正反转标志 */
typedef enum
{
MOTOR_DIRECTION_NORMAL = 0,
MOTOR_DIRECTION_REVERSE = 1
} Motor_Reverse_Flag_e;
/* 反馈量正反标志 */
typedef enum
{
FEEDBACK_DIRECTION_NORMAL = 0,
FEEDBACK_DIRECTION_REVERSE = 1
} Feedback_Reverse_Flag_e;
typedef enum
{
MOTOR_STOP = 0,
MOTOR_ENALBED = 1,
} Motor_Working_Type_e;
typedef enum
{
SEND_MODE_NORMAL = 0,
SEND_MODE_MANUAL = 1
} MOTOR_SEND_MODE_e;
/* 电机控制设置,包括闭环类型,反转标志和反馈来源 */
typedef struct
{
Closeloop_Type_e outer_loop_type; // 最外层的闭环,未设置时默认为最高级的闭环
Closeloop_Type_e close_loop_type; // 使用几个闭环(串级)
Motor_Reverse_Flag_e motor_reverse_flag; // 是否反转
Feedback_Reverse_Flag_e feedback_reverse_flag; // 反馈是否反向
Feedback_Source_e angle_feedback_source; // 角度反馈类型
Feedback_Source_e speed_feedback_source; // 速度反馈类型
uint8_t feedforward_flag; // 前馈标志
MOTOR_SEND_MODE_e send_mode; // 发送模式
} Motor_Control_Setting_s;
/* 电机控制器,包括其他来源的反馈数据指针,3环控制器和电机的参考输入*/
// 后续增加前馈数据指针
typedef struct
{
float *other_angle_feedback_ptr; // 其他反馈来源的反馈数据指针
float *other_speed_feedback_ptr;
float *speed_feedforward_ptr;
float *current_feedforward_ptr;
PIDInstance current_PID;
PIDInstance speed_PID;
PIDInstance angle_PID;
float pid_ref; // 将会作为每个环的输入和输出顺次通过串级闭环
float output_external; // 外部输入,用于手动发送报文
} Motor_Controller_s;
/* 电机类型枚举 */
typedef enum
{
MOTOR_TYPE_NONE = 0,
GM6020,
M3508,
M2006,
LK9025,
HT04,
GM6020_NEW //新版固件6020
} Motor_Type_e;
/**
* @brief 电机控制器初始化结构体,包括三环PID的配置以及两个反馈数据来源指针
* 如果不需要某个控制环,可以不设置对应的pid config
* 需要其他数据来源进行反馈闭环,不仅要设置这里的指针还需要在Motor_Control_Setting_s启用其他数据来源标志
*/
typedef struct
{
float *other_angle_feedback_ptr; // 角度反馈数据指针,注意电机使用total_angle
float *other_speed_feedback_ptr; // 速度反馈数据指针,单位为angle per sec
float *speed_feedforward_ptr; // 速度前馈数据指针
float *current_feedforward_ptr; // 电流前馈数据指针
PID_Init_Config_s current_PID;
PID_Init_Config_s speed_PID;
PID_Init_Config_s angle_PID;
} Motor_Controller_Init_s;
/* 用于初始化CAN电机的结构体,各类电机通用 */
typedef struct
{
Motor_Controller_Init_s controller_param_init_config;
Motor_Control_Setting_s controller_setting_init_config;
Motor_Type_e motor_type;
CAN_Init_Config_s can_init_config;
} Motor_Init_Config_s;
#endif // !MOTOR_DEF_H
modules/motor/motor_task.c
+25
View File
@@ -0,0 +1,25 @@
#include "motor_task.h"
#include "LK9025.h"
#include "HT04.h"
#include "dji_motor.h"
#include "step_motor.h"
#include "servo_motor.h"
void MotorControlTask()
{
// static uint8_t cnt = 0; 设定不同电机的任务频率
// if(cnt%5==0) //200hz
// if(cnt%10==0) //100hz
DJIMotorControl();
/* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */
LKMotorControl();
// legacy support
// 由于ht04电机的反馈方式为接收到一帧消息后立刻回传,以此方式连续发送可能导致总线拥塞
// 为了保证高频率控制,HTMotor中提供了以任务方式启动控制的接口,可通过宏定义切换
// HTMotorControl();
// 将所有的CAN设备集中在一处发送,最高反馈频率仅能达到500Hz,为了更好的控制效果,应使用新的HTMotorControlInit()接口
// StepMotorControl();
}
modules/motor/motor_task.h
+25
View File
@@ -0,0 +1,25 @@
/**
* @file motor_task.h
* @author neozng
* @brief 对所有电机,舵机等控制任务的进一步封装,MotorControlTask()将在操作系统中按一定频率调用
* @version beta
* @date 2022-11-01
*
* @copyright Copyright (c) 2022
*
*/
#ifndef MOTOR_TASK_H
#define MOTOR_TASK_H
/**
* @brief 电机控制闭环任务,在RTOS中应该设定为1Khz运行
* 舵机控制任务的频率设定为20Hz或更低
*
* @note 好无语,就一个函数罢了,干脆全部放到头文件里好了.
*
*/
void MotorControlTask();
#endif // !MOTOR_TASK_H
modules/motor/servo_motor/servo_motor.c
+78
View File
@@ -0,0 +1,78 @@
#include "servo_motor.h"
#include "stdlib.h"
#include "memory.h"
#include "bsp_log.h"
uint8_t servo_angle_read[6]={0x55 ,0x55 ,0x04, 0x15 ,0x01 ,0x01 ,};
uint8_t servo_angle_write[16]={0x55 ,0x55, 0x08, 0x03, 0x01 ,0xF4 ,0x01 ,0x01 ,0x20 ,0x03,0x55 ,0x55 ,0x04, 0x15 ,0x01 ,0x01 ,};
uint8_t servo_unload[6]={0x55,0x55,0x04,0x14,0x01,0x01};
/*第二版*/
static ServoInstance *servo_motor_instance[SERVO_MOTOR_CNT];
static uint8_t servo_idx = 0; // register servo_idx,是该文件的全局舵机索引,在注册时使用
static void DecodeServo();
// 通过此函数注册一个舵机
ServoInstance *ServoInit(Servo_Init_Config_s *Servo_Init_Config)
{
ServoInstance *servo = (ServoInstance *)malloc(sizeof(ServoInstance));
memset(servo, 0, sizeof(ServoInstance));
USART_Init_Config_s config;
servo->servo_type = Servo_Init_Config->servo_type;
switch (Servo_Init_Config->servo_type)
{
case Bus_Servo:
config.module_callback = DecodeServo;
config.recv_buff_size = Servo_MAX_BUFF;
config.usart_handle = Servo_Init_Config->_handle;
servo->usart_instance = USARTRegister(&config);
break;
case PWM_Servo:
servo->pwm_instance = PWMRegister(&Servo_Init_Config->pwm_init_config);
break;
default:
LOGERROR("Servo type error");
break;
}
servo->servo_id = Servo_Init_Config->servo_id;
servo_idx++;
servo_motor_instance[servo->servo_id] = servo;
return servo;
}
//@todo PWM舵机的角度设置需要根据相应定时器PWM等参数进行计算(是否需要规范定时器PWM的初始化参数，以便于计算)
void ServoSetAngle(ServoInstance *servo, float angle)
{
switch (servo->servo_type)
{
case Bus_Servo:
servo_angle_write[8] = (uint16_t)angle&0xff;
servo_angle_write[9] = (uint16_t)angle>>8;
USARTSend(servo->usart_instance, servo_angle_write, 16, USART_TRANSFER_DMA);
// USARTSend(servo->usart_instance, servo_angle_read, 6, USART_TRANSFER_DMA);
break;
case PWM_Servo:
servo->angle = angle;
PWMSetDutyRatio(servo->pwm_instance, angle);
break;
default:
break;
}
}
//@todo 只读取了角度 还有电压，动作是否完成等 且只支持一个串口
static void DecodeServo()
{
for (uint8_t i = 0; i < servo_idx; i++)
{
if (servo_motor_instance[i]->servo_type == Bus_Servo)
{
if (servo_motor_instance[i]->usart_instance->recv_buff[0] == Servo_Frame_First && servo_motor_instance[i]->usart_instance->recv_buff[1] == Servo_Frame_Second)
{
if (servo_motor_instance[i]->usart_instance->recv_buff[3] == 21)
{
servo_motor_instance[i]->recv_angle = (servo_motor_instance[i]->usart_instance->recv_buff[7] << 8 | servo_motor_instance[i]->usart_instance->recv_buff[6]);
}
}
}
}
}
modules/motor/servo_motor/servo_motor.h
+45
View File
@@ -0,0 +1,45 @@
#ifndef SERVO_MOTOR_H
#define SERVO_MOTOR_H
#include "main.h"
#include "tim.h"
#include <stdint-gcc.h>
#include "bsp_pwm.h"
#include "bsp_usart.h"
#define SERVO_MOTOR_CNT 7
#define Servo_Frame_First 0x55
#define Servo_Frame_Second 0x55
#define Servo_MAX_BUFF 10
#define SERVO_MOVE_CMD 0x03
#define SERVO_UNLOAD_CMD 0x14
#define SERVO_POS_READ_CMD 0x15
typedef enum
{
Servo_None_Type = 0,
Bus_Servo = 1,
PWM_Servo = 2,
}ServoType_e;
/* 用于初始化不同舵机的结构体,各类舵机通用 */
typedef struct
{
PWM_Init_Config_s pwm_init_config;
ServoType_e servo_type;
UART_HandleTypeDef *_handle;
uint8_t servo_id;
}Servo_Init_Config_s;
typedef struct
{
uint8_t servo_id;
float angle;
uint16_t recv_angle;
PWMInstance *pwm_instance;
USARTInstance *usart_instance;
ServoType_e servo_type;
}ServoInstance;
ServoInstance *ServoInit(Servo_Init_Config_s *Servo_Init_Config);
void ServoSetAngle(ServoInstance *servo, float angle);
#endif // SERVO_MOTOR_H
modules/motor/servo_motor/servo_motor.md
+23
View File
@@ -0,0 +1,23 @@
##舵机的使用
<p align='left' >panrui@hnu.edu.cn</p>
> todo: 角度设置等需要规范化
### 舵机基础知识
已最常见的SG90舵机为例，SG90舵机要求工作在频率为50HZ——周期为20ms的PWM波，且对应信号的高低电平在0.5ms - 2.5ms之间，对应的舵机转动角度如下表所示（当然也可以按照这个线性的对应关系去达到转动自己想要的角度，如想要转动60°，则高电平脉宽为大概为1.2ms，具体能不能转到特定的角度还和舵机的精度有关）
>0.5ms-------------0度； 2.5%
>1.0ms------------45度； 5.0%
>1.5ms------------90度； 7.5%
>2.0ms-----------135度； 10.0%
>2.5ms-----------180度； 12.5%
根据`<font color=black size=3>Tout = (PSC+1)* (ARR+1)/Tclk</font>`公式
则我们需要产生50Hz的PWM波，则预分频的系数为 Prescaler = 168-1，自动重装载值 Counter Period = 20000-1，此时定时器产生的频率为 168Mhz/168/20000 = 50Hz。 当然这个值也可以自己设置，只要满足产生的频率为50Hz即可.
`__HAL_TIM_SET_COMPARE(htim, Channel, compare_value);`
这是设置占空比的函数
eg：当初始占空比为1200/20000则为6%，根据20*6%=1.2ms （1.2-0.5）/(2.5-0.5)*180=63° 故舵机会转动63°
为了方便通过上述eg我们将所需要的角度与PWM计数值对应关系封装成函数。需要在初始化的時候输入我们所需要的角度和相关定时器参数即可。这样我们就可以设置SG90为参数范围内(0~180°)任意度数。
---
modules/motor/step_motor/X_V2.c
+1774
View File
File diff suppressed because it is too large Load Diff
modules/motor/step_motor/X_V2.h
+231
View File
@@ -0,0 +1,231 @@
#ifndef __X_V2_H
#define __X_V2_H
#include <stdint.h>
#include <stdbool.h>
/**********************************************************
*** X_V2步进闭环控制例程
*** 编写作者：ZHANGDATOU
*** 技术支持：张大头闭环伺服
*** 淘宝店铺：https://zhangdatou.taobao.com
*** CSDN博客：http s://blog.csdn.net/zhangdatou666
*** qq交流群：262438510
**********************************************************/
#define ABS(x) ((x) > 0 ? (x) : -(x))
typedef enum {
S_VBUS = 5, // 读取总线电压
S_CBUS = 6, // 读取总线电流
S_CPHA = 7, // 读取相电流
S_ENCO = 8, // 读取编码器原始值
S_CLKC = 9, // 读取实时脉冲数
S_ENCL = 10, // 读取经过线性化校准后的编码器值
S_CLKI = 11, // 读取输入脉冲数
S_TPOS = 12, // 读取电机目标位置
S_SPOS = 13, // 读取电机实时设定的目标位置
S_VEL = 14, // 读取电机实时转速
S_CPOS = 15, // 读取电机实时位置
S_PERR = 16, // 读取电机位置误差
S_VBAT = 17, // 读取多圈编码器电池电压（Y42）
S_TEMP = 18, // 读取电机实时温度（Y42）
S_FLAG = 19, // 读取电机状态标志位
S_OFLAG = 20, // 读取回零状态标志位
S_OAF = 21, // 读取电机状态标志位 + 回零状态标志位（Y42）
S_PIN = 22, // 读取引脚状态（Y42）
}SysParams_t;
#define MMCL_LEN 512
extern uint16_t MMCL_count, MMCL_cmd[MMCL_LEN];
/**
***********************************************************
***********************************************************
***
***
*** @brief 后缀带有（Y42）为Y42新增命令，X42不能用，其他通用
***
***
***********************************************************
***********************************************************
***/
/**********************************************************
*** 触发动作命令
**********************************************************/
// 触发编码器校准
void X_V2_Trig_Encoder_Cal(uint8_t addr);
// 重启电机（Y42）
void X_V2_Reset_Motor(uint8_t addr);
// 将当前位置清零
void X_V2_Reset_CurPos_To_Zero(uint8_t addr);
// 解除堵转保护
void X_V2_Reset_Clog_Pro(uint8_t addr);
// 恢复出厂设置
void X_V2_Restore_Motor(uint8_t addr);
/**********************************************************
*** 运动控制命令
**********************************************************/
// 多电机命令（Y42）
void X_V2_Multi_Motor_Cmd(uint8_t addr);
// 电机使能控制
void X_V2_En_Control(uint8_t addr, bool state, bool snF);
// 力矩模式
void X_V2_Torque_Control(uint8_t addr, uint8_t sign, uint16_t t_ramp, uint16_t torque, bool snF);
// 力矩模式限速控制（Y42）
void X_V2_Torque_LV_Control(uint8_t addr, uint8_t sign, uint16_t t_ramp, uint16_t torque, bool snF, float maxVel);
// 速度模式控制
void X_V2_Vel_Control(uint8_t addr, uint8_t dir, uint16_t acc, float vel, bool snF);
// 速度模式限电流控制（Y42）
void X_V2_Vel_LC_Control(uint8_t addr, uint8_t dir, uint16_t acc, float vel, bool snF, uint16_t maxCur);
// 直通限速位置模式控制
void X_V2_Bypass_Pos_LV_Control(uint8_t addr, uint8_t dir, float vel, float pos, uint8_t raf, bool snF);
// 直通限速位置模式限电流控制（Y42）
void X_V2_Bypass_Pos_LV_LC_Control(uint8_t addr, uint8_t dir, float vel, float pos, uint8_t raf, bool snF, uint16_t maxCur);
// 梯形曲线加减速位置模式控制
void X_V2_Traj_Pos_Control(uint8_t addr, uint8_t dir, uint16_t acc, uint16_t dec, float vel, float pos, uint8_t raf, bool snF);
// 梯形曲线加减速位置模式限电流控制（Y42）
void X_V2_Traj_Pos_LC_Control(uint8_t addr, uint8_t dir, uint16_t acc, uint16_t dec, float vel, float pos, uint8_t raf, bool snF, uint16_t maxCur);
// 让电机立即停止运动
void X_V2_Stop_Now(uint8_t addr, bool snF);
// 触发多机同步开始运动
void X_V2_Synchronous_motion(uint8_t addr);
/**********************************************************
*** 原点回零命令
**********************************************************/
// 设置单圈回零的零点位置
void X_V2_Origin_Set_O(uint8_t addr, bool svF);
// 触发回零
void X_V2_Origin_Trigger_Return(uint8_t addr, uint8_t o_mode, bool snF);
// 强制中断并退出回零
void X_V2_Origin_Interrupt(uint8_t addr);
// 读取回零参数
void X_V2_Origin_Read_Params(uint8_t addr);
// 修改回零参数
void X_V2_Origin_Modify_Params(uint8_t addr, bool svF, uint8_t o_mode, uint8_t o_dir, uint16_t o_vel, uint32_t o_tm, uint16_t sl_vel, uint16_t sl_ma, uint16_t sl_ms, bool potF);
/**********************************************************
*** 读取系统参数命令
**********************************************************/
// 定时返回信息命令（Y42）
void X_V2_Auto_Return_Sys_Params_Timed(uint8_t addr, SysParams_t s, uint16_t time_ms);
// 读取系统参数
void X_V2_Read_Sys_Params(uint8_t addr, SysParams_t s);
/**********************************************************
*** 读写驱动参数命令
**********************************************************/
// 修改电机ID地址
void X_V2_Modify_Motor_ID(uint8_t addr, bool svF, uint8_t id);
// 修改细分值
void X_V2_Modify_MicroStep(uint8_t addr, bool svF, uint8_t mstep);
// 修改掉电标志
void X_V2_Modify_PDFlag(uint8_t addr, bool pdf);
// 读取选项参数状态（Y42）
void X_V2_Read_Opt_Param_Sta(uint8_t addr);
// 修改电机类型（Y42）
void X_V2_Modify_Motor_Type(uint8_t addr, bool svF, bool mottype);
// 修改固件类型（Y42）
void X_V2_Modify_Firmware_Type(uint8_t addr, bool svF, bool fwtype);
// 修改开环/闭环控制模式（Y42）
void X_V2_Modify_Ctrl_Mode(uint8_t addr, bool svF, bool ctrl_mode);
// 修改电机运动正方向（Y42）
void X_V2_Modify_Motor_Dir(uint8_t addr, bool svF, bool dir);
// 修改锁定按键功能（Y42）
void X_V2_Modify_Lock_Btn(uint8_t addr, bool svF, bool lockbtn);
// 修改命令位置角度是否继续缩小10倍输入（Y42）
void X_V2_Modify_S_Vel(uint8_t addr, bool svF, bool s_vel);
// 修改开环模式工作电流
void X_V2_Modify_OM_ma(uint8_t addr, bool svF, uint16_t om_ma);
// 修改闭环模式最大电流
void X_V2_Modify_FOC_mA(uint8_t addr, bool svF, uint16_t foc_mA);
// 读取PID参数
void X_V2_Read_PID_Params(uint8_t addr);
// 修改PID参数
void X_V2_Modify_PID_Params(uint8_t addr, bool svF, uint32_t kp, uint32_t ki, uint32_t kd);
// 读取DMX512协议参数（Y42）
void X_V2_Read_DMX512_Params(uint8_t addr);
// 修改DMX512协议参数（Y42）
void X_V2_Modify_DMX512_Params(uint8_t addr, bool svF, uint16_t tch, uint8_t nch, uint8_t mode, uint16_t vel, uint16_t acc, uint16_t vel_step, uint32_t pos_step);
// 读取位置到达窗口（Y42）
void X_V2_Read_Pos_Window(uint8_t addr);
// 修改位置到达窗口（Y42）
void X_V2_Modify_Pos_Window(uint8_t addr, bool svF, uint16_t prw);
// 读取过热过流保护检测阈值（Y42）
void X_V2_Read_Otocp(uint8_t addr);
// 修改过热过流保护检测阈值（Y42）
void X_V2_Modify_Otocp(uint8_t addr, bool svF, uint16_t otp, uint16_t ocp, uint16_t time_ms);
// 读取心跳保护功能时间（Y42）
void X_V2_Read_Heart_Protect(uint8_t addr);
// 修改心跳保护功能时间（Y42）
void X_V2_Modify_Heart_Protect(uint8_t addr, bool svF, uint32_t hp);
// 读取积分限幅/刚性系数（Y42）
void X_V2_Read_Integral_Limit(uint8_t addr);
// 修改积分限幅/刚性系数（Y42）
void X_V2_Modify_Integral_Limit(uint8_t addr, bool svF, uint32_t il);
/**********************************************************
*** 读取所有驱动参数命令
**********************************************************/
// 读取系统状态参数
void X_V2_Read_System_State_Params(uint8_t addr);
// 读取驱动配置参数
void X_V2_Read_Motor_Conf_Params(uint8_t addr);
/**
***********************************************************
***********************************************************
***
***
*** @brief 以下是把相应命令加载到Y42多电机命令上的函数（Y42）
***
***
***********************************************************
***********************************************************
***/
/**********************************************************
*** 触发动作命令
**********************************************************/
// 触发编码器校准 - 加载到多电机指令上
void X_V2_MMCL_Trig_Encoder_Cal(uint8_t addr);
// 重启电机 - 加载到多电机指令上
void X_V2_MMCL_Reset_Motor(uint8_t addr);
// 将当前位置清零 - 加载到多电机指令上
void X_V2_MMCL_Reset_CurPos_To_Zero(uint8_t addr);
// 解除堵转保护 - 加载到多电机指令上
void X_V2_MMCL_Reset_Clog_Pro(uint8_t addr);
// 恢复出厂设置 - 加载到多电机指令上
void X_V2_MMCL_Restore_Motor(uint8_t addr);
/**********************************************************
*** 运动控制命令
**********************************************************/
// 电机使能控制 - 加载到多电机指令上
void X_V2_MMCL_En_Control(uint8_t addr, bool state, bool snF);
// 速度模式控制 - 加载到多电机指令上
void X_V2_MMCL_Vel_Control(uint8_t addr, uint8_t dir, uint16_t vel, uint8_t acc, bool snF);
// 位置模式控制 - 加载到多电机指令上
void X_V2_MMCL_Pos_Control(uint8_t addr, uint8_t dir, uint16_t vel, uint8_t acc, uint32_t clk, bool raF, bool snF);
// 让电机立即停止运动 - 加载到多电机指令上
void X_V2_MMCL_Stop_Now(uint8_t addr, bool snF);
// 触发多机同步开始运动 - 加载到多电机指令上
void X_V2_MMCL_Synchronous_motion(uint8_t addr);
/**********************************************************
*** 原点回零命令
**********************************************************/
// 设置单圈回零的零点位置 - 加载到多电机指令上
void X_V2_MMCL_Origin_Set_O(uint8_t addr, bool svF);
// 触发回零 - 加载到多电机指令上
void X_V2_MMCL_Origin_Trigger_Return(uint8_t addr, uint8_t o_mode, bool snF);
// 强制中断并退出回零 - 加载到多电机指令上
void X_V2_MMCL_Origin_Interrupt(uint8_t addr);
// 修改回零参数 - 加载到多电机指令上
void X_V2_MMCL_Origin_Modify_Params(uint8_t addr, bool svF, uint8_t o_mode, uint8_t o_dir, uint16_t o_vel, uint32_t o_tm, uint16_t sl_vel, uint16_t sl_ma, uint16_t sl_ms, bool potF);
/**********************************************************
*** 读取系统参数命令
**********************************************************/
// 定时返回信息命令（Y42） - 加载到多电机指令上
void X_V2_MMCL_Auto_Return_Sys_Params_Timed(uint8_t addr, SysParams_t s, uint16_t time_ms);
// 读取系统参数 - 加载到多电机指令上
void X_V2_MMCL_Read_Sys_Params(uint8_t addr, SysParams_t s);
/**********************************************************
*** 读写驱动参数命令
**********************************************************/
#endif
modules/motor/step_motor/step_motor.c
+26
View File
@@ -0,0 +1,26 @@
#include "step_motor.h"
#include "X_V2.h"
#include "usart.h"
#define MOTOR_UHART0 &huart1
#define MOTOR_UHART1 &huart1
void step_sendcmd(uint8_t addr ,uint8_t *cmd, uint16_t len)
{
// 发送步进电机控制命令
if (len > 0 && cmd != NULL)
{
if(addr == 1 || addr == 2)
{
HAL_UART_Transmit_DMA(MOTOR_UHART0, cmd, len );
}
else if (addr == 3)
{
HAL_UART_Transmit_DMA(MOTOR_UHART1, cmd, len );
}
}
}
modules/motor/step_motor/step_motor.h
+12
View File
@@ -0,0 +1,12 @@
#ifndef STEP_MOTOR_H
#define STEP_MOTOR_H
#include <stdint.h>
#include <stdbool.h>
// 步进电机控制命令
void step_sendcmd(uint8_t addr, uint8_t *cmd, uint16_t len);
#endif
modules/oled/oled.c
+378
View File
@@ -0,0 +1,378 @@
#include "oled.h"
#include "oledfont.h"
#include "main.h"
#include <stdio.h>
#include <stdarg.h>
extern I2C_HandleTypeDef hi2c2;
static uint8_t OLED_GRAM[128][8];
/**
* @brief 写数据或者指令到OLED， 如果使用的是SPI，请重写这个函数
* @param[in] dat: 要写入的字节
* @param[in] cmd: OLED_CMD 代表写入的字节是指令; OLED_DATA 代表写入的字节是数据
* @retval none
*/
void oled_write_byte(uint8_t dat, uint8_t cmd)
{
static uint8_t cmd_data[2];
if(cmd == OLED_CMD)
{
cmd_data[0] = 0x00;
}
else
{
cmd_data[0] = 0x40;
}
cmd_data[1] = dat;
HAL_I2C_Master_Transmit(&hi2c2, OLED_I2C_ADDRESS, cmd_data, 2, 10);
}
/**
* @brief 初始化OLED模块，
* @param[in] none
* @retval none
*/
void OLED_init(void)
{
oled_write_byte(0xAE, OLED_CMD); //display off
oled_write_byte(0x20, OLED_CMD); //Set Memory Addressing Mode
oled_write_byte(0x10, OLED_CMD); //00,Horizontal Addressing Mode;01,Vertical Addressing Mode;10,Page Addressing Mode (RESET);11,Invalid
oled_write_byte(0xb0, OLED_CMD); //Set Page Start Address for Page Addressing Mode,0-7
oled_write_byte(0xc8, OLED_CMD); //Set COM Output Scan Direction
oled_write_byte(0x00, OLED_CMD); //---set low column address
oled_write_byte(0x10, OLED_CMD); //---set high column address
oled_write_byte(0x40, OLED_CMD); //--set start line address
oled_write_byte(0x81, OLED_CMD); //--set contrast control register
oled_write_byte(0xff, OLED_CMD); //brightness 0x00~0xff
oled_write_byte(0xa1, OLED_CMD); //--set segment re-map 0 to 127
oled_write_byte(0xa6, OLED_CMD); //--set normal display
oled_write_byte(0xa8, OLED_CMD); //--set multiplex ratio(1 to 64)
oled_write_byte(0x3F, OLED_CMD); //
oled_write_byte(0xa4, OLED_CMD); //0xa4,Output follows RAM content;0xa5,Output ignores RAM content
oled_write_byte(0xd3, OLED_CMD); //-set display offset
oled_write_byte(0x00, OLED_CMD); //-not offset
oled_write_byte(0xd5, OLED_CMD); //--set display clock divide ratio/oscillator frequency
oled_write_byte(0xf0, OLED_CMD); //--set divide ratio
oled_write_byte(0xd9, OLED_CMD); //--set pre-charge period
oled_write_byte(0x22, OLED_CMD); //
oled_write_byte(0xda, OLED_CMD); //--set com pins hardware configuration
oled_write_byte(0x12, OLED_CMD);
oled_write_byte(0xdb, OLED_CMD); //--set vcomh
oled_write_byte(0x20, OLED_CMD); //0x20,0.77xVcc
oled_write_byte(0x8d, OLED_CMD); //--set DC-DC enable
oled_write_byte(0x14, OLED_CMD); //
oled_write_byte(0xaf, OLED_CMD); //--turn on oled panel
}
/**
* @brief 打开OLED显示
* @param[in] none
* @retval none
*/
void OLED_display_on(void)
{
oled_write_byte(0x8d, OLED_CMD);
oled_write_byte(0x14, OLED_CMD);
oled_write_byte(0xaf, OLED_CMD);
}
/**
* @brief 关闭OLED显示
* @param[in] none
* @retval none
*/
void OLED_display_off(void)
{
oled_write_byte(0x8d, OLED_CMD);
oled_write_byte(0x10, OLED_CMD);
oled_write_byte(0xae, OLED_CMD);
}
/**
* @brief 操作GRAM内存(128*8char数组)
* @param[in] pen: 操作类型.
PEN_CLEAR: 设置为0x00
PEN_WRITE: 设置为0xff
PEN_INVERSION: 按位取反
* @retval none
*/
void OLED_operate_gram(pen_typedef pen)
{
uint8_t i, n;
for (i = 0; i < 8; i++)
{
for (n = 0; n < 128; n++)
{
if (pen == PEN_WRITE)
{
OLED_GRAM[n][i] = 0xff;
}
else if (pen == PEN_CLEAR)
{
OLED_GRAM[n][i] = 0x00;
}
else
{
OLED_GRAM[n][i] = 0xff - OLED_GRAM[n][i];
}
}
}
}
/**
* @brief 设置光标起点(x,y)
* @param[in] x:x轴, 从 0 到 127
* @param[in] y:y轴, 从 0 到 7
* @retval none
*/
void OLED_set_pos(uint8_t x, uint8_t y)
{
oled_write_byte((0xb0 + y), OLED_CMD); //set page address y
oled_write_byte(((x&0xf0)>>4)|0x10, OLED_CMD); //set column high address
oled_write_byte((x&0x0f), OLED_CMD); //set column low address
}
/**
* @brief 操作GRAM中的一个位，相当于操作屏幕的一个点
* @param[in] x:x轴, [0,X_WIDTH-1]
* @param[in] y:y轴, [0,Y_WIDTH-1]
* @param[in] pen: 操作类型,
PEN_CLEAR: 设置 (x,y) 点为 0
PEN_WRITE: 设置 (x,y) 点为 1
PEN_INVERSION: (x,y) 值反转
* @retval none
*/
void OLED_draw_point(int8_t x, int8_t y, pen_typedef pen)
{
uint8_t page = 0, row = 0;
/* check the corrdinate */
if ((x < 0) || (x > (X_WIDTH - 1)) || (y < 0) || (y > (Y_WIDTH - 1)))
{
return;
}
page = y / 8;
row = y % 8;
if (pen == PEN_WRITE)
{
OLED_GRAM[x][page] |= 1 << row;
}
else if (pen == PEN_INVERSION)
{
OLED_GRAM[x][page] ^= 1 << row;
}
else
{
OLED_GRAM[x][page] &= ~(1 << row);
}
}
/**
* @brief 画一条直线，从(x1,y1)到(x2,y2)
* @param[in] x1: 起点
* @param[in] y1: 起点
* @param[in] x2: 终点
* @param[in] y2: 终点
* @param[in] pen: 操作类型,PEN_CLEAR,PEN_WRITE,PEN_INVERSION.
* @retval none
*/
void OLED_draw_line(uint8_t x1, uint8_t y1, uint8_t x2, uint8_t y2, pen_typedef pen)
{
uint8_t col = 0, row = 0;
uint8_t x_st = 0, x_ed = 0, y_st = 0, y_ed = 0;
float k = 0.0f, b = 0.0f;
if (y1 == y2)
{
(x1 <= x2) ? (x_st = x1):(x_st = x2);
(x1 <= x2) ? (x_ed = x2):(x_ed = x1);
for (col = x_st; col <= x_ed; col++)
{
OLED_draw_point(col, y1, pen);
}
}
else if (x1 == x2)
{
(y1 <= y2) ? (y_st = y1):(y_st = y2);
(y1 <= y2) ? (y_ed = y2):(y_ed = y1);
for (row = y_st; row <= y_ed; row++)
{
OLED_draw_point(x1, row, pen);
}
}
else
{
k = ((float)(y2 - y1)) / (x2 - x1);
b = (float)y1 - k * x1;
(x1 <= x2) ? (x_st = x1):(x_st = x2);
(x1 <= x2) ? (x_ed = x2):(x_ed = x2);
for (col = x_st; col <= x_ed; col++)
{
OLED_draw_point(col, (uint8_t)(col * k + b), pen);
}
}
}
/**
* @brief 显示一个字符
* @param[in] row: 字符的开始行
* @param[in] col: 字符的开始列
* @param[in] chr: 字符
* @retval none
*/
void OLED_show_char(uint8_t row, uint8_t col, uint8_t chr)
{
uint8_t x = col * 6;
uint8_t y = row * 12;
uint8_t temp, t, t1;
uint8_t y0 = y;
chr = chr - ' ';
for (t = 0; t < 12; t++)
{
temp = asc2_1206[chr][t];
for (t1 = 0; t1 < 8; t1++)
{
if (temp&0x80)
OLED_draw_point(x, y, PEN_WRITE);
else
OLED_draw_point(x, y, PEN_CLEAR);
temp <<= 1;
y++;
if ((y - y0) == 12)
{
y = y0;
x++;
break;
}
}
}
}
/**
* @brief 显示一个字符串
* @param[in] row: 字符串的开始行
* @param[in] col: 字符串的开始列
* @param[in] chr: 字符串
* @retval none
*/
void OLED_show_string(uint8_t row, uint8_t col, uint8_t *chr)
{
uint8_t n =0;
while (chr[n] != '\0')
{
OLED_show_char(row, col, chr[n]);
col++;
if (col > 20)
{
col = 0;
row += 1;
}
n++;
}
}
/**
* @brief 格式输出
* @param[in] row: 开始列，0 <= row <= 4;
* @param[in] col: 开始行， 0 <= col <= 20;
* @param[in] *fmt:格式化输出字符串
* @note 如果字符串长度大于一行，额外的字符会换行
* @retval none
*/
void OLED_printf(uint8_t row, uint8_t col, const char *fmt,...)
{
static uint8_t LCD_BUF[128] = {0};
static va_list ap;
uint8_t remain_size = 0;
if ((row > 4) || (col > 20) )
{
return;
}
va_start(ap, fmt);
vsprintf((char *)LCD_BUF, fmt, ap);
va_end(ap);
remain_size = 21 - col;
LCD_BUF[remain_size] = '\0';
OLED_show_string(row, col, LCD_BUF);
}
/**
* @brief 发送数据到OLED的GRAM
* @param[in] none
* @retval none
*/
void OLED_refresh_gram(void)
{
uint8_t i, n;
for (i = 0; i < 8; i++)
{
OLED_set_pos(0, i);
for (n = 0; n < 128; n++)
{
oled_write_byte(OLED_GRAM[n][i], OLED_DATA);
}
}
}
/**
* @brief 显示RM的LOGO
* @param[in] none
* @retval none
*/
void OLED_LOGO(void)
{
uint8_t temp_char = 0;
uint8_t x = 0, y = 0;
uint8_t i = 0;
OLED_operate_gram(PEN_CLEAR);
for(; y < 64; y += 8)
{
for(x = 0; x < 128; x++)
{
temp_char = LOGO_BMP[x][y/8];
for(i = 0; i < 8; i++)
{
if(temp_char & 0x80)
{
OLED_draw_point(x, y + i,PEN_WRITE);
}
else
{
OLED_draw_point(x,y + i,PEN_CLEAR);
}
temp_char <<= 1;
}
}
}
OLED_refresh_gram();
}
modules/oled/oled.h
+145
View File
@@ -0,0 +1,145 @@
/**
* @file oled.h
* @author your name (you@domain.com)
* @brief 待重构实现
* @version 0.1
* @date 2023-02-14
* @todo 请重构show string/init/clean/update buffer等的实现
*
* @copyright Copyright (c) 2023
*
*/
#ifndef OLED_H
#define OLED_H
#include <stdint.h>
// the I2C address of oled
#define OLED_I2C_ADDRESS 0x78
// the resolution of oled 128*64
#define MAX_COLUMN 128
#define MAX_ROW 64
#define X_WIDTH MAX_COLUMN
#define Y_WIDTH MAX_ROW
#define OLED_CMD 0x00
#define OLED_DATA 0x01
#define CHAR_SIZE_WIDTH 6
#define CHAR_SIZE_HIGHT 12
typedef enum
{
PEN_CLEAR = 0x00,
PEN_WRITE = 0x01,
PEN_INVERSION = 0x02,
} pen_typedef;
/**
* @brief 初始化OLED模块，
* @param[in] none
* @retval none
*/
extern void OLED_init(void);
/**
* @brief 打开OLED显示
* @param[in] none
* @retval none
*/
extern void OLED_display_on(void);
/**
* @brief 关闭OLED显示
* @param[in] none
* @retval none
*/
extern void OLED_display_off(void);
/**
* @brief 操作GRAM内存(128*8char数组)
* @param[in] pen: 操作类型.
PEN_CLEAR: 设置为0x00
PEN_WRITE: 设置为0xff
PEN_INVERSION: 按位取反
* @retval none
*/
extern void OLED_operate_gram(pen_typedef pen);
/**
* @brief 设置光标起点(x,y)
* @param[in] x:x轴, 从 0 到 127
* @param[in] y:y轴, 从 0 到 7
* @retval none
*/
extern void OLED_set_pos(uint8_t x, uint8_t y);
/**
* @brief 操作GRAM中的一个位，相当于操作屏幕的一个点
* @param[in] x:x轴, [0,X_WIDTH-1]
* @param[in] y:y轴, [0,Y_WIDTH-1]
* @param[in] pen: 操作类型,
PEN_CLEAR: 设置 (x,y) 点为 0
PEN_WRITE: 设置 (x,y) 点为 1
PEN_INVERSION: (x,y) 值反转
* @retval none
*/
extern void OLED_draw_point(int8_t x, int8_t y, pen_typedef pen);
/**
* @brief 画一条直线，从(x1,y1)到(x2,y2)
* @param[in] x1: 起点
* @param[in] y1: 起点
* @param[in] x2: 终点
* @param[in] y2: 终点
* @param[in] pen: 操作类型,PEN_CLEAR,PEN_WRITE,PEN_INVERSION.
* @retval none
*/
extern void OLED_draw_line(uint8_t x1, uint8_t y1, uint8_t x2, uint8_t y2, pen_typedef pen);
/**
* @brief 显示一个字符
* @param[in] row: 字符的开始行
* @param[in] col: 字符的开始列
* @param[in] chr: 字符
* @retval none
*/
extern void OLED_show_char(uint8_t row, uint8_t col, uint8_t chr);
/**
* @brief 显示一个字符串
* @param[in] row: 字符串的开始行
* @param[in] col: 字符串的开始列
* @param[in] chr: 字符串
* @retval none
*/
extern void OLED_show_string(uint8_t row, uint8_t col, uint8_t *chr);
/**
* @brief 格式输出
* @param[in] row: 开始列，0 <= row <= 4;
* @param[in] col: 开始行， 0 <= col <= 20;
* @param[in] *fmt:格式化输出字符串
* @note 如果字符串长度大于一行，额外的字符会换行
* @retval none
*/
extern void OLED_printf(uint8_t row, uint8_t col, const char *fmt, ...);
/**
* @brief 发送数据到OLED的GRAM
* @param[in] none
* @retval none
*/
extern void OLED_refresh_gram(void);
/**
* @brief 显示RM的LOGO
* @param[in] none
* @retval none
*/
extern void OLED_LOGO(void);
#endif
modules/oled/oled.md
+11
View File
@@ -0,0 +1,11 @@
# oled
当前oled支持不完整,api较为混乱. 需要使用bsp_iic进行实现的重构,并提供统一方便的接口
请使用字库软件制作自己的图标和不同大小的ascii码.
> 后续尝试移植一些图形库使得功能更加丰富
> oled主要作调试和log/错误显示等使用
> 可以提供给视觉和机械的同学调试接口,方便他们通过显示屏进行简单的设置
*可以引入RoboMaster oled,或额外增加一个编码器用于控制oled界面并设定一些功能.*
modules/oled/oledfont.h
+247
View File
@@ -0,0 +1,247 @@
/**
***************************************(C) COPYRIGHT 2019 DJI***************************************
* @file oledfont.h
* @brief character lib of oled
* @note
* @Version V1.0.0
* @Date Oct-7-2019
***************************************(C) COPYRIGHT 2019 DJI***************************************
*/
#ifndef __OLED__FONT__H
#define __OLED__FONT__H
//the common ascii character
const unsigned char asc2_1206[95][12]={
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},/*" ",0*/
{0x00,0x00,0x00,0x00,0x3F,0x40,0x00,0x00,0x00,0x00,0x00,0x00},/*"!",1*/
{0x00,0x00,0x30,0x00,0x40,0x00,0x30,0x00,0x40,0x00,0x00,0x00},/*""",2*/
{0x09,0x00,0x0B,0xC0,0x3D,0x00,0x0B,0xC0,0x3D,0x00,0x09,0x00},/*"#",3*/
{0x18,0xC0,0x24,0x40,0x7F,0xE0,0x22,0x40,0x31,0x80,0x00,0x00},/*"$",4*/
{0x18,0x00,0x24,0xC0,0x1B,0x00,0x0D,0x80,0x32,0x40,0x01,0x80},/*"%",5*/
{0x03,0x80,0x1C,0x40,0x27,0x40,0x1C,0x80,0x07,0x40,0x00,0x40},/*"&",6*/
{0x10,0x00,0x60,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},/*"'",7*/
{0x00,0x00,0x00,0x00,0x00,0x00,0x1F,0x80,0x20,0x40,0x40,0x20},/*"(",8*/
{0x00,0x00,0x40,0x20,0x20,0x40,0x1F,0x80,0x00,0x00,0x00,0x00},/*")",9*/
{0x09,0x00,0x06,0x00,0x1F,0x80,0x06,0x00,0x09,0x00,0x00,0x00},/*"*",10*/
{0x04,0x00,0x04,0x00,0x3F,0x80,0x04,0x00,0x04,0x00,0x00,0x00},/*"+",11*/
{0x00,0x10,0x00,0x60,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},/*",",12*/
{0x04,0x00,0x04,0x00,0x04,0x00,0x04,0x00,0x04,0x00,0x00,0x00},/*"-",13*/
{0x00,0x00,0x00,0x40,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},/*".",14*/
{0x00,0x20,0x01,0xC0,0x06,0x00,0x38,0x00,0x40,0x00,0x00,0x00},/*"/",15*/
{0x1F,0x80,0x20,0x40,0x20,0x40,0x20,0x40,0x1F,0x80,0x00,0x00},/*"0",16*/
{0x00,0x00,0x10,0x40,0x3F,0xC0,0x00,0x40,0x00,0x00,0x00,0x00},/*"1",17*/
{0x18,0xC0,0x21,0x40,0x22,0x40,0x24,0x40,0x18,0x40,0x00,0x00},/*"2",18*/
{0x10,0x80,0x20,0x40,0x24,0x40,0x24,0x40,0x1B,0x80,0x00,0x00},/*"3",19*/
{0x02,0x00,0x0D,0x00,0x11,0x00,0x3F,0xC0,0x01,0x40,0x00,0x00},/*"4",20*/
{0x3C,0x80,0x24,0x40,0x24,0x40,0x24,0x40,0x23,0x80,0x00,0x00},/*"5",21*/
{0x1F,0x80,0x24,0x40,0x24,0x40,0x34,0x40,0x03,0x80,0x00,0x00},/*"6",22*/
{0x30,0x00,0x20,0x00,0x27,0xC0,0x38,0x00,0x20,0x00,0x00,0x00},/*"7",23*/
{0x1B,0x80,0x24,0x40,0x24,0x40,0x24,0x40,0x1B,0x80,0x00,0x00},/*"8",24*/
{0x1C,0x00,0x22,0xC0,0x22,0x40,0x22,0x40,0x1F,0x80,0x00,0x00},/*"9",25*/
{0x00,0x00,0x00,0x00,0x08,0x40,0x00,0x00,0x00,0x00,0x00,0x00},/*":",26*/
{0x00,0x00,0x00,0x00,0x04,0x60,0x00,0x00,0x00,0x00,0x00,0x00},/*";",27*/
{0x00,0x00,0x04,0x00,0x0A,0x00,0x11,0x00,0x20,0x80,0x40,0x40},/*"<",28*/
{0x09,0x00,0x09,0x00,0x09,0x00,0x09,0x00,0x09,0x00,0x00,0x00},/*"=",29*/
{0x00,0x00,0x40,0x40,0x20,0x80,0x11,0x00,0x0A,0x00,0x04,0x00},/*">",30*/
{0x18,0x00,0x20,0x00,0x23,0x40,0x24,0x00,0x18,0x00,0x00,0x00},/*"?",31*/
{0x1F,0x80,0x20,0x40,0x27,0x40,0x29,0x40,0x1F,0x40,0x00,0x00},/*"@",32*/
{0x00,0x40,0x07,0xC0,0x39,0x00,0x0F,0x00,0x01,0xC0,0x00,0x40},/*"A",33*/
{0x20,0x40,0x3F,0xC0,0x24,0x40,0x24,0x40,0x1B,0x80,0x00,0x00},/*"B",34*/
{0x1F,0x80,0x20,0x40,0x20,0x40,0x20,0x40,0x30,0x80,0x00,0x00},/*"C",35*/
{0x20,0x40,0x3F,0xC0,0x20,0x40,0x20,0x40,0x1F,0x80,0x00,0x00},/*"D",36*/
{0x20,0x40,0x3F,0xC0,0x24,0x40,0x2E,0x40,0x30,0xC0,0x00,0x00},/*"E",37*/
{0x20,0x40,0x3F,0xC0,0x24,0x40,0x2E,0x00,0x30,0x00,0x00,0x00},/*"F",38*/
{0x0F,0x00,0x10,0x80,0x20,0x40,0x22,0x40,0x33,0x80,0x02,0x00},/*"G",39*/
{0x20,0x40,0x3F,0xC0,0x04,0x00,0x04,0x00,0x3F,0xC0,0x20,0x40},/*"H",40*/
{0x20,0x40,0x20,0x40,0x3F,0xC0,0x20,0x40,0x20,0x40,0x00,0x00},/*"I",41*/
{0x00,0x60,0x20,0x20,0x20,0x20,0x3F,0xC0,0x20,0x00,0x20,0x00},/*"J",42*/
{0x20,0x40,0x3F,0xC0,0x24,0x40,0x0B,0x00,0x30,0xC0,0x20,0x40},/*"K",43*/
{0x20,0x40,0x3F,0xC0,0x20,0x40,0x00,0x40,0x00,0x40,0x00,0xC0},/*"L",44*/
{0x3F,0xC0,0x3C,0x00,0x03,0xC0,0x3C,0x00,0x3F,0xC0,0x00,0x00},/*"M",45*/
{0x20,0x40,0x3F,0xC0,0x0C,0x40,0x23,0x00,0x3F,0xC0,0x20,0x00},/*"N",46*/
{0x1F,0x80,0x20,0x40,0x20,0x40,0x20,0x40,0x1F,0x80,0x00,0x00},/*"O",47*/
{0x20,0x40,0x3F,0xC0,0x24,0x40,0x24,0x00,0x18,0x00,0x00,0x00},/*"P",48*/
{0x1F,0x80,0x21,0x40,0x21,0x40,0x20,0xE0,0x1F,0xA0,0x00,0x00},/*"Q",49*/
{0x20,0x40,0x3F,0xC0,0x24,0x40,0x26,0x00,0x19,0xC0,0x00,0x40},/*"R",50*/
{0x18,0xC0,0x24,0x40,0x24,0x40,0x22,0x40,0x31,0x80,0x00,0x00},/*"S",51*/
{0x30,0x00,0x20,0x40,0x3F,0xC0,0x20,0x40,0x30,0x00,0x00,0x00},/*"T",52*/
{0x20,0x00,0x3F,0x80,0x00,0x40,0x00,0x40,0x3F,0x80,0x20,0x00},/*"U",53*/
{0x20,0x00,0x3E,0x00,0x01,0xC0,0x07,0x00,0x38,0x00,0x20,0x00},/*"V",54*/
{0x38,0x00,0x07,0xC0,0x3C,0x00,0x07,0xC0,0x38,0x00,0x00,0x00},/*"W",55*/
{0x20,0x40,0x39,0xC0,0x06,0x00,0x39,0xC0,0x20,0x40,0x00,0x00},/*"X",56*/
{0x20,0x00,0x38,0x40,0x07,0xC0,0x38,0x40,0x20,0x00,0x00,0x00},/*"Y",57*/
{0x30,0x40,0x21,0xC0,0x26,0x40,0x38,0x40,0x20,0xC0,0x00,0x00},/*"Z",58*/
{0x00,0x00,0x00,0x00,0x7F,0xE0,0x40,0x20,0x40,0x20,0x00,0x00},/*"[",59*/
{0x00,0x00,0x70,0x00,0x0C,0x00,0x03,0x80,0x00,0x40,0x00,0x00},/*"\",60*/
{0x00,0x00,0x40,0x20,0x40,0x20,0x7F,0xE0,0x00,0x00,0x00,0x00},/*"]",61*/
{0x00,0x00,0x20,0x00,0x40,0x00,0x20,0x00,0x00,0x00,0x00,0x00},/*"^",62*/
{0x00,0x10,0x00,0x10,0x00,0x10,0x00,0x10,0x00,0x10,0x00,0x10},/*"_",63*/
{0x00,0x00,0x00,0x00,0x40,0x00,0x00,0x00,0x00,0x00,0x00,0x00},/*"`",64*/
{0x00,0x00,0x02,0x80,0x05,0x40,0x05,0x40,0x03,0xC0,0x00,0x40},/*"a",65*/
{0x20,0x00,0x3F,0xC0,0x04,0x40,0x04,0x40,0x03,0x80,0x00,0x00},/*"b",66*/
{0x00,0x00,0x03,0x80,0x04,0x40,0x04,0x40,0x06,0x40,0x00,0x00},/*"c",67*/
{0x00,0x00,0x03,0x80,0x04,0x40,0x24,0x40,0x3F,0xC0,0x00,0x40},/*"d",68*/
{0x00,0x00,0x03,0x80,0x05,0x40,0x05,0x40,0x03,0x40,0x00,0x00},/*"e",69*/
{0x00,0x00,0x04,0x40,0x1F,0xC0,0x24,0x40,0x24,0x40,0x20,0x00},/*"f",70*/
{0x00,0x00,0x02,0xE0,0x05,0x50,0x05,0x50,0x06,0x50,0x04,0x20},/*"g",71*/
{0x20,0x40,0x3F,0xC0,0x04,0x40,0x04,0x00,0x03,0xC0,0x00,0x40},/*"h",72*/
{0x00,0x00,0x04,0x40,0x27,0xC0,0x00,0x40,0x00,0x00,0x00,0x00},/*"i",73*/
{0x00,0x10,0x00,0x10,0x04,0x10,0x27,0xE0,0x00,0x00,0x00,0x00},/*"j",74*/
{0x20,0x40,0x3F,0xC0,0x01,0x40,0x07,0x00,0x04,0xC0,0x04,0x40},/*"k",75*/
{0x20,0x40,0x20,0x40,0x3F,0xC0,0x00,0x40,0x00,0x40,0x00,0x00},/*"l",76*/
{0x07,0xC0,0x04,0x00,0x07,0xC0,0x04,0x00,0x03,0xC0,0x00,0x00},/*"m",77*/
{0x04,0x40,0x07,0xC0,0x04,0x40,0x04,0x00,0x03,0xC0,0x00,0x40},/*"n",78*/
{0x00,0x00,0x03,0x80,0x04,0x40,0x04,0x40,0x03,0x80,0x00,0x00},/*"o",79*/
{0x04,0x10,0x07,0xF0,0x04,0x50,0x04,0x40,0x03,0x80,0x00,0x00},/*"p",80*/
{0x00,0x00,0x03,0x80,0x04,0x40,0x04,0x50,0x07,0xF0,0x00,0x10},/*"q",81*/
{0x04,0x40,0x07,0xC0,0x02,0x40,0x04,0x00,0x04,0x00,0x00,0x00},/*"r",82*/
{0x00,0x00,0x06,0x40,0x05,0x40,0x05,0x40,0x04,0xC0,0x00,0x00},/*"s",83*/
{0x00,0x00,0x04,0x00,0x1F,0x80,0x04,0x40,0x00,0x40,0x00,0x00},/*"t",84*/
{0x04,0x00,0x07,0x80,0x00,0x40,0x04,0x40,0x07,0xC0,0x00,0x40},/*"u",85*/
{0x04,0x00,0x07,0x00,0x04,0xC0,0x01,0x80,0x06,0x00,0x04,0x00},/*"v",86*/
{0x06,0x00,0x01,0xC0,0x07,0x00,0x01,0xC0,0x06,0x00,0x00,0x00},/*"w",87*/
{0x04,0x40,0x06,0xC0,0x01,0x00,0x06,0xC0,0x04,0x40,0x00,0x00},/*"x",88*/
{0x04,0x10,0x07,0x10,0x04,0xE0,0x01,0x80,0x06,0x00,0x04,0x00},/*"y",89*/
{0x00,0x00,0x04,0x40,0x05,0xC0,0x06,0x40,0x04,0x40,0x00,0x00},/*"z",90*/
{0x00,0x00,0x00,0x00,0x04,0x00,0x7B,0xE0,0x40,0x20,0x00,0x00},/*"{",91*/
{0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0xF0,0x00,0x00,0x00,0x00},/*"|",92*/
{0x00,0x00,0x40,0x20,0x7B,0xE0,0x04,0x00,0x00,0x00,0x00,0x00},/*"}",93*/
{0x40,0x00,0x80,0x00,0x40,0x00,0x20,0x00,0x20,0x00,0x40,0x00},/*"~",94*/
};
// 你可以使用图标字库软件生成自己的pattern,软件会输出一个数组,将数组复制到这里即可.
//the logo of robomaster
const unsigned char LOGO_BMP[128][8] = {
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x06},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x1E},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xDE},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xD8},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xD8},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xD8},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xDE},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFE},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xF6},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x72},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x7C},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFE},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xC6},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xC6},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xC6},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFE},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x7C},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x1E},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xDE},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xD6},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xD6},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xD6},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xD6},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xDE},
{0x40,0x00,0x00,0x00,0x00,0x00,0x00,0xFE},
{0x60,0x00,0x00,0x00,0x00,0x10,0x00,0x6C},
{0x70,0x00,0x00,0x00,0x00,0x30,0x00,0x00},
{0x78,0x00,0x00,0x00,0x00,0xF0,0x00,0x7C},
{0x7C,0x00,0x00,0x00,0x07,0xF0,0x00,0xFE},
{0x7E,0x00,0x00,0x00,0x3F,0xF0,0x00,0xC6},
{0x7F,0x00,0x00,0x01,0xFF,0xF0,0x00,0xC6},
{0x7F,0x80,0x00,0x0F,0xFF,0xF0,0x00,0xC6},
{0x7F,0xC0,0x00,0x7F,0xFF,0xF0,0x00,0xFE},
{0x7F,0xE0,0x03,0xFF,0xFF,0xF0,0x00,0x7C},
{0x7F,0xF0,0x3F,0xFF,0xFF,0xF0,0x00,0x02},
{0x7F,0xF8,0x3F,0xFF,0xFF,0xF0,0x00,0x06},
{0x7F,0xFC,0x3F,0xFF,0xFF,0xF0,0x00,0x1E},
{0x7F,0xFE,0x3F,0xFF,0xFF,0xF0,0x00,0xBC},
{0x7F,0xFF,0x3F,0xFF,0xFF,0xF0,0x00,0xE0},
{0x7F,0xFF,0xBF,0xFF,0xFF,0x80,0x00,0xF8},
{0x7F,0xFF,0xFF,0xFF,0xFC,0x00,0x00,0x3E},
{0x7F,0xFF,0xFF,0xFF,0xE0,0x00,0x00,0x0E},
{0x7F,0xFF,0xFF,0xFF,0x00,0x00,0x00,0xB8},
{0x7F,0xFF,0xFF,0xF8,0x00,0x00,0x00,0xE0},
{0x7F,0xFF,0xFF,0xF0,0x00,0x00,0x00,0xFE},
{0x7F,0xFF,0xFF,0xF0,0x00,0x00,0x00,0x1E},
{0x7F,0xFF,0xFF,0xF0,0x00,0x00,0x00,0x02},
{0x7F,0xFF,0xFF,0xF0,0x00,0x00,0x00,0x00},
{0x7F,0xEF,0xFF,0xF0,0x02,0x00,0x00,0x06},
{0x7F,0xE7,0xFF,0xF0,0x02,0x00,0x00,0x0E},
{0x7F,0xE3,0xFF,0xF0,0x02,0x00,0x00,0x1C},
{0x7F,0xE1,0xFF,0xF8,0x03,0x00,0x00,0xBA},
{0x7F,0xE0,0xFF,0xFC,0x03,0x00,0x00,0xF6},
{0x7F,0xE0,0x7F,0xFE,0x03,0x80,0x00,0xE6},
{0x7F,0xE0,0x3F,0xFF,0x03,0x80,0x00,0xF6},
{0x7F,0xE0,0x3F,0xFF,0x83,0xC0,0x00,0x3E},
{0x7F,0xE0,0x3F,0xFF,0xC3,0xC0,0x00,0x0E},
{0x7F,0xE0,0x3F,0xFF,0xE3,0xE0,0x00,0x02},
{0x7F,0xE0,0x3F,0xFF,0xF3,0xE0,0x00,0x00},
{0x7F,0xE0,0x3F,0xFF,0xFB,0xF0,0x00,0x02},
{0x7F,0xE0,0x3F,0xFF,0xFF,0xF0,0x00,0x66},
{0x7F,0xE0,0x3F,0xFF,0xFF,0xF8,0x00,0xF6},
{0x7F,0xE0,0x3F,0xFF,0xFF,0xF8,0x00,0xD6},
{0x7F,0xE0,0x3F,0xFF,0xFF,0xFC,0x00,0xD6},
{0x7F,0xF0,0x7F,0xFF,0xFF,0xFC,0x00,0xD6},
{0x7F,0xF8,0xFF,0xF7,0xFF,0xFE,0x00,0xD6},
{0x7F,0xFF,0xFF,0xF3,0xFF,0xFE,0x00,0xDE},
{0x3F,0xFF,0xFF,0xE1,0xFF,0xFF,0x00,0x8C},
{0x3F,0xFF,0xFF,0xE0,0xFF,0xCF,0x00,0x40},
{0x1F,0xFF,0xFF,0xC0,0x7F,0xC7,0x80,0xC0},
{0x1F,0xFF,0xFF,0xC0,0x3F,0xC3,0x80,0xC0},
{0x0F,0xFF,0xFF,0x80,0x1F,0xC1,0xC0,0xFE},
{0x07,0xFF,0xFF,0x00,0x0F,0xC0,0xC0,0xFE},
{0x03,0xFF,0xFE,0x00,0x07,0xC0,0x60,0xC0},
{0x01,0xFF,0xFC,0x00,0x03,0xC0,0x20,0xC0},
{0x00,0x7F,0xF0,0x00,0x01,0xC0,0x00,0x86},
{0x00,0x0F,0x80,0x00,0x00,0xC0,0x00,0x16},
{0x00,0x00,0x00,0x00,0x00,0x40,0x00,0xD6},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xD6},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xD6},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xD6},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xD6},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xD0},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xC0},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x06},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x1E},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xDE},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xD8},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xD8},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xD8},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xDE},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xDE},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xF6},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x72},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x62},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xF6},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xF6},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xD6},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xD6},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xD6},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xDE},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x8C},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
{0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
};
#endif
modules/opencv/opencv.c
+99
View File
@@ -0,0 +1,99 @@
#include "opencv.h"
#include "daemon.h"
#define FRAME_SOF 0xA5
#define FRAME_PAYLOAD_LEN (2+2+4*12) // 52
#define FRAME_FULL_LEN (1+1+FRAME_PAYLOAD_LEN) //54
float opencv_Vision_data[12];
uint8_t Vison_revc_flag = 0; // 视觉数据接收标志位
uint8_t vision_used_flag = 0; // 视觉数据使用标志位,用于判断是否需要使用视觉数据
float Vison_Frequency; // 视觉数据接收频率,单位Hz
static USARTInstance *cv_usart_instance;
static DaemonInstance *OPENCV_vision_daemon_instance;
/**
* @brief 接收解包回调函数,将在bsp_usart.c中被usart rx callback调用
* @todo 1.提高可读性,将get_protocol_info的第四个参数增加一个float类型buffer
* 2.添加标志位解码
*/
static void DecodeVision()
{
DaemonReload(OPENCV_vision_daemon_instance); // 喂狗,防止离线
static uint32_t counter;
float dt = DWT_GetDeltaT(&counter);
Vison_Frequency = 1.0f / dt; // 计算频率
const uint16_t len = cv_usart_instance->recv_buff_size;
uint16_t flag_register;
uint8_t *buf = cv_usart_instance->recv_buff;
if (len != FRAME_FULL_LEN) {
// 长度不符
LOGERROR("Vision frame length error");
return;
}
if (buf[0] != FRAME_SOF) {
// SOF 错误
LOGERROR("Vision frame SOF error");
return;
}
uint8_t payload_len = buf[1];
if (payload_len != FRAME_PAYLOAD_LEN) {
// Payload 长度不符
LOGERROR("Vision frame payload length error");
return;
}
// 解析命令号和 flags（小端）
uint16_t cmd_id = buf[2] | (buf[3] << 8);
uint16_t flags = buf[4] | (buf[5] << 8);
// 解析 12 个 float
for (int i = 0; i < 12; i++) {
// 从 buf[6 + 4*i] 连续读 4 字节
memcpy(&opencv_Vision_data[i], buf + 6 + 4*i, sizeof(float));
}
// // 解析任务标志位(拟合最接近的int)
// int last_task_cmd = Vison_task_cmd;
// Vison_task_cmd = (int)(Vison_data[10] + 0.5f);
// if (Vison_task_cmd != last_task_cmd) {
// Vison_task_changed = 1; // 任务命令变化
// }
Vison_revc_flag = 1; // 设置接收标志位
// vision_used_flag = 1; // 设置视觉数据使用标志位
//解析暂停标志位
// run = (int)(Vison_data[11] + 0.5f);
// 处理...
// printf("Got frame: cmd=0x%04X flags=0x%04X\n", cmd_id, flags);
// for (int i = 0; i < 12; i += 2) {
// printf(" coord %d: (%.3f, %.3f)\n",
// i/2, data[i], data[i+1]);
// float a = data[0];
// TODO: code to resolve flag_register;
}
void VisionOfflineCallback(void *owner_id)
{
Vison_revc_flag = 0; // 清除接收标志位
}
void Vision_Uart_Init(UART_HandleTypeDef *_handle)
{
USART_Init_Config_s conf;
conf.module_callback = DecodeVision;
conf.recv_buff_size = 54;
conf.usart_handle = _handle;
cv_usart_instance = USARTRegister(&conf);
// 为master process注册daemon,用于判断视觉通信是否离线
Daemon_Init_Config_s daemon_conf = {
.callback = VisionOfflineCallback, // 离线时调用的回调函数,会重启串口接收
.owner_id = NULL,
.reload_count = 10,
};
OPENCV_vision_daemon_instance = DaemonRegister(&daemon_conf);
}
modules/opencv/opencv.h
+22
View File
@@ -0,0 +1,22 @@
#ifndef OPENCV_H
#define OPENCV_H
#include "bsp_usart.h"
#include "opencv.h"
#include "bsp_log.h"
#include "bsp_dwt.h"
#include "usart.h"
#include "robot_def.h"
#include <string.h>
#include <stdio.h>
void Vision_Uart_Init(UART_HandleTypeDef *_handle);
extern float opencv_Vision_data[12]; // 视觉数据,12个float
extern uint8_t Vison_revc_flag; // 视觉数据接收标志位
extern uint8_t vision_used_flag; // 视觉数据使用标志位,用于判断是否需要使用视觉数据
extern float Vison_Frequency; // 视觉数据频率
#endif // !OPENCV_H
modules/referee/crc_ref.c
+192
View File
@@ -0,0 +1,192 @@
#include "crc_ref.h"
#include <stdio.h>
const uint8_t CRC8_INIT = 0xff;
const uint8_t CRC8_TAB[256] =
{
0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83, 0xc2, 0x9c, 0x7e, 0x20, 0xa3, 0xfd, 0x1f, 0x41,
0x9d, 0xc3, 0x21, 0x7f, 0xfc, 0xa2, 0x40, 0x1e, 0x5f, 0x01, 0xe3, 0xbd, 0x3e, 0x60, 0x82, 0xdc,
0x23, 0x7d, 0x9f, 0xc1, 0x42, 0x1c, 0xfe, 0xa0, 0xe1, 0xbf, 0x5d, 0x03, 0x80, 0xde, 0x3c, 0x62,
0xbe, 0xe0, 0x02, 0x5c, 0xdf, 0x81, 0x63, 0x3d, 0x7c, 0x22, 0xc0, 0x9e, 0x1d, 0x43, 0xa1, 0xff,
0x46, 0x18, 0xfa, 0xa4, 0x27, 0x79, 0x9b, 0xc5, 0x84, 0xda, 0x38, 0x66, 0xe5, 0xbb, 0x59, 0x07,
0xdb, 0x85, 0x67, 0x39, 0xba, 0xe4, 0x06, 0x58, 0x19, 0x47, 0xa5, 0xfb, 0x78, 0x26, 0xc4, 0x9a,
0x65, 0x3b, 0xd9, 0x87, 0x04, 0x5a, 0xb8, 0xe6, 0xa7, 0xf9, 0x1b, 0x45, 0xc6, 0x98, 0x7a, 0x24,
0xf8, 0xa6, 0x44, 0x1a, 0x99, 0xc7, 0x25, 0x7b, 0x3a, 0x64, 0x86, 0xd8, 0x5b, 0x05, 0xe7, 0xb9,
0x8c, 0xd2, 0x30, 0x6e, 0xed, 0xb3, 0x51, 0x0f, 0x4e, 0x10, 0xf2, 0xac, 0x2f, 0x71, 0x93, 0xcd,
0x11, 0x4f, 0xad, 0xf3, 0x70, 0x2e, 0xcc, 0x92, 0xd3, 0x8d, 0x6f, 0x31, 0xb2, 0xec, 0x0e, 0x50,
0xaf, 0xf1, 0x13, 0x4d, 0xce, 0x90, 0x72, 0x2c, 0x6d, 0x33, 0xd1, 0x8f, 0x0c, 0x52, 0xb0, 0xee,
0x32, 0x6c, 0x8e, 0xd0, 0x53, 0x0d, 0xef, 0xb1, 0xf0, 0xae, 0x4c, 0x12, 0x91, 0xcf, 0x2d, 0x73,
0xca, 0x94, 0x76, 0x28, 0xab, 0xf5, 0x17, 0x49, 0x08, 0x56, 0xb4, 0xea, 0x69, 0x37, 0xd5, 0x8b,
0x57, 0x09, 0xeb, 0xb5, 0x36, 0x68, 0x8a, 0xd4, 0x95, 0xcb, 0x29, 0x77, 0xf4, 0xaa, 0x48, 0x16,
0xe9, 0xb7, 0x55, 0x0b, 0x88, 0xd6, 0x34, 0x6a, 0x2b, 0x75, 0x97, 0xc9, 0x4a, 0x14, 0xf6, 0xa8,
0x74, 0x2a, 0xc8, 0x96, 0x15, 0x4b, 0xa9, 0xf7, 0xb6, 0xe8, 0x0a, 0x54, 0xd7, 0x89, 0x6b, 0x35,
};
uint16_t CRC_INIT = 0xffff;
const uint16_t wCRC_Table[256] =
{
0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf,
0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7,
0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e,
0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876,
0x2102, 0x308b, 0x0210, 0x1399, 0x6726, 0x76af, 0x4434, 0x55bd,
0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5,
0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c,
0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, 0xc974,
0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb,
0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3,
0x5285, 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a,
0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72,
0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9,
0xef4e, 0xfec7, 0xcc5c, 0xddd5, 0xa96a, 0xb8e3, 0x8a78, 0x9bf1,
0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738,
0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70,
0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 0xf0b7,
0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff,
0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036,
0x18c1, 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e,
0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5,
0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd,
0xb58b, 0xa402, 0x9699, 0x8710, 0xf3af, 0xe226, 0xd0bd, 0xc134,
0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c,
0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3,
0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 0x3efb,
0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232,
0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a,
0xe70e, 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1,
0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9,
0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330,
0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78
};
// CRC8
void Append_CRC8_Check_Sum( uint8_t *pchMessage, uint16_t dwLength);
uint32_t Verify_CRC8_Check_Sum( uint8_t *pchMessage, uint16_t dwLength);
uint8_t Get_CRC8_Check_Sum( uint8_t *pchMessage, uint16_t dwLength, uint8_t ucCRC8 );
// CRC16
void Append_CRC16_Check_Sum(uint8_t * pchMessage,uint32_t dwLength);
uint32_t Verify_CRC16_Check_Sum(uint8_t *pchMessage, uint32_t dwLength);
uint16_t Get_CRC16_Check_Sum(uint8_t *pchMessage,uint32_t dwLength,uint16_t wCRC);
/* CRC校验 */
uint8_t Get_CRC8_Check_Sum( uint8_t *pchMessage, uint16_t dwLength, uint8_t ucCRC8 )
{
uint8_t ucIndex;
while (dwLength--)
{
ucIndex = ucCRC8^(*pchMessage++);
ucCRC8 = CRC8_TAB[ucIndex];
}
return ucCRC8;
}
/*
** Descriptions: CRC8 Verify function
** Input: Data to Verify,Stream length = Data + checksum
** Output: True or False (CRC Verify Result)
*/
uint32_t Verify_CRC8_Check_Sum( uint8_t *pchMessage, uint16_t dwLength)
{
uint8_t ucExpected = 0;
if (pchMessage == 0 || dwLength <= 2)
{
return 0;
}
ucExpected = Get_CRC8_Check_Sum( pchMessage, dwLength-1, CRC8_INIT);
return ( ucExpected == pchMessage[dwLength-1] );
}
/*
** Descriptions: append CRC8 to the end of data
** Input: Data to CRC and append,Stream length = Data + checksum
** Output: True or False (CRC Verify Result)
*/
void Append_CRC8_Check_Sum( uint8_t *pchMessage, uint16_t dwLength)
{
uint8_t ucCRC = 0;
if (pchMessage == 0 || dwLength <= 2)
{
return;
}
ucCRC = Get_CRC8_Check_Sum( (uint8_t *)pchMessage, dwLength-1, CRC8_INIT);
pchMessage[dwLength-1] = ucCRC;
}
/*
** Descriptions: CRC16 checksum function
** Input: Data to check,Stream length, initialized checksum
** Output: CRC checksum
*/
uint16_t Get_CRC16_Check_Sum(uint8_t *pchMessage,uint32_t dwLength,uint16_t wCRC)
{
uint8_t chData;
if (pchMessage == NULL)
{
return 0xFFFF;
}
while(dwLength--)
{
chData = *pchMessage++;
(wCRC) = ((uint16_t)(wCRC) >> 8) ^
wCRC_Table[((uint16_t)(wCRC) ^ (uint16_t)(chData)) & 0x00ff];
}
return wCRC;
}
/*
** Descriptions: CRC16 Verify function
** Input: Data to Verify,Stream length = Data + checksum
** Output: True or False (CRC Verify Result)
*/
uint32_t Verify_CRC16_Check_Sum(uint8_t *pchMessage, uint32_t dwLength)
{
uint16_t wExpected = 0;
if ((pchMessage == NULL) || (dwLength <= 2))
{
return FALSE;
}
wExpected = Get_CRC16_Check_Sum ( pchMessage, dwLength - 2, CRC_INIT);
return ( (wExpected & 0xff) == pchMessage[dwLength - 2]
&& ((wExpected >> 8) & 0xff) == pchMessage[dwLength - 1]);
}
/*
** Descriptions: append CRC16 to the end of data
** Input: Data to CRC and append,Stream length = Data + checksum
** Output: True or False (CRC Verify Result)
*/
void Append_CRC16_Check_Sum(uint8_t * pchMessage,uint32_t dwLength)
{
uint16_t wCRC = 0;
if ((pchMessage == NULL) || (dwLength <= 2))
{
return;
}
wCRC = Get_CRC16_Check_Sum ( (uint8_t *)pchMessage, dwLength-2, CRC_INIT );
pchMessage[dwLength-2] = (uint8_t)(wCRC & 0x00ff);
pchMessage[dwLength-1] = (uint8_t)((wCRC >> 8)& 0x00ff);
}
modules/referee/crc_ref.h
+20
View File
@@ -0,0 +1,20 @@
#ifndef __CRC_H_
#define __CRC_H_
// 裁判系统官方CRC校验,LUT和module/algorithms中的不同,后续需要统一实现crc,提供8/16/32的支持
#include <stdint.h>
#define TRUE 1
#define FALSE 0
// CRC8
void Append_CRC8_Check_Sum(uint8_t *pchMessage, uint16_t dwLength);
uint32_t Verify_CRC8_Check_Sum(uint8_t *pchMessage, uint16_t dwLength);
uint8_t Get_CRC8_Check_Sum(uint8_t *pchMessage, uint16_t dwLength, uint8_t ucCRC8);
// CRC16
void Append_CRC16_Check_Sum(uint8_t *pchMessage, uint32_t dwLength);
uint32_t Verify_CRC16_Check_Sum(uint8_t *pchMessage, uint32_t dwLength);
uint16_t Get_CRC16_Check_Sum(uint8_t *pchMessage, uint32_t dwLength, uint16_t wCRC);
#endif
modules/referee/referee.md
+196
View File
@@ -0,0 +1,196 @@
# referee
## referee运行流程
首先在chassis的初始化中调用裁判系统初始化函数，将要绘制的uidata的指针传递给接口，接口会返回裁判系统的反馈数据指针。然后，在refereeUItask里进行UI初始化，确定ui发送的目标并绘制初始化UI。完成后，uitask会以10hz的频率按顺序更新UI。
## 如何绘制你的自定义UI？以绘制超级电容能量条为例
UI的绘制包含初始化和TASK两个部分，初始化部分在`MyUIInit`函数中，TASK部分在`MyUIRefresh`函数中。
### 初始化部分
初始化部分的UI主要有两个目的：静态UI的绘制、为动态UI的绘制做准备。
分析超级电容能量条功能可知，此UI包含如下：
Power：xxx Power为静态不变的，冒号后的xxx为变化的量。
方框以及方框内的能量条：方框为静态不变的，能量条为变化的量。（参考游戏血条）
因而，静态UI的绘制包含如下：
绘制字符“Power:”、绘制矩形方框。
为动态UI的准备如下：
绘制矩形方框内的初始能量条、绘制Power的初始值。
### 绘制字符“Power:”
设置绘制用结构体，此处使用数组是因为需要绘制多个字符。本次绘制的字符为“Power:”，只是用到了第6个，即xxx[5]：
```c
static String_Data_t UI_State_sta[6]; // 静态
```
字符格式以及内容设置：
```c
UICharDraw(&UI_State_sta[5], "ss5", UI_Graph_ADD, 7, UI_Color_Green, 18, 2, 620, 230, "Power:");
//各参数意义如下，函数定义中有详细注释：
string String_Data类型变量指针，用于存放字符串数据
stringname[3] 字符串名称，用于标识更改
String_Operate 字符串操作，初始化时一般使用"UI_Graph_ADD"
String_Layer 图层0-9
String_Color 字符串颜色
String_Size 字号
String_Width 字符串线宽
Start_x、Start_y 开始坐标
*stringdata 字符串数据
//设置完毕后，使用“Char_ReFresh”发送即可：
UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[5]);
```
### 绘制能量框
定义一个图形类结构体，用于绘制能量框：
```c
static Graph_Data_t UI_energy_line[3]; // 电容能量条
```
能量框参数设置以及发送函数：
```c
UIRectangleDraw(&UI_energy_line[0],"ss6", UI_Graph_ADD, 7, UI_Color_Green,20, 720, 220, 820, 240)
UIRefresh(&referee_recv_info->referee_id, 1,UI_energy_line[0]);
```
### 绘制power的初始值
```c
UIFloatDraw(&UI_Energy[1], "sd5", UI_Graph_ADD, 8, UI_Color_Green, 18, 2, 2, 750, 230, 24000);
```
### 绘制能量条的初始值
```c
UILineDraw(&UI_Energy[2], "sd6", UI_Graph_ADD, 8, UI_Color_Pink, 30, 720, 160, 1020, 160);
```
将两个图形打包发送
```
UIRefresh(&referee_recv_info->referee_id, 2, UI_Energy[1], UI_Energy[2]);
```
## TASK部分
task中UI处于动态变化，此时需要检测所画的UI是否发生变化，若发生变化，则刷新对应UI。
### 添加变化检测
绘制功率部分UI，我们需要的是`Chassis_Power_Data_s`中的数据，我们定义`Chassis_Power_Data_s Chassis_Power_Data;`和`Chassis_Power_Data_s Chassis_last_Power_Data;`分别存储此次和上次的对应数据，本次和上次对应检测变化的需求。
```c
typedef struct
{
Referee_Interactive_Flag_t Referee_Interactive_Flag;
// 为UI绘制以及交互数据所用
chassis_mode_e chassis_mode; // 底盘模式
gimbal_mode_e gimbal_mode; // 云台模式
shoot_mode_e shoot_mode; // 发射模式设置
friction_mode_e friction_mode; // 摩擦轮关闭
lid_mode_e lid_mode; // 弹舱盖打开
Chassis_Power_Data_s Chassis_Power_Data; // 功率控制
// 上一次的模式，用于flag判断
chassis_mode_e chassis_last_mode;
gimbal_mode_e gimbal_last_mode;
shoot_mode_e shoot_last_mode;
friction_mode_e friction_last_mode;
lid_mode_e lid_last_mode;
Chassis_Power_Data_s Chassis_last_Power_Data;
} Referee_Interactive_info_t;
```
添加功率变化标志位，`uint32_t Power_flag : 1;`，1为检测到变化，0为未检测到变换
```
typedef struct
{
uint32_t chassis_flag : 1;
uint32_t gimbal_flag : 1;
uint32_t shoot_flag : 1;
uint32_t lid_flag : 1;
uint32_t friction_flag : 1;
uint32_t Power_flag : 1;
} Referee_Interactive_Flag_t;
```
在变化检测函数中增加对应判断,由于voltage和能量条的变化对应同一个参数`Chassis_last_Power_Data.chassis_power_mx`的变化，所以只需要一个参数即可：
```
static void UIChangeCheck(Referee_Interactive_info_t *_Interactive_data)
{
if (_Interactive_data->chassis_mode != _Interactive_data->chassis_last_mode)
......
......
if (_Interactive_data->lid_mode != _Interactive_data->lid_last_mode)
{
_Interactive_data->Referee_Interactive_Flag.lid_flag = 1;
_Interactive_data->lid_last_mode = _Interactive_data->lid_mode;
}
if (_Interactive_data->Chassis_Power_Data.chassis_power_mx != _Interactive_data->Chassis_last_Power_Data.chassis_power_mx)
{
_Interactive_data->Referee_Interactive_Flag.Power_flag = 1;
_Interactive_data->Chassis_last_Power_Data.chassis_power_mx = _Interactive_data->Chassis_Power_Data.chassis_power_mx;
}
}
```
### 根据功率的变化绘制UI
在绘制变化的UI时，由于初始化时已经使用`UI_Graph_ADD`操作添加了UI，所以在绘制时，需要使用`UI_Graph_Change`操作，以便于刷新UI。
同时，完成UI刷新后，需要将对应的flag置0，以便于下次检测变化
```
if (_Interactive_data->Referee_Interactive_Flag.Power_flag == 1)
{
UIFloatDraw(&UI_Energy[1], "sd5", UI_Graph_Change, 8, UI_Color_Green, 18, 2, 2, 750, 230, _Interactive_data->Chassis_Power_Data.chassis_power_mx * 1000);
UILineDraw(&UI_Energy[2], "sd6", UI_Graph_Change, 8, UI_Color_Pink, 30, 720, 160, (uint32_t)750 + _Interactive_data->Chassis_Power_Data.chassis_power_mx * 30, 160);
UIRefresh(&referee_recv_info->referee_id, 2, UI_Energy[1], UI_Energy[2]);
_Interactive_data->Referee_Interactive_Flag.Power_flag = 0;
}
```
---
若需要进行多机交互，可增加此函数：
```c
void CommBetweenRobotSend(referee_id_t *_id, robot_interactive_data_t *_data)
{
Communicate_SendData_t SendData;
uint8_t temp_datalength = Interactive_Data_LEN_Head + Communicate_Data_LEN; // 计算交互数据长度 6+n,n为交互数据长度
SendData.FrameHeader.SOF = REFEREE_SOF;
SendData.FrameHeader.DataLength = temp_datalength;
SendData.FrameHeader.Seq = UI_Seq;
SendData.FrameHeader.CRC8 = Get_CRC8_Check_Sum((uint8_t *)&SendData, LEN_CRC8, 0xFF);
SendData.CmdID = ID_student_interactive;
SendData.datahead.data_cmd_id = Communicate_Data_ID;
SendData.datahead.sender_ID = _id->Robot_ID;
SendData.datahead.receiver_ID = _id->Receiver_Robot_ID;
SendData.Data = *_data;
SendData.frametail = Get_CRC16_Check_Sum((uint8_t *)&SendData, LEN_HEADER + LEN_CMDID + temp_datalength, 0xFFFF);
RefereeSend((uint8_t *)&SendData, LEN_HEADER + LEN_CMDID + temp_datalength + LEN_TAIL); // 发送
UI_Seq++; // 包序号+1
}
```
modules/referee/referee_UI.c
+424
View File
@@ -0,0 +1,424 @@
/**
* @file referee_UI.C
* @author kidneygood (you@domain.com)
* @brief
* @version 0.1
* @date 2023-1-18
*
* @copyright Copyright (c) 2022
*
*/
#include "referee_UI.h"
#include "string.h"
#include "crc_ref.h"
#include "stdio.h"
#include "rm_referee.h"
// 包序号
/********************************************删除操作*************************************
**参数：_id 对应的id结构体
Del_Operate 对应头文件删除操作
Del_Layer 要删除的层 取值0-9
*****************************************************************************************/
void UIDelete(referee_id_t *_id, uint8_t Del_Operate, uint8_t Del_Layer)
{
static UI_delete_t UI_delete_data;
uint8_t temp_datalength = Interactive_Data_LEN_Head + UI_Operate_LEN_Del; // 计算交互数据长度
UI_delete_data.FrameHeader.SOF = REFEREE_SOF;
UI_delete_data.FrameHeader.DataLength = temp_datalength;
UI_delete_data.FrameHeader.Seq = UI_Seq;
UI_delete_data.FrameHeader.CRC8 = Get_CRC8_Check_Sum((uint8_t *)&UI_delete_data, LEN_CRC8, 0xFF);
UI_delete_data.CmdID = ID_student_interactive;
UI_delete_data.datahead.data_cmd_id = UI_Data_ID_Del;
UI_delete_data.datahead.receiver_ID = _id->Cilent_ID;
UI_delete_data.datahead.sender_ID = _id->Robot_ID;
UI_delete_data.Delete_Operate = Del_Operate; // 删除操作
UI_delete_data.Layer = Del_Layer;
UI_delete_data.frametail = Get_CRC16_Check_Sum((uint8_t *)&UI_delete_data, LEN_HEADER + LEN_CMDID + temp_datalength, 0xFFFF);
/* 填入0xFFFF,关于crc校验 */
RefereeSend((uint8_t *)&UI_delete_data, LEN_HEADER + LEN_CMDID + temp_datalength + LEN_TAIL); // 发送
UI_Seq++; // 包序号+1
}
/************************************************绘制直线*************************************************
**参数：*graph Graph_Data类型变量指针，用于存放图形数据
graphname[3] 图片名称，用于标识更改
Graph_Operate 图片操作，见头文件
Graph_Layer 图层0-9
Graph_Color 图形颜色
Graph_Width 图形线宽
Start_x、Start_y 起点xy坐标
End_x、End_y 终点xy坐标
**********************************************************************************************************/
void UILineDraw(Graph_Data_t *graph, char graphname[3], uint32_t Graph_Operate, uint32_t Graph_Layer, uint32_t Graph_Color,
uint32_t Graph_Width, uint32_t Start_x, uint32_t Start_y, uint32_t End_x, uint32_t End_y)
{
int i;
for (i = 0; i < 3 && graphname[i] != '\0'; i++) // 填充至‘0’为止
{
graph->graphic_name[2 - i] = graphname[i]; // 按内存地址增大方向填充，所以会有i与2-i
}
graph->operate_tpye = Graph_Operate;
graph->graphic_tpye = UI_Graph_Line;
graph->layer = Graph_Layer;
graph->color = Graph_Color;
graph->start_angle = 0;
graph->end_angle = 0;
graph->width = Graph_Width;
graph->start_x = Start_x;
graph->start_y = Start_y;
graph->radius = 0;
graph->end_x = End_x;
graph->end_y = End_y;
}
/************************************************绘制矩形*************************************************
**参数：*graph Graph_Data类型变量指针，用于存放图形数据
graphname[3] 图片名称，用于标识更改
Graph_Operate 图片操作，见头文件
Graph_Layer 图层0-9
Graph_Color 图形颜色
Graph_Width 图形线宽
Start_x、Start_y 起点xy坐标
End_x、End_y 对角顶点xy坐标
**********************************************************************************************************/
void UIRectangleDraw(Graph_Data_t *graph, char graphname[3], uint32_t Graph_Operate, uint32_t Graph_Layer, uint32_t Graph_Color,
uint32_t Graph_Width, uint32_t Start_x, uint32_t Start_y, uint32_t End_x, uint32_t End_y)
{
int i;
for (i = 0; i < 3 && graphname[i] != '\0'; i++)
{
graph->graphic_name[2 - i] = graphname[i];
}
graph->graphic_tpye = UI_Graph_Rectangle;
graph->operate_tpye = Graph_Operate;
graph->layer = Graph_Layer;
graph->color = Graph_Color;
graph->start_angle = 0;
graph->end_angle = 0;
graph->width = Graph_Width;
graph->start_x = Start_x;
graph->start_y = Start_y;
graph->radius = 0;
graph->end_x = End_x;
graph->end_y = End_y;
}
/************************************************绘制整圆*************************************************
**参数：*graph Graph_Data类型变量指针，用于存放图形数据
graphname[3] 图片名称，用于标识更改
Graph_Operate 图片操作，见头文件
Graph_Layer 图层0-9
Graph_Color 图形颜色
Graph_Width 图形线宽
Start_x、Start_y 圆心xy坐标
Graph_Radius 圆形半径
**********************************************************************************************************/
void UICircleDraw(Graph_Data_t *graph, char graphname[3], uint32_t Graph_Operate, uint32_t Graph_Layer, uint32_t Graph_Color,
uint32_t Graph_Width, uint32_t Start_x, uint32_t Start_y, uint32_t Graph_Radius)
{
int i;
for (i = 0; i < 3 && graphname[i] != '\0'; i++)
{
graph->graphic_name[2 - i] = graphname[i];
}
graph->graphic_tpye = UI_Graph_Circle;
graph->operate_tpye = Graph_Operate;
graph->layer = Graph_Layer;
graph->color = Graph_Color;
graph->start_angle = 0;
graph->end_angle = 0;
graph->width = Graph_Width;
graph->start_x = Start_x;
graph->start_y = Start_y;
graph->radius = Graph_Radius;
graph->end_x = 0;
graph->end_y = 0;
}
/************************************************绘制椭圆*************************************************
**参数：*graph Graph_Data类型变量指针，用于存放图形数据
graphname[3] 图片名称，用于标识更改
Graph_Operate 图片操作，见头文件
Graph_Layer 图层0-9
Graph_Color 图形颜色
Graph_Width 图形线宽
Start_x、Start_y 圆心xy坐标
End_x、End_y xy半轴长度
**********************************************************************************************************/
void UIOvalDraw(Graph_Data_t *graph, char graphname[3], uint32_t Graph_Operate, uint32_t Graph_Layer, uint32_t Graph_Color,
uint32_t Graph_Width, uint32_t Start_x, uint32_t Start_y, uint32_t end_x, uint32_t end_y)
{
int i;
for (i = 0; i < 3 && graphname[i] != '\0'; i++)
{
graph->graphic_name[2 - i] = graphname[i];
}
graph->graphic_tpye = UI_Graph_Ellipse;
graph->operate_tpye = Graph_Operate;
graph->layer = Graph_Layer;
graph->color = Graph_Color;
graph->width = Graph_Width;
graph->start_angle = 0;
graph->end_angle = 0;
graph->width = Graph_Width;
graph->start_x = Start_x;
graph->start_y = Start_y;
graph->radius = 0;
graph->end_x = end_x;
graph->end_y = end_y;
}
/************************************************绘制圆弧*************************************************
**参数：*graph Graph_Data类型变量指针，用于存放图形数据
graphname[3] 图片名称，用于标识更改
Graph_Operate 图片操作，见头文件
Graph_Layer 图层0-9
Graph_Color 图形颜色
Graph_StartAngle,Graph_EndAngle 起始终止角度
Graph_Width 图形线宽
Start_y,Start_y 圆心xy坐标
x_Length,y_Length xy半轴长度
**********************************************************************************************************/
void UIArcDraw(Graph_Data_t *graph, char graphname[3], uint32_t Graph_Operate, uint32_t Graph_Layer, uint32_t Graph_Color,
uint32_t Graph_StartAngle, uint32_t Graph_EndAngle, uint32_t Graph_Width, uint32_t Start_x, uint32_t Start_y,
uint32_t end_x, uint32_t end_y)
{
int i;
for (i = 0; i < 3 && graphname[i] != '\0'; i++)
{
graph->graphic_name[2 - i] = graphname[i];
}
graph->graphic_tpye = UI_Graph_Arc;
graph->operate_tpye = Graph_Operate;
graph->layer = Graph_Layer;
graph->color = Graph_Color;
graph->start_angle = Graph_StartAngle;
graph->end_angle = Graph_EndAngle;
graph->width = Graph_Width;
graph->start_x = Start_x;
graph->start_y = Start_y;
graph->radius = 0;
graph->end_x = end_x;
graph->end_y = end_y;
}
/************************************************绘制浮点型数据*************************************************
**参数：*graph Graph_Data类型变量指针，用于存放图形数据
graphname[3] 图片名称，用于标识更改
Graph_Operate 图片操作，见头文件
Graph_Layer 图层0-9
Graph_Color 图形颜色
Graph_Size 字号
Graph_Digit 小数位数
Graph_Width 图形线宽
Start_x、Start_y 开始坐标
radius=a&0x3FF; a为浮点数乘以1000后的32位整型数
end_x=(a>>10)&0x7FF;
end_y=(a>>21)&0x7FF;
**********************************************************************************************************/
void UIFloatDraw(Graph_Data_t *graph, char graphname[3], uint32_t Graph_Operate, uint32_t Graph_Layer, uint32_t Graph_Color,
uint32_t Graph_Size, uint32_t Graph_Digit, uint32_t Graph_Width, uint32_t Start_x, uint32_t Start_y, int32_t Graph_Float)
{
int i;
for (i = 0; i < 3 && graphname[i] != '\0'; i++)
{
graph->graphic_name[2 - i] = graphname[i];
}
graph->graphic_tpye = UI_Graph_Float;
graph->operate_tpye = Graph_Operate;
graph->layer = Graph_Layer;
graph->color = Graph_Color;
graph->width = Graph_Width;
graph->start_x = Start_x;
graph->start_y = Start_y;
graph->start_angle = Graph_Size;
graph->end_angle = Graph_Digit;
graph->radius = Graph_Float & 0x3FF;
graph->end_x = (Graph_Float >> 10) & 0x7FF;
graph->end_y = (Graph_Float >> 21) & 0x7FF;
}
/************************************************绘制整型数据*************************************************
**参数：*graph Graph_Data类型变量指针，用于存放图形数据
graphname[3] 图片名称，用于标识更改
Graph_Operate 图片操作，见头文件
Graph_Layer 图层0-9
Graph_Color 图形颜色
Graph_Size 字号
Graph_Width 图形线宽
Start_x、Start_y 开始坐标
radius=a&0x3FF; a为32位整型数
end_x=(a>>10)&0x7FF;
end_y=(a>>21)&0x7FF;
**********************************************************************************************************/
void UIIntDraw(Graph_Data_t *graph, char graphname[3], uint32_t Graph_Operate, uint32_t Graph_Layer, uint32_t Graph_Color,
uint32_t Graph_Size, uint32_t Graph_Width, uint32_t Start_x, uint32_t Start_y, int32_t Graph_Integer)
{
int i;
for (i = 0; i < 3 && graphname[i] != '\0'; i++)
{
graph->graphic_name[2 - i] = graphname[i];
}
graph->graphic_tpye = UI_Graph_Int;
graph->operate_tpye = Graph_Operate;
graph->layer = Graph_Layer;
graph->color = Graph_Color;
graph->start_angle = Graph_Size;
graph->end_angle = 0;
graph->width = Graph_Width;
graph->start_x = Start_x;
graph->start_y = Start_y;
graph->radius = Graph_Integer & 0x3FF;
graph->end_x = (Graph_Integer >> 10) & 0x7FF;
graph->end_y = (Graph_Integer >> 21) & 0x7FF;
}
/************************************************绘制字符型数据*************************************************
**参数：*graph Graph_Data类型变量指针，用于存放图形数据
graphname[3] 图片名称，用于标识更改
Graph_Operate 图片操作，见头文件
Graph_Layer 图层0-9
Graph_Color 图形颜色
Graph_Size 字号
Graph_Width 图形线宽
Start_x、Start_y 开始坐标
**参数：*graph Graph_Data类型变量指针，用于存放图形数据
fmt需要显示的字符串
此函数的实现和具体使用类似于printf函数
**********************************************************************************************************/
void UICharDraw(String_Data_t *graph, char graphname[3], uint32_t Graph_Operate, uint32_t Graph_Layer, uint32_t Graph_Color,
uint32_t Graph_Size, uint32_t Graph_Width, uint32_t Start_x, uint32_t Start_y, char *fmt, ...)
{
int i;
for (i = 0; i < 3 && graphname[i] != '\0'; i++)
{
graph->Graph_Control.graphic_name[2 - i] = graphname[i];
}
graph->Graph_Control.graphic_tpye = UI_Graph_Char;
graph->Graph_Control.operate_tpye = Graph_Operate;
graph->Graph_Control.layer = Graph_Layer;
graph->Graph_Control.color = Graph_Color;
graph->Graph_Control.width = Graph_Width;
graph->Graph_Control.start_x = Start_x;
graph->Graph_Control.start_y = Start_y;
graph->Graph_Control.start_angle = Graph_Size;
graph->Graph_Control.radius = 0;
graph->Graph_Control.end_x = 0;
graph->Graph_Control.end_y = 0;
va_list ap;
va_start(ap, fmt);
vsprintf((char *)graph->show_Data, fmt, ap); // 使用参数列表进行格式化并输出到字符串
va_end(ap);
graph->Graph_Control.end_angle = strlen((const char *)graph->show_Data);
}
/* UI推送函数（使更改生效）
参数： cnt 图形个数
... 图形变量参数
Tips：：该函数只能推送1，2，5，7个图形，其他数目协议未涉及
*/
void UIGraphRefresh(referee_id_t *_id, int cnt, ...)
{
UI_GraphReFresh_t UI_GraphReFresh_data;
Graph_Data_t graphData;
uint8_t temp_datalength = LEN_HEADER + LEN_CMDID + Interactive_Data_LEN_Head + UI_Operate_LEN_PerDraw * cnt + LEN_TAIL; // 计算交互数据长度
static uint8_t buffer[512]; // 交互数据缓存
va_list ap; // 创建一个 va_list 类型变量
va_start(ap, cnt); // 初始化 va_list 变量为一个参数列表
UI_GraphReFresh_data.FrameHeader.SOF = REFEREE_SOF;
UI_GraphReFresh_data.FrameHeader.DataLength = Interactive_Data_LEN_Head + cnt * UI_Operate_LEN_PerDraw;
UI_GraphReFresh_data.FrameHeader.Seq = UI_Seq;
UI_GraphReFresh_data.FrameHeader.CRC8 = Get_CRC8_Check_Sum((uint8_t *)&UI_GraphReFresh_data, LEN_CRC8, 0xFF);
UI_GraphReFresh_data.CmdID = ID_student_interactive;
switch (cnt)
{
case 1:
UI_GraphReFresh_data.datahead.data_cmd_id = UI_Data_ID_Draw1;
break;
case 2:
UI_GraphReFresh_data.datahead.data_cmd_id = UI_Data_ID_Draw2;
break;
case 5:
UI_GraphReFresh_data.datahead.data_cmd_id = UI_Data_ID_Draw5;
break;
case 7:
UI_GraphReFresh_data.datahead.data_cmd_id = UI_Data_ID_Draw7;
break;
}
UI_GraphReFresh_data.datahead.receiver_ID = _id->Cilent_ID;
UI_GraphReFresh_data.datahead.sender_ID = _id->Robot_ID;
memcpy(buffer, (uint8_t *)&UI_GraphReFresh_data, LEN_HEADER + LEN_CMDID + Interactive_Data_LEN_Head); // 将帧头、命令码、交互数据帧头三部分复制到缓存中
for (uint8_t i = 0; i < cnt; i++) // 发送交互数据的数据帧，并计算CRC16校验值
{
graphData = va_arg(ap, Graph_Data_t); // 访问参数列表中的每个项,第二个参数是你要返回的参数的类型,在取值时需要将其强制转化为指定类型的变量
memcpy(buffer + (LEN_HEADER + LEN_CMDID + Interactive_Data_LEN_Head + UI_Operate_LEN_PerDraw * i), (uint8_t *)&graphData, UI_Operate_LEN_PerDraw);
}
Append_CRC16_Check_Sum(buffer, temp_datalength);
RefereeSend(buffer, temp_datalength);
va_end(ap); // 结束可变参数的获取
}
/************************************************UI推送字符（使更改生效）*********************************/
void UICharRefresh(referee_id_t *_id, String_Data_t string_Data)
{
static UI_CharReFresh_t UI_CharReFresh_data;
uint8_t temp_datalength = Interactive_Data_LEN_Head + UI_Operate_LEN_DrawChar; // 计算交互数据长度
UI_CharReFresh_data.FrameHeader.SOF = REFEREE_SOF;
UI_CharReFresh_data.FrameHeader.DataLength = temp_datalength;
UI_CharReFresh_data.FrameHeader.Seq = UI_Seq;
UI_CharReFresh_data.FrameHeader.CRC8 = Get_CRC8_Check_Sum((uint8_t *)&UI_CharReFresh_data, LEN_CRC8, 0xFF);
UI_CharReFresh_data.CmdID = ID_student_interactive;
UI_CharReFresh_data.datahead.data_cmd_id = UI_Data_ID_DrawChar;
UI_CharReFresh_data.datahead.receiver_ID = _id->Cilent_ID;
UI_CharReFresh_data.datahead.sender_ID = _id->Robot_ID;
UI_CharReFresh_data.String_Data = string_Data;
UI_CharReFresh_data.frametail = Get_CRC16_Check_Sum((uint8_t *)&UI_CharReFresh_data, LEN_HEADER + LEN_CMDID + temp_datalength, 0xFFFF);
RefereeSend((uint8_t *)&UI_CharReFresh_data, LEN_HEADER + LEN_CMDID + temp_datalength + LEN_TAIL); // 发送
UI_Seq++; // 包序号+1
}
modules/referee/referee_UI.h
+72
View File
@@ -0,0 +1,72 @@
#ifndef REFEREE_UI_H
#define REFEREE_UI_H
#include "stdarg.h"
#include "stdint.h"
#include "referee_protocol.h"
#include "rm_referee.h"
#pragma pack(1) // 按1字节对齐
/* 此处的定义只与UI绘制有关 */
typedef struct
{
xFrameHeader FrameHeader;
uint16_t CmdID;
ext_student_interactive_header_data_t datahead;
uint8_t Delete_Operate; // 删除操作
uint8_t Layer;
uint16_t frametail;
} UI_delete_t;
typedef struct
{
xFrameHeader FrameHeader;
uint16_t CmdID;
ext_student_interactive_header_data_t datahead;
uint16_t frametail;
} UI_GraphReFresh_t;
typedef struct
{
xFrameHeader FrameHeader;
uint16_t CmdID;
ext_student_interactive_header_data_t datahead;
String_Data_t String_Data;
uint16_t frametail;
} UI_CharReFresh_t; // 打印字符串数据
#pragma pack()
void UIDelete(referee_id_t *_id, uint8_t Del_Operate, uint8_t Del_Layer);
void UILineDraw(Graph_Data_t *graph, char graphname[3], uint32_t Graph_Operate, uint32_t Graph_Layer, uint32_t Graph_Color,
uint32_t Graph_Width, uint32_t Start_x, uint32_t Start_y, uint32_t End_x, uint32_t End_y);
void UIRectangleDraw(Graph_Data_t *graph, char graphname[3], uint32_t Graph_Operate, uint32_t Graph_Layer, uint32_t Graph_Color,
uint32_t Graph_Width, uint32_t Start_x, uint32_t Start_y, uint32_t End_x, uint32_t End_y);
void UICircleDraw(Graph_Data_t *graph, char graphname[3], uint32_t Graph_Operate, uint32_t Graph_Layer, uint32_t Graph_Color,
uint32_t Graph_Width, uint32_t Start_x, uint32_t Start_y, uint32_t Graph_Radius);
void UIOvalDraw(Graph_Data_t *graph, char graphname[3], uint32_t Graph_Operate, uint32_t Graph_Layer, uint32_t Graph_Color,
uint32_t Graph_Width, uint32_t Start_x, uint32_t Start_y, uint32_t end_x, uint32_t end_y);
void UIArcDraw(Graph_Data_t *graph, char graphname[3], uint32_t Graph_Operate, uint32_t Graph_Layer, uint32_t Graph_Color,
uint32_t Graph_StartAngle, uint32_t Graph_EndAngle, uint32_t Graph_Width, uint32_t Start_x, uint32_t Start_y,
uint32_t end_x, uint32_t end_y);
void UIFloatDraw(Graph_Data_t *graph, char graphname[3], uint32_t Graph_Operate, uint32_t Graph_Layer, uint32_t Graph_Color,
uint32_t Graph_Size, uint32_t Graph_Digit, uint32_t Graph_Width, uint32_t Start_x, uint32_t Start_y, int32_t Graph_Float);
void UIIntDraw(Graph_Data_t *graph, char graphname[3], uint32_t Graph_Operate, uint32_t Graph_Layer, uint32_t Graph_Color,
uint32_t Graph_Size, uint32_t Graph_Width, uint32_t Start_x, uint32_t Start_y, int32_t Graph_Integer);
void UICharDraw(String_Data_t *graph, char graphname[3], uint32_t Graph_Operate, uint32_t Graph_Layer, uint32_t Graph_Color,
uint32_t Graph_Size, uint32_t Graph_Width, uint32_t Start_x, uint32_t Start_y, char *fmt, ...);
void UIGraphRefresh(referee_id_t *_id, int cnt, ...);
void UICharRefresh(referee_id_t *_id, String_Data_t string_Data);
#endif
modules/referee/referee_protocol.h
+392
View File
@@ -0,0 +1,392 @@
/**
* @file referee_protocol.h
* @author kidneygood (you@domain.com)
* @version 0.1
* @date 2022-12-02
*
* @copyright Copyright (c) HNU YueLu EC 2022 all rights reserved
*
*/
#ifndef referee_protocol_H
#define referee_protocol_H
#include "stdint.h"
/****************************宏定义部分****************************/
#define REFEREE_SOF 0xA5 // 起始字节,协议固定为0xA5
#define Robot_Red 0
#define Robot_Blue 1
#define Communicate_Data_LEN 5 // 自定义交互数据长度，该长度决定了我方发送和他方接收，自定义交互数据协议更改时只需要更改此宏定义即可
#pragma pack(1)
/****************************通信协议格式****************************/
/* 通信协议格式偏移，枚举类型,代替#define声明 */
typedef enum
{
FRAME_HEADER_Offset = 0,
CMD_ID_Offset = 5,
DATA_Offset = 7,
} JudgeFrameOffset_e;
/* 通信协议长度 */
typedef enum
{
LEN_HEADER = 5, // 帧头长
LEN_CMDID = 2, // 命令码长度
LEN_TAIL = 2, // 帧尾CRC16
LEN_CRC8 = 4, // 帧头CRC8校验长度=帧头+数据长+包序号
} JudgeFrameLength_e;
/****************************帧头****************************/
/****************************帧头****************************/
/* 帧头偏移 */
typedef enum
{
SOF = 0, // 起始位
DATA_LENGTH = 1, // 帧内数据长度,根据这个来获取数据长度
SEQ = 3, // 包序号
CRC8 = 4 // CRC8
} FrameHeaderOffset_e;
/* 帧头定义 */
typedef struct
{
uint8_t SOF;
uint16_t DataLength;
uint8_t Seq;
uint8_t CRC8;
} xFrameHeader;
/****************************cmd_id命令码说明****************************/
/****************************cmd_id命令码说明****************************/
/* 命令码ID,用来判断接收的是什么数据 */
typedef enum
{
ID_game_state = 0x0001, // 比赛状态数据
ID_game_result = 0x0002, // 比赛结果数据
ID_game_robot_survivors = 0x0003, // 比赛机器人血量数据
ID_event_data = 0x0101, // 场地事件数据
ID_supply_projectile_action = 0x0102, // 场地补给站动作标识数据
ID_supply_projectile_booking = 0x0103, // 场地补给站预约子弹数据
ID_game_robot_state = 0x0201, // 机器人状态数据
ID_power_heat_data = 0x0202, // 实时功率热量数据
ID_game_robot_pos = 0x0203, // 机器人位置数据
ID_buff_musk = 0x0204, // 机器人增益数据
ID_aerial_robot_energy = 0x0205, // 空中机器人能量状态数据
ID_robot_hurt = 0x0206, // 伤害状态数据
ID_shoot_data = 0x0207, // 实时射击数据
ID_student_interactive = 0x0301, // 机器人间交互数据
} CmdID_e;
/* 命令码数据段长,根据官方协议来定义长度，还有自定义数据长度 */
typedef enum
{
LEN_game_state = 3, // 0x0001
LEN_game_result = 1, // 0x0002
LEN_game_robot_HP = 2, // 0x0003
LEN_event_data = 4, // 0x0101
LEN_supply_projectile_action = 4, // 0x0102
LEN_game_robot_state = 27, // 0x0201
LEN_power_heat_data = 14, // 0x0202
LEN_game_robot_pos = 16, // 0x0203
LEN_buff_musk = 1, // 0x0204
LEN_aerial_robot_energy = 1, // 0x0205
LEN_robot_hurt = 1, // 0x0206
LEN_shoot_data = 7, // 0x0207
LEN_receive_data = 6 + Communicate_Data_LEN, // 0x0301
} JudgeDataLength_e;
/****************************接收数据的详细说明****************************/
/****************************接收数据的详细说明****************************/
/* ID: 0x0001 Byte: 3 比赛状态数据 */
typedef struct
{
uint8_t game_type : 4;
uint8_t game_progress : 4;
uint16_t stage_remain_time;
} ext_game_state_t;
/* ID: 0x0002 Byte: 1 比赛结果数据 */
typedef struct
{
uint8_t winner;
} ext_game_result_t;
/* ID: 0x0003 Byte: 32 比赛机器人血量数据 */
typedef struct
{
uint16_t red_1_robot_HP;
uint16_t red_2_robot_HP;
uint16_t red_3_robot_HP;
uint16_t red_4_robot_HP;
uint16_t red_5_robot_HP;
uint16_t red_7_robot_HP;
uint16_t red_outpost_HP;
uint16_t red_base_HP;
uint16_t blue_1_robot_HP;
uint16_t blue_2_robot_HP;
uint16_t blue_3_robot_HP;
uint16_t blue_4_robot_HP;
uint16_t blue_5_robot_HP;
uint16_t blue_7_robot_HP;
uint16_t blue_outpost_HP;
uint16_t blue_base_HP;
} ext_game_robot_HP_t;
/* ID: 0x0101 Byte: 4 场地事件数据 */
typedef struct
{
uint32_t event_type;
} ext_event_data_t;
/* ID: 0x0102 Byte: 3 场地补给站动作标识数据 */
typedef struct
{
uint8_t supply_projectile_id;
uint8_t supply_robot_id;
uint8_t supply_projectile_step;
uint8_t supply_projectile_num;
} ext_supply_projectile_action_t;
/* ID: 0X0201 Byte: 27 机器人状态数据 */
typedef struct
{
uint8_t robot_id;
uint8_t robot_level;
uint16_t remain_HP;
uint16_t max_HP;
uint16_t shooter_id1_17mm_cooling_rate;
uint16_t shooter_id1_17mm_cooling_limit;
uint16_t shooter_id1_17mm_speed_limit;
uint16_t shooter_id2_17mm_cooling_rate;
uint16_t shooter_id2_17mm_cooling_limit;
uint16_t shooter_id2_17mm_speed_limit;
uint16_t shooter_id1_42mm_cooling_rate;
uint16_t shooter_id1_42mm_cooling_limit;
uint16_t shooter_id1_42mm_speed_limit;
uint16_t chassis_power_limit;
uint8_t mains_power_gimbal_output : 1;
uint8_t mains_power_chassis_output : 1;
uint8_t mains_power_shooter_output : 1;
} ext_game_robot_state_t;
/* ID: 0X0202 Byte: 14 实时功率热量数据 */
typedef struct
{
uint16_t chassis_volt;
uint16_t chassis_current;
float chassis_power; // 瞬时功率
uint16_t chassis_power_buffer; // 60焦耳缓冲能量
uint16_t shooter_heat0; // 17mm
uint16_t shooter_heat1;
} ext_power_heat_data_t;
/* ID: 0x0203 Byte: 16 机器人位置数据 */
typedef struct
{
float x;
float y;
float z;
float yaw;
} ext_game_robot_pos_t;
/* ID: 0x0204 Byte: 1 机器人增益数据 */
typedef struct
{
uint8_t power_rune_buff;
} ext_buff_musk_t;
/* ID: 0x0205 Byte: 1 空中机器人能量状态数据 */
typedef struct
{
uint8_t attack_time;
} aerial_robot_energy_t;
/* ID: 0x0206 Byte: 1 伤害状态数据 */
typedef struct
{
uint8_t armor_id : 4;
uint8_t hurt_type : 4;
} ext_robot_hurt_t;
/* ID: 0x0207 Byte: 7 实时射击数据 */
typedef struct
{
uint8_t bullet_type;
uint8_t shooter_id;
uint8_t bullet_freq;
float bullet_speed;
} ext_shoot_data_t;
/****************************机器人交互数据****************************/
/****************************机器人交互数据****************************/
/* 发送的内容数据段最大为 113 检测是否超出大小限制?实际上图形段不会超，数据段最多30个，也不会超*/
/* 交互数据头结构 */
typedef struct
{
uint16_t data_cmd_id; // 由于存在多个内容 ID，但整个cmd_id 上行频率最大为 10Hz，请合理安排带宽。注意交互部分的上行频率
uint16_t sender_ID;
uint16_t receiver_ID;
} ext_student_interactive_header_data_t;
/* 机器人id */
typedef enum
{
// 红方机器人ID
RobotID_RHero = 1,
RobotID_REngineer = 2,
RobotID_RStandard1 = 3,
RobotID_RStandard2 = 4,
RobotID_RStandard3 = 5,
RobotID_RAerial = 6,
RobotID_RSentry = 7,
RobotID_RRadar = 9,
// 蓝方机器人ID
RobotID_BHero = 101,
RobotID_BEngineer = 102,
RobotID_BStandard1 = 103,
RobotID_BStandard2 = 104,
RobotID_BStandard3 = 105,
RobotID_BAerial = 106,
RobotID_BSentry = 107,
RobotID_BRadar = 109,
} Robot_ID_e;
/* 交互数据ID */
typedef enum
{
UI_Data_ID_Del = 0x100,
UI_Data_ID_Draw1 = 0x101,
UI_Data_ID_Draw2 = 0x102,
UI_Data_ID_Draw5 = 0x103,
UI_Data_ID_Draw7 = 0x104,
UI_Data_ID_DrawChar = 0x110,
/* 自定义交互数据部分 */
Communicate_Data_ID = 0x0200,
} Interactive_Data_ID_e;
/* 交互数据长度 */
typedef enum
{
Interactive_Data_LEN_Head = 6,
UI_Operate_LEN_Del = 2,
UI_Operate_LEN_PerDraw = 15,
UI_Operate_LEN_DrawChar = 15 + 30,
/* 自定义交互数据部分 */
// Communicate_Data_LEN = 5,
} Interactive_Data_Length_e;
/****************************自定义交互数据****************************/
/*
学生机器人间通信 cmd_id 0x0301，内容 ID:0x0200~0x02FF
自定义交互数据 机器人间通信：0x0301。
发送频率：上限 10Hz
*/
// 自定义交互数据协议，可更改，更改后需要修改最上方宏定义数据长度的值
typedef struct
{
uint8_t data[Communicate_Data_LEN]; // 数据段,n需要小于113
} robot_interactive_data_t;
// 机器人交互信息_发送
typedef struct
{
xFrameHeader FrameHeader;
uint16_t CmdID;
ext_student_interactive_header_data_t datahead;
robot_interactive_data_t Data; // 数据段
uint16_t frametail;
} Communicate_SendData_t;
// 机器人交互信息_接收
typedef struct
{
ext_student_interactive_header_data_t datahead;
robot_interactive_data_t Data; // 数据段
} Communicate_ReceiveData_t;
/****************************UI交互数据****************************/
/* 图形数据 */
typedef struct
{
uint8_t graphic_name[3];
uint32_t operate_tpye : 3;
uint32_t graphic_tpye : 3;
uint32_t layer : 4;
uint32_t color : 4;
uint32_t start_angle : 9;
uint32_t end_angle : 9;
uint32_t width : 10;
uint32_t start_x : 11;
uint32_t start_y : 11;
uint32_t radius : 10;
uint32_t end_x : 11;
uint32_t end_y : 11;
} Graph_Data_t;
typedef struct
{
Graph_Data_t Graph_Control;
uint8_t show_Data[30];
} String_Data_t; // 打印字符串数据
/* 删除操作 */
typedef enum
{
UI_Data_Del_NoOperate = 0,
UI_Data_Del_Layer = 1,
UI_Data_Del_ALL = 2, // 删除全部图层，后面的参数已经不重要了。
} UI_Delete_Operate_e;
/* 图形配置参数__图形操作 */
typedef enum
{
UI_Graph_ADD = 1,
UI_Graph_Change = 2,
UI_Graph_Del = 3,
} UI_Graph_Operate_e;
/* 图形配置参数__图形类型 */
typedef enum
{
UI_Graph_Line = 0, // 直线
UI_Graph_Rectangle = 1, // 矩形
UI_Graph_Circle = 2, // 整圆
UI_Graph_Ellipse = 3, // 椭圆
UI_Graph_Arc = 4, // 圆弧
UI_Graph_Float = 5, // 浮点型
UI_Graph_Int = 6, // 整形
UI_Graph_Char = 7, // 字符型
} UI_Graph_Type_e;
/* 图形配置参数__图形颜色 */
typedef enum
{
UI_Color_Main = 0, // 红蓝主色
UI_Color_Yellow = 1,
UI_Color_Green = 2,
UI_Color_Orange = 3,
UI_Color_Purplish_red = 4, // 紫红色
UI_Color_Pink = 5,
UI_Color_Cyan = 6, // 青色
UI_Color_Black = 7,
UI_Color_White = 8,
} UI_Graph_Color_e;
#pragma pack()
#endif
modules/referee/referee_task.c
+299
View File
@@ -0,0 +1,299 @@
/**
* @file referee.C
* @author kidneygood (you@domain.com)
* @brief
* @version 0.1
* @date 2022-11-18
*
* @copyright Copyright (c) 2022
*
*/
#include "referee_task.h"
#include "robot_def.h"
#include "rm_referee.h"
#include "referee_UI.h"
#include "string.h"
#include "cmsis_os.h"
static Referee_Interactive_info_t *Interactive_data; // UI绘制需要的机器人状态数据
static referee_info_t *referee_recv_info; // 接收到的裁判系统数据
uint8_t UI_Seq; // 包序号，供整个referee文件使用
// @todo 不应该使用全局变量
/**
* @brief 判断各种ID，选择客户端ID
* @param referee_info_t *referee_recv_info
* @retval none
* @attention
*/
static void DeterminRobotID()
{
// id小于7是红色,大于7是蓝色,0为红色，1为蓝色 #define Robot_Red 0 #define Robot_Blue 1
referee_recv_info->referee_id.Robot_Color = referee_recv_info->GameRobotState.robot_id > 7 ? Robot_Blue : Robot_Red;
referee_recv_info->referee_id.Robot_ID = referee_recv_info->GameRobotState.robot_id;
referee_recv_info->referee_id.Cilent_ID = 0x0100 + referee_recv_info->referee_id.Robot_ID; // 计算客户端ID
referee_recv_info->referee_id.Receiver_Robot_ID = 0;
}
static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_info_t *_Interactive_data);
static void UIChangeCheck(Referee_Interactive_info_t *_Interactive_data); // 模式切换检测
static void RobotModeTest(Referee_Interactive_info_t *_Interactive_data); // 测试用函数，实现模式自动变化
referee_info_t *UITaskInit(UART_HandleTypeDef *referee_usart_handle, Referee_Interactive_info_t *UI_data)
{
referee_recv_info = RefereeInit(referee_usart_handle); // 初始化裁判系统的串口,并返回裁判系统反馈数据指针
Interactive_data = UI_data; // 获取UI绘制需要的机器人状态数据
referee_recv_info->init_flag = 1;
return referee_recv_info;
}
void UITask()
{
RobotModeTest(Interactive_data); // 测试用函数，实现模式自动变化,用于检查该任务和裁判系统是否连接正常
MyUIRefresh(referee_recv_info, Interactive_data);
}
static Graph_Data_t UI_shoot_line[10]; // 射击准线
static Graph_Data_t UI_Energy[3]; // 电容能量条
static String_Data_t UI_State_sta[6]; // 机器人状态,静态只需画一次
static String_Data_t UI_State_dyn[6]; // 机器人状态,动态先add才能change
static uint32_t shoot_line_location[10] = {540, 960, 490, 515, 565};
void MyUIInit()
{
if (!referee_recv_info->init_flag)
vTaskDelete(NULL); // 如果没有初始化裁判系统则直接删除ui任务
while (referee_recv_info->GameRobotState.robot_id == 0)
osDelay(100); // 若还未收到裁判系统数据,等待一段时间后再检查
DeterminRobotID(); // 确定ui要发送到的目标客户端
UIDelete(&referee_recv_info->referee_id, UI_Data_Del_ALL, 0); // 清空UI
// 绘制发射基准线
UILineDraw(&UI_shoot_line[0], "sl0", UI_Graph_ADD, 7, UI_Color_White, 3, 710, shoot_line_location[0], 1210, shoot_line_location[0]);
UILineDraw(&UI_shoot_line[1], "sl1", UI_Graph_ADD, 7, UI_Color_White, 3, shoot_line_location[1], 340, shoot_line_location[1], 740);
UILineDraw(&UI_shoot_line[2], "sl2", UI_Graph_ADD, 7, UI_Color_Yellow, 2, 810, shoot_line_location[2], 1110, shoot_line_location[2]);
UILineDraw(&UI_shoot_line[3], "sl3", UI_Graph_ADD, 7, UI_Color_Yellow, 2, 810, shoot_line_location[3], 1110, shoot_line_location[3]);
UILineDraw(&UI_shoot_line[4], "sl4", UI_Graph_ADD, 7, UI_Color_Yellow, 2, 810, shoot_line_location[4], 1110, shoot_line_location[4]);
UIGraphRefresh(&referee_recv_info->referee_id, 5, UI_shoot_line[0], UI_shoot_line[1], UI_shoot_line[2], UI_shoot_line[3], UI_shoot_line[4]);
// 绘制车辆状态标志指示
UICharDraw(&UI_State_sta[0], "ss0", UI_Graph_ADD, 8, UI_Color_Main, 15, 2, 150, 750, "chassis:");
UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[0]);
UICharDraw(&UI_State_sta[1], "ss1", UI_Graph_ADD, 8, UI_Color_Yellow, 15, 2, 150, 700, "gimbal:");
UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[1]);
UICharDraw(&UI_State_sta[2], "ss2", UI_Graph_ADD, 8, UI_Color_Orange, 15, 2, 150, 650, "shoot:");
UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[2]);
UICharDraw(&UI_State_sta[3], "ss3", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 150, 600, "frict:");
UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[3]);
UICharDraw(&UI_State_sta[4], "ss4", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 150, 550, "lid:");
UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[4]);
// 绘制车辆状态标志，动态
// 由于初始化时xxx_last_mode默认为0，所以此处对应UI也应该设为0时对应的UI，防止模式不变的情况下无法置位flag，导致UI无法刷新
UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_ADD, 8, UI_Color_Main, 15, 2, 270, 750, "zeroforce");
UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[0]);
UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_ADD, 8, UI_Color_Yellow, 15, 2, 270, 700, "zeroforce");
UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[1]);
UICharDraw(&UI_State_dyn[2], "sd2", UI_Graph_ADD, 8, UI_Color_Orange, 15, 2, 270, 650, "off");
UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[2]);
UICharDraw(&UI_State_dyn[3], "sd3", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 270, 600, "off");
UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[3]);
UICharDraw(&UI_State_dyn[4], "sd4", UI_Graph_ADD, 8, UI_Color_Pink, 15, 2, 270, 550, "open ");
UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[4]);
// 底盘功率显示，静态
UICharDraw(&UI_State_sta[5], "ss5", UI_Graph_ADD, 7, UI_Color_Green, 18, 2, 620, 230, "Power:");
UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[5]);
// 能量条框
UIRectangleDraw(&UI_Energy[0], "ss6", UI_Graph_ADD, 7, UI_Color_Green, 2, 720, 140, 1220, 180);
UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_Energy[0]);
// 底盘功率显示,动态
UIFloatDraw(&UI_Energy[1], "sd5", UI_Graph_ADD, 8, UI_Color_Green, 18, 2, 2, 750, 230, 24000);
// 能量条初始状态
UILineDraw(&UI_Energy[2], "sd6", UI_Graph_ADD, 8, UI_Color_Pink, 30, 720, 160, 1020, 160);
UIGraphRefresh(&referee_recv_info->referee_id, 2, UI_Energy[1], UI_Energy[2]);
}
// 测试用函数，实现模式自动变化,用于检查该任务和裁判系统是否连接正常
static uint8_t count = 0;
static uint16_t count1 = 0;
static void RobotModeTest(Referee_Interactive_info_t *_Interactive_data) // 测试用函数，实现模式自动变化
{
count++;
if (count >= 50)
{
count = 0;
count1++;
}
switch (count1 % 4)
{
case 0:
{
_Interactive_data->chassis_mode = CHASSIS_ZERO_FORCE;
_Interactive_data->gimbal_mode = GIMBAL_ZERO_FORCE;
_Interactive_data->shoot_mode = SHOOT_ON;
_Interactive_data->friction_mode = FRICTION_ON;
_Interactive_data->lid_mode = LID_OPEN;
_Interactive_data->Chassis_Power_Data.chassis_power_mx += 3.5;
if (_Interactive_data->Chassis_Power_Data.chassis_power_mx >= 18)
_Interactive_data->Chassis_Power_Data.chassis_power_mx = 0;
break;
}
case 1:
{
_Interactive_data->chassis_mode = CHASSIS_ROTATE;
_Interactive_data->gimbal_mode = GIMBAL_FREE_MODE;
_Interactive_data->shoot_mode = SHOOT_OFF;
_Interactive_data->friction_mode = FRICTION_OFF;
_Interactive_data->lid_mode = LID_CLOSE;
break;
}
case 2:
{
_Interactive_data->chassis_mode = CHASSIS_NO_FOLLOW;
_Interactive_data->gimbal_mode = GIMBAL_GYRO_MODE;
_Interactive_data->shoot_mode = SHOOT_ON;
_Interactive_data->friction_mode = FRICTION_ON;
_Interactive_data->lid_mode = LID_OPEN;
break;
}
case 3:
{
_Interactive_data->chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
_Interactive_data->gimbal_mode = GIMBAL_ZERO_FORCE;
_Interactive_data->shoot_mode = SHOOT_OFF;
_Interactive_data->friction_mode = FRICTION_OFF;
_Interactive_data->lid_mode = LID_CLOSE;
break;
}
default:
break;
}
}
static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_info_t *_Interactive_data)
{
UIChangeCheck(_Interactive_data);
// chassis
if (_Interactive_data->Referee_Interactive_Flag.chassis_flag == 1)
{
switch (_Interactive_data->chassis_mode)
{
case CHASSIS_ZERO_FORCE:
UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "zeroforce");
break;
case CHASSIS_ROTATE:
UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "rotate ");
// 此处注意字数对齐问题，字数相同才能覆盖掉
break;
case CHASSIS_NO_FOLLOW:
UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "nofollow ");
break;
case CHASSIS_FOLLOW_GIMBAL_YAW:
UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_Change, 8, UI_Color_Main, 15, 2, 270, 750, "follow ");
break;
}
UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[0]);
_Interactive_data->Referee_Interactive_Flag.chassis_flag = 0;
}
// gimbal
if (_Interactive_data->Referee_Interactive_Flag.gimbal_flag == 1)
{
switch (_Interactive_data->gimbal_mode)
{
case GIMBAL_ZERO_FORCE:
{
UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "zeroforce");
break;
}
case GIMBAL_FREE_MODE:
{
UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "free ");
break;
}
case GIMBAL_GYRO_MODE:
{
UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_Change, 8, UI_Color_Yellow, 15, 2, 270, 700, "gyro ");
break;
}
}
UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[1]);
_Interactive_data->Referee_Interactive_Flag.gimbal_flag = 0;
}
// shoot
if (_Interactive_data->Referee_Interactive_Flag.shoot_flag == 1)
{
UICharDraw(&UI_State_dyn[2], "sd2", UI_Graph_Change, 8, UI_Color_Pink, 15, 2, 270, 650, _Interactive_data->shoot_mode == SHOOT_ON ? "on " : "off");
UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[2]);
_Interactive_data->Referee_Interactive_Flag.shoot_flag = 0;
}
// friction
if (_Interactive_data->Referee_Interactive_Flag.friction_flag == 1)
{
UICharDraw(&UI_State_dyn[3], "sd3", UI_Graph_Change, 8, UI_Color_Pink, 15, 2, 270, 600, _Interactive_data->friction_mode == FRICTION_ON ? "on " : "off");
UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[3]);
_Interactive_data->Referee_Interactive_Flag.friction_flag = 0;
}
// lid
if (_Interactive_data->Referee_Interactive_Flag.lid_flag == 1)
{
UICharDraw(&UI_State_dyn[4], "sd4", UI_Graph_Change, 8, UI_Color_Pink, 15, 2, 270, 550, _Interactive_data->lid_mode == LID_OPEN ? "open " : "close");
UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[4]);
_Interactive_data->Referee_Interactive_Flag.lid_flag = 0;
}
// power
if (_Interactive_data->Referee_Interactive_Flag.Power_flag == 1)
{
UIFloatDraw(&UI_Energy[1], "sd5", UI_Graph_Change, 8, UI_Color_Green, 18, 2, 2, 750, 230, _Interactive_data->Chassis_Power_Data.chassis_power_mx * 1000);
UILineDraw(&UI_Energy[2], "sd6", UI_Graph_Change, 8, UI_Color_Pink, 30, 720, 160, (uint32_t)750 + _Interactive_data->Chassis_Power_Data.chassis_power_mx * 30, 160);
UIGraphRefresh(&referee_recv_info->referee_id, 2, UI_Energy[1], UI_Energy[2]);
_Interactive_data->Referee_Interactive_Flag.Power_flag = 0;
}
}
/**
* @brief 模式切换检测,模式发生切换时，对flag置位
* @param Referee_Interactive_info_t *_Interactive_data
* @retval none
* @attention
*/
static void UIChangeCheck(Referee_Interactive_info_t *_Interactive_data)
{
if (_Interactive_data->chassis_mode != _Interactive_data->chassis_last_mode)
{
_Interactive_data->Referee_Interactive_Flag.chassis_flag = 1;
_Interactive_data->chassis_last_mode = _Interactive_data->chassis_mode;
}
if (_Interactive_data->gimbal_mode != _Interactive_data->gimbal_last_mode)
{
_Interactive_data->Referee_Interactive_Flag.gimbal_flag = 1;
_Interactive_data->gimbal_last_mode = _Interactive_data->gimbal_mode;
}
if (_Interactive_data->shoot_mode != _Interactive_data->shoot_last_mode)
{
_Interactive_data->Referee_Interactive_Flag.shoot_flag = 1;
_Interactive_data->shoot_last_mode = _Interactive_data->shoot_mode;
}
if (_Interactive_data->friction_mode != _Interactive_data->friction_last_mode)
{
_Interactive_data->Referee_Interactive_Flag.friction_flag = 1;
_Interactive_data->friction_last_mode = _Interactive_data->friction_mode;
}
if (_Interactive_data->lid_mode != _Interactive_data->lid_last_mode)
{
_Interactive_data->Referee_Interactive_Flag.lid_flag = 1;
_Interactive_data->lid_last_mode = _Interactive_data->lid_mode;
}
if (_Interactive_data->Chassis_Power_Data.chassis_power_mx != _Interactive_data->Chassis_last_Power_Data.chassis_power_mx)
{
_Interactive_data->Referee_Interactive_Flag.Power_flag = 1;
_Interactive_data->Chassis_last_Power_Data.chassis_power_mx = _Interactive_data->Chassis_Power_Data.chassis_power_mx;
}
}
modules/referee/referee_task.h
+25
View File
@@ -0,0 +1,25 @@
#ifndef REFEREE_H
#define REFEREE_H
#include "rm_referee.h"
#include "robot_def.h"
/**
* @brief 初始化裁判系统交互任务(UI和多机通信)
*
*/
referee_info_t *UITaskInit(UART_HandleTypeDef *referee_usart_handle, Referee_Interactive_info_t *UI_data);
/**
* @brief 在referee task之前调用,添加在freertos.c中
*
*/
void MyUIInit();
/**
* @brief 裁判系统交互任务(UI和多机通信)
*
*/
void UITask();
#endif // REFEREE_H
modules/referee/rm_referee.c
+149
View File
@@ -0,0 +1,149 @@
/**
* @file rm_referee.C
* @author kidneygood (you@domain.com)
* @brief
* @version 0.1
* @date 2022-11-18
*
* @copyright Copyright (c) 2022
*
*/
#include "rm_referee.h"
#include "string.h"
#include "crc_ref.h"
#include "bsp_usart.h"
#include "task.h"
#include "daemon.h"
#include "bsp_log.h"
#include "cmsis_os.h"
#define RE_RX_BUFFER_SIZE 255u // 裁判系统接收缓冲区大小
static USARTInstance *referee_usart_instance; // 裁判系统串口实例
static DaemonInstance *referee_daemon; // 裁判系统守护进程
static referee_info_t referee_info; // 裁判系统数据
/**
* @brief 读取裁判数据,中断中读取保证速度
* @param buff: 读取到的裁判系统原始数据
* @retval 是否对正误判断做处理
* @attention 在此判断帧头和CRC校验,无误再写入数据，不重复判断帧头
*/
static void JudgeReadData(uint8_t *buff)
{
uint16_t judge_length; // 统计一帧数据长度
if (buff == NULL) // 空数据包，则不作任何处理
return;
// 写入帧头数据(5-byte),用于判断是否开始存储裁判数据
memcpy(&referee_info.FrameHeader, buff, LEN_HEADER);
// 判断帧头数据(0)是否为0xA5
if (buff[SOF] == REFEREE_SOF)
{
// 帧头CRC8校验
if (Verify_CRC8_Check_Sum(buff, LEN_HEADER) == TRUE)
{
// 统计一帧数据长度(byte),用于CR16校验
judge_length = buff[DATA_LENGTH] + LEN_HEADER + LEN_CMDID + LEN_TAIL;
// 帧尾CRC16校验
if (Verify_CRC16_Check_Sum(buff, judge_length) == TRUE)
{
// 2个8位拼成16位int
referee_info.CmdID = (buff[6] << 8 | buff[5]);
// 解析数据命令码,将数据拷贝到相应结构体中(注意拷贝数据的长度)
// 第8个字节开始才是数据 data=7
switch (referee_info.CmdID)
{
case ID_game_state: // 0x0001
memcpy(&referee_info.GameState, (buff + DATA_Offset), LEN_game_state);
break;
case ID_game_result: // 0x0002
memcpy(&referee_info.GameResult, (buff + DATA_Offset), LEN_game_result);
break;
case ID_game_robot_survivors: // 0x0003
memcpy(&referee_info.GameRobotHP, (buff + DATA_Offset), LEN_game_robot_HP);
break;
case ID_event_data: // 0x0101
memcpy(&referee_info.EventData, (buff + DATA_Offset), LEN_event_data);
break;
case ID_supply_projectile_action: // 0x0102
memcpy(&referee_info.SupplyProjectileAction, (buff + DATA_Offset), LEN_supply_projectile_action);
break;
case ID_game_robot_state: // 0x0201
memcpy(&referee_info.GameRobotState, (buff + DATA_Offset), LEN_game_robot_state);
break;
case ID_power_heat_data: // 0x0202
memcpy(&referee_info.PowerHeatData, (buff + DATA_Offset), LEN_power_heat_data);
break;
case ID_game_robot_pos: // 0x0203
memcpy(&referee_info.GameRobotPos, (buff + DATA_Offset), LEN_game_robot_pos);
break;
case ID_buff_musk: // 0x0204
memcpy(&referee_info.BuffMusk, (buff + DATA_Offset), LEN_buff_musk);
break;
case ID_aerial_robot_energy: // 0x0205
memcpy(&referee_info.AerialRobotEnergy, (buff + DATA_Offset), LEN_aerial_robot_energy);
break;
case ID_robot_hurt: // 0x0206
memcpy(&referee_info.RobotHurt, (buff + DATA_Offset), LEN_robot_hurt);
break;
case ID_shoot_data: // 0x0207
memcpy(&referee_info.ShootData, (buff + DATA_Offset), LEN_shoot_data);
break;
case ID_student_interactive: // 0x0301 syhtodo接收代码未测试
memcpy(&referee_info.ReceiveData, (buff + DATA_Offset), LEN_receive_data);
break;
}
}
}
// 首地址加帧长度,指向CRC16下一字节,用来判断是否为0xA5,从而判断一个数据包是否有多帧数据
if (*(buff + sizeof(xFrameHeader) + LEN_CMDID + referee_info.FrameHeader.DataLength + LEN_TAIL) == 0xA5)
{ // 如果一个数据包出现了多帧数据,则再次调用解析函数,直到所有数据包解析完毕
JudgeReadData(buff + sizeof(xFrameHeader) + LEN_CMDID + referee_info.FrameHeader.DataLength + LEN_TAIL);
}
}
}
/*裁判系统串口接收回调函数,解析数据 */
static void RefereeRxCallback()
{
DaemonReload(referee_daemon);
JudgeReadData(referee_usart_instance->recv_buff);
}
// 裁判系统丢失回调函数,重新初始化裁判系统串口
static void RefereeLostCallback(void *arg)
{
USARTServiceInit(referee_usart_instance);
LOGWARNING("[rm_ref] lost referee data");
}
/* 裁判系统通信初始化 */
referee_info_t *RefereeInit(UART_HandleTypeDef *referee_usart_handle)
{
USART_Init_Config_s conf;
conf.module_callback = RefereeRxCallback;
conf.usart_handle = referee_usart_handle;
conf.recv_buff_size = RE_RX_BUFFER_SIZE; // mx 255(u8)
referee_usart_instance = USARTRegister(&conf);
Daemon_Init_Config_s daemon_conf = {
.callback = RefereeLostCallback,
.owner_id = referee_usart_instance,
.reload_count = 30, // 0.3s没有收到数据,则认为丢失,重启串口接收
};
referee_daemon = DaemonRegister(&daemon_conf);
return &referee_info;
}
/**
* @brief 裁判系统数据发送函数
* @param
*/
void RefereeSend(uint8_t *send, uint16_t tx_len)
{
USARTSend(referee_usart_instance, send, tx_len, USART_TRANSFER_DMA);
osDelay(115);
}
modules/referee/rm_referee.h
+100
View File
@@ -0,0 +1,100 @@
#ifndef RM_REFEREE_H
#define RM_REFEREE_H
#include "usart.h"
#include "referee_protocol.h"
#include "robot_def.h"
#include "bsp_usart.h"
#include "FreeRTOS.h"
extern uint8_t UI_Seq;
#pragma pack(1)
typedef struct
{
uint8_t Robot_Color; // 机器人颜色
uint16_t Robot_ID; // 本机器人ID
uint16_t Cilent_ID; // 本机器人对应的客户端ID
uint16_t Receiver_Robot_ID; // 机器人车间通信时接收者的ID，必须和本机器人同颜色
} referee_id_t;
// 此结构体包含裁判系统接收数据以及UI绘制与机器人车间通信的相关信息
typedef struct
{
referee_id_t referee_id;
xFrameHeader FrameHeader; // 接收到的帧头信息
uint16_t CmdID;
ext_game_state_t GameState; // 0x0001
ext_game_result_t GameResult; // 0x0002
ext_game_robot_HP_t GameRobotHP; // 0x0003
ext_event_data_t EventData; // 0x0101
ext_supply_projectile_action_t SupplyProjectileAction; // 0x0102
ext_game_robot_state_t GameRobotState; // 0x0201
ext_power_heat_data_t PowerHeatData; // 0x0202
ext_game_robot_pos_t GameRobotPos; // 0x0203
ext_buff_musk_t BuffMusk; // 0x0204
aerial_robot_energy_t AerialRobotEnergy; // 0x0205
ext_robot_hurt_t RobotHurt; // 0x0206
ext_shoot_data_t ShootData; // 0x0207
// 自定义交互数据的接收
Communicate_ReceiveData_t ReceiveData;
uint8_t init_flag;
} referee_info_t;
// 模式是否切换标志位，0为未切换，1为切换，static定义默认为0
typedef struct
{
uint32_t chassis_flag : 1;
uint32_t gimbal_flag : 1;
uint32_t shoot_flag : 1;
uint32_t lid_flag : 1;
uint32_t friction_flag : 1;
uint32_t Power_flag : 1;
} Referee_Interactive_Flag_t;
// 此结构体包含UI绘制与机器人车间通信的需要的其他非裁判系统数据
typedef struct
{
Referee_Interactive_Flag_t Referee_Interactive_Flag;
// 为UI绘制以及交互数据所用
chassis_mode_e chassis_mode; // 底盘模式
gimbal_mode_e gimbal_mode; // 云台模式
shoot_mode_e shoot_mode; // 发射模式设置
friction_mode_e friction_mode; // 摩擦轮关闭
lid_mode_e lid_mode; // 弹舱盖打开
Chassis_Power_Data_s Chassis_Power_Data; // 功率控制
// 上一次的模式，用于flag判断
chassis_mode_e chassis_last_mode;
gimbal_mode_e gimbal_last_mode;
shoot_mode_e shoot_last_mode;
friction_mode_e friction_last_mode;
lid_mode_e lid_last_mode;
Chassis_Power_Data_s Chassis_last_Power_Data;
} Referee_Interactive_info_t;
#pragma pack()
/**
* @brief 裁判系统通信初始化,该函数会初始化裁判系统串口,开启中断
*
* @param referee_usart_handle 串口handle,C板一般用串口6
* @return referee_info_t* 返回裁判系统反馈的数据,包括热量/血量/状态等
*/
referee_info_t *RefereeInit(UART_HandleTypeDef *referee_usart_handle);
/**
* @brief UI绘制和交互数的发送接口,由UI绘制任务和多机通信函数调用
* @note 内部包含了一个实时系统的延时函数,这是因为裁判系统接收CMD数据至高位10Hz
*
* @param send 发送数据首地址
* @param tx_len 发送长度
*/
void RefereeSend(uint8_t *send, uint16_t tx_len);
#endif // !REFEREE_H
modules/remote/remote.md
+85
View File
@@ -0,0 +1,85 @@
remote_control
<p align='right'>neozng1@hnu.edu.cn</p>
```
/*************************发射机DT7***************************
* *
* ----------------------------------------------------- *
* | (上-1) (上-1) | *
* |SW_L|(中-3) SW_R|(中-3) | *
* | (下-2) (下-2) | *
* | | *
* | | ^ | | ^ | | *
* | | 3 |左摇杆 右摇杆| 1 | | *
* | --- --- --- --- | *
* |< 2> < 0>| *
* | --- --- --- --- | *
* | | | | | | *
* | | | | | | *
* | | *
* ----------------------------------------------------- *
* *
**************************遥控器信息**************************
*域 通道0 通道1 通道2 通道3 S1 S2 *
*偏移 0 11 22 33 44 46 *
*长度(bit) 11 11 11 11 2 2 *
*符号位 无 无 无 无 无 无 *
*范围 ***********最大值1684********* *最大值3* *
* * 中间值1024 * *最小值1* *
* ***********最小值364********** *
*功能 1:上 1:上 *
* 2:下 2:下 *
* 3:中 3:中 *
* *
***************************鼠标信息***************************
*域 鼠标x轴 鼠标y轴 鼠标z轴 鼠标左键 鼠标右键*
*偏移 48 64 80 86 94 *
*长度 16 16 16 8 8 *
*符号位 有 有 有 无 无 *
*范围 ******最大值32767***** ***最大值1*** *
* * 最小值-32768 * ***最小值0*** *
* ******静止值0********* *
*功能 ***鼠标在XYZ轴的移动速度*** *鼠标左右键是否按下*
* * 负值表示往左移动 * * 0:没按下 *
* ***正值表示往右移动******** *****1:按下*********
* *
* *
***************************键盘信息***************************
*域 按键 *
*偏移 102 *
*长度 16 *
*符号位 无 *
*范围 位值标识 *
*功能 每个按键对应一个bit *
* Bit 0:W键 *
* Bit 1:S键 *
* Bit 2:A键 *
* Bit 3:D键 *
* Bit 4:Shift键 *
* Bit 5:Ctrl键 *
* Bit 6:Q键 *
* Bit 7:E键 *
* Bit 8:R键 *
* Bit 9:F键 *
* Bit10:G键 *
* Bit11:Z键 *
* Bit12:X键 *
* Bit13:C键 *
* Bit14:V键 *
* Bit15:B键 *
**************************************************************/
```
目前的映射:
拨轮向下打到底进入紧急停止模式(后续改为关闭遥控器停止,利用daemon; daemon已经模块完成);拨轮向上打开启摩擦轮,超过一半开始发射(速度环,连发)
左侧开关:
- 上:键鼠控制
- 中:视觉控制(没有识别到目标的时候仍然可以使用遥控器控制云台)
- 下:遥控器控制
右侧开关:
- 上:弹舱开
- 中:底盘云台分离(底盘不旋转,全向移动)
- 下:底盘跟随云台
modules/remote/remote_control.c
+252
View File
@@ -0,0 +1,252 @@
#include "remote_control.h"
#include "string.h"
#include "bsp_usart.h"
#include "memory.h"
#include "stdlib.h"
#include "daemon.h"
#include "bsp_log.h"
#define USE_DT7_REMOTE
// #define USE_SBUS_REMOTE_FUS
#ifdef USE_DT7_REMOTE
#define REMOTE_CONTROL_FRAME_SIZE 18u // 遥控器接收的buffer大小
// 遥控器数据
static RC_ctrl_t rc_ctrl[2]; //[0]:当前数据TEMP,[1]:上一次的数据LAST.用于按键持续按下和切换的判断
static uint8_t rc_init_flag = 0; // 遥控器初始化标志位
// 遥控器拥有的串口实例,因为遥控器是单例,所以这里只有一个,就不封装了
static USARTInstance *rc_usart_instance;
static DaemonInstance *rc_daemon_instance;
/**
* @brief 矫正遥控器摇杆的值,超过660或者小于-660的值都认为是无效值,置0
*
*/
static void RectifyRCjoystick()
{
for (uint8_t i = 0; i < 5; ++i)
if (abs(*(&rc_ctrl[TEMP].rc.rocker_l_ + i)) > 660)
*(&rc_ctrl[TEMP].rc.rocker_l_ + i) = 0;
}
/**
* @brief 遥控器数据解析
*
* @param sbus_buf 接收buffer
*/
static void sbus_to_rc(const uint8_t *sbus_buf)
{
// 摇杆,直接解算时减去偏置
rc_ctrl[TEMP].rc.rocker_r_ = ((sbus_buf[0] | (sbus_buf[1] << 8)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 0
rc_ctrl[TEMP].rc.rocker_r1 = (((sbus_buf[1] >> 3) | (sbus_buf[2] << 5)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 1
rc_ctrl[TEMP].rc.rocker_l_ = (((sbus_buf[2] >> 6) | (sbus_buf[3] << 2) | (sbus_buf[4] << 10)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 2
rc_ctrl[TEMP].rc.rocker_l1 = (((sbus_buf[4] >> 1) | (sbus_buf[5] << 7)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 3
rc_ctrl[TEMP].rc.dial = ((sbus_buf[16] | (sbus_buf[17] << 8)) & 0x07FF) - RC_CH_VALUE_OFFSET; // 左侧拨轮
RectifyRCjoystick();
// 开关,0左1右
rc_ctrl[TEMP].rc.switch_right = ((sbus_buf[5] >> 4) & 0x0003); //!< Switch right
rc_ctrl[TEMP].rc.switch_left = ((sbus_buf[5] >> 4) & 0x000C) >> 2; //!< Switch left
// 鼠标解析
rc_ctrl[TEMP].mouse.x = (sbus_buf[6] | (sbus_buf[7] << 8)); //!< Mouse X axis
rc_ctrl[TEMP].mouse.y = (sbus_buf[8] | (sbus_buf[9] << 8)); //!< Mouse Y axis
rc_ctrl[TEMP].mouse.press_l = sbus_buf[12]; //!< Mouse Left Is Press ?
rc_ctrl[TEMP].mouse.press_r = sbus_buf[13]; //!< Mouse Right Is Press ?
// 位域的按键值解算,直接memcpy即可,注意小端低字节在前,即lsb在第一位,msb在最后
*(uint16_t *)&rc_ctrl[TEMP].key[KEY_PRESS] = (uint16_t)(sbus_buf[14] | (sbus_buf[15] << 8));
if (rc_ctrl[TEMP].key[KEY_PRESS].ctrl) // ctrl键按下
rc_ctrl[TEMP].key[KEY_PRESS_WITH_CTRL] = rc_ctrl[TEMP].key[KEY_PRESS];
else
memset(&rc_ctrl[TEMP].key[KEY_PRESS_WITH_CTRL], 0, sizeof(Key_t));
if (rc_ctrl[TEMP].key[KEY_PRESS].shift) // shift键按下
rc_ctrl[TEMP].key[KEY_PRESS_WITH_SHIFT] = rc_ctrl[TEMP].key[KEY_PRESS];
else
memset(&rc_ctrl[TEMP].key[KEY_PRESS_WITH_SHIFT], 0, sizeof(Key_t));
uint16_t key_now = rc_ctrl[TEMP].key[KEY_PRESS].keys, // 当前按键是否按下
key_last = rc_ctrl[LAST].key[KEY_PRESS].keys, // 上一次按键是否按下
key_with_ctrl = rc_ctrl[TEMP].key[KEY_PRESS_WITH_CTRL].keys, // 当前ctrl组合键是否按下
key_with_shift = rc_ctrl[TEMP].key[KEY_PRESS_WITH_SHIFT].keys, // 当前shift组合键是否按下
key_last_with_ctrl = rc_ctrl[LAST].key[KEY_PRESS_WITH_CTRL].keys, // 上一次ctrl组合键是否按下
key_last_with_shift = rc_ctrl[LAST].key[KEY_PRESS_WITH_SHIFT].keys; // 上一次shift组合键是否按下
for (uint16_t i = 0, j = 0x1; i < 16; j <<= 1, i++)
{
if (i == 4 || i == 5) // 4,5位为ctrl和shift,直接跳过
continue;
// 如果当前按键按下,上一次按键没有按下,且ctrl和shift组合键没有按下,则按键按下计数加1(检测到上升沿)
if ((key_now & j) && !(key_last & j) && !(key_with_ctrl & j) && !(key_with_shift & j))
rc_ctrl[TEMP].key_count[KEY_PRESS][i]++;
// 当前ctrl组合键按下,上一次ctrl组合键没有按下,则ctrl组合键按下计数加1(检测到上升沿)
if ((key_with_ctrl & j) && !(key_last_with_ctrl & j))
rc_ctrl[TEMP].key_count[KEY_PRESS_WITH_CTRL][i]++;
// 当前shift组合键按下,上一次shift组合键没有按下,则shift组合键按下计数加1(检测到上升沿)
if ((key_with_shift & j) && !(key_last_with_shift & j))
rc_ctrl[TEMP].key_count[KEY_PRESS_WITH_SHIFT][i]++;
}
memcpy(&rc_ctrl[LAST], &rc_ctrl[TEMP], sizeof(RC_ctrl_t)); // 保存上一次的数据,用于按键持续按下和切换的判断
}
/**
* @brief 对sbus_to_rc的简单封装,用于注册到bsp_usart的回调函数中
*
*/
static void RemoteControlRxCallback()
{
DaemonReload(rc_daemon_instance); // 先喂狗
sbus_to_rc(rc_usart_instance->recv_buff); // 进行协议解析
}
#endif
#ifdef USE_SBUS_REMOTE_FUS
#define REMOTE_CONTROL_FRAME_SIZE 25u // 遥控器接收的buffer大小
// 遥控器数据
static RC_ctrl_t rc_ctrl[2]; //[0]:当前数据TEMP,[1]:上一次的数据LAST.用于按键持续按下和切换的判断
static uint8_t rc_init_flag = 0; // 遥控器初始化标志位
// static uint8_t sbus_rc_buf[25];
// 遥控器拥有的串口实例,因为遥控器是单例,所以这里只有一个,就不封装了
static USARTInstance *rc_usart_instance;
static DaemonInstance *rc_daemon_instance;
static int16_t sRecvChanelData[16];
/**
* @brief 矫正遥控器摇杆的值,超过660或者小于-660的值都认为是无效值,置0
*
*/
static void RectifyRCjoystick()
{
for (uint8_t i = 0; i < 5; ++i)
if (abs(*(&rc_ctrl[TEMP].rc.rocker_l_ + i)) > 785)
*(&rc_ctrl[TEMP].rc.rocker_l_ + i) = 0;
}
/**
* @brief 遥控器数据解析
*
* @param sbus_buf 接收buffer
*/
static void sbus_to_rc(const uint8_t *sbus_buf)
{
// LOGINFO("[rc] sbus_to_rc");
// memcpy(sbus_rc_buf, sbus_buf, REMOTE_CONTROL_FRAME_SIZE);
const int channel_bits = 11;
const int channel_mask = (1 << channel_bits) - 1;
// 读取左右摇杆和开关状态
sRecvChanelData[ 0 ] = ( (sbus_buf[ 2] & 0x07) << 8 ) + ((sbus_buf[ 1] >> 0) & 0xFF); //右水平
sRecvChanelData[ 1 ] = ( (sbus_buf[ 3] & 0x3F) << 5 ) + ((sbus_buf[ 2] >> 3) & 0x1F); //右竖直
sRecvChanelData[ 2 ] = ( (sbus_buf[ 5] & 0x01) << 10) + ((sbus_buf[ 4] & 0xFF) << 2) + ((sbus_buf[ 3] >> 6) & 0x03); //左竖直
sRecvChanelData[ 3 ] = ( (sbus_buf[ 6] & 0x0F) << 7 ) + ((sbus_buf[ 5] >> 1) & 0x7F); //左水平
sRecvChanelData[ 4 ] = ( (sbus_buf[ 7] & 0x7F) << 4 ) + ((sbus_buf[ 6] >> 4) & 0x0F); //左侧拨轮
sRecvChanelData[ 5 ] = ( (sbus_buf[ 9] & 0x03) << 9 ) + ((sbus_buf[ 8] & 0xFF) << 1 ) + ((sbus_buf[ 7] >> 7) & 0x01); //右侧拨轮
sRecvChanelData[ 6 ] = ( (sbus_buf[10] & 0x1F) << 6 ) + ((sbus_buf[ 9] >> 2) & 0x3F); //开关A
sRecvChanelData[ 7 ] = ( (sbus_buf[11] & 0xFF) << 3 ) + ((sbus_buf[10] >> 5) & 0x07); //开关B
sRecvChanelData[ 8 ] = ( (sbus_buf[13] & 0x07) << 8 ) + ((sbus_buf[12] >> 0) & 0xFF); //开关C
sRecvChanelData[ 9 ] = ( (sbus_buf[14] & 0x3F) << 5 ) + ((sbus_buf[13] >> 3) & 0x1F); //开关D
rc_ctrl[TEMP].rc.rocker_r_ = sRecvChanelData[0] - RC_CH_VALUE_OFFSET;
rc_ctrl[TEMP].rc.rocker_r1 = sRecvChanelData[1] - RC_CH_VALUE_OFFSET;
rc_ctrl[TEMP].rc.rocker_l1 = sRecvChanelData[2] - RC_CH_VALUE_OFFSET;
rc_ctrl[TEMP].rc.rocker_l_ = sRecvChanelData[3] - RC_CH_VALUE_OFFSET;
rc_ctrl[TEMP].rc.dial1 = sRecvChanelData[4] - RC_CH_VALUE_OFFSET;
rc_ctrl[TEMP].rc.dial2 = sRecvChanelData[5] - RC_CH_VALUE_OFFSET;
if(sRecvChanelData[ 6 ] == 240)
{
rc_ctrl[TEMP].rc.swa = RC_SW_UP;
}
else
{
rc_ctrl[TEMP].rc.swa = RC_SW_DOWN;
}
if(sRecvChanelData[ 7 ] == 240)
{
rc_ctrl[TEMP].rc.swb = RC_SW_UP;
}
else
{
rc_ctrl[TEMP].rc.swb = RC_SW_DOWN;
}
if(sRecvChanelData[ 8 ] == 240)
{
rc_ctrl[TEMP].rc.swc = RC_SW_UP;
}
else if(sRecvChanelData[ 8 ] == 1024)
{
rc_ctrl[TEMP].rc.swc = RC_SW_MID;
}
else
{
rc_ctrl[TEMP].rc.swc = RC_SW_DOWN;
}
if(sRecvChanelData[ 9 ] == 240)
{
rc_ctrl[TEMP].rc.swd = RC_SW_UP;
}
else
{
rc_ctrl[TEMP].rc.swd = RC_SW_DOWN;
}
RectifyRCjoystick();
// memcpy(&rc_ctrl[LAST], &rc_ctrl[TEMP], sizeof(RC_ctrl_t)); // 保存上一次的数据,用于按键持续按下和切换的判断
}
/**
* @brief 对sbus_to_rc的简单封装,用于注册到bsp_usart的回调函数中
*
*/
static void RemoteControlRxCallback()
{
DaemonReload(rc_daemon_instance); // 先喂狗
if (rc_usart_instance->recv_buff[0] == 0x0f) // 遥控器的帧头
{
sbus_to_rc(rc_usart_instance->recv_buff); // 进行协议解析
}
}
#endif
/**
* @brief 遥控器离线的回调函数,注册到守护进程中,串口掉线时调用
*
*/
static void RCLostCallback(void *id)
{
// memset(rc_ctrl, 0, sizeof(rc_ctrl)); // 清空遥控器数据
USARTServiceInit(rc_usart_instance); // 尝试重新启动接收
LOGWARNING("[rc] remote control lost");
}
RC_ctrl_t *RemoteControlInit(UART_HandleTypeDef *rc_usart_handle)
{
USART_Init_Config_s conf;
conf.module_callback = RemoteControlRxCallback;
conf.usart_handle = rc_usart_handle;
conf.recv_buff_size = REMOTE_CONTROL_FRAME_SIZE;
rc_usart_instance = USARTRegister(&conf);
// 进行守护进程的注册,用于定时检查遥控器是否正常工作
Daemon_Init_Config_s daemon_conf = {
.reload_count = 10, // 100ms未收到数据视为离线,遥控器的接收频率实际上是1000/14Hz(大约70Hz)
.callback = RCLostCallback,
.owner_id = NULL, // 只有1个遥控器,不需要owner_id
};
rc_daemon_instance = DaemonRegister(&daemon_conf);
rc_init_flag = 1;
return rc_ctrl;
}
uint8_t RemoteControlIsOnline()
{
if (rc_init_flag)
return DaemonIsOnline(rc_daemon_instance);
return 0;
}
modules/remote/remote_control.h
+139
View File
@@ -0,0 +1,139 @@
/**
* @file remote_control.h
* @author DJI 2016
* @author modified by neozng
* @brief 遥控器模块定义头文件
* @version beta
* @date 2022-11-01
*
* @copyright Copyright (c) 2016 DJI corp
* @copyright Copyright (c) 2022 HNU YueLu EC all rights reserved
*
*/
#ifndef REMOTE_CONTROL_H
#define REMOTE_CONTROL_H
#include <stdint.h>
#include "main.h"
#include "usart.h"
// 用于遥控器数据读取,遥控器数据是一个大小为2的数组
#define LAST 1
#define TEMP 0
// 获取按键操作
#define KEY_PRESS 0
#define KEY_STATE 1
#define KEY_PRESS_WITH_CTRL 1
#define KEY_PRESS_WITH_SHIFT 2
// 检查接收值是否出错
#define RC_CH_VALUE_MIN ((uint16_t)364)
#define RC_CH_VALUE_OFFSET ((uint16_t)1024)
#define RC_CH_VALUE_MAX ((uint16_t)1684)
/* ----------------------- RC Switch Definition----------------------------- */
#define RC_SW_UP ((uint16_t)1) // 开关向上时的值
#define RC_SW_MID ((uint16_t)3) // 开关中间时的值
#define RC_SW_DOWN ((uint16_t)2) // 开关向下时的值
// 三个判断开关状态的宏
#define switch_is_down(s) (s == RC_SW_DOWN)
#define switch_is_mid(s) (s == RC_SW_MID)
#define switch_is_up(s) (s == RC_SW_UP)
/* ----------------------- PC Key Definition-------------------------------- */
// 对应key[x][0~16],获取对应的键;例如通过key[KEY_PRESS][Key_W]获取W键是否按下,后续改为位域后删除
#define Key_W 0
#define Key_S 1
#define Key_D 2
#define Key_A 3
#define Key_Shift 4
#define Key_Ctrl 5
#define Key_Q 6
#define Key_E 7
#define Key_R 8
#define Key_F 9
#define Key_G 10
#define Key_Z 11
#define Key_X 12
#define Key_C 13
#define Key_V 14
#define Key_B 15
/* ----------------------- Data Struct ------------------------------------- */
// 待测试的位域结构体,可以极大提升解析速度
typedef union
{
struct // 用于访问键盘状态
{
uint16_t w : 1;
uint16_t s : 1;
uint16_t d : 1;
uint16_t a : 1;
uint16_t shift : 1;
uint16_t ctrl : 1;
uint16_t q : 1;
uint16_t e : 1;
uint16_t r : 1;
uint16_t f : 1;
uint16_t g : 1;
uint16_t z : 1;
uint16_t x : 1;
uint16_t c : 1;
uint16_t v : 1;
uint16_t b : 1;
};
uint16_t keys; // 用于memcpy而不需要进行强制类型转换
} Key_t;
// @todo 当前结构体嵌套过深,需要进行优化
typedef struct
{
struct
{
int16_t rocker_l_; // 左水平
int16_t rocker_l1; // 左竖直
int16_t rocker_r_; // 右水平
int16_t rocker_r1; // 右竖直
int16_t dial; // 侧边拨轮
uint8_t switch_left; // 左侧开关
uint8_t switch_right; // 右侧开关
uint8_t swa;
uint8_t swb;
uint8_t swc;
uint8_t swd;
int16_t dial1;
int16_t dial2;
} rc;
struct
{
int16_t x;
int16_t y;
uint8_t press_l;
uint8_t press_r;
} mouse;
Key_t key[3]; // 改为位域后的键盘索引,空间减少8倍,速度增加16~倍
uint8_t key_count[3][16];
} RC_ctrl_t;
/* ------------------------- Internal Data ----------------------------------- */
/**
* @brief 初始化遥控器,该函数会将遥控器注册到串口
*
* @attention 注意分配正确的串口硬件,遥控器在C板上使用USART3
*
*/
RC_ctrl_t *RemoteControlInit(UART_HandleTypeDef *rc_usart_handle);
/**
* @brief 检查遥控器是否在线,若尚未初始化也视为离线
*
* @return uint8_t 1:在线 0:离线
*/
uint8_t RemoteControlIsOnline();
#endif
modules/standard_cmd/standard_cmd.md
+16
View File
@@ -0,0 +1,16 @@
# 标准命令
这是一个体力活，也是一个艺术品。请把不同的控制命令module进行封装，以转化成标准的消息类型，包括云台角度速度，底盘速度，发射频率是否发射等信息，供RobotCMD应用或其他应用使用。通过这种方式，开发者可以更专注于cmd命令的编写，而不需要为每台机器人/不同的控制器编写命令转换。
是否将下面的模块都放到standard_cmd文件夹下？似乎没有必要。
## remote control
## master machine
## ps handle
## key and mouse
## 图传链路
图传链路的数据解析似乎和键鼠是同一套协议，不过走的是串口，可能需要额外添加支持模块
modules/standard_cmd/std_cmd.c
View File
modules/standard_cmd/std_cmd.h
View File

Some files were not shown because too many files have changed in this diff Show More