左头自瞄差不多了

添加云台数据
上位机通讯正常
2025-03-05 15:02:44 +08:00 · 2025-03-05 10:29:39 +08:00 · 2025-03-04 17:26:45 +08:00 · 2025-03-04 17:20:57 +08:00
24 changed files with 10520 additions and 7233 deletions
--- a/.vscode/c_cpp_properties.json
+++ b/.vscode/c_cpp_properties.json
@ -0,0 +1,18 @@
+{
+  "configurations": [
+    {
+      "name": "windows-gcc-x64",
+      "includePath": [
+        "${workspaceFolder}/**"
+      ],
+      "compilerPath": "gcc",
+      "cStandard": "${default}",
+      "cppStandard": "${default}",
+      "intelliSenseMode": "windows-gcc-x64",
+      "compilerArgs": [
+        ""
+      ]
+    }
+  ],
+  "version": 4
+}
--- a/.vscode/launch.json
+++ b/.vscode/launch.json
@ -0,0 +1,24 @@
+{
+  "version": "0.2.0",
+  "configurations": [
+    {
+      "name": "C/C++ Runner: Debug Session",
+      "type": "cppdbg",
+      "request": "launch",
+      "args": [],
+      "stopAtEntry": false,
+      "externalConsole": true,
+      "cwd": "c:/Users/lvzucheng/Documents/XRobot/User/device",
+      "program": "c:/Users/lvzucheng/Documents/XRobot/User/device/build/Debug/outDebug",
+      "MIMode": "gdb",
+      "miDebuggerPath": "gdb",
+      "setupCommands": [
+        {
+          "description": "Enable pretty-printing for gdb",
+          "text": "-enable-pretty-printing",
+          "ignoreFailures": true
+        }
+      ]
+    }
+  ]
+}
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@ -0,0 +1,59 @@
+{
+  "C_Cpp_Runner.cCompilerPath": "gcc",
+  "C_Cpp_Runner.cppCompilerPath": "g++",
+  "C_Cpp_Runner.debuggerPath": "gdb",
+  "C_Cpp_Runner.cStandard": "",
+  "C_Cpp_Runner.cppStandard": "",
+  "C_Cpp_Runner.msvcBatchPath": "C:/Program Files/Microsoft Visual Studio/VR_NR/Community/VC/Auxiliary/Build/vcvarsall.bat",
+  "C_Cpp_Runner.useMsvc": false,
+  "C_Cpp_Runner.warnings": [
+    "-Wall",
+    "-Wextra",
+    "-Wpedantic",
+    "-Wshadow",
+    "-Wformat=2",
+    "-Wcast-align",
+    "-Wconversion",
+    "-Wsign-conversion",
+    "-Wnull-dereference"
+  ],
+  "C_Cpp_Runner.msvcWarnings": [
+    "/W4",
+    "/permissive-",
+    "/w14242",
+    "/w14287",
+    "/w14296",
+    "/w14311",
+    "/w14826",
+    "/w44062",
+    "/w44242",
+    "/w14905",
+    "/w14906",
+    "/w14263",
+    "/w44265",
+    "/w14928"
+  ],
+  "C_Cpp_Runner.enableWarnings": true,
+  "C_Cpp_Runner.warningsAsError": false,
+  "C_Cpp_Runner.compilerArgs": [],
+  "C_Cpp_Runner.linkerArgs": [],
+  "C_Cpp_Runner.includePaths": [],
+  "C_Cpp_Runner.includeSearch": [
+    "*",
+    "**/*"
+  ],
+  "C_Cpp_Runner.excludeSearch": [
+    "**/build",
+    "**/build/**",
+    "**/.*",
+    "**/.*/**",
+    "**/.vscode",
+    "**/.vscode/**"
+  ],
+  "C_Cpp_Runner.useAddressSanitizer": false,
+  "C_Cpp_Runner.useUndefinedSanitizer": false,
+  "C_Cpp_Runner.useLeakSanitizer": false,
+  "C_Cpp_Runner.showCompilationTime": false,
+  "C_Cpp_Runner.useLinkTimeOptimization": false,
+  "C_Cpp_Runner.msvcSecureNoWarnings": false
+}
--- a/MDK-ARM/DevC.uvoptx
+++ b/MDK-ARM/DevC.uvoptx
@ -10,7 +10,7 @@
    <aExt>*.s*; *.src; *.a*</aExt>
    <oExt>*.obj; *.o</oExt>
    <lExt>*.lib</lExt>
-    <tExt>*.txt; *.h; *.inc</tExt>
+    <tExt>*.txt; *.h; *.inc; *.md</tExt>
    <pExt>*.plm</pExt>
    <CppX>*.cpp</CppX>
    <nMigrate>0</nMigrate>
@ -387,6 +387,36 @@
          <WinNumber>1</WinNumber>
          <ItemText>ref</ItemText>
        </Ww>
