415 lines
15 KiB
C
415 lines
15 KiB
C
/*
|
||
* @file virtual_chassis.c
|
||
* @brief 虚拟底盘设备驱动实现 - 控制端
|
||
* @details 作为控制端，发送控制命令到底盘，接收底盘反馈数据
|
||
*/
|
||
|
||
/* Includes ----------------------------------------------------------------- */
|
||
#include "device/virtual_chassis.h"
|
||
#include "bsp/can.h"
|
||
#include "bsp/time.h"
|
||
#include <string.h>
|
||
#include <math.h>
|
||
|
||
/* Private typedef ---------------------------------------------------------- */
|
||
/* Private define ----------------------------------------------------------- */
|
||
|
||
/* Private macro ------------------------------------------------------------ */
|
||
/* Private variables -------------------------------------------------------- */
|
||
/* Private function prototypes ---------------------------------------------- */
|
||
/**
|
||
* @brief 解析关节电机反馈数据 (ID: 124-127)
|
||
* @param chassis 虚拟底盘结构体
|
||
* @param id CAN ID
|
||
* @param data 数据包
|
||
* @return 解析结果
|
||
*/
|
||
static int8_t Virtual_Chassis_DecodeJointMotor(Virtual_Chassis_t *chassis, uint16_t id, uint8_t *data) {
|
||
if (chassis == NULL || data == NULL) {
|
||
return DEVICE_ERR_NULL;
|
||
}
|
||
|
||
int motor_idx = id - chassis->param.motors.joint_feedback_base_id;
|
||
if (motor_idx < 0 || motor_idx >= 4) {
|
||
return DEVICE_ERR;
|
||
}
|
||
|
||
// 解析关节电机反馈数据：转矩电流(2字节) + 位置(3字节) + 速度(3字节)
|
||
|
||
// 1. 解析转矩电流 - 前2字节，16位有符号整数 (精度0.01 Nm)
|
||
int16_t torque_int;
|
||
memcpy(&torque_int, &data[0], sizeof(int16_t));
|
||
chassis->data.joint[motor_idx].torque_current = (float)torque_int / 100.0f;
|
||
|
||
// 2. 解析位置 - 第3-5字节，24位有符号整数 (精度0.0001 rad)
|
||
int32_t angle_int = 0;
|
||
angle_int |= ((int32_t)data[2]) << 16;
|
||
angle_int |= ((int32_t)data[3]) << 8;
|
||
angle_int |= ((int32_t)data[4]);
|
||
|
||
// 符号扩展（24位转32位）
|
||
if (angle_int & 0x800000) {
|
||
angle_int |= 0xFF000000;
|
||
}
|
||
chassis->data.joint[motor_idx].rotor_abs_angle = (float)angle_int / 10000.0f;
|
||
|
||
// 3. 解析速度 - 第6-8字节，24位有符号整数 (精度0.001 rad/s)
|
||
int32_t velocity_int = 0;
|
||
velocity_int |= ((int32_t)data[5]) << 16;
|
||
velocity_int |= ((int32_t)data[6]) << 8;
|
||
velocity_int |= ((int32_t)data[7]);
|
||
|
||
// 符号扩展（24位转32位）
|
||
if (velocity_int & 0x800000) {
|
||
velocity_int |= 0xFF000000;
|
||
}
|
||
chassis->data.joint[motor_idx].rotor_speed = (float)velocity_int / 1000.0f;
|
||
|
||
chassis->data.joint[motor_idx].temp = 0.0f;
|
||
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
/**
|
||
* @brief 解析轮子电机反馈数据 (ID: 130-131)
|
||
* @param chassis 虚拟底盘结构体
|
||
* @param id CAN ID
|
||
* @param data 数据包
|
||
* @return 解析结果
|
||
*/
|
||
static int8_t Virtual_Chassis_DecodeWheelMotor(Virtual_Chassis_t *chassis, uint16_t id, uint8_t *data) {
|
||
if (chassis == NULL || data == NULL) {
|
||
return DEVICE_ERR_NULL;
|
||
}
|
||
|
||
int wheel_idx;
|
||
if (id == chassis->param.motors.wheel_left_feedback_id) {
|
||
wheel_idx = 0; // 左轮
|
||
} else if (id == chassis->param.motors.wheel_right_feedback_id) {
|
||
wheel_idx = 1; // 右轮
|
||
} else {
|
||
return DEVICE_ERR;
|
||
}
|
||
|
||
// 解析轮子电机反馈数据：角度(2字节) + 速度(2字节) + 力矩电流(2字节) + 编码器(2字节)
|
||
|
||
// 角度 - 精度0.01度，需转换为弧度
|
||
int16_t angle_int;
|
||
angle_int = (data[0] << 8) | data[1];
|
||
chassis->data.wheel[wheel_idx].rotor_abs_angle = (float)angle_int / 100.0f * M_PI / 180.0f;
|
||
|
||
// 速度 - 精度1dps，直接赋值
|
||
int16_t speed_int;
|
||
speed_int = (data[2] << 8) | data[3];
|
||
chassis->data.wheel[wheel_idx].rotor_speed = (float)speed_int;
|
||
|
||
// 力矩电流
|
||
int16_t current_int;
|
||
current_int = (data[4] << 8) | data[5];
|
||
chassis->data.wheel[wheel_idx].torque_current = (float)current_int;
|
||
|
||
// 编码器值（暂不使用）
|
||
chassis->data.wheel[wheel_idx].temp = 0.0f;
|
||
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
/**
|
||
* @brief 解析IMU数据
|
||
* @param chassis 虚拟底盘结构体
|
||
* @param id CAN ID
|
||
* @param data 数据包
|
||
* @return 解析结果
|
||
*/
|
||
static int8_t Virtual_Chassis_DecodeIMU(Virtual_Chassis_t *chassis, uint16_t id, uint8_t *data) {
|
||
if (chassis == NULL || data == NULL) {
|
||
return DEVICE_ERR_NULL;
|
||
}
|
||
|
||
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;
|
||
|
||
// 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;
|
||
|
||
// 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;
|
||
}
|
||
else if (id == chassis->param.imu.gyro_id) {
|
||
// 陀螺仪数据 - x/y轴24位，z轴16位
|
||
// x: 24位有符号整数 (字节0-2) - 精度0.001°/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) * M_PI / 180.0f; // °/s -> rad/s
|
||
|
||
// y: 24位有符号整数 (字节3-5) - 精度0.001°/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) * M_PI / 180.0f; // °/s -> rad/s
|
||
|
||
// z: 16位有符号整数 (字节6-7) - 精度0.01°/s
|
||
int16_t z_int;
|
||
memcpy(&z_int, &data[6], sizeof(int16_t));
|
||
chassis->data.imu.gyro.z = ((float)z_int / 100.0f) * M_PI / 180.0f; // °/s -> rad/s
|
||
}
|
||
else if (id == chassis->param.imu.euler_id) {
|
||
// 欧拉角数据 - yaw/pitch轴24位，roll轴16位
|
||
// yaw: 24位有符号整数 (字节0-2) - 精度0.0001°
|
||
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) * M_PI / 180.0f; // ° -> rad
|
||
|
||
// pitch: 24位有符号整数 (字节3-5) - 精度0.0001°
|
||
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) * M_PI / 180.0f; // ° -> rad
|
||
|
||
// roll: 16位有符号整数 (字节6-7) - 精度0.001°
|
||
int16_t rol_int;
|
||
memcpy(&rol_int, &data[6], sizeof(int16_t));
|
||
chassis->data.imu.euler.rol = ((float)rol_int / 1000.0f) * M_PI / 180.0f; // ° -> rad
|
||
}
|
||
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;
|
||
}
|
||
else {
|
||
return DEVICE_ERR;
|
||
}
|
||
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
/* Exported functions ------------------------------------------------------- */
|
||
|
||
/**
|
||
* @brief 初始化虚拟底盘设备
|
||
*/
|
||
int8_t Virtual_Chassis_Init(Virtual_Chassis_t *chassis, Virtual_Chassis_Param_t *param) {
|
||
if (chassis == NULL || param == NULL) {
|
||
return DEVICE_ERR_NULL;
|
||
}
|
||
|
||
// 复制参数配置
|
||
chassis->param = *param;
|
||
|
||
// 初始化设备头部
|
||
chassis->header.online = false;
|
||
chassis->header.last_online_time = 0;
|
||
|
||
// 初始化数据
|
||
memset(&chassis->data, 0, sizeof(Virtual_Chassis_Feedback_t));
|
||
memset(&chassis->output, 0, sizeof(Virtual_Chassis_Output_t));
|
||
|
||
BSP_CAN_Init();
|
||
|
||
// 注册关节电机反馈ID (124-127)
|
||
for (int i = 0; i < 4; i++) {
|
||
BSP_CAN_RegisterId(chassis->param.motors.can, chassis->param.motors.joint_feedback_base_id + i, 3);
|
||
}
|
||
|
||
// 注册轮子电机反馈ID (130-131)
|
||
BSP_CAN_RegisterId(chassis->param.motors.can, chassis->param.motors.wheel_left_feedback_id, 3);
|
||
BSP_CAN_RegisterId(chassis->param.motors.can, chassis->param.motors.wheel_right_feedback_id, 3);
|
||
|
||
// 注册IMU数据ID (769-772 / 0x301-0x304)
|
||
BSP_CAN_RegisterId(chassis->param.imu.can, chassis->param.imu.accl_id, 3);
|
||
BSP_CAN_RegisterId(chassis->param.imu.can, chassis->param.imu.gyro_id, 3);
|
||
BSP_CAN_RegisterId(chassis->param.imu.can, chassis->param.imu.euler_id, 3);
|
||
BSP_CAN_RegisterId(chassis->param.imu.can, chassis->param.imu.quat_id, 3);
|
||
|
||
// 设置设备在线状态
|
||
chassis->header.online = true;
|
||
chassis->header.last_online_time = BSP_TIME_Get_us();
|
||
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
|
||
/**
|
||
* @brief 更新虚拟底盘数据（接收底盘反馈数据）
|
||
*/
|
||
int8_t Virtual_Chassis_Update(Virtual_Chassis_t *chassis) {
|
||
if (chassis == NULL) {
|
||
return DEVICE_ERR_NULL;
|
||
}
|
||
|
||
BSP_CAN_Message_t msg;
|
||
|
||
// 接收IMU数据
|
||
if (BSP_CAN_GetMessage(chassis->param.imu.can, chassis->param.imu.accl_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
|
||
Virtual_Chassis_DecodeIMU(chassis, chassis->param.imu.accl_id, msg.data);
|
||
}
|
||
if (BSP_CAN_GetMessage(chassis->param.imu.can, chassis->param.imu.gyro_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
|
||
Virtual_Chassis_DecodeIMU(chassis, chassis->param.imu.gyro_id, msg.data);
|
||
}
|
||
if (BSP_CAN_GetMessage(chassis->param.imu.can, chassis->param.imu.euler_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
|
||
Virtual_Chassis_DecodeIMU(chassis, chassis->param.imu.euler_id, msg.data);
|
||
}
|
||
if (BSP_CAN_GetMessage(chassis->param.imu.can, chassis->param.imu.quat_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
|
||
Virtual_Chassis_DecodeIMU(chassis, chassis->param.imu.quat_id, msg.data);
|
||
}
|
||
|
||
// 接收关节电机反馈数据 (124-127)
|
||
for (int i = 0; i < 4; i++) {
|
||
uint16_t joint_id = chassis->param.motors.joint_feedback_base_id + i;
|
||
if (BSP_CAN_GetMessage(chassis->param.motors.can, joint_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
|
||
Virtual_Chassis_DecodeJointMotor(chassis, joint_id, msg.data);
|
||
}
|
||
}
|
||
|
||
// 接收轮子电机反馈数据 (130-131)
|
||
if (BSP_CAN_GetMessage(chassis->param.motors.can, chassis->param.motors.wheel_left_feedback_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
|
||
Virtual_Chassis_DecodeWheelMotor(chassis, chassis->param.motors.wheel_left_feedback_id, msg.data);
|
||
}
|
||
if (BSP_CAN_GetMessage(chassis->param.motors.can, chassis->param.motors.wheel_right_feedback_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
|
||
Virtual_Chassis_DecodeWheelMotor(chassis, chassis->param.motors.wheel_right_feedback_id, msg.data);
|
||
}
|
||
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
|
||
/**
|
||
* @brief 发送电机使能命令
|
||
*/
|
||
int8_t Virtual_Chassis_Enable(Virtual_Chassis_t *chassis) {
|
||
if (chassis == NULL) {
|
||
return DEVICE_ERR_NULL;
|
||
}
|
||
|
||
BSP_CAN_StdDataFrame_t enable_frame = {
|
||
.id = chassis->param.motors.enable_id,
|
||
.dlc = 0,
|
||
.data = {0}
|
||
};
|
||
|
||
chassis->output.enable_joints = true;
|
||
|
||
if (BSP_CAN_TransmitStdDataFrame(chassis->param.motors.can, &enable_frame) == BSP_OK) {
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
return DEVICE_ERR;
|
||
}
|
||
|
||
/**
|
||
* @brief 发送关节电机力矩控制命令
|
||
*/
|
||
int8_t Virtual_Chassis_SendJointTorque(Virtual_Chassis_t *chassis, float torques[4]) {
|
||
if (chassis == NULL || torques == NULL) {
|
||
return DEVICE_ERR_NULL;
|
||
}
|
||
|
||
BSP_CAN_StdDataFrame_t torque_frame = {
|
||
.id = chassis->param.motors.joint_cmd_id,
|
||
.dlc = 8,
|
||
.data = {0}
|
||
};
|
||
|
||
// 8字节数据分别是4个电机的力矩 (每个电机2字节，有符号整数，精度0.01 Nm)
|
||
for (int i = 0; i < 4; i++) {
|
||
int16_t torque_raw = (int16_t)(torques[i] * 100.0f);
|
||
memcpy(&torque_frame.data[i * 2], &torque_raw, sizeof(int16_t));
|
||
chassis->output.joint_torque[i] = torques[i];
|
||
}
|
||
|
||
return BSP_CAN_TransmitStdDataFrame(chassis->param.motors.can, &torque_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
|
||
}
|
||
|
||
/**
|
||
* @brief 发送轮子电机控制命令
|
||
*/
|
||
int8_t Virtual_Chassis_SendWheelCommand(Virtual_Chassis_t *chassis, float left_out, float right_out) {
|
||
if (chassis == NULL) {
|
||
return DEVICE_ERR_NULL;
|
||
}
|
||
int16_t torque_control = (int16_t)(left_out * 2048);
|
||
// 构建CAN帧
|
||
BSP_CAN_StdDataFrame_t tx_frame;
|
||
tx_frame.id = chassis->param.motors.wheel_left_id;
|
||
tx_frame.dlc = 8;
|
||
|
||
tx_frame.data[0] = 0xA1;
|
||
tx_frame.data[1] = 0x00;
|
||
tx_frame.data[2] = 0x00;
|
||
tx_frame.data[3] = 0x00;
|
||
tx_frame.data[4] = (uint8_t)(torque_control & 0xFF);
|
||
tx_frame.data[5] = (uint8_t)((torque_control >> 8) & 0xFF);
|
||
tx_frame.data[6] = 0x00;
|
||
tx_frame.data[7] = 0x00;
|
||
if (BSP_CAN_TransmitStdDataFrame(chassis->param.motors.can, &tx_frame) != BSP_OK) {
|
||
return DEVICE_ERR;
|
||
}
|
||
torque_control = (int16_t)(right_out * 2048);
|
||
// 构建CAN帧
|
||
tx_frame.id = chassis->param.motors.wheel_right_id;
|
||
tx_frame.dlc = 8;
|
||
tx_frame.data[0] = 0xA1;
|
||
tx_frame.data[1] = 0x00;
|
||
tx_frame.data[2] = 0x00;
|
||
tx_frame.data[3] = 0x00;
|
||
tx_frame.data[4] = (uint8_t)(torque_control & 0xFF);
|
||
tx_frame.data[5] = (uint8_t)((torque_control >> 8) & 0xFF);
|
||
tx_frame.data[6] = 0x00;
|
||
tx_frame.data[7] = 0x00;
|
||
if (BSP_CAN_TransmitStdDataFrame(chassis->param.motors.can, &tx_frame) != BSP_OK) {
|
||
return DEVICE_ERR;
|
||
}
|
||
}
|
||
|
||
|
||
/* Private functions -------------------------------------------------------- */
|