提交一下

.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
+}

.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
+        }
+      ]
+    }
+  ]
+}

.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
+}

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,26 @@
           <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>
       </WatchWindow1>
       <Tracepoint>
         <THDelay>0</THDelay>
@@ -1718,7 +1738,7 @@
       <GroupNumber>11</GroupNumber>
       <FileNumber>100</FileNumber>
       <FileType>1</FileType>
-      <tvExp>0</tvExp>
+      <tvExp>1</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
       <PathWithFileName>..\User\device\ai.c</PathWithFileName>
@@ -1786,7 +1806,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>

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>

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)) {
+  typedef struct __attribute__((packed))
-  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)) {
+  typedef struct __attribute__((packed))
-  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;

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;
 

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),
+  UNUSED(ai);
-                           (uint8_t *)&(ai->form_host),
+  if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AI), rxbuf,
-                           sizeof(ai->form_host)) == HAL_OK)
+                           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) {
+int8_t AI_ParseHost(AI_t *ai) {
-  (void)cmd_host;
+  if (!CRC16_Verify((const uint8_t *)&(rxbuf), sizeof(ai->form_host)))
-  if (!CRC16_Verify((const uint8_t *)&(ai->form_host), sizeof(ai->form_host)))
     goto error;
-  cmd_host->gimbal_delta.pit = ai->form_host.data.gimbal.pit;
+  ai->ai_online = true;
-  cmd_host->gimbal_delta.yaw = ai->form_host.data.gimbal.yaw;
+  memcpy(&(ai->form_host), rxbuf, sizeof(ai->form_host));
-  cmd_host->gimbal_delta.rol = ai->form_host.data.gimbal.rol;
+  memset(rxbuf, 0, AI_LEN_RX_BUFF);
-  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;
   return DEVICE_OK;
 
 error:
@@ -78,10 +87,19 @@ 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->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;

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_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

@@ -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 = 0x208,  /* 8 */
 
   CAN_GM6020_YAW_ID = 0x209, /* 5 */
   CAN_GM6020_PIT_ID = 0x20A, /* 6 */

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 = 10.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 = 30.f,
+    .min_shoot_delay = (uint32_t)(1000.0f / 10.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},
 };

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);

User/task/ai.c

@@ -42,6 +42,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,17 +52,23 @@ 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);
     bool ref_update = (osMessageQueueGet(task_runtime.msgq.referee.ai,
                                          &(referee_ai), NULL, 0) == osOK);
+    
     AI_PackMCU(&ai, &quat);
     if (ref_update) AI_PackRef(&ai, &(referee_ai));