改发送

User/component/user_math.h

@@ -25,6 +25,10 @@ extern "C" {
 #define M_PI 3.14159265358979323846f
 #endif
 
+#ifndef M_PI_2
+#define M_PI_2 1.57079632679489661923f
+#endif
+
 #ifndef M_2PI
 #define M_2PI 6.28318530717958647692f
 #endif

User/module/balance_chassis.c

@@ -24,10 +24,16 @@
 #define CAN_FEEDBACK_WHEEL_RIGHT_ID 131 // 右轮反馈ID
 
 /* Private macro ------------------------------------------------------------ */
+#define CMD_TIMEOUT_MS 50  // 50ms超时时间，允许控制频率在10-20Hz之间
+
 /* Private variables -------------------------------------------------------- */
 static bool joint_command_received = false;
 static bool wheel_command_received[2] = {false, false};
 
+// 超时管理 - 防止控制频率不同导致的控制/relax交替
+static uint32_t joint_last_cmd_time = 0;
+static uint32_t wheel_last_cmd_time[2] = {0, 0};
+
 /* Private function --------------------------------------------------------- */
 
 /**
@@ -41,6 +47,28 @@ static int8_t Chassis_ProcessCANCommands(Chassis_t *chassis) {
     wheel_command_received[0] = false;
     wheel_command_received[1] = false;
   
+        // 检查ID 128 - 左轮控制命令 (与电机发送格式一致)
+    if (BSP_CAN_GetMessage(BSP_CAN_1, CAN_CMD_WHEEL_LEFT_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
+        wheel_command_received[0] = true;
+        wheel_last_cmd_time[0] = BSP_TIME_Get_ms(); // 更新左轮命令时间戳
+        float left_out = (float)((int16_t)(rx_msg.data[4] | (rx_msg.data[5] << 8))) / 2048.0f;
+        chassis->output.wheel[0] = left_out;
+        MOTOR_LK_SetOutput(&chassis->param.wheel_param[0], left_out);
+
+    }
+    
+    // 检查ID 129 - 右轮控制命令 (与电机发送格式一致) 
+    if (BSP_CAN_GetMessage(BSP_CAN_1, CAN_CMD_WHEEL_RIGHT_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
+        wheel_command_received[1] = true;
+        wheel_last_cmd_time[1] = BSP_TIME_Get_ms(); // 更新右轮命令时间戳
+        float right_out = (float)((int16_t)(rx_msg.data[4] | (rx_msg.data[5] << 8))) / 2048.0f;
+        chassis->output.wheel[1] = right_out;
+        MOTOR_LK_SetOutput(&chassis->param.wheel_param[1], right_out);
+    }
+
+    BSP_TIME_Delay_ms(1); // 短暂延时，避免总线冲突
+
+
     // 检查ID 121 - 使能4个关节电机
     if (BSP_CAN_GetMessage(BSP_CAN_1, CAN_CMD_ENABLE_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
         joint_command_received = true;
@@ -52,12 +80,16 @@ static int8_t Chassis_ProcessCANCommands(Chassis_t *chassis) {
     // 检查ID 122 - 运控模式控制4个关节电机
     if (BSP_CAN_GetMessage(BSP_CAN_1, CAN_CMD_JOINT_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
         joint_command_received = true;
+        joint_last_cmd_time = BSP_TIME_Get_ms(); // 更新关节命令时间戳
+
+        
+        
         // 8字节数据分别是4个电机的力矩 (每个电机2字节，有符号整数，精度0.01 Nm)
         for (int i = 0; i < 4; i++) {
             int16_t torque_raw;
             memcpy(&torque_raw, &rx_msg.data[i * 2], sizeof(int16_t));
             float torque = (float)torque_raw / 100.0f; // 转换为浮点数力矩值
-            
+            chassis->output.joint[i] = torque;
             // 使用运控模式控制电机，只设置力矩，其他参数为0
             MOTOR_LZ_MotionParam_t motion_param = {
                 .target_angle = 0.0f,
@@ -70,21 +102,6 @@ static int8_t Chassis_ProcessCANCommands(Chassis_t *chassis) {
         }
     }
 
-    // 检查ID 128 - 左轮控制命令 (与电机发送格式一致)
-    if (BSP_CAN_GetMessage(BSP_CAN_1, CAN_CMD_WHEEL_LEFT_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
-        wheel_command_received[0] = true;
-        float left_out = (float)((int16_t)(rx_msg.data[4] | (rx_msg.data[5] << 8))) / 2048.0f;
-        MOTOR_LK_SetOutput(&chassis->param.wheel_param[0], left_out);
-
-    }
-    
-    // 检查ID 129 - 右轮控制命令 (与电机发送格式一致) 
-    if (BSP_CAN_GetMessage(BSP_CAN_1, CAN_CMD_WHEEL_RIGHT_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
-        wheel_command_received[1] = true;
-        float right_out = (float)((int16_t)(rx_msg.data[4] | (rx_msg.data[5] << 8))) / 2048.0f;
-        MOTOR_LK_SetOutput(&chassis->param.wheel_param[1], right_out);
-    }
-    BSP_TIME_Delay_us(400); // 确保CAN总线有足够时间处理消息
     return DEVICE_OK;
 }
 
@@ -310,17 +327,25 @@ int8_t Chassis_Ctrl(Chassis_t *chassis) {
     // 处理CAN控制命令
     Chassis_ProcessCANCommands(chassis);
     
-    // 如果没有收到关节控制命令，关节电机进入relax模式
+    uint32_t current_time = BSP_TIME_Get_ms();
-    if (!joint_command_received) {
+    
+    // 关节电机超时检查 - 只有在超时时才执行relax
+    if (!joint_command_received && 
+        (current_time - joint_last_cmd_time) > CMD_TIMEOUT_MS) {
         for (int i = 0; i < 4; i++) {
             MOTOR_LZ_Relax(&chassis->param.joint_param[i]);
+
+            chassis->output.joint[i] = 0.0f; // 松弛时输出力矩设为0
         }
     }
     
-    // 如果没有收到轮子控制命令，轮子电机进入relax模式
+    // 轮子电机超时检查 - 只有在超时时才执行relax
     for (int i = 0; i < 2; i++) {
-        if (!wheel_command_received[i]) {
+        if (!wheel_command_received[i] && 
+            (current_time - wheel_last_cmd_time[i]) > CMD_TIMEOUT_MS) {
             MOTOR_LK_Relax(&chassis->param.wheel_param[i]);
+
+            chassis->output.wheel[i] = 0.0f; // 松弛时输出设为0
         }
     }
     

User/task/1.c

@@ -0,0 +1,95 @@
+/*
+    imu Task
+    
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "task/user_task.h"
+/* USER INCLUDE BEGIN */
+#include "bsp/pwm.h"
+#include "component/ahrs.h"
+#include "component/pid.h"
+#include "device/bmi088.h"
+/* USER INCLUDE END */
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+/* USER STRUCT BEGIN */
+
+BMI088_t bmi088;
+
+AHRS_t gimbal_ahrs;
+AHRS_Magn_t magn;
+AHRS_Eulr_t eulr_to_send;
+
+KPID_t imu_temp_ctrl_pid;
+
+BMI088_Cali_t cali_bmi088= {
+  .gyro_offset = {0.0f,0.0f,0.0f},
+};
+
+/* USER STRUCT END */
+
+/* Private function --------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+void Task_imu(void *argument) {
+  (void)argument; /* 未使用argument，消除警告 */
+
+  
+  /* 计算任务运行到指定频率需要等待的tick数 */
+  const uint32_t delay_tick = osKernelGetTickFreq() / IMU_FREQ;
+
+  osDelay(IMU_INIT_DELAY); /* 延时一段时间再开启任务 */
+
+  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
+  /* USER CODE INIT BEGIN */
+  BMI088_Init(&bmi088,&cali_bmi088);
+  AHRS_Init(&gimbal_ahrs, &magn, BMI088_GetUpdateFreq(&bmi088));
+
+  /* USER CODE INIT END */
+  
+  while (1) {
+    tick += delay_tick; /* 计算下一个唤醒时刻 */
+    /* USER CODE BEGIN */
+    BMI088_WaitNew();
+    BMI088_AcclStartDmaRecv();
+    BMI088_AcclWaitDmaCplt();
+
+    BMI088_GyroStartDmaRecv();
+    BMI088_GyroWaitDmaCplt();
+
+    /* 锁住RTOS内核防止数据解析过程中断，造成错误 */
+    osKernelLock();
+    /* 接收完所有数据后，把数据从原始字节加工成方便计算的数据 */
+  BMI088_ParseAccl(&bmi088);
+  /* 扩大加速度数据10倍，并交换x和y */
+  float temp_x = bmi088.accl.x * 10.0f;
+  float temp_y = bmi088.accl.y * 10.0f;
+  bmi088.accl.x = temp_y;
+  bmi088.accl.y = -temp_x;
+  bmi088.accl.z *= 10.0f;
+
+  BMI088_ParseGyro(&bmi088);
+  /* 交换陀螺仪x和y */
+  float temp_gyro_x = bmi088.gyro.x;
+  bmi088.gyro.x = bmi088.gyro.y;
+  bmi088.gyro.y = -temp_gyro_x;
+    // IST8310_Parse(&ist8310);
+
+    /* 根据设备接收到的数据进行姿态解析 */
+    AHRS_Update(&gimbal_ahrs, &bmi088.accl, &bmi088.gyro, &magn);
+
+    /* 根据解析出来的四元数计算欧拉角 */
+  AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs);
+  /* 交换pit和rol */
+  float temp_rol = eulr_to_send.rol;
+  eulr_to_send.rol = eulr_to_send.pit;
+  eulr_to_send.pit = temp_rol;
+    osKernelUnlock();
+    /* USER CODE END */
+    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
+  }
+  
+}

