哎

CMakeLists.txt

@@ -62,6 +62,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     User/component/error_detect.c
     User/component/filter.c
     User/component/freertos_cli.c
+    User/component/limiter.c
     User/component/lqr.c
     User/component/pid.c
     User/component/user_math.c
@@ -77,14 +78,16 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     User/device/motor_lk.c
     User/device/motor_lz.c
     User/device/motor_rm.c
+    User/device/vision_bridge.c
 
     # User/module sources
     User/module/balance_chassis.c
     User/module/config.c
-    User/module/shoot.c
     User/module/gimbal.c
+    User/module/shoot.c
 
     # User/task sources
+    User/task/ai.c
     User/task/atti_esit.c
     User/task/blink.c
     User/task/ctrl_chassis.c

MDK-ARM/CtrBoard-H7_ALL.uvoptx

@@ -185,6 +185,21 @@
           <WinNumber>1</WinNumber>
           <ItemText>RF</ItemText>
         </Ww>
+        <Ww>
+          <count>6</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>ai</ItemText>
+        </Ww>
+        <Ww>
+          <count>7</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>cmd_ai</ItemText>
+        </Ww>
+        <Ww>
+          <count>8</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>gimbal</ItemText>
+        </Ww>
       </WatchWindow1>
       <Tracepoint>
         <THDelay>0</THDelay>
@@ -1168,6 +1183,18 @@
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
+    <File>
+      <GroupNumber>7</GroupNumber>
+      <FileNumber>74</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\User\component\limiter.c</PathWithFileName>
+      <FilenameWithoutPath>limiter.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
   </Group>
 
   <Group>
@@ -1178,7 +1205,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>74</FileNumber>
+      <FileNumber>75</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1190,7 +1217,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>75</FileNumber>
+      <FileNumber>76</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1202,7 +1229,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>76</FileNumber>
+      <FileNumber>77</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1214,7 +1241,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>77</FileNumber>
+      <FileNumber>78</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1226,7 +1253,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>78</FileNumber>
+      <FileNumber>79</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1238,7 +1265,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>79</FileNumber>
+      <FileNumber>80</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1250,7 +1277,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>80</FileNumber>
+      <FileNumber>81</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1262,7 +1289,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>81</FileNumber>
+      <FileNumber>82</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1274,7 +1301,7 @@
     </File>
     <File>
       <GroupNumber>8</GroupNumber>
-      <FileNumber>82</FileNumber>
+      <FileNumber>83</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1284,6 +1311,18 @@
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
+    <File>
+      <GroupNumber>8</GroupNumber>
+      <FileNumber>84</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\User\device\vision_bridge.c</PathWithFileName>
+      <FilenameWithoutPath>vision_bridge.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
   </Group>
 
   <Group>
@@ -1294,7 +1333,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>83</FileNumber>
+      <FileNumber>85</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1306,7 +1345,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>84</FileNumber>
+      <FileNumber>86</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1318,7 +1357,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>85</FileNumber>
+      <FileNumber>87</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1330,7 +1369,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>86</FileNumber>
+      <FileNumber>88</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1350,7 +1389,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>87</FileNumber>
+      <FileNumber>89</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1362,7 +1401,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>88</FileNumber>
+      <FileNumber>90</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1374,7 +1413,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>89</FileNumber>
+      <FileNumber>91</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1386,7 +1425,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>90</FileNumber>
+      <FileNumber>92</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1398,7 +1437,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>91</FileNumber>
+      <FileNumber>93</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1410,7 +1449,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>92</FileNumber>
+      <FileNumber>94</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1422,7 +1461,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>93</FileNumber>
+      <FileNumber>95</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1434,7 +1473,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>94</FileNumber>
+      <FileNumber>96</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1446,7 +1485,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>95</FileNumber>
+      <FileNumber>97</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1458,7 +1497,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>96</FileNumber>
+      <FileNumber>98</FileNumber>
       <FileType>5</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1468,6 +1507,18 @@
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
+    <File>
+      <GroupNumber>10</GroupNumber>
+      <FileNumber>99</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\User\task\ai.c</PathWithFileName>
+      <FilenameWithoutPath>ai.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
   </Group>
 
   <Group>

MDK-ARM/CtrBoard-H7_ALL.uvprojx

@@ -779,6 +779,11 @@
               <FileType>1</FileType>
               <FilePath>..\User\component\vmc.c</FilePath>
             </File>
+            <File>
+              <FileName>limiter.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\component\limiter.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>
@@ -829,6 +834,11 @@
               <FileType>1</FileType>
               <FilePath>..\User\device\gimbal_imu.c</FilePath>
             </File>
+            <File>
+              <FileName>vision_bridge.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\device\vision_bridge.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>
@@ -909,6 +919,11 @@
               <FileType>5</FileType>
               <FilePath>..\User\task\user_task.h</FilePath>
             </File>
+            <File>
+              <FileName>ai.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\task\ai.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>

User/component/component_config.yaml

@@ -19,6 +19,9 @@ filter:
 freertos_cli:
   dependencies: []
   enabled: true
+limiter:
+  dependencies: []
+  enabled: true
 pid:
   dependencies:
   - component/filter

User/component/limiter.c

@@ -0,0 +1,167 @@
+/*
+  限制器
+*/
+
+#include "limiter.h"
+#include "user_math.h"
+
+#include <math.h>
+#include <stddef.h>
+
+#define POWER_BUFF_THRESHOLD 20
+#define CHASSIS_POWER_CHECK_FREQ 10
+#define CHASSIS_POWER_FACTOR_PASS 0.9f
+#define CHASSIS_POWER_FACTOR_NO_PASS 1.5f
+
+#define CHASSIS_MOTOR_CIRCUMFERENCE 0.12f
+
+/**
+ * @brief 限制底盘功率不超过power_limit
+ *
+ * @param power_limit 最大功率
+ * @param motor_out 电机输出值
+ * @param speed 电机转速
+ * @param len 电机数量
+ * @return int8_t 0对应没有错误
+ */
+int8_t PowerLimit_ChassicOutput(float power_limit, float *motor_out,
+                                float *speed, uint32_t len) {
+  /* power_limit小于0时不进行限制 */
+  if (motor_out == NULL || speed == NULL || power_limit < 0) return -1;
+
+  float sum_motor_out = 0.0f;
+  for (uint32_t i = 0; i < len; i++) {
+    /* 总功率计算 P=F(由转矩电流表示)*V(由转速表示) */
+    sum_motor_out +=
+        fabsf(motor_out[i]) * fabsf(speed[i]) * CHASSIS_MOTOR_CIRCUMFERENCE;
+  }
+
+  /* 保持每个电机输出值缩小时比例不变 */
+  if (sum_motor_out > power_limit) {
+    for (uint32_t i = 0; i < len; i++) {
+      motor_out[i] *= power_limit / sum_motor_out;
+    }
+  }
+
+  return 0;
+}
+
+/**
+ * @brief 电容输入功率计算
+ *
+ * @param power_in 底盘当前功率
+ * @param power_limit 裁判系统功率限制值
+ * @param power_buffer 缓冲能量
+ * @return float 裁判系统输出最大值
+ */
+float PowerLimit_CapInput(float power_in, float power_limit,
+                          float power_buffer) {
+  float target_power = 0.0f;
+
+  /* 计算下一个检测周期的剩余缓冲能量 */
+  float heat_buff = power_buffer - (float)(power_in - power_limit) /
+                                       (float)CHASSIS_POWER_CHECK_FREQ;
+  if (heat_buff < POWER_BUFF_THRESHOLD) { /* 功率限制 */
+    target_power = power_limit * CHASSIS_POWER_FACTOR_PASS;
+  } else {
+    target_power = power_limit * CHASSIS_POWER_FACTOR_NO_PASS;
+  }
+
+  return target_power;
+}
+
+/**
+ * @brief 使用缓冲能量计算底盘最大功率
+ *
+ * @param power_limit 裁判系统功率限制值
+ * @param power_buffer 缓冲能量
+ * @return float 底盘输出最大值
+ */
+float PowerLimit_TargetPower(float power_limit, float power_buffer) {
+  float target_power = 0.0f;
+
+  /* 根据剩余缓冲能量计算输出功率 */
+  target_power = power_limit * (power_buffer - 10.0f) / 20.0f;
+  if (target_power < 0.0f) target_power = 0.0f;
+
+  return target_power;
+}
+
+/**
+ * @brief 射击频率控制
+ *
+ * @param heat 当前热量
+ * @param heat_limit 热量上限
+ * @param cooling_rate 冷却速率
+ * @param heat_increase 冷却增加
+ * @param shoot_freq 经过热量限制后的射击频率
+ * @return float 射击频率
+ */
+float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate,
+                          float heat_increase, bool is_big) {
+  float heat_percent = heat / heat_limit;
+  float stable_freq = cooling_rate / heat_increase;
+  if (is_big)
+    return stable_freq;
+  else
+    return (heat_percent > 0.7f) ? stable_freq : 3.0f * stable_freq;
+}
+
+/**
+ * @brief 计算电机与IMU坐标系之间的偏差（处理跨越±π的情况）
+ * @param motor_angle 电机角度
+ * @param imu_angle IMU角度
+ * @return 偏差值（已归一化到 -π ~ π）
+ */
+static float CalcMotorImuOffset(float motor_angle, float imu_angle) {
+  float offset = motor_angle - imu_angle;
+  if (offset > M_PI) offset -= M_2PI;
+  if (offset < -M_PI) offset += M_2PI;
+  return offset;
+}
+
+/**
+ * @brief 圆周角度限位器 - 考虑电机与IMU坐标系偏差
+ */
+void CircleAngleLimit(float *setpoint, float motor_angle, float imu_angle,
+                      float limit_max, float limit_min, float range) {
+  if (setpoint == NULL) return;
+  
+  /* 计算电机与IMU坐标系偏差 */
+  float motor_imu_offset = CalcMotorImuOffset(motor_angle, imu_angle);
+  
+  /* 将IMU setpoint转换为电机角度后进行限位检查 */
+  float motor_target = *setpoint + motor_imu_offset;
+  
+  /* 检查是否超过最大限位 */
+  if (CircleError(motor_target, limit_max, range) > 0) {
+    *setpoint = limit_max - motor_imu_offset;
+  }
+  /* 检查是否低于最小限位 */
+  if (CircleError(motor_target, limit_min, range) < 0) {
+    *setpoint = limit_min - motor_imu_offset;
+  }
+}
+
+/**
+ * @brief 圆周角度增量限位器 - 考虑电机与IMU坐标系偏差
+ */
+void CircleAngleDeltaLimit(float *delta, float setpoint, float motor_angle,
+                           float imu_angle, float limit_max, float limit_min,
+                           float range) {
+  if (delta == NULL) return;
+  
+  /* 计算电机与IMU坐标系偏差 */
+  float motor_imu_offset = CalcMotorImuOffset(motor_angle, imu_angle);
+  
+  /* 计算应用增量后在电机坐标系下的目标角度 */
+  float motor_target_after_delta = setpoint + motor_imu_offset + *delta;
+  
+  /* 计算到限位边界的距离 */
+  float delta_max = CircleError(limit_max, motor_target_after_delta, range);
+  float delta_min = CircleError(limit_min, motor_target_after_delta, range);
+  
+  /* 限制增量 */
+  if (*delta > delta_max) *delta = delta_max;
+  if (*delta < delta_min) *delta = delta_min;
+}

