正常的英雄

.vscode/settings.json

@@ -1,6 +0,0 @@
-{
-    "files.associations": {
-        "user_math.h": "c",
-        "shoot.h": "c"
-    }
-}

MDK-ARM/DevC.uvoptx

@@ -412,31 +412,6 @@
           <WinNumber>1</WinNumber>
           <ItemText>cmd_host</ItemText>
         </Ww>
-        <Ww>
-          <count>11</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>gimbal_ahrs</ItemText>
-        </Ww>
-        <Ww>
-          <count>12</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>eulr_to_send</ItemText>
-        </Ww>
-        <Ww>
-          <count>13</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>up_data</ItemText>
-        </Ww>
-        <Ww>
-          <count>14</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>AHRS_Eulr_t</ItemText>
-        </Ww>
-        <Ww>
-          <count>15</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>gimbal_encoder</ItemText>
-        </Ww>
       </WatchWindow1>
       <Tracepoint>
         <THDelay>0</THDelay>
@@ -1700,7 +1675,7 @@
 
   <Group>
     <GroupName>User/device</GroupName>
-    <tvExp>1</tvExp>
+    <tvExp>0</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
@@ -1780,7 +1755,7 @@
 
   <Group>
     <GroupName>User/module</GroupName>
-    <tvExp>1</tvExp>
+    <tvExp>0</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>

Middlewares/Third_Party/Protocol/protocol.h

@@ -10,15 +10,12 @@
 extern "C" {
 #endif
 
-#define AI_NOTICE_FIRE (1 << 0)
+#define AI_NOTICE_AUTOAIM (1 << 0)
-#define AI_NOTICE_TRACKING (1 << 1)
+#define AI_NOTICE_HITBUFF (1 << 1)
-
+#define AI_NOTICE_AUTOMATIC (1 << 2)
-#define AI_NOTICE_AUTOAIM (1 << 2)
+#define AI_NOTICE_FIRE (1 << 3)
-#define AI_NOTICE_HITBUFF (1 << 3)
-#define AI_NOTICE_AUTOMATIC (1 << 4)
 
 #define AI_ID_MCU (0xC4)
-#define AI_ID_MCU2 (0xD4)
 #define AI_ID_REF (0xA8)
 
 #define AI_TEAM_RED (0x01)
@@ -36,15 +33,16 @@ typedef struct __attribute__((packed)) {
     float q3;
   } quat; /* 四元数 */
 
-  // float chassis_speed; /* 底盘速度(哨兵) */
-  struct __attribute__((packed)) {
-    float yaw; /* 偏航角（Yaw angle） */
-    float pit; /* 俯仰角（Pitch angle） */
-    float rol; /* 翻滚角（Roll angle） */
-  } gimbal;    /* 欧拉角 */
-
   uint8_t notice; /* 控制命令 */
 
+  float ball_speed; /* 子弹初速度 */
+
+  struct __attribute__((packed)) {
+    float left;
+    float right;
+  } distance; /* 左右距离(哨兵) */
+
+  float chassis_speed; /* 底盘速度(哨兵) */
 } Protocol_UpDataMCU_t;
 
 /* 电控 -> 视觉 裁判系统数据结构体*/
@@ -55,21 +53,19 @@ typedef struct __attribute__((packed)) {
 
 /* 视觉 -> 电控 数据结构体*/
 typedef struct __attribute__((packed)) {
-
   struct __attribute__((packed)) {
     float yaw; /* 偏航角（Yaw angle） */
     float pit; /* 俯仰角（Pitch angle） */
     float rol; /* 翻滚角（Roll angle） */
   } gimbal;    /* 欧拉角 */
 
+  uint8_t notice; /* 控制命令 */
 
   struct __attribute__((packed)) {
     float vx;         /* x轴移动速度 */
     float vy;         /* y轴移动速度 */
     float wz;         /* z轴转动速度 */
   } chassis_move_vec; /* 底盘移动向量 */
-  uint8_t notice; /* 控制命令 */
-
 } Protocol_DownData_t;
 
 typedef struct __attribute__((packed)) {

User/bsp/mm.h

@@ -13,10 +13,6 @@ extern "C" {
 /* Exported types ----------------------------------------------------------- */
 /* Exported functions prototypes -------------------------------------------- */
 void *BSP_Malloc(size_t size);
-/**
- * @brief  
- * @param pv 
- */
 void BSP_Free(void *pv);
 
 #ifdef __cplusplus

User/component/ballistics.c

@@ -4,40 +4,9 @@
 
 #include "ballistics.h"
 
-/**
- * @brief 
- * @param b 
- */
 void Ballistics_Init(Ballistics_t *b) { (void)b; }
-
+void Ballistics_Apply(Ballistics_t *b, float bullet_speed) {
-
-/**
- * @brief 
- * @param b 
- * @param bullet_speed 
- */
-void Ballistics_Apply(Ballistics_t *b, float bullet_speed)
-{
   (void)b;
   (void)bullet_speed;
 }
-
-
-
-/**
- * @brief 
- * @param bullet_speed 弹丸速度
- * @param distance 距离
- * @return 高度补偿值
- */
-float Simpal_Ballistics_Apply(float bullet_speed, float distance)
-{
-  return distance /1000 / bullet_speed;
-}
-
-
-/**
- * @brief 
- * @param b 
- */
 void Ballistics_Reset(Ballistics_t *b) { (void)b; }

User/component/ballistics.h

@@ -16,14 +16,6 @@ typedef struct {
 
 void Ballistics_Init(Ballistics_t *b);
 void Ballistics_Apply(Ballistics_t *b, float bullet_speed);
-
-/**
- * @brief 
- * @param bullet_speed  弹丸速度
- * @param distance  距离 
- * @return  高度补偿值
- */
-float Simpal_Ballistics_Apply(float bullet_speed, float distance);
 void Ballistics_Reset(Ballistics_t *b);
 
 #ifdef __cplusplus

User/component/cmd.c

@@ -158,8 +158,8 @@ static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
       CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_STOP);
     } else {
       /* ai切换至自瞄模式，启用host控制 */
-      cmd->host_overwrite = true;
       cmd->ai_status = AI_STATUS_AUTOAIM;
+      cmd->host_overwrite = true;
       CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_START);
     }
   } else {
@@ -190,19 +190,27 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
       /* 左拨杆相应行为选择和解析 */
     case CMD_SW_UP:
       cmd->chassis.mode = CHASSIS_MODE_BREAK;
+      // cmd->host_overwrite = false;
+      // cmd->shoot.ai_fire = false;
       break;
 
     case CMD_SW_MID:
       cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
+      // cmd->host_overwrite = true;
+      // cmd->shoot.ai_fire = false;
       break;
 
     case CMD_SW_DOWN:
       cmd->chassis.mode = CHASSIS_MODE_ROTOR;
       cmd->chassis.mode_rotor = ROTOR_MODE_CW;
+      // cmd->host_overwrite = true;
+      // cmd->shoot.ai_fire = true;
       break;
 
     case CMD_SW_ERR:
       cmd->chassis.mode = CHASSIS_MODE_RELAX;
+      // cmd->host_overwrite = false;
+      // cmd->shoot.ai_fire = false;
       break;
   }
   switch (rc->sw_r) {
@@ -223,20 +231,37 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
       cmd->shoot.mode = SHOOT_MODE_LOADED;
       cmd->shoot.fire_mode = FIRE_MODE_SINGLE;
       cmd->shoot.fire = true;
+      break;
+		/*
+		    case CMD_SW_UP:
+      cmd->gimbal.mode = GIMBAL_MODE_RELAX;
+      cmd->shoot.mode = SHOOT_MODE_SAFE;
       break;
 
+    case CMD_SW_MID:
+      cmd->gimbal.mode = GIMBAL_MODE_RELAX;
+      cmd->shoot.fire = false;
+      cmd->shoot.mode = SHOOT_MODE_LOADED;
+      break;
+
+    case CMD_SW_DOWN:
+      cmd->gimbal.mode = GIMBAL_MODE_RELAX;
+      cmd->shoot.mode = SHOOT_MODE_LOADED;
+      cmd->shoot.fire_mode = FIRE_MODE_SINGLE;
+      cmd->shoot.fire = true;
+      break;
+		*/
     case CMD_SW_ERR:
       cmd->gimbal.mode = GIMBAL_MODE_RELAX;
       cmd->shoot.mode = SHOOT_MODE_RELAX;
+      // cmd->host_overwrite = false;
   }
   /* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */
-  if (cmd->ai_status == AI_STATUS_OFFLINE || cmd->host_overwrite == false) {
   cmd->chassis.ctrl_vec.vx = rc->ch_l_x;
   cmd->chassis.ctrl_vec.vy = rc->ch_l_y;
   cmd->gimbal.delta_eulr.yaw = rc->ch_r_x * dt_sec * cmd->param->sens_rc;
   cmd->gimbal.delta_eulr.pit = rc->ch_r_y * dt_sec * cmd->param->sens_rc;
 }
-}
 
 /**
  * @brief rc失控时机器人恢复放松模式
@@ -330,17 +355,12 @@ int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec) {
   cmd->gimbal.delta_eulr.yaw = host->gimbal_delta.yaw;
   cmd->gimbal.delta_eulr.pit = host->gimbal_delta.pit;
 
-  cmd->shoot.mode = SHOOT_MODE_LOADED;
-  if (host->tracking){
-    cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
-  }else{
-    cmd->gimbal.mode = GIMBAL_MODE_SEARCH;
-  }
   /* host射击命令，设置不同的射击频率和弹丸初速度 */
