添加了SEARCH模式

Middlewares/Third_Party/Protocol/protocol.h

@@ -59,6 +59,7 @@ typedef struct __attribute__((packed)) {
     float rol; /* 翻滚角（Roll angle） */
   } gimbal;    /* 欧拉角 */
   
+  float distance; /* 距离 */
   uint8_t notice; /* 控制命令 */
 
   struct __attribute__((packed)) {

User/component/cmd.c

@@ -221,7 +221,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_ABSOLUTE;
+      cmd->gimbal.mode = GIMBAL_MODE_SEARCH;
       cmd->shoot.fire = false;
       cmd->shoot.mode = SHOOT_MODE_LOADED;
       break;

User/component/cmd.h

@@ -43,6 +43,7 @@ typedef enum {
   GIMBAL_MODE_RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */
   GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */
   GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */
+  GIMBAL_MODE_SEARCH, /* 搜索模式，通过云台旋转搜索目标 */
 } CMD_GimbalMode_t;
 
 /* 射击运行模式 */

User/device/ai.c

@@ -67,7 +67,7 @@ int8_t AI_ParseHost(AI_t *ai, CMD_Host_t *cmd_host) {
   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));
+  memcpy(&(ai->form_host.DownPackage), ai->form_host.rx_buffer, sizeof(ai->form_host.DownPackage));
   // 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;
@@ -75,13 +75,17 @@ int8_t AI_ParseHost(AI_t *ai, CMD_Host_t *cmd_host) {
   // 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;
+  // float yaw_out = PID_Calc(&ai->pid[0], 0.0f, ai->form_host.DownPackage.data.gimbal.yaw, 0.0f , 1.0/10.0f);
-  cmd_host->gimbal_delta.yaw = ai->form_host.data.data.gimbal.yaw;
+  // float pit_out = PID_Calc(&ai->pid[1], 0.0f, ai->form_host.DownPackage.data.gimbal.pit, 0.0f , 1.0/10.0f);
-  cmd_host->gimbal_delta.rol = ai->form_host.data.data.gimbal.rol;
+  cmd_host->gimbal_delta.pit = ai->form_host.DownPackage.data.gimbal.pit;
-  cmd_host->fire = (ai->form_host.data.data.notice & AI_NOTICE_FIRE);
+  cmd_host->gimbal_delta.yaw = ai->form_host.DownPackage.data.gimbal.yaw;
-  cmd_host->chassis_move_vec.vx = ai->form_host.data.data.chassis_move_vec.vx;
+  // cmd_host->gimbal_delta.pit = pit_out;
-  cmd_host->chassis_move_vec.vy = ai->form_host.data.data.chassis_move_vec.vy;
+  // cmd_host->gimbal_delta.yaw = yaw_out;
-  cmd_host->chassis_move_vec.wz = ai->form_host.data.data.chassis_move_vec.wz;
+  cmd_host->gimbal_delta.rol = ai->form_host.DownPackage.data.gimbal.rol;
+  cmd_host->fire = (ai->form_host.DownPackage.data.notice & AI_NOTICE_FIRE);
+  cmd_host->chassis_move_vec.vx = ai->form_host.DownPackage.data.chassis_move_vec.vx;
+  cmd_host->chassis_move_vec.vy = ai->form_host.DownPackage.data.chassis_move_vec.vy;
+  cmd_host->chassis_move_vec.wz = ai->form_host.DownPackage.data.chassis_move_vec.wz;
 
   return DEVICE_OK;
 
@@ -95,7 +99,7 @@ int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host) {
   if (cmd_host == NULL) return DEVICE_ERR_NULL;
   ai->ai_online = false;
   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(&(ai->form_host.DownPackage), 0, sizeof(ai->form_host.DownPackage));
   memset(cmd_host, 0, sizeof(*cmd_host));
   return 0;
 }

User/device/ai.h

@@ -38,7 +38,7 @@ typedef struct __packed {
   osThreadId_t thread_alert;
   struct {
     uint8_t rx_buffer[sizeof(Protocol_DownPackage_t)]; 
-    Protocol_DownPackage_t data;
+    Protocol_DownPackage_t DownPackage;
   }form_host;
 
   struct {

User/module/gimbal.c

@@ -9,6 +9,9 @@
 
 /* Private typedef ---------------------------------------------------------- */
 /* Private define ----------------------------------------------------------- */
+#define YAW_DELTA_MAX (5.7f + M_PI)
+#define YAW_DELTA_MIN (1.8f + M_PI)
+
 /* Private macro ------------------------------------------------------------ */
 /* Private variables -------------------------------------------------------- */
 /* Private function  -------------------------------------------------------- */
@@ -39,6 +42,8 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, CMD_GimbalMode_t mode) {
       g->setpoint.eulr.yaw = g->feedback.eulr.imu.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;
     }
   }
 
@@ -138,6 +143,18 @@ int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now) {
   }
 
   /* 处理yaw控制命令 */
+  /* 处理yaw控制命令，添加yaw限位 */
+  const float yaw_delta_max =
+      CircleError(YAW_DELTA_MAX,
+                  (g->feedback.eulr.encoder.yaw + g->setpoint.eulr.yaw - 
+                   g->feedback.eulr.imu.yaw),
+                  M_2PI);
+  const float yaw_delta_min =
+      CircleError(YAW_DELTA_MIN,
+                  (g->feedback.eulr.encoder.yaw + g->setpoint.eulr.yaw - 
+                   g->feedback.eulr.imu.yaw),
+                  M_2PI);
+  Clip(&(g_cmd->delta_eulr.yaw), yaw_delta_min, yaw_delta_max);
   CircleAdd(&(g->setpoint.eulr.yaw), g_cmd->delta_eulr.yaw, M_2PI);
 
   /* 处理pitch控制命令，软件限位 */
@@ -197,6 +214,26 @@ int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now) {
     case GIMBAL_MODE_RELATIVE:
       for (uint8_t i = 0; i < GIMBAL_ACTR_NUM; i++) g->out[i] = 0.0f;
       break;
+    
+    case GIMBAL_MODE_SEARCH:
+      // g->setpoint.eulr.yaw = YAW_DELTA_MIN + (YAW_DELTA_MAX - YAW_DELTA_MIN) * (sinf(now / 100.0f) + 1.0f) / 2.0f; // 在min和max之间摆动
+      g->setpoint.eulr.yaw = YAW_DELTA_MIN + (YAW_DELTA_MAX - YAW_DELTA_MIN) * (1.0f - sinf(now / 500.0f)) / 2.0f; // 在min和max之间摆动
+      g->setpoint.eulr.pit = g->limit.min + (g->limit.max - g->limit.min) * (sinf(now / 100.0f) + 1.0f) / 2.0f; // 在min和max之间摆动
+
+      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);
+      break;
   }
 
   /* 输出滤波 */