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

增加了LK电机和HT电机的基本支持,待编写控制

Browse Source
This commit is contained in:
NeoZng
2022-12-13 19:40:03 +08:00
parent 2f41e67de0
commit 11329aaddb
21 changed files with 280 additions and 171 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
modules/imu/ins_task.c
View File

@@ -112,7 +112,7 @@ void INS_Task(void)
// 将重力从导航坐标系n转换到机体系b,随后根据加速度计数据计算运动加速度
float gravity_b[3];
EarthFrameToBodyFrame(gravity, gravity_b, INS.q);
for (uint8_t i = 0; i < 3; i++) // 同样过一个低通滤波
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);
}
@@ -218,7 +218,7 @@ static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[
param->flag = 0;
}
float gyro_temp[3];
for (uint8_t i = 0; i < 3; i++)
for (uint8_t i = 0; i < 3; ++i)
gyro_temp[i] = gyro[i] * param->scale[i];
gyro[X] = c_11 * gyro_temp[X] +
@@ -232,7 +232,7 @@ static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[
c_33 * gyro_temp[Z];
float accel_temp[3];
for (uint8_t i = 0; i < 3; i++)
for (uint8_t i = 0; i < 3; ++i)
accel_temp[i] = accel[i];
accel[X] = c_11 * accel_temp[X] +