+        <Ww>
+          <count>6</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>shoot</ItemText>
+        </Ww>
+        <Ww>
+          <count>7</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>ai</ItemText>
+        </Ww>
+        <Ww>
+          <count>8</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>for_chassis</ItemText>
+        </Ww>
+        <Ww>
+          <count>9</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>for_shoot</ItemText>
+        </Ww>
+        <Ww>
+          <count>10</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>cmd</ItemText>
+        </Ww>
+        <Ww>
+          <count>11</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>host</ItemText>
+        </Ww>
      </WatchWindow1>
      <Tracepoint>
        <THDelay>0</THDelay>
@ -1786,7 +1816,7 @@
      <GroupNumber>12</GroupNumber>
      <FileNumber>105</FileNumber>
      <FileType>1</FileType>
-      <tvExp>1</tvExp>
+      <tvExp>0</tvExp>
      <tvExpOptDlg>0</tvExpOptDlg>
      <bDave2>0</bDave2>
      <PathWithFileName>..\User\module\shoot.c</PathWithFileName>
--- a/MDK-ARM/DevC.uvprojx
+++ b/MDK-ARM/DevC.uvprojx
@ -16,7 +16,7 @@
        <TargetCommonOption>
          <Device>STM32F407IGHx</Device>
          <Vendor>STMicroelectronics</Vendor>
-          <PackID>Keil.STM32F4xx_DFP.2.14.0</PackID>
+          <PackID>Keil.STM32F4xx_DFP.2.15.0</PackID>
          <PackURL>http://www.keil.com/pack/</PackURL>
          <Cpu>IRAM(0x20000000-0x2001FFFF) IRAM2(0x10000000-0x1000FFFF) IROM(0x8000000-0x80FFFFF)  CLOCK(25000000) FPU2 CPUTYPE("Cortex-M4")</Cpu>
          <FlashUtilSpec></FlashUtilSpec>
@ -1726,13 +1726,13 @@
      <TargetName>Debug</TargetName>
      <ToolsetNumber>0x4</ToolsetNumber>
      <ToolsetName>ARM-ADS</ToolsetName>
-      <pCCUsed>6140000::V6.14::ARMCLANG</pCCUsed>
+      <pCCUsed>6160000::V6.16::ARMCLANG</pCCUsed>
      <uAC6>1</uAC6>
      <TargetOption>
        <TargetCommonOption>
          <Device>STM32F407IGHx</Device>
          <Vendor>STMicroelectronics</Vendor>
-          <PackID>Keil.STM32F4xx_DFP.2.14.0</PackID>
+          <PackID>Keil.STM32F4xx_DFP.2.15.0</PackID>
          <PackURL>http://www.keil.com/pack/</PackURL>
          <Cpu>IRAM(0x20000000-0x2001FFFF) IRAM2(0x10000000-0x1000FFFF) IROM(0x8000000-0x80FFFFF)  CLOCK(25000000) FPU2 CPUTYPE("Cortex-M4")</Cpu>
          <FlashUtilSpec></FlashUtilSpec>
@ -2044,7 +2044,7 @@
            <uC99>0</uC99>
            <uGnu>0</uGnu>
            <useXO>0</useXO>
-            <v6Lang>6</v6Lang>
+            <v6Lang>5</v6Lang>
            <v6LangP>3</v6LangP>
            <vShortEn>1</vShortEn>
            <vShortWch>1</vShortWch>
@ -3443,11 +3443,6 @@
    <Layers>
      <Layer>
        <LayName>DevC</LayName>
-        <LayDesc></LayDesc>
-        <LayUrl></LayUrl>
-        <LayKeys></LayKeys>
-        <LayCat></LayCat>
-        <LayLic></LayLic>
        <LayTarg>0</LayTarg>
        <LayPrjMark>1</LayPrjMark>
      </Layer>
--- a/MDK-ARM/DevC/DevC.axf
+++ b/MDK-ARM/DevC/DevC.axf
--- a/MDK-ARM/DevC/DevC.hex
+++ b/MDK-ARM/DevC/DevC.hex
--- a/Middlewares/Third_Party/Protocol/protocol.h
+++ b/Middlewares/Third_Party/Protocol/protocol.h
@ -7,7 +7,8 @@
 #include <stdint.h>

 #ifdef __cplusplus
