修改射速

User/module/config.c

@@ -5,10 +5,12 @@
 /* Includes ----------------------------------------------------------------- */
 #include "module/config.h"
 #include "bsp/can.h"
+#include "device/motor_dm.h"
 #include "device/motor_rm.h"
 #include "component/pid.h"
 #include "component/user_math.h"
 #include "device/rc_can.h"
+#include <stdbool.h>
 
 /* Private typedef ---------------------------------------------------------- */
 /* Private define ----------------------------------------------------------- */
@@ -83,6 +85,76 @@ Config_RobotParam_t robot_config = {
     
         
         },
+    .gimbal_param = {
+        .pid = {
+            .yaw_omega = {
+                .k = 0.5f,
+                .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,
+            },
+            .yaw_angle = {
+                .k = 10.0f,
+                .p = 3.0f,
+                .i = 0.0f,
+                .d = 0.1f,
+                .i_limit = 0.0f,
+                .out_limit = 10.0f,
+                .d_cutoff_freq = -1.0f,
+                .range = M_2PI,
+            },
+            .pit_omega = {
+                .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,
+            },
+            .pit_angle = {
+                .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,
+            },
+        },
+        .mech_zero = {
+            .yaw = 0.0f,
+            .pit = 3.8f,
+        },
+        .travel = {
+            .yaw = -1.0f,
+            .pit = 0.8f,
+        },
+        .low_pass_cutoff_freq = {
+            .out = -1.0f,
+            .gyro = 1000.0f,
+        },
+        .pit_motor ={
+            .can = BSP_CAN_2,
+            .id = 0x209,
+            .gear = false,
+            .module = MOTOR_GM6020,
+            .reverse = true,
+        },
+        .yaw_motor = {
+            .can = BSP_CAN_1,
+            .can_id = 0x1,
+            .master_id = 0x11,
+            .module = MOTOR_DM_J4310,
+            .reverse = false,
+        }
+    },
     .rc_can_param = {
         .can = BSP_CAN_1,
         .mode = RC_CAN_MODE_MASTER,

User/module/config.h

@@ -15,7 +15,9 @@ extern "C" {
 
 typedef struct {
     Shoot_Params_t shoot_param;
+    Gimbal_Params_t gimbal_param;
     RC_CAN_Param_t rc_can_param;
+
     // Shoot_Params_t shoot_param; /* 射击参数 */
 } Config_RobotParam_t;
 

User/module/gimbal.c

@@ -0,0 +1,251 @@
+/*
+ * 云台模组
+ */
+
+/* Includes ----------------------------------------------------------------- */
+#include "gimbal.h"
+#include "bsp/can.h"
+#include "bsp/time.h"
+#include "component/filter.h"
+#include "component/pid.h"
+#include "device/motor_dm.h"
+#include "device/motor_rm.h"
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+/* Private function  -------------------------------------------------------- */
+
+/**
+ * \brief 设置云台模式
+ *
+ * \param c 包含云台数据的结构体
+ * \param mode 要设置的模式
+ *
+ * \return 函数运行结果
+ */
+static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
+  if (g == NULL)
+    return -1;
+  if (mode == g->mode)
+    return GIMBAL_OK;
+
+  PID_Reset(&g->pid.yaw_angle);
+  PID_Reset(&g->pid.yaw_omega);
+  PID_Reset(&g->pid.pit_angle);
+  PID_Reset(&g->pid.pit_omega);
+  LowPassFilter2p_Reset(&g->filter_out.yaw, 0.0f);
+  LowPassFilter2p_Reset(&g->filter_out.pit, 0.0f);
+
+  MOTOR_DM_Enable(&(g->param->yaw_motor));
+  
+  AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */
+  // if (g->mode == GIMBAL_MODE_RELAX) {
+  //   if (mode == GIMBAL_MODE_ABSOLUTE) {
+  //     g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
+  //   } else if (mode == GIMBAL_MODE_RELATIVE) {
+  //     g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
+  //   }
+  // }
+  g->setpoint.eulr.pit = g->feedback.imu.eulr.rol;
+  g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
+
+  g->mode = mode;
+  return 0;
+}
+
+/* Exported functions ------------------------------------------------------- */
+
+/**
+ * \brief 初始化云台
+ *
+ * \param g 包含云台数据的结构体
+ * \param param 包含云台参数的结构体指针
+ * \param target_freq 任务预期的运行频率
+ *
+ * \return 函数运行结果
+ */
+int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param,
+                   float target_freq) {
+  if (g == NULL)
+    return -1;
+
+  g->param = param;
+  g->mode = GIMBAL_MODE_RELAX; /* 设置默认模式 */
+
+  /* 初始化云台电机控制PID和LPF */
+  PID_Init(&(g->pid.yaw_angle), KPID_MODE_NO_D, target_freq,
+           &(g->param->pid.yaw_angle));
+  PID_Init(&(g->pid.yaw_omega), KPID_MODE_CALC_D, target_freq,
+           &(g->param->pid.yaw_omega));
+  PID_Init(&(g->pid.pit_angle), KPID_MODE_NO_D, target_freq,
+           &(g->param->pid.pit_angle));
+  PID_Init(&(g->pid.pit_omega), KPID_MODE_CALC_D, target_freq,
+           &(g->param->pid.pit_omega));
+
+  LowPassFilter2p_Init(&g->filter_out.yaw, target_freq,
+                       g->param->low_pass_cutoff_freq.out);
+  LowPassFilter2p_Init(&g->filter_out.pit, target_freq,
+                       g->param->low_pass_cutoff_freq.out);
+  g->limit.yaw.max = g->param->mech_zero.yaw + g->param->travel.yaw;
+  g->limit.yaw.min = g->param->mech_zero.yaw;
+  g->limit.pit.max = g->param->mech_zero.pit + g->param->travel.pit;
+  g->limit.pit.min = g->param->mech_zero.pit;
+  BSP_CAN_Init();
+    
+  MOTOR_RM_Register(&(g->param->pit_motor));
+  MOTOR_DM_Register(&(g->param->yaw_motor));
+
+  MOTOR_DM_Enable(&(g->param->yaw_motor));
+  return 0;
+}
+
+/**
+ * \brief 通过CAN设备更新云台反馈信息
+ *
+ * \param gimbal 云台
+ * \param can CAN设备
+ *
+ * \return 函数运行结果
+ */
+int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal) {
+  if (gimbal == NULL)
+    return -1;
+
+  /* 更新RM电机反馈数据（pitch轴） */
+  MOTOR_RM_Update(&(gimbal->param->pit_motor));
+  MOTOR_RM_t *rm_motor = MOTOR_RM_GetMotor(&(gimbal->param->pit_motor));
+  if (rm_motor != NULL) {
+    gimbal->feedback.motor.pit = rm_motor->feedback;
+  }
+
+  /* 更新DM电机反馈数据（yaw轴） */
+  MOTOR_DM_Update(&(gimbal->param->yaw_motor));
+  MOTOR_DM_t *dm_motor = MOTOR_DM_GetMotor(&(gimbal->param->yaw_motor));
+  if (dm_motor != NULL) {
+    gimbal->feedback.motor.yaw = dm_motor->motor.feedback;
+  }
+
+  return 0;
+}
+
+int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){
+
+  if (gimbal == NULL) {
+    return -1;
+  }
+  gimbal->feedback.imu.gyro = imu->gyro;
+  gimbal->feedback.imu.eulr = imu->eulr;
+}
+
+/**
+ * \brief 运行云台控制逻辑
+ *
+ * \param g 包含云台数据的结构体
+ * \param g_cmd 云台控制指令
+ *
+ * \return 函数运行结果
+ */
+int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
+  if (g == NULL || g_cmd == NULL) {
+    return -1;
+  }
+
+  g->dt = (BSP_TIME_Get_us() - g->lask_wakeup) / 1000000.0f;
+  g->lask_wakeup = BSP_TIME_Get_us();
+
+  Gimbal_SetMode(g, g_cmd->mode);
+
+  /* 处理yaw控制命令，软件限位 - 使用电机绝对角度 */
+  float delta_yaw = g_cmd->delta_yaw * g->dt;
+  if (g->param->travel.yaw > 0) {
+    /* 计算当前电机角度与IMU角度的偏差 */
+    float motor_imu_offset = g->feedback.motor.yaw.rotor_abs_angle - g->feedback.imu.eulr.yaw;
+    /* 处理跨越±π的情况 */
+    if (motor_imu_offset > M_PI) motor_imu_offset -= M_2PI;
+    if (motor_imu_offset < -M_PI) motor_imu_offset += M_2PI;
+    
+    /* 计算到限位边界的距离 */
+    const float delta_max = CircleError(g->limit.yaw.max,
+        (g->setpoint.eulr.yaw + motor_imu_offset + delta_yaw), M_2PI);
+    const float delta_min = CircleError(g->limit.yaw.min,
+        (g->setpoint.eulr.yaw + motor_imu_offset + delta_yaw), M_2PI);
+    
+    /* 限制控制命令 */
+    if (delta_yaw > delta_max) delta_yaw = delta_max;
+    if (delta_yaw < delta_min) delta_yaw = delta_min;
+  }
+  CircleAdd(&(g->setpoint.eulr.yaw), delta_yaw, M_2PI);
+
+  /* 处理pitch控制命令，软件限位 - 使用电机绝对角度 */
+  float delta_pit = g_cmd->delta_pit * g->dt;
+  if (g->param->travel.pit > 0) {
+    /* 计算当前电机角度与IMU角度的偏差 */
+    float motor_imu_offset = g->feedback.motor.pit.rotor_abs_angle - g->feedback.imu.eulr.rol;
+    /* 处理跨越±π的情况 */
+    if (motor_imu_offset > M_PI) motor_imu_offset -= M_2PI;
+    if (motor_imu_offset < -M_PI) motor_imu_offset += M_2PI;
+    
+    /* 计算到限位边界的距离 */
+    const float delta_max = CircleError(g->limit.pit.max,
+        (g->setpoint.eulr.pit + motor_imu_offset + delta_pit), M_2PI);
+    const float delta_min = CircleError(g->limit.pit.min,
+        (g->setpoint.eulr.pit + motor_imu_offset + delta_pit), M_2PI);
+    
+    /* 限制控制命令 */
+    if (delta_pit > delta_max) delta_pit = delta_max;
+    if (delta_pit < delta_min) delta_pit = delta_min;
+  }
+  CircleAdd(&(g->setpoint.eulr.pit), delta_pit, M_2PI);
+
+  /* 控制相关逻辑 */
+  float yaw_omega_set_point, pit_omega_set_point;
+  switch (g->mode) {
+  case GIMBAL_MODE_RELAX:
+    g->out.yaw = 0.0f;
+    g->out.pit = 0.0f;
+    break;
+
+  case GIMBAL_MODE_ABSOLUTE:
+    yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
+                                   g->feedback.imu.eulr.yaw, 0.0f, g->dt);
+    g->out.yaw = PID_Calc(&(g->pid.pit_omega), yaw_omega_set_point,
+                          g->feedback.imu.gyro.z, 0.f, g->dt);
+
+    pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), g->setpoint.eulr.pit,
+                                   g->feedback.imu.eulr.rol, 0.0f, g->dt);
+    g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point,
+                          g->feedback.imu.gyro.y, 0.f, g->dt);
+    // yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
+    //                                g->feedback.motor.yaw.rotor_abs_angle, 0.0f, g->dt);
+    // g->out.yaw = PID_Calc(&(g->pid.pit_omega), yaw_omega_set_point,
+    //                       g->feedback.imu.gyro.z, 0.f, g->dt);
+
+    // pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), g->setpoint.eulr.pit,
+    //                                g->feedback.motor.pit.rotor_abs_angle, 0.0f, g->dt);
+    // g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point,
+    //                       g->feedback.imu.gyro.y, 0.f, g->dt);
+    break;
+
+    /* 输出滤波 */
+    g->out.yaw = LowPassFilter2p_Apply(&g->filter_out.yaw, g->out.yaw);
+    g->out.pit = LowPassFilter2p_Apply(&g->filter_out.pit, g->out.pit);
+
+    return 0;
+  }
+}
+
+  /**
+   * \brief 云台输出
+   *
+   * \param s 包含云台数据的结构体
+   * \param out CAN设备云台输出结构体
+   */
+void Gimbal_Output(Gimbal_t *g){
+    MOTOR_RM_SetOutput(&g->param->pit_motor, g->out.pit);
+    MOTOR_MIT_Output_t output = {0};
+    output.torque = g->out.yaw * 2.5;
+    MOTOR_RM_Ctrl(&g->param->pit_motor);
+    MOTOR_DM_MITCtrl(&g->param->yaw_motor, &output);
+  }

