robofish/rm_balance
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

改解析

Browse Source
This commit is contained in:
Robofish 2025-10-06 20:34:24 +08:00
parent 0e8981bfa8
commit 880cbdb8d9
1 changed files with 60 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

145
User/device/virtual_chassis.c
View File

@ -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;
}