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

增加五连杆解算和速度估计

Browse Source
This commit is contained in:
kai
2024-01-18 23:33:54 +08:00
parent 4b6a04067f
commit 7efa8b11bb
16 changed files with 186 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
modules/imu/ins_task.c
View File

@@ -111,6 +111,7 @@ attitude_t *INS_Init(void)
// noise of accel is relatively big and of high freq,thus lpf is used
INS.AccelLPF = 0.0085;
INS.DGyroLPF = 0.008;
DWT_GetDeltaT(&INS_DWT_Count);
return (attitude_t *)&INS.Gyro; // @todo: 这里偷懒了,不要这样做! 修改INT_t结构体可能会导致异常,待修复.
}
@@ -132,6 +133,9 @@ void INS_Task(void)
INS.Accel[X] = BMI088.Accel[X];
INS.Accel[Y] = BMI088.Accel[Y];
INS.Accel[Z] = BMI088.Accel[Z];
INS.dgyro[X] = (BMI088.Gyro[X] - INS.Gyro[X])/ (INS.DGyroLPF + dt) + INS.dgyro[X] * INS.DGyroLPF / (INS.DGyroLPF + dt);
INS.dgyro[Y] = (BMI088.Gyro[Y] - INS.Gyro[Y])/ (INS.DGyroLPF + dt) + INS.dgyro[Y] * INS.DGyroLPF / (INS.DGyroLPF + dt);
INS.dgyro[Z] = (BMI088.Gyro[Z] - INS.Gyro[Z])/ (INS.DGyroLPF + dt) + INS.dgyro[Z] * INS.DGyroLPF / (INS.DGyroLPF + dt);
INS.Gyro[X] = BMI088.Gyro[X];
INS.Gyro[Y] = BMI088.Gyro[Y];
INS.Gyro[Z] = BMI088.Gyro[Z];