zimiao

CMakeLists.txt

@@ -78,7 +78,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     User/device/motor_dm.c
     User/device/motor_rm.c
     User/device/supercap.c
-
+    User/device/gimbal_imu.c
     # User/module sources
     User/module/chassis.c
 

Core/Src/fdcan.c

@@ -106,7 +106,7 @@ void MX_FDCAN2_Init(void)
   hfdcan2.Init.ExtFiltersNbr = 1;
   hfdcan2.Init.RxFifo0ElmtsNbr = 0;
   hfdcan2.Init.RxFifo0ElmtSize = FDCAN_DATA_BYTES_8;
-  hfdcan2.Init.RxFifo1ElmtsNbr = 16;
+  hfdcan2.Init.RxFifo1ElmtsNbr = 20;
   hfdcan2.Init.RxFifo1ElmtSize = FDCAN_DATA_BYTES_8;
   hfdcan2.Init.RxBuffersNbr = 0;
   hfdcan2.Init.RxBufferSize = FDCAN_DATA_BYTES_8;

User/bsp/fdcan.c

@@ -2,6 +2,7 @@
 #include "fdcan.h"
 #include "bsp/fdcan.h"
 #include "bsp/bsp.h"
+#include "stm32h7xx_hal_fdcan.h"
 #include <fdcan.h>
 #include <cmsis_os2.h>
 #include <string.h>
@@ -27,13 +28,13 @@
   #define FDCAN2_FILTER_CONFIG_TABLE(X) \
     X(0,       FDCAN_STANDARD_ID,   FDCAN_FILTER_MASK,   0x000     ,   0x000     ,   0) \
     X(1,       FDCAN_EXTENDED_ID,   FDCAN_FILTER_MASK,   0x00000000,   0x00000000,   0) 
-    #define FDCAN2_GLOBAL_FILTER FDCAN_REJECT, FDCAN_REJECT, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE/* 全局过滤器参数（用于 HAL_FDCAN_ConfigGlobalFilter） */
+    #define FDCAN2_GLOBAL_FILTER FDCAN_ACCEPT_IN_RX_FIFO1, FDCAN_ACCEPT_IN_RX_FIFO1, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE/* 全局过滤器参数（用于 HAL_FDCAN_ConfigGlobalFilter） */
 #endif
 #ifdef FDCAN3_EN
   #define FDCAN3_FILTER_CONFIG_TABLE(X) \
     X(0,       FDCAN_STANDARD_ID,   FDCAN_FILTER_MASK,   0x000     ,   0x000     ,   0) \
     X(1,       FDCAN_EXTENDED_ID,   FDCAN_FILTER_MASK,   0x00000000,   0x00000000,   0) 
-  #define FDCAN3_GLOBAL_FILTER FDCAN_REJECT, FDCAN_REJECT, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE/* 全局过滤器参数（用于 HAL_FDCAN_ConfigGlobalFilter） */
+  #define FDCAN3_GLOBAL_FILTER FDCAN_REJECT, FDCAN_ACCEPT_IN_RX_FIFO1, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE/* 全局过滤器参数（用于 HAL_FDCAN_ConfigGlobalFilter） */
 #endif
 
 /* ====宏展开实现==== */
@@ -65,13 +66,21 @@ typedef struct BSP_FDCAN_QueueNode {
 } BSP_FDCAN_QueueNode_t;
 
 /* Private variables -------------------------------------------------------- */
-static BSP_FDCAN_QueueNode_t *queue_list = NULL;
+// static BSP_FDCAN_QueueNode_t *queue_list = NULL;
-static osMutexId_t queue_mutex = NULL;
+// static osMutexId_t queue_mutex = NULL;
-static void (*FDCAN_Callback[BSP_FDCAN_NUM][HAL_FDCAN_CB_NUM])(void);
+// static void (*FDCAN_Callback[BSP_FDCAN_NUM][HAL_FDCAN_CB_NUM])(void);
-static bool inited = false;
+// static bool inited = false;
-static BSP_FDCAN_IdParser_t id_parser = NULL;
+// static BSP_FDCAN_IdParser_t id_parser = NULL;
-static BSP_FDCAN_TxQueue_t tx_queues[BSP_FDCAN_NUM];
+// static BSP_FDCAN_TxQueue_t tx_queues[BSP_FDCAN_NUM];
-static const uint8_t fdcan_dlc2len[16] = {0,1,2,3,4,5,6,7,8,12,16,20,24,32,48,64};
+// static const uint8_t fdcan_dlc2len[16] = {0,1,2,3,4,5,6,7,8,12,16,20,24,32,48,64};
+
+ BSP_FDCAN_QueueNode_t *queue_list = NULL;
+ osMutexId_t queue_mutex = NULL;
+ void (*FDCAN_Callback[BSP_FDCAN_NUM][HAL_FDCAN_CB_NUM])(void);
+ bool inited = false;
+ BSP_FDCAN_IdParser_t id_parser = NULL;
+ BSP_FDCAN_TxQueue_t tx_queues[BSP_FDCAN_NUM];
+ const uint8_t fdcan_dlc2len[16] = {0,1,2,3,4,5,6,7,8,12,16,20,24,32,48,64};
 
 /* Private function prototypes ---------------------------------------------- */
 static BSP_FDCAN_t FDCAN_Get(FDCAN_HandleTypeDef *hfdcan);
@@ -578,4 +587,42 @@ uint32_t BSP_FDCAN_ParseId(uint32_t original_id, BSP_FDCAN_FrameType_t frame_typ
     return BSP_FDCAN_DefaultIdParser(original_id, frame_type);
 }
 /* */
+int8_t BSP_FDCAN_WaitTxMailboxEmpty(BSP_FDCAN_t can, uint32_t timeout) {
+    if (!inited) {
+        return BSP_ERR_INITED;
+    }
+    if (can >= BSP_FDCAN_NUM) {
+        return BSP_ERR;
+    }
     
+    FDCAN_HandleTypeDef *hfdcan = BSP_FDCAN_GetHandle(can);
+    if (hfdcan == NULL) {
+        return BSP_ERR_NULL;
+    }
+    
+    uint32_t start_time = HAL_GetTick();
+    
+    // 如果超时时间为0，立即检查并返回
+    if (timeout == 0) {
+        uint32_t free_level = HAL_FDCAN_GetTxFifoFreeLevel(hfdcan);
+        return (free_level > 0) ? BSP_OK : BSP_ERR_TIMEOUT;
+    }
+    
+    // 等待至少有一个 FIFO 空闲
+    while (true) {
+        uint32_t free_level = HAL_FDCAN_GetTxFifoFreeLevel(hfdcan);
+        if (free_level > 0) {
+            return BSP_OK;
+        }
+        
+        // 检查超时
+        if (timeout != BSP_FDCAN_TIMEOUT_FOREVER) {
+            uint32_t elapsed = HAL_GetTick() - start_time;
+            if (elapsed >= timeout) {
+                return BSP_ERR_TIMEOUT;
+            }
+        }
+        
+        osDelay(1);
+    }
+}

User/bsp/fdcan.h

@@ -132,6 +132,7 @@ int32_t BSP_FDCAN_GetQueueCount(BSP_FDCAN_t can, uint32_t can_id);
 int8_t BSP_FDCAN_FlushQueue(BSP_FDCAN_t can, uint32_t can_id);
 int8_t BSP_FDCAN_RegisterIdParser(BSP_FDCAN_IdParser_t parser);
 uint32_t BSP_FDCAN_ParseId(uint32_t original_id, BSP_FDCAN_FrameType_t frame_type);
+int8_t BSP_FDCAN_WaitTxMailboxEmpty(BSP_FDCAN_t can, uint32_t timeout);
 #ifdef __cplusplus
 }
 #endif

User/device/dm_imu.c

