添加了云台数据发送到ai

.vscode/settings.json

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

MDK-ARM/DevC.uvoptx

@@ -422,6 +422,21 @@
           <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>
@@ -1685,7 +1700,7 @@
 
   <Group>
     <GroupName>User/device</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
@@ -1765,7 +1780,7 @@
 
   <Group>
     <GroupName>User/module</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>

Middlewares/Third_Party/Protocol/protocol.h

@@ -10,13 +10,15 @@
 extern "C" {
 #endif
 
-#define AI_NOTICE_AUTOAIM (1 << 0)
+#define AI_NOTICE_FIRE (1 << 0)
-#define AI_NOTICE_HITBUFF (1 << 1)
+#define AI_NOTICE_TRACKING (1 << 1)
-#define AI_NOTICE_AUTOMATIC (1 << 2)
+
-#define AI_NOTICE_FIRE (1 << 3)
+#define AI_NOTICE_AUTOAIM (1 << 2)
-#define AI_NOTICE_TRACKING (1 << 4)
+#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)
@@ -34,16 +36,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;    /* 欧拉角 */
+
+  float bullet_speed  /* 弹丸速度 */;
   uint8_t notice; /* 控制命令 */
 
-  float ball_speed; /* 子弹初速度 */
-
-  struct __attribute__((packed)) {
-    float left;
-    float right;
-  } distance; /* 左右距离(哨兵) */
-
-  float chassis_speed; /* 底盘速度(哨兵) */
 } Protocol_UpDataMCU_t;
 
 /* 电控 -> 视觉 裁判系统数据结构体*/

User/bsp/mm.h

@@ -13,6 +13,10 @@ 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,10 +4,40 @@
 
 #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;
 }
-void Ballistics_Reset(Ballistics_t *b) { (void)b; }
 
+
+
+/**
+ * @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,6 +16,14 @@ 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->ai_status = AI_STATUS_AUTOAIM;
       cmd->host_overwrite = true;
+      cmd->ai_status = AI_STATUS_AUTOAIM;
       CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_START);
     }
   } else {
@@ -190,31 +190,19 @@ 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;
-      cmd->ai_status = AI_STATUS_STOP;
       break;
 
     case CMD_SW_MID:
       cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
-      cmd->host_overwrite = true;
-      cmd->shoot.ai_fire = false;
-      cmd->ai_status = AI_STATUS_AUTOMATIC;
       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;
-      cmd->ai_status = AI_STATUS_AUTOMATIC;
       break;
 
     case CMD_SW_ERR:
       cmd->chassis.mode = CHASSIS_MODE_RELAX;
-      cmd->host_overwrite = false;
-      cmd->shoot.ai_fire = false;
-      cmd->ai_status = AI_STATUS_STOP;
       break;
   }
   switch (rc->sw_r) {
@@ -225,7 +213,7 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
       break;
 
     case CMD_SW_MID:
-      cmd->gimbal.mode = GIMBAL_MODE_SEARCH;
+      cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
       cmd->shoot.fire = false;
       cmd->shoot.mode = SHOOT_MODE_LOADED;
       break;
@@ -235,36 +223,19 @@ 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;
+  }
 }
 
 /**
@@ -360,6 +331,11 @@ int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec) {
   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) {
     cmd->shoot.fire = true;

User/component/cmd.h

@@ -179,6 +179,7 @@ typedef struct {
 
 /* AI行为状态 */
 typedef enum {
+  AI_STATUS_OFFLINE,  /* 离线状态 */
   AI_STATUS_STOP,      /* 停止状态 */
   AI_STATUS_AUTOAIM,   /* 自瞄状态 */
   AI_STATUS_HITSWITCH, /* 打符状态 */
@@ -255,6 +256,7 @@ typedef struct {
     float wz;         /* z轴转动速度 */
   } chassis_move_vec; /* 底盘移动向量 */
 
+  bool tracking; /* 是否追踪目标 */
   bool fire; /* 开火状态 */
 } CMD_Host_t;
 

User/device/ai.c

@@ -124,10 +124,12 @@ int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *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;

User/device/ai.h

@@ -57,7 +57,7 @@ int8_t AI_StartReceiving(AI_t *ai);
 bool AI_WaitDmaCplt(void);
 int8_t AI_ParseHost(AI_t *ai);
 int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host);
-int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat);
+int8_t AI_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);

User/module/config.c

@@ -403,26 +403,6 @@ 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,
@@ -578,25 +558,6 @@ 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,
@@ -617,25 +578,6 @@ 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,
@@ -662,7 +604,7 @@ static const Config_RobotParam_t param_sentry_gimbal = {
       .yaw_travel_rad = -1.0f,
       .pitch_travel_rad = 1.0f,
       .yaw_imu = true,
-      .pit_imu = true,
+      .pit_imu = false,
     },
 
     .low_pass_cutoff_freq = {

User/module/gimbal.c

@@ -292,6 +292,13 @@ 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

@@ -141,6 +141,9 @@ 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 运行云台控制逻辑
  *
@@ -153,6 +156,9 @@ 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);
 
+uint8_t Gimbal_PackAI(Gimbal_t *gimbal, const AHRS_Eulr_t *eulr_gimbal_for_ai);
+
+
 /**
  * \brief 复制云台输出值
  *

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 --------------------------------------------------------- */
@@ -66,10 +68,11 @@ void Task_Ai(void *argument) {
 
     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.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);
+    osMessageQueueReset(task_runtime.msgq.ai.quat);
+    osMessageQueuePut(task_runtime.msgq.ai.quat, &(gimbal_ahrs.quat), 0, 0);
 
     /* PID控制IMU温度，PWM输出 */
     BSP_PWM_Set(BSP_PWM_IMU_HEAT,

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 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 --------------------------------------------------------- */
@@ -66,12 +68,15 @@ 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,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);
 

User/task/user_task.h

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