User/module/gimbal.h

@@ -0,0 +1,180 @@
+/*
+ * 云台模组
+ */
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include "component/ahrs.h"
+#include "component/filter.h"
+#include "component/pid.h"
+#include "device/motor.h"
+#include "device/motor_dm.h"
+#include "device/motor_rm.h"
+
+/* Exported constants ------------------------------------------------------- */
+#define GIMBAL_OK (0)        /* 运行正常 */
+#define GIMBAL_ERR (-1)      /* 运行时发现了其他错误 */
+#define GIMBAL_ERR_NULL (-2) /* 运行时发现NULL指针 */
+#define GIMBAL_ERR_MODE (-3) /* 运行时配置了错误的CMD_GimbalMode_t */
+
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+
+typedef enum {
+  GIMBAL_MODE_RELAX,    /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */
+  GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */
+  GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */
+} Gimbal_Mode_t;
+
+typedef struct {
+  Gimbal_Mode_t mode;
+  float delta_yaw;
+  float delta_pit;
+} Gimbal_CMD_t;
+
+/* 软件限位 */
+typedef struct {
+  float max;
+  float min;
+} Gimbal_Limit_t;
+
+/* 云台参数的结构体，包含所有初始化用的参数，通常是const，存好几组。*/
+typedef struct {
+  MOTOR_RM_Param_t pit_motor; /* pitch轴电机参数 */
+  MOTOR_DM_Param_t yaw_motor; /* yaw轴电机参数 */
+  struct {
+    KPID_Params_t yaw_omega; /* yaw轴角速度环PID参数 */
+    KPID_Params_t yaw_angle; /* yaw轴角位置环PID参数 */
+    KPID_Params_t pit_omega; /* pitch轴角速度环PID参数 */
+    KPID_Params_t pit_angle; /* pitch轴角位置环PID参数 */
+  } pid;
+
+  /* 低通滤波器截止频率 */
+  struct {
+    float out;  /* 电机输出 */
+    float gyro; /* 陀螺仪数据 */
+  } low_pass_cutoff_freq;
+
+  struct {
+    float yaw; /* yaw轴机械限位 */
+    float pit; /* pitch轴机械限位 */
+  } mech_zero;
+
+  struct {
+    float yaw; /* yaw轴机械限位行程 -1表示无限位 */
+    float pit; /* pitch轴机械限位行程 -1表示无限位*/
+  } travel;
+
+} Gimbal_Params_t;
+
+typedef struct {
+  AHRS_Gyro_t gyro;
+  AHRS_Eulr_t eulr;
+} Gimbal_IMU_t;
+/* 云台反馈数据的结构体，包含反馈控制用的反馈数据 */
+typedef struct {
+  Gimbal_IMU_t imu;
+  struct {
+    MOTOR_Feedback_t yaw; /* yaw轴电机反馈 */
+    MOTOR_Feedback_t pit; /* pitch轴电机反馈 */
+  } motor;
+} Gimbal_Feedback_t;
+
+/* 云台输出数据的结构体*/
+typedef struct {
+  float yaw; /* yaw轴电机输出 */
+  float pit; /* pitch轴电机输出 */
+} Gimbal_Output_t;
+/*
+ * 运行的主结构体，所有这个文件里的函数都在操作这个结构体。
+ * 包含了初始化参数，中间变量，输出变量。
+ */
+typedef struct {
+  uint64_t lask_wakeup;
+  float dt;
+
+  Gimbal_Params_t *param; /* 云台的参数，用Gimbal_Init设定 */
+
+  /* 模块通用 */
+  Gimbal_Mode_t mode; /* 云台模式 */
+
+  /* PID计算的目标值 */
+  struct {
+    AHRS_Eulr_t eulr; /* 表示云台姿态的欧拉角 */
+  } setpoint;
+
+  struct {
+    KPID_t yaw_angle; /* yaw轴角位置环PID */
+    KPID_t yaw_omega; /* yaw轴角速度环PID */
+    KPID_t pit_angle; /* pitch轴角位置环PID */
+    KPID_t pit_omega; /* pitch轴角速度环PID */
+  } pid;
+
+  struct {
+    Gimbal_Limit_t yaw;
+    Gimbal_Limit_t pit;
+  } limit;
+
+  struct {
+    LowPassFilter2p_t yaw;
+    LowPassFilter2p_t pit;
+  } filter_out;
+
+  Gimbal_Output_t out;        /* 云台输出 */
+  Gimbal_Feedback_t feedback; /* 反馈 */
+
+} Gimbal_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+
+/**
+ * \brief 初始化云台
+ *
+ * \param g 包含云台数据的结构体
+ * \param param 包含云台参数的结构体指针
+ * \param target_freq 任务预期的运行频率
+ *
+ * \return 函数运行结果
+ */
+int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param,
+                   float target_freq);
+
+/**
+ * \brief 通过CAN设备更新云台反馈信息
+ *
+ * \param gimbal 云台
+ * \param can CAN设备
+ *
+ * \return 函数运行结果
+ */
+int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal);
+
+int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu);
+/**
+ * \brief 运行云台控制逻辑
+ *
+ * \param g 包含云台数据的结构体
+ * \param fb 云台反馈信息
+ * \param g_cmd 云台控制指令
+ * \param dt_sec 两次调用的时间间隔
+ *
+ * \return 函数运行结果
+ */
+int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd);
+
+/**
+ * \brief 云台输出
+ *
+ * \param s 包含云台数据的结构体
+ * \param out CAN设备云台输出结构体
+ */
+void Gimbal_Output(Gimbal_t *g);
+
+#ifdef __cplusplus
+}
+#endif

