add gimbal

gimbal.c

@@ -0,0 +1,323 @@
+/*
+ * 云台模组
+ */
+
+/* Includes ----------------------------------------------------------------- */
+#include "gimbal.h"
+#include "bsp/can.h"
+#include "bsp/time.h"
+#include <math.h> 
+#include "component/filter.h"
+#include "component/limiter.h"
+#include "component/pid.h"
+#include "device/gimbal_imu.h"
+#include "device/motor_dm.h"
+#include "device/motor_rm.h"
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+/* Private function  -------------------------------------------------------- */
+#define GIMBAL_YAW_INERTIA 0.05f 
+#define GIMBAL_PIT_INERTIA 0.03f
+/**
+ * \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;
+
+  // 检查是否relax->absolute
+  int relax_to_absolute = (g->mode == GIMBAL_MODE_RELAX && mode == GIMBAL_MODE_ABSOLUTE);
+
+  PID_Reset(&g->pid.yaw_angle);
+  PID_Reset(&g->pid.yaw_omega);
+  PID_Reset(&g->pid.pit_angle);
+  PID_Reset(&g->pid.pit_omega);
+  PID_Reset(&g->pid.aimbot.yaw_angle);
+  PID_Reset(&g->pid.aimbot.yaw_omega);
+  PID_Reset(&g->pid.aimbot.pit_angle);
+  PID_Reset(&g->pid.aimbot.pit_omega);
+  PID_Reset(&g->pid.recover.yaw_angle);
+  PID_Reset(&g->pid.recover.pit_angle);
+  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 (relax_to_absolute) {
+    // pitch回到限位中点
+    g->setpoint.eulr.pit = 0.5f * (g->limit.pit.max + g->limit.pit.min);
+  } else {
+    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 = (Gimbal_Params_t *)param;
+  g->mode = GIMBAL_MODE_RELAX; /* 设置默认模式 */
+  
+  /* 先初始化CAN */
+  BSP_CAN_Init();
+  
+  /* 初始化云台电机控制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));
+  PID_Init(&(g->pid.aimbot.yaw_angle), KPID_MODE_SET_D, target_freq,
+           &(g->param->pid.yaw_angle));
+  PID_Init(&(g->pid.aimbot.yaw_omega), KPID_MODE_SET_D, target_freq,
+           &(g->param->pid.yaw_omega));
+  PID_Init(&(g->pid.aimbot.pit_angle), KPID_MODE_SET_D, target_freq,
+           &(g->param->pid.pit_angle));
+  PID_Init(&(g->pid.aimbot.pit_omega), KPID_MODE_SET_D, target_freq,
+           &(g->param->pid.pit_omega));
+
+  /* Recover模式PID（单环，编码器反馈） */
+  PID_Init(&(g->pid.recover.yaw_angle), KPID_MODE_NO_D, target_freq,
+           &(g->param->pid.recover.yaw_angle));
+  PID_Init(&(g->pid.recover.pit_angle), KPID_MODE_NO_D, target_freq,
+           &(g->param->pid.recover.pit_angle));
+
+  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;
+  
+  MOTOR_RM_Register(&(g->param->pit_motor));
+  MOTOR_DM_Register(&(g->param->yaw_motor));
+
+  MOTOR_DM_Enable(&(g->param->yaw_motor));
+  
+  /* 最后初始化IMU，确保CAN和电机都已经设置好 */
+  GIMBAL_IMU_Init(&g->imu, &(g->param->imu));
+  
+  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){
+
+  if (gimbal == NULL) {
+    return -1;
+  }
+  GIMBAL_IMU_Update(&gimbal->imu);
+  
+  /* 将IMU数据复制到feedback中供控制使用 */
+  gimbal->feedback.imu.eulr = gimbal->imu.data.eulr;
+  gimbal->feedback.imu.gyro = gimbal->imu.data.gyro;
+  gimbal->feedback.imu.accl = gimbal->imu.data.accl;
+  
+  return 0;
+}
+
+/**
+ * \brief 运行云台控制逻辑
+ *
+ * \param g 包含云台数据的结构体
+ * \param g_cmd 云台控制指令
+ *
+ * \return 函数运行结果
+ */
+int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) {
+  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();
+
+  /* AI自瞄保护：mode_ai==0 丢失目标，保持AI模式但锁定当前姿态 */
+  Gimbal_SetMode(g, g_cmd->mode);
+
+  /* 处理yaw控制命令，软件限位 */
+  float delta_yaw = g_cmd->delta_yaw * g->dt * 1.5f;
+  if (g->param->travel.yaw > 0) {
+    CircleAngleDeltaLimit(&delta_yaw, g->setpoint.eulr.yaw,
+                          g->feedback.motor.yaw.rotor_abs_angle,
+                          g->feedback.imu.eulr.yaw,
+                          g->limit.yaw.max, g->limit.yaw.min, M_2PI);
+  }
+  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) {
+    CircleAngleDeltaLimit(&delta_pit, g->setpoint.eulr.pit,
+                          g->feedback.motor.pit.rotor_abs_angle,
+                          g->feedback.imu.eulr.rol,
+                          g->limit.pit.max, g->limit.pit.min, M_2PI);
+  }
+  CircleAdd(&(g->setpoint.eulr.pit), delta_pit, M_2PI);
+   
+  if (g_cmd->mode == GIMBAL_MODE_AI_CONTROL && g_cmd->mode_ai != 0) {
+    /* AI 模式且有目标(mode_ai==1/2)：跟踪，用AI下发的setpoint */
+    g->setpoint.eulr.yaw = g_cmd->setpoint_yaw;
+    g->setpoint.eulr.pit = g_cmd->setpoint_pit;
+  } else if (g_cmd->mode == GIMBAL_MODE_RECOVER) {
+    /* 双零点选近：mech_zero 和 mech_zero+π，选离当前编码器角度最近的 */
+    float yaw_enc = g->feedback.motor.yaw.rotor_abs_angle;
+    float zero0 = g->param->mech_zero.yaw;
+    float zero1 = zero0 + M_PI;
+    if (zero1 >= M_2PI) zero1 -= M_2PI;
+    float err0 = fabsf(CircleError(zero0, yaw_enc, M_2PI));
+    float err1 = fabsf(CircleError(zero1, yaw_enc, M_2PI));
+    g->setpoint.eulr.yaw = (err0 <= err1) ? zero0 : zero1;
+    g->setpoint.eulr.pit = 0.5f * (g->limit.pit.max + g->limit.pit.min);
+  }
+
+  /* 控制相关逻辑 */
+  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:
+  case GIMBAL_MODE_RELATIVE:
+    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.yaw_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);
+
+    break;
+  case GIMBAL_MODE_AI_CONTROL:
+    if (g_cmd->mode_ai == 0) {
+      /* 丢失目标：setpoint保持不变，用普通PID锁住当前姿态，无前馈 */
+      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.yaw_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);
+    } else {
+      /* mode_ai==1/2：跟踪目标，带前馈 */
+      /* --- YAW --- */
+      yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
+                                     g->feedback.imu.eulr.yaw, 0.0f, g->dt);
+      yaw_omega_set_point += g_cmd->ff_vel_yaw;
+      g->out.yaw = PID_Calc(&(g->pid.yaw_omega), yaw_omega_set_point,
+                            g->feedback.imu.gyro.z, 0.f, g->dt);
+      g->out.yaw += g_cmd->ff_accl_yaw * GIMBAL_YAW_INERTIA;
+
+      /* --- PIT --- */
+      pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), -g->setpoint.eulr.pit,
+                                     g->feedback.imu.eulr.rol, 0.0f, g->dt);
+      pit_omega_set_point -= g_cmd->ff_vel_pit;
+      g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point,
+                            g->feedback.imu.gyro.y, 0.f, g->dt);
+      g->out.pit -= g_cmd->ff_accl_pit * GIMBAL_PIT_INERTIA;
+    }
+    break;
+  case GIMBAL_MODE_RECOVER:
+    g->out.yaw = PID_Calc(&(g->pid.recover.yaw_angle), g->setpoint.eulr.yaw,
+                          g->feedback.motor.yaw.rotor_abs_angle, 0.0f, g->dt);
+    g->out.pit = PID_Calc(&(g->pid.recover.pit_angle), g->setpoint.eulr.pit,
+                          g->feedback.motor.pit.rotor_abs_angle, 0.0f, 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 * 5.0f;
+    output.kd = 0.3f;
+    MOTOR_RM_Ctrl(&g->param->pit_motor);
+    MOTOR_DM_MITCtrl(&g->param->yaw_motor, &output);
+  }
+/**
+ * @brief 导出云台UI数据
+ *
+ * @param g 云台结构体
+ * @param ui UI结构体
+ */
+void Gimbal_DumpUI(const Gimbal_t *g, Gimbal_RefereeUI_t *ui) {
+  ui->mode = g->mode;
+}