User/component/limiter.h

@@ -0,0 +1,93 @@
+/*
+  限制器
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdbool.h>
+#include <stdint.h>
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/**
+ * @brief 限制底盘功率不超过power_limit
+ *
+ * @param power_limit 最大功率
+ * @param motor_out 电机输出值
+ * @param speed 电机转速
+ * @param len 电机数量
+ * @return int8_t 0对应没有错误
+ */
+int8_t PowerLimit_ChassicOutput(float power_limit, float *motor_out,
+                                float *speed, uint32_t len);
+/**
+ * @brief 电容输入功率计算
+ *
+ * @param power_in 底盘当前功率
+ * @param power_limit 裁判系统功率限制值
+ * @param power_buffer 缓冲能量
+ * @return float 裁判系统输出最大值
+ */
+float PowerLimit_CapInput(float power_in, float power_limit,
+                          float power_buffer);
+/**
+ * @brief 使用缓冲能量计算底盘最大功率
+ *
+ * @param power_limit 裁判系统功率限制值
+ * @param power_buffer 缓冲能量
+ * @return float 底盘输出最大值
+ */
+float PowerLimit_TargetPower(float power_limit, float power_buffer);
+
+/**
+ * @brief 射击频率控制
+ *
+ * @param heat 当前热量
+ * @param heat_limit 热量上限
+ * @param cooling_rate 冷却速率
+ * @param heat_increase 冷却增加
+ * @param shoot_freq 经过热量限制后的射击频率
+ * @return float 射击频率
+ */
+float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate,
+                          float heat_increase, bool is_big);
+
+/**
+ * @brief 圆周角度限位器 - 考虑电机与IMU坐标系偏差
+ * @note 用于将 IMU 坐标系下的目标角度限制在电机坐标系的限位范围内
+ *
+ * @param setpoint 目标角度（IMU坐标系），会被直接修改
+ * @param motor_angle 电机当前角度
+ * @param imu_angle IMU当前角度
+ * @param limit_max 电机坐标系下的最大限位值
+ * @param limit_min 电机坐标系下的最小限位值
+ * @param range 角度循环范围（通常为 M_2PI）
+ */
+void CircleAngleLimit(float *setpoint, float motor_angle, float imu_angle,
+                      float limit_max, float limit_min, float range);
+
+/**
+ * @brief 圆周角度增量限位器 - 考虑电机与IMU坐标系偏差
+ * @note 用于限制增量控制时不超过电机限位范围
+ *
+ * @param delta 增量值，会被直接修改
+ * @param setpoint 当前目标角度（IMU坐标系）
+ * @param motor_angle 电机当前角度
+ * @param imu_angle IMU当前角度
+ * @param limit_max 电机坐标系下的最大限位值
+ * @param limit_min 电机坐标系下的最小限位值
+ * @param range 角度循环范围（通常为 M_2PI）
+ */
+void CircleAngleDeltaLimit(float *delta, float setpoint, float motor_angle,
+                           float imu_angle, float limit_max, float limit_min,
+                           float range);

User/device/ai.c

@@ -1,152 +0,0 @@
-/*
-AI
-*/
-
-/* Includes ----------------------------------------------------------------- */
-#include "ai.h"
-
-#include <string.h>
-
-#include "bsp\delay.h"
-#include "bsp\uart.h"
-#include "component\crc16.h"
-#include "component\crc8.h"
-#include "component\user_math.h"
-#include "component\filter.h"
-
-
-/* Private define ----------------------------------------------------------- */
-#define AI_LEN_RX_BUFF (sizeof(Protocol_DownPackage_t))
-
-/* Private macro ------------------------------------------------------------ */
-/* Private typedef ---------------------------------------------------------- */
-/* Private variables -------------------------------------------------------- */
-static volatile uint32_t drop_message = 0;
-
-static uint8_t rxbuf[AI_LEN_RX_BUFF];
-
-static bool inited = false;
-
-static osThreadId_t thread_alert;
-
-/* Private function  -------------------------------------------------------- */
-
-static void Ai_RxCpltCallback(void) {
-  osThreadFlagsSet(thread_alert, SIGNAL_AI_RAW_REDY);
-}
-
-static void Ai_IdleLineCallback(void) {
-  osThreadFlagsSet(thread_alert, SIGNAL_AI_RAW_REDY);
-}
-
-/* Exported functions ------------------------------------------------------- */
-int8_t AI_Init(AI_t *ai) {
-  UNUSED(ai);
-  ASSERT(ai);
-  if (inited) return DEVICE_ERR_INITED;
-  VERIFY((thread_alert = osThreadGetId()) != NULL);
-
-  BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_RX_CPLT_CB,
-                            Ai_RxCpltCallback);
-  BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_IDLE_LINE_CB,
-                            Ai_IdleLineCallback);
-  
-  inited = true;
-  return 0;
-}
-
-int8_t AI_Restart(void) {
-  __HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_AI));
-  __HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_AI));
-  return DEVICE_OK;
-}
-
-int8_t AI_StartReceiving(AI_t *ai) {
-  UNUSED(ai);
-  if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AI), rxbuf,
-                           AI_LEN_RX_BUFF) == HAL_OK)
-    return DEVICE_OK;
-  return DEVICE_ERR;
-}
-
-bool AI_WaitDmaCplt(void) {
-  return (osThreadFlagsWait(SIGNAL_AI_RAW_REDY, osFlagsWaitAll, 0) ==
-          SIGNAL_AI_RAW_REDY);
-}
-
-int8_t AI_ParseHost(AI_t *ai) {
-  if (!CRC16_Verify((const uint8_t *)&(rxbuf), sizeof(ai->from_host)))
-    goto error;
-  ai->ai_online = true;
-  memcpy(&(ai->from_host), rxbuf, sizeof(ai->from_host));
-  memset(rxbuf, 0, AI_LEN_RX_BUFF);
-  return DEVICE_OK;
-
-error:
-  drop_message++;
-  return DEVICE_ERR;
-}
-
-void AI_PackCmd(AI_t *ai, CMD_Host_t *cmd_host) {
-  cmd_host->gimbal_delta.yaw = ai->from_host.data.gimbal.yaw;
-  cmd_host->gimbal_delta.pit = ai->from_host.data.gimbal.pit;
-  cmd_host->fire = (ai->from_host.data.notice & AI_NOTICE_FIRE);
-  cmd_host->chassis_move_vec.vx = ai->from_host.data.chassis_move_vec.vx;
-  cmd_host->chassis_move_vec.vy = ai->from_host.data.chassis_move_vec.vy;
-  cmd_host->chassis_move_vec.wz = ai->from_host.data.chassis_move_vec.wz;
-}
-
-int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host) {
-  if (ai == NULL) return DEVICE_ERR_NULL;
-  if (cmd_host == NULL) return DEVICE_ERR_NULL;
-  ai->ai_online = false;
-  memset(&(ai->from_host), 0, sizeof(ai->from_host));
-  memset(cmd_host, 0, sizeof(*cmd_host));
-  return 0;
-}
-
-int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat) {
-  ai->to_host.mcu.id = AI_ID_MCU;
-  memcpy((void *)&(ai->to_host.mcu.package.data.quat), (const void *)quat,
-         sizeof(*quat));
-  ai->to_host.mcu.package.data.notice = 0;
-  if (ai->status == AI_STATUS_AUTOAIM)
-    ai->to_host.mcu.package.data.notice |= AI_NOTICE_AUTOAIM;
-  else if (ai->status == AI_STATUS_HITSWITCH)
-    ai->to_host.mcu.package.data.notice |= AI_NOTICE_HITBUFF;
-  else if (ai->status == AI_STATUS_AUTOMATIC)
-    ai->to_host.mcu.package.data.notice |= AI_NOTICE_AUTOMATIC;
-
-  ai->to_host.mcu.package.crc16 = CRC16_Calc(
-      (const uint8_t *)&(ai->to_host.mcu.package),
-      sizeof(ai->to_host.mcu.package) - sizeof(uint16_t), CRC16_INIT);
-  return DEVICE_OK;
-}
-
-int8_t AI_PackRef(AI_t *ai, const Referee_ForAI_t *ref) {
-  (void)ref;
-  ai->to_host.ref.id = AI_ID_REF;
-  ai->to_host.ref.package.crc16 = CRC16_Calc(
-      (const uint8_t *)&(ai->to_host.ref.package),
-      sizeof(ai->to_host.ref.package) - sizeof(uint16_t), CRC16_INIT);
-  return DEVICE_OK;
-}
-
-int8_t AI_StartSend(AI_t *ai, bool ref_update) {
-  if (ref_update) {
-    if (HAL_UART_Transmit_DMA(
-            BSP_UART_GetHandle(BSP_UART_AI), (uint8_t *)&(ai->to_host),
-            sizeof(ai->to_host.ref) + sizeof(ai->to_host.mcu)) == HAL_OK)
-      return DEVICE_OK;
-    else
-      return DEVICE_ERR;
-  } else {
-    if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_AI),
-                              (uint8_t *)&(ai->to_host.mcu),
-                              sizeof(ai->to_host.mcu)) == HAL_OK)
-      return DEVICE_OK;
-    else
-      return DEVICE_ERR;
-  }
-}
-

