From 880cbdb8d97b50475e3f6140a795d0d4b76ee739 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Mon, 6 Oct 2025 20:34:24 +0800 Subject: [PATCH] =?UTF-8?q?=E6=94=B9=E8=A7=A3=E6=9E=90?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/device/virtual_chassis.c | 145 ++++++++++++++-------------------- 1 file changed, 60 insertions(+), 85 deletions(-) diff --git a/User/device/virtual_chassis.c b/User/device/virtual_chassis.c index c2e3e0c..227031a 100644 --- a/User/device/virtual_chassis.c +++ b/User/device/virtual_chassis.c @@ -114,6 +114,24 @@ static int8_t Virtual_Chassis_DecodeWheelMotor(Virtual_Chassis_t *chassis, uint1 return DEVICE_OK; } +/* 数据范围定义 - 与发送端一致 */ +#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 QUATERNION_MIN (-1.0f) +#define QUATERNION_MAX (1.0f) + +/* 反向映射宏：将16位整数还原为浮点值 */ +#define UNMAP_FROM_INT16(int_val, min, max) \ + (((float)(int_val) + 32768.0f) / 65535.0f * ((max) - (min)) + (min)) + /** * @brief 解析IMU数据 * @param chassis 虚拟底盘结构体 @@ -127,103 +145,58 @@ static int8_t Virtual_Chassis_DecodeIMU(Virtual_Chassis_t *chassis, uint16_t id, } if (id == chassis->param.imu.accl_id) { - // 加速度计数据 - x/y轴24位，z轴16位 - // x: 24位有符号整数 (字节0-2) - 精度1/1000000 - int32_t x_int = 0; - x_int |= ((int32_t)data[0]) << 16; - x_int |= ((int32_t)data[1]) << 8; - x_int |= ((int32_t)data[2]); - // 符号扩展（24位转32位） - if (x_int & 0x800000) { - x_int |= 0xFF000000; - } - chassis->data.imu.accl.x = (float)x_int / 1000000.0f; + // 加速度计数据 - 每轴使用16位编码 (6字节总共) + int16_t ax_int, ay_int, az_int; - // y: 24位有符号整数 (字节3-5) - 精度1/1000000 - int32_t y_int = 0; - y_int |= ((int32_t)data[3]) << 16; - y_int |= ((int32_t)data[4]) << 8; - y_int |= ((int32_t)data[5]); - // 符号扩展（24位转32位） - if (y_int & 0x800000) { - y_int |= 0xFF000000; - } - chassis->data.imu.accl.y = (float)y_int / 1000000.0f; + memcpy(&ax_int, &data[0], sizeof(int16_t)); + memcpy(&ay_int, &data[2], sizeof(int16_t)); + memcpy(&az_int, &data[4], sizeof(int16_t)); - // z: 16位有符号整数 (字节6-7) - 精度1/10000 - int16_t z_int; - memcpy(&z_int, &data[6], sizeof(int16_t)); - chassis->data.imu.accl.z = (float)z_int / 10000.0f; + // 反向映射到原始浮点值 + chassis->data.imu.accl.x = UNMAP_FROM_INT16(ax_int, ACCEL_CAN_MIN, ACCEL_CAN_MAX); + chassis->data.imu.accl.y = UNMAP_FROM_INT16(ay_int, ACCEL_CAN_MIN, ACCEL_CAN_MAX); + chassis->data.imu.accl.z = UNMAP_FROM_INT16(az_int, ACCEL_CAN_MIN, ACCEL_CAN_MAX); } else if (id == chassis->param.imu.gyro_id) { - // 陀螺仪数据 - x/y轴24位，z轴16位 - // x: 24位有符号整数 (字节0-2) - 精度0.001 rad/s (不再转换单位) - int32_t x_int = 0; - x_int |= ((int32_t)data[0]) << 16; - x_int |= ((int32_t)data[1]) << 8; - x_int |= ((int32_t)data[2]); - // 符号扩展（24位转32位） - if (x_int & 0x800000) { - x_int |= 0xFF000000; - } - chassis->data.imu.gyro.x = (float)x_int / 1000.0f; // 直接除以1000，保持rad/s + // 陀螺仪数据 - 每轴使用16位编码 (6字节总共) + int16_t gx_int, gy_int, gz_int; - // y: 24位有符号整数 (字节3-5) - 精度0.001 rad/s (不再转换单位) - int32_t y_int = 0; - y_int |= ((int32_t)data[3]) << 16; - y_int |= ((int32_t)data[4]) << 8; - y_int |= ((int32_t)data[5]); - // 符号扩展（24位转32位） - if (y_int & 0x800000) { - y_int |= 0xFF000000; - } - chassis->data.imu.gyro.y = -(float)y_int / 1000.0f; // 直接除以1000，保持rad/s + memcpy(&gx_int, &data[0], sizeof(int16_t)); + memcpy(&gy_int, &data[2], sizeof(int16_t)); + memcpy(&gz_int, &data[4], sizeof(int16_t)); - // z: 16位有符号整数 (字节6-7) - 精度0.01 rad/s (不再转换单位) - int16_t z_int; - memcpy(&z_int, &data[6], sizeof(int16_t)); - chassis->data.imu.gyro.z = (float)z_int / 100.0f; // 直接除以100，保持rad/s + // 反向映射到原始浮点值 + chassis->data.imu.gyro.x = UNMAP_FROM_INT16(gx_int, GYRO_CAN_MIN, GYRO_CAN_MAX); + chassis->data.imu.gyro.y = UNMAP_FROM_INT16(gy_int, GYRO_CAN_MIN, GYRO_CAN_MAX); + chassis->data.imu.gyro.z = UNMAP_FROM_INT16(gz_int, GYRO_CAN_MIN, GYRO_CAN_MAX); } else if (id == chassis->param.imu.euler_id) { - // 欧拉角数据 - yaw/pitch轴24位，roll轴16位 - // yaw: 24位有符号整数 (字节0-2) - 精度0.0001 rad (不再转换单位) - int32_t yaw_int = 0; - yaw_int |= ((int32_t)data[0]) << 16; - yaw_int |= ((int32_t)data[1]) << 8; - yaw_int |= ((int32_t)data[2]); - // 符号扩展（24位转32位） - if (yaw_int & 0x800000) { - yaw_int |= 0xFF000000; - } - chassis->data.imu.euler.yaw = (float)yaw_int / 10000.0f; // 直接除以10000，保持rad + // 欧拉角数据 - 每角使用16位编码 (6字节总共) + int16_t yaw_int, pitch_int, roll_int; - // pitch: 24位有符号整数 (字节3-5) - 精度0.0001 rad (不再转换单位) - int32_t pit_int = 0; - pit_int |= ((int32_t)data[3]) << 16; - pit_int |= ((int32_t)data[4]) << 8; - pit_int |= ((int32_t)data[5]); - // 符号扩展（24位转32位） - if (pit_int & 0x800000) { - pit_int |= 0xFF000000; - } - chassis->data.imu.euler.pit = -(float)pit_int / 10000.0f; // 直接除以10000，保持rad + memcpy(&yaw_int, &data[0], sizeof(int16_t)); + memcpy(&pitch_int, &data[2], sizeof(int16_t)); + memcpy(&roll_int, &data[4], sizeof(int16_t)); - // roll: 16位有符号整数 (字节6-7) - 精度0.001 rad (不再转换单位) - int16_t rol_int; - memcpy(&rol_int, &data[6], sizeof(int16_t)); - chassis->data.imu.euler.rol = (float)rol_int / 1000.0f; // 直接除以1000，保持rad + // 反向映射到原始浮点值 + chassis->data.imu.euler.yaw = UNMAP_FROM_INT16(yaw_int, YAW_CAN_MIN, YAW_CAN_MAX); + chassis->data.imu.euler.pit = UNMAP_FROM_INT16(pitch_int, PITCH_CAN_MIN, PITCH_CAN_MAX); + chassis->data.imu.euler.rol = UNMAP_FROM_INT16(roll_int, ROLL_CAN_MIN, ROLL_CAN_MAX); } else if (id == chassis->param.imu.quat_id) { - // 四元数数据 - q0/q1/q2/q3 各2字节，精度1/32767 - int16_t q0, q1, q2, q3; - memcpy(&q0, &data[0], 2); - memcpy(&q1, &data[2], 2); - memcpy(&q2, &data[4], 2); - memcpy(&q3, &data[6], 2); - chassis->data.imu.quat.q0 = q0 / 32767.0f; - chassis->data.imu.quat.q1 = q1 / 32767.0f; - chassis->data.imu.quat.q2 = q2 / 32767.0f; - chassis->data.imu.quat.q3 = q3 / 32767.0f; + // 四元数数据 - 每个分量使用16位编码 (8字节总共) + int16_t q0_int, q1_int, q2_int, q3_int; + + memcpy(&q0_int, &data[0], sizeof(int16_t)); + memcpy(&q1_int, &data[2], sizeof(int16_t)); + memcpy(&q2_int, &data[4], sizeof(int16_t)); + memcpy(&q3_int, &data[6], sizeof(int16_t)); + + // 反向映射到原始浮点值 + chassis->data.imu.quat.q0 = UNMAP_FROM_INT16(q0_int, QUATERNION_MIN, QUATERNION_MAX); + chassis->data.imu.quat.q1 = UNMAP_FROM_INT16(q1_int, QUATERNION_MIN, QUATERNION_MAX); + chassis->data.imu.quat.q2 = UNMAP_FROM_INT16(q2_int, QUATERNION_MIN, QUATERNION_MAX); + chassis->data.imu.quat.q3 = UNMAP_FROM_INT16(q3_int, QUATERNION_MIN, QUATERNION_MAX); } else { return DEVICE_ERR; @@ -408,6 +381,8 @@ int8_t Virtual_Chassis_SendWheelCommand(Virtual_Chassis_t *chassis, float left_o if (BSP_CAN_TransmitStdDataFrame(chassis->param.motors.can, &tx_frame) != BSP_OK) { return DEVICE_ERR; } + + return DEVICE_OK; }