111

Core/Src/stm32f4xx_it.c

@@ -287,6 +287,7 @@ void USART1_IRQHandler(void)
   /* USER CODE END USART1_IRQn 0 */
   HAL_UART_IRQHandler(&huart1);
   /* USER CODE BEGIN USART1_IRQn 1 */
+  BSP_UART_IRQHandler(&huart1);
 
   /* USER CODE END USART1_IRQn 1 */
 }

User/bsp/bsp_config.yaml

@@ -72,4 +72,6 @@ uart:
     name: DR16
   - instance: USART6
     name: NUC
+  - instance: USART1
+    name: '1'
   enabled: true

User/bsp/gpio.c

@@ -73,14 +73,20 @@ int8_t BSP_GPIO_RegisterCallback(BSP_GPIO_t gpio, void (*callback)(void)) {
 int8_t BSP_GPIO_EnableIRQ(BSP_GPIO_t gpio) {
   switch (gpio) {
     case BSP_GPIO_KEY:
+#if defined(KEY_EXTI_IRQn)
       HAL_NVIC_EnableIRQ(KEY_EXTI_IRQn);
-      break;
+#endif
+      return BSP_OK;
     case BSP_GPIO_ACCL_INT:
+#if defined(ACCL_INT_EXTI_IRQn)
       HAL_NVIC_EnableIRQ(ACCL_INT_EXTI_IRQn);
-      break;
+#endif
+      return BSP_OK;
     case BSP_GPIO_GYRO_INT:
+#if defined(GYRO_INT_EXTI_IRQn)
       HAL_NVIC_EnableIRQ(GYRO_INT_EXTI_IRQn);
-      break;
+#endif
+      return BSP_OK;
     default:
       return BSP_ERR;
   }
@@ -90,14 +96,20 @@ int8_t BSP_GPIO_EnableIRQ(BSP_GPIO_t gpio) {
 int8_t BSP_GPIO_DisableIRQ(BSP_GPIO_t gpio) {
   switch (gpio) {
     case BSP_GPIO_KEY:
+#if defined(KEY_EXTI_IRQn)
       HAL_NVIC_DisableIRQ(KEY_EXTI_IRQn);
-      break;
+#endif
+      return BSP_OK;
     case BSP_GPIO_ACCL_INT:
+#if defined(ACCL_INT_EXTI_IRQn)
       HAL_NVIC_DisableIRQ(ACCL_INT_EXTI_IRQn);
-      break;
+#endif
+      return BSP_OK;
     case BSP_GPIO_GYRO_INT:
+#if defined(GYRO_INT_EXTI_IRQn)
       HAL_NVIC_DisableIRQ(GYRO_INT_EXTI_IRQn);
-      break;
+#endif
+      return BSP_OK;
     default:
       return BSP_ERR;
   }

User/bsp/uart.c

@@ -27,6 +27,8 @@ static BSP_UART_t UART_Get(UART_HandleTypeDef *huart) {
     return BSP_UART_DR16;
   else if (huart->Instance == USART6)
     return BSP_UART_NUC;
+  else if (huart->Instance == USART1)
+    return BSP_UART_1;
   else
     return BSP_UART_ERR;
 }
@@ -119,6 +121,8 @@ UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart) {
       return &huart3;
     case BSP_UART_NUC:
       return &huart6;
+    case BSP_UART_1:
+      return &huart1;
     default:
       return NULL;
   }

User/bsp/uart.h

@@ -29,6 +29,7 @@ extern "C" {
 typedef enum {
   BSP_UART_DR16,
   BSP_UART_NUC,
+  BSP_UART_1,
   BSP_UART_NUM,
   BSP_UART_ERR,
 } BSP_UART_t;

User/device/dr16.c

@@ -3,7 +3,7 @@
 //     Example：
 
 //   DR16_Init(&dr16);
-//  
+  
 //   while (1) {
 //     DR16_StartDmaRecv(&dr16);
 //     if (DR16_WaitDmaCplt(20)) {
@@ -114,7 +114,7 @@
 
 //   dr16->header.online = true;
 //   dr16->header.last_online_time = BSP_TIME_Get_us();
-//  
+  
 //   memset(&(dr16->data), 0, sizeof(dr16->data));
 
 //   float full_range = (float)(DR16_CH_VALUE_MAX - DR16_CH_VALUE_MIN);
@@ -139,7 +139,7 @@
 
 //   // 解析键盘按键 - 使用union简化代码
 //   uint16_t key_value = dr16->raw_data.key;
-//  
+  
 //   // 解析键盘位映射（W-B键，位0-15）
 //   for (int i = DR16_KEY_W; i <= DR16_KEY_B; i++) {
 //     dr16->data.keyboard.key[i] = (key_value & (1 << i)) != 0;
@@ -151,7 +151,7 @@
 
 //   // 解析第五通道
 //   dr16->data.ch_res = 2.0f * ((float)dr16->raw_data.res - DR16_CH_VALUE_MID) / full_range;
-//  
+  
 //   return DEVICE_OK;
 // }
 

User/device/dr16.h

@@ -12,11 +12,11 @@
 
 // /* USER INCLUDE BEGIN */
 
-///* USER INCLUDE END */
+// ///* USER INCLUDE END */
 
 // /* USER DEFINE BEGIN */
 
-///* USER DEFINE END */
+// ///* USER DEFINE END */
 
 // /* Exported constants ------------------------------------------------------- */
 // /* Exported macro ----------------------------------------------------------- */
@@ -110,7 +110,7 @@
 
 // /* USER FUNCTION BEGIN */
 
-///* USER FUNCTION END */
+// ///* USER FUNCTION END */
 
 // #ifdef __cplusplus
 // }

User/device/motor_rm.c

@@ -56,12 +56,12 @@ static int8_t MOTOR_RM_GetLogicalIndex(uint16_t can_id, MOTOR_RM_Module_t module
     switch (module) {
         case MOTOR_M2006:
         case MOTOR_M3508:
-            if (can_id >= M3508_M2006_FB_ID_BASE && can_id < M3508_M2006_FB_ID_BASE + 7) {
+            if (can_id >= M3508_M2006_FB_ID_BASE && can_id <= M3508_M2006_FB_ID_BASE + 7) {
                 return can_id - M3508_M2006_FB_ID_BASE;
             }
             break;
         case MOTOR_GM6020:
-            if (can_id >= GM6020_FB_ID_BASE && can_id < GM6020_FB_ID_BASE + 6) {
+            if (can_id >= GM6020_FB_ID_BASE && can_id <= GM6020_FB_ID_BASE + 6) {
                 return can_id - GM6020_FB_ID_BASE + 4;
             }
             break;
@@ -74,7 +74,7 @@ static int8_t MOTOR_RM_GetLogicalIndex(uint16_t can_id, MOTOR_RM_Module_t module
 static float MOTOR_RM_GetRatio(MOTOR_RM_Module_t module) {
     switch (module) {
         case MOTOR_M2006: return 36.0f;
-        case MOTOR_M3508: return 268.0f/17.0f ;   // 通用3508减速比3591.0f / 187.0f;前面的是舵轮减速箱减速比
+        case MOTOR_M3508: return 3591.0f / 187.0f;
         case MOTOR_GM6020: return 1.0f;
         default: return 1.0f;
     }

User/module/cmd.c

@@ -146,15 +146,19 @@ int8_t CMD_CtrlSet(Chassis_CMD_t *c_cmd, const CMD_RC_t *rc, Gimbal_CMD_t *g_cmd
         {
         case CMD_SW_UP:
             g_cmd->ctrl_mode = GIMBAL_MODE_REMOTE;
+            s_cmd->control_mode = SHOOT_REMOTE;
             break;
         case CMD_SW_MID:
             g_cmd->ctrl_mode = GIMBAL_MODE_AI;
+            s_cmd->control_mode = SHOOT_AI;
             break;  
         case CMD_SW_DOWN:
             g_cmd->ctrl_mode = GIMBAL_MODE_AI;
+            s_cmd->control_mode = SHOOT_AI;
             break;
         case CMD_SW_ERR:
             g_cmd->ctrl_mode = GIMBAL_MODE_REMOTE;
+            s_cmd->control_mode = SHOOT_REMOTE;
             break;
         }
 

User/module/cmd.h

@@ -47,6 +47,8 @@ typedef struct
 
     struct
     {
+
+        
         float ch_l_x; /* 遥控器左侧摇杆横轴值，上为正 */
         float ch_l_y; /* 遥控器左侧摇杆纵轴值，右为正 */
         float ch_r_x; /* 遥控器右侧摇杆横轴值，上为正 */
@@ -126,8 +128,15 @@ typedef enum {
     SHOOT_MODE_NUM
 }Shoot_Mode_t;
 
+typedef enum {
+    SHOOT_REMOTE = 0,/* 遥控器控制 */
+    SHOOT_AI,       /* AI控制 */
+} Shoot_Control_Mode_t;
+
 typedef struct {
   Shoot_Mode_t mode;/* 射击模式 */
+
+  Shoot_Control_Mode_t control_mode;/* 控制模式 */
   bool ready;       /* 准备射击 */
   bool firecmd;     /* 射击 */
 }Shoot_CMD_t;

User/module/config.c

@@ -145,7 +145,7 @@ static const Config_Param_t config = {
         },
 
         .pid.yaw_4310_motor_omega = {
-            .k = 0.0f, 
+            .k = 0.1f, 
             .p = 0.3f, 
             .i = 0.0f, 
             .d = 0.0f, 
@@ -165,10 +165,19 @@ static const Config_Param_t config = {
             // .d_cutoff_freq = 0.0f, 
             // .range = M_2PI
             /*上面一组pid会导致电机超调，可能是因为pid计算错误，正在进行重新修改pid*/
+            // .k = 2.0f, 
+            // .p = 5.0f, 
+            // .i = 0.0f, 
+            // .d = 0.0f, 
+            // .i_limit = 1.0f, 
+            // .out_limit = 10.0f, 
+            // .d_cutoff_freq = 0.0f, 
+            // .range = M_2PI
+
             .k = 2.0f, 
-            .p = 5.0f, 
+            .p = 7.0f, 
-            .i = 0.0f, 
+            .i = 0.5f, 
-            .d = 0.0f, 
+            .d = 1.0f, 
             .i_limit = 1.0f, 
             .out_limit = 10.0f, 
             .d_cutoff_freq = 0.0f, 
@@ -185,8 +194,17 @@ static const Config_Param_t config = {
             // .d_cutoff_freq = -1.0f, 
             // .range = -1.0f
 
+            // .k = 0.35f, 
+            // .p = 0.3f, 
+            // .i = 0.0f, 
+            // .d = 0.0f, 
+            // .i_limit = 1.0f, 
+            // .out_limit = 1.0f, 
+            // .d_cutoff_freq = -1.0f, 
+            // .range = -1.0f
+
             .k = 0.35f, 
-            .p = 0.3f, 
+            .p = 0.35f, 
             .i = 0.0f, 
             .d = 0.0f, 
             .i_limit = 1.0f, 
@@ -222,8 +240,8 @@ static const Config_Param_t config = {
               .basic.extra_deceleration_ratio = 1,
               .basic.ratio_multilevel[0] = 1.0f,
               .basic.num_trig_tooth = 8,
-              .basic.shot_freq = 20.0f,
+              .basic.shot_freq = 5.0f,
-              .basic.shot_burst_num = 5,
+              .basic.shot_burst_num = 1,
 
               .jamDetection.enable = true,
               .jamDetection.threshold = 120.0f,
@@ -253,8 +271,8 @@ static const Config_Param_t config = {
                 },
 
                 .trig_2006={
-                .k=0.5f,
+                .k=0.7f,
-                .p=0.5f,
+                .p=0.7f,
                 .i=0.5f,
                 .d=0.05f,
                 .i_limit=0.2f,

User/module/gimbal.c

@@ -115,6 +115,7 @@ int8_t Gimbal_Init(Gimbal_t *g,  Gimbal_Param_t *param,float target_freq)
   PID_Init(&g->pid.yaw_6020_omega,KPID_MODE_CALC_D, target_freq,&g->param->pid.yaw_6020_motor_omega );
   /*4310大yaw pid初始化 大yaw单环控*/
   PID_Init(&g->pid.yaw_4310_angle,KPID_MODE_SET_D, target_freq,&g->param->pid.yaw_4310_motor_angle );
+  PID_Init(&g->pid.yaw_4310_omega,KPID_MODE_CALC_D, target_freq,&g->param->pid.yaw_4310_motor_omega );
   /*4310 pitch pid初始化 单环*/
   PID_Init(&g->pid.pitch_4310_angle,KPID_MODE_NO_D, target_freq,&g->param->pid.pitch_4310_motor_angle );
   PID_Init(&g->pid.pitch_4310_omega,KPID_MODE_CALC_D, target_freq,&g->param->pid.pitch_4310_motor_omega );
@@ -246,7 +247,7 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){
  Gimbal_SetMode(g, g_cmd->mode);
 
 /*处理小yaw轴控制命令*/
-float delta_yaw_6020 = 5.0f*g_cmd->delta_yaw_6020 * g->dt;
+float delta_yaw_6020 = 10.0f*g_cmd->delta_yaw_6020 * g->dt;
 
 if(g->param->travel.yaw_6020 > 0)  // 有限位才处理
 {
@@ -276,7 +277,7 @@ const float delta_min_6020 = CircleError(g->limit.yaw_6020.min,
  g->setpoint.yaw_4310=small_yaw_center_offset;
 
   /*处理大pitch控制命令*/
-float delta_pitch_4310 = 5.0f*g_cmd->delta_pitch_4310*g->dt;
+float delta_pitch_4310 = 8.0f*g_cmd->delta_pitch_4310*g->dt;
 if(g->param->travel.pitch_4310 > 0)  // 有限位才处理
 {
    /* 计算当前电机角度与IMU角度的偏差 */
@@ -330,11 +331,29 @@ g->setpoint.eulr.pit = g_cmd->set_pitch;
     /*4310大YAW控制
     这里是单环控制的，有需要加双环
     */
-    yaw_omega_set_point =PID_Calc(&g->pid.yaw_4310_angle,g->setpoint.yaw_4310,
+    // /* 大YAW角度环：使用环形误差，避免过零点抽搐 */
-                                g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle,g->feedback.motor.yaw_6020_motor_feedback.rotor_speed,g->dt);
+    // float yaw4310_err = CircleError(
-    // g->out.yaw_4310 = PID_Calc(&g->pid.yaw_4310_omega,yaw_omega_set_point,
+    //   g->setpoint.yaw_4310,
-    //                           g->feedback.imu.gyro.z,0.0f,g->dt);
+    //   g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle,
-    g->out.yaw_4310 = yaw_omega_set_point;  // 直接输出速度环目标值作为电机输出
+    //   M_2PI
+    // );
+    // /* 构造等效的包角设定值，使 PID 内部误差=set-ref=环形误差 */
+    // float yaw4310_set_wrapped = g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle + yaw4310_err;
+    // yaw_omega_set_point = PID_Calc(
+    //   &g->pid.yaw_4310_angle,
+    //   yaw4310_set_wrapped,
+    //   g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle,
+    //   g->feedback.motor.yaw_6020_motor_feedback.rotor_speed,
+    //   g->dt
+    // );
+
+yaw_omega_set_point =PID_Calc(&g->pid.yaw_4310_angle,3.90f,
+                                g->feedback.motor.yaw_4310_motor_feedback.rotor_abs_angle,g->feedback.motor.yaw_6020_motor_feedback.rotor_speed,g->dt);
+
+
+    g->out.yaw_4310 = PID_Calc(&g->pid.yaw_4310_omega,yaw_omega_set_point,
+                              g->feedback.motor.yaw_4310_motor_feedback.rotor_speed,0.0f,g->dt);
+    // g->out.yaw_4310 = yaw_omega_set_point;  // 直接输出速度环目标值作为电机输出
       /*控制云台4310电机 也是单环，但是加了重力补偿函数，可以根据不一样的情况去拟合函数*/
     pitch_omega_set_point =PID_Calc(&g->pid.pitch_4310_angle,g->setpoint.eulr.pit,
                                     g->feedback.imu.eulr.rol,0.0f,g->dt);
@@ -376,7 +395,8 @@ void Gimbal_Output(Gimbal_t *g)
   MOTOR_RM_SetOutput(&g->param->yaw_6020_motor, g->out.yaw_6020);
 
    MOTOR_MIT_Output_t output_yaw_4310 = {0};
-    output_yaw_4310.torque = g->out.yaw_4310 * 1.0f;
+    output_yaw_4310.torque = g->out.yaw_4310 * 2.0f;
+    // output_yaw_4310.kp = 0.2f;
     output_yaw_4310.kd = 0.1f;
     // output_yaw_4310.kd=0.1f;
 

User/task/Task8.c

@@ -6,7 +6,8 @@
 /* Includes ----------------------------------------------------------------- */
 #include "task/user_task.h"
 /* USER INCLUDE BEGIN */
-
+#include "module/cmd.h"
+#include "bsp/uart.h"
 /* USER INCLUDE END */
 
 /* Private typedef ---------------------------------------------------------- */
@@ -14,7 +15,7 @@
 /* Private macro ------------------------------------------------------------ */
 /* Private variables -------------------------------------------------------- */
 /* USER STRUCT BEGIN */
-
+CMD_RC_t  remote;
 /* USER STRUCT END */
 
 /* Private function --------------------------------------------------------- */
@@ -36,6 +37,11 @@ void Task_Task8(void *argument) {
   while (1) {
     tick += delay_tick; /* 计算下一个唤醒时刻 */
     /* USER CODE BEGIN */
+    osMessageQueueGet(task_runtime.msgq.RC_REMOTE, &remote, NULL, 0);
+
+
+      BSP_UART_Transmit(BSP_UART_1, (uint8_t *)&remote, sizeof(CMD_RC_t), false);  
+
 
     /* USER CODE END */
     osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */

User/task/ai.c

@@ -49,6 +49,7 @@ void Task_ai(void *argument) {
 		AI_Get_NUC(&ai,&ai_cmd);		
     osMessageQueueReset(task_runtime.msgq.gimbal.ai.g_cmd);
     osMessageQueuePut(task_runtime.msgq.gimbal.ai.g_cmd, &ai_cmd, 0, 0);	
+    osMessageQueuePut(task_runtime.msgq.shoot.ai.s_cmd, &ai_cmd, 0, 0);
     /* USER CODE END */
     osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
   }

User/task/init.c

@@ -37,7 +37,7 @@ void Task_Init(void *argument) {
   task_runtime.thread.cmd = osThreadNew(Task_cmd, NULL, &attr_cmd);
   task_runtime.thread.gimbal = osThreadNew(Task_gimbal, NULL, &attr_gimbal);
   task_runtime.thread.shoot = osThreadNew(Task_shoot, NULL, &attr_shoot);
-  task_runtime.thread.ai = osThreadNew(Task_ai, NULL, &attr_ai);
+  // task_runtime.thread.ai = osThreadNew(Task_ai, NULL, &attr_ai);
   task_runtime.thread.Task8 = osThreadNew(Task_Task8, NULL, &attr_Task8);
 
   // 创建消息队列
@@ -54,7 +54,8 @@ void Task_Init(void *argument) {
   task_runtime.msgq.shoot.cmd = osMessageQueueNew(2u, sizeof(Shoot_CMD_t),NULL);
   task_runtime.msgq.gimbal.ai.feedback = osMessageQueueNew(2u, sizeof(Gimbal_feedback_t),NULL);
   task_runtime.msgq.gimbal.ai.g_cmd = osMessageQueueNew(2u, sizeof(AI_cmd_t),NULL);
-
+  task_runtime.msgq.shoot.ai.s_cmd = osMessageQueueNew(2u, sizeof(AI_cmd_t),NULL);
+   task_runtime.msgq.RC_REMOTE = osMessageQueueNew(2u, sizeof(CMD_RC_t), NULL);
   /* USER MESSAGE END */
 
   osKernelUnlock(); // 解锁内核

User/task/rc.c

@@ -60,6 +60,8 @@ void Task_rc(void *argument) {
     osMessageQueueReset(task_runtime.msgq.cmd.raw.rc);
     osMessageQueuePut(task_runtime.msgq.cmd.raw.rc, &cmd_rc, 0, 0);
 	
+    osMessageQueueReset(task_runtime.msgq.RC_REMOTE);
+    osMessageQueuePut(task_runtime.msgq.RC_REMOTE, &cmd_rc, 0, 0);
 
     /* USER CODE END */
     osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */

User/task/shoot.c

@@ -4,11 +4,12 @@
 */
 
 /* Includes ----------------------------------------------------------------- */
+#include "module/cmd.h"
 #include "task/user_task.h"
 /* USER INCLUDE BEGIN */
 #include "module/shoot.h"
 #include "module/config.h"
-
+#include "device/ai.h"
 /* USER INCLUDE END */
 
 /* Private typedef ---------------------------------------------------------- */
@@ -18,6 +19,7 @@
 /* USER STRUCT BEGIN */
 Shoot_t shoot;
 Shoot_CMD_t shoot_cmd;
+AI_cmd_t shoot_ai_cmd;
 /* USER STRUCT END */
 
 /* Private function --------------------------------------------------------- */
@@ -42,8 +44,28 @@ Shoot_SetMode(&shoot,SHOOT_MODE_SINGLE);
     tick += delay_tick; /* 计算下一个唤醒时刻 */
     /* USER CODE BEGIN */
 osMessageQueueGet(task_runtime.msgq.shoot.cmd, &shoot_cmd, NULL, 0);
+osMessageQueueGet(task_runtime.msgq.shoot.ai.s_cmd, &shoot_ai_cmd, NULL, 0);
   
+// if(shoot_cmd.control_mode==SHOOT_REMOTE)
+// {
+//   //do nothing，使用遥控器的指令
+// }
+// else if(shoot_cmd.control_mode==SHOOT_AI)
+// {
+//   shoot_cmd.mode = SHOOT_MODE_SINGLE;
+// if(shoot_ai_cmd.mode==0 || shoot_ai_cmd.mode==1)
+// {
+//   shoot_cmd.firecmd=false;
+//   shoot_cmd.ready=true;
 
+// }
+// else if(shoot_ai_cmd.mode==2)
+// {
+  
+//   shoot_cmd.firecmd=true;
+//   shoot_cmd.ready=true;
+// }
+//   }
 
     Shoot_UpdateFeedback(&shoot);
     Shoot_SetMode(&shoot,shoot_cmd.mode);

User/task/user_task.h

@@ -91,6 +91,7 @@ typedef struct {
                osMessageQueueId_t quat;                  
                 osMessageQueueId_t feedback;
                 osMessageQueueId_t g_cmd;
+                
                 }ai;
                  /* 新增的 ai 消息队列 主要是给自瞄*/
             } gimbal;
@@ -98,8 +99,13 @@ typedef struct {
             struct 
             {
                 osMessageQueueId_t cmd;   
+            struct{
+
+                osMessageQueueId_t s_cmd;
+                }ai;
             }shoot;
 
+           osMessageQueueId_t RC_REMOTE; 
         } msgq;
         /* USER MESSAGE END */