User/device/ai.h

@@ -1,69 +0,0 @@
-/*
-  AI
-*/
-
-#pragma once
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/* Includes ----------------------------------------------------------------- */
-#include <cmsis_os2.h>
-#include <stdbool.h>
-#include <stdint.h>
-
-#include "component\ahrs.h"
-#include "component\cmd.h"
-#include "component\user_math.h"
-#include "component\filter.h"
-#include "device\device.h"
-#include "device\referee.h"
-#include "protocol.h"
-
-/* Exported constants ------------------------------------------------------- */
-/* Exported macro ----------------------------------------------------------- */
-/* Exported types ----------------------------------------------------------- */
-
-typedef struct __packed {
-  uint8_t id;
-  Protocol_UpPackageReferee_t package;
-} AI_UpPackageReferee_t;
-
-typedef struct __packed {
-  uint8_t id;
-  Protocol_UpPackageMCU_t package;
-} AI_UpPackageMCU_t;
-
-typedef struct __packed {
-  osThreadId_t thread_alert;
-
-  Protocol_DownPackage_t from_host;
-
-  struct {
-    AI_UpPackageReferee_t ref;
-    AI_UpPackageMCU_t mcu;
-  } to_host;
-
-  CMD_AI_Status_t status;
-  bool ai_online;
-} AI_t;
-
-/* Exported functions prototypes -------------------------------------------- */
-int8_t AI_Init(AI_t *ai);
-int8_t AI_Restart(void);
-
-int8_t AI_StartReceiving(AI_t *ai);
-bool AI_WaitDmaCplt(void);
-int8_t AI_ParseHost(AI_t *ai);
-int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host);
-int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat);
-int8_t AI_PackRef(AI_t *ai, const Referee_ForAI_t *ref);
-int8_t AI_StartSend(AI_t *ai, bool option);
-void AI_PackCmd(AI_t *ai, CMD_Host_t *cmd_host);
-#ifdef __cplusplus
-}
-#endif
-
-
-

User/device/vision_bridge.c

@@ -0,0 +1,120 @@
+#include "vision_bridge.h"
+#include "device/device.h"
+#include "bsp/uart.h"
+#include "component/crc16.h"
+#include <string.h>
+#include "bsp/fdcan.h"
+
+#define AI_CMD_CAN_DLC       (8u)  /* 1字节mode + 3.5字节yaw + 3.5字节pit */
+#define AI_CMD_ANGLE_SCALE   (10000000.0f)  /* 0.0000001 rad per LSB */
+
+int8_t AI_Init(AI_t *ai, AI_Param_t *param) {
+  if (ai == NULL) return DEVICE_ERR_NULL;
+  
+  BSP_FDCAN_Init();
+  
+	memset(ai, 0, sizeof(AI_t));
+
+	ai->param = param;
+	BSP_FDCAN_RegisterId(ai->param->can, ai->param->vision_id, 3);
+
+  return 0;
+}
+
+
+int8_t AI_ParseForHost(AI_t* ai, AI_RawData_t* raw_data){
+		ai->tohost.head[0]='M';
+		ai->tohost.head[1]='R';	
+		ai->tohost.mode=raw_data->mode;
+		ai->tohost.pitch=raw_data->pitch;
+		ai->tohost.yaw=raw_data->yaw;	
+		ai->tohost.pitch_vel=raw_data->pitch_vel;
+		ai->tohost.yaw_vel=raw_data->yaw_vel;	
+		ai->tohost.q[0]=raw_data->q[0];	
+		ai->tohost.q[1]=raw_data->q[1];	
+		ai->tohost.q[2]=raw_data->q[2];	
+		ai->tohost.q[3]=raw_data->q[3];		
+		ai->tohost.bullet_count=10;
+		ai->tohost.bullet_speed=10.5;	
+		
+		ai->tohost.crc16=CRC16_Calc(((const uint8_t*)&(ai->tohost)),sizeof(ai->tohost)-sizeof(uint16_t), CRC16_INIT );
+		if(CRC16_Verify(((const uint8_t*)&(ai->tohost)), sizeof(ai->tohost))!=true){
+				  return DEVICE_ERR;
+		}
+		      return DEVICE_OK;
+}
+
+int8_t AI_Get(AI_t *ai, AI_cmd_t* outcmd) {
+		if(ai->fromhost.head[0]!='M'&&ai->fromhost.head[1]!='R'){
+					return DEVICE_ERR;
+		}
+//		CRC16_Calc(&ai->fromhost,sizeof(ai->fromhost),ai->fromhost.crc16);
+		if(CRC16_Verify((const uint8_t*)&(ai->fromhost), sizeof(ai->fromhost))!=true){
+				  return DEVICE_ERR;
+		}
+		outcmd->gimbal.setpoint.pit	=	ai->fromhost.pitch;
+		outcmd->gimbal.setpoint.yaw	=	ai->fromhost.yaw;
+		outcmd->mode								=	ai->fromhost.mode;
+		outcmd->gimbal.accl.pit			=	ai->fromhost.pitch_acc;
+		outcmd->gimbal.vel.pit			=	ai->fromhost.pitch_vel;
+		outcmd->gimbal.accl.yaw			=	ai->fromhost.yaw_acc;
+		outcmd->gimbal.vel.yaw			=	ai->fromhost.yaw_vel;		
+  return DEVICE_OK;
+}
+
+
+/* 打包并通过 CAN2 发送AI命令给下层板（mode + yaw + pit） */
+void AI_SendCmdOnFDCAN(AI_t *ai, const AI_cmd_t *cmd) {
+  if (cmd == NULL) return;
+
+  /* 将float角度转换为int32_t定点数（0.0000001 rad/LSB） */
+  const int32_t yaw = (int32_t)(cmd->gimbal.setpoint.yaw * AI_CMD_ANGLE_SCALE);
+  const int32_t pit = (int32_t)(cmd->gimbal.setpoint.pit * AI_CMD_ANGLE_SCALE);
+
+  BSP_FDCAN_StdDataFrame_t frame = {0};
+  frame.id = ai->param->vision_id;
+  frame.dlc = AI_CMD_CAN_DLC;
+  
+  /* mode(1字节) + yaw(3.5字节) + pit(3.5字节) */
+  frame.data[0] = cmd->mode;
+  frame.data[1] = (uint8_t)((yaw >> 20) & 0xFF);
+  frame.data[2] = (uint8_t)((yaw >> 12) & 0xFF);
+  frame.data[3] = (uint8_t)((yaw >> 4) & 0xFF);
+  frame.data[4] = (uint8_t)(((yaw & 0xF) << 4) | ((pit >> 24) & 0xF));
+  frame.data[5] = (uint8_t)((pit >> 16) & 0xFF);
+  frame.data[6] = (uint8_t)((pit >> 8) & 0xFF);
+  frame.data[7] = (uint8_t)(pit & 0xFF);
+  
+  (void)BSP_FDCAN_TransmitStdDataFrame(ai->param->can, &frame);
+}
+
+/* 从CAN消息解析AI命令 (mode + yaw + pit) */
+ void AI_ParseCmdFromCan(AI_t *ai, AI_cmd_t *cmd) {
+
+  if (cmd == NULL) {
+    return;
+  }
+		BSP_FDCAN_Message_t msg;
+  if (BSP_FDCAN_GetMessage(ai->param->can, ai->param->vision_id, &msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) != 0) {
+      return;
+  }
+
+  /* 解析mode (1字节) */
+  cmd->mode = msg.data[0];
+  
+  /* 解析yaw (3.5字节) */
+  int32_t yaw_raw = (int32_t)((msg.data[1] << 20) | (msg.data[2] << 12) | (msg.data[3] << 4) | ((msg.data[4] >> 4) & 0xF));
+  if (yaw_raw & 0x08000000) yaw_raw |= 0xF0000000;
+  cmd->gimbal.setpoint.yaw = (float)yaw_raw / AI_CMD_ANGLE_SCALE;
+  
+  /* 解析pit (3.5字节) */
+  int32_t pit_raw = (int32_t)(((msg.data[4] & 0xF) << 24) | (msg.data[5] << 16) | (msg.data[6] << 8) | msg.data[7]);
+  if (pit_raw & 0x08000000) pit_raw |= 0xF0000000;
+  cmd->gimbal.setpoint.pit = (float)pit_raw / AI_CMD_ANGLE_SCALE;
+  
+  /* 其他字段根据需要可以初始化为0 */
+  cmd->gimbal.vel.yaw = 0.0f;
+  cmd->gimbal.vel.pit = 0.0f;
+  cmd->gimbal.accl.yaw = 0.0f;
+  cmd->gimbal.accl.pit = 0.0f;
+}