-extern "C" {
+extern "C"
+{
 #endif

 #define AI_NOTICE_AUTOAIM (1 << 0)
@ -24,60 +25,64 @@ extern "C" {
  typedef uint8_t Protocol_ID_t;

  /* 电控 -> 视觉 MCU数据结构体*/
-typedef struct __attribute__((packed)) {
-  struct __attribute__((packed)) {
+  typedef struct __attribute__((packed))
+  {
+    struct __attribute__((packed))
+    {
      float q0;
      float q1;
      float q2;
      float q3;
    } quat; /* 四元数 */
-
+    struct __attribute__((packed))
+    {
+      float yaw;
+      float pit;
+      float rol;
+    } gimbal;       /* 欧拉角 */
    uint8_t notice; /* 控制命令 */
-
-  float ball_speed; /* 子弹初速度 */
-
-  struct __attribute__((packed)) {
-    float left;
-    float right;
-  } distance; /* 左右距离(哨兵) */
-
-  float chassis_speed; /* 底盘速度(哨兵) */
  } Protocol_UpDataMCU_t;

  /* 电控 -> 视觉 裁判系统数据结构体*/
-typedef struct __attribute__((packed)) {
+  typedef struct __attribute__((packed))
+  {
    uint16_t team; /* 本身队伍 */
    uint16_t time; /* 比赛开始时间 */
  } Protocol_UpDataReferee_t;

  /* 视觉 -> 电控 数据结构体*/
-typedef struct __attribute__((packed)) {
-  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)) {
+    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)) {
+  typedef struct __attribute__((packed))
+  {
    Protocol_UpDataMCU_t data;
    uint16_t crc16;
  } Protocol_UpPackageMCU_t;

-typedef struct __attribute__((packed)) {
+  typedef struct __attribute__((packed))
+  {
    Protocol_UpDataReferee_t data;
    uint16_t crc16;
  } Protocol_UpPackageReferee_t;

-typedef struct __attribute__((packed)) {
+  typedef struct __attribute__((packed))
+  {
    Protocol_DownData_t data;
    uint16_t crc16;
  } Protocol_DownPackage_t;
--- a/User/component/cmd.c
+++ b/User/component/cmd.c
@ -14,12 +14,14 @@
 * @return uint16_t 行为对应的按键
 */
 static inline CMD_KeyValue_t CMD_BehaviorToKey(CMD_t *cmd,
-                                               CMD_Behavior_t behavior) {
+                                               CMD_Behavior_t behavior)
+{
  return cmd->param->map.key_map[behavior].key;
 }

 static inline CMD_ActiveType_t CMD_BehaviorToActive(CMD_t *cmd,
-                                                    CMD_Behavior_t behavior) {
+                                                    CMD_Behavior_t behavior)
+{
  return cmd->param->map.key_map[behavior].active;
 }

@ -32,37 +34,47 @@ static inline CMD_ActiveType_t CMD_BehaviorToActive(CMD_t *cmd,
 * @return true 按下
 * @return false 未按下
 */
-static bool CMD_KeyPressedRc(const CMD_RC_t *rc, CMD_KeyValue_t key) {
+static bool CMD_KeyPressedRc(const CMD_RC_t *rc, CMD_KeyValue_t key)
+{
  /* 按下按键为鼠标左、右键 */
-  if (key == CMD_L_CLICK) {
+  if (key == CMD_L_CLICK)
+  {
    return rc->mouse.l_click;
  }
-  if (key == CMD_R_CLICK) {
+  if (key == CMD_R_CLICK)
+  {
    return rc->mouse.r_click;
  }
  return rc->key & (1u << key);
 }

 static bool CMD_BehaviorOccurredRc(const CMD_RC_t *rc, CMD_t *cmd,
-                                   CMD_Behavior_t behavior) {
+                                   CMD_Behavior_t behavior)
+{
  CMD_KeyValue_t key = CMD_BehaviorToKey(cmd, behavior);
  CMD_ActiveType_t active = CMD_BehaviorToActive(cmd, behavior);

  bool now_key_pressed, last_key_pressed;

  /* 按下按键为鼠标左、右键 */
-  if (key == CMD_L_CLICK) {
+  if (key == CMD_L_CLICK)
+  {
    now_key_pressed = rc->mouse.l_click;
    last_key_pressed = cmd->mouse_last.l_click;
-  } else if (key == CMD_R_CLICK) {
+  }
+  else if (key == CMD_R_CLICK)
+  {
    now_key_pressed = rc->mouse.r_click;
    last_key_pressed = cmd->mouse_last.r_click;
-  } else {
+  }
+  else
+  {
    now_key_pressed = rc->key & (1u << key);
    last_key_pressed = cmd->key_last & (1u << key);
  }

-  switch (active) {
+  switch (active)
+  {
  case CMD_ACTIVE_PRESSING:
    return now_key_pressed && !last_key_pressed;
  case CMD_ACTIVE_RASING:
@ -79,7 +91,8 @@ static bool CMD_BehaviorOccurredRc(const CMD_RC_t *rc, CMD_t *cmd,
 * @param cmd 主结构体
 * @param dt_sec 两次解析的间隔
 */
-static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
+static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec)
+{
  cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;

  /* 云台设置为鼠标控制欧拉角的变化，底盘的控制向量设置为零 */
@ -91,85 +104,111 @@ static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
  cmd->shoot.reverse_trig = false;

  /* 按键行为映射相关逻辑 */
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FORE)) {
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FORE))
+  {
    cmd->chassis.ctrl_vec.vy += cmd->param->move.move_sense;
  }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BACK)) {
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BACK))
+  {
    cmd->chassis.ctrl_vec.vy -= cmd->param->move.move_sense;
  }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_LEFT)) {
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_LEFT))
+  {
    cmd->chassis.ctrl_vec.vx -= cmd->param->move.move_sense;
  }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_RIGHT)) {
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_RIGHT))
+  {
    cmd->chassis.ctrl_vec.vx += cmd->param->move.move_sense;
  }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ACCELERATE)) {
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ACCELERATE))
+  {
    cmd->chassis.ctrl_vec.vx *= cmd->param->move.move_fast_sense;
    cmd->chassis.ctrl_vec.vy *= cmd->param->move.move_fast_sense;
  }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_DECELEBRATE)) {
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_DECELEBRATE))
+  {
    cmd->chassis.ctrl_vec.vx *= cmd->param->move.move_slow_sense;
    cmd->chassis.ctrl_vec.vy *= cmd->param->move.move_slow_sense;
  }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE)) {
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE))
+  {
    /* 切换至开火模式，设置相应的射击频率和弹丸初速度 */
    cmd->shoot.mode = SHOOT_MODE_LOADED;
    cmd->shoot.fire = true;
-  } else {
+  }
+  else
+  {
    /* 切换至准备模式，停止射击 */
    cmd->shoot.mode = SHOOT_MODE_LOADED;
    cmd->shoot.fire = false;
  }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE_MODE)) {
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE_MODE))
+  {
    /* 每按一次依次切换开火下一个模式 */
    cmd->shoot.fire_mode++;
    cmd->shoot.fire_mode %= FIRE_MODE_NUM;
  }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ROTOR)) {
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ROTOR))
+  {
    /* 切换到小陀螺模式 */
    cmd->chassis.mode = CHASSIS_MODE_ROTOR;
    cmd->chassis.mode_rotor = ROTOR_MODE_RAND;
  }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_OPENCOVER)) {
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_OPENCOVER))
+  {
    /* 每按一次开、关弹舱盖 */
    cmd->shoot.cover_open = !cmd->shoot.cover_open;
  }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BUFF)) {
-    if (cmd->ai_status == AI_STATUS_HITSWITCH) {
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BUFF))
+  {
+    if (cmd->ai_status == AI_STATUS_HITSWITCH)
+    {
      /* 停止ai的打符模式，停用host控制 */
      CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_STOP);
      cmd->host_overwrite = false;
      cmd->ai_status = AI_STATUS_STOP;
-    } else if (cmd->ai_status == AI_STATUS_AUTOAIM) {
+    }
+    else if (cmd->ai_status == AI_STATUS_AUTOAIM)
+    {
      /* 自瞄模式中切换失败提醒 */
-    } else {
+    }
+    else
+    {
      /* ai切换至打符模式，启用host控制 */
      CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_START);
      cmd->ai_status = AI_STATUS_HITSWITCH;
      cmd->host_overwrite = true;
    }
  }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_AUTOAIM)) {
-    if (cmd->ai_status == AI_STATUS_AUTOAIM) {
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_AUTOAIM))
+  {
+    if (cmd->ai_status == AI_STATUS_AUTOAIM)
+    {
      /* 停止ai的自瞄模式，停用host控制 */
      cmd->host_overwrite = false;
      cmd->ai_status = AI_STATUS_STOP;
      CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_STOP);
-    } else {
+    }
+    else
+    {
      /* ai切换至自瞄模式，启用host控制 */
      cmd->ai_status = AI_STATUS_AUTOAIM;
      cmd->host_overwrite = true;
      CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_START);
    }