User/module/shoot.c

@@ -116,7 +116,7 @@ int8_t Shoot_Control(Shoot_t *c, const Shoot_CMD_t *cmd)
                 for(int i=0;i<SHOOT_FRIC_NUM;i++)
                 {	/* 计算跟随输出->计算修正输出->加和、滤波、输出 */ 
                     // c->output.out_follow[i]=PID_Calc(&c->pid.fric_follow[i],c->target_variable.target_rpm/MAX_FRIC_RPM,c->feedback.fric_rpm[i],0,c->dt);
-                    c->output.out_follow[i]=PID_Calc(&c->pid.fric_follow[i],300.0f/MAX_FRIC_RPM,c->feedback.fric_rpm[i],0,c->dt);
+                    c->output.out_follow[i]=PID_Calc(&c->pid.fric_follow[i],3000.0f/MAX_FRIC_RPM,c->feedback.fric_rpm[i],0,c->dt);
                     
                     c->output.out_err[i]=PID_Calc(&c->pid.fric_err[i],c->feedback.fric_avgrpm,c->feedback.fric_rpm[i],0,c->dt);
                     ScaleSumTo1(&c->output.out_follow[i], &c->output.out_err[i]);

User/task/atti_esti.c

@@ -4,13 +4,14 @@
 */
 
 /* Includes ----------------------------------------------------------------- */
+#include "cmsis_os2.h"
 #include "task/user_task.h"
 /* USER INCLUDE BEGIN */
 #include "bsp/pwm.h"
 #include "component/ahrs.h"
 #include "component/pid.h"
 #include "device/bmi088.h"
-
+#include "module/gimbal.h"
 /* USER INCLUDE END */
 
 /* Private typedef ---------------------------------------------------------- */
@@ -26,9 +27,13 @@ AHRS_Eulr_t eulr_to_send;
 
 KPID_t imu_temp_ctrl_pid;
 
+Gimbal_IMU_t gimbal_to_send;
+
 BMI088_Cali_t cali_bmi088= {
   .gyro_offset = {0.0f,0.0f,0.0f},
 };
+
+
 /* USER STRUCT END */
 
 /* Private function --------------------------------------------------------- */
@@ -68,6 +73,12 @@ void Task_atti_esti(void *argument) {
     AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs);
     osKernelUnlock();
 
+    gimbal_to_send.eulr = eulr_to_send;
+    gimbal_to_send.gyro = bmi088.gyro;
+
+    osMessageQueueReset(task_runtime.msgq.gimbal.imu);
+    osMessageQueuePut(task_runtime.msgq.gimbal.imu, &gimbal_to_send, 0, 0);
+
     /* USER CODE END */
   }
   