User/device/vision_bridge.h

@@ -0,0 +1,99 @@
+/*
+ * 自瞄模组
+ */
+
+#pragma once
+
+#include "component/user_math.h"
+#include "bsp/fdcan.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+
+struct __attribute__((packed)) AI_ToHost
+{
+  uint8_t head[2];
+  uint8_t mode;  // 0: 空闲, 1: 自瞄, 2: 小符, 3: 大符
+  float q[4];    // wxyz顺序	/q4,q0,q1,q2/
+  float yaw;
+  float yaw_vel;
+  float pitch;
+  float pitch_vel;
+  float bullet_speed;
+  uint16_t bullet_count;  // 子弹累计发送次数
+  uint16_t crc16;
+};
+
+struct __attribute__((packed)) AI_FromHost
+{
+  uint8_t head[2];
+  uint8_t mode;  // 0: 不控制, 1: 控制云台但不开火，2: 控制云台且开火
+  float yaw;
+  float yaw_vel;
+  float yaw_acc;
+  float pitch;
+  float pitch_vel;
+  float pitch_acc;
+  uint16_t crc16;
+};
+
+typedef struct {
+	uint8_t mode;
+	struct{
+		struct{
+				float yaw;
+				float pit;		
+		}setpoint;
+		struct{
+					float pit;
+					float yaw;	
+					}accl;
+		struct{
+					float pit;
+					float yaw;	
+					}vel;		
+	}gimbal;
+	
+}AI_cmd_t;
+
+typedef struct  {
+  uint8_t mode;  // 0: 空闲, 1: 自瞄, 2: 小符, 3: 大符
+  float q[4];    // wxyz顺序	/q4,q0,q1,q2/
+  float yaw;
+  float yaw_vel;
+  float pitch;
+  float pitch_vel;
+  float bullet_speed;
+  uint16_t bullet_count;  // 子弹累计发送次数
+}AI_RawData_t;
+
+typedef struct  {
+	BSP_FDCAN_t can;
+  uint16_t vision_id;
+}AI_Param_t;
+
+typedef struct __attribute__((packed)) {
+	struct AI_ToHost tohost;
+	struct AI_FromHost fromhost;
+  AI_Param_t *param;
+}AI_t;
+
+
+int8_t AI_Init(AI_t *ai, AI_Param_t *param);
+
+int8_t AI_StartReceiving(AI_t *ai);
+
+int8_t AI_Get(AI_t *ai, AI_cmd_t* ai_cmd);
+
+int8_t AI_ParseForHost(AI_t* ai, AI_RawData_t* raw_data);
+
+int8_t AI_StartSend(AI_t *ai);
+
+void AI_SendCmdOnCan(AI_t *ai, const AI_cmd_t *cmd);
+
+ void AI_ParseCmdFromCan(AI_t *ai, AI_cmd_t *cmd);
+#ifdef __cplusplus
+}
+#endif

User/module/balance_chassis.c

@@ -21,12 +21,14 @@
 #define MAX_ACCELERATION    2.0f     /* 最大加速度 (m/s²) */
 #define WHEEL_GEAR_RATIO    8.0f     /* 轮毂电机扭矩 */
 #define BASE_LEG_LENGTH     0.16f    /* 基础腿长 (m) */
-#define LEG_LENGTH_RANGE    0.18f    /* 腿长调节范围 (m) */
+#define LEG_LENGTH_RANGE    0.16f    /* 腿长调节范围 (m) */
 #define MIN_LEG_LENGTH      0.10f    /* 最小腿长 (m) */
 #define MAX_LEG_LENGTH      0.34f    /* 最大腿长 (m) */
 #define NON_CONTACT_THETA   0.16f    /* 离地时的摆角目标 (rad) */
 #define LEFT_BASE_FORCE     60.0f    /* 左腿基础支撑力 (N) */
 #define RIGHT_BASE_FORCE    60.0f    /* 右腿基础支撑力 (N) */
+
+
 // float L_fn = 0.0f, L_tp = 0.0f, R_fn = 0.0f, R_tp = 0.0f,LF =0.0f,RF=0.0f;
 /* Private function prototypes ---------------------------------------------- */
 static int8_t Chassis_MotorEnable(Chassis_t *c);
@@ -35,6 +37,7 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode);
 static void Chassis_UpdateChassisState(Chassis_t *c);
 static void Chassis_ResetControllers(Chassis_t *c);
 static void Chassis_SelectYawZeroPoint(Chassis_t *c);
+static void Chassis_JumpControl(Chassis_t *c, const Chassis_CMD_t *c_cmd, float *target_L0, float *extra_force);
 
 /* Private functions -------------------------------------------------------- */
 
@@ -239,6 +242,88 @@ static float Chassis_CalcSpringForce(float leg_length)
                   + (L_AC - L_BD*sin_theta)*(L_AC - L_BD*sin_theta)) * L_BE * leg_length);
 }
 
+/**
+ * @brief 跳跃控制状态机
+ * @param c 底盘结构体指针
+ * @param c_cmd 控制指令
+ * @param target_L0 输出的目标腿长数组[2]
+ * @param extra_force 输出的额外力数组[2]
+ */
+static void Chassis_JumpControl(Chassis_t *c, const Chassis_CMD_t *c_cmd, float *target_L0, float *extra_force) {
+  uint32_t current_time = (uint32_t)(BSP_TIME_Get_us() / 1000);  /* 当前时间 ms */
+  uint32_t elapsed_time = current_time - c->jump.state_start_time;
+  
+  /* 初始化额外力为0 */
+  extra_force[0] = 0.0f;
+  extra_force[1] = 0.0f;
+  
+  /* 检测跳跃触发 */
+  if (c->jump.trigger && c->jump.state == JUMP_IDLE) {
+    c->jump.state = JUMP_CROUCH;
+    c->jump.state_start_time = current_time;
+    c->jump.trigger = false;  /* 清除触发标志 */
+  }
+  
+  /* 跳跃状态机 */
+  switch (c->jump.state) {
+    case JUMP_IDLE:
+      /* 待机状态，使用正常腿长控制 */
+      break;
+      
+    case JUMP_CROUCH:
+      /* 蓄力阶段：缩短腿长 */
+      target_L0[0] = c->param->jump_params.crouch_leg_length;
+      target_L0[1] = c->param->jump_params.crouch_leg_length;
+      
+      if (elapsed_time >= c->param->jump_params.crouch_time_ms) {
+        c->jump.state = JUMP_LAUNCH;
+        c->jump.state_start_time = current_time;
+      }
+      break;
+      
+    case JUMP_LAUNCH:
+      /* 起跳阶段：发力向下推地面 */
+      target_L0[0] = MAX_LEG_LENGTH;  /* 腿伸长 */
+      target_L0[1] = MAX_LEG_LENGTH;
+      extra_force[0] = c->param->jump_params.launch_force;  /* 额外向下的力 */
+      extra_force[1] = c->param->jump_params.launch_force;
+      
+      if (elapsed_time >= c->param->jump_params.launch_time_ms) {
+        c->jump.state = JUMP_RETRACT;
+        c->jump.state_start_time = current_time;
+      }
+      break;
+      
+    case JUMP_RETRACT:
+      /* 收腿阶段：腿收缩准备落地 */
+      target_L0[0] = c->param->jump_params.retract_leg_length;
+      target_L0[1] = c->param->jump_params.retract_leg_length;
+      extra_force[0] = c->param->jump_params.retract_force;  /* 前馈力帮助收腿 */
+      extra_force[1] = c->param->jump_params.retract_force;
+      
+      /* 检测落地或超时 */
+      if ( elapsed_time >= c->param->jump_params.retract_time_ms) {
+        c->jump.state = JUMP_LAND;
+        c->jump.state_start_time = current_time;
+      }
+      break;
+      
+    case JUMP_LAND:
+      /* 落地阶段：缓冲并恢复正常 */
+      /* 使用正常腿长，让PID自动恢复 */
+      
+      if (elapsed_time >= c->param->jump_params.land_time_ms) {
+        c->jump.state = JUMP_IDLE;
+        c->jump.state_start_time = current_time;
+      }
+      break;
+      
+    default:
+      c->jump.state = JUMP_IDLE;
+      break;
+  }
+}
+
 /* Public functions --------------------------------------------------------- */
 
 /**
@@ -307,6 +392,14 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
   memset(&c->chassis_state, 0, sizeof(c->chassis_state));
   memset(&c->yaw_control, 0, sizeof(c->yaw_control));
 
+  /* 初始化跳跃状态 */
+  c->jump.state = JUMP_IDLE;
+  c->jump.trigger = false;
+  c->jump.state_start_time = 0;
+  c->jump.crouch_leg_length = c->param->jump_params.crouch_leg_length;
+  c->jump.launch_force = c->param->jump_params.launch_force;
+  c->jump.retract_leg_length = c->param->jump_params.retract_leg_length;
+
   return CHASSIS_OK;
 }
 
@@ -429,8 +522,6 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
     } break;
     
     // case CHASSIS_MODE_CALIBRATE: {