-  } else {
+  }
+  else
+  {
    cmd->host_overwrite = false;
    // TODO: 修复逻辑
  }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_REVTRIG)) {
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_REVTRIG))
+  {
    /* 按下拨弹反转 */
    cmd->shoot.reverse_trig = true;
  }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FOLLOWGIMBAL35)) {
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FOLLOWGIMBAL35))
+  {
    cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
  }
  /* 保存当前按下的键位状态 */
@ -184,27 +223,38 @@ static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
 * @param cmd 主结构体
 * @param dt_sec 两次解析的间隔
 */
-static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
-  switch (rc->sw_l) {
+static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec)
+{
+  switch (rc->sw_l)
+  {
    /* 左拨杆相应行为选择和解析 */
  case CMD_SW_UP:
    cmd->chassis.mode = CHASSIS_MODE_BREAK;
+    cmd->ai_status = AI_STATUS_STOP;
+    cmd->host_overwrite = false;
    break;

  case CMD_SW_MID:
    cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
+    cmd->ai_status = AI_STATUS_AUTOAIM;
+    cmd->host_overwrite = true;
    break;

  case CMD_SW_DOWN:
    cmd->chassis.mode = CHASSIS_MODE_ROTOR;
    cmd->chassis.mode_rotor = ROTOR_MODE_CW;
+    cmd->ai_status = AI_STATUS_AUTOAIM;
+    cmd->host_overwrite = true;
    break;

  case CMD_SW_ERR:
    cmd->chassis.mode = CHASSIS_MODE_RELAX;
+    cmd->ai_status = AI_STATUS_STOP;
+    cmd->host_overwrite = false;
    break;
  }
-  switch (rc->sw_r) {
+  switch (rc->sw_r)
+  {
    /* 右拨杆相应行为选择和解析*/
  case CMD_SW_UP:
    cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
@ -222,43 +272,29 @@ 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;
  }
  /* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */
+  if (cmd->ai_status == AI_STATUS_STOP || 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失控时机器人恢复放松模式
 *
 * @param cmd 主结构体
 */
-static void CMD_RcLostLogic(CMD_t *cmd) {
+static void CMD_RcLostLogic(CMD_t *cmd)
+{
  /* 机器人底盘、云台、射击运行模式恢复至放松模式 */
  cmd->chassis.mode = CHASSIS_MODE_RELAX;
  cmd->gimbal.mode = GIMBAL_MODE_RELAX;
@ -272,10 +308,13 @@ static void CMD_RcLostLogic(CMD_t *cmd) {
 * @param param 参数
 * @return int8_t 0对应没有错误
 */
-int8_t CMD_Init(CMD_t *cmd, const CMD_Params_t *param) {
+int8_t CMD_Init(CMD_t *cmd, const CMD_Params_t *param)
+{
  /* 指针检测 */
-  if (cmd == NULL) return -1;
-  if (param == NULL) return -1;
+  if (cmd == NULL)
+    return -1;
+  if (param == NULL)
+    return -1;

  /* 设置机器人的命令参数，初始化控制方式为rc控制 */
  cmd->pc_ctrl = false;
@ -301,10 +340,13 @@ inline bool CMD_CheckHostOverwrite(CMD_t *cmd) { return cmd->host_overwrite; }
 * @param dt_sec 两次解析的间隔
 * @return int8_t 0对应没有错误
 */
-int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
+int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec)
+{
  /* 指针检测 */
-  if (rc == NULL) return -1;
-  if (cmd == NULL) return -1;
+  if (rc == NULL)
+    return -1;
+  if (cmd == NULL)
+    return -1;

  /* 在pc控制和rc控制间切换 */
  if (CMD_KeyPressedRc(rc, CMD_KEY_SHIFT) &&
@ -315,12 +357,18 @@ int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
      CMD_KeyPressedRc(rc, CMD_KEY_CTRL) && CMD_KeyPressedRc(rc, CMD_KEY_E))
    cmd->pc_ctrl = false;
  /*c当rc丢控时，恢复机器人至默认状态 */
-  if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR)) {
+  if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR))
+  {
    CMD_RcLostLogic(cmd);
-  } else {
-    if (cmd->pc_ctrl) {
+  }
+  else
+  {
+    if (cmd->pc_ctrl)
+    {
      CMD_PcLogic(rc, cmd, dt_sec);
-    } else {
+    }
+    else
+    {
      CMD_RcLogic(rc, cmd, dt_sec);
    }
  }
@ -335,21 +383,27 @@ int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
 * @param dt_sec 两次解析的间隔
 * @return int8_t 0对应没有错误
 */
-int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec) {
+int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec)
+{
  (void)dt_sec; /* 未使用dt_sec，消除警告 */
  /* 指针检测 */
-  if (host == NULL) return -1;
-  if (cmd == NULL) return -1;
+  if (host == NULL)
+    return -1;
+  if (cmd == NULL)
+    return -1;

  /* 云台欧拉角设置为host相应的变化的欧拉角 */
  cmd->gimbal.delta_eulr.yaw = host->gimbal_delta.yaw;
  cmd->gimbal.delta_eulr.pit = host->gimbal_delta.pit;

  /* host射击命令，设置不同的射击频率和弹丸初速度 */
-  if (host->fire) {
+  if (host->fire)
+  {
    cmd->shoot.mode = SHOOT_MODE_LOADED;
    cmd->shoot.fire = true;
-  } else {
+  }
+  else
+  {
    cmd->shoot.mode = SHOOT_MODE_SAFE;
  }
  return 0;
@ -362,11 +416,14 @@ int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec) {
 * @param cmd 要添加的命令
 * @return int8_t 0对应没有错误
 */
-int8_t CMD_RefereeAdd(CMD_RefereeCmd_t *ref, CMD_UI_t cmd) {
+int8_t CMD_RefereeAdd(CMD_RefereeCmd_t *ref, CMD_UI_t cmd)
+{
  /* 指针检测 */
-  if (ref == NULL) return -1;
+  if (ref == NULL)
+    return -1;
  /* 越界检测 */
-  if (ref->counter >= CMD_REFEREE_MAX_NUM || ref->counter < 0) return -1;
+  if (ref->counter >= CMD_REFEREE_MAX_NUM || ref->counter < 0)
+    return -1;

  /* 添加机器人当前行为状态到画图的命令队列中 */
  ref->cmd[ref->counter] = cmd;
--- a/User/component/cmd.h
+++ b/User/component/cmd.h
@ -21,7 +21,8 @@ typedef enum {
  ROBOT_MODEL_HERO,         /* 英雄机器人 */
  ROBOT_MODEL_ENGINEER,     /* 工程机器人 */
  ROBOT_MODEL_DRONE,        /* 空中机器人 */
-  ROBOT_MODEL_SENTRY,       /* 哨兵机器人 */
+  ROBOT_MODEL_SENTRY_LEFT,  /* 哨兵机器人左头 */
+  ROBOT_MODEL_SENTRY_RIGHT, /* 哨兵机器人右头 */
  ROBOT_MODEL_NUM,          /* 型号数量 */
 } CMD_RobotModel_t;

--- a/User/component/user_math.c
+++ b/User/component/user_math.c
@ -112,3 +112,17 @@ 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();
+  }
+}
--- a/User/component/user_math.h
+++ b/User/component/user_math.h
@ -104,3 +104,53 @@ 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);
--- a/User/device/ai.c
+++ b/User/device/ai.c
@ -12,31 +12,45 @@
 #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 osThreadId_t thread_alert;
+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) {
-  if (ai == NULL) return DEVICE_ERR_NULL;
+  UNUSED(ai);
+  ASSERT(ai);
  if (inited) return DEVICE_ERR_INITED;
-  if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
+  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;
 }
@ -48,9 +62,9 @@ int8_t AI_Restart(void) {
 }

 int8_t AI_StartReceiving(AI_t *ai) {
-  if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AI),
-                           (uint8_t *)&(ai->form_host),
-                           sizeof(ai->form_host)) == HAL_OK)
+  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;
 }
@ -60,17 +74,12 @@ bool AI_WaitDmaCplt(void) {
          SIGNAL_AI_RAW_REDY);
 }

-int8_t AI_ParseHost(AI_t *ai, CMD_Host_t *cmd_host) {
-  (void)cmd_host;
-  if (!CRC16_Verify((const uint8_t *)&(ai->form_host), sizeof(ai->form_host)))
+int8_t AI_ParseHost(AI_t *ai) {
+  if (!CRC16_Verify((const uint8_t *)&(rxbuf), sizeof(ai->form_host)))
    goto error;
-  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;
+  ai->ai_online = true;
+  memcpy(&(ai->form_host), rxbuf, sizeof(ai->form_host));
+  memset(rxbuf, 0, AI_LEN_RX_BUFF);
  return DEVICE_OK;

 error:
@ -78,19 +87,31 @@ 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.pit = ai->form_host.data.gimbal.pit;
+  cmd_host->fire = (ai->form_host.data.notice & AI_NOTICE_FIRE);
+  // cmd_host->fire = true;
+  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(cmd_host, 0, sizeof(*cmd_host));
  return 0;
 }

-int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat) {
+int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat, const AHRS_Eulr_t *gimbal_ai) {
  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;
--- a/User/device/ai.h
+++ b/User/device/ai.h
@ -16,6 +16,7 @@ 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"
@ -45,6 +46,7 @@ typedef struct __packed {
  } to_host;

  CMD_AI_Status_t status;
+  bool ai_online;
 } AI_t;

 /* Exported functions prototypes -------------------------------------------- */
@ -53,11 +55,12 @@ int8_t AI_Restart(void);

 int8_t AI_StartReceiving(AI_t *ai);
 bool AI_WaitDmaCplt(void);
-int8_t AI_ParseHost(AI_t *ai, CMD_Host_t *cmd_host);
+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_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat, const AHRS_Eulr_t *gimbal_ai);
 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