User/task/ctrl_gimbal.c

@@ -4,9 +4,12 @@
 */
 
 /* Includes ----------------------------------------------------------------- */
+#include "cmsis_os2.h"
+#include "component/ahrs.h"
 #include "task/user_task.h"
 /* USER INCLUDE BEGIN */
-
+#include "module/gimbal.h"
+#include "module/config.h"
 /* USER INCLUDE END */
 
 /* Private typedef ---------------------------------------------------------- */
@@ -14,7 +17,9 @@
 /* Private macro ------------------------------------------------------------ */
 /* Private variables -------------------------------------------------------- */
 /* USER STRUCT BEGIN */
-
+Gimbal_t gimbal;
+Gimbal_IMU_t gimbal_imu;
+Gimbal_CMD_t gimbal_cmd;
 /* USER STRUCT END */
 
 /* Private function --------------------------------------------------------- */
@@ -30,13 +35,22 @@ void Task_ctrl_gimbal(void *argument) {
 
   uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
   /* USER CODE INIT BEGIN */
-
+  Gimbal_Init(&gimbal, &Config_GetRobotParam()->gimbal_param, CTRL_GIMBAL_FREQ);
   /* USER CODE INIT END */
   
   while (1) {
     tick += delay_tick; /* 计算下一个唤醒时刻 */
     /* USER CODE BEGIN */
+    if (osMessageQueueGet(task_runtime.msgq.gimbal.imu, &gimbal_imu, NULL, 0) == osOK) {
+      Gimbal_UpdateIMU(&gimbal, &gimbal_imu);
+    }
+    osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &gimbal_cmd, NULL, 0);
 
+    Gimbal_UpdateFeedback(&gimbal);
+
+    Gimbal_Control(&gimbal, &gimbal_cmd);
+
+    Gimbal_Output(&gimbal);
     /* USER CODE END */
     osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
   }