-
-      
     //   Chassis_LQRControl(c, c_cmd);
     //   LF= Chassis_CalcSpringForce(c->vmc_[0].leg.L0);
     //   RF= Chassis_CalcSpringForce(c->vmc_[1].leg.L0);
@@ -449,6 +540,12 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
       Chassis_LQRControl(c, c_cmd);
       Chassis_Output(c);
       break;
+    case CHASSIS_MODE_BALANCE_ROTOR:
+      Chassis_LQRControl(c, c_cmd);
+      c->output.wheel[0] += c_cmd->move_vec.vy * 0.2f;
+      c->output.wheel[1] -= c_cmd->move_vec.vy * 0.2f;
+      Chassis_Output(c);
+      break;
 
     default:
       return CHASSIS_ERR_MODE;
@@ -538,13 +635,14 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
   LQR_State_t target_state = {
       .theta = 0.0f,
       .d_theta = 0.0f,
-      .phi = -0.2f,
+      .phi = -0.1f,
       .d_phi = 0.0f,
       .x = c->chassis_state.target_x,
       .d_x = c->chassis_state.target_velocity_x,
   };
 
   /* ==================== Yaw轴控制 ==================== */
+  if (c_cmd->mode!= CHASSIS_MODE_BALANCE_ROTOR || c_cmd->move_vec.vy == 0.0f) {
     c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle;
 
     float manual_offset = c_cmd->move_vec.vy * M_PI_2;
@@ -564,6 +662,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
 
     c->yaw_control.yaw_force = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw,
                                         c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt);
+  }
 
   /* ==================== 左腿LQR控制 ==================== */
   if (c->vmc_[0].leg.is_ground_contact) {
@@ -608,9 +707,15 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
 
   /* ==================== 腿长控制 ==================== */
   float target_L0[2];
+  float jump_extra_force[2] = {0.0f, 0.0f};
+  
+  /* 基础腿长目标 */
   target_L0[0] = BASE_LEG_LENGTH + c_cmd->height * LEG_LENGTH_RANGE + roll_compensation;
   target_L0[1] = BASE_LEG_LENGTH + c_cmd->height * LEG_LENGTH_RANGE - roll_compensation;
 
+  /* 跳跃控制：可能会修改目标腿长和额外力 */
+  Chassis_JumpControl(c, c_cmd, target_L0, jump_extra_force);
+
   /* 腿长限幅 */
   target_L0[0] = LIMIT(target_L0[0], MIN_LEG_LENGTH, MAX_LEG_LENGTH);
   target_L0[1] = LIMIT(target_L0[1], MIN_LEG_LENGTH, MAX_LEG_LENGTH);
@@ -634,11 +739,20 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
                                     c->vmc_[0].leg.L0, leg_d_length[0], c->dt);
   float spring_force_left = Chassis_CalcSpringForce(c->vmc_[0].leg.L0);
   if (isnan(spring_force_left)) spring_force_left = 0.0f;  /* 处理无效值 */
-  float virtual_force_left = c->lqr[0].control_output.Tp * sinf(c->vmc_[0].leg.theta) +
-                              pid_output_left + LEFT_BASE_FORCE - spring_force_left;
   
-  if (VMC_InverseSolve(&c->vmc_[0], virtual_force_left,
+  /* 收腿阶段特殊处理：完全忽略LQR输出，只用PID+前馈力收腿 */
-                       c->lqr[0].control_output.Tp + Delta_Tp[0]) == 0) {
+  float virtual_force_left, virtual_torque_left;
+  if (c->jump.state == JUMP_RETRACT) {
+    /* 收腿阶段：纯收腿控制，不受LQR和基础支撑力影响 */
+    virtual_force_left = pid_output_left - spring_force_left + jump_extra_force[0];
+    virtual_torque_left = 0.0f;  /* 收腿时不控制摆角 */
+  } else {
+    virtual_force_left = c->lqr[0].control_output.Tp * sinf(c->vmc_[0].leg.theta) +
+                          pid_output_left + LEFT_BASE_FORCE - spring_force_left + jump_extra_force[0];
+    virtual_torque_left = c->lqr[0].control_output.Tp + Delta_Tp[0];
+  }
+
+  if (VMC_InverseSolve(&c->vmc_[0], virtual_force_left, virtual_torque_left) == 0) {
     VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]);
   } else {
     c->output.joint[0] = 0.0f;
@@ -650,11 +764,20 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
                                      c->vmc_[1].leg.L0, leg_d_length[1], c->dt);
   float spring_force_right = Chassis_CalcSpringForce(c->vmc_[1].leg.L0);
   if (isnan(spring_force_right)) spring_force_right = 0.0f;  /* 处理无效值 */
-  float virtual_force_right = c->lqr[1].control_output.Tp * sinf(c->vmc_[1].leg.theta) +
-                               pid_output_right + RIGHT_BASE_FORCE - spring_force_right;
   
-  if (VMC_InverseSolve(&c->vmc_[1], virtual_force_right,
+  /* 收腿阶段特殊处理：完全忽略LQR输出，只用PID+前馈力收腿 */
-                       c->lqr[1].control_output.Tp + Delta_Tp[1]) == 0) {
+  float virtual_force_right, virtual_torque_right;
+  if (c->jump.state == JUMP_RETRACT) {
+    /* 收腿阶段：纯收腿控制，不受LQR和基础支撑力影响 */
+    virtual_force_right = pid_output_right - spring_force_right + jump_extra_force[1];
+    virtual_torque_right = 0.0f;  /* 收腿时不控制摆角 */
+  } else {
+    virtual_force_right = c->lqr[1].control_output.Tp * sinf(c->vmc_[1].leg.theta) +
+                           pid_output_right + RIGHT_BASE_FORCE - spring_force_right + jump_extra_force[1];
+    virtual_torque_right = c->lqr[1].control_output.Tp + Delta_Tp[1];
+  }
+
+  if (VMC_InverseSolve(&c->vmc_[1], virtual_force_right, virtual_torque_right) == 0) {
     VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]);
   } else {
     c->output.joint[2] = 0.0f;
@@ -666,8 +789,21 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
     c->output.wheel[i] = LIMIT(c->output.wheel[i], -1.0f, 1.0f);
   }
   for (int i = 0; i < 4; i++) {
-    c->output.joint[i] = LIMIT(c->output.joint[i], -15.0f, 15.0f);
+    c->output.joint[i] = LIMIT(c->output.joint[i], -60.0f, 60.0f);
   }
 
   return CHASSIS_OK;
 }
+
+/**
+ * @brief 触发跳跃
+ * @param c 底盘结构体指针
+ */
+void Chassis_TriggerJump(Chassis_t *c) {
+  if (c == NULL) return;
+  
+  /* 只在平衡模式且待机状态下可触发跳跃 */
+  if (c->mode == CHASSIS_MODE_WHELL_LEG_BALANCE && c->jump.state == JUMP_IDLE) {
+    c->jump.trigger = true;
+  }
+}

User/module/balance_chassis.h

@@ -40,13 +40,24 @@ typedef enum {
   CHASSIS_MODE_RELAX,            /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */
   CHASSIS_MODE_RECOVER,          /* 复位模式 */
   // CHASSIS_MODE_CALIBRATE,        /* 校准模式 */
-  CHASSIS_MODE_WHELL_LEG_BALANCE /* 轮子+腿平衡模式，底盘自我平衡 */
+  CHASSIS_MODE_WHELL_LEG_BALANCE, /* 轮子+腿平衡模式，底盘自我平衡 */
+  CHASSIS_MODE_BALANCE_ROTOR /*小陀螺*/
 } Chassis_Mode_t;
 