--- a/User/device/can.h
+++ b/User/device/can.h
@ -72,7 +72,7 @@ typedef enum {

  CAN_M3508_FRIC1_ID = 0x205, /* 5 */
  CAN_M3508_FRIC2_ID = 0x206, /* 6 */
-  CAN_M2006_TRIG_ID = 0x207,  /* 7 */
+  CAN_M2006_TRIG_ID = 0x207,  /* 8 */

  CAN_GM6020_YAW_ID = 0x209, /* 5 */
  CAN_GM6020_PIT_ID = 0x20A, /* 6 */
--- a/User/module/config.c
+++ b/User/module/config.c
@ -345,12 +345,196 @@ static const Config_RobotParam_t param_hero = {
    }, /* can */
 }; /* param_hero */      

+static const Config_RobotParam_t param_sentry_left = {
+  .model = ROBOT_MODEL_SENTRY_LEFT,
+
+  .chassis = { /* 底盘模块参数 */
+    .type = CHASSIS_TYPE_MECANUM,
+
+    .motor_pid_param = {
+      .k = 0.001f,
+      .p = 1.0f,
+      .i = 0.0f,
+      .d = 0.0f,
+      .i_limit = 1.0f,
+      .out_limit = 1.0f,
+      .d_cutoff_freq = -1.0f,
+      .range = -1.0f,
+    },
+
+    .follow_pid_param = {
+      .k = 0.5f,
+      .p = 1.0f,
+      .i = 0.0f,
+      .d = 0.0f,
+      .i_limit = 1.0f,
+      .out_limit = 1.0f,
+      .d_cutoff_freq = -1.0f,
+      .range = M_2PI,
+    },
+
+    .low_pass_cutoff_freq = {
+      .in = -1.0f,
+      .out = -1.0f,
+    },
+		
+		.reverse = {
+			.yaw = false,
+		},
+		
+  }, /* 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,
+        .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 = 10.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_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,
+        .i = 0.0f,
+        .d = 0.0f,
+        .i_limit = 1.0f,
+        .out_limit = 1.0f,
+        .d_cutoff_freq = -1.0f,
+        .range = -1.0f,
+      }, {
+        /* GIMBAL_PID_PIT_ANGLE_IDX */
+        .k = 2.0f,
+        .p = 5.0f,
+        .i = 2.5f,
+        .d = 0.0f,
+        .i_limit = 0.0f,
+        .out_limit = 10.0f,
+        .d_cutoff_freq = -1.0f,
+        .range = M_2PI,
+      },
+    }, /* pid */
+
+    .pitch_travel_rad = 1.05f,
+    
+    .low_pass_cutoff_freq = {
+      .out = -1.0f,
+      .gyro = 1000.0f,
+    },
+
+    .reverse = {
+      .yaw = false,
+      .pit = true,
+    },
+  }, /* gimbal */
+
+  .shoot = { /* 射击模块参数 */
+            
+    .fric_pid_param = {
+      .k = 0.001f,
+      .p = 1.0f,
+      .i = 0.2f,
+      .d = 0.01f,
+      .i_limit = 0.5f,
+      .out_limit = 0.5f,
+      .d_cutoff_freq = -1.0f,
+    },
+    .trig_pid_param = {
+      .k = 12.0f,
+      .p = 1.0f,
+      .i = 0.0f,
+      .d = 0.0450000018f,
+      .i_limit = 1.0f,
+      .out_limit = 1.0f,
+      .d_cutoff_freq = -1.0f,
+      .range = M_2PI,
+    },
+    .low_pass_cutoff_freq = {
+      .in = {
+        .fric = -1.0f,
+        .trig = -1.0f,
+      },
+      .out = {
+        .fric = -1.0f,
+        .trig = -1.0f,
+      },
+    },
+    .num_trig_tooth = 8.0f,
+    .trig_gear_ratio = 36.0f,
+    .fric_radius = 0.03f,
+    .cover_open_duty = 0.10f,
+    .cover_close_duty = 0.050f,
+    .model = SHOOT_MODEL_17MM,
+    .bullet_speed = 25.f,
+    .min_shoot_delay = (uint32_t)(1000.0f / 1.0f),
+  }, /* shoot */
+
+  .can = {
+    .chassis = BSP_CAN_1,
+    .gimbal = BSP_CAN_2,
+    .shoot = BSP_CAN_2,      
+    .cap = BSP_CAN_1,
+    }, /* can */
+}; /* param_sentry_left */   
+
 /* static const Config_RobotParam_t param_xxx; */

 static const Config_PilotCfg_t cfg_qs = {
  .param = {
 		.sens_mouse = 0.06f,
-		.sens_rc = 6.0f,
+		.sens_rc = 10.0f,
    .map = {
      .key_map[CMD_BEHAVIOR_FORE] = {CMD_ACTIVE_PRESSED, CMD_KEY_W},
      .key_map[CMD_BEHAVIOR_BACK] = {CMD_ACTIVE_PRESSED, CMD_KEY_S},
@ -413,7 +597,8 @@ static const Config_RobotParamMap_t robot_param_map[] = {
    {"hero", &param_hero},
    // {"engineer", &param_engineer},
    // {"drone", &param_drone},
-    // {"sentry", &param_sentry},
+    {"sentry_left", &param_sentry_left},
+    // {"sentry_right" &param_sentry_right},
    /* {"xxx", &param_xxx}, */
    {NULL, NULL},
 };
--- a/User/module/gimbal.c
+++ b/User/module/gimbal.c
@ -36,7 +36,7 @@ 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) {
-      g->setpoint.eulr.yaw = g->feedback.eulr.imu.yaw;
+      g->setpoint.eulr.yaw = g->feedback.eulr.encoder.yaw;
    } else if (mode == GIMBAL_MODE_RELATIVE) {
      g->setpoint.eulr.yaw = g->feedback.eulr.encoder.yaw;
    }
@ -140,12 +140,12 @@ int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now) {
  const float delta_max =
      CircleError(g->limit.max,
                  (g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
-                   g->feedback.eulr.imu.pit),
+                   g->feedback.eulr.encoder.pit),
                  M_2PI);
  const float delta_min =
      CircleError(g->limit.min,
                  (g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
-                   g->feedback.eulr.imu.pit),
+                   g->feedback.eulr.encoder.pit),
                  M_2PI);
  Clip(&(g_cmd->delta_eulr.pit), delta_min, delta_max);
  g->setpoint.eulr.pit += g_cmd->delta_eulr.pit;