User/task/init.c

@@ -8,6 +8,8 @@
 
 /* USER INCLUDE BEGIN */
 #include "module/shoot.h"
+#include "module/gimbal.h"
+#include "component/ahrs.h"
 /* USER INCLUDE END */
 
 /* Private typedef ---------------------------------------------------------- */
@@ -40,7 +42,9 @@ void Task_Init(void *argument) {
   // 创建消息队列
   /* USER MESSAGE BEGIN */
   task_runtime.msgq.user_msg= osMessageQueueNew(2u, 10, NULL);
-  task_runtime.msgq.shoot.shoot_cmd=osMessageQueueNew(2u, sizeof(Shoot_CMD_t), NULL);
+  task_runtime.msgq.shoot.shoot_cmd = osMessageQueueNew(2u, sizeof(Shoot_CMD_t), NULL);
+  task_runtime.msgq.gimbal.imu= osMessageQueueNew(2u, sizeof(Gimbal_IMU_t), NULL);
+  task_runtime.msgq.gimbal.cmd= osMessageQueueNew(2u, sizeof(Gimbal_CMD_t), NULL);
   /* USER MESSAGE END */
 
   osKernelUnlock(); // 解锁内核

User/task/rc.c

@@ -11,6 +11,7 @@
 #include "device/rc_can.h"
 #include "module/config.h"
 #include "module/shoot.h"
+#include "module/gimbal.h"
 #include <stdbool.h>
 // #include
 /* USER INCLUDE END */
@@ -23,6 +24,7 @@
 DR16_t dr16;
 RC_CAN_t rc_can;
 Shoot_CMD_t for_shoot;
+Gimbal_CMD_t cmd_for_gimbal;
 /* USER STRUCT END */
 
 /* Private function --------------------------------------------------------- */
@@ -75,6 +77,32 @@ void Task_rc(void *argument) {
 
     RC_CAN_SendData(&rc_can,  RC_CAN_DATA_JOYSTICK);
     RC_CAN_SendData(&rc_can,  RC_CAN_DATA_SWITCH);
+
+    switch (dr16.data.sw_l) {
+    case DR16_SW_UP:
+      cmd_for_gimbal.mode = GIMBAL_MODE_RELAX;
+      cmd_for_gimbal.delta_yaw = 0.0f;
+      cmd_for_gimbal.delta_pit = 0.0f;
+      break;
+    case DR16_SW_MID:
+      cmd_for_gimbal.mode = GIMBAL_MODE_ABSOLUTE;
+      cmd_for_gimbal.delta_yaw = -dr16.data.ch_r_x*5.0f;
+      cmd_for_gimbal.delta_pit = dr16.data.ch_r_y*5.0f;
+      break;
+    case DR16_SW_DOWN:
+      cmd_for_gimbal.mode = GIMBAL_MODE_RELATIVE;
+      cmd_for_gimbal.delta_yaw = -dr16.data.ch_r_x* 5.0f;
+      cmd_for_gimbal.delta_pit = dr16.data.ch_r_y*5.0f;
+      break;
+    default:
+      cmd_for_gimbal.mode = GIMBAL_MODE_RELAX;
+      cmd_for_gimbal.delta_yaw = 0.0f;
+      cmd_for_gimbal.delta_pit = 0.0f;
+      break;
+    }
+    osMessageQueueReset(task_runtime.msgq.gimbal.cmd);
+    osMessageQueuePut(task_runtime.msgq.gimbal.cmd, &cmd_for_gimbal, 0, 0);
+
     osMessageQueueReset(task_runtime.msgq.shoot.shoot_cmd);
     osMessageQueuePut(task_runtime.msgq.shoot.shoot_cmd, &for_shoot, 0, 0);
     /* USER CODE END */

User/task/user_task.h

@@ -49,6 +49,10 @@ typedef struct {
         struct {
             osMessageQueueId_t shoot_cmd; /* 发射命令队列 */
         }shoot;
+        struct {
+            osMessageQueueId_t imu;
+            osMessageQueueId_t cmd;
+        }gimbal;
     } msgq;
     /* USER MESSAGE END */