417 lines
13 KiB
C
417 lines
13 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>
|
||
|
||
/* Private typedef ---------------------------------------------------------- */
|
||
/* Private define ----------------------------------------------------------- */
|
||
|
||
|
||
/* Private macro ------------------------------------------------------------ */
|
||
/* Private variables -------------------------------------------------------- */
|
||
/* Private function prototypes ---------------------------------------------- */
|
||
static int8_t Virtual_Chassis_DecodeMotor(Virtual_Chassis_t *chassis, uint8_t id, uint8_t *data) {
|
||
if (chassis == NULL || data == NULL) {
|
||
return DEVICE_ERR_NULL;
|
||
}
|
||
|
||
// 1. 解析转矩电流 - 前2字节，16位有符号整数 (精度0.01 Nm)
|
||
int16_t torque_int;
|
||
memcpy(&torque_int, &data[0], sizeof(int16_t));
|
||
chassis->data.joint[id-chassis->param.motors.feedback_id].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[id-chassis->param.motors.feedback_id].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[id-chassis->param.motors.feedback_id].rotor_speed = (float)velocity_int / 1000.0f;
|
||
|
||
chassis->data.joint[id-chassis->param.motors.feedback_id].temp = 0.0f;
|
||
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
static int8_t Virtual_Chassis_DecodeIMU(Virtual_Chassis_t *chassis, uint8_t id, uint8_t *data, uint8_t len) {
|
||
if (chassis == NULL || data == NULL || len < 8) {
|
||
return DEVICE_ERR_NULL;
|
||
}
|
||
|
||
float *float_data = (float*)data;
|
||
|
||
switch (id) {
|
||
case 0x66: // 加速度计数据 - 8字节，2个float值 (x, y)
|
||
chassis->data.imu.accl.x = float_data[0];
|
||
chassis->data.imu.accl.y = float_data[1];
|
||
// z轴数据未传输，保持之前的值或设为0
|
||
// chassis->data.imu.accl.z = 0.0f;
|
||
break;
|
||
|
||
case 0x67: // 陀螺仪数据 - 8字节，2个float值 (x, y)
|
||
chassis->data.imu.gyro.x = float_data[0];
|
||
chassis->data.imu.gyro.y = float_data[1];
|
||
// z轴数据未传输，保持之前的值或设为0
|
||
// chassis->data.imu.gyro.z = 0.0f;
|
||
break;
|
||
|
||
case 0x68: // 欧拉角数据 - 8字节，2个float值 (yaw, pitch)
|
||
chassis->data.imu.euler.yaw = float_data[0];
|
||
chassis->data.imu.euler.pit = float_data[1];
|
||
// roll轴数据未传输，保持之前的值或设为0
|
||
// chassis->data.imu.euler.rol = 0.0f;
|
||
break;
|
||
|
||
case 0x69: // 四元数数据 - 8字节，2个float值 (q0, q1)
|
||
chassis->data.imu.quat.q0 = float_data[0];
|
||
chassis->data.imu.quat.q1 = float_data[1];
|
||
// q2, q3数据未传输，保持之前的值或设为0
|
||
// chassis->data.imu.quat.q2 = 0.0f;
|
||
// chassis->data.imu.quat.q3 = 0.0f;
|
||
break;
|
||
|
||
default:
|
||
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();
|
||
|
||
for (int i = 0; i < 4; i++) {
|
||
BSP_CAN_RegisterId(chassis->param.motors.can, 124 + i, 3); // 电机反馈ID
|
||
BSP_CAN_RegisterId(chassis->param.imu.can, 0x66 + 1, 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;
|
||
BSP_CAN_GetMessage(chassis->param.motors.can, 124, &msg, BSP_CAN_TIMEOUT_IMMEDIATE);
|
||
|
||
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
/**
|
||
* @brief 接收底盘反馈数据
|
||
*/
|
||
int8_t Virtual_Chassis_ReceiveFeedback(Virtual_Chassis_t *chassis) {
|
||
if (chassis == NULL) {
|
||
return DEVICE_ERR_NULL;
|
||
}
|
||
|
||
// 处理4个电机的反馈数据 (ID 124-127)
|
||
for (int i = 0; i < VIRTUAL_CHASSIS_MOTOR_COUNT; i++) {
|
||
Virtual_Chassis_ProcessMotorFeedback(chassis, i);
|
||
}
|
||
|
||
// 处理IMU反馈数据
|
||
Virtual_Chassis_ProcessIMUFeedback(chassis);
|
||
|
||
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,
|
||
};
|
||
|
||
if (BSP_CAN_TransmitStdDataFrame(chassis->param.motors.can, &enable_frame) == BSP_OK) {
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
return DEVICE_ERR;
|
||
}
|
||
|
||
/**
|
||
* @brief 发送电机力矩控制命令
|
||
*/
|
||
int8_t Virtual_Chassis_SendTorqueCommand(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.MIT_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 获取电机反馈数据
|
||
*/
|
||
Virtual_Chassis_MotorFeedback_t* Virtual_Chassis_GetMotorFeedback(Virtual_Chassis_t *chassis, uint8_t motor_id) {
|
||
if (chassis == NULL || motor_id >= VIRTUAL_CHASSIS_MOTOR_COUNT) {
|
||
return NULL;
|
||
}
|
||
|
||
return &chassis->data.motors[motor_id];
|
||
}
|
||
|
||
/**
|
||
* @brief 获取IMU反馈数据
|
||
*/
|
||
Virtual_Chassis_IMUFeedback_t* Virtual_Chassis_GetIMUFeedback(Virtual_Chassis_t *chassis) {
|
||
if (chassis == NULL) {
|
||
return NULL;
|
||
}
|
||
|
||
return &chassis->data.imu;
|
||
}
|
||
|
||
/**
|
||
* @brief 设置目标力矩
|
||
*/
|
||
int8_t Virtual_Chassis_SetTargetTorques(Virtual_Chassis_t *chassis, float torques[VIRTUAL_CHASSIS_MOTOR_COUNT]) {
|
||
if (chassis == NULL || torques == NULL) {
|
||
return DEVICE_ERR_NULL;
|
||
}
|
||
|
||
memcpy(chassis->data.target_torques, torques, sizeof(float) * VIRTUAL_CHASSIS_MOTOR_COUNT);
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
/**
|
||
* @brief 使能电机并发送命令
|
||
*/
|
||
int8_t Virtual_Chassis_Enable(Virtual_Chassis_t *chassis) {
|
||
if (chassis == NULL) {
|
||
return DEVICE_ERR_NULL;
|
||
}
|
||
|
||
return Virtual_Chassis_SendEnableCommand(chassis);
|
||
}
|
||
|
||
/**
|
||
* @brief 检查虚拟底盘是否在线
|
||
*/
|
||
bool Virtual_Chassis_IsOnline(Virtual_Chassis_t *chassis) {
|
||
if (chassis == NULL) {
|
||
return false;
|
||
}
|
||
|
||
uint64_t current_time = BSP_TIME_Get_us();
|
||
uint64_t timeout_us = VIRTUAL_CHASSIS_TIMEOUT_MS * 1000;
|
||
|
||
return chassis->header.online &&
|
||
(current_time - chassis->header.last_online_time) < timeout_us;
|
||
}
|
||
|
||
/* Private functions -------------------------------------------------------- */
|
||
|
||
/**
|
||
* @brief 注册CAN接收ID (接收底盘反馈数据)
|
||
*/
|
||
static int8_t Virtual_Chassis_RegisterCANIds(Virtual_Chassis_t *chassis) {
|
||
if (chassis == NULL) {
|
||
return DEVICE_ERR_NULL;
|
||
}
|
||
|
||
// 注册电机反馈数据ID
|
||
for (int i = 0; i < VIRTUAL_CHASSIS_MOTOR_COUNT; i++) {
|
||
if (BSP_CAN_RegisterId(chassis->param.motors[i].can, chassis->param.motors[i].feedback_id, 0) != BSP_OK) {
|
||
return DEVICE_ERR;
|
||
}
|
||
}
|
||
|
||
// 注册IMU反馈数据ID
|
||
if (BSP_CAN_RegisterId(chassis->param.imu_can, chassis->param.accl_id, 0) != BSP_OK) {
|
||
return DEVICE_ERR;
|
||
}
|
||
|
||
if (BSP_CAN_RegisterId(chassis->param.imu_can, chassis->param.gyro_id, 0) != BSP_OK) {
|
||
return DEVICE_ERR;
|
||
}
|
||
|
||
if (BSP_CAN_RegisterId(chassis->param.imu_can, chassis->param.euler_id, 0) != BSP_OK) {
|
||
return DEVICE_ERR;
|
||
}
|
||
|
||
if (BSP_CAN_RegisterId(chassis->param.imu_can, chassis->param.quat_id, 0) != BSP_OK) {
|
||
return DEVICE_ERR;
|
||
}
|
||
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
/**
|
||
* @brief 处理电机反馈数据
|
||
*/
|
||
static int8_t Virtual_Chassis_ProcessMotorFeedback(Virtual_Chassis_t *chassis, uint8_t motor_id) {
|
||
BSP_CAN_Message_t rx_msg;
|
||
|
||
if (BSP_CAN_GetMessage(chassis->param.motors[motor_id].can, chassis->param.motors[motor_id].feedback_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
|
||
// 解析数据：转矩电流(2字节) + 位置(3字节) + 速度(3字节) = 8字节
|
||
|
||
// 转矩电流 (精度0.01 Nm)
|
||
int16_t torque_int;
|
||
memcpy(&torque_int, &rx_msg.data[0], sizeof(int16_t));
|
||
chassis->data.motors[motor_id].current_torque = (float)torque_int / 100.0f;
|
||
|
||
// 位置 (精度0.0001 rad)
|
||
int32_t angle_int = 0;
|
||
angle_int |= ((int32_t)rx_msg.data[2]) << 16;
|
||
angle_int |= ((int32_t)rx_msg.data[3]) << 8;
|
||
angle_int |= ((int32_t)rx_msg.data[4]);
|
||
if (angle_int & 0x800000) { // 符号扩展
|
||
angle_int |= 0xFF000000;
|
||
}
|
||
chassis->data.motors[motor_id].current_angle = (float)angle_int / 10000.0f;
|
||
|
||
// 速度 (精度0.001 rad/s)
|
||
int32_t velocity_int = 0;
|
||
velocity_int |= ((int32_t)rx_msg.data[5]) << 16;
|
||
velocity_int |= ((int32_t)rx_msg.data[6]) << 8;
|
||
velocity_int |= ((int32_t)rx_msg.data[7]);
|
||
if (velocity_int & 0x800000) { // 符号扩展
|
||
velocity_int |= 0xFF000000;
|
||
}
|
||
chassis->data.motors[motor_id].current_velocity = (float)velocity_int / 1000.0f;
|
||
|
||
// 更新在线状态
|
||
chassis->data.motors[motor_id].online = true;
|
||
chassis->data.motors[motor_id].last_update_time = BSP_TIME_Get_us();
|
||
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
return DEVICE_ERR;
|
||
}
|
||
|
||
/**
|
||
* @brief 处理IMU反馈数据
|
||
*/
|
||
static int8_t Virtual_Chassis_ProcessIMUFeedback(Virtual_Chassis_t *chassis) {
|
||
BSP_CAN_Message_t rx_msg;
|
||
bool imu_updated = false;
|
||
|
||
// 处理加速度计数据
|
||
if (BSP_CAN_GetMessage(chassis->param.imu_can, chassis->param.accl_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
|
||
float *accl_data = (float*)rx_msg.data;
|
||
chassis->data.imu.accl.x = accl_data[0];
|
||
chassis->data.imu.accl.y = accl_data[1];
|
||
if (rx_msg.dlc >= 12) { // 如果有第三个数据
|
||
chassis->data.imu.accl.z = accl_data[2];
|
||
}
|
||
imu_updated = true;
|
||
}
|
||
|
||
// 处理陀螺仪数据
|
||
if (BSP_CAN_GetMessage(chassis->param.imu_can, chassis->param.gyro_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
|
||
float *gyro_data = (float*)rx_msg.data;
|
||
chassis->data.imu.gyro.x = gyro_data[0];
|
||
chassis->data.imu.gyro.y = gyro_data[1];
|
||
if (rx_msg.dlc >= 12) { // 如果有第三个数据
|
||
chassis->data.imu.gyro.z = gyro_data[2];
|
||
}
|
||
imu_updated = true;
|
||
}
|
||
|
||
// 处理欧拉角数据
|
||
if (BSP_CAN_GetMessage(chassis->param.imu_can, chassis->param.euler_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
|
||
float *euler_data = (float*)rx_msg.data;
|
||
chassis->data.imu.euler.yaw = euler_data[0];
|
||
chassis->data.imu.euler.pit = euler_data[1];
|
||
if (rx_msg.dlc >= 12) { // 如果有第三个数据
|
||
chassis->data.imu.euler.rol = euler_data[2];
|
||
}
|
||
imu_updated = true;
|
||
}
|
||
|
||
// 处理四元数数据（可选）
|
||
if (BSP_CAN_GetMessage(chassis->param.imu_can, chassis->param.quat_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
|
||
float *quat_data = (float*)rx_msg.data;
|
||
chassis->data.imu.quat.q0 = quat_data[0];
|
||
chassis->data.imu.quat.q1 = quat_data[1];
|
||
if (rx_msg.dlc >= 16) { // 如果有更多数据
|
||
chassis->data.imu.quat.q2 = quat_data[2];
|
||
chassis->data.imu.quat.q3 = quat_data[3];
|
||
}
|
||
imu_updated = true;
|
||
}
|
||
|
||
if (imu_updated) {
|
||
chassis->data.imu.online = true;
|
||
chassis->data.imu.last_update_time = BSP_TIME_Get_us();
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
return DEVICE_ERR;
|
||
} |