@ -163,14 +163,14 @@ int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now) {
    case GIMBAL_MODE_ABSOLUTE:
      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);
+                   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.imu.pit, 0.0f, g->dt);
+                   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,
                   g->feedback.gyro.x, 0.f, g->dt);
@ -201,6 +201,20 @@ int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now) {
  return 0;
 }

+/**
+ * \brief 打包云台数据给AI
+ *
+ * \param g 包含云台数据的结构体
+ * \param gimbal_for_ai 云台AI数据
+ *
+ * \return 函数运行结果
+ */
+uint8_t Gimbal_PackAI(Gimbal_t *g, const AHRS_Eulr_t *ai){
+  memcpy((void *)ai, (const void *)&(g->feedback.eulr.encoder),
+  sizeof(g->feedback.eulr.encoder));
+return 0;
+}
+
 /**
 * \brief 复制云台输出值
 *
--- a/User/module/gimbal.h
+++ b/User/module/gimbal.h
@ -145,6 +145,16 @@ int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal, const CAN_t *can);
 */
 int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now);

+/**
+ * \brief 打包云台数据给AI
+ *
+ * \param g 包含云台数据的结构体
+ * \param gimbal_for_ai 云台AI数据
+ *
+ * \return 函数运行结果
+ */
+uint8_t Gimbal_PackAI(Gimbal_t *g, const AHRS_Eulr_t *ai);
+
 /**
 * \brief 复制云台输出值
 *
--- a/User/module/shoot.c
+++ b/User/module/shoot.c
@ -248,6 +248,7 @@ int8_t Shoot_Control(Shoot_t *s, CMD_ShootCmd_t *s_cmd,
  s->setpoint.fric_rpm[1] =
      CalculateRpm(s->fire_ctrl.bullet_speed, s->param->fric_radius,
                   (s->param->model == SHOOT_MODEL_17MM));
+  s->setpoint.fric_rpm[1] = -s->setpoint.fric_rpm[1];
  s->setpoint.fric_rpm[0] = -s->setpoint.fric_rpm[1];

  /* 计算拨弹电机位置的目标值 */
