TuxMonkey/bf_original_balance_chassis
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-24 03:27:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into referee

Browse Source
This commit is contained in:
Kidenygood
2023-04-17 16:22:02 +08:00
parent 11655fbe46 1818edf117
commit 6f54bc201b
524 changed files with 20243 additions and 139813 deletions
Download Patch File Download Diff File Expand all files Collapse all files

BIN
modules/BMI088/bmi088-datasheet.pdf Normal file
View File

Binary file not shown.

168
modules/BMI088/bmi088.c
View File

@@ -151,7 +151,7 @@ static uint8_t BMI088AccelInit(BMI088Instance *bmi088)
*/
static uint8_t BMI088GyroInit(BMI088Instance *bmi088)
{
// 后续添加reset和通信检查
// 后续添加reset和通信检查?
// code to go here ...
BMI088GyroWriteSingleReg(bmi088, BMI088_GYRO_SOFTRESET, BMI088_GYRO_SOFTRESET_VALUE); // 软复位
DWT_Delay(0.08);
@@ -164,9 +164,8 @@ static uint8_t BMI088GyroInit(BMI088Instance *bmi088)
DWT_Delay(0.001);
// 初始化寄存器,提高可读性
uint8_t reg = 0;
uint8_t data = 0;
uint8_t error = 0;
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++)
{
@@ -195,25 +194,31 @@ static void BMI088AccSPIFinishCallback(SPIInstance *spi)
{
static BMI088Instance *bmi088;
bmi088 = (BMI088Instance *)(spi->id);
// code to go here ...
// 若第一次读取加速度,则在这里启动温度读取
// 如果使用异步姿态更新,此处唤醒量测更新的任务
}
static void BMI088GyroSPIFinishCallback(SPIInstance *spi)
{
static BMI088Instance *bmi088;
bmi088 = (BMI088Instance *)(spi->id);
// 若不是异步,啥也不做;否则启动姿态的预测步(propagation)
}
static void BMI088AccINTCallback(GPIOInstance *gpio)
{
static BMI088Instance *bmi088;
bmi088 = (BMI088Instance *)(gpio->id);
// 启动加速度计数据读取(和温度读取,如果有必要),并转换为实际值
// 读取完毕会调用BMI088AccSPIFinishCallback
}
static void BMI088GyroINTCallback(GPIOInstance *gpio)
{
static BMI088Instance *bmi088;
bmi088 = (BMI088Instance *)(gpio->id);
// 启动陀螺仪数据读取,并转换为实际值
// 读取完毕会调用BMI088GyroSPIFinishCallback
}
// -------------------------以上为私有函数,private用于IT模式下的中断处理---------------------------------//
@@ -227,64 +232,44 @@ static void BMI088GyroINTCallback(GPIOInstance *gpio)
* @param bmi088
* @return BMI088_Data_t
*/
BMI088_Data_t BMI088Acquire(BMI088Instance *bmi088)
uint8_t BMI088Acquire(BMI088Instance *bmi088, BMI088_Data_t *data_store)
{
// 分配空间保存返回的数据,指针传递
static BMI088_Data_t data_store;
static float dt_imu = 1.0; // 初始化为1,这样也可以不用first_read_flag,各有优劣
// 如果是blocking模式,则主动触发一次读取并返回数据
static uint8_t buf[6] = {0}; // 最多读取6个byte(gyro/acc,temp是2)
static uint8_t first_read_flag; // 判断是否时第一次进入此函数(第一次读取)
// 用于初始化DWT的计数,暂时没想到更好的方法
if (!first_read_flag)
DWT_GetDeltaT(&bmi088->bias_dwt_cnt); // 初始化delta
else
dt_imu = DWT_GetDeltaT(&bmi088->bias_dwt_cnt);
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;
// 读取accel的x轴数据首地址,bmi088内部自增读取地址 // 3* sizeof(int16_t)
BMI088AccelRead(bmi088, BMI088_ACCEL_XOUT_L, buf, 6);
static float calc_coef_acc; // 防止重复计算
if (!first_read_flag) // 初始化的时候赋值
calc_coef_acc = bmi088->BMI088_ACCEL_SEN * bmi088->acc_coef; // 你要是不爽可以用宏或者全局变量,但我认为你现在很爽
bmi088->acc[0] = calc_coef_acc * (float)(int16_t)(((buf[1]) << 8) | buf[0]);
bmi088->acc[1] = calc_coef_acc * (float)(int16_t)(((buf[3]) << 8) | buf[2]);
bmi088->acc[3] = calc_coef_acc * (float)(int16_t)(((buf[5]) << 8) | buf[4]);
BMI088GyroRead(bmi088, BMI088_GYRO_X_L, buf, 6); // 连续读取3个(3*2=6)轴的角速度
static float gyrosen, bias1, bias2, bias3;
if (!first_read_flag)
{ // 先保存,减少访问内存的开销,直接访问栈上变量
gyrosen = bmi088->BMI088_GYRO_SEN;
bias1 = bmi088->gyro_offset[0];
bias2 = bmi088->gyro_offset[1];
bias3 = bmi088->gyro_offset[2];
first_read_flag = 1; // 最后在这里,完成一次读取,标志第一次读取完成
} // 别担心,初始化调用的时候offset(即零飘bias)是0
bmi088->gyro[0] = (float)(int16_t)(((buf[1]) << 8) | buf[0]) * gyrosen - bias1 * dt_imu;
bmi088->gyro[0] = (float)(int16_t)(((buf[3]) << 8) | buf[2]) * gyrosen - bias2 * dt_imu;
bmi088->gyro[0] = (float)(int16_t)(((buf[5]) << 8) | buf[4]) * gyrosen - bias3 * dt_imu;
BMI088AccelRead(bmi088, BMI088_TEMP_M, buf, 2); // 读温度,温度传感器在accel上
bmi088->temperature = (float)(int16_t)(((buf[0] << 3) | (buf[1] >> 5))) * BMI088_TEMP_FACTOR + BMI088_TEMP_OFFSET;
return data_store;
return 1;
}
// 如果是IT模式,则检查标志位.当传感器数据准备好会触发外部中断,中断服务函数会将标志位置1
if (bmi088->work_mode == BMI088_BLOCK_TRIGGER_MODE && bmi088->update_flag.imu_ready == 1)
return data_store;
{
memcpy(data_store, &bmi088->gyro, sizeof(BMI088_Data_t));
bmi088->update_flag.imu_ready = 0;
return 1;
}
// 如果数据还没准备好,则返回空数据?或者返回上一次的数据?或者返回错误码? @todo
if (bmi088->update_flag.imu_ready == 0)
return data_store;
return 0;
}
/* pre calibrate parameter to go here */
#warning 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
// macro to go here... 预设标定参数 gNorm
#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中,用于后续数据消噪和单位转换归一化
@@ -300,80 +285,58 @@ 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)
int16_t bmi088_raw_temp; // 临时变量,暂存数据移位拼接后的值
uint8_t buf[6] = {0}; // buffer
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.5)
if (DWT_GetTimeline_s() - startTime > 12.01)
{ // 两次都没有成功就切换标定模式,丢给下一个if处理,使用预标定参数
_bmi088->cali_mode = BMI088_LOAD_PRE_CALI_MODE;
break;
}
DWT_Delay(0.005);
DWT_Delay(0.0005);
_bmi088->gNorm = 0;
_bmi088->gyro_offset[0] = 0;
_bmi088->gyro_offset[1] = 0;
_bmi088->gyro_offset[2] = 0;
for (uint8_t i = 0; i < 3; i++) // 重置gNorm和零飘
_bmi088->gyro_offset[i] = 0;
// @todo : 这里也有获取bmi088数据的操作,后续与BMI088Acquire合并.注意标定时的工作模式是阻塞,且offset和acc_coef要初始化成0和1,标定完成后再设定为标定值
// @todo : 这里也有获取bmi088数据的操作,后续与BMI088Acquire合并.注意标定时的工作模式是阻塞,且offset和acc_coef要初始化成0和1,标定完成后再设定为标定值
for (uint16_t i = 0; i < CaliTimes; ++i) // 提前计算,优化
{
BMI088AccelRead(_bmi088, BMI088_ACCEL_XOUT_L, buf, 6); // 读取
bmi088_raw_temp = (int16_t)((buf[1]) << 8) | buf[0]; // 拼接
_bmi088->acc[0] = bmi088_raw_temp * _bmi088->BMI088_ACCEL_SEN; // 计算真实值
bmi088_raw_temp = (int16_t)((buf[3]) << 8) | buf[2];
_bmi088->acc[1] = bmi088_raw_temp * _bmi088->BMI088_ACCEL_SEN;
bmi088_raw_temp = (int16_t)((buf[5]) << 8) | buf[4];
_bmi088->acc[2] = bmi088_raw_temp * _bmi088->BMI088_ACCEL_SEN;
gNormTemp = sqrtf(_bmi088->acc[0] * _bmi088->acc[0] +
_bmi088->acc[1] * _bmi088->acc[1] +
_bmi088->acc[2] * _bmi088->acc[2]);
BMI088Acquire(_bmi088, &raw_data);
gNormTemp = NormOf3d(raw_data.acc);
_bmi088->gNorm += gNormTemp; // 计算范数并累加,最后除以calib times获取单次值
BMI088GyroRead(_bmi088, BMI088_GYRO_CHIP_ID, buf, 8); // 可保存提前计算,优化
bmi088_raw_temp = (int16_t)((buf[1]) << 8) | buf[0];
_bmi088->gyro[0] = bmi088_raw_temp * _bmi088->BMI088_ACCEL_SEN;
_bmi088->gyro_offset[0] += _bmi088->gyro[0];
bmi088_raw_temp = (int16_t)((buf[3]) << 8) | buf[2];
_bmi088->gyro[1] = bmi088_raw_temp * _bmi088->BMI088_ACCEL_SEN;
_bmi088->gyro_offset[1] += _bmi088->gyro[1];
bmi088_raw_temp = (int16_t)((buf[5]) << 8) | buf[4];
_bmi088->gyro[2] = bmi088_raw_temp * _bmi088->BMI088_ACCEL_SEN;
_bmi088->gyro_offset[2] += _bmi088->gyro[2]; // 累加当前值,最后除以calib times获得零飘
// 因为标定时传感器静止,所以采集到的值就是漂移
for (uint8_t i = 0; i < 3; i++)
_bmi088->gyro_offset[i] += raw_data.gyro[i]; // 因为标定时传感器静止,所以采集到的值就是漂移,累加当前值,最后除以calib times获得零飘
if (i == 0) // 避免未定义的行为(else中)
{
gNormMax = gNormTemp; // 初始化成当前的重力加速度模长
gNormMin = gNormTemp;
gNormMax = gNormMin = gNormTemp; // 初始化成当前的重力加速度模长
for (uint8_t j = 0; j < 3; ++j)
{
gyroMax[j] = _bmi088->gyro[j];
gyroMin[j] = _bmi088->gyro[j];
gyroMax[j] = raw_data.gyro[j];
gyroMin[j] = raw_data.gyro[j];
}
}
else // 更新gNorm的Min Max和gyro的minmax
{
if (gNormTemp > gNormMax)
gNormMax = gNormTemp;
if (gNormTemp < gNormMin)
gNormMin = gNormTemp;
gNormMax = gNormMax > gNormTemp ? gNormMax : gNormTemp;
gNormMin = gNormMin < gNormTemp ? 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];
gyroMax[j] = gyroMax[j] > _bmi088->gyro[j] ? gyroMax[j] : _bmi088->gyro[j];
gyroMin[j] = gyroMin[j] < _bmi088->gyro[j] ? gyroMin[j] : _bmi088->gyro[j];
}
}
@@ -387,15 +350,11 @@ void BMI088CalibrateIMU(BMI088Instance *_bmi088)
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; // 三轴零飘
BMI088AccelRead(_bmi088, BMI088_TEMP_M, buf, 2);
bmi088_raw_temp = (int16_t)((buf[0] << 3) | (buf[1] >> 5)); // 保存标定时的温度,如果已知温度和零飘的关系
// 这里直接存到temperature,可以另外增加BMI088Instance的成员变量TempWhenCalib
_bmi088->temperature = bmi088_raw_temp * BMI088_TEMP_FACTOR + BMI088_TEMP_OFFSET;
_bmi088->temperature = raw_data.temperature * BMI088_TEMP_FACTOR + BMI088_TEMP_OFFSET; // 保存标定时的温度,如果已知温度和零飘的关系
// caliTryOutCount++; 保存已经尝试的标定次数?由你.
} while (gNormDiff > 0.5f ||
fabsf(_bmi088->gNorm - 9.8f) > 0.5f ||
@@ -410,12 +369,12 @@ void BMI088CalibrateIMU(BMI088Instance *_bmi088)
// 离线标定
if (_bmi088->cali_mode == BMI088_LOAD_PRE_CALI_MODE) // 如果标定失败也会进来,直接使用离线数据
{
// 读取标定数据
// code to go here ...
_bmi088->gyro_offset[0] = BMI088_PRE_CALI_ACC_X_OFFSET;
// ...
// acc_coef,gNorm ...
_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参数判断
@@ -428,14 +387,12 @@ BMI088Instance *BMI088Register(BMI088_Init_Config_s *config)
{
// 申请内存
BMI088Instance *bmi088_instance = (BMI088Instance *)zero_malloc(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)
@@ -485,14 +442,9 @@ BMI088Instance *BMI088Register(BMI088_Init_Config_s *config)
// 可以增加try out times,超出次数则返回错误
} while (error != 0);
// 尚未标定时先设置为默认值,使得数据拼接和缩放可以正常进行,后续合并到BMI088Acquire()??
bmi088_instance->acc_coef = 1.0; // 尚未初始化时设定为1,使得BMI088Acquire可以正常使用
bmi088_instance->BMI088_GYRO_SEN = BMI088_GYRO_2000_SEN; // 后续改为从initTable中获取
bmi088_instance->BMI088_ACCEL_SEN = BMI088_ACCEL_6G_SEN; // 或使用宏字符串拼接
// bmi088->gNorm =
// 标定acc和gyro
BMI088CalibrateIMU(bmi088_instance);
bmi088_instance->work_mode = BMI088_BLOCK_PERIODIC_MODE; // 临时设置为阻塞模式
BMI088CalibrateIMU(bmi088_instance); // 标定acc和gyro
bmi088_instance->work_mode = config->work_mode; // 恢复工作模式
return bmi088_instance;
}

2
modules/BMI088/bmi088.h
View File

@@ -104,7 +104,7 @@ BMI088Instance *BMI088Register(BMI088_Init_Config_s *config);
* @param bmi088 BMI088实例指针
* @return BMI088_Data_t 读取到的数据
*/
BMI088_Data_t BMI088Acquire(BMI088Instance *bmi088);
uint8_t BMI088Acquire(BMI088Instance *bmi088,BMI088_Data_t* data_store);
/**
* @brief 标定传感器.BMI088在初始化的时候会调用此函数. 提供接口方便标定离线数据

14
modules/algorithm/QuaternionEKF.c
View File

@@ -47,7 +47,7 @@ static void IMU_QuaternionEKF_xhatUpdate(KalmanFilter_t *kf);
* @param[in] lambda fading coefficient 0.9996
* @param[in] lpf lowpass filter coefficient 0
*/
void IMU_QuaternionEKF_Init(float process_noise1, float process_noise2, float measure_noise, float lambda, float lpf)
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;
@@ -69,10 +69,10 @@ void IMU_QuaternionEKF_Init(float process_noise1, float process_noise2, float me
Matrix_Init(&QEKF_INS.ChiSquare, 1, 1, (float *)QEKF_INS.ChiSquare_Data);
// 姿态初始化
QEKF_INS.IMU_QuaternionEKF.xhat_data[0] = 1;
QEKF_INS.IMU_QuaternionEKF.xhat_data[1] = 0;
QEKF_INS.IMU_QuaternionEKF.xhat_data[2] = 0;
QEKF_INS.IMU_QuaternionEKF.xhat_data[3] = 0;
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;
@@ -99,10 +99,6 @@ void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay,
// 0.5(Ohm-Ohm^bias)*deltaT,用于更新工作点处的状态转移F矩阵
static float halfgxdt, halfgydt, halfgzdt;
static float accelInvNorm;
if (!QEKF_INS.Initialized)
{
IMU_QuaternionEKF_Init(10, 0.001, 1000000 * 10, 0.9996 * 0 + 1, 0);
}
/* F, number with * represent vals to be set
0 1* 2* 3* 4 5

2
modules/algorithm/QuaternionEKF.h
View File

@@ -69,7 +69,7 @@ typedef struct
extern QEKF_INS_t QEKF_INS;
extern float chiSquare;
extern float ChiSquareTestThreshold;
void IMU_QuaternionEKF_Init(float process_noise1, float process_noise2, float measure_noise, float lambda, float lpf);
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

30
modules/algorithm/user_lib.c
View File

@@ -162,4 +162,34 @@ int float_rounding(float raw)
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];
}

10
modules/algorithm/user_lib.h
View File

@@ -88,7 +88,7 @@ extern uint8_t GlobalDebugMode;
#define VAL_MAX(a, b) ((a) > (b) ? (a) : (b))
/**
* @brief 返回一块干净的内存,不过仍然需要强制转换为你需要的类型
* @brief 返回一块干净的内<EFBFBD>?,不过仍然需要强制转<EFBFBD>?为你需要的类型
*
* @param size 分配大小
* @return void*
@@ -114,6 +114,14 @@ 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);
//<2F><><EFBFBD>ȸ<EFBFBD>ʽ<EFBFBD><CABD>Ϊ-PI~PI
#define rad_format(Ang) loop_float_constrain((Ang), -PI, PI)

30
modules/can_comm/can_comm.c
View File

@@ -2,6 +2,7 @@
#include "memory.h"
#include "stdlib.h"
#include "crc8.h"
#include "bsp_dwt.h"
/**
* @brief 重置CAN comm的接收状态和buffer
@@ -25,7 +26,7 @@ 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也等于我们设定接收长度(这是因为暂时不支持动态包长)
@@ -51,25 +52,18 @@ static void CANCommRxCallback(CANInstance *_instance)
// 收完这一包以后刚好等于总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)
{
CANCommResetRx(comm);
return; // 重置状态然后返回
}
else // tail正确, 对数据进行crc8校验
{
if (comm->raw_recvbuf[comm->recv_buf_len - 2] ==
crc_8(comm->raw_recvbuf + 2, comm->recv_data_len))
{ // 通过校验,复制数据到unpack_data中
memcpy(comm->unpacked_recv_data, comm->raw_recvbuf + 2, comm->recv_data_len); // 数据量大的话考虑使用DMA
{
// 如果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
}
CANCommResetRx(comm);
return; // 重置状态然后返回
}
return; // 访问完一个can comm直接退出,一次中断只处理一个实例的回调
CANCommResetRx(comm);
return; // 重置状态然后返回
}
}
}
@@ -108,7 +102,7 @@ void CANCommSend(CANCommInstance *instance, uint8_t *data)
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);
CANTransmit(instance->can_ins, 1);
}
}

5
modules/can_comm/can_comm.md
View File

@@ -7,6 +7,9 @@
> 1. 对`CANCommGet()`进行修改，使得其可以返回数据是否更新的相关信息。
## 重要提醒
如果传输过程中出现多次丢包或长度校验不通过，尤其是传输长度较大的时候，请开启CAN的Auto Retransmission，并尝试修改CANComm实例的发送和接受ID（以提高在总线仲裁中的优先级）。
## 总览和封装说明
@@ -135,3 +138,5 @@ CAN comm的通信协议如下：
接收的流程见代码注释。
流程图如下：![未命名文件](../../assets/CANcomm.png)

10
modules/general_def.h
View File

@@ -3,15 +3,15 @@
// 一些module的通用数值型定义,注意条件macro兼容,一些宏可能在math.h中已经定义过了
#ifndef PI
#define PI 3.1415926535f
#endif // !PI
#endif
#define PI2 (PI * 2.0f) // 2 pi
#define RAD_2_ANGLE 57.2957795f // 180/pi
#define ANGLE_2_RAD 0.01745329252f // pi/180
#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_ANGLE_PER_SEC 6.0f // ×360°/60sec
#define RPM_2_RAD_PER_SEC 0.104719755f // ×2pi/60sec
#endif // !GENERAL_DEF_H

6
modules/imu/BMI088driver.c
View File

@@ -347,11 +347,11 @@ void BMI088_Read(IMU_Data_t *bmi088)
if (caliOffset)
{
bmi088_raw_temp = (int16_t)((buf[3]) << 8) | buf[2];
bmi088->Gyro[0] = bmi088_raw_temp * BMI088_GYRO_SEN - bmi088->GyroOffset[0] * dt;
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] * dt;
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] * dt;
bmi088->Gyro[2] = bmi088_raw_temp * BMI088_GYRO_SEN - bmi088->GyroOffset[2];
}
else
{

44
modules/imu/ins_task.c
View File

@@ -43,6 +43,33 @@ static void IMU_Temperature_Ctrl(void)
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)
{
while (BMI088Init(&hspi1, 1) != BMI088_NO_ERROR)
@@ -55,19 +82,22 @@ attitude_t *INS_Init(void)
IMU_Param.Roll = 0;
IMU_Param.flag = 1;
IMU_QuaternionEKF_Init(10, 0.001, 10000000, 1, 0);
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 = 2000,
.IntegralLimit = 300,
.DeadBand = 0,
.Kp = 1000,
.Ki = 20,
.Kd = 0,
.Improve = 0x01}; // enable integratiaon limit
.IntegralLimit = 300,
.DeadBand = 0,
.Kp = 1000,
.Ki = 20,
.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_GetDeltaT64(&INS_DWT_Count);
return (attitude_t *)&INS.Gyro; // @todo: 这里偷懒了,不要这样做! 修改INT_t结构体可能会导致异常,待修复.
}

2
modules/master_machine/master_process.c
View File

@@ -82,7 +82,7 @@ void VisionSend(Vision_Send_s *send)
uint8_t send_buff[VISION_SEND_SIZE];
uint16_t tx_len;
// TODO: code to set flag_register
flag_register = 30<<8|0b00000001;
flag_register = 30<<8|0b00000001;
// 将数据转化为seasky协议的数据包
get_protocol_send_data(0x02, flag_register, &send->yaw, 3, send_buff, &tx_len);
USARTSend(vision_usart_instance, send_buff, tx_len, USART_TRANSFER_DMA); // 和视觉通信使用IT,防止和接收使用的DMA冲突

2
modules/master_machine/湖南大学RoboMaster电控组通信协议.md
View File

@@ -13,7 +13,7 @@
## 一、串口配置
通信方式是串口，配置为波特率 115200，8 位数据位，1 位停止位，无硬件流控，无校验位。
通信方式是串口，配置为波特率 921600，8 位数据位，1 位停止位，无硬件流控，无校验位。
## 二、数据帧说明

85
modules/message_center/message_center.c
View File

@@ -1,6 +1,7 @@
#include "message_center.h"
#include "stdlib.h"
#include "string.h"
#include "bsp_log.h"
/* message_center是fake head node,是方便链表编写的技巧,这样就不需要处理链表头的特殊情况 */
static Publisher_t message_center = {
@@ -12,6 +13,7 @@ static void CheckName(char *name)
{
if (strnlen(name, MAX_EVENT_NAME_LEN + 1) >= MAX_EVENT_NAME_LEN)
{
LOGERROR("EVENT NAME TOO LONG:%s", name);
while (1)
; // 进入这里说明事件名超出长度限制
}
@@ -21,65 +23,12 @@ static void CheckLen(uint8_t len1, uint8_t len2)
{
if (len1 != len2)
{
LOGERROR("EVENT LEN NOT SAME:%d,%d", len1, len2);
while (1)
; // 进入这里说明相同事件的消息长度却不同
}
}
Subscriber_t *SubRegister(char *name, uint8_t data_len)
{
CheckName(name);
Publisher_t *node = &message_center; // 可以将message_center看作对消息管理器的抽象,它用于管理所有pub和sub
while (node->next_event_node) // 遍历链表,如果当前有发布者已经注册
{
node = node->next_event_node; // 指向下一个发布者(发布者发布的事件)
if (strcmp(name, node->event_name) == 0) // 如果事件名相同就订阅这个事件
{
CheckLen(data_len, node->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(sizeof(data_len));
}
// 如果是第一个订阅者,特殊处理一下
if (node->first_subs == NULL)
{
node->first_subs = ret;
return ret;
}
// 遍历订阅者链表,直到到达尾部
Subscriber_t *sub = node->first_subs; // 作为iterator
while (sub->next_subs_queue) // 遍历订阅了该事件的订阅者链表
{
sub = sub->next_subs_queue; // 移动到下一个订阅者,遇到空指针停下,说明到了链表尾部
}
sub->next_subs_queue = ret; // 把刚刚创建的订阅者接上
return ret;
}
// 事件名不同,在下一轮循环访问下一个结点
}
// 遍历完,发现尚未注册事件(还没有发布者);那么创建一个事件,此时node是publisher链表的最后一个结点
node->next_event_node = (Publisher_t *)malloc(sizeof(Publisher_t));
memset(node->next_event_node, 0, sizeof(Publisher_t));
strcpy(node->next_event_node->event_name, name);
node->next_event_node->data_len = data_len;
// 同之前,创建subscriber作为新事件的第一个订阅者
Subscriber_t *ret = (Subscriber_t *)malloc(sizeof(Subscriber_t));
memset(ret, 0, sizeof(Subscriber_t));
ret->data_len = data_len;
for (size_t i = 0; i < QUEUE_SIZE; ++i)
{ // 给消息队列分配空间
ret->queue[i] = malloc(sizeof(data_len));
}
// 新建的订阅者是该发布者的第一个订阅者,发布者会通过这个指针顺序访问所有订阅者
node->next_event_node->first_subs = ret;
return ret;
}
Publisher_t *PubRegister(char *name, uint8_t data_len)
{
CheckName(name);
@@ -103,6 +52,34 @@ Publisher_t *PubRegister(char *name, uint8_t data_len)
return node->next_event_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(sizeof(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)
{

16
modules/motor/DJImotor/dji_motor.c
View File

@@ -107,6 +107,7 @@ static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *conf
break;
default: // other motors should not be registered here
LOGERROR("You must not register other motors using the API of DJI motor.");
while (1)
; // 其他电机不应该在这里注册
}
@@ -123,7 +124,7 @@ static void DecodeDJIMotor(CANInstance *_instance)
// 这里对can instance的id进行了强制转换,从而获得电机的instance实例地址
// _instance指针指向的id是对应电机instance的地址,通过强制转换为电机instance的指针,再通过->运算符访问电机的成员motor_measure,最后取地址获得指针
uint8_t *rxbuff = _instance->rx_buff;
DJI_Motor_Measure_s *measure = &(((DJIMotorInstance *)_instance->id)->motor_measure); // measure要多次使用,保存指针减小访存开销
DJI_Motor_Measure_s *measure = &(((DJIMotorInstance *)_instance->id)->measure); // measure要多次使用,保存指针减小访存开销
// 解析数据并对电流和速度进行滤波,电机的反馈报文具体格式见电机说明手册
measure->last_ecd = measure->ecd;
@@ -222,7 +223,7 @@ void DJIMotorControl()
DJIMotorInstance *motor;
Motor_Control_Setting_s *motor_setting; // 电机控制参数
Motor_Controller_s *motor_controller; // 电机控制器
DJI_Motor_Measure_s *motor_measure; // 电机测量值
DJI_Motor_Measure_s *measure; // 电机测量值
float pid_measure, pid_ref; // 电机PID测量值和设定值
// 遍历所有电机实例,进行串级PID的计算并设置发送报文的值
@@ -231,10 +232,10 @@ void DJIMotorControl()
motor = dji_motor_instance[i];
motor_setting = &motor->motor_settings;
motor_controller = &motor->motor_controller;
motor_measure = &motor->motor_measure;
measure = &motor->measure;
pid_ref = motor_controller->pid_ref; // 保存设定值,防止motor_controller->pid_ref在计算过程中被修改
if (motor_setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
pid_ref*= -1; // 设置反转
pid_ref *= -1; // 设置反转
// pid_ref会顺次通过被启用的闭环充当数据的载体
// 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出
if ((motor_setting->close_loop_type & ANGLE_LOOP) && motor_setting->outer_loop_type == ANGLE_LOOP)
@@ -242,7 +243,7 @@ void DJIMotorControl()
if (motor_setting->angle_feedback_source == OTHER_FEED)
pid_measure = *motor_controller->other_angle_feedback_ptr;
else
pid_measure = motor_measure->total_angle; // MOTOR_FEED,对total angle闭环,防止在边界处出现突跃
pid_measure = measure->total_angle; // MOTOR_FEED,对total angle闭环,防止在边界处出现突跃
// 更新pid_ref进入下一个环
pid_ref = PIDCalculate(&motor_controller->angle_PID, pid_measure, pid_ref);
}
@@ -253,7 +254,7 @@ void DJIMotorControl()
if (motor_setting->speed_feedback_source == OTHER_FEED)
pid_measure = *motor_controller->other_speed_feedback_ptr;
else // MOTOR_FEED
pid_measure = motor_measure->speed_aps;
pid_measure = measure->speed_aps;
// 更新pid_ref进入下一个环
pid_ref = PIDCalculate(&motor_controller->speed_PID, pid_measure, pid_ref);
}
@@ -261,12 +262,11 @@ void DJIMotorControl()
// 计算电流环,目前只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身传感器的反馈
if (motor_setting->close_loop_type & CURRENT_LOOP)
{
pid_ref = PIDCalculate(&motor_controller->current_PID, motor_measure->real_current, pid_ref);
pid_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref);
}
// 获取最终输出
set = (int16_t)pid_ref;
// 分组填入发送数据
group = motor->sender_group;

2
modules/motor/DJImotor/dji_motor.h
View File

@@ -49,7 +49,7 @@ typedef struct
typedef struct
{
DJI_Motor_Measure_s motor_measure; // 电机测量值
DJI_Motor_Measure_s measure; // 电机测量值
Motor_Control_Setting_s motor_settings; // 电机设置
Motor_Controller_s motor_controller; // 电机控制器

16
modules/motor/HTmotor/HT04.c
View File

@@ -42,16 +42,16 @@ 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)->motor_measure; // 将can实例中保存的id转换成电机实例的指针
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 = RAD_2_ANGLE * uint_to_float(tmp, P_MIN, P_MAX, 16);
measure->total_angle = uint_to_float(tmp, P_MIN, P_MAX, 16);
tmp = (uint16_t)((rxbuff[3] << 4) | (rxbuff[4] >> 4));
measure->speed_aps = SPEED_SMOOTH_COEF * uint_to_float(tmp, V_MIN, V_MAX, 12) +
(1 - SPEED_SMOOTH_COEF) * measure->speed_aps;
measure->speed_rads = SPEED_SMOOTH_COEF * uint_to_float(tmp, V_MIN, V_MAX, 12) +
(1 - SPEED_SMOOTH_COEF) * measure->speed_rads;
tmp = (uint16_t)(((rxbuff[4] & 0x0f) << 8) | rxbuff[5]);
measure->real_current = CURRENT_SMOOTH_COEF * uint_to_float(tmp, T_MIN, T_MAX, 12) +
@@ -97,7 +97,7 @@ void HTMotorControl()
for (size_t i = 0; i < idx; i++)
{ // 先获取地址避免反复寻址
motor = ht_motor_instance[i];
measure = &motor->motor_measure;
measure = &motor->measure;
setting = &motor->motor_settings;
motor_can = motor->motor_can_instace;
pid_ref = motor->pid_ref;
@@ -109,7 +109,7 @@ void HTMotorControl()
else
pid_measure = measure->real_current;
// measure单位是rad,ref是角度,统一到angle下计算,方便建模
pid_ref = PIDCalculate(&motor->angle_PID, pid_measure * RAD_2_ANGLE, pid_ref);
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))
@@ -120,9 +120,9 @@ void HTMotorControl()
if (setting->angle_feedback_source == OTHER_FEED)
pid_measure = *motor->other_speed_feedback_ptr;
else
pid_measure = measure->speed_aps;
pid_measure = measure->speed_rads;
// measure单位是rad / s ,ref是angle per sec,统一到angle下计算
pid_ref = PIDCalculate(&motor->speed_PID, pid_measure * RAD_2_ANGLE, pid_ref);
pid_ref = PIDCalculate(&motor->speed_PID, pid_measure * RAD_2_DEGREE, pid_ref);
}
if (setting->close_loop_type & CURRENT_LOOP)

32
modules/motor/HTmotor/HT04.h
View File

@@ -18,17 +18,17 @@
#define T_MAX 18.0f
typedef struct // HT04
{
float last_angle;
{
float last_angle;
float total_angle; // 角度为多圈角度,范围是-95.5~95.5,单位为rad
float speed_aps;
float speed_rads;
float real_current;
} HTMotor_Measure_t;
/* HT电机类型定义*/
typedef struct
{
HTMotor_Measure_t motor_measure;
HTMotor_Measure_t measure;
Motor_Control_Setting_s motor_settings;
@@ -42,7 +42,7 @@ typedef struct
float pid_ref;
Motor_Working_Type_e stop_flag; // 启停标志
CANInstance *motor_can_instace;
} HTMotorInstance;
@@ -55,16 +55,16 @@ typedef enum
} HTMotor_Mode_t;
/**
* @brief
*
* @param config
* @return HTMotorInstance*
* @brief
*
* @param config
* @return HTMotorInstance*
*/
HTMotorInstance *HTMotorInit(Motor_Init_Config_s *config);
/**
* @brief 设定电机的参考值
*
*
* @param motor 要设定的电机
* @param current 设定值
*/
@@ -72,20 +72,20 @@ void HTMotorSetRef(HTMotorInstance *motor, float ref);
/**
* @brief 给所有的HT电机发送控制指令
*
*
*/
void HTMotorControl();
/**
* @brief 停止电机,之后电机不会响应HTMotorSetRef设定的值
*
* @param motor
*
* @param motor
*/
void HTMotorStop(HTMotorInstance *motor);
/**
* @brief 启动电机
*
*
* @param motor 要启动的电机
*/
void HTMotorEnable(HTMotorInstance *motor);
@@ -96,8 +96,8 @@ void HTMotorEnable(HTMotorInstance *motor);
* 注意,校准时务必将电机和其他机构分离,电机会旋转360°!
* 注意,校准时务必将电机和其他机构分离,电机会旋转360°!
* 注意,校准时务必将电机和其他机构分离,电机会旋转360°!
*
* @param motor
*
* @param motor
*/
void HTMotorCalibEncoder(HTMotorInstance *motor);

7
modules/motor/LKmotor/LK9025.c
View File

@@ -1,5 +1,6 @@
#include "LK9025.h"
#include "stdlib.h"
#include "general_def.h"
static uint8_t idx;
static LKMotorInstance *lkmotor_instance[LK_MOTOR_MX_CNT] = {NULL};
@@ -23,8 +24,8 @@ static void LKMotorDecode(CANInstance *_instance)
measure->angle_single_round = ECD_ANGLE_COEF_LK * measure->ecd;
measure->speed_aps = (1 - SPEED_SMOOTH_COEF) * measure->speed_aps +
SPEED_SMOOTH_COEF * (float)((int16_t)(rx_buff[5] << 8 | rx_buff[4]));
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]));
@@ -101,7 +102,7 @@ void LKMotorControl()
if (setting->angle_feedback_source == OTHER_FEED)
pid_measure = *motor->other_speed_feedback_ptr;
else
pid_measure = measure->speed_aps;
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;

3
modules/motor/LKmotor/LK9025.h
View File

@@ -14,13 +14,14 @@
#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_aps; // speed angle per sec(degree:°)
float speed_rads; // speed rad/s
int16_t real_current; // 实际电流
uint8_t temperate; // 温度,C°

2
modules/referee/referee_communication.c
View File

@@ -9,7 +9,7 @@
*/
#include "referee_communication.h"
#include "crc.h"
#include "crc_ref.h"
#include "stdio.h"
#include "rm_referee.h"

2
modules/remote/remote_control.c
View File

@@ -32,7 +32,7 @@ static void RectifyRCjoystick()
* @param[out] rc_ctrl: remote control data struct point
* @retval none
*/
uint16_t aaaaa;
static void sbus_to_rc(const uint8_t *sbus_buf)
{