gimbal.h

@@ -0,0 +1,224 @@
+/*
+ * 云台模组
+ */
+
+#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"
+#include "device/gimbal_imu.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_AI_CONTROL,  /* AI控制模式，直接接受AI下发的目标角度 */
+  GIMBAL_MODE_RECOVER  /* 自起模式，底盘翻倒后自动恢复到正常姿态 */
+} Gimbal_Mode_t;
+
+/* UI 导出结构（供 referee 系统绘制） */
+typedef struct {
+  Gimbal_Mode_t mode;
+} Gimbal_RefereeUI_t;
+
+typedef struct {
+  Gimbal_Mode_t mode;
+  uint8_t mode_ai; /* AI模式细分，0表示不控制，1表示控制云台但不开火，2表示控制云台且开火 */
+  float delta_yaw;
+  float delta_pit;
+    /* GIMBAL_MODE_AI_CONTROL 速度/加速度前馈（来自 NUC） */
+  float setpoint_yaw;
+  float setpoint_pit;
+  float ff_vel_yaw;    /* yaw 速度前馈（rad/s），叠加到内环角速度设定值 */
+  float ff_vel_pit;    /* pit 速度前馈（rad/s） */
+  float ff_accl_yaw;   /* yaw 加速度前馈（归一化），叠加到力矩输出 */
+  float ff_accl_pit;   /* pit 加速度前馈（归一化） */
+} Gimbal_CMD_t;
+
+typedef struct {
+  bool ctrl;
+  float yaw;
+  float pit;
+} Gimbal_AI_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轴电机参数 */
+  GIMBAL_IMU_Param_t imu;   /* 云台IMU参数 */
+  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参数 */
+    struct {
+      KPID_Params_t yaw_angle; /* yaw轴角位置环PID参数 */
+      KPID_Params_t yaw_omega; /* yaw轴角速度环PID参数 */
+      KPID_Params_t pit_angle; /* pitch轴角位置环PID参数 */
+      KPID_Params_t pit_omega; /* pitch轴角速度环PID参数 */
+    } aimbot;
+    struct {
+      KPID_Params_t yaw_angle; /* recover yaw位置环PID参数 */
+      KPID_Params_t pit_angle; /* recover pitch位置环PID参数 */
+    } recover;
+  } 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 {
+  struct {
+    AHRS_Eulr_t eulr; /* 云台姿态欧拉角 */
+    AHRS_Gyro_t gyro; /* 云台陀螺仪数据 */
+    AHRS_Accl_t accl; /* 云台加速度计数据 */
+  }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; /* 云台模式 */
+  GIMBAL_IMU_t imu; /* 云台IMU数据 */
+
+  /* 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 */
+    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 */
+    } aimbot;
+    struct {
+      KPID_t yaw_angle; /* recover yaw位置环PID */
+      KPID_t pit_angle; /* recover pitch位置环PID */
+    } recover;
+  } 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);
+/**
+ * \brief 运行云台控制逻辑
+ *
+ * \param g 包含云台数据的结构体
+ * \param fb 云台反馈信息
+ * \param g_cmd 云台控制指令
+ * \param dt_sec 两次调用的时间间隔
+ *
+ * \return 函数运行结果
+ */
+int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai);
+
+/**
+ * \brief 云台输出
+ *
+ * \param s 包含云台数据的结构体
+ * \param out CAN设备云台输出结构体
+ */
+void Gimbal_Output(Gimbal_t *g);
+
+#ifdef __cplusplus
+}
+#endif