+/* 跳跃状态枚举 */
+typedef enum {
+  JUMP_IDLE,      /* 待机状态 */
+  JUMP_CROUCH,    /* 蓄力阶段：腿缩短蓄力 */
+  JUMP_LAUNCH,    /* 起跳阶段：发力向下 */
+  JUMP_RETRACT,   /* 收腿阶段：腿收缩准备落地 */
+  JUMP_LAND       /* 落地阶段：恢复正常控制 */
+} Jump_State_t;
+
 typedef struct {
   Chassis_Mode_t mode;           /* 底盘模式 */
   MoveVector_t move_vec;         /* 底盘运动向量 */
   float height;               /* 目标高度 */
+  bool jump_trigger;             /* 跳跃触发标志 */
 } Chassis_CMD_t;
 
 typedef struct {
@@ -83,6 +94,17 @@ typedef struct {
   KPID_Params_t leg_length;         /* 腿长PID控制参数，用于控制虚拟腿长度 */
   KPID_Params_t leg_theta;          /* 摆角PID控制参数，用于控制虚拟腿摆角 */
 
+  struct {
+    uint32_t crouch_time_ms;      /* 蓄力时间 (ms) */
+    uint32_t launch_time_ms;      /* 起跳发力时间 (ms) */
+    uint32_t retract_time_ms;     /* 收腿时间 (ms) */
+    uint32_t land_time_ms;        /* 落地缓冲时间 (ms) */
+    float crouch_leg_length;      /* 蓄力时腿长 (m) */
+    float launch_force;           /* 起跳力 (N) */
+    float retract_leg_length;     /* 收腿时腿长 (m) */
+    float retract_force;          /* 收腿前馈力 (N)，负值表示向上收 */
+  } jump_params;
+
   float mech_zero_yaw; /* 机械零点 */
 
   float theta;
@@ -136,6 +158,16 @@ typedef struct {
     bool is_reversed;     /* 是否使用反转的零点（180度零点），影响前后方向 */
   } yaw_control;
 
+  /* 跳跃控制相关 */
+  struct {
+    Jump_State_t state;       /* 跳跃状态 */
+    bool trigger;             /* 跳跃触发标志 */
+    uint32_t state_start_time; /* 当前状态开始时间 (ms) */
+    float crouch_leg_length;  /* 蓄力时的腿长 */
+    float launch_force;       /* 起跳力 (N) */
+    float retract_leg_length; /* 收腿时的腿长 */
+  } jump;
+
   /* PID计算的目标值 */
   struct {
     AHRS_Eulr_t chassis;
@@ -208,7 +240,13 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd);
  */
 int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd);
 
-
+/**
+ * \brief 触发跳跃
+ *
+ * \param c 包含底盘数据的结构体
+ * \note 只在CHASSIS_MODE_WHELL_LEG_BALANCE模式下有效
+ */
+void Chassis_TriggerJump(Chassis_t *c);
 
 /**
  * \brief 底盘输出值

User/module/config.c

@@ -24,9 +24,9 @@ Config_RobotParam_t robot_config = {
     .gimbal_param = {
         .pid = {
             .yaw_omega = {
-                .k = 2.0f,
+                .k = 0.25f,
                 .p = 1.0f,
-                .i = 0.3f,
+                .i = 0.0f,
                 .d = 0.0f,
                 .i_limit = 1.0f,
                 .out_limit = 1.0f,
@@ -104,8 +104,8 @@ Config_RobotParam_t robot_config = {
 
     .shoot_param = {
             .trig_step_angle=M_2PI/8,
-            .shot_delay_time=1.0f,
+            .shot_delay_time=0.05f,
-            .shot_burst_num=20,
+            .shot_burst_num=1,
             .fric_motor_param[0] = {
                 .can = BSP_CAN_1,
                 .id = 0x201,
@@ -317,24 +317,39 @@ Config_RobotParam_t robot_config = {
         },
 
 .lqr_gains = {
-    .k11_coeff = { -2.120324305163214e+02f, 2.384281822810979e+02f, -1.162210131498081e+02f, -2.405740963481636e+00f }, // theta
+    .k11_coeff = { -2.210764178888050e+02f, 2.426363711747753e+02f, -1.195592871161101e+02f, -3.943946668753056e+00f }, // theta
-    .k12_coeff = { 4.320836680770054e-01f, 1.360781729068518e-01f, -8.343612068216379e+00f, -8.600090910123033e-02f },  // d_theta
+    .k12_coeff = { -2.137164311515632e+00f, 1.217737477437830e+00f, -9.525046061205586e+00f, -3.204960462460905e-01f },  // d_theta
-    .k13_coeff = { -4.493046202256553e+01f, 4.577344792125822e+01f, -1.629835599958664e+01f, -1.007247166173255e+00f },  // x
+    .k13_coeff = { -5.403421185903361e+01f, 5.400834583224349e+01f, -1.871421701200579e+01f, -2.187399624044644e+00f },  // x
-    .k14_coeff = { -4.823335755850488e+01f, 4.987409533322209e+01f, -1.917162943665977e+01f, -1.473642463713354e+00f },  // d_x
+    .k14_coeff = { -4.592744044784984e+01f, 4.660766512779469e+01f, -1.805265440365850e+01f, -2.699994690713614e+00f },  // d_x
-    .k15_coeff = { -4.920796990864941e+01f, 6.450325939254844e+01f, -3.268284723443183e+01f, 7.841340265493823e+00f }, // phi
+    .k15_coeff = { -9.685784551581214e+01f, 1.159388980795003e+02f, -5.296171221139453e+01f, 1.123906253873898e+01f }, // phi
-    .k16_coeff = { -1.264530042590822e+01f, 1.663296191858249e+01f, -8.467446023207946e+00f, 2.096442008644146e+00f },  // d_phi
+    .k16_coeff = { -2.294801139621741e+01f, 2.749641396600526e+01f, -1.259823164155369e+01f, 2.761284676172917e+00f },  // d_phi
-    .k21_coeff = { 1.636475235954215e+02f, -1.128937920212271e+02f, 4.887401528348596e+00f, 1.459120525493287e+01f }, // theta
+    .k21_coeff = { 7.727438740554770e+01f, -3.291782819657940e+01f, -1.757884718209812e+01f, 1.497962178015115e+01f }, // theta
-    .k22_coeff = { 9.939826270288583e+00f, -8.353709902293666e+00f, 1.640639416293288e+00f, 1.492185865897709e+00f },  // d_theta
+    .k22_coeff = { -4.313876591098404e-01f, 3.014310593976116e+00f, -2.775571423570345e+00f, 1.949396772465259e+00f },  // d_theta
-    .k23_coeff = { -4.968599085108445e+01f, 6.646556717472484e+01f, -3.397333983104631e+01f, 7.847958130301292e+00f },  // x
+    .k23_coeff = { -8.163301739920055e+01f, 9.889264843312036e+01f, -4.547892032101780e+01f, 9.369462147227104e+00f },  // x
-    .k24_coeff = { -7.188031995054928e+01f, 9.082377283123918e+01f, -4.412648091833633e+01f, 9.959213854333724e+00f },  // d_x
+    .k24_coeff = { -1.025410522837800e+02f, 1.187387974704523e+02f, -5.202150278239223e+01f, 1.038835798060997e+01f },  // d_x
-    .k25_coeff = { 2.360507220981238e+02f, -2.398095392324340e+02f, 8.536061841837561e+01f, 2.469595316477948e+00f }, // phi
+    .k25_coeff = { 2.742584002836992e+02f, -2.735816033702905e+02f, 9.479793291043821e+01f, 7.314831370012082e+00f }, // phi
-    .k26_coeff = { 6.296425580760564e+01f, -6.412220242164098e+01f, 2.293354052056732e+01f, 4.870876539985159e-01f },  // d_phi
+    .k26_coeff = { 6.734357804702088e+01f, -6.735254573603545e+01f, 2.346143287673895e+01f, 1.501777400084277e+00f },  // d_phi
 },
+    .jump_params = {
+        .crouch_time_ms = 300,
+        .launch_time_ms = 120,
+        .retract_time_ms = 80,
+        .land_time_ms = 300,
+        .crouch_leg_length = 0.14f,
+        .launch_force = 200.0f,
+        .retract_leg_length = 0.16f,   /* 收腿目标更短 */
+        .retract_force = -120.0f,      /* 收腿前馈力加大 */
+    },
         .theta = 0.0f,
         .x = 0.0f,
         .phi = -0.1f,
     
-    }
+    },
+
+    .ai_param = {
+        .can = BSP_FDCAN_2,
+        .vision_id = 0x104,
+    },
     /* USER CODE END robot_config */
 };
 

User/module/config.h

@@ -13,6 +13,7 @@ extern "C" {
 #include "module/shoot.h"
 #include "module/balance_chassis.h"
 #include "module/gimbal.h"
+#include "device/vision_bridge.h"
 /**
  * @brief 机器人参数配置结构体
  * @note 在此添加您的配置参数
@@ -22,6 +23,7 @@ typedef struct {
     Shoot_Params_t shoot_param;
     Chassis_Params_t chassis_param;
     Gimbal_Params_t gimbal_param;
+    AI_Param_t ai_param;
     /* USER CODE END Config_RobotParam */
 } Config_RobotParam_t;
 

User/module/gimbal.c

@@ -8,6 +8,7 @@
 #include "bsp/time.h"
 #include <math.h> 
 #include "component/filter.h"
+#include "component/limiter.h"
 #include "component/pid.h"
 #include "device/gimbal_imu.h"
 #include "device/motor_dm.h"
