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

10
modules/imu/BMI088driver.c
View File

@@ -144,7 +144,7 @@ void Calibrate_MPU_Offset(IMU_Data_t *bmi088)
bmi088->GyroOffset[1] = 0;
bmi088->GyroOffset[2] = 0;
for (uint16_t i = 0; i < CaliTimes; i++)
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];
@@ -176,7 +176,7 @@ void Calibrate_MPU_Offset(IMU_Data_t *bmi088)
{
gNormMax = gNormTemp;
gNormMin = gNormTemp;
for (uint8_t j = 0; j < 3; j++)
for (uint8_t j = 0; j < 3; ++j)
{
gyroMax[j] = bmi088->Gyro[j];
gyroMin[j] = bmi088->Gyro[j];
@@ -188,7 +188,7 @@ void Calibrate_MPU_Offset(IMU_Data_t *bmi088)
gNormMax = gNormTemp;
if (gNormTemp < gNormMin)
gNormMin = gNormTemp;
for (uint8_t j = 0; j < 3; j++)
for (uint8_t j = 0; j < 3; ++j)
{
if (bmi088->Gyro[j] > gyroMax[j])
gyroMax[j] = bmi088->Gyro[j];
@@ -198,7 +198,7 @@ void Calibrate_MPU_Offset(IMU_Data_t *bmi088)
}
gNormDiff = gNormMax - gNormMin;
for (uint8_t j = 0; j < 3; j++)
for (uint8_t j = 0; j < 3; ++j)
gyroDiff[j] = gyroMax[j] - gyroMin[j];
if (gNormDiff > 0.5f ||
gyroDiff[0] > 0.15f ||
@@ -209,7 +209,7 @@ void Calibrate_MPU_Offset(IMU_Data_t *bmi088)
}
bmi088->gNorm /= (float)CaliTimes;
for (uint8_t i = 0; i < 3; i++)
for (uint8_t i = 0; i < 3; ++i)
bmi088->GyroOffset[i] /= (float)CaliTimes;
BMI088_accel_read_muli_reg(BMI088_TEMP_M, buf, 2);

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] +