改轮腿

User/module/config.c

@@ -253,6 +253,7 @@ Config_RobotParam_t robot_config = {
                 .hip_length = 0.0f    // 髋宽 (L5) (m)
             }
         },
+        
 .lqr_gains = {
     .k11_coeff = { -1.498109852818409e+02f, 1.918923604951453e+02f, -1.080984818173493e+02f, -1.540703437137126e+00f }, // theta
     .k12_coeff = { 8.413682810667103e+00f, -6.603584762798329e+00f, -6.421063158996702e+00f, -5.450666844349098e-02f },  // d_theta

User/task/atti_esti.c

@@ -6,11 +6,14 @@
 /* Includes ----------------------------------------------------------------- */
 #include "task/user_task.h"
 /* USER INCLUDE BEGIN */
+#include "bsp/mm.h"
 #include "bsp/pwm.h"
+#include "bsp/gpio.h"
 #include "component/ahrs.h"
 #include "component/pid.h"
-#include "device/bmi088.h"
 #include "module/gimbal.h"
+#include "device/bmi088.h"
+#include "task/user_task.h"
 /* USER INCLUDE END */
 
 /* Private typedef ---------------------------------------------------------- */
@@ -29,9 +32,42 @@ KPID_t imu_temp_ctrl_pid;
 Gimbal_IMU_t gimbal_to_send;
 
 BMI088_Cali_t cali_bmi088= {
-  .gyro_offset = {0.0f,0.0f,0.0f},
+  .gyro_offset = {-0.00149519544f,-0.00268901442f,0.00169837975f},
 };
 
+static const KPID_Params_t imu_temp_ctrl_pid_param = {
+    .k = 0.3f,
+    .p = 1.0f,
+    .i = 0.01f,
+    .d = 0.0f,
+    .i_limit = 1.0f,
+    .out_limit = 1.0f,
+};
+
+/* 校准相关变量 */
+typedef enum {
+  CALIB_IDLE,     // 空闲状态
+  CALIB_RUNNING,  // 正在校准
+  CALIB_DONE      // 校准完成
+} CalibState_t;
+
+static CalibState_t calib_state = CALIB_IDLE;
+static uint16_t calib_count = 0;
+static struct {
+  float x_sum;
+  float y_sum; 
+  float z_sum;
+} gyro_sum= {0.0f,0.0f,0.0f};
+static void start_gyro_calibration(void) {
+  if (calib_state == CALIB_IDLE) {
+    calib_state = CALIB_RUNNING;
+    calib_count = 0;
+    gyro_sum.x_sum = 0.0f;
+    gyro_sum.y_sum = 0.0f;
+    gyro_sum.z_sum = 0.0f;
+  }
+}
+#define CALIB_SAMPLES 5000  // 校准采样数量
 
 /* USER STRUCT END */
 
@@ -50,7 +86,13 @@ void Task_atti_esti(void *argument) {
   /* USER CODE INIT BEGIN */
   BMI088_Init(&bmi088,&cali_bmi088);
   AHRS_Init(&gimbal_ahrs, &magn, BMI088_GetUpdateFreq(&bmi088));
-
+  PID_Init(&imu_temp_ctrl_pid, KPID_MODE_NO_D,
+           1.0f / BMI088_GetUpdateFreq(&bmi088), &imu_temp_ctrl_pid_param);
+  BSP_PWM_Start(BSP_PWM_IMU_HEAT_PWM);
+  
+  /* 注册按钮回调函数并启用中断 */
+  BSP_GPIO_RegisterCallback(BSP_GPIO_USER_KEY, start_gyro_calibration);
+  BSP_GPIO_EnableIRQ(BSP_GPIO_USER_KEY);
   /* USER CODE INIT END */
   
   while (1) {
@@ -70,21 +112,54 @@ void Task_atti_esti(void *argument) {
     BMI088_ParseGyro(&bmi088);
     // IST8310_Parse(&ist8310);
 
-    /* 根据设备接收到的数据进行姿态解析 */
+    /* 陀螺仪校准处理 */
-    AHRS_Update(&gimbal_ahrs, &bmi088.accl, &bmi088.gyro, &magn);
+    if (calib_state == CALIB_RUNNING) {
+      /* 累加陀螺仪数据用于计算零偏 */
+      gyro_sum.x_sum += bmi088.gyro.x;
+      gyro_sum.y_sum += bmi088.gyro.y;
+      gyro_sum.z_sum += bmi088.gyro.z;
+      calib_count++;
 
-    /* 根据解析出来的四元数计算欧拉角 */
+      /* 达到采样数量后计算平均值并更新零偏 */
-    AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs);
+      if (calib_count >= CALIB_SAMPLES) {
+        /* 计算平均值作为零偏 */
+        cali_bmi088.gyro_offset.x = gyro_sum.x_sum / CALIB_SAMPLES;
+        cali_bmi088.gyro_offset.y = gyro_sum.y_sum / CALIB_SAMPLES;
+        cali_bmi088.gyro_offset.z = gyro_sum.z_sum / CALIB_SAMPLES;
+        
+        /* 校准完成，重置为空闲状态以便下次校准 */
+        calib_state = CALIB_IDLE;
+        
+        /* 重新初始化BMI088以应用新的校准数据 */
+        BMI088_Init(&bmi088, &cali_bmi088);
+        AHRS_Init(&gimbal_ahrs, &magn, BMI088_GetUpdateFreq(&bmi088));
+      }
+    }
+
+    /* 只有在非校准状态下才进行正常的姿态解析 */
+    if (calib_state != CALIB_RUNNING) {
+      /* 根据设备接收到的数据进行姿态解析 */
+      AHRS_Update(&gimbal_ahrs, &bmi088.accl, &bmi088.gyro, &magn);
+
+      /* 根据解析出来的四元数计算欧拉角 */
+      AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs);
+    }
+    
     osKernelUnlock();
 
+    /* 创建修改后的数据副本用于发送到消息队列 */
     gimbal_to_send.eulr = eulr_to_send;
     gimbal_to_send.gyro = bmi088.gyro;
 
     osMessageQueueReset(task_runtime.msgq.gimbal.imu);
     osMessageQueuePut(task_runtime.msgq.gimbal.imu, &gimbal_to_send, 0, 0);
 
+    BSP_PWM_SetComp(BSP_PWM_IMU_HEAT_PWM, PID_Calc(&imu_temp_ctrl_pid, 40.0f, bmi088.temp, 0.0f, 0.0f));
+
     /* USER CODE END */
     osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
   }
   
-}
+}
+
+

User/task/rc.c

@@ -61,12 +61,12 @@ void Task_rc(void *argument) {
       cmd_for_chassis.mode = CHASSIS_MODE_RELAX;
       break;
     case DR16_SW_MID:
-      // cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
+      cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
-      cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
+      // cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
       break;
     case DR16_SW_DOWN:
-      // cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
+      cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
-      cmd_for_chassis.mode = CHASSIS_MODE_JUMP;
+      // cmd_for_chassis.mode = CHASSIS_MODE_JUMP;
       break;
     default:
       cmd_for_chassis.mode = CHASSIS_MODE_RELAX;