@@ -163,7 +164,7 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal){
  *
  * \return 函数运行结果
  */
-int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
+int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) {
   if (g == NULL || g_cmd == NULL) {
     return -1;
   }
@@ -173,45 +174,23 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
 
   Gimbal_SetMode(g, g_cmd->mode);
 
-  /* 处理yaw控制命令，软件限位 - 使用电机绝对角度 */
+  /* 处理yaw控制命令，软件限位 */
   float delta_yaw = g_cmd->delta_yaw * g->dt * 1.5f;
   if (g->param->travel.yaw > 0) {
-    /* 计算当前电机角度与IMU角度的偏差 */
+    CircleAngleDeltaLimit(&delta_yaw, g->setpoint.eulr.yaw,
-    float motor_imu_offset = g->feedback.motor.yaw.rotor_abs_angle - g->feedback.imu.eulr.yaw;
+                          g->feedback.motor.yaw.rotor_abs_angle,
-    /* 处理跨越±π的情况 */
+                          g->feedback.imu.eulr.yaw,
-    if (motor_imu_offset > M_PI) motor_imu_offset -= M_2PI;
+                          g->limit.yaw.max, g->limit.yaw.min, M_2PI);
-    if (motor_imu_offset < -M_PI) motor_imu_offset += M_2PI;
-    
-    /* 计算到限位边界的距离 */
-    const float delta_max = CircleError(g->limit.yaw.max,
-        (g->setpoint.eulr.yaw + motor_imu_offset + delta_yaw), M_2PI);
-    const float delta_min = CircleError(g->limit.yaw.min,
-        (g->setpoint.eulr.yaw + motor_imu_offset + delta_yaw), M_2PI);
-    
-    /* 限制控制命令 */
-    if (delta_yaw > delta_max) delta_yaw = delta_max;
-    if (delta_yaw < delta_min) delta_yaw = delta_min;
   }
   CircleAdd(&(g->setpoint.eulr.yaw), delta_yaw, M_2PI);
 
-  /* 处理pitch控制命令，软件限位 - 使用电机绝对角度 */
+  /* 处理pitch控制命令，软件限位 */
   float delta_pit = g_cmd->delta_pit * g->dt;
   if (g->param->travel.pit > 0) {
-    /* 计算当前电机角度与IMU角度的偏差 */
+    CircleAngleDeltaLimit(&delta_pit, g->setpoint.eulr.pit,
-    float motor_imu_offset = g->feedback.motor.pit.rotor_abs_angle - g->feedback.imu.eulr.rol;
+                          g->feedback.motor.pit.rotor_abs_angle,
-    /* 处理跨越±π的情况 */
+                          g->feedback.imu.eulr.rol,
-    if (motor_imu_offset > M_PI) motor_imu_offset -= M_2PI;
+                          g->limit.pit.max, g->limit.pit.min, M_2PI);
-    if (motor_imu_offset < -M_PI) motor_imu_offset += M_2PI;
-    
-    /* 计算到限位边界的距离 */
-    const float delta_max = CircleError(g->limit.pit.max,
-        (g->setpoint.eulr.pit + motor_imu_offset + delta_pit), M_2PI);
-    const float delta_min = CircleError(g->limit.pit.min,
-        (g->setpoint.eulr.pit + motor_imu_offset + delta_pit), M_2PI);
-    
-    /* 限制控制命令 */
-    if (delta_pit > delta_max) delta_pit = delta_max;
-    if (delta_pit < delta_min) delta_pit = delta_min;
   }
   CircleAdd(&(g->setpoint.eulr.pit), delta_pit, M_2PI);
 
@@ -222,12 +201,32 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
     g->out.yaw = 0.0f;
     g->out.pit = 0.0f;
     break;
+  case GIMBAL_MODE_AI_CONTROL:
+    if (g_ai != NULL && g_ai->ctrl) {
+      g->setpoint.eulr.yaw = g_ai->yaw;
+      g->setpoint.eulr.pit = -g_ai->pit;
+      
+      /* 限位处理 */
+      if (g->param->travel.yaw > 0) {
+        CircleAngleLimit(&g->setpoint.eulr.yaw,
+                         g->feedback.motor.yaw.rotor_abs_angle,
+                         g->feedback.imu.eulr.yaw,
+                         g->limit.yaw.max, g->limit.yaw.min, M_2PI);
+      }
+      if (g->param->travel.pit > 0) {
+        CircleAngleLimit(&g->setpoint.eulr.pit,
+                         g->feedback.motor.pit.rotor_abs_angle,
+                         g->feedback.imu.eulr.rol,
+                         g->limit.pit.max, g->limit.pit.min, M_2PI);
+      }
+    }
+    /* fallthrough - AI控制模式也需要执行PID计算 */
     
   case GIMBAL_MODE_ABSOLUTE:
   case GIMBAL_MODE_RELATIVE:
     yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
                                    g->feedback.imu.eulr.yaw, 0.0f, g->dt);
-    g->out.yaw = PID_Calc(&(g->pid.pit_omega), yaw_omega_set_point,
+    g->out.yaw = PID_Calc(&(g->pid.yaw_omega), yaw_omega_set_point,
                           g->feedback.imu.gyro.z, 0.f, g->dt);
 
     pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), g->setpoint.eulr.pit,
@@ -235,7 +234,6 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
     g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point,
                           g->feedback.imu.gyro.y, 0.f, g->dt);
 
-              
     break;
   }
   

User/module/gimbal.h

@@ -30,6 +30,7 @@ typedef enum {
   GIMBAL_MODE_RELAX,    /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */
   GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */
   GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */
+  GIMBAL_MODE_AI_CONTROL /* AI控制模式，直接接受AI下发的目标角度 */
 } Gimbal_Mode_t;
 
 typedef struct {
@@ -38,6 +39,12 @@ typedef struct {
   float delta_pit;
 } Gimbal_CMD_t;
 
+typedef struct {
+  bool ctrl;
+  float yaw;
+  float pit;
+} Gimbal_AI_t;
+
 /* 软件限位 */
 typedef struct {
   float max;
@@ -168,7 +175,7 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal);
  *
  * \return 函数运行结果
  */
-int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd);
+int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai);
 
 /**
  * \brief 云台输出

User/module/shoot.c

@@ -134,7 +134,7 @@ int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq)
 	
     memset(&s->shoot_Anglecalu,0,sizeof(s->shoot_Anglecalu));
     memset(&s->output,0,sizeof(s->output));
-    s->target_variable.target_rpm=4000.0f;  /* 归一化目标转速 */
+    s->target_variable.target_rpm=6000.0f;  /* 归一化目标转速 */
 
 	return 0;
 }
@@ -257,7 +257,6 @@ int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd)
                     }
                     else if(last_firecmd==false&&cmd->firecmd==true)
                     {
-                        Shoot_ResetCalu(s);
                         Shoot_ResetOutput(s);
                         s->running_state=SHOOT_STATE_FIRE;
                         s->shoot_Anglecalu.num_to_shoot+=s->param->shot_burst_num;
@@ -281,7 +280,6 @@ int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd)
                 MOTOR_RM_SetOutput(&s->param->trig_motor_param, s->output.outlpf_trig);
                 if(!cmd->firecmd)
                     {
-                        Shoot_ResetCalu(s);
                         Shoot_ResetOutput(s);
                         s->running_state=SHOOT_STATE_READY;
                     }

User/task/ai.c

@@ -0,0 +1,60 @@
+/*
+    ai Task
+    
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "cmsis_os2.h"
+#include "task/user_task.h"
+/* USER INCLUDE BEGIN */
+#include "bsp/fdcan.h"
+#include "module/config.h"
+#include "module/gimbal.h"
+#include "device/vision_bridge.h"
+/* USER INCLUDE END */
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+/* USER STRUCT BEGIN */
+AI_cmd_t cmd_ai;
+AI_t ai;
+Gimbal_AI_t ai_for_gimbal;
+/* USER STRUCT END */
+
+/* Private function --------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+void Task_ai(void *argument) {
+  (void)argument; /* 未使用argument，消除警告 */
+
+  
+  /* 计算任务运行到指定频率需要等待的tick数 */
+  const uint32_t delay_tick = osKernelGetTickFreq() / AI_FREQ;
+
+  osDelay(AI_INIT_DELAY); /* 延时一段时间再开启任务 */
+
+  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
+  /* USER CODE INIT BEGIN */
+  AI_Init(&ai, &Config_GetRobotParam()->ai_param);
+  /* 注册CAN接收ID */
+  /* USER CODE INIT END */
+  
+  while (1) {
+    tick += delay_tick; /* 计算下一个唤醒时刻 */
+    /* USER CODE BEGIN */
+    AI_ParseCmdFromCan( &ai,&cmd_ai);
+    if (cmd_ai.mode != 0){
+      ai_for_gimbal.ctrl = true;
+    } else {
+      ai_for_gimbal.ctrl = false;
+    }
+    ai_for_gimbal.yaw = cmd_ai.gimbal.setpoint.yaw;
+    ai_for_gimbal.pit = cmd_ai.gimbal.setpoint.pit;
+    osMessageQueueReset(task_runtime.msgq.gimbal.ai_cmd);
+    osMessageQueuePut(task_runtime.msgq.gimbal.ai_cmd, &ai_for_gimbal, 0, 0); 
+    /* USER CODE END */
+    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
+  }
+  
+}

User/task/config.yaml

@@ -46,3 +46,10 @@
   function: Task_ctrl_shoot
   name: ctrl_shoot
   stack: 256
+- delay: 0
+  description: ''
+  freq_control: true
+  frequency: 500.0
+  function: Task_ai
+  name: ai
+  stack: 256

User/task/ctrl_chassis.c

@@ -62,11 +62,15 @@ void Task_ctrl_chassis(void *argument) {
  
     Chassis_UpdateFeedback(&chassis);
 
+    /* 检查跳跃触发 */
+    if (chassis_cmd.jump_trigger) {
+      Chassis_TriggerJump(&chassis);
+      chassis_cmd.jump_trigger = false;  /* 清除触发标志 */
+    }
+
     Chassis_Control(&chassis, &chassis_cmd);     
     /* USER CODE END */
     osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
   }
   
 }
-
-

User/task/ctrl_gimbal.c

@@ -19,6 +19,7 @@
 /* USER STRUCT BEGIN */
 Gimbal_t gimbal;
 Gimbal_CMD_t gimbal_cmd;
+Gimbal_AI_t gimbal_ai;
 // BSP_FDCAN_StdDataFrame_t can_frame;
 /* USER STRUCT END */
 
@@ -48,14 +49,14 @@ void Task_ctrl_gimbal(void *argument) {
     
     Gimbal_UpdateIMU(&gimbal);
     osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &gimbal_cmd, NULL, 0);
-
+    osMessageQueueGet(task_runtime.msgq.gimbal.ai_cmd, &gimbal_ai, NULL, 0);
 
     Gimbal_UpdateFeedback(&gimbal);
 
     osMessageQueueReset(task_runtime.msgq.chassis.yaw); // 重置消息队列，防止阻塞
     osMessageQueuePut(task_runtime.msgq.chassis.yaw, &gimbal.feedback.motor.yaw, 0, 0);
     
-    Gimbal_Control(&gimbal, &gimbal_cmd);
+    Gimbal_Control(&gimbal, &gimbal_cmd, &gimbal_ai);
 
     Gimbal_Output(&gimbal);
 

User/task/ctrl_shoot.c

@@ -4,7 +4,6 @@
 */
 
 /* Includes ----------------------------------------------------------------- */
-#include "device/motor_rm.h"
 #include "task/user_task.h"
 /* USER INCLUDE BEGIN */
 #include "module/shoot.h"

User/task/init.c