@ -255,10 +256,10 @@ int8_t Shoot_Control(Shoot_t *s, CMD_ShootCmd_t *s_cmd,
      (s_cmd->fire)) {
    /* 将拨弹电机角度进行循环加法，每次加(减)射出一颗弹丸的弧度变化 */
    if (s_cmd->reverse_trig) { /* 反转拨弹 */
-      CircleAdd(&(s->setpoint.trig_angle), M_2PI / s->param->num_trig_tooth,
+      CircleAdd(&(s->setpoint.trig_angle), -M_2PI / s->param->num_trig_tooth,
                M_2PI);
    } else {
-      CircleAdd(&(s->setpoint.trig_angle), -M_2PI / s->param->num_trig_tooth,
+      CircleAdd(&(s->setpoint.trig_angle), M_2PI / s->param->num_trig_tooth,
                M_2PI);
      s->fire_ctrl.shooted++;
      s->fire_ctrl.last_shoot = now;
--- a/User/task/ai.c
+++ b/User/task/ai.c
@ -17,11 +17,13 @@ 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 --------------------------------------------------------- */
@ -42,6 +44,7 @@ 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());
@ -51,18 +54,25 @@ void Task_Ai(void *argument) {

    AI_StartReceiving(&ai);
    if (AI_WaitDmaCplt()) {
-      AI_ParseHost(&ai, &cmd_host);
+      AI_ParseHost(&ai);
+      last_online_tick = tick;
    } else {
-      AI_HandleOffline(&ai, &cmd_host);
+      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);
--- a/User/task/command.c
+++ b/User/task/command.c
@ -68,7 +68,6 @@ void Task_Command(void *argument) {
      if (osMessageQueueGet(task_runtime.msgq.cmd.raw.host, &host, 0, 0) ==
          osOK)
        CMD_ParseHost(&host, &cmd, 1.0f / (float)TASK_FREQ_CTRL_COMMAND);
-
    osKernelUnlock(); /* 锁住RTOS内核防止控制过程中断，造成错误 */

    /* 将需要与其他任务分享的数据放到消息队列中 */
--- a/User/task/ctrl_gimbal.c
+++ b/User/task/ctrl_gimbal.c
@ -22,11 +22,13 @@ CMD_GimbalCmd_t gimbal_cmd;
 Gimbal_t gimbal;
 CAN_GimbalOutput_t gimbal_out;
 Referee_GimbalUI_t gimbal_ui;
+AHRS_Eulr_t 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 gimbal_for_ai;
 #endif

 /* Private function --------------------------------------------------------- */
@ -66,12 +68,15 @@ void Task_CtrlGimbal(void *argument) {

    osKernelLock(); /* 锁住RTOS内核防止控制过程中断，造成错误 */
    Gimbal_UpdateFeedback(&gimbal, &can);
+    Gimbal_PackAI(&gimbal, &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, &gimbal_for_ai, 0, 0);

    Gimbal_DumpUI(&gimbal, &gimbal_ui);
    osMessageQueueReset(task_runtime.msgq.ui.gimbal);
--- a/User/task/init.c
+++ b/User/task/init.c
@ -30,7 +30,8 @@
 *
 * \param argument 未使用
 */
-void Task_Init(void *argument) {
+void Task_Init(void *argument)
+{
    (void)argument; /* 未使用argument，消除警告 */

    Config_Get(&task_runtime.cfg); /* 获取机器人配置 */
@ -96,7 +97,8 @@ 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);

--- a/User/task/user_task.h
+++ b/User/task/user_task.h
@ -58,6 +58,7 @@ typedef struct {
      osMessageQueueId_t accl;     /* IMU读取 */
      osMessageQueueId_t gyro;     /* IMU读取 */
      osMessageQueueId_t eulr_imu; /* 姿态解算得到 */
+      osMessageQueueId_t eulr_encoder; /* 编码器读取 */
    } gimbal;

    /* 控制指令 */
Author	SHA1	Message	Date
RB	88a1f2bae4	左头自瞄差不多了	2025-03-05 15:02:44 +08:00
RB	180b6d728d	添加云台数据	2025-03-05 10:29:39 +08:00
RB	b6e1393f4d	上位机通讯正常	2025-03-04 17:26:45 +08:00
RB	772c44f3d9	提交一下	2025-03-04 17:20:57 +08:00