-  if (host->fire && cmd->shoot.ai_fire) {
+  if (host->fire) {
+    cmd->shoot.mode = SHOOT_MODE_LOADED;
     cmd->shoot.fire = true;
   } else {
-    cmd->shoot.fire = true;
+    cmd->shoot.mode = SHOOT_MODE_SAFE;
   }
   return 0;
 }

User/component/cmd.h

@@ -42,7 +42,6 @@ typedef enum {
 typedef enum {
   GIMBAL_MODE_RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */
   GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */
-  GIMBAL_MODE_SEARCH, /* 搜索模式，通过云台旋转搜索目标 */
   GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */
 } CMD_GimbalMode_t;
 
@@ -179,7 +178,6 @@ typedef struct {
 
 /* AI行为状态 */
 typedef enum {
-  AI_STATUS_OFFLINE,  /* 离线状态 */
   AI_STATUS_STOP,      /* 停止状态 */
   AI_STATUS_AUTOAIM,   /* 自瞄状态 */
   AI_STATUS_HITSWITCH, /* 打符状态 */
@@ -256,7 +254,6 @@ typedef struct {
     float wz;         /* z轴转动速度 */
   } chassis_move_vec; /* 底盘移动向量 */
 
-  bool tracking; /* 是否追踪目标 */
   bool fire; /* 开火状态 */
 } CMD_Host_t;
 

User/component/user_math.c

@@ -112,17 +112,3 @@ inline float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm) {
   /* 不为裁判系统设定值时,计算转速 */
   return 60.0f * (float)bullet_speed / (M_2PI * fric_radius);
 }
-
-/**
- * @brief 断言失败处理
- *
- * @param file 文件名
- * @param line 行号
- */
-void VerifyFailed(const char *file, uint32_t line) {
-  UNUSED(file);
-  UNUSED(line);
-  while (1) {
-    __NOP();
-  }
-}

User/component/user_math.h

@@ -104,53 +104,3 @@ float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm);
 #ifdef __cplusplus
 }
 #endif
-
-#ifdef DEBUG
-
-/**
- * @brief 如果表达式的值为假则运行处理函数
- *
- */
-#define ASSERT(expr)                    \
-  do {                                  \
-    if (!(expr)) {                      \
-      VerifyFailed(__FILE__, __LINE__); \
-    }                                   \
-  } while (0)
-#else
-
-/**
- * @brief 未定DEBUG，表达式不会运行，断言被忽略
- *
- */
-#define ASSERT(expr) ((void)(0))
-#endif
-
-#ifdef DEBUG
-
-/**
- * @brief 如果表达式的值为假则运行处理函数
- *
- */
-#define VERIFY(expr)                    \
-  do {                                  \
-    if (!(expr)) {                      \
-      VerifyFailed(__FILE__, __LINE__); \
-    }                                   \
-  } while (0)
-#else
-
-/**
- * @brief 表达式会运行，忽略表达式结果
- *
- */
-#define VERIFY(expr) ((void)(expr))
-#endif
-
-/**
- * @brief 断言失败处理
- *
- * @param file 文件名
- * @param line 行号
- */
-void VerifyFailed(const char *file, uint32_t line);

User/device/ai.c

@@ -12,51 +12,34 @@
 #include "component\crc16.h"
 #include "component\crc8.h"
 #include "component\user_math.h"
-#include "component\filter.h"
-
 
 /* Private define ----------------------------------------------------------- */
-#define AI_HOST_MAX_CONTROL_VALUE (0.001f)
-
-#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 osThreadId_t thread_alert;
 
 static bool inited = false;
 