@@ -39,6 +39,7 @@ void Task_Init(void *argument) {
   task_runtime.thread.monitor = osThreadNew(Task_monitor, NULL, &attr_monitor);
   task_runtime.thread.blink = osThreadNew(Task_blink, NULL, &attr_blink);
   task_runtime.thread.ctrl_shoot = osThreadNew(Task_ctrl_shoot, NULL, &attr_ctrl_shoot);
+  task_runtime.thread.ai = osThreadNew(Task_ai, NULL, &attr_ai);
 
   // 创建消息队列
   /* USER MESSAGE BEGIN */
@@ -48,6 +49,7 @@ void Task_Init(void *argument) {
   task_runtime.msgq.chassis.cmd = osMessageQueueNew(2u, sizeof(Chassis_CMD_t), NULL);
   task_runtime.msgq.chassis.yaw = osMessageQueueNew(2u, sizeof(MOTOR_Feedback_t), NULL);
   task_runtime.msgq.gimbal.cmd = osMessageQueueNew(2u, sizeof(Gimbal_CMD_t), NULL);
+  task_runtime.msgq.gimbal.ai_cmd = osMessageQueueNew(2u, sizeof(Gimbal_AI_t), NULL);
   /* USER MESSAGE END */
 
   osKernelUnlock(); // 解锁内核

User/task/rc.c

@@ -10,6 +10,7 @@
 #include "module/shoot.h"
 #include "module/balance_chassis.h"
 #include "module/gimbal.h"
+#include "component/user_math.h"
 /* USER INCLUDE END */
 
 /* Private typedef ---------------------------------------------------------- */
@@ -84,12 +85,20 @@ void Task_rc(void *argument) {
       cmd_for_gimbal.delta_pit = 0.0f;
       break;
     case DR16_SW_MID:
-      cmd_for_gimbal.mode = GIMBAL_MODE_ABSOLUTE;
+      if (dr16.data.sw_r == DR16_SW_UP || dr16.data.sw_r == DR16_SW_ERR) {
+        cmd_for_gimbal.mode = GIMBAL_MODE_RELATIVE;
+      } else {
+        cmd_for_gimbal.mode = GIMBAL_MODE_AI_CONTROL;
+      }
       cmd_for_gimbal.delta_yaw = -dr16.data.ch_r_x * 5.0f;
       cmd_for_gimbal.delta_pit = dr16.data.ch_r_y * 5.0f;
       break;
     case DR16_SW_DOWN:
-      cmd_for_gimbal.mode = GIMBAL_MODE_ABSOLUTE;
+      if (dr16.data.sw_r == DR16_SW_UP || dr16.data.sw_r == DR16_SW_ERR) {
+        cmd_for_gimbal.mode = GIMBAL_MODE_RELATIVE;
+      } else {
+        cmd_for_gimbal.mode = GIMBAL_MODE_AI_CONTROL;
+      }
       cmd_for_gimbal.delta_yaw = -dr16.data.ch_r_x * 5.0f;
       cmd_for_gimbal.delta_pit = dr16.data.ch_r_y * 5.0f;
       break;
@@ -104,6 +113,20 @@ void Task_rc(void *argument) {
     osMessageQueuePut(task_runtime.msgq.gimbal.cmd, &cmd_for_gimbal, 0, 0);
 
 /************************* 底盘命令 **************************************/
+    /* 跳跃触发检测：ch_res 从 -1.0f 松开回 0 时触发 */
+    static bool ch_res_was_min = false;  /* 记录上次是否在最低位置 */
+    const float CH_RES_MIN_THRESHOLD = -0.9f;  /* 判定为最低位置的阈值 */
+    const float CH_RES_RELEASE_THRESHOLD = -0.3f;  /* 判定为松开的阈值 */
+    
+    cmd_for_chassis.jump_trigger = false;  /* 默认不触发 */
+    if (dr16.data.ch_res < CH_RES_MIN_THRESHOLD) {
+      ch_res_was_min = true;  /* 记录已到达最低位置 */
+    } else if (ch_res_was_min && dr16.data.ch_res > CH_RES_RELEASE_THRESHOLD) {
+      /* 从最低位置松开，触发跳跃 */
+      cmd_for_chassis.jump_trigger = true;
+      ch_res_was_min = false;  /* 重置状态 */
+    }
+
 switch (dr16.data.sw_l) {
     case DR16_SW_UP:
       cmd_for_chassis.mode = CHASSIS_MODE_RELAX;
@@ -114,7 +137,7 @@ switch (dr16.data.sw_l) {
       break;
     case DR16_SW_DOWN:
     // cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
-      // cmd_for_chassis.mode = CHASSIS_MODE_ROTOR;
+      // cmd_for_chassis.mode = CHASSIS_MODE_BALANCE_ROTOR;
       cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
       // cmd_for_chassis.mode = CHASSIS_MODE_CALIBRATE;
       break;
@@ -125,7 +148,7 @@ switch (dr16.data.sw_l) {
     cmd_for_chassis.move_vec.vx = dr16.data.ch_l_y;
     cmd_for_chassis.move_vec.vy = dr16.data.ch_l_x;
     cmd_for_chassis.move_vec.wz = dr16.data.ch_r_x;
-    cmd_for_chassis.height = dr16.data.ch_res;
+    cmd_for_chassis.height = max(dr16.data.ch_res, 0.0f);
 
     osMessageQueueReset(
         task_runtime.msgq.chassis.cmd); // 重置消息队列，防止阻塞

User/task/user_task.c

@@ -22,12 +22,12 @@ const osThreadAttr_t attr_atti_esit = {
 const osThreadAttr_t attr_ctrl_chassis = {
     .name = "ctrl_chassis",
     .priority = osPriorityNormal,
-    .stack_size = 256 * 8,
+    .stack_size = 256 * 4,
 };
 const osThreadAttr_t attr_ctrl_gimbal = {
     .name = "ctrl_gimbal",
     .priority = osPriorityNormal,
-    .stack_size = 256 * 8,
+    .stack_size = 256 * 4,
 };
 const osThreadAttr_t attr_monitor = {
     .name = "monitor",
@@ -42,5 +42,10 @@ const osThreadAttr_t attr_blink = {
 const osThreadAttr_t attr_ctrl_shoot = {
     .name = "ctrl_shoot",
     .priority = osPriorityNormal,
-    .stack_size = 256 * 8,
+    .stack_size = 256 * 4,
+};
+const osThreadAttr_t attr_ai = {
+    .name = "ai",
+    .priority = osPriorityNormal,
+    .stack_size = 256 * 4,
 };

User/task/user_task.h

@@ -19,6 +19,7 @@ extern "C" {
 #define MONITOR_FREQ (500.0)
 #define BLINK_FREQ (500.0)
 #define CTRL_SHOOT_FREQ (500.0)
+#define AI_FREQ (500.0)
 
 /* 任务初始化延时ms */
 #define TASK_INIT_DELAY (100u)
@@ -29,6 +30,7 @@ extern "C" {
 #define MONITOR_INIT_DELAY (0)
 #define BLINK_INIT_DELAY (0)
 #define CTRL_SHOOT_INIT_DELAY (0)
+#define AI_INIT_DELAY (0)
 
 /* Exported defines --------------------------------------------------------- */
 /* Exported macro ----------------------------------------------------------- */
@@ -45,6 +47,7 @@ typedef struct {
         osThreadId_t monitor;
         osThreadId_t blink;
         osThreadId_t ctrl_shoot;
+        osThreadId_t ai;
     } thread;
 
     /* USER MESSAGE BEGIN */
@@ -58,6 +61,7 @@ typedef struct {
         struct {
             osMessageQueueId_t imu;
             osMessageQueueId_t cmd;
+            osMessageQueueId_t ai_cmd;
         }gimbal;
         struct {
             osMessageQueueId_t shoot_cmd; /* 发射命令队列 */
@@ -96,6 +100,7 @@ typedef struct {
         UBaseType_t monitor;
         UBaseType_t blink;
         UBaseType_t ctrl_shoot;
+        UBaseType_t ai;
     } stack_water_mark;
 
     /* 各任务运行频率 */
@@ -106,6 +111,7 @@ typedef struct {
         float monitor;
         float blink;
         float ctrl_shoot;
+        float ai;
     } freq;
 
     /* 任务最近运行时间 */
@@ -116,6 +122,7 @@ typedef struct {
         float monitor;
         float blink;
         float ctrl_shoot;
+        float ai;
     } last_up_time;
 
 } Task_Runtime_t;
@@ -132,6 +139,7 @@ extern const osThreadAttr_t attr_ctrl_gimbal;
 extern const osThreadAttr_t attr_monitor;
 extern const osThreadAttr_t attr_blink;
 extern const osThreadAttr_t attr_ctrl_shoot;
+extern const osThreadAttr_t attr_ai;
 
 /* 任务函数声明 */
 void Task_Init(void *argument);
@@ -142,6 +150,7 @@ void Task_ctrl_gimbal(void *argument);
 void Task_monitor(void *argument);
 void Task_blink(void *argument);
 void Task_ctrl_shoot(void *argument);
+void Task_ai(void *argument);
 
 #ifdef __cplusplus
 }

utils/Simulation-master/balance/series_legs/get_k_length.m

@@ -48,8 +48,8 @@ function K = get_k_length(leg_length)
     B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
     B=double(B);
     
-    Q=diag([1500 100 2500 1500 8000 500]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
+    Q=diag([1500 200 5000 2000 20000 1000]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
-    R=[250 0;0 50];                %T Tp
+    R=[240 0;0 60];                %T Tp
     
     K=lqr(A,B,Q,R);