User/task/atti_esti.c

@@ -144,13 +144,34 @@ void Task_atti_esti(void *argument) {
     
     osKernelUnlock();
 
+    /* 创建修改后的数据副本用于发送到消息队列 */
+    AHRS_Accl_t accl_modified;
+    AHRS_Gyro_t gyro_modified;
+    AHRS_Eulr_t eulr_modified;
+    
+    /* 加速度数据：x和y互换，y取负值，全部乘10 */
+    accl_modified.x = bmi088.accl.y * 10.0f;
+    accl_modified.y = -bmi088.accl.x * 10.0f;
+    accl_modified.z = bmi088.accl.z * 10.0f;
+    
+    
+    /* 陀螺仪数据：x和y互换，y取负值 */
+    gyro_modified.x = bmi088.gyro.y;
+    gyro_modified.y = -bmi088.gyro.x;
+    gyro_modified.z = bmi088.gyro.z;
+    
+    /* 欧拉角数据：roll和pitch互换 */
+    eulr_modified.yaw = eulr_to_send.yaw;
+    eulr_modified.pit = eulr_to_send.rol;  /* pitch = 原roll */
+    eulr_modified.rol = eulr_to_send.pit;  /* roll = 原pitch */
+
     osMessageQueueReset(task_runtime.msgq.imu.accl);
     osMessageQueueReset(task_runtime.msgq.imu.gyro);
     osMessageQueueReset(task_runtime.msgq.imu.eulr);
     osMessageQueueReset(task_runtime.msgq.imu.quat);
-    osMessageQueuePut(task_runtime.msgq.imu.accl, &bmi088.accl, 0, 0);
+    osMessageQueuePut(task_runtime.msgq.imu.accl, &accl_modified, 0, 0);
-    osMessageQueuePut(task_runtime.msgq.imu.gyro, &bmi088.gyro, 0, 0);
+    osMessageQueuePut(task_runtime.msgq.imu.gyro, &gyro_modified, 0, 0);
-    osMessageQueuePut(task_runtime.msgq.imu.eulr, &eulr_to_send, 0, 0);
+    osMessageQueuePut(task_runtime.msgq.imu.eulr, &eulr_modified, 0, 0);
     osMessageQueuePut(task_runtime.msgq.imu.quat, &gimbal_ahrs.quat, 0, 0);
 
     BSP_PWM_SetComp(BSP_PWM_IMU_HEAT_PWM, PID_Calc(&imu_temp_ctrl_pid, 50.0f, bmi088.temp, 0.0f, 0.0f));

User/task/imu.c

@@ -6,6 +6,7 @@
 /* Includes ----------------------------------------------------------------- */
 #include "task/user_task.h"
 /* USER INCLUDE BEGIN */
+#include "component/user_math.h"
 #include "bsp/can.h"
 #include "bsp/time.h"
 #include "component/ahrs.h"
@@ -20,7 +21,29 @@
 #define CAN_ID_AHRS_GYRO    0x302  /* 陀螺仪数据 */
 #define CAN_ID_AHRS_EULR    0x303  /* 欧拉角数据 */
 #define CAN_ID_AHRS_QUAT    0x304  /* 四元数数据 */
+
+/* 数据范围定义 */
+#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       (M_PI_2)   /* π/2 弧度 ≈ 90° */
+#define PITCH_CAN_MIN       (-M_PI_2)  /* -π/2 弧度 ≈ -90° */
+#define ROLL_CAN_MAX        (M_2PI)
+#define ROLL_CAN_MIN        (-M_2PI)
+#define YAW_CAN_MAX         (M_2PI)
+#define YAW_CAN_MIN         (-M_PI_2)  /* -π 弧度 ≈ -180° */
+#define QUATERNION_MIN      (-1.0f)
+#define QUATERNION_MAX      (1.0f)
+
 /* Private macro ------------------------------------------------------------ */
+/* 数据映射宏：将浮点值映射到16位整数范围 */
+#define MAP_TO_INT16(val, min, max) \
+    ((int16_t)(((val) - (min)) / ((max) - (min)) * 65535.0f - 32768.0f))
+
+/* 限制值在指定范围内 */
+#define CLAMP(val, min, max) \
+    (((val) < (min)) ? (min) : (((val) > (max)) ? (max) : (val)))
 /* Private variables -------------------------------------------------------- */
 /* USER STRUCT BEGIN */
 AHRS_Accl_t accl;
@@ -48,133 +71,88 @@ void Task_imu(void *argument) {
   while (1) {
     tick += delay_tick; /* 计算下一个唤醒时刻 */
     /* USER CODE BEGIN */
-    /* 获取加速度计数据并通过CAN发送 - 使用24位精度，充分利用8字节 */
+    /* 获取加速度计数据并通过CAN发送 - 每轴使用16位编码 */
     if (osMessageQueueGet(task_runtime.msgq.imu.accl, &accl, NULL, 0) == osOK) {
         BSP_CAN_StdDataFrame_t accl_frame;
         accl_frame.id = CAN_ID_AHRS_ACCL;
-        accl_frame.dlc = 8;  /* 充分利用8字节数据帧 */
+        accl_frame.dlc = 6;  /* 3轴 × 2字节 = 6字节 */
         
-        /* 使用24位精度存储x/y轴，16位存储z轴 + 2字节预留
+        /* 限制并映射加速度数据到16位整数 */
-         * x: 24位 (精度1/1000000，范围±8.388g)
+        float ax = CLAMP(accl.x, ACCEL_CAN_MIN, ACCEL_CAN_MAX);
-         * y: 24位 (精度1/1000000，范围±8.388g) 
+        float ay = CLAMP(accl.y, ACCEL_CAN_MIN, ACCEL_CAN_MAX);
-         * z: 16位 (精度1/10000，范围±3.276g)
+        float az = CLAMP(accl.z, ACCEL_CAN_MIN, ACCEL_CAN_MAX);
-         * 预留: 2字节用于扩展或校验
-         */
         
-        // X轴 - 24位有符号整数 (字节0-2)
+        int16_t ax_int = MAP_TO_INT16(ax, ACCEL_CAN_MIN, ACCEL_CAN_MAX);
-        int32_t x_int = (int32_t)(accl.x * 1000000.0f);
+        int16_t ay_int = MAP_TO_INT16(ay, ACCEL_CAN_MIN, ACCEL_CAN_MAX);
-        x_int = (x_int > 8388607) ? 8388607 : ((x_int < -8388608) ? -8388608 : x_int);
+        int16_t az_int = MAP_TO_INT16(az, ACCEL_CAN_MIN, ACCEL_CAN_MAX);
-        accl_frame.data[0] = (x_int >> 16) & 0xFF;
-        accl_frame.data[1] = (x_int >> 8) & 0xFF;
-        accl_frame.data[2] = x_int & 0xFF;
         
-        // Y轴 - 24位有符号整数 (字节3-5)
+        memcpy(&accl_frame.data[0], &ax_int, sizeof(int16_t));
-        int32_t y_int = (int32_t)(accl.y * 1000000.0f);
+        memcpy(&accl_frame.data[2], &ay_int, sizeof(int16_t));
-        y_int = (y_int > 8388607) ? 8388607 : ((y_int < -8388608) ? -8388608 : y_int);
+        memcpy(&accl_frame.data[4], &az_int, sizeof(int16_t));
-        accl_frame.data[3] = (y_int >> 16) & 0xFF;
-        accl_frame.data[4] = (y_int >> 8) & 0xFF;
-        accl_frame.data[5] = y_int & 0xFF;
-        
-        // Z轴 - 16位有符号整数 (字节6-7)
-        int16_t z_int = (int16_t)(accl.z * 10000.0f);
-        memcpy(&accl_frame.data[6], &z_int, sizeof(int16_t));
         
         BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &accl_frame);
     }
     
-    /* 获取陀螺仪数据并通过CAN发送 - 使用24位精度，充分利用8字节 */
+    /* 获取陀螺仪数据并通过CAN发送 - 每轴使用16位编码 */
     if (osMessageQueueGet(task_runtime.msgq.imu.gyro, &gyro, NULL, 0) == osOK) {
         BSP_CAN_StdDataFrame_t gyro_frame;
         gyro_frame.id = CAN_ID_AHRS_GYRO;
-        gyro_frame.dlc = 8;  /* 充分利用8字节数据帧 */
+        gyro_frame.dlc = 6;  /* 3轴 × 2字节 = 6字节 */
         
-        /* 使用24位精度存储x/y轴，16位存储z轴 + 2字节预留
+        /* 限制并映射陀螺仪数据到16位整数 */
-         * x: 24位 (精度1/1000，范围±8388°/s)
+        float gx = CLAMP(gyro.x, GYRO_CAN_MIN, GYRO_CAN_MAX);
-         * y: 24位 (精度1/1000，范围±8388°/s)
+        float gy = CLAMP(gyro.y, GYRO_CAN_MIN, GYRO_CAN_MAX);
-         * z: 16位 (精度1/100，范围±327°/s) 
+        float gz = CLAMP(gyro.z, GYRO_CAN_MIN, GYRO_CAN_MAX);
-         * 预留: 2字节用于扩展或校验
-         */
         
-        // X轴 - 24位有符号整数 (字节0-2) - 精度0.001°/s
+        int16_t gx_int = MAP_TO_INT16(gx, GYRO_CAN_MIN, GYRO_CAN_MAX);
-        int32_t x_int = (int32_t)(gyro.x * 1000.0f);
+        int16_t gy_int = MAP_TO_INT16(gy, GYRO_CAN_MIN, GYRO_CAN_MAX);
-        x_int = (x_int > 8388607) ? 8388607 : ((x_int < -8388608) ? -8388608 : x_int);
+        int16_t gz_int = MAP_TO_INT16(gz, GYRO_CAN_MIN, GYRO_CAN_MAX);
-        gyro_frame.data[0] = (x_int >> 16) & 0xFF;
-        gyro_frame.data[1] = (x_int >> 8) & 0xFF;
-        gyro_frame.data[2] = x_int & 0xFF;
         
-        // Y轴 - 24位有符号整数 (字节3-5) - 精度0.001°/s
+        memcpy(&gyro_frame.data[0], &gx_int, sizeof(int16_t));
-        int32_t y_int = (int32_t)(gyro.y * 1000.0f);
+        memcpy(&gyro_frame.data[2], &gy_int, sizeof(int16_t));
-        y_int = (y_int > 8388607) ? 8388607 : ((y_int < -8388608) ? -8388608 : y_int);
+        memcpy(&gyro_frame.data[4], &gz_int, sizeof(int16_t));
-        gyro_frame.data[3] = (y_int >> 16) & 0xFF;
-        gyro_frame.data[4] = (y_int >> 8) & 0xFF;
-        gyro_frame.data[5] = y_int & 0xFF;
-        
-        // Z轴 - 16位有符号整数 (字节6-7) - 精度0.01°/s
-        int16_t z_int = (int16_t)(gyro.z * 100.0f);
-        memcpy(&gyro_frame.data[6], &z_int, sizeof(int16_t));
         
         BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &gyro_frame);
     }
     
-    /* 获取欧拉角数据并通过CAN发送 - 使用24位精度，充分利用8字节 */
+    /* 获取欧拉角数据并通过CAN发送 - 每角使用16位编码 */
     if (osMessageQueueGet(task_runtime.msgq.imu.eulr, &eulr, NULL, 0) == osOK) {
         BSP_CAN_StdDataFrame_t eulr_frame;
         eulr_frame.id = CAN_ID_AHRS_EULR;
-        eulr_frame.dlc = 8;  /* 充分利用8字节数据帧 */
+        eulr_frame.dlc = 6;  /* 3个角度 × 2字节 = 6字节 */
         
-        /* 使用更高精度存储欧拉角
+        /* 限制并映射欧拉角数据到16位整数 */
-         * yaw: 24位 (精度1/10000，范围±838.8°)
+        float yaw = CLAMP(eulr.yaw, YAW_CAN_MIN, YAW_CAN_MAX);
-         * pitch: 24位 (精度1/10000，范围±838.8°) 
+        float pitch = CLAMP(eulr.pit, PITCH_CAN_MIN, PITCH_CAN_MAX);
-         * roll: 16位 (精度1/1000，范围±32.767°)
+        float roll = CLAMP(eulr.rol, ROLL_CAN_MIN, ROLL_CAN_MAX);
-         * 预留: 2字节用于扩展或校验
-         */
         
-        // Yaw - 24位有符号整数 (字节0-2) - 精度0.0001°
+        int16_t yaw_int = MAP_TO_INT16(yaw, YAW_CAN_MIN, YAW_CAN_MAX);
-        int32_t yaw_int = (int32_t)(eulr.yaw * 10000.0f);
+        int16_t pitch_int = MAP_TO_INT16(pitch, PITCH_CAN_MIN, PITCH_CAN_MAX);
-        yaw_int = (yaw_int > 8388607) ? 8388607 : ((yaw_int < -8388608) ? -8388608 : yaw_int);
+        int16_t roll_int = MAP_TO_INT16(roll, ROLL_CAN_MIN, ROLL_CAN_MAX);
-        eulr_frame.data[0] = (yaw_int >> 16) & 0xFF;
-        eulr_frame.data[1] = (yaw_int >> 8) & 0xFF;
-        eulr_frame.data[2] = yaw_int & 0xFF;
         
-        // Pitch - 24位有符号整数 (字节3-5) - 精度0.0001°
+        memcpy(&eulr_frame.data[0], &yaw_int, sizeof(int16_t));
-        int32_t pit_int = (int32_t)(eulr.pit * 10000.0f);
+        memcpy(&eulr_frame.data[2], &pitch_int, sizeof(int16_t));
-        pit_int = (pit_int > 8388607) ? 8388607 : ((pit_int < -8388608) ? -8388608 : pit_int);
+        memcpy(&eulr_frame.data[4], &roll_int, sizeof(int16_t));
-        eulr_frame.data[3] = (pit_int >> 16) & 0xFF;
-        eulr_frame.data[4] = (pit_int >> 8) & 0xFF;
-        eulr_frame.data[5] = pit_int & 0xFF;
-        
-        // Roll - 16位有符号整数 (字节6-7) - 精度0.001°
-        int16_t rol_int = (int16_t)(eulr.rol * 1000.0f);
-        memcpy(&eulr_frame.data[6], &rol_int, sizeof(int16_t));
         
         BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &eulr_frame);
     }
     