-static LowPassFilter2p_t low_pass_filter;
-
 /* 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);
+  if (ai == NULL) return DEVICE_ERR_NULL;
-  ASSERT(ai);
   if (inited) return DEVICE_ERR_INITED;
-  VERIFY((thread_alert = osThreadGetId()) != NULL);
+  if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_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);
-  LowPassFilter2p_Init(&low_pass_filter, 1000.0f, 5.0f); // 示例参数
-
   inited = true;
+
+  ai->ai_online = false;
   return 0;
 }
 
@@ -67,9 +50,9 @@ int8_t AI_Restart(void) {
 }
 
 int8_t AI_StartReceiving(AI_t *ai) {
-  UNUSED(ai);
+  if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AI),
-  if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AI), rxbuf,
+                           ai->form_host.rx_buffer,
-                           AI_LEN_RX_BUFF) == HAL_OK)
+                           sizeof(ai->form_host.rx_buffer)) == HAL_OK)
     return DEVICE_OK;
   return DEVICE_ERR;
 }
@@ -79,19 +62,27 @@ bool AI_WaitDmaCplt(void) {
           SIGNAL_AI_RAW_REDY);
 }
 
-int8_t AI_ParseHost(AI_t *ai) {
+int8_t AI_ParseHost(AI_t *ai, CMD_Host_t *cmd_host) {
-  if (!CRC16_Verify((const uint8_t *)&(rxbuf), sizeof(ai->form_host)))
+  (void)cmd_host;
-    goto error;
   ai->ai_online = true;
+  if (!CRC16_Verify(ai->form_host.rx_buffer, sizeof(ai->form_host.rx_buffer)))
+    goto error;
+  memcpy(&(ai->form_host.data), ai->form_host.rx_buffer, sizeof(ai->form_host.data));
+  // cmd_host->gimbal_delta.pit = ai->form_host.data.gimbal.pit;
+  // cmd_host->gimbal_delta.yaw = ai->form_host.data.gimbal.yaw;
+  // cmd_host->gimbal_delta.rol = ai->form_host.data.gimbal.rol;
+  // cmd_host->fire = (ai->form_host.data.notice & AI_NOTICE_FIRE);
+  // cmd_host->chassis_move_vec.vx = ai->form_host.data.chassis_move_vec.vx;
+  // cmd_host->chassis_move_vec.vy = ai->form_host.data.chassis_move_vec.vy;
+  // cmd_host->chassis_move_vec.wz = ai->form_host.data.chassis_move_vec.wz;
+  cmd_host->gimbal_delta.pit = ai->form_host.data.data.gimbal.pit;
+  cmd_host->gimbal_delta.yaw = ai->form_host.data.data.gimbal.yaw;
+  cmd_host->gimbal_delta.rol = ai->form_host.data.data.gimbal.rol;
+  cmd_host->fire = (ai->form_host.data.data.notice & AI_NOTICE_FIRE);
+  cmd_host->chassis_move_vec.vx = ai->form_host.data.data.chassis_move_vec.vx;
+  cmd_host->chassis_move_vec.vy = ai->form_host.data.data.chassis_move_vec.vy;
+  cmd_host->chassis_move_vec.wz = ai->form_host.data.data.chassis_move_vec.wz;
 
-  // 对接收到的数据进行滤波处理
-  for (size_t i = 0; i < sizeof(ai->form_host.data.gimbal); ++i) {
-    ((float*)&ai->form_host.data.gimbal)[i] = 
-      LowPassFilter2p_Apply(&low_pass_filter, ((float*)&ai->form_host.data.gimbal)[i]);
-  }
-
-  memcpy(&(ai->form_host), rxbuf, sizeof(ai->form_host));
-  memset(rxbuf, 0, AI_LEN_RX_BUFF);
   return DEVICE_OK;
 
 error:
@@ -99,37 +90,20 @@ error:
   return DEVICE_ERR;
 }
 
-void AI_PackCmd(AI_t *ai, CMD_Host_t *cmd_host) {
-  cmd_host->gimbal_delta.yaw = -ai->form_host.data.gimbal.yaw;
-  cmd_host->gimbal_delta.yaw = cmd_host->gimbal_delta.yaw - 0.00025;
-  cmd_host->gimbal_delta.pit = -ai->form_host.data.gimbal.pit;
-  cmd_host->gimbal_delta.pit = cmd_host->gimbal_delta.pit + 0.0005;
-  Clip(&(cmd_host->gimbal_delta.yaw), -AI_HOST_MAX_CONTROL_VALUE,
-        AI_HOST_MAX_CONTROL_VALUE);
-  Clip(&(cmd_host->gimbal_delta.pit), -AI_HOST_MAX_CONTROL_VALUE,
-        AI_HOST_MAX_CONTROL_VALUE);
-
-  cmd_host->fire = (ai->form_host.data.notice & AI_NOTICE_FIRE);
-  cmd_host->chassis_move_vec.vx = ai->form_host.data.chassis_move_vec.vx;
-  cmd_host->chassis_move_vec.vy = ai->form_host.data.chassis_move_vec.vy;
-  cmd_host->chassis_move_vec.wz = ai->form_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->form_host), 0, sizeof(ai->form_host));
+  memset(ai->form_host.rx_buffer, 0, sizeof(ai->form_host.rx_buffer));
+  memset(&(ai->form_host.data), 0, sizeof(ai->form_host.data));
   memset(cmd_host, 0, sizeof(*cmd_host));
   return 0;
 }
 
-int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat, const AHRS_Eulr_t *gimbal_ai) {
+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));
-  memcpy((void *)&(ai->to_host.mcu.package.data.gimbal), (const void *)gimbal_ai,
-         sizeof(*gimbal_ai));
   ai->to_host.mcu.package.data.notice = 0;
   if (ai->status == AI_STATUS_AUTOAIM)
     ai->to_host.mcu.package.data.notice |= AI_NOTICE_AUTOAIM;

User/device/ai.h

@@ -16,7 +16,6 @@ extern "C" {
 #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"
@@ -37,8 +36,10 @@ typedef struct __packed {
 
 typedef struct __packed {
   osThreadId_t thread_alert;
-
+  struct {
-  Protocol_DownPackage_t form_host;
+    uint8_t rx_buffer[sizeof(Protocol_DownPackage_t)]; 
+    Protocol_DownPackage_t data;
+  }form_host;
 
   struct {
     AI_UpPackageReferee_t ref;
@@ -46,6 +47,7 @@ typedef struct __packed {
   } to_host;
 
   CMD_AI_Status_t status;
+
   bool ai_online;
 } AI_t;
 
@@ -55,12 +57,11 @@ 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_ParseHost(AI_t *ai, CMD_Host_t *cmd_host);
 int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host);
-int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat, const AHRS_Eulr_t *gimbal_ai);
+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/can.h

@@ -200,7 +200,7 @@ typedef union {
 } CAN_GimbalMotor_t;
 
 typedef union {
-  CAN_MotorFeedback_t as_array[4];
+  CAN_MotorFeedback_t as_array[3];
   struct {
     CAN_MotorFeedback_t fric1;
     CAN_MotorFeedback_t fric2;

User/device/referee.h

@@ -23,9 +23,9 @@ extern "C" {
 #define REF_SWITCH_STATUS(ref, stat) ((ref).ref_status = (stat))
 #define CHASSIS_POWER_MAX_WITHOUT_REF 2000.0f /* 裁判系统离线底盘最大功率 */
 