@@ -0,0 +1,271 @@
+/*
+    DM_IMU数据获取（CAN）
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "dm_imu.h"
+
+#include "bsp/fdcan.h"
+#include "bsp/time.h"
+#include "component/user_math.h"
+#include <string.h>
+
+/* Private define ----------------------------------------------------------- */
+#define DM_IMU_OFFLINE_TIMEOUT      1000    // 设备离线判定时间1000ms
+
+#define ACCEL_CAN_MAX (58.8f)
+#define ACCEL_CAN_MIN	(-58.8f)
+#define GYRO_CAN_MAX	(34.88f)
+#define GYRO_CAN_MIN	(-34.88f)
+#define PITCH_CAN_MAX	(90.0f)
+#define PITCH_CAN_MIN	(-90.0f)
+#define ROLL_CAN_MAX	(180.0f)
+#define ROLL_CAN_MIN	(-180.0f)
+#define YAW_CAN_MAX		(180.0f)
+#define YAW_CAN_MIN 	(-180.0f)
+#define TEMP_MIN			(0.0f)
+#define TEMP_MAX			(60.0f)
+#define Quaternion_MIN	(-1.0f)
+#define Quaternion_MAX	(1.0f)
+
+
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* Private variables -------------------------------------------------------- */
+/* Private function --------------------------------------------------------- */
+
+static uint8_t count = 0; // 计数器，用于判断设备是否离线
+/**
+ * @brief: 无符号整数转换为浮点数函数
+ */
+static float uint_to_float(int x_int, float x_min, float x_max, int bits)
+{
+    float span = x_max - x_min;
+    float offset = x_min;
+    return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
+}
+
+/**
+ * @brief 解析加速度计数据
+ */
+static int8_t DM_IMU_ParseAccelData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
+    if (imu == NULL || data == NULL || len < 8) {
+        return DEVICE_ERR;
+    }
+    int8_t temp = data[1];
+    uint16_t acc_x_raw = (data[3] << 8) | data[2];
+    uint16_t acc_y_raw = (data[5] << 8) | data[4];
+    uint16_t acc_z_raw = (data[7] << 8) | data[6];
+    imu->data.temp = (float)temp;
+    imu->data.accl.x = uint_to_float(acc_x_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16);
+    imu->data.accl.y = uint_to_float(acc_y_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16);
+    imu->data.accl.z = uint_to_float(acc_z_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16);
+    return DEVICE_OK;
+}
+/**
+ * @brief 解析陀螺仪数据
+ */
+static int8_t DM_IMU_ParseGyroData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
+    if (imu == NULL || data == NULL || len < 8) {
+        return DEVICE_ERR;
+    }
+    uint16_t gyro_x_raw = (data[3] << 8) | data[2];
+    uint16_t gyro_y_raw = (data[5] << 8) | data[4];
+    uint16_t gyro_z_raw = (data[7] << 8) | data[6];
+    imu->data.gyro.x = uint_to_float(gyro_x_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16);
+    imu->data.gyro.y = uint_to_float(gyro_y_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16);
+    imu->data.gyro.z = uint_to_float(gyro_z_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16);
+    return DEVICE_OK;
+}
+/**
+ * @brief 解析欧拉角数据
+ */
+static int8_t DM_IMU_ParseEulerData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
+    if (imu == NULL || data == NULL || len < 8) {
+        return DEVICE_ERR;
+    }
+    uint16_t pit_raw = (data[3] << 8) | data[2];
+    uint16_t yaw_raw = (data[5] << 8) | data[4];
+    uint16_t rol_raw = (data[7] << 8) | data[6];
+    imu->data.euler.pit = uint_to_float(pit_raw, PITCH_CAN_MIN, PITCH_CAN_MAX, 16) * M_DEG2RAD_MULT;
+    imu->data.euler.yaw = uint_to_float(yaw_raw, YAW_CAN_MIN, YAW_CAN_MAX, 16) * M_DEG2RAD_MULT;
+    imu->data.euler.rol = uint_to_float(rol_raw, ROLL_CAN_MIN, ROLL_CAN_MAX, 16) * M_DEG2RAD_MULT;
+    return DEVICE_OK;
+}
+
+/**
+ * @brief 解析四元数数据
+ */
+static int8_t DM_IMU_ParseQuaternionData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
+    if (imu == NULL || data == NULL || len < 8) {
+        return DEVICE_ERR;
+    }
+    int w = (data[1] << 6) | ((data[2] & 0xF8) >> 2);
+    int x = ((data[2] & 0x03) << 12) | (data[3] << 4) | ((data[4] & 0xF0) >> 4);
+    int y = ((data[4] & 0x0F) << 10) | (data[5] << 2) | ((data[6] & 0xC0) >> 6);
+    int z = ((data[6] & 0x3F) << 8) | data[7];
+    imu->data.quat.q0 = uint_to_float(w, Quaternion_MIN, Quaternion_MAX, 14);
+    imu->data.quat.q1 = uint_to_float(x, Quaternion_MIN, Quaternion_MAX, 14);
+    imu->data.quat.q2 = uint_to_float(y, Quaternion_MIN, Quaternion_MAX, 14);
+    imu->data.quat.q3 = uint_to_float(z, Quaternion_MIN, Quaternion_MAX, 14);
+    return DEVICE_OK;
+}
+
+
+/* Exported functions ------------------------------------------------------- */
+
+/**
+ * @brief 初始化DM IMU设备
+ */
+int8_t DM_IMU_Init(DM_IMU_t *imu, DM_IMU_Param_t *param) {
+    if (imu == NULL || param == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    
+    // 初始化设备头部
+    imu->header.online = false;
+    imu->header.last_online_time = 0;
+    
+    // 配置参数
+    imu->param.can = param->can;
+    imu->param.can_id = param->can_id;
+    imu->param.device_id = param->device_id;
+    imu->param.master_id = param->master_id;
+    
+    // 清零数据
+    memset(&imu->data, 0, sizeof(DM_IMU_Data_t));
+    
+    // 注册CAN接收队列，用于接收回复报文
+    int8_t result = BSP_FDCAN_RegisterId(imu->param.can, imu->param.master_id, 10);
+    if (result != BSP_OK) {
+        return DEVICE_ERR;
+    }
+
+    return DEVICE_OK;
+}
+
+/**
+ * @brief 请求IMU数据
+ */
+int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid) {
+    if (imu == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    
+    // 构造发送数据：id_L, id_H(DM_IMU_ID), RID, 0xcc
+    uint8_t tx_data[4] = {
+        imu->param.device_id & 0xFF,        // id_L
+        (imu->param.device_id >> 8) & 0xFF, // id_H
+        (uint8_t)rid,                       // RID
+        0xCC                                // 固定值
+    };
+    
+    // 发送标准数据帧
+    BSP_FDCAN_StdDataFrame_t frame = {
+        .id = imu->param.can_id,
+        .dlc = 4,
+    };
+    memcpy(frame.data, tx_data, 4);
+    BSP_FDCAN_WaitTxMailboxEmpty(imu->param.can, 1); // 等待发送邮箱空闲
+    int8_t result = BSP_FDCAN_TransmitStdDataFrame(imu->param.can, &frame);
+    return (result == BSP_OK) ? DEVICE_OK : DEVICE_ERR;
+}
+
+/**
+ * @brief 更新IMU数据（从CAN中获取所有数据并解析）
+ */
+int8_t DM_IMU_Update(DM_IMU_t *imu) {
+    if (imu == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    
+    BSP_FDCAN_Message_t msg;
+    int8_t result;
+    bool data_received = false;
+    
+    // 持续接收所有可用消息
+    while ((result = BSP_FDCAN_GetMessage(imu->param.can, imu->param.master_id, &msg, BSP_FDCAN_TIMEOUT_IMMEDIATE)) == BSP_OK) {
+        // 验证回复数据格式（至少检查数据长度）
+        if (msg.dlc < 3) {
+            continue; // 跳过无效消息
+        }
+        
+        // 根据数据位的第0位确定反馈报文类型
+        uint8_t rid = msg.data[0] & 0x0F; // 取第0位的低4位作为RID
+        
+        // 根据RID类型解析数据
+        int8_t parse_result = DEVICE_ERR;
+        switch (rid) {
+            case 0x01: // RID_ACCL
+                parse_result = DM_IMU_ParseAccelData(imu, msg.data, msg.dlc);
+                break;
+            case 0x02: // RID_GYRO
+                parse_result = DM_IMU_ParseGyroData(imu, msg.data, msg.dlc);
+                break;
+            case 0x03: // RID_EULER
+                parse_result = DM_IMU_ParseEulerData(imu, msg.data, msg.dlc);
+                break;
+            case 0x04: // RID_QUATERNION
+                parse_result = DM_IMU_ParseQuaternionData(imu, msg.data, msg.dlc);
+                break;
+            default:
+                continue; // 跳过未知类型的消息
+        }
+        
+        // 如果解析成功，标记为收到数据
+        if (parse_result == DEVICE_OK) {
+            data_received = true;
+        }
+    }
+    
+    // 如果收到任何有效数据，更新设备状态
+    if (data_received) {
+        imu->header.online = true;
+        imu->header.last_online_time = BSP_TIME_Get_ms();
+        return DEVICE_OK;
+    }
+    
+    return DEVICE_ERR;
+}
+
+/** 
+ * @brief 自动更新IMU所有数据（包括加速度计、陀螺仪、欧拉角和四元数,最高1khz）
+ */ 
+int8_t DM_IMU_AutoUpdateAll(DM_IMU_t *imu){
+    if (imu == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    switch (count) {
+        case 0:
+            DM_IMU_Request(imu, RID_ACCL);
+            break;
+        case 1:
+            DM_IMU_Request(imu, RID_GYRO);
+            break;
+        case 2:
+            DM_IMU_Request(imu, RID_EULER);
+            break;
+        case 3:
+            DM_IMU_Request(imu, RID_QUATERNION);
+            DM_IMU_Update(imu); // 更新所有数据
+            break;
+    }
+    count++;
+    if (count >= 4) {
+        count = 0; // 重置计数器
+    }
+    return DEVICE_OK;
+}
+
+/**
+ * @brief 检查设备是否在线
+ */
+bool DM_IMU_IsOnline(DM_IMU_t *imu) {
+    if (imu == NULL) {
+        return false;
+    }
+    
+    uint32_t current_time = BSP_TIME_Get_ms();
+    return imu->header.online && 
+           (current_time - imu->header.last_online_time < DM_IMU_OFFLINE_TIMEOUT);
+}

User/device/dm_imu.h

@@ -0,0 +1,90 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include "device/device.h"
+#include "component/ahrs/ahrs.h"
+#include "bsp/fdcan.h"
+/* Exported constants ------------------------------------------------------- */
+
+#define DM_IMU_CAN_ID_DEFAULT 0x6FF
+#define DM_IMU_ID_DEFAULT 0x42
+#define DM_IMU_MST_ID_DEFAULT 0x43
+
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+
+typedef struct {
+  BSP_FDCAN_t can;       // CAN总线句柄
+  uint16_t can_id;     // CAN通信ID
+  uint8_t device_id;   // 设备ID
+  uint8_t master_id;   // 主机ID
+} DM_IMU_Param_t;
+typedef enum {
+  RID_ACCL = 0x01, // 加速度计
+  RID_GYRO = 0x02, // 陀螺仪
+  RID_EULER = 0x03, // 欧拉角
+  RID_QUATERNION = 0x04, // 四元数
+} DM_IMU_RID_t;
+
+typedef struct {
+  AHRS_Accl_t accl; // 加速度计
+  AHRS_Gyro_t gyro; // 陀螺仪
+  AHRS_Eulr_t euler; // 欧拉角
+  AHRS_Quaternion_t quat; // 四元数
+  float temp; // 温度
+} DM_IMU_Data_t;
+
+typedef struct {
+  DEVICE_Header_t header;
+  DM_IMU_Param_t param; // IMU参数配置
+  DM_IMU_Data_t data; // IMU数据
+} DM_IMU_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+
+/**
+ * @brief 初始化DM IMU设备
+ * @param imu DM IMU设备结构体指针
+ * @param param IMU参数配置指针
+ * @return DEVICE_OK 成功，其他值失败
+ */
+int8_t DM_IMU_Init(DM_IMU_t *imu, DM_IMU_Param_t *param);
+
+/**
+ * @brief 请求IMU数据
+ * @param imu DM IMU设备结构体指针
+ * @param rid 请求的数据类型
+ * @return DEVICE_OK 成功，其他值失败
+ */
+int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid);
+
+
+/**
+ * @brief 更新IMU数据（从CAN中获取所有数据并解析）
+ * @param imu DM IMU设备结构体指针
+ * @return DEVICE_OK 成功，其他值失败
+ */
+int8_t DM_IMU_Update(DM_IMU_t *imu);
+
+/**
+ * @brief 自动更新IMU所有数据（包括加速度计、陀螺仪、欧拉角和四元数,最高1khz，运行4次才有完整数据）
+ * @param imu DM IMU设备结构体指针
+ * @return DEVICE_OK 成功，其他值失败
+ */
+int8_t DM_IMU_AutoUpdateAll(DM_IMU_t *imu);
+
+/**
+ * @brief 检查设备是否在线
+ * @param imu DM IMU设备结构体指针
+ * @return true 在线，false 离线
+ */
+bool DM_IMU_IsOnline(DM_IMU_t *imu);
+
+
+#ifdef __cplusplus
+}
+#endif

User/device/gimbal_imu.c

@@ -0,0 +1,165 @@
+/*
+ * Gimbal IMU Device - 接收来自云台的CAN IMU数据
+ */
+
+/* Includes ----------------------------------------------------------------- */
+#include "gimbal_imu.h"
+
+#include <string.h>
+
+#include "bsp/fdcan.h"
+#include "bsp/time.h"
+
+/* Private function prototypes ---------------------------------------------- */
+
+/**
+ * @brief 无符号整数转换为浮点数
+ * 
+ * @param x_int 输入的整数值
+ * @param x_min 最小值
+ * @param x_max 最大值
+ * @param bits 位数
+ * @return float 转换后的浮点数
+ */
+static float uint_to_float(uint32_t x_int, float x_min, float x_max, int bits) {
+  float span = x_max - x_min;
+  float offset = x_min;
+  return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset;
+}
+
+/**
+ * @brief 解析CAN消息中的IMU数据
+ */
+static void GIMBAL_IMU_ParseMessage(GIMBAL_IMU_FROMCAN_t *gimbal_imu, uint32_t id, const uint8_t *data) {
+  if (gimbal_imu == NULL || data == NULL) {
+    return;
+  }
+  
+  /* 判断是哪个ID的数据 */
+  if (id == gimbal_imu->param.accl_id) {
+    /* 解析加速度计数据 - 21位精度解包 */
+    uint32_t acc_x = ((uint32_t)data[0] << 13) | ((uint32_t)data[1] << 5) | ((uint32_t)data[2] >> 3);
+    uint32_t acc_y = (((uint32_t)data[2] & 0x07) << 18) | ((uint32_t)data[3] << 10) | ((uint32_t)data[4] << 2) | ((uint32_t)data[5] >> 6);
+    uint32_t acc_z = (((uint32_t)data[5] & 0x3F) << 15) | ((uint32_t)data[6] << 7) | ((uint32_t)data[7] >> 1);
+    
+    gimbal_imu->data.accl.x = uint_to_float(acc_x, GIMBAL_IMU_ACCEL_MIN, GIMBAL_IMU_ACCEL_MAX, 21);
+    gimbal_imu->data.accl.y = uint_to_float(acc_y, GIMBAL_IMU_ACCEL_MIN, GIMBAL_IMU_ACCEL_MAX, 21);
+    gimbal_imu->data.accl.z = uint_to_float(acc_z, GIMBAL_IMU_ACCEL_MIN, GIMBAL_IMU_ACCEL_MAX, 21);
+    
+  } else if (id == gimbal_imu->param.gyro_id) {
+    /* 解析陀螺仪数据 - 21位精度解包 */
+    uint32_t gyro_x = ((uint32_t)data[0] << 13) | ((uint32_t)data[1] << 5) | ((uint32_t)data[2] >> 3);
+    uint32_t gyro_y = (((uint32_t)data[2] & 0x07) << 18) | ((uint32_t)data[3] << 10) | ((uint32_t)data[4] << 2) | ((uint32_t)data[5] >> 6);
+    uint32_t gyro_z = (((uint32_t)data[5] & 0x3F) << 15) | ((uint32_t)data[6] << 7) | ((uint32_t)data[7] >> 1);
+    
+    gimbal_imu->data.gyro.x = uint_to_float(gyro_x, GIMBAL_IMU_GYRO_MIN, GIMBAL_IMU_GYRO_MAX, 21);
+    gimbal_imu->data.gyro.y = uint_to_float(gyro_y, GIMBAL_IMU_GYRO_MIN, GIMBAL_IMU_GYRO_MAX, 21);
+    gimbal_imu->data.gyro.z = uint_to_float(gyro_z, GIMBAL_IMU_GYRO_MIN, GIMBAL_IMU_GYRO_MAX, 21);
+    
+  } else if (id == gimbal_imu->param.eulr_id) {
+    /* 解析欧拉角数据 - 21位精度解包 */
+    uint32_t pit = ((uint32_t)data[0] << 13) | ((uint32_t)data[1] << 5) | ((uint32_t)data[2] >> 3);
+    uint32_t yaw = (((uint32_t)data[2] & 0x07) << 18) | ((uint32_t)data[3] << 10) | ((uint32_t)data[4] << 2) | ((uint32_t)data[5] >> 6);
+    uint32_t rol = (((uint32_t)data[5] & 0x3F) << 15) | ((uint32_t)data[6] << 7) | ((uint32_t)data[7] >> 1);
+    
+    gimbal_imu->data.eulr.pit = uint_to_float(pit, GIMBAL_IMU_PITCH_MIN, GIMBAL_IMU_PITCH_MAX, 21);
+    gimbal_imu->data.eulr.yaw = uint_to_float(yaw, GIMBAL_IMU_YAW_MIN, GIMBAL_IMU_YAW_MAX, 21);
+    gimbal_imu->data.eulr.rol = uint_to_float(rol, GIMBAL_IMU_ROLL_MIN, GIMBAL_IMU_ROLL_MAX, 21);
+    
+  } else if (id == gimbal_imu->param.quat_id) {
+    /* 解析四元数数据 - 使用14位精度解包 */
+    uint16_t q0 = ((uint16_t)data[1] << 6) | ((uint16_t)data[2] >> 2);
+    uint16_t q1 = (((uint16_t)data[2] & 0x03) << 12) | ((uint16_t)data[3] << 4) | ((uint16_t)data[4] >> 4);
+    uint16_t q2 = (((uint16_t)data[4] & 0x0F) << 10) | ((uint16_t)data[5] << 2) | ((uint16_t)data[6] >> 6);
+    uint16_t q3 = (((uint16_t)data[6] & 0x3F) << 8) | (uint16_t)data[7];
+    
+    gimbal_imu->data.quat.q0 = uint_to_float(q0, GIMBAL_IMU_QUAT_MIN, GIMBAL_IMU_QUAT_MAX, 14);
+    gimbal_imu->data.quat.q1 = uint_to_float(q1, GIMBAL_IMU_QUAT_MIN, GIMBAL_IMU_QUAT_MAX, 14);
+    gimbal_imu->data.quat.q2 = uint_to_float(q2, GIMBAL_IMU_QUAT_MIN, GIMBAL_IMU_QUAT_MAX, 14);
+    gimbal_imu->data.quat.q3 = uint_to_float(q3, GIMBAL_IMU_QUAT_MIN, GIMBAL_IMU_QUAT_MAX, 14);
+  }
+}
+
+/* Exported functions ------------------------------------------------------- */
+
+/**
+ * @brief 初始化Gimbal IMU设备
+ * @param gimbal_imu Gimbal IMU结构体指针
+ * @param param 参数配置
+ * @return 初始化结果
+ *   - DEVICE_OK: 成功
+ *   - DEVICE_ERR_NULL: 参数为空
+ */
+int8_t GIMBAL_IMU_Init(GIMBAL_IMU_FROMCAN_t *gimbal_imu, const GIMBAL_IMU_Param_t *param) {
+  if (gimbal_imu == NULL || param == NULL) {
+    return DEVICE_ERR_NULL;
+  }
+  
+  /* 清空数据 */
+  memset(gimbal_imu, 0, sizeof(GIMBAL_IMU_FROMCAN_t));
+  
+  /* 复制参数 */
+  gimbal_imu->param = *param;
+  
+  /* 设置设备类型 */
+  gimbal_imu->header.online = false;
+  
+  /* 注册CAN ID到消息队列 */
+  BSP_FDCAN_RegisterId(param->can, param->accl_id, BSP_FDCAN_DEFAULT_QUEUE_SIZE);
+  BSP_FDCAN_RegisterId(param->can, param->gyro_id, BSP_FDCAN_DEFAULT_QUEUE_SIZE);
+  BSP_FDCAN_RegisterId(param->can, param->eulr_id, BSP_FDCAN_DEFAULT_QUEUE_SIZE);
+  BSP_FDCAN_RegisterId(param->can, param->quat_id, BSP_FDCAN_DEFAULT_QUEUE_SIZE);
+  
+  return DEVICE_OK;
+}
+
+/**
+ * @brief 更新Gimbal IMU设备状态 - 从CAN队列读取并解析数据
+ * @param gimbal_imu Gimbal IMU结构体指针
+ * @return 更新结果
+ *   - DEVICE_OK: 成功
+ *   - DEVICE_ERR_NULL: 参数为空
+ */
+int8_t GIMBAL_IMU_Update(GIMBAL_IMU_FROMCAN_t *gimbal_imu) {
+  if (gimbal_imu == NULL) {
+    return DEVICE_ERR_NULL;
+  }
+  
+  BSP_FDCAN_Message_t msg;
+  bool received = false;
+  
+  /* 读取所有可用的CAN消息 */
+  while (BSP_FDCAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.accl_id, &msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
+    GIMBAL_IMU_ParseMessage(gimbal_imu, gimbal_imu->param.accl_id, msg.data);
+    received = true;
+  }
+  
+  while (BSP_FDCAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.gyro_id, &msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
+    GIMBAL_IMU_ParseMessage(gimbal_imu, gimbal_imu->param.gyro_id, msg.data);
+    received = true;
+  }
+  
+  while (BSP_FDCAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.eulr_id, &msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
+    GIMBAL_IMU_ParseMessage(gimbal_imu, gimbal_imu->param.eulr_id, msg.data);
+    received = true;
+  }
+  
+  while (BSP_FDCAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.quat_id, &msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
+    GIMBAL_IMU_ParseMessage(gimbal_imu, gimbal_imu->param.quat_id, msg.data);
+    received = true;
+  }
+  
+  /* 更新在线状态和时间戳 */
+  if (received) {
+    gimbal_imu->header.online = true;
+    gimbal_imu->header.last_online_time = BSP_TIME_Get();
+  } else {
+    /* 超时检测 - 100ms */
+    uint64_t now_time = BSP_TIME_Get();
+    if (now_time - gimbal_imu->header.last_online_time > 100000) {  /* 100ms超时，单位微秒 */
+      gimbal_imu->header.online = false;
+    }
+  }
+  
+  return DEVICE_OK;
+}

User/device/gimbal_imu.h

@@ -0,0 +1,88 @@
+/*
+ * Gimbal IMU Device - 接收来自云台的CAN IMU数据
+ */
+
+#pragma once
+
+#include <math.h>
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "bsp/fdcan.h"
+#include "component/ahrs/ahrs.h"
+#include "device/device.h"
+
+/* Exported constants ------------------------------------------------------- */
+
+/* 数据范围定义（与发送端保持一致） */
+#define GIMBAL_IMU_ACCEL_MAX (58.8f)
+#define GIMBAL_IMU_ACCEL_MIN (-58.8f)
+#define GIMBAL_IMU_GYRO_MAX  (34.88f)
+#define GIMBAL_IMU_GYRO_MIN  (-34.88f)
+#define GIMBAL_IMU_PITCH_MAX (90.0f)
+#define GIMBAL_IMU_PITCH_MIN (-90.0f)
+#define GIMBAL_IMU_ROLL_MAX  (180.0f)
+#define GIMBAL_IMU_ROLL_MIN  (-180.0f)
+#define GIMBAL_IMU_YAW_MAX   (180.0f)
+#define GIMBAL_IMU_YAW_MIN   (-180.0f)
+#define GIMBAL_IMU_QUAT_MIN  (-1.0f)
+#define GIMBAL_IMU_QUAT_MAX  (1.0f)
+
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+
+/* Gimbal IMU参数配置 */
+typedef struct {
+    BSP_FDCAN_t can;      /* 使用的CAN总线 */
+    uint16_t accl_id;
+    uint16_t gyro_id;
+    uint16_t eulr_id;
+    uint16_t quat_id;
+} GIMBAL_IMU_Param_t;
+
+typedef struct {
+  AHRS_Accl_t accl;           /* 加速度计数据 */
+  AHRS_Gyro_t gyro;           /* 陀螺仪数据 */
+  AHRS_Eulr_t eulr;           /* 欧拉角数据 */
+  AHRS_Quaternion_t quat;     /* 四元数数据 */
+  float temp;                 /* 温度数据 (摄氏度) */
+} GIMBAL_IMU_Data_t;
+
+
+/* Gimbal IMU完整数据结构 */
+typedef struct {
+  DEVICE_Header_t header;
+  GIMBAL_IMU_Param_t param;   /* 参数配置 */
+  GIMBAL_IMU_Data_t data;     /* IMU数据 */
+} GIMBAL_IMU_FROMCAN_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+
+/**
+ * @brief 初始化Gimbal IMU设备
+ * @param gimbal_imu Gimbal IMU结构体指针
+ * @param param 参数配置
+ * @return 初始化结果
+ *   - DEVICE_OK: 成功
+ *   - DEVICE_ERR_NULL: 参数为空
+ */
+int8_t GIMBAL_IMU_Init(GIMBAL_IMU_FROMCAN_t *gimbal_imu, const GIMBAL_IMU_Param_t *param);
+
+/**
+ * @brief 更新Gimbal IMU设备状态
+ * @param gimbal_imu Gimbal IMU结构体指针
+ * @return 更新结果
+ *   - DEVICE_OK: 成功
+ *   - DEVICE_ERR_NULL: 参数为空
+ */
+int8_t GIMBAL_IMU_Update(GIMBAL_IMU_FROMCAN_t *gimbal_imu);
+
+
+#ifdef __cplusplus
+}
+#endif

