添加imu

CMakeLists.txt

@@ -71,6 +71,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     User/device/bmi088.c
     User/device/buzzer.c
     User/device/dr16.c
+    User/device/gimbal_imu.c
     User/device/motor.c
     User/device/motor_dm.c
     User/device/motor_lk.c

User/device/gimbal_imu.c

@@ -0,0 +1,144 @@
+/*
+ * Gimbal IMU Device - 接收来自云台的CAN IMU数据
+ */
+
+/* Includes ----------------------------------------------------------------- */
+#include "gimbal_imu.h"
+
+#include <string.h>
+
+#include "bsp/can.h"
+
+/* Private function prototypes ---------------------------------------------- */
+
+/**
+ * @brief 无符号整数转换为浮点数
+ * 
+ * @param x_int 输入的整数值
+ * @param x_min 最小值
+ * @param x_max 最大值
+ * @param bits 位数
+ * @return float 转换后的浮点数
+ */
+static float uint_to_float(uint16_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_t *gimbal_imu, uint32_t id, const uint8_t *data) {
+  if (gimbal_imu == NULL || data == NULL) {
+    return;
+  }
+  
+  /* 更新在线状态 */
+  gimbal_imu->header.online = true;
+  
+  /* 判断是哪个ID的数据 */
+  if (id == gimbal_imu->param.accl_id) {
+    /* 解析加速度计数据 */
+    uint16_t acc_x = (uint16_t)data[2] | ((uint16_t)data[3] << 8);
+    uint16_t acc_y = (uint16_t)data[4] | ((uint16_t)data[5] << 8);
+    uint16_t acc_z = (uint16_t)data[6] | ((uint16_t)data[7] << 8);
+    
+    gimbal_imu->data.accl.x = uint_to_float(acc_x, GIMBAL_IMU_ACCEL_MIN, GIMBAL_IMU_ACCEL_MAX, 16);
+    gimbal_imu->data.accl.y = uint_to_float(acc_y, GIMBAL_IMU_ACCEL_MIN, GIMBAL_IMU_ACCEL_MAX, 16);
+    gimbal_imu->data.accl.z = uint_to_float(acc_z, GIMBAL_IMU_ACCEL_MIN, GIMBAL_IMU_ACCEL_MAX, 16);
+    
+    /* 温度数据 */
+    gimbal_imu->data.temp = (float)data[1];
+    
+  } else if (id == gimbal_imu->param.gyro_id) {
+    /* 解析陀螺仪数据 */
+    uint16_t gyro_x = (uint16_t)data[2] | ((uint16_t)data[3] << 8);
+    uint16_t gyro_y = (uint16_t)data[4] | ((uint16_t)data[5] << 8);
+    uint16_t gyro_z = (uint16_t)data[6] | ((uint16_t)data[7] << 8);
+    
+    gimbal_imu->data.gyro.x = uint_to_float(gyro_x, GIMBAL_IMU_GYRO_MIN, GIMBAL_IMU_GYRO_MAX, 16);
+    gimbal_imu->data.gyro.y = uint_to_float(gyro_y, GIMBAL_IMU_GYRO_MIN, GIMBAL_IMU_GYRO_MAX, 16);
+    gimbal_imu->data.gyro.z = uint_to_float(gyro_z, GIMBAL_IMU_GYRO_MIN, GIMBAL_IMU_GYRO_MAX, 16);
+    
+  } else if (id == gimbal_imu->param.eulr_id) {
+    /* 解析欧拉角数据 */
+    uint16_t pit = (uint16_t)data[2] | ((uint16_t)data[3] << 8);
+    uint16_t yaw = (uint16_t)data[4] | ((uint16_t)data[5] << 8);
+    uint16_t rol = (uint16_t)data[6] | ((uint16_t)data[7] << 8);
+    
+    gimbal_imu->data.eulr.pit = uint_to_float(pit, GIMBAL_IMU_PITCH_MIN, GIMBAL_IMU_PITCH_MAX, 16);
+    gimbal_imu->data.eulr.yaw = uint_to_float(yaw, GIMBAL_IMU_YAW_MIN, GIMBAL_IMU_YAW_MAX, 16);
+    gimbal_imu->data.eulr.rol = uint_to_float(rol, GIMBAL_IMU_ROLL_MIN, GIMBAL_IMU_ROLL_MAX, 16);
+    
+  } 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设备
+ */
+int8_t GIMBAL_IMU_Init(GIMBAL_IMU_t *gimbal_imu, const GIMBAL_IMU_Param_t *param) {
+  if (gimbal_imu == NULL || param == NULL) {
+    return -1;
+  }
+  
+  /* 清空数据 */
+  memset(gimbal_imu, 0, sizeof(GIMBAL_IMU_t));
+  
+  /* 复制参数 */
+  gimbal_imu->param = *param;
+  
+  /* 设置设备类型 */
+  gimbal_imu->header.online = false;
+  
+  /* 注册CAN ID到消息队列 */
+  BSP_CAN_RegisterId(param->can, param->accl_id, BSP_CAN_DEFAULT_QUEUE_SIZE);
+  BSP_CAN_RegisterId(param->can, param->gyro_id, BSP_CAN_DEFAULT_QUEUE_SIZE);
+  BSP_CAN_RegisterId(param->can, param->eulr_id, BSP_CAN_DEFAULT_QUEUE_SIZE);
+  BSP_CAN_RegisterId(param->can, param->quat_id, BSP_CAN_DEFAULT_QUEUE_SIZE);
+  
+  return 0;
+}
+
+/**
+ * @brief 更新Gimbal IMU设备状态 - 从CAN队列读取并解析数据
+ */
+int8_t GIMBAL_IMU_Update(GIMBAL_IMU_t *gimbal_imu) {
+  if (gimbal_imu == NULL) {
+    return -1;
+  }
+  
+  BSP_CAN_Message_t msg;
+  
+  /* 读取所有可用的CAN消息 */
+  while (BSP_CAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.accl_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == 0) {
+    GIMBAL_IMU_ParseMessage(gimbal_imu, gimbal_imu->param.accl_id, msg.data);
+  }
+  
+  while (BSP_CAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.gyro_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == 0) {
+    GIMBAL_IMU_ParseMessage(gimbal_imu, gimbal_imu->param.gyro_id, msg.data);
+  }
+  
+  while (BSP_CAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.eulr_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == 0) {
+    GIMBAL_IMU_ParseMessage(gimbal_imu, gimbal_imu->param.eulr_id, msg.data);
+  }
+  
+  while (BSP_CAN_GetMessage(gimbal_imu->param.can, gimbal_imu->param.quat_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == 0) {
+    GIMBAL_IMU_ParseMessage(gimbal_imu, gimbal_imu->param.quat_id, msg.data);
+  }
+  
+  return 0;
+}

User/device/gimbal_imu.h

@@ -0,0 +1,81 @@
+/*
+ * Gimbal IMU Device - 接收来自云台的CAN IMU数据
+ */
+
+#pragma once
+
+#include <math.h>
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "bsp/can.h"
+#include "component/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_CAN_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_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+
+/**
+ * @brief 初始化Gimbal IMU设备
+ * 
+ * @param gimbal_imu Gimbal IMU结构体指针
+ * @param param 参数配置
+ * @return int8_t 0:成功 -1:失败
+ */
+int8_t GIMBAL_IMU_Init(GIMBAL_IMU_t *gimbal_imu, const GIMBAL_IMU_Param_t *param);
+
+
+int8_t GIMBAL_IMU_Update(GIMBAL_IMU_t *gimbal_imu);
+
+
+#ifdef __cplusplus
+}
+#endif

User/task/init.c

@@ -33,8 +33,8 @@ void Task_Init(void *argument) {
   /* 创建任务线程 */
   task_runtime.thread.rc = osThreadNew(Task_rc, NULL, &attr_rc);
   task_runtime.thread.atti_esit = osThreadNew(Task_atti_esit, NULL, &attr_atti_esit);
-  // task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis);
+  task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis);
-  // task_runtime.thread.ctrl_gimbal = osThreadNew(Task_ctrl_gimbal, NULL, &attr_ctrl_gimbal);
+  task_runtime.thread.ctrl_gimbal = osThreadNew(Task_ctrl_gimbal, NULL, &attr_ctrl_gimbal);
   task_runtime.thread.monitor = osThreadNew(Task_monitor, NULL, &attr_monitor);
   task_runtime.thread.blink = osThreadNew(Task_blink, NULL, &attr_blink);
   task_runtime.thread.ctrl_shoot = osThreadNew(Task_ctrl_shoot, NULL, &attr_ctrl_shoot);