-#define REF_UI_MAX_GRAPIC_NUM (7) /* 最大图形数量 */
+#define REF_UI_MAX_GRAPIC_NUM (7)
-#define REF_UI_MAX_STRING_NUM (7) /* 最大字符串数量 */
+#define REF_UI_MAX_STRING_NUM (7)
-#define REF_UI_MAX_DEL_NUM (3) /* 最大删除数量 */
+#define REF_UI_MAX_DEL_NUM (3)
 
 /* Exported types ----------------------------------------------------------- */
 typedef struct __packed {

User/module/config.c

@@ -138,12 +138,7 @@ static const Config_RobotParam_t param_default = {
       },
     }, /* pid */
 
-    .limit_and_imu = {
+    .pitch_travel_rad = 1.05f,
-      .yaw_travel_rad = -1.0f,
-      .pitch_travel_rad = 2.0f,
-      .yaw_imu = false,
-      .pit_imu = false,
-    },
     
     .low_pass_cutoff_freq = {
       .out = -1.0f,
@@ -155,6 +150,7 @@ static const Config_RobotParam_t param_default = {
       .pit = true,
     },
 
+    .pit_ctrl_reverse = false,
   }, /* gimbal */
 
   .shoot = { /* 射击模块参数 */
@@ -195,7 +191,7 @@ static const Config_RobotParam_t param_default = {
     .cover_close_duty = 0.050f,
     .model = SHOOT_MODEL_17MM,
     .bullet_speed = 30.f,
-    .min_shoot_delay = (uint32_t)(1000.0f / 5.0f),
+    .min_shoot_delay = (uint32_t)(1000.0f / 10.0f),
   }, /* shoot */
 
   .can = {
@@ -206,7 +202,11 @@ static const Config_RobotParam_t param_default = {
     }, /* can */
 }; /* param_default */
 
+#ifdef DEBUG
+Config_RobotParam_t param_hero = {
+#else
 static const Config_RobotParam_t param_hero = {
+#endif
   .model = ROBOT_MODEL_HERO,
 
   .chassis = { /* 底盘模块参数 */
@@ -240,7 +240,7 @@ static const Config_RobotParam_t param_hero = {
     },
 		
 		.reverse = {
-			.yaw = true,
+			.yaw = false,
 		},
   }, /* chassis */
 
@@ -289,12 +289,7 @@ static const Config_RobotParam_t param_hero = {
       },
     }, /* pid */
 
-		.limit_and_imu = {
+		.pitch_travel_rad = 0.9685447f,
-      .yaw_travel_rad = -1.0f,
-      .pitch_travel_rad = 1.0f,
-      .yaw_imu = true,
-      .pit_imu = true,
-    },
     
     .low_pass_cutoff_freq = {
       .out = -1.0f,
@@ -302,10 +297,11 @@ static const Config_RobotParam_t param_hero = {
     },
 
     .reverse = {
-      .yaw = true,
+      .yaw = false,
-      .pit = true,
+      .pit = false,
     },
 
+    .pit_ctrl_reverse = false,
   }, /* gimbal */
 
   .shoot = { /* 射击模块参数 */
@@ -339,8 +335,8 @@ static const Config_RobotParam_t param_hero = {
         .trig = -1.0f,
       },
     },
-    .num_trig_tooth = 6.0f,
+    .num_trig_tooth = 5.0f,
-    .trig_gear_ratio = 3591.0f / 187.0f,
+    .trig_gear_ratio = 19.0f,
     .fric_radius = 0.03f,
     .cover_open_duty = 0.125f,
     .cover_close_duty = 0.075f,
@@ -403,6 +399,26 @@ static const Config_RobotParam_t param_sentry_chassis = {
   .gimbal = { /* 云台模块参数 */
     .pid = {
       {
+//        /* GIMBAL_PID_YAW_OMEGA_IDX */
+//        .k = 0.25f,
+//        .p = 1.0f,
+//        .i = 1.0f,
+//        .d = 0.0f,
+//        .i_limit = 1.0f,
+//        .out_limit = 1.0f,
+//        .d_cutoff_freq = -1.0f,
+//        .range = -1.0f,
+//      }, {
+//        /* GIMBAL_PID_YAW_ANGLE_IDX */
+//        .k = 12.0f,
+//        .p = 1.0f,
+//        .i = 0.0f,
+//        .d = 0.05f,
+//        .i_limit = 0.0f,
+//        .out_limit = 10.0f,
+//        .d_cutoff_freq = -1.0f,
+//        .range = M_2PI,
+				/* GIMBAL_PID_YAW_OMEGA_IDX */
         .k = 0.24f,
         .p = 1.0f,
         .i = 0.5f,
@@ -422,6 +438,25 @@ static const Config_RobotParam_t param_sentry_chassis = {
         .d_cutoff_freq = -1.0f,
         .range = M_2PI,
       }, {
+//        /* GIMBAL_PID_PIT_OMEGA_IDX */
+//        .k = 0.35f,
+//        .p = 1.0f,
+//        .i = 0.f,
+//        .d = 0.003f,
+//        .i_limit = 1.0f,
+//        .out_limit = 1.0f,
+//        .d_cutoff_freq = -1.0f,
+//        .range = -1.0f,
+//      }, {
+//        /* GIMBAL_PID_PIT_ANGLE_IDX */
+//        .k = 15.0f,
+//        .p = 1.0f,
+//        .i = 0.0f,
+//        .d = 0.0f,
+//        .i_limit = 0.0f,
+//        .out_limit = 10.0f,
+//        .d_cutoff_freq = -1.0f,
+//        .range = M_2PI,
 				/* GIMBAL_PID_PIT_OMEGA_IDX */
         .k = 0.25f,
         .p = 1.0f,
@@ -444,12 +479,7 @@ static const Config_RobotParam_t param_sentry_chassis = {
       },
     }, /* pid */
 
-    .limit_and_imu = {
+    .pitch_travel_rad = 0.85f,
-      .yaw_travel_rad = -1.0f,
-      .pitch_travel_rad = 1.0f,
-      .yaw_imu = true,
-      .pit_imu = true,
-    },
     
     .low_pass_cutoff_freq = {
       .out = -1.0f,
@@ -461,6 +491,7 @@ static const Config_RobotParam_t param_sentry_chassis = {
       .pit = true,
     },
 
+    .pit_ctrl_reverse = true,
   }, /* gimbal */
 
   .shoot = { /* 射击模块参数 */
@@ -558,6 +589,25 @@ static const Config_RobotParam_t param_sentry_gimbal = {
   .gimbal = { /* 云台模块参数 */
     .pid = {
       {
+//        /* GIMBAL_PID_YAW_OMEGA_IDX */
+//        .k = 0.25f,
+//        .p = 1.0f,
+//        .i = 1.0f,
+//        .d = 0.0f,
+//        .i_limit = 1.0f,
+//        .out_limit = 1.0f,
+//        .d_cutoff_freq = -1.0f,
+//        .range = -1.0f,
+//      }, {
+//        /* GIMBAL_PID_YAW_ANGLE_IDX */
+//        .k = 12.0f,
+//        .p = 1.0f,
+//        .i = 0.0f,
+//        .d = 0.05f,
+//        .i_limit = 0.0f,
+//        .out_limit = 10.0f,
+//        .d_cutoff_freq = -1.0f,
+//        .range = M_2PI,
 				/* GIMBAL_PID_YAW_OMEGA_IDX */
         .k = 0.24f,
         .p = 1.0f,
@@ -578,6 +628,25 @@ static const Config_RobotParam_t param_sentry_gimbal = {
         .d_cutoff_freq = -1.0f,
         .range = M_2PI,
       }, {
+//        /* GIMBAL_PID_PIT_OMEGA_IDX */
+//        .k = 0.35f,
+//        .p = 1.0f,
+//        .i = 0.f,
+//        .d = 0.003f,
+//        .i_limit = 1.0f,
+//        .out_limit = 1.0f,
+//        .d_cutoff_freq = -1.0f,
+//        .range = -1.0f,
+//      }, {
+//        /* GIMBAL_PID_PIT_ANGLE_IDX */
+//        .k = 15.0f,
+//        .p = 1.0f,
+//        .i = 0.0f,
+//        .d = 0.0f,
+//        .i_limit = 0.0f,
+//        .out_limit = 10.0f,
+//        .d_cutoff_freq = -1.0f,
+//        .range = M_2PI,
 				/* GIMBAL_PID_PIT_OMEGA_IDX */
         .k = 0.25f,
         .p = 1.0f,
@@ -600,12 +669,7 @@ static const Config_RobotParam_t param_sentry_gimbal = {
       },
     }, /* pid */
 
-    .limit_and_imu = {
+    .pitch_travel_rad = 0.85f,
-      .yaw_travel_rad = -1.0f,
-      .pitch_travel_rad = 1.0f,
-      .yaw_imu = true,
-      .pit_imu = false,
-    },
     
     .low_pass_cutoff_freq = {
       .out = -1.0f,
@@ -617,6 +681,7 @@ static const Config_RobotParam_t param_sentry_gimbal = {
       .pit = true,
     },
 
+    .pit_ctrl_reverse = true,
   }, /* gimbal */
 
   .shoot = { /* 射击模块参数 */
@@ -657,7 +722,7 @@ static const Config_RobotParam_t param_sentry_gimbal = {
     .cover_close_duty = 0.050f,
     .model = SHOOT_MODEL_17MM,
     .bullet_speed = 25.f,
-    .min_shoot_delay = (uint32_t)(1000.0f / 1.0f),
+    .min_shoot_delay = (uint32_t)(1000.0f / 20.0f),
   }, /* shoot */
 
   .can = {

User/module/config.h

@@ -47,8 +47,8 @@ typedef struct {
   } cali; /* 校准 */
 
   AHRS_Eulr_t mech_zero; /* 机械零点 */
-  float gimbal_pitch_limit;    /* 云台pitch轴软件限位最高点 */
+  float gimbal_limit;    /* 云台pitch轴软件限位最高点 */
-  float gimbal_yaw_limit;      /* 云台yaw轴软件限位最高点 */
+  
 } Config_t;
 
 /* 机器人参数和对应字符串的映射  */

User/module/gimbal.c

@@ -25,10 +25,6 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, CMD_GimbalMode_t mode) {
   if (g == NULL) return -1;
   if (mode == g->mode) return GIMBAL_OK;
 
-  /* 获取当前反馈值，防止位置突变 */
-  g->setpoint.eulr.yaw = g->feedback.eulr.encoder.yaw;
-  g->setpoint.eulr.pit = g->feedback.eulr.encoder.pit;
-
   /* 切换模式后重置PID和滤波器 */
   for (uint8_t i = 0; i < GIMBAL_PID_NUM; i++) {
     PID_Reset(g->pid + i);
@@ -40,15 +36,9 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, CMD_GimbalMode_t mode) {
   AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */
   if (g->mode == GIMBAL_MODE_RELAX) {
     if (mode == GIMBAL_MODE_ABSOLUTE) {
-      if (g->param->limit_and_imu.yaw_imu) {
       g->setpoint.eulr.yaw = g->feedback.eulr.imu.yaw;
-      } else {
-        g->setpoint.eulr.yaw = g->feedback.eulr.encoder.yaw;
-      }
     } else if (mode == GIMBAL_MODE_RELATIVE) {
       g->setpoint.eulr.yaw = g->feedback.eulr.encoder.yaw;
-    } else if (mode == GIMBAL_MODE_SEARCH) {
-      g->setpoint.eulr.yaw = g->feedback.eulr.encoder.yaw;
     }
   }
 
@@ -76,12 +66,9 @@ int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param, float limit_max,
 
   /* 设置软件限位 */
   if (g->param->reverse.pit) CircleReverse(&limit_max);
-  g->limit.pit_min = g->limit.pit_max = limit_max;
+  g->limit.min = g->limit.max = limit_max;
-  CircleAdd(&(g->limit.pit_min), -g->param->limit_and_imu.pitch_travel_rad, M_2PI);
+  CircleAdd(&(g->limit.min), -g->param->pitch_travel_rad, M_2PI);
 
-  if (g->param->reverse.yaw) CircleReverse(&limit_max);
-  g->limit.yaw_min = g->limit.yaw_max = limit_max;
-  CircleAdd(&(g->limit.yaw_min), -g->param->limit_and_imu.yaw_travel_rad, M_2PI);
   /* 初始化云台电机控制PID和LPF */
   PID_Init(&(g->pid[GIMBAL_PID_YAW_ANGLE_IDX]), KPID_MODE_NO_D, target_freq,
            &(g->param->pid[GIMBAL_PID_YAW_ANGLE_IDX]));
@@ -143,80 +130,38 @@ int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now) {
   Gimbal_SetMode(g, g_cmd->mode);
 
   /* yaw坐标正方向与遥控器操作逻辑相反 */
-  g_cmd->delta_eulr.pit = -g_cmd->delta_eulr.pit;
+  g_cmd->delta_eulr.pit = g_cmd->delta_eulr.pit;
+  if (g->param->pit_ctrl_reverse){
+    g_cmd->delta_eulr.yaw = -g_cmd->delta_eulr.yaw;
+  }else{
     g_cmd->delta_eulr.yaw = -g_cmd->delta_eulr.yaw;
-
-  // /* 处理yaw控制命令 */
-  // CircleAdd(&(g->setpoint.eulr.yaw), g_cmd->delta_eulr.yaw, M_2PI);
-
-  if (g->param->limit_and_imu.yaw_travel_rad > 0.0f) {
-    /* 处理yaw控制命令，软件限位 */
-    float delta_yaw_max;
-    float delta_yaw_min;
-
-    if (g->param->limit_and_imu.yaw_imu)
-    {
-      delta_yaw_max =
-        CircleError(g->limit.yaw_max,
-                    (g->feedback.eulr.encoder.yaw + g->setpoint.eulr.yaw -
-                    g->feedback.eulr.imu.yaw),
-                    M_2PI);
-      delta_yaw_min =
-        CircleError(g->limit.yaw_min,
-                    (g->feedback.eulr.encoder.yaw + g->setpoint.eulr.yaw -
-                    g->feedback.eulr.imu.yaw),
-                    M_2PI);
-    }else
-    {
-      delta_yaw_max =
-        CircleError(g->limit.yaw_max,
-                    (g->feedback.eulr.encoder.yaw + g->setpoint.eulr.yaw -
-                    g->feedback.eulr.encoder.yaw),
-                    M_2PI);
-      delta_yaw_min =
-        CircleError(g->limit.yaw_min,
-                    (g->feedback.eulr.encoder.yaw + g->setpoint.eulr.yaw -
-                    g->feedback.eulr.encoder.yaw),
-                    M_2PI);
   }
 
-    Clip(&(g_cmd->delta_eulr.yaw), delta_yaw_min, delta_yaw_max);
+  /* 处理yaw控制命令 */
-  }else (CircleAdd(&(g->setpoint.eulr.yaw), g_cmd->delta_eulr.yaw, M_2PI));
+  CircleAdd(&(g->setpoint.eulr.yaw), g_cmd->delta_eulr.yaw, M_2PI);
 
-  g->setpoint.eulr.yaw += g_cmd->delta_eulr.yaw;
-  if (g->param->limit_and_imu.pitch_travel_rad > 0.0f) {
   /* 处理pitch控制命令，软件限位 */
-    float delta_pitch_max;
+  const float delta_max =
-    float delta_pitch_min;
+      CircleError(g->limit.max,
-
-    if (g->param->limit_and_imu.pit_imu)
-    {
-      delta_pitch_max =
-        CircleError(g->limit.pit_max,
                   (g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
                    g->feedback.eulr.imu.pit),
                   M_2PI);
-      delta_pitch_min =
+  const float delta_min =
-        CircleError(g->limit.pit_min,
+      CircleError(g->limit.min,
                   (g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
                    g->feedback.eulr.imu.pit),
                   M_2PI);
-    }else
+  // const float delta_max =
-    {
+  //     CircleError(g->limit.max,
-      delta_pitch_max =
+  //                 (g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
-        CircleError(g->limit.pit_max,
+  //                  g->feedback.eulr.encoder.pit),
-                    (g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
+  //                 M_2PI);
-                    g->feedback.eulr.encoder.pit),
+  // const float delta_min =
-                    M_2PI);
+  //     CircleError(g->limit.min,
-      delta_pitch_min =
+  //                 (g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
-        CircleError(g->limit.pit_min,
+  //                  g->feedback.eulr.encoder.pit),
-                    (g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
+  //                 M_2PI);
-                    g->feedback.eulr.encoder.pit),
+  Clip(&(g_cmd->delta_eulr.pit), delta_min, delta_max);
-                    M_2PI);
-    }                  
-    Clip(&(g_cmd->delta_eulr.pit), delta_pitch_min, delta_pitch_max);
-  }else (CircleAdd(&(g->setpoint.eulr.pit), g_cmd->delta_eulr.pit, M_2PI));
-
   g->setpoint.eulr.pit += g_cmd->delta_eulr.pit;
 
   /* 重置输入指令，防止重复处理 */
@@ -229,11 +174,7 @@ int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now) {
       for (uint8_t i = 0; i < GIMBAL_ACTR_NUM; i++) g->out[i] = 0.0f;
       break;
 
-    case GIMBAL_MODE_SEARCH:
-      g->setpoint.eulr.pit = g->limit.pit_min + (g->limit.pit_max - g->limit.pit_min) * (sinf(now / 100.0f) + 1.0f) / 2.0f; // 在min和max之间摆动
-      g->setpoint.eulr.yaw = g->limit.yaw_min + (g->limit.yaw_max - g->limit.yaw_min) * (sinf(now / 100.0f) + 1.0f) / 2.0f; // 在min和max之间摆动
     case GIMBAL_MODE_ABSOLUTE:
-      if (g->param->limit_and_imu.yaw_imu && g->param->limit_and_imu.pit_imu){
       yaw_omega_set_point =
           PID_Calc(&(g->pid[GIMBAL_PID_YAW_ANGLE_IDX]), g->setpoint.eulr.yaw,
                    g->feedback.eulr.imu.yaw, 0.0f, g->dt);
@@ -244,27 +185,17 @@ int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now) {
       pit_omega_set_point =
           PID_Calc(&(g->pid[GIMBAL_PID_PIT_ANGLE_IDX]), g->setpoint.eulr.pit,
                    g->feedback.eulr.imu.pit, 0.0f, g->dt);
+      // pit_omega_set_point =
+      //     PID_Calc(&(g->pid[GIMBAL_PID_PIT_ANGLE_IDX]), g->setpoint.eulr.pit,
+      //              g->feedback.eulr.encoder.pit, 0.0f, g->dt);
            
       g->out[GIMBAL_ACTR_PIT_IDX] =
           PID_Calc(&(g->pid[GIMBAL_PID_PIT_OMEGA_IDX]), pit_omega_set_point,
-                    0.0, 0.f, g->dt);
+                   g->feedback.gyro.x, 0.f, g->dt);
-      }else{
-        yaw_omega_set_point =
-            PID_Calc(&(g->pid[GIMBAL_PID_YAW_ANGLE_IDX]), g->setpoint.eulr.yaw,
-                    g->feedback.eulr.encoder.yaw, 0.0f, g->dt);
-        g->out[GIMBAL_ACTR_YAW_IDX] =
-            PID_Calc(&(g->pid[GIMBAL_PID_YAW_OMEGA_IDX]), yaw_omega_set_point,
-                    g->feedback.gyro.z, 0.f, g->dt);
-
-        pit_omega_set_point =
-            PID_Calc(&(g->pid[GIMBAL_PID_PIT_ANGLE_IDX]), g->setpoint.eulr.pit,
-                    g->feedback.eulr.encoder.pit, 0.0f, g->dt);
-            
-        g->out[GIMBAL_ACTR_PIT_IDX] =
-            PID_Calc(&(g->pid[GIMBAL_PID_PIT_OMEGA_IDX]), pit_omega_set_point,
-                    0.0, 0.f, g->dt);
-      }
       
+      // g->out[GIMBAL_ACTR_PIT_IDX] =
+      //     PID_Calc(&(g->pid[GIMBAL_PID_PIT_OMEGA_IDX]), pit_omega_set_point,
+      //              0.0, 0.f, g->dt);
       break;
 
     case GIMBAL_MODE_RELATIVE:
@@ -292,13 +223,6 @@ int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now) {
   return 0;
 }
 
-
-uint8_t Gimbal_PackAI(Gimbal_t *gimbal, const AHRS_Eulr_t *eulr_gimbal_for_ai) {
-  memcpy((void *)eulr_gimbal_for_ai, (const void *)&(gimbal->feedback.eulr.encoder),
-         sizeof(gimbal->feedback.eulr.encoder));
-  return 0;
-}
-
 /**
  * \brief 复制云台输出值
  *

User/module/gimbal.h

@@ -52,13 +52,7 @@ typedef struct {
     float gyro; /* 陀螺仪数据 */
   } low_pass_cutoff_freq;
 
-  /* 设置云台限位和是否启用imu */
-  struct {
-    float yaw_travel_rad;  /* 云台yaw轴行程弧度 */
   float pitch_travel_rad; /* 云台pitch轴行程弧度 */
-    bool yaw_imu;
-    bool pit_imu;
-  } limit_and_imu;
 
   /* 设置默认运动方向 */
   struct {
@@ -66,14 +60,14 @@ typedef struct {
     bool pit;
   } reverse;
 
+  bool pit_ctrl_reverse; /* 云台pitch遥控方向 */
+  
 } Gimbal_Params_t;
 
 /* 软件限位 */
 typedef struct {
-  float yaw_max;
+  float max;
-  float yaw_min;
+  float min;
-  float pit_max;
-  float pit_min;
 } Gimbal_Limit_t;
 
 /* 云台反馈数据的结构体，包含反馈控制用的反馈数据 */
@@ -141,9 +135,6 @@ int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param, float limit,
  */
 int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal, const CAN_t *can);
 
-uint8_t Gimbal_PackAI(Gimbal_t *gimbal, const AHRS_Eulr_t *eulr_gimbal_for_ai);
-
-
 /**
  * \brief 运行云台控制逻辑
  *
@@ -156,9 +147,6 @@ uint8_t Gimbal_PackAI(Gimbal_t *gimbal, const AHRS_Eulr_t *eulr_gimbal_for_ai);
  */
 int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now);
 
-uint8_t Gimbal_PackAI(Gimbal_t *gimbal, const AHRS_Eulr_t *eulr_gimbal_for_ai);
-
-
 /**
  * \brief 复制云台输出值
  *

User/module/shoot.c

@@ -252,9 +252,9 @@ int8_t Shoot_Control(Shoot_t *s, CMD_ShootCmd_t *s_cmd,
 
   float fric_rpm = CalculateRpm(s->fire_ctrl.bullet_speed, s->param->fric_radius,
                                 (s->param->model == SHOOT_MODEL_17MM));
-  s->setpoint.fric_rpm[0] = fric_rpm;
+  s->setpoint.fric_rpm[0] = -fric_rpm;
-  s->setpoint.fric_rpm[1] = -fric_rpm;
+  s->setpoint.fric_rpm[1] = fric_rpm;
-  s->setpoint.fric_rpm[2] = fric_rpm;
+  s->setpoint.fric_rpm[2] = -fric_rpm;
 
   /* 计算拨弹电机位置的目标值 */
   if (((now - s->fire_ctrl.last_shoot) >= s->fire_ctrl.period_ms) &&

User/task/ai.c

@@ -17,13 +17,11 @@ AI_t ai;
 CMD_Host_t cmd_host;
 AHRS_Quaternion_t quat;
 Referee_ForAI_t referee_ai;
-AHRS_Eulr_t gimbal_ai;
 #else
 static AI_t ai;
 static CMD_Host_t cmd_host;
 static AHRS_Quaternion_t quat;
 static Referee_ForAI_t referee_ai;
-static AHRS_Eulr_t gimbal_ai;
 #endif
 
 /* Private function --------------------------------------------------------- */
@@ -44,7 +42,9 @@ void Task_Ai(void *argument) {
   AI_Init(&ai);
 
   uint32_t tick = osKernelGetTickCount();
+
   uint32_t last_online_tick = tick;
+
   while (1) {
 #ifdef DEBUG
     task_runtime.stack_water_mark.ai = osThreadGetStackSpace(osThreadGetId());
@@ -54,25 +54,19 @@ void Task_Ai(void *argument) {
 
     AI_StartReceiving(&ai);
     if (AI_WaitDmaCplt()) {
-      AI_ParseHost(&ai);
+      AI_ParseHost(&ai, &cmd_host);
       last_online_tick = tick;
     } else {
       if (tick - last_online_tick > 300) AI_HandleOffline(&ai, &cmd_host);
     }
-
-    if (ai.status != AI_STATUS_STOP && ai.ai_online){
-      AI_PackCmd(&ai, &cmd_host);
     osMessageQueueReset(task_runtime.msgq.cmd.raw.host);
     osMessageQueuePut(task_runtime.msgq.cmd.raw.host, &(cmd_host), 0, 0);
-    }
 
     osMessageQueueGet(task_runtime.msgq.ai.quat, &(quat), NULL, 0);
     osMessageQueueGet(task_runtime.msgq.cmd.ai, &(ai.status), NULL, 0);
-    osMessageQueueGet(task_runtime.msgq.gimbal.eulr_encoder, &(gimbal_ai), NULL, 0);
     bool ref_update = (osMessageQueueGet(task_runtime.msgq.referee.ai,
                                          &(referee_ai), NULL, 0) == osOK);
-    
+    AI_PackMCU(&ai, &quat);
-    AI_PackMCU(&ai, &quat, &gimbal_ai);
     if (ref_update) AI_PackRef(&ai, &(referee_ai));
 
     AI_StartSend(&(ai), ref_update);

User/task/atti_esti.c

@@ -126,10 +126,10 @@ void Task_AttiEsti(void *argument) {
     osMessageQueuePut(task_runtime.msgq.gimbal.accl, &bmi088.accl, 0, 0);
     osMessageQueueReset(task_runtime.msgq.gimbal.eulr_imu);
     osMessageQueuePut(task_runtime.msgq.gimbal.eulr_imu, &eulr_to_send, 0, 0);
-    osMessageQueueReset(task_runtime.msgq.gimbal.gyro);
-    osMessageQueuePut(task_runtime.msgq.gimbal.gyro, &bmi088.gyro, 0, 0);
     osMessageQueueReset(task_runtime.msgq.ai.quat);
     osMessageQueuePut(task_runtime.msgq.ai.quat, &(gimbal_ahrs.quat), 0, 0);
+    osMessageQueueReset(task_runtime.msgq.gimbal.gyro);
+    osMessageQueuePut(task_runtime.msgq.gimbal.gyro, &bmi088.gyro, 0, 0);
 
     /* PID控制IMU温度，PWM输出 */
     BSP_PWM_Set(BSP_PWM_IMU_HEAT,

User/task/cli.c

@@ -523,7 +523,7 @@ static BaseType_t Command_SetGimbalLim(char *out_buffer, size_t len,
   switch (fsm.stage) {
     case 0:
       Config_Get(&cfg);
-      snprintf(out_buffer, len, "(old)pit_limit:%f,(old)yaw_limit:%f\r\n", cfg.gimbal_pitch_limit, cfg.gimbal_yaw_limit);
+      snprintf(out_buffer, len, "(old)limit:%f\r\n", cfg.gimbal_limit);
       fsm.stage = 1;
       return pdPASS;
     case 1:
@@ -538,12 +538,12 @@ static BaseType_t Command_SetGimbalLim(char *out_buffer, size_t len,
       }
       Config_Get(&cfg);
       osThreadResume(task_runtime.thread.ctrl_gimbal);
-      cfg.gimbal_pitch_limit = can.motor.gimbal.named.pit.rotor_angle;
+      cfg.gimbal_limit = can.motor.gimbal.named.pit.rotor_angle;
-      cfg.gimbal_yaw_limit = can.motor.gimbal.named.yaw.rotor_angle;
 
       Config_Set(&cfg);
       Config_Get(&cfg);
-      snprintf(out_buffer, len, "(new)limit_pitch:%f,limit_yaw%f\r\nDone.", cfg.gimbal_pitch_limit, cfg.gimbal_yaw_limit);
+      snprintf(out_buffer, len, "(new)limit:%f\r\nDone.", cfg.gimbal_limit);
+
       fsm.stage = 2;
       return pdPASS;
     case 2:
@@ -692,7 +692,7 @@ void Task_CLI(void *argument) {
   BSP_USB_Printf(CLI_WELCOME_MESSAGE);
 
   /* 开始运行命令行界面 */
-  BSP_USB_Printf("qut-move-rm>");
+  BSP_USB_Printf("rm>");
   while (1) {
 #ifdef DEBUG
     /* 记录任务所使用的的栈空间 */
@@ -730,7 +730,7 @@ void Task_CLI(void *argument) {
           index = 0; /* 重置索引，准备接收下一段命令 */
           memset(input, 0x00, strlen(input)); /* 清空输入缓存 */
         }
-        BSP_USB_Printf("qut-move-rm>");
+        BSP_USB_Printf("rm>");
       } else if (rx_char == '\b' || rx_char == 0x7Fu) {
         /* 如果输入的是退格键则清空一位输入缓存，同时进行界限保护 */
         if (index > 0) {

User/task/ctrl_gimbal.c

@@ -22,13 +22,11 @@ CMD_GimbalCmd_t gimbal_cmd;
 Gimbal_t gimbal;
 CAN_GimbalOutput_t gimbal_out;
 Referee_GimbalUI_t gimbal_ui;
-AHRS_Eulr_t eulr_gimbal_for_ai;
 #else
 static CMD_GimbalCmd_t gimbal_cmd;
 static Gimbal_t gimbal;
 static CAN_GimbalOutput_t gimbal_out;
 static Referee_GimbalUI_t gimbal_ui;
-static AHRS_Eulr_t eulr_gimbal_for_ai;
 #endif
 
 /* Private function --------------------------------------------------------- */
@@ -45,7 +43,7 @@ void Task_CtrlGimbal(void *argument) {
   const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CTRL_GIMBAL;
   /* 初始化云台 */
   Gimbal_Init(&gimbal, &(task_runtime.cfg.robot_param->gimbal),
-              task_runtime.cfg.gimbal_pitch_limit, (float)TASK_FREQ_CTRL_GIMBAL);
+              task_runtime.cfg.gimbal_limit, (float)TASK_FREQ_CTRL_GIMBAL);
 
   /* 延时一段时间再开启任务 */
   osMessageQueueGet(task_runtime.msgq.can.feedback.gimbal, &can, NULL,
@@ -68,15 +66,12 @@ void Task_CtrlGimbal(void *argument) {
 
     osKernelLock(); /* 锁住RTOS内核防止控制过程中断，造成错误 */
     Gimbal_UpdateFeedback(&gimbal, &can);
-    Gimbal_PackAI(&gimbal, &eulr_gimbal_for_ai);
     Gimbal_Control(&gimbal, &gimbal_cmd, tick);
     Gimbal_DumpOutput(&gimbal, &gimbal_out);
 
     osKernelUnlock();
     osMessageQueueReset(task_runtime.msgq.can.output.gimbal);
     osMessageQueuePut(task_runtime.msgq.can.output.gimbal, &gimbal_out, 0, 0);
-    osMessageQueueReset(task_runtime.msgq.gimbal.eulr_encoder);
-    osMessageQueuePut(task_runtime.msgq.gimbal.eulr_encoder, &eulr_gimbal_for_ai, 0, 0);
 
     Gimbal_DumpUI(&gimbal, &gimbal_ui);
     osMessageQueueReset(task_runtime.msgq.ui.gimbal);

User/task/init.c

@@ -30,8 +30,7 @@
  *
  * \param argument 未使用
  */
-void Task_Init(void *argument)
+void Task_Init(void *argument) {
-{
   (void)argument; /* 未使用argument，消除警告 */
 
   Config_Get(&task_runtime.cfg); /* 获取机器人配置 */
@@ -97,8 +96,7 @@ void Task_Init(void *argument)
       osMessageQueueNew(2u, sizeof(AHRS_Eulr_t), NULL);
   task_runtime.msgq.gimbal.gyro =
       osMessageQueueNew(2u, sizeof(AHRS_Gyro_t), NULL);
-    task_runtime.msgq.gimbal.eulr_encoder =
+
-        osMessageQueueNew(2u, sizeof(AHRS_Eulr_t), NULL);
   task_runtime.msgq.cap_info =
       osMessageQueueNew(2u, sizeof(CAN_Capacitor_t), NULL);
 

User/task/user_task.h

@@ -58,8 +58,6 @@ typedef struct {
       osMessageQueueId_t accl;     /* IMU读取 */
       osMessageQueueId_t gyro;     /* IMU读取 */
       osMessageQueueId_t eulr_imu; /* 姿态解算得到 */
-      osMessageQueueId_t eulr_encoder; /* 读编码器得到 */
-      
     } gimbal;
 
     /* 控制指令 */