User/module/chassis.c

@@ -124,7 +124,7 @@ int8_t Chassis_Init(Chassis_t *c, const Chassis_Params_t *param,
         c->feedback.motor[i].temp = 0;
     }
      //³õʼ»¯PIDºÍ»ìºÏÆ÷
-    PID_Init(&c->pid.follow, KPID_MODE_NO_D, target_freq, &param->pid.follow_pid_param);
+    PID_Init(&c->pid.follow, KPID_MODE_CALC_D, target_freq, &param->pid.follow_pid_param);
     Mixer_Init(&c->mixer, mixer_mode);
      //ÇåÁãÔ˶¯ÏòÁ¿ºÍÊä³ö
     c->move_vec.vx = c->move_vec.vy = c->move_vec.wz = 0.0f;

User/module/cmd/cmd.c

@@ -38,7 +38,7 @@ static void CMD_RC_BuildChassisCmd(CMD_t *ctx) {
 /* 从RC输入生成云台命令 */
 static void CMD_RC_BuildGimbalCmd(CMD_t *ctx) {
   CMD_RCModeMap_t *map = &ctx->config->rc_mode_map;
-  
+    ctx->output.gimbal.cmd.is_ai = false;
   /* 根据拨杆选择云台模式 */
   switch (ctx->input.rc.sw[0]) {
       case CMD_SW_UP:
@@ -56,8 +56,8 @@ static void CMD_RC_BuildGimbalCmd(CMD_t *ctx) {
   }
   
   /* 左摇杆控制云台 */
-  ctx->output.gimbal.cmd.delta_yaw = -ctx->input.rc.joy_left.x * 2.0f;
+  ctx->output.gimbal.cmd.delta_yaw = -ctx->input.rc.joy_left.x * 4.0f;
-  ctx->output.gimbal.cmd.delta_pit = -ctx->input.rc.joy_left.y * 1.5f;
+  ctx->output.gimbal.cmd.delta_pit = -ctx->input.rc.joy_left.y * 2.5f;
 }
 
 /* 从RC输入生成射击命令 */
@@ -143,7 +143,7 @@ static void CMD_PC_BuildGimbalCmd(CMD_t *ctx) {
       ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELAX;
       return;
   }
-  
+  ctx->output.gimbal.cmd.is_ai = false;
   ctx->output.gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE;
   
   /* 鼠标控制云台 */
@@ -191,9 +191,14 @@ static void CMD_NUC_BuildGimbalCmd(CMD_t *ctx) {
   }
   
   /* 使用AI提供的云台控制数据 */
-  ctx->output.gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE;
+  
-  ctx->output.gimbal.cmd.delta_yaw = ctx->input.nuc.gimbal.setpoint.yaw;
+  if (ctx->input.nuc.mode!=0) {
-  ctx->output.gimbal.cmd.delta_pit = ctx->input.nuc.gimbal.setpoint.pit;
+  ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELATIVE;
+  ctx->output.gimbal.cmd.is_ai = true;
+  ctx->output.gimbal.cmd.setpoint_yaw = ctx->input.nuc.gimbal.setpoint.yaw;
+  ctx->output.gimbal.cmd.setpoint_pit = ctx->input.nuc.gimbal.setpoint.pit;
+  }
+
 }
 
 /* 从NUC/AI输入生成射击命令 */
@@ -205,17 +210,23 @@ static void CMD_NUC_BuildShootCmd(CMD_t *ctx) {
   
   /* 根据AI模式决定射击行为 */
   switch (ctx->input.nuc.mode) {
-    case 1:
+    case 0:
       ctx->output.shoot.cmd.ready = false;
       ctx->output.shoot.cmd.firecmd = false; 
       break;
-    case 2:
+    case 1:
       ctx->output.shoot.cmd.ready = true;
       ctx->output.shoot.cmd.firecmd = false; 
       break;
-    case 3:
+    case 2:
-      ctx->output.shoot.cmd.ready = true;
+    if (ctx->input.rc.sw[1]==CMD_SW_DOWN) {
-      ctx->output.shoot.cmd.firecmd = true; 
+    ctx->output.shoot.cmd.ready = true;
+    ctx->output.shoot.cmd.firecmd = true; 
+    }else {
+    ctx->output.shoot.cmd.ready = true;
+    ctx->output.shoot.cmd.firecmd = false; 
+    }
+
       break;
   }
 }

User/module/config.c

@@ -8,6 +8,7 @@
 #include "device/motor_dm.h"
 #include "module/cmd/cmd.h"
 #include <stdbool.h>
+#include "device/gimbal_imu.h"
 /* Private typedef ---------------------------------------------------------- */
 /* Private define ----------------------------------------------------------- */
 /* Private macro ------------------------------------------------------------ */
@@ -15,9 +16,21 @@
 
 /* Exported variables ------------------------------------------------------- */
 
+
+
 // 机器人参数配置
 Config_RobotParam_t robot_config = {
-
+    .ai_param = {
+        .can = BSP_FDCAN_2,
+        .vision_id = 0x104,
+    },
+    .imu_param = {
+        .can=BSP_FDCAN_2,
+        .accl_id=0x100,
+        .gyro_id=0x101,
+        .eulr_id=0x102,
+        .quat_id=0x103,
+    },
     .chassis_param = {
     /* DJI3508µç»ú*/
         .motor_param = {
@@ -55,7 +68,7 @@ Config_RobotParam_t robot_config = {
     .pid = {
       /* µ×Å̵ç»ú PID */
       .motor_pid_param = {
-        .k = 0.001f,
+        .k = 0.0015f,
         .p = 1.0f,
         .i = 0.0f,
         .d = 0.0f,
@@ -67,10 +80,10 @@ Config_RobotParam_t robot_config = {
 
       /* ¸úËæ */
       .follow_pid_param = {
-        .k = 0.5f,
+        .k = 1.2f,
         .p = 1.0f,
-        .i = 0.0f,
+        .i = 0.5f,
-        .d = 0.0f,
+        .d = 0.01f,
         .i_limit = 1.0f,
         .out_limit = 1.0f,
         .d_cutoff_freq = -1.0f,
@@ -96,54 +109,98 @@ Config_RobotParam_t robot_config = {
 
     .gimbal_param = {
         .pid = {
-            .yaw_omega = {
+            .absolute = {
-                .k = 1.0f,
+                .yaw_omega = {
-                .p = 1.0f,
+                    .k = 1.0f,//1
-                .i = 0.3f,
+                    .p = 1.0f,
-                .d = 0.0f,
+                    .i = 0.3f,
-                .i_limit = 1.0f,
+                    .d = 0.0f,
-                .out_limit = 1.0f,
+                    .i_limit = 1.0f,
-                .d_cutoff_freq = -1.0f,
+                    .out_limit = 1.0f,
-                .range = -1.0f,
+                    .d_cutoff_freq = -1.0f,
+                    .range = -1.0f,
+                },
+                .yaw_angle = {
+                    .k = 4.0f,
+                    .p = 5.0f,
+                    .i = 0.0f,
+                    .d = 0.0f,
+                    .i_limit = 0.0f,
+                    .out_limit = 10.0f,
+                    .d_cutoff_freq = -1.0f,
+                    .range = M_2PI,
+                },
+                .pit_omega = {
+                    .k = 1.0f,//0.4
+                    .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 = 6.0f,
+                    .p = 3.0f,
+                    .i = 2.0f,
+                    .d = 0.0f,
+                    .i_limit = 2.0f,
+                    .out_limit = 10.0f,
+                    .d_cutoff_freq = -1.0f,
+                    .range = M_2PI,
+                },
             },
-            .yaw_angle = {
+                .relative = {
-                .k = 6.0f,
+                    .yaw_omega = {
-                .p = 4.0f,
+                    .k = 0.8f,
-                .i = 0.0f,
+                    .p = 0.8f,
-                .d = 0.05f,
+                    .i = 0.3f,
-                .i_limit = 0.0f,
+                    .d = 0.0f,
-                .out_limit = 10.0f,
+                    .i_limit = 1.0f,
-                .d_cutoff_freq = -1.0f,
+                    .out_limit = 1.0f,
-                .range = M_2PI,
+                    .d_cutoff_freq = -1.0f,
-            },
+                    .range = -1.0f,
-            .pit_omega = {
+                },
-                .k = 0.4f,
+                .yaw_angle = {
-                .p = 1.0f,
+                    .k = 5.0f,
-                .i = 0.0f,
+                    .p = 2.5f,
-                .d = 0.0f,
+                    .i = 0.0f,
-                .i_limit = 1.0f,
+                    .d = 0.0f,
-                .out_limit = 1.0f,
+                    .i_limit = 0.0f,
-                .d_cutoff_freq = -1.0f,
+                    .out_limit = 10.0f,
-                .range = -1.0f,
+                    .d_cutoff_freq = -1.0f,
-            },
+                    .range = M_2PI,
-            .pit_angle = {
+                },
-                .k = 8.0f,
+                .pit_omega = {
-                .p = 5.0f,
+                    .k = 0.8f,//0.4
-                .i = 2.5f,
+                    .p = 0.8f,
-                .d = 0.03f,
+                    .i = 0.3f,
-                .i_limit = 0.0f,
+                    .d = 0.0f,
-                .out_limit = 10.0f,
+                    .i_limit = 1.0f,
-                .d_cutoff_freq = -1.0f,
+                    .out_limit = 1.0f,
-                .range = M_2PI,
+                    .d_cutoff_freq = -1.0f,
+                    .range = -1.0f,
+                },
+                .pit_angle = {
+                    .k = 5.0f,
+                    .p = 2.0f,
+                    .i = 0.0f,
+                    .d = 0.25f,
+                    .i_limit = 2.0f,
+                    .out_limit = 10.0f,
+                    .d_cutoff_freq = -1.0f,
+                    .range = M_2PI,
+                },
             },
         },
         .mech_zero = {
             .yaw = 0.0f,
-            .pit = 2.98220015,  //0.195206895
+            .pit = 3.15f,  
         },
         .travel = {
             .yaw = -1.0f,
-            .pit = 0.6,
+            .pit = 0.6f,
         },
         .low_pass_cutoff_freq = {
             .out = -1.0f,
@@ -338,9 +395,9 @@ Config_RobotParam_t robot_config = {
             .move_slow_mult = 0.5f,
         },
         .rc_mode_map = {
-            .sw_left_up   = CHASSIS_MODE_BREAK,
+            .sw_left_up   = CHASSIS_MODE_RELAX,
             .sw_left_mid  = CHASSIS_MODE_FOLLOW_GIMBAL,
-            .sw_left_down = CHASSIS_MODE_BREAK,
+            .sw_left_down = CHASSIS_MODE_RELAX,
             .gimbal_sw_up   = GIMBAL_MODE_ABSOLUTE,
             .gimbal_sw_mid  = GIMBAL_MODE_RELATIVE,
             .gimbal_sw_down = GIMBAL_MODE_ABSOLUTE,

User/module/config.h

@@ -18,12 +18,16 @@ extern "C" {
 #include "module/track.h"
 #include "module/cmd/cmd.h"
 #include "component/PowerControl.h"
+#include "device/gimbal_imu.h"
+#include "module/vision_bridge.h"
 typedef struct {
     Shoot_Params_t shoot_param;
 	Gimbal_Params_t gimbal_param;
     Chassis_Params_t chassis_param;
     Track_Params_t track_param;
     CMD_Config_t cmd_param;
+        GIMBAL_IMU_Param_t imu_param;
+        AI_Param_t ai_param;
 } Config_RobotParam_t;
 
 extern power_model_t cha;

User/module/gimbal.c

@@ -31,10 +31,15 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
   if (mode == g->mode)
     return GIMBAL_OK;
 
-  PID_Reset(&g->pid.yaw_angle);
+  PID_Reset(&g->pid.absolute.yaw_angle);
-  PID_Reset(&g->pid.yaw_omega);
+  PID_Reset(&g->pid.absolute.yaw_omega);
-  PID_Reset(&g->pid.pit_angle);
+  PID_Reset(&g->pid.absolute.pit_angle);
-  PID_Reset(&g->pid.pit_omega);
+  PID_Reset(&g->pid.absolute.pit_omega);
+  PID_Reset(&g->pid.relative.yaw_angle);
+  PID_Reset(&g->pid.relative.yaw_omega);
+  PID_Reset(&g->pid.relative.pit_angle);
+  PID_Reset(&g->pid.relative.pit_omega);
+
   LowPassFilter2p_Reset(&g->filter_out.yaw, 0.0f);
   LowPassFilter2p_Reset(&g->filter_out.pit, 0.0f);
 
@@ -77,14 +82,23 @@ int8_t Gimbal_Init(Gimbal_t *g, Gimbal_Params_t *param,
   g->mode = GIMBAL_MODE_RELAX; /* 设置默认模式 */
 
   /* 初始化云台电机控制PID和LPF */
-  PID_Init(&(g->pid.yaw_angle), KPID_MODE_CALC_D, target_freq,
+  PID_Init(&(g->pid.absolute.yaw_angle), KPID_MODE_CALC_D, target_freq,
-           &(g->param->pid.yaw_angle));
+           &(g->param->pid.absolute.yaw_angle));
-  PID_Init(&(g->pid.yaw_omega), KPID_MODE_CALC_D, target_freq,
+  PID_Init(&(g->pid.absolute.yaw_omega), KPID_MODE_CALC_D, target_freq,
-           &(g->param->pid.yaw_omega));
+           &(g->param->pid.absolute.yaw_omega));
-  PID_Init(&(g->pid.pit_angle), KPID_MODE_CALC_D, target_freq,
+  PID_Init(&(g->pid.absolute.pit_angle), KPID_MODE_CALC_D, target_freq,
-           &(g->param->pid.pit_angle));
+           &(g->param->pid.absolute.pit_angle));
-  PID_Init(&(g->pid.pit_omega), KPID_MODE_CALC_D, target_freq,
+  PID_Init(&(g->pid.absolute.pit_omega), KPID_MODE_CALC_D, target_freq,
-           &(g->param->pid.pit_omega));
+           &(g->param->pid.absolute.pit_omega));
+
+  PID_Init(&(g->pid.relative.yaw_angle), KPID_MODE_CALC_D, target_freq,
+           &(g->param->pid.relative.yaw_angle));
+  PID_Init(&(g->pid.relative.yaw_omega), KPID_MODE_CALC_D, target_freq,
+           &(g->param->pid.relative.yaw_omega));
+  PID_Init(&(g->pid.relative.pit_angle), KPID_MODE_CALC_D, target_freq,
+           &(g->param->pid.relative.pit_angle));
+  PID_Init(&(g->pid.relative.pit_omega), KPID_MODE_CALC_D, target_freq,
+           &(g->param->pid.relative.pit_omega));
 
   LowPassFilter2p_Init(&g->filter_out.yaw, target_freq,
                        g->param->low_pass_cutoff_freq.out);
@@ -162,6 +176,8 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
 
   Gimbal_SetMode(g, g_cmd->mode);
 
+  g->out.pit_feedforward = g_cmd->feedforward_pit;
+
   /* 处理yaw控制命令，软件限位 - 使用电机绝对角度 */
   float delta_yaw = g_cmd->delta_yaw * g->dt * 1.5f;
   if (g->param->travel.yaw > 0) {
@@ -221,6 +237,10 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
   CircleAdd(&(g->setpoint.eulr.pit), delta_pit, M_2PI);
   // g->setpoint.eulr.pit = (g_cmd->delta_pit)/3.0f + 2.98f + 1/3.0f; //2.98f为机械零点位置
 
+    if (g_cmd->is_ai) {
+    g->setpoint.eulr.yaw = g_cmd->setpoint_yaw;
+    g->setpoint.eulr.pit = g_cmd->setpoint_pit;
+    }
   /* 控制相关逻辑 */
   float yaw_omega_set_point, pit_omega_set_point;
   switch (g->mode) {
@@ -230,14 +250,14 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
     break;
 
   case GIMBAL_MODE_ABSOLUTE:
-    yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
+    yaw_omega_set_point = PID_Calc(&(g->pid.absolute.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->out.yaw = PID_Calc(&(g->pid.absolute.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,
+    pit_omega_set_point = PID_Calc(&(g->pid.absolute.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->out.pit = PID_Calc(&(g->pid.absolute.pit_omega), pit_omega_set_point,
                           g->feedback.imu.gyro.x, 0.f, g->dt);
 
     /* 输出滤波 */
@@ -246,14 +266,14 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
     break;
   
   case GIMBAL_MODE_RELATIVE:
-    yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
+    yaw_omega_set_point = PID_Calc(&(g->pid.relative.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->out.yaw = PID_Calc(&(g->pid.relative.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,
+    pit_omega_set_point = PID_Calc(&(g->pid.relative.pit_angle), g->setpoint.eulr.pit,
                                    g->feedback.imu.eulr.pit, 0.0f, g->dt);
-    g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point,
+    g->out.pit = PID_Calc(&(g->pid.relative.pit_omega), pit_omega_set_point,
                           g->feedback.imu.gyro.x, 0.f, g->dt);
 
     /* 输出滤波 */
@@ -273,7 +293,7 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
    */
 void Gimbal_Output(Gimbal_t *g){
     MOTOR_MIT_Output_t output_pit = {0};
-    output_pit.torque = g->out.pit * 5.0f; // 乘以减速比
+    output_pit.torque = g->out.pit * 5.0f; //+ g->out.pit_feedforward; // 乘以减速比 并加上前馈
     output_pit.kd = 1.0f;
     MOTOR_DM_MITCtrl(&g->param->pit_motor, &output_pit);
     

User/module/gimbal.h

@@ -36,6 +36,10 @@ typedef struct {
   Gimbal_Mode_t mode;
   float delta_yaw;
   float delta_pit;
+  float feedforward_pit; 
+  float setpoint_yaw;
+  float setpoint_pit;
+  bool is_ai;
 } Gimbal_CMD_t;
 
 /* 软件限位 */
@@ -49,10 +53,19 @@ typedef struct {
   MOTOR_DM_Param_t pit_motor; /* pitch轴电机参数 */
   MOTOR_DM_Param_t yaw_motor; /* yaw轴电机参数 */
   struct {
-    KPID_Params_t yaw_omega; /* yaw轴角速度环PID参数 */
+    struct {
-    KPID_Params_t yaw_angle; /* yaw轴角位置环PID参数 */
+      KPID_Params_t yaw_omega; /* yaw轴角速度环PID参数 */
-    KPID_Params_t pit_omega; /* pitch轴角速度环PID参数 */
+      KPID_Params_t yaw_angle; /* yaw轴角位置环PID参数 */
-    KPID_Params_t pit_angle; /* pitch轴角位置环PID参数 */
+      KPID_Params_t pit_omega; /* pitch轴角速度环PID参数 */
+      KPID_Params_t pit_angle; /* pitch轴角位置环PID参数 */
+    } absolute;
+
+    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参数 */
+    } relative;
   } pid;
 
   /* 低通滤波器截止频率 */
@@ -90,6 +103,7 @@ typedef struct {
 typedef struct {
   float yaw; /* yaw轴电机输出 */
   float pit; /* pitch轴电机输出 */
+  float pit_feedforward;
 } Gimbal_Output_t;
 /*
  * 运行的主结构体，所有这个文件里的函数都在操作这个结构体。
@@ -110,10 +124,18 @@ typedef struct {
   } setpoint;
 
   struct {
-    KPID_t yaw_angle; /* yaw轴角位置环PID */
+    struct {
-    KPID_t yaw_omega; /* yaw轴角速度环PID */
+      KPID_t yaw_angle; /* yaw轴角位置环PID */
-    KPID_t pit_angle; /* pitch轴角位置环PID */
+      KPID_t yaw_omega; /* yaw轴角速度环PID */
-    KPID_t pit_omega; /* pitch轴角速度环PID */
+      KPID_t pit_angle; /* pitch轴角位置环PID */
+      KPID_t pit_omega; /* pitch轴角速度环PID */
+    } absolute;
+    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 */
+    } relative;
   } pid;
 
   struct {

User/module/shoot.c

@@ -46,7 +46,7 @@ void Task(void *argument) {
 /* Private variables -------------------------------------------------------- */
 static bool last_firecmd;
 
-float maxTrigrpm=1500.0f;
+float maxTrigrpm=4000.0f;
 /* Private function  -------------------------------------------------------- */
 
 /**
@@ -177,7 +177,7 @@ int8_t Shoot_CaluTargetRPM(Shoot_t *s, float target_speed)
 		s->target_variable.fric_rpm=5000.0f;
 		break;
 		case SHOOT_PROJECTILE_42MM:
-		s->target_variable.fric_rpm=6500.0f;
+		s->target_variable.fric_rpm=3600.0f;//6500
 		break;	
 	}
     return SHOOT_OK;

User/module/vision_bridge.c

@@ -3,10 +3,21 @@
 #include "bsp/uart.h"
 #include "component/crc16.h"
 #include <string.h>
+#include "bsp/fdcan.h"
 
-int8_t AI_Init(AI_t *ai) {
+#define AI_CMD_CAN_DLC       (8u)  /* 1字节mode + 3.5字节yaw + 3.5字节pit */
+#define AI_CMD_ANGLE_SCALE   (10000000.0f)  /* 0.0000001 rad per LSB */
+
+int8_t AI_Init(AI_t *ai, AI_Param_t *param) {
   if (ai == NULL) return DEVICE_ERR_NULL;
+  
+  BSP_FDCAN_Init();
+  
 	memset(ai, 0, sizeof(AI_t));
+
+	ai->param = param;
+	BSP_FDCAN_RegisterId(ai->param->can, ai->param->vision_id, 3);
+
   return 0;
 }
 
@@ -29,7 +40,7 @@ int8_t AI_ParseForHost(AI_t* ai, AI_RawData_t* raw_data){
 		ai->tohost.q[2]=raw_data->q[2];	
 		ai->tohost.q[3]=raw_data->q[3];		
 		ai->tohost.bullet_count=10;
-		ai->tohost.bullet_speed=17.5;	
+		ai->tohost.bullet_speed=10.5;	
 		
 		ai->tohost.crc16=CRC16_Calc(((const uint8_t*)&(ai->tohost)),sizeof(ai->tohost)-sizeof(uint16_t), CRC16_INIT );
 		if(CRC16_Verify(((const uint8_t*)&(ai->tohost)), sizeof(ai->tohost))!=true){
@@ -60,3 +71,60 @@ int8_t AI_Get(AI_t *ai, AI_cmd_t* outcmd) {
 		outcmd->gimbal.vel.yaw			=	ai->fromhost.yaw_vel;		
   return DEVICE_OK;
 }
+
+
+/* 打包并通过 CAN2 发送AI命令给下层板（mode + yaw + pit） */
+void AI_SendCmdOnFDCAN(AI_t *ai, const AI_cmd_t *cmd) {
+  if (cmd == NULL) return;
+
+  /* 将float角度转换为int32_t定点数（0.0000001 rad/LSB） */
+  const int32_t yaw = (int32_t)(cmd->gimbal.setpoint.yaw * AI_CMD_ANGLE_SCALE);
+  const int32_t pit = (int32_t)(cmd->gimbal.setpoint.pit * AI_CMD_ANGLE_SCALE);
+
+  BSP_FDCAN_StdDataFrame_t frame = {0};
+  frame.id = ai->param->vision_id;
+  frame.dlc = AI_CMD_CAN_DLC;
+  
+  /* mode(1字节) + yaw(3.5字节) + pit(3.5字节) */
+  frame.data[0] = cmd->mode;
+  frame.data[1] = (uint8_t)((yaw >> 20) & 0xFF);
+  frame.data[2] = (uint8_t)((yaw >> 12) & 0xFF);
+  frame.data[3] = (uint8_t)((yaw >> 4) & 0xFF);
+  frame.data[4] = (uint8_t)(((yaw & 0xF) << 4) | ((pit >> 24) & 0xF));
+  frame.data[5] = (uint8_t)((pit >> 16) & 0xFF);
+  frame.data[6] = (uint8_t)((pit >> 8) & 0xFF);
+  frame.data[7] = (uint8_t)(pit & 0xFF);
+  
+  (void)BSP_FDCAN_TransmitStdDataFrame(ai->param->can, &frame);
+}
+
+/* 从CAN消息解析AI命令 (mode + yaw + pit) */
+ void AI_ParseCmdFromCan(AI_t *ai, AI_cmd_t *cmd) {
+
+  if (cmd == NULL) {
+    return;
+  }
+		BSP_FDCAN_Message_t msg;
+  if (BSP_FDCAN_GetMessage(ai->param->can, ai->param->vision_id, &msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) != 0) {
+      return;
+  }
+
+  /* 解析mode (1字节) */
+  cmd->mode = msg.data[0];
+  
+  /* 解析yaw (3.5字节) */
+  int32_t yaw_raw = (int32_t)((msg.data[1] << 20) | (msg.data[2] << 12) | (msg.data[3] << 4) | ((msg.data[4] >> 4) & 0xF));
+  if (yaw_raw & 0x08000000) yaw_raw |= 0xF0000000;
+  cmd->gimbal.setpoint.yaw = (float)yaw_raw / AI_CMD_ANGLE_SCALE;
+  
+  /* 解析pit (3.5字节) */
+  int32_t pit_raw = (int32_t)(((msg.data[4] & 0xF) << 24) | (msg.data[5] << 16) | (msg.data[6] << 8) | msg.data[7]);
+  if (pit_raw & 0x08000000) pit_raw |= 0xF0000000;
+  cmd->gimbal.setpoint.pit = (float)pit_raw / AI_CMD_ANGLE_SCALE;
+  
+  /* 其他字段根据需要可以初始化为0 */
+  cmd->gimbal.vel.yaw = 0.0f;
+  cmd->gimbal.vel.pit = 0.0f;
+  cmd->gimbal.accl.yaw = 0.0f;
+  cmd->gimbal.accl.pit = 0.0f;
+}

User/module/vision_bridge.h

@@ -5,7 +5,7 @@
 #pragma once
 
 #include "component\user_math.h"
-
+#include "bsp\fdcan.h"
 #ifdef __cplusplus
 extern "C" {
 #endif
@@ -69,12 +69,20 @@ typedef struct  {
   uint16_t bullet_count;  // 子弹累计发送次数
 }AI_RawData_t;
 
+typedef struct  {
+	BSP_FDCAN_t can;
+  uint16_t vision_id;
+}AI_Param_t;
+
 typedef struct __attribute__((packed)) {
 	struct AI_ToHost tohost;
 	struct AI_FromHost fromhost;
+  AI_Param_t *param;
 }AI_t;
 
 
+int8_t AI_Init(AI_t *ai, AI_Param_t *param);
+
 int8_t AI_StartReceiving(AI_t *ai);
 
 int8_t AI_Get(AI_t *ai, AI_cmd_t* ai_cmd);
@@ -83,6 +91,9 @@ int8_t AI_ParseForHost(AI_t* ai, AI_RawData_t* raw_data);
 
 int8_t AI_StartSend(AI_t *ai);
 
+void AI_SendCmdOnCan(AI_t *ai, const AI_cmd_t *cmd);
+
+ void AI_ParseCmdFromCan(AI_t *ai, AI_cmd_t *cmd);
 #ifdef __cplusplus
 }
 #endif

User/task/atti_esti.c

@@ -17,6 +17,8 @@
 #include "module/gimbal.h"
 #include "module/chassis.h"
 #include "device/bmi088.h"
+#include "device/dm_imu.h"
+#include "module/config.h"
 /* USER INCLUDE END */
 
 /* Private typedef ---------------------------------------------------------- */
@@ -32,13 +34,14 @@ BMI088_t bmi088;
 AHRS_t chassis_ahrs;
 AHRS_Magn_t magn;
 AHRS_Eulr_t eulr_chassis;
-AHRS_Eulr_t eulr_gimbal;
 KPID_t imu_temp_ctrl_pid;
 
 Gimbal_IMU_t gimbal_to_send;
 //Chassis_IMU_t chassis_to_send;
 AHRS_Quaternion_t imu_quat_for_ai;
 
+GIMBAL_IMU_FROMCAN_t imu_to_can;
+
 BMI088_Cali_t cali_bmi088= {
   .gyro_offset = {-0.00147764047f,-0.00273479894f,0.00154074503f},
 };
@@ -106,6 +109,7 @@ void Task_atti_esti(void *argument) {
            1.0f / BMI088_GetUpdateFreq(&bmi088), &imu_temp_ctrl_pid_param);
   BSP_PWM_Start(BSP_PWM_IMU_HEAT);
 
+  GIMBAL_IMU_Init(&imu_to_can, &Config_GetRobotParam()->imu_param);
   /* 注册按钮回调函数并启用中断 */
   BSP_GPIO_RegisterCallback(BSP_GPIO_USER_KEY, start_gyro_calibration);
   BSP_GPIO_EnableIRQ(BSP_GPIO_USER_KEY);
@@ -163,20 +167,20 @@ void Task_atti_esti(void *argument) {
     
     osKernelUnlock();
 
-    if (fdcan_ready) {
+    //从can读取头部Imu数据
-      Atti_UpdateEulrFromCan();
+    GIMBAL_IMU_Update(&imu_to_can);
-    }
 
     /* 创建修改后的数据副本用于发送到消息队列 */
-    gimbal_to_send.eulr = eulr_gimbal;
+    gimbal_to_send.eulr=imu_to_can.data.eulr;
-    gimbal_to_send.gyro = bmi088.gyro;
+    gimbal_to_send.gyro=imu_to_can.data.gyro;
+
     osMessageQueueReset(task_runtime.msgq.gimbal.imu);
     osMessageQueuePut(task_runtime.msgq.gimbal.imu, &gimbal_to_send, 0, 0);
     
-    imu_quat_for_ai.q0 = chassis_ahrs.quat.q0;
+    imu_quat_for_ai.q0 = imu_to_can.data.quat.q0;
-    imu_quat_for_ai.q1 = chassis_ahrs.quat.q1;
+    imu_quat_for_ai.q1 = imu_to_can.data.quat.q1;
-    imu_quat_for_ai.q2 = chassis_ahrs.quat.q2;
+    imu_quat_for_ai.q2 = imu_to_can.data.quat.q2;
-    imu_quat_for_ai.q3 = chassis_ahrs.quat.q3;
+    imu_quat_for_ai.q3 = imu_to_can.data.quat.q3;
     osMessageQueuePut(task_runtime.msgq.ai.rawdata_FromIMU, &imu_quat_for_ai, 0, 0);
 
 //    chassis_to_send.eulr = eulr_to_send;
@@ -193,22 +197,3 @@ void Task_atti_esti(void *argument) {
   }
   
 }
-
-static void Atti_UpdateEulrFromCan(void) {
-  BSP_FDCAN_Message_t rx_msg;
-
-  /* Drain queue to keep the latest gimbal orientation */
-  while (BSP_FDCAN_GetMessage(BSP_FDCAN_2, ATTI_EULR_CAN_ID, &rx_msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
-    if (rx_msg.dlc < ATTI_EULR_CAN_DLC) {
-      continue;
-    }
-
-    const int16_t yaw_raw = (int16_t)((rx_msg.data[0] << 8) | rx_msg.data[1]);
-    const int16_t pit_raw = (int16_t)((rx_msg.data[2] << 8) | rx_msg.data[3]);
-    const int16_t rol_raw = (int16_t)((rx_msg.data[4] << 8) | rx_msg.data[5]);
-
-    eulr_gimbal.yaw = ((float)yaw_raw) / ATTI_EULR_SCALE;
-    eulr_gimbal.pit = ((float)pit_raw) / ATTI_EULR_SCALE;
-    eulr_gimbal.rol = ((float)rol_raw) / ATTI_EULR_SCALE;
-  }
-}

User/task/cmd.c

@@ -15,14 +15,12 @@
 #include "module/track.h"
 #include "module/cmd/cmd.h"
 #include "bsp/fdcan.h"
+#include "module/vision_bridge.h"
 //#define DEAD_AREA 0.05f
 /* USER INCLUDE END */
 
 /* Private typedef ---------------------------------------------------------- */
 /* Private define ----------------------------------------------------------- */
-#define AI_CMD_CAN_ID        (0x101u)
-#define AI_CMD_CAN_DLC       (5u)  /* 1字节mode + 2字节yaw + 2字节pit */
-#define AI_CMD_ANGLE_SCALE   (10000.0f)  /* 0.0001 rad per LSB */
 /* Private macro ------------------------------------------------------------ */
 /* Private variables -------------------------------------------------------- */
 /* USER STRUCT BEGIN */
@@ -38,10 +36,11 @@ Chassis_CMD_t  *cmd_for_chassis;
 Gimbal_CMD_t   *cmd_for_gimbal;
 Track_CMD_t    *cmd_for_track;
 static CMD_t cmd;
+
+AI_t ai;
 /* USER STRUCT END */
 
 /* Private function --------------------------------------------------------- */
-static void AI_ParseCmdFromCan(const BSP_FDCAN_Message_t *msg, AI_cmd_t *cmd);
 /* Exported functions ------------------------------------------------------- */
 void Task_cmd(void *argument) {
   (void)argument; /* 未使用argument，消除警告 */
@@ -55,9 +54,8 @@ void Task_cmd(void *argument) {
   uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
   /* USER CODE INIT BEGIN */
   CMD_Init(&cmd, &Config_GetRobotParam()->cmd_param);
-  
+  AI_Init(&ai, &Config_GetRobotParam()->ai_param);
   /* 注册CAN接收ID */
-  BSP_FDCAN_RegisterId(BSP_FDCAN_2, AI_CMD_CAN_ID, BSP_FDCAN_DEFAULT_QUEUE_SIZE);
   /* USER CODE INIT END */
   
   while (1) {
@@ -70,10 +68,7 @@ void Task_cmd(void *argument) {
     #endif
     
     /* 从CAN2接收AI命令 */
-    BSP_FDCAN_Message_t can_msg;
+    AI_ParseCmdFromCan( &ai,&cmd_ai);
-    if (BSP_FDCAN_GetMessage(BSP_FDCAN_2, AI_CMD_CAN_ID, &can_msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == 0) {
-      AI_ParseCmdFromCan(&can_msg, &cmd_ai);
-    }
 
     CMD_Update(&cmd);
     
@@ -82,6 +77,14 @@ void Task_cmd(void *argument) {
     cmd_for_gimbal = CMD_GetGimbalCmd(&cmd);
     cmd_for_shoot = CMD_GetShootCmd(&cmd);
     cmd_for_track = CMD_GetTrackCmd(&cmd);
+
+    if (cmd_for_shoot->firecmd) {
+        /* 发射时添加pitch前馈，抵消后坐力。此时为负值表示向下压。需要根据实际情况调整 */
+        cmd_for_gimbal->feedforward_pit = -1.0f; 
+    } else {
+        cmd_for_gimbal->feedforward_pit = 0.0f;
+    }
+
 	osMessageQueueReset(task_runtime.msgq.gimbal.cmd);
     osMessageQueuePut(task_runtime.msgq.gimbal.cmd, cmd_for_gimbal, 0, 0);
 	osMessageQueueReset(task_runtime.msgq.shoot.cmd);
@@ -95,27 +98,3 @@ void Task_cmd(void *argument) {
   }
   
 }
-
-/* 从CAN消息解析AI命令 (mode + yaw + pit) */
-static void AI_ParseCmdFromCan(const BSP_FDCAN_Message_t *msg, AI_cmd_t *cmd) {
-  if (msg == NULL || cmd == NULL || msg->dlc < AI_CMD_CAN_DLC) {
-    return;
-  }
-  
-  /* 解析mode (1字节) */
-  cmd->mode = msg->data[0];
-  
-  /* 解析yaw (2字节，高字节在前) */
-  int16_t yaw_raw = (int16_t)((msg->data[1] << 8) | msg->data[2]);
-  cmd->gimbal.setpoint.yaw = (float)yaw_raw / AI_CMD_ANGLE_SCALE;
-  
-  /* 解析pit (2字节，高字节在前) */
-  int16_t pit_raw = (int16_t)((msg->data[3] << 8) | msg->data[4]);
-  cmd->gimbal.setpoint.pit = (float)pit_raw / AI_CMD_ANGLE_SCALE;
-  
-  /* 其他字段根据需要可以初始化为0 */
-  cmd->gimbal.vel.yaw = 0.0f;
-  cmd->gimbal.vel.pit = 0.0f;
-  cmd->gimbal.accl.yaw = 0.0f;
-  cmd->gimbal.accl.pit = 0.0f;
-}

User/task/ctrl_gimbal.c

@@ -44,7 +44,8 @@ void Task_ctrl_gimbal(void *argument) {
       Gimbal_UpdateIMU(&gimbal, &gimbal_imu);
     }
     osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &gimbal_cmd, NULL, 0);
-
+    // gimbal_cmd.mode = GIMBAL_MODE_ABSOLUTE;
+    // gimbal_cmd.feedforward_pit = 0.0f;
 
     Gimbal_UpdateFeedback(&gimbal);
 

hero_DM02.ioc

@@ -96,7 +96,7 @@ FDCAN2.NominalSyncJumpWidth=5
 FDCAN2.NominalTimeSeg1=14
 FDCAN2.NominalTimeSeg2=5
 FDCAN2.ProtocolException=ENABLE
-FDCAN2.RxFifo1ElmtsNbr=16
+FDCAN2.RxFifo1ElmtsNbr=20
 FDCAN2.StdFiltersNbr=1
 FDCAN2.TxFifoQueueElmtsNbr=16
 FDCAN3.AutoRetransmission=ENABLE

ozone/hero.jdebug.user

@@ -1,34 +1,51 @@
 
 
-GraphedExpression="(eulr_gimbal).yaw", Color=#e56a6f
+
-GraphedExpression="(eulr_gimbal).pit", Color=#35792b
+Breakpoint=D:/CUBEMX/hero/god-yuan-hero/User/module/gimbal.c:158:10, State=BP_STATE_DISABLED
-OpenDocument="tasks.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=3637
+GraphedExpression="((((gimbal).feedback).imu).eulr).yaw", Color=#e56a6f, Show=0
+GraphedExpression="(cmd_ai).mode", DisplayFormat=DISPLAY_FORMAT_DEC, Color=#35792b
+OpenDocument="stm32h7xx_hal_uart_ex.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_uart_ex.c", Line=994
+OpenDocument="cmsis_gcc.h", FilePath="D:/CUBEMX/hero/god-yuan-hero/Drivers/CMSIS/Include/cmsis_gcc.h", Line=1193
 OpenDocument="startup_stm32h723xx.s", FilePath="D:/CUBEMX/hero/god-yuan-hero/startup_stm32h723xx.s", Line=48
+OpenDocument="cmd.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/task/cmd.c", Line=51
+OpenDocument="cmd.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/module/cmd/cmd.c", Line=0
+OpenDocument="vision_bridge.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/module/vision_bridge.c", Line=78
+OpenDocument="tasks.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=3419
+OpenDocument="motor.h", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/device/motor.h", Line=20
+OpenDocument="filter.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/component/filter/filter.c", Line=42
+OpenDocument="user_math.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/component/user_math.c", Line=45
+OpenDocument="pid.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/component/pid.c", Line=61
+OpenDocument="gimbal.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/module/gimbal.c", Line=220
+OpenDocument="ctrl_gimbal.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/task/ctrl_gimbal.c", Line=16
+OpenDocument="fdcan.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/bsp/fdcan.c", Line=57
 OpenDocument="main.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/Core/Src/main.c", Line=60
+OpenDocument="queue.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/Middlewares/Third_Party/FreeRTOS/Source/queue.c", Line=1275
+OpenDocument="chassis.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/module/chassis.c", Line=213
+OpenDocument="stm32h7xx_hal_uart.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_uart.c", Line=3332
+OpenDocument="atti_esti.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/task/atti_esti.c", Line=4
+OpenDocument="fdcan.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/Core/Src/fdcan.c", Line=0
+OpenDocument="time.c", FilePath="D:/CUBEMX/hero/god-yuan-hero/User/bsp/time.c", Line=17
 OpenToolbar="Debug", Floating=0, x=0, y=0
-OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=1, w=726, h=641, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1
+OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=1, w=727, h=620, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1
-OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=326, h=930, TabPos=0, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
+OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=326, h=930, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
-OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=0, w=726, h=288, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
+OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=0, w=727, h=309, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
 OpenWindow="Memory 1", DockArea=BOTTOM, x=0, y=0, w=239, h=544, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, EditorAddress=0xFFFF5088
-OpenWindow="Watched Data 1", DockArea=RIGHT, x=0, y=1, w=726, h=641, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
+OpenWindow="Watched Data 1", DockArea=RIGHT, x=0, y=1, w=727, h=620, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
-OpenWindow="Functions", DockArea=LEFT, x=0, y=0, w=326, h=930, TabPos=1, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
+OpenWindow="Functions", DockArea=LEFT, x=0, y=0, w=326, h=930, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
 OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=866, h=525, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
-OpenWindow="Timeline", DockArea=BOTTOM, x=1, y=0, w=1453, h=544, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=1, CodePaneShown=1, PinCursor="Cursor Movable", TimePerDiv="1 ns / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="1187;-113", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=1, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="1226;-69", CodeGraphLegendShown=1, CodeGraphLegendPosition="1177;0"
+OpenWindow="Timeline", DockArea=BOTTOM, x=1, y=0, w=1453, h=544, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="200 ms / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="21;187", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="1232;-69", CodeGraphLegendShown=0, CodeGraphLegendPosition="1248;0"
 OpenWindow="Console", DockArea=BOTTOM, x=2, y=0, w=866, h=525, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
 SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1"
-TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[120;144;294]
+TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[120;144;463]
-TableHeader="Functions", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Address";"Size";"#Insts";"Source"], ColWidths=[1594;104;100;100;798]
+TableHeader="Functions", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Address";"Size";"#Insts";"Source"], ColWidths=[1594;104;100;100;100]
-TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[215;100;100;100;1014]
-TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" (eulr_gimbal).yaw";" (eulr_gimbal).pit"], ColWidths=[100;100;100;100]
-TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[118;100;144;144;124;144;110;164;154]
 TableHeader="Power Sampling", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[100;100;100]
-TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[250;282;91;100]
 TableHeader="RegisterSelectionDialog", SortCol="None", SortOrder="ASCENDING", VisibleCols=[], ColWidths=[]
+TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[215;100;100;100;1014]
+TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[250;229;145;100]
+TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" ((((gimbal).feedback).imu).eulr).yaw";" (cmd_ai).mode"], ColWidths=[100;100;100;100]
+TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[118;144;114;114;114;114;110;126;126]
 TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[200;100;100;366]
 WatchedExpression="cmd", RefreshRate=5, Window=Watched Data 1
-WatchedExpression="eulr_gimbal", RefreshRate=5, Window=Watched Data 1
-WatchedExpression="aiToCmd", RefreshRate=5, Window=Watched Data 1
-WatchedExpression="eulr_gimbal", RefreshRate=5, Window=Watched Data 1
 WatchedExpression="bmi088", RefreshRate=5, Window=Watched Data 1
 WatchedExpression="chassis", RefreshRate=5, Window=Watched Data 1
 WatchedExpression="gimbal", RefreshRate=5, Window=Watched Data 1
@@ -36,3 +53,7 @@ WatchedExpression="shoot", RefreshRate=5, Window=Watched Data 1
 WatchedExpression="track_cmd", RefreshRate=5, Window=Watched Data 1
 WatchedExpression="track", RefreshRate=5, Window=Watched Data 1
 WatchedExpression="robot_config", RefreshRate=5, Window=Watched Data 1
+WatchedExpression="imu_to_can", RefreshRate=5, Window=Watched Data 1
+WatchedExpression="cmd_ai", RefreshRate=5, Window=Watched Data 1
+WatchedExpression="ai", RefreshRate=5, Window=Watched Data 1
+WatchedExpression="queue_list", RefreshRate=5, Window=Watched Data 1