-    /* 获取四元数数据并通过CAN发送 - 优化精度分配 */
+    /* 获取四元数数据并通过CAN发送 - 每个分量使用16位编码 */
     if (osMessageQueueGet(task_runtime.msgq.imu.quat, &quat, NULL, 0) == osOK) {
         BSP_CAN_StdDataFrame_t quat_frame;
         quat_frame.id = CAN_ID_AHRS_QUAT;
-        quat_frame.dlc = 8;  /* 充分利用8字节数据帧 */
+        quat_frame.dlc = 8;  /* 4个四元数分量 × 2字节 = 8字节 */
         
-        /* 优化四元数精度分配，四元数范围-1~1
+        /* 限制并映射四元数到16位整数 */
-         * q0: 16位 (精度1/32767，最高精度)
+        float q0 = CLAMP(quat.q0, QUATERNION_MIN, QUATERNION_MAX);
-         * q1: 16位 (精度1/32767，最高精度)  
+        float q1 = CLAMP(quat.q1, QUATERNION_MIN, QUATERNION_MAX);
-         * q2: 16位 (精度1/32767，最高精度)
+        float q2 = CLAMP(quat.q2, QUATERNION_MIN, QUATERNION_MAX);
-         * q3: 16位 (精度1/32767，最高精度)
+        float q3 = CLAMP(quat.q3, QUATERNION_MIN, QUATERNION_MAX);
-         * 使用int16_t的全部范围，精度提升至1/32767
-         */
         
-        // 将四元数归一化并转换为int16_t，使用int16_t全部范围
+        int16_t q0_int = MAP_TO_INT16(q0, QUATERNION_MIN, QUATERNION_MAX);
-        int16_t q0_int = (int16_t)(quat.q0 * 32767.0f);
+        int16_t q1_int = MAP_TO_INT16(q1, QUATERNION_MIN, QUATERNION_MAX);
-        int16_t q1_int = (int16_t)(quat.q1 * 32767.0f);
+        int16_t q2_int = MAP_TO_INT16(q2, QUATERNION_MIN, QUATERNION_MAX);
-        int16_t q2_int = (int16_t)(quat.q2 * 32767.0f);
+        int16_t q3_int = MAP_TO_INT16(q3, QUATERNION_MIN, QUATERNION_MAX);
-        int16_t q3_int = (int16_t)(quat.q3 * 32767.0f);
-        
-        // 限制范围防止溢出
-        q0_int = (q0_int > 32767) ? 32767 : ((q0_int < -32767) ? -32767 : q0_int);
-        q1_int = (q1_int > 32767) ? 32767 : ((q1_int < -32767) ? -32767 : q1_int);
-        q2_int = (q2_int > 32767) ? 32767 : ((q2_int < -32767) ? -32767 : q2_int);
-        q3_int = (q3_int > 32767) ? 32767 : ((q3_int < -32767) ? -32767 : q3_int);
         
         memcpy(&quat_frame.data[0], &q0_int, sizeof(int16_t));
         memcpy(&quat_frame.data[2], &q1_int, sizeof(int16_t));