robofish/MRobot
1
0
Fork 0
mirror of https://github.com/goldenfishs/MRobot.git synced 2025-11-02 04:23:10 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

增加dm电机

Browse Source
This commit is contained in:
Robofish 2025-10-01 11:46:16 +08:00
parent e3ed160f42
commit d2eea18ecc
4 changed files with 1081 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

498
assets/User_code/device/motor_dm.c Normal file
View File

@ -0,0 +1,498 @@
#define MOTOR_DM_FLOAT_TO_INT_SIGNED(x, x_min, x_max, bits) \
((int32_t)roundf(((x) / ((x_max) - (x_min))) * (1 << (bits)) + (1 << ((bits) - 1))))
#define MOTOR_DM_INT_TO_FLOAT_SIGNED(x_int, x_min, x_max, bits) \
(((float)((int32_t)(x_int) - (1 << ((bits) - 1))) * ((x_max) - (x_min)) / (float)(1 << (bits))))
/* Includes ----------------------------------------------------------------- */
#include "device/motor_dm.h"
#include "bsp/mm.h"
#include "bsp/time.h"
#include "component/user_math.h"
#include "string.h"
#include "math.h"
/* Private constants -------------------------------------------------------- */
/* DM电机数据映射范围 */
#define DM_P_MIN (-12.56637f)
#define DM_P_MAX (12.56637f)
#define DM_V_MIN (-30.0f)
#define DM_V_MAX (30.0f)
#define DM_T_MIN (-12.0f)
#define DM_T_MAX (12.0f)
#define DM_KP_MIN (0.0f)
#define DM_KP_MAX (500.0f)
#define DM_KD_MIN (0.0f)
#define DM_KD_MAX (5.0f)
/* CAN ID偏移量 */
#define DM_CAN_ID_OFFSET_POS_VEL 0x100
#define DM_CAN_ID_OFFSET_VEL 0x200
/* Private macro ------------------------------------------------------------ */
#define FLOAT_TO_UINT(x, x_min, x_max, bits) \
(uint32_t)((x - x_min) * ((1 << bits) - 1) / (x_max - x_min))
#define UINT_TO_FLOAT(x_int, x_min, x_max, bits) \
((float)(x_int) * (x_max - x_min) / ((1 << bits) - 1) + x_min)
/* Private variables -------------------------------------------------------- */
static MOTOR_DM_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
static int float_to_uint(float x_float, float x_min, float x_max, int bits)
{
/* Converts a float to an unsigned int, given range and number of bits */
float span = x_max - x_min;
float offset = x_min;
return (int) ((x_float-offset)*((float)((1<<bits)-1))/span);
}
static float uint_to_float(int x_int, float x_min, float x_max, int bits)
{
/* converts unsigned int to float, given range and number of bits */
float span = x_max - x_min;
float offset = x_min;
return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
}
/* Private function prototypes ---------------------------------------------- */
static int8_t MOTOR_DM_ParseFeedbackFrame(MOTOR_DM_t *motor, const uint8_t *data);
static int8_t MOTOR_DM_SendMITCmd(MOTOR_DM_t *motor, MOTOR_MIT_Output_t *output);
static int8_t MOTOR_DM_SendPosVelCmd(MOTOR_DM_t *motor, float pos, float vel);
static int8_t MOTOR_DM_SendVelCmd(MOTOR_DM_t *motor, float vel);
static MOTOR_DM_CANManager_t* MOTOR_DM_GetCANManager(BSP_CAN_t can);
/* Private functions -------------------------------------------------------- */
/**
* @brief 解析DM电机反馈帧
* @param motor 电机实例
* @param data CAN数据
* @return 解析结果
*/
static int8_t MOTOR_DM_ParseFeedbackFrame(MOTOR_DM_t *motor, const uint8_t *data) {
if (motor == NULL || data == NULL) {
return DEVICE_ERR_NULL;
}
uint16_t p_int=(data[1]<<8)|data[2];
motor->motor.feedback.rotor_abs_angle = uint_to_float(p_int, DM_P_MIN, DM_P_MAX, 16); // (-12.5,12.5)
uint16_t v_int=(data[3]<<4)|(data[4]>>4);
motor->motor.feedback.rotor_speed = uint_to_float(v_int, DM_V_MIN, DM_V_MAX, 12); // (-30.0,30.0)
uint16_t t_int=((data[4]&0xF)<<8)|data[5];
motor->motor.feedback.torque_current = uint_to_float(t_int, DM_T_MIN, DM_T_MAX, 12); // (-12.0,12.0)
motor->motor.feedback.temp = (float)(data[6]);
while (motor->motor.feedback.rotor_abs_angle < 0) {
motor->motor.feedback.rotor_abs_angle += M_2PI;
}
while (motor->motor.feedback.rotor_abs_angle >= M_2PI) {
motor->motor.feedback.rotor_abs_angle -= M_2PI;
}
if (motor->param.reverse) {
motor->motor.feedback.rotor_abs_angle = M_2PI - motor->motor.feedback.rotor_abs_angle;
motor->motor.feedback.rotor_speed = -motor->motor.feedback.rotor_speed;
motor->motor.feedback.torque_current = -motor->motor.feedback.torque_current;
}
return DEVICE_OK;
}
/**
* @brief 发送MIT模式控制命令
* @param motor 电机实例
* @param output MIT控制参数
* @return 发送结果
*/
static int8_t MOTOR_DM_SendMITCmd(MOTOR_DM_t *motor, MOTOR_MIT_Output_t *output) {
if (motor == NULL || output == NULL) {
return DEVICE_ERR_NULL;
}
uint8_t data[8];
uint16_t pos_tmp,vel_tmp,kp_tmp,kd_tmp,tor_tmp;
uint16_t id = motor->param.can_id;
pos_tmp = float_to_uint(output->angle, DM_P_MIN , DM_P_MAX, 16);
vel_tmp = float_to_uint(output->velocity, DM_V_MIN , DM_V_MAX, 12);
kp_tmp = float_to_uint(output->kp, DM_KP_MIN, DM_KP_MAX, 12);
kd_tmp = float_to_uint(output->kd, DM_KD_MIN, DM_KD_MAX, 12);
tor_tmp = float_to_uint(output->torque, DM_T_MIN , DM_T_MAX, 12);
/* 打包数据 */
data[0] = (pos_tmp >> 8);
data[1] = pos_tmp;
data[2] = (vel_tmp >> 4);
data[3] = ((vel_tmp&0xF)<<4)|(kp_tmp>>8);
data[4] = kp_tmp;
data[5] = (kd_tmp >> 4);
data[6] = ((kd_tmp&0xF)<<4)|(tor_tmp>>8);
data[7] = tor_tmp;
/* 发送CAN消息 */
BSP_CAN_StdDataFrame_t frame;
frame.id = motor->param.can_id;
frame.dlc = 8;
memcpy(frame.data, data, 8);
BSP_CAN_WaitTxMailboxEmpty(motor->param.can, 1);
return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
/**
* @brief 发送位置速度模式控制命令
* @param motor 电机实例
* @param pos 目标位置
* @param vel 目标速度
* @return 发送结果
*/
static int8_t MOTOR_DM_SendPosVelCmd(MOTOR_DM_t *motor, float pos, float vel) {
if (motor == NULL) {
return DEVICE_ERR_NULL;
}
uint8_t data[8] = {0};
/* 直接发送浮点数数据 */
memcpy(&data[0], &pos, 4); // 位置，低位在前
memcpy(&data[4], &vel, 4); // 速度，低位在前
/* 发送CAN消息，ID为原ID+0x100 */
uint32_t can_id = DM_CAN_ID_OFFSET_POS_VEL + motor->param.can_id;
BSP_CAN_StdDataFrame_t frame;
frame.id = can_id;
frame.dlc = 8;
memcpy(frame.data, data, 8);
BSP_CAN_WaitTxMailboxEmpty(motor->param.can, 1);
return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
/**
* @brief 发送速度模式控制命令
* @param motor 电机实例
* @param vel 目标速度
* @return 发送结果
*/
static int8_t MOTOR_DM_SendVelCmd(MOTOR_DM_t *motor, float vel) {
if (motor == NULL) {
return DEVICE_ERR_NULL;
}
uint8_t data[4] = {0};
/* 直接发送浮点数数据 */
memcpy(&data[0], &vel, 4); // 速度，低位在前
/* 发送CAN消息，ID为原ID+0x200 */
uint32_t can_id = DM_CAN_ID_OFFSET_VEL + motor->param.can_id;
BSP_CAN_StdDataFrame_t frame;
frame.id = can_id;
frame.dlc = 4;
memcpy(frame.data, data, 4);
BSP_CAN_WaitTxMailboxEmpty(motor->param.can, 1);
return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
/**
* @brief 获取指定CAN总线的管理器
* @param can CAN总线
* @return CAN管理器指针
*/
static MOTOR_DM_CANManager_t* MOTOR_DM_GetCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) {
return NULL;
}
return can_managers[can];
}
/**
* @brief 创建CAN管理器
* @param can CAN总线
* @return 创建结果
*/
static int8_t MOTOR_DM_CreateCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return DEVICE_ERR;
if (can_managers[can] != NULL) return DEVICE_OK;
can_managers[can] = (MOTOR_DM_CANManager_t*)BSP_Malloc(sizeof(MOTOR_DM_CANManager_t));
if (can_managers[can] == NULL) return DEVICE_ERR;
memset(can_managers[can], 0, sizeof(MOTOR_DM_CANManager_t));
can_managers[can]->can = can;
return DEVICE_OK;
}
/* Exported functions ------------------------------------------------------- */
/**
* @brief 注册一个DM电机
* @param param 电机参数
* @return 注册结果
*/
int8_t MOTOR_DM_Register(MOTOR_DM_Param_t *param) {
if (param == NULL) {
return DEVICE_ERR_NULL;
}
/* 创建CAN管理器 */
if (MOTOR_DM_CreateCANManager(param->can) != DEVICE_OK) {
return DEVICE_ERR;
}
/* 获取CAN管理器 */
MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager(param->can);
if (manager == NULL) {
return DEVICE_ERR;
}
/* 检查是否已注册 */
for (int i = 0; i < manager->motor_count; i++) {
if (manager->motors[i] && manager->motors[i]->param.master_id == param->master_id) {
return DEVICE_ERR_INITED;
}
}
/* 检查是否已达到最大数量 */
if (manager->motor_count >= MOTOR_DM_MAX_MOTORS) {
return DEVICE_ERR;
}
/* 分配内存 */
MOTOR_DM_t *motor = (MOTOR_DM_t *)BSP_Malloc(sizeof(MOTOR_DM_t));
if (motor == NULL) {
return DEVICE_ERR;
}
/* 初始化电机 */
memset(motor, 0, sizeof(MOTOR_DM_t));
memcpy(&motor->param, param, sizeof(MOTOR_DM_Param_t));
motor->motor.header.online = false;
motor->motor.reverse = param->reverse;
/* 注册CAN接收ID - DM电机使用Master ID接收反馈 */
uint16_t feedback_id = param->master_id;
if (BSP_CAN_RegisterId(param->can, feedback_id, 3) != BSP_OK) {
BSP_Free(motor);
return DEVICE_ERR;
}
/* 添加到管理器 */
manager->motors[manager->motor_count] = motor;
manager->motor_count++;
return DEVICE_OK;
}
/**
* @brief 更新指定电机数据
* @param param 电机参数
* @return 更新结果
*/
int8_t MOTOR_DM_Update(MOTOR_DM_Param_t *param) {
if (param == NULL) {
return DEVICE_ERR_NULL;
}
MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager(param->can);
if (manager == NULL) {
return DEVICE_ERR_NO_DEV;
}
/* 查找电机 */
MOTOR_DM_t *motor = NULL;
for (int i = 0; i < manager->motor_count; i++) {
if (manager->motors[i] && manager->motors[i]->param.master_id == param->master_id) {
motor = manager->motors[i];
break;
}
}
if (motor == NULL) {
return DEVICE_ERR_NO_DEV;
}
/* 主动接收CAN消息 */
uint16_t feedback_id = param->master_id;
BSP_CAN_Message_t rx_msg;
if (BSP_CAN_GetMessage(param->can, feedback_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
uint64_t now_time = BSP_TIME_Get();
if (now_time - motor->motor.header.last_online_time > 100000) { // 100ms超时，单位微秒
motor->motor.header.online = false;
}
return DEVICE_ERR;
}
motor->motor.header.online = true;
motor->motor.header.last_online_time = BSP_TIME_Get();
MOTOR_DM_ParseFeedbackFrame(motor, rx_msg.data);
return DEVICE_OK;
}
/**
* @brief 更新所有电机数据
* @return 更新结果
*/
int8_t MOTOR_DM_UpdateAll(void) {
int8_t ret = DEVICE_OK;
for (int can = 0; can < BSP_CAN_NUM; can++) {
MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager((BSP_CAN_t)can);
if (manager == NULL) continue;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_DM_t *motor = manager->motors[i];
if (motor != NULL) {
if (MOTOR_DM_Update(&motor->param) != DEVICE_OK) {
ret = DEVICE_ERR;
}
}
}
}
return ret;
}
/**
* @brief MIT模式控制
* @param param 电机参数
* @param output MIT控制参数
* @return 控制结果
*/
int8_t MOTOR_DM_MITCtrl(MOTOR_DM_Param_t *param, MOTOR_MIT_Output_t *output) {
if (param == NULL || output == NULL) {
return DEVICE_ERR_NULL;
}
MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param);
if (motor == NULL) {
return DEVICE_ERR_NO_DEV;
}
return MOTOR_DM_SendMITCmd(motor, output);
}
/**
* @brief 位置速度模式控制
* @param param 电机参数
* @param target_pos 目标位置
* @param target_vel 目标速度
* @return 控制结果
*/
int8_t MOTOR_DM_PosVelCtrl(MOTOR_DM_Param_t *param, float target_pos, float target_vel) {
if (param == NULL) {
return DEVICE_ERR_NULL;
}
MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param);
if (motor == NULL) {
return DEVICE_ERR_NO_DEV;
}
return MOTOR_DM_SendPosVelCmd(motor, target_pos, target_vel);
}
/**
* @brief 速度模式控制
* @param param 电机参数
* @param target_vel 目标速度
* @return 控制结果
*/
int8_t MOTOR_DM_VelCtrl(MOTOR_DM_Param_t *param, float target_vel) {
if (param == NULL) {
return DEVICE_ERR_NULL;
}
MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param);
if (motor == NULL) {
return DEVICE_ERR_NO_DEV;
}
return MOTOR_DM_SendVelCmd(motor, target_vel);
}
/**
* @brief 获取指定电机的实例指针
* @param param 电机参数
* @return 电机实例指针
*/
MOTOR_DM_t* MOTOR_DM_GetMotor(MOTOR_DM_Param_t *param) {
if (param == NULL) {
return NULL;
}
MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager(param->can);
if (manager == NULL) {
return NULL;
}
/* 查找对应的电机 */
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_DM_t *motor = manager->motors[i];
if (motor && motor->param.can == param->can &&
motor->param.master_id == param->master_id) {
return motor;
}
}
return NULL;
}
int8_t MOTOR_DM_Enable(MOTOR_DM_Param_t *param){
if (param == NULL) {
return DEVICE_ERR_NULL;
}
MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param);
if (motor == NULL) {
return DEVICE_ERR_NO_DEV;
}
BSP_CAN_StdDataFrame_t frame;
frame.id = motor->param.can_id;
frame.dlc = 8;
frame.data[0] = 0XFF;
frame.data[1] = 0xFF;
frame.data[2] = 0xFF;
frame.data[3] = 0xFF;
frame.data[4] = 0xFF;
frame.data[5] = 0xFF;
frame.data[6] = 0xFF;
frame.data[7] = 0xFC;
BSP_CAN_WaitTxMailboxEmpty(motor->param.can, 1);
return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
/**
* @brief 使电机松弛（设置输出为0）
* @param param 电机参数
* @return 操作结果
*/
int8_t MOTOR_DM_Relax(MOTOR_DM_Param_t *param) {
if (param == NULL) {
return DEVICE_ERR_NULL;
}
MOTOR_MIT_Output_t output = {0};
return MOTOR_DM_MITCtrl(param, &output);
}
/**
* @brief 使电机离线（设置在线状态为false）
* @param param 电机参数
* @return 操作结果
*/
int8_t MOTOR_DM_Offine(MOTOR_DM_Param_t *param) {
if (param == NULL) {
return DEVICE_ERR_NULL;
}
MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param);
if (motor == NULL) {
return DEVICE_ERR_NO_DEV;
}
motor->motor.header.online = false;
return DEVICE_OK;
}

98
assets/User_code/device/motor_dm.h Normal file
View File

@ -0,0 +1,98 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "device/device.h"
#include "device/motor.h"
#include "bsp/can.h"
/* Exported constants ------------------------------------------------------- */
#define MOTOR_DM_MAX_MOTORS 32
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef enum {
MOTOR_DM_J4310,
} MOTOR_DM_Module_t;
/*每个电机需要的参数*/
typedef struct {
BSP_CAN_t can;
uint16_t master_id; /* 主站ID，用于发送控制命令 */
uint16_t can_id; /* 反馈ID，用于接收电机反馈 */
MOTOR_DM_Module_t module;
bool reverse;
} MOTOR_DM_Param_t;
/*电机实例*/
typedef struct{
MOTOR_DM_Param_t param;
MOTOR_t motor;
} MOTOR_DM_t;
/*CAN管理器，管理一个CAN总线上所有的电机*/
typedef struct {
BSP_CAN_t can;
MOTOR_DM_t *motors[MOTOR_DM_MAX_MOTORS];
uint8_t motor_count;
} MOTOR_DM_CANManager_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 注册一个LK电机
* @param param 电机参数
* @return
*/
int8_t MOTOR_DM_Register(MOTOR_DM_Param_t *param);
/**
* @brief 更新指定电机数据
* @param param 电机参数
* @return
*/
int8_t MOTOR_DM_Update(MOTOR_DM_Param_t *param);
/**
* @brief 更新所有电机数据
* @return
*/
int8_t MOTOR_DM_UpdateAll(void);
int8_t MOTOR_DM_MITCtrl(MOTOR_DM_Param_t *param, MOTOR_MIT_Output_t *output);
int8_t MOTOR_DM_PosVelCtrl(MOTOR_DM_Param_t *param, float target_pos, float target_vel);
int8_t MOTOR_DM_VelCtrl(MOTOR_DM_Param_t *param, float target_vel);
/**
* @brief 获取指定电机的实例指针
* @param param 电机参数
* @return
*/
MOTOR_DM_t* MOTOR_DM_GetMotor(MOTOR_DM_Param_t *param);
int8_t MOTOR_DM_Enable(MOTOR_DM_Param_t *param);
/**
* @brief 使电机松弛（设置输出为0）
* @param param
* @return
*/
int8_t MOTOR_DM_Relax(MOTOR_DM_Param_t *param);
/**
* @brief 使电机离线（设置在线状态为false）
* @param param
* @return
*/
int8_t MOTOR_DM_Offine(MOTOR_DM_Param_t *param);
#ifdef __cplusplus
}
#endif

328
assets/User_code/device/rc_can.c Normal file
View File

@ -0,0 +1,328 @@
/* Includes ----------------------------------------------------------------- */
#include "device/rc_can.h"
#include "bsp/time.h"
#include "device/device.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Private constants -------------------------------------------------------- */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Private macro ------------------------------------------------------------ */
/* Private types ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER VARIABLE BEGIN */
/* USER VARIABLE END */
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
/* Private function prototypes ---------------------------------------------- */
static int8_t RC_CAN_ValidateParams(const RC_CAN_Param_t *param);
static int8_t RC_CAN_RegisterIds(RC_CAN_t *rc_can);
/* Exported functions ------------------------------------------------------- */
/**
* @brief 初始化RC CAN发送模块
* @param rc_can RC_CAN结构体指针
* @param param 初始化参数
* @return DEVICE_OK 成功，其他值失败
*/
int8_t RC_CAN_Init(RC_CAN_t *rc_can, RC_CAN_Param_t *param) {
if (rc_can == NULL || param == NULL) {
return DEVICE_ERR_NULL;
}
// 参数验证
if (RC_CAN_ValidateParams(param) != DEVICE_OK) {
return DEVICE_ERR;
}
rc_can->param = *param;
// 初始化header
rc_can->header.online = false;
rc_can->header.last_online_time = 0;
// 手动初始化数据结构
rc_can->data.joy.ch_l_x = 0.0f;
rc_can->data.joy.ch_l_y = 0.0f;
rc_can->data.joy.ch_r_x = 0.0f;
rc_can->data.joy.ch_r_y = 0.0f;
rc_can->data.sw.sw_l = RC_CAN_SW_ERR;
rc_can->data.sw.sw_r = RC_CAN_SW_ERR;
rc_can->data.sw.ch_res = 0.0f;
rc_can->data.mouse.x = 0.0f;
rc_can->data.mouse.y = 0.0f;
rc_can->data.mouse.z = 0.0f;
rc_can->data.mouse.mouse_l = false;
rc_can->data.mouse.mouse_r = false;
rc_can->data.keyboard.key_value = 0;
for (int i = 0; i < 6; i++) {
rc_can->data.keyboard.keys[i] = RC_CAN_KEY_NONE;
}
// 注册CAN ID队列（从机模式需要接收数据）
if (rc_can->param.mode == RC_CAN_MODE_SLAVE) {
return RC_CAN_RegisterIds(rc_can);
}
return DEVICE_OK;
}
/**
* @brief 更新并发送数据到CAN总线
* @param rc_can RC_CAN结构体指针
* @param data_type 数据类型
* @return DEVICE_OK 成功，其他值失败
*/
int8_t RC_CAN_SendData(RC_CAN_t *rc_can, RC_CAN_DataType_t data_type) {
if (rc_can == NULL) {
return DEVICE_ERR_NULL;
}
if (rc_can->param.mode != RC_CAN_MODE_MASTER) {
return DEVICE_ERR;
}
BSP_CAN_StdDataFrame_t frame;
frame.dlc = 8;
// 边界裁剪宏
#define RC_CAN_CLAMP(x, min, max) ((x) < (min) ? (min) : ((x) > (max) ? (max) : (x)))
switch (data_type) {
case RC_CAN_DATA_JOYSTICK: {
frame.id = rc_can->param.joy_id;
float l_x = RC_CAN_CLAMP(rc_can->data.joy.ch_l_x, -0.999969f, 0.999969f);
float l_y = RC_CAN_CLAMP(rc_can->data.joy.ch_l_y, -0.999969f, 0.999969f);
float r_x = RC_CAN_CLAMP(rc_can->data.joy.ch_r_x, -0.999969f, 0.999969f);
float r_y = RC_CAN_CLAMP(rc_can->data.joy.ch_r_y, -0.999969f, 0.999969f);
int16_t l_x_i = (int16_t)(l_x * 32768.0f);
int16_t l_y_i = (int16_t)(l_y * 32768.0f);
int16_t r_x_i = (int16_t)(r_x * 32768.0f);
int16_t r_y_i = (int16_t)(r_y * 32768.0f);
frame.data[0] = (uint8_t)(l_x_i & 0xFF);
frame.data[1] = (uint8_t)((l_x_i >> 8) & 0xFF);
frame.data[2] = (uint8_t)(l_y_i & 0xFF);
frame.data[3] = (uint8_t)((l_y_i >> 8) & 0xFF);
frame.data[4] = (uint8_t)(r_x_i & 0xFF);
frame.data[5] = (uint8_t)((r_x_i >> 8) & 0xFF);
frame.data[6] = (uint8_t)(r_y_i & 0xFF);
frame.data[7] = (uint8_t)((r_y_i >> 8) & 0xFF);
break;
}
case RC_CAN_DATA_SWITCH: {
frame.id = rc_can->param.sw_id;
frame.data[0] = (uint8_t)(rc_can->data.sw.sw_l);
frame.data[1] = (uint8_t)(rc_can->data.sw.sw_r);
float ch_res = RC_CAN_CLAMP(rc_can->data.sw.ch_res, -0.999969f, 0.999969f);
int16_t ch_res_i = (int16_t)(ch_res * 32768.0f);
frame.data[2] = (uint8_t)(ch_res_i & 0xFF);
frame.data[3] = (uint8_t)((ch_res_i >> 8) & 0xFF);
frame.data[4] = 0; // 保留字节
frame.data[5] = 0; // 保留字节
frame.data[6] = 0; // 保留字节
frame.data[7] = 0; // 保留字节
break;
}
case RC_CAN_DATA_MOUSE: {
frame.id = rc_can->param.mouse_id;
// 鼠标x/y/z一般为增量，若有极限也可加clamp
int16_t x = (int16_t)(rc_can->data.mouse.x);
int16_t y = (int16_t)(rc_can->data.mouse.y);
int16_t z = (int16_t)(rc_can->data.mouse.z);
frame.data[0] = (uint8_t)(x & 0xFF);
frame.data[1] = (uint8_t)((x >> 8) & 0xFF);
frame.data[2] = (uint8_t)(y & 0xFF);
frame.data[3] = (uint8_t)((y >> 8) & 0xFF);
frame.data[4] = (uint8_t)(z & 0xFF);
frame.data[5] = (uint8_t)((z >> 8) & 0xFF);
frame.data[6] = (uint8_t)(rc_can->data.mouse.mouse_l ? 1 : 0);
frame.data[7] = (uint8_t)(rc_can->data.mouse.mouse_r ? 1 : 0);
break;
}
case RC_CAN_DATA_KEYBOARD: {
frame.id = rc_can->param.keyboard_id;
frame.data[0] = (uint8_t)(rc_can->data.keyboard.key_value & 0xFF);
frame.data[1] = (uint8_t)((rc_can->data.keyboard.key_value >> 8) & 0xFF);
for (int i = 0; i < 6; i++) {
frame.data[2 + i] = (i < 6) ? (uint8_t)(rc_can->data.keyboard.keys[i]) : 0;
}
break;
}
default:
return DEVICE_ERR;
}
#undef RC_CAN_CLAMP
if (BSP_CAN_Transmit(rc_can->param.can, BSP_CAN_FORMAT_STD_DATA, frame.id,
frame.data, frame.dlc) != BSP_OK) {
return DEVICE_ERR;
}
return DEVICE_OK;
}
/**
* @brief 接收并更新CAN数据
* @param rc_can RC_CAN结构体指针
* @param data_type 数据类型
* @return DEVICE_OK 成功，其他值失败
*/
int8_t RC_CAN_Update(RC_CAN_t *rc_can, RC_CAN_DataType_t data_type) {
if (rc_can == NULL) {
return DEVICE_ERR_NULL;
}
// 只有从机模式才能接收数据
if (rc_can->param.mode != RC_CAN_MODE_SLAVE) {
return DEVICE_ERR;
}
BSP_CAN_Message_t msg;
switch (data_type) {
case RC_CAN_DATA_JOYSTICK:
if (BSP_CAN_GetMessage(rc_can->param.can, rc_can->param.joy_id, &msg,
BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
return DEVICE_ERR;
}
// 解包数据
int16_t ch_l_x = (int16_t)((msg.data[1] << 8) | msg.data[0]);
int16_t ch_l_y = (int16_t)((msg.data[3] << 8) | msg.data[2]);
int16_t ch_r_x = (int16_t)((msg.data[5] << 8) | msg.data[4]);
int16_t ch_r_y = (int16_t)((msg.data[7] << 8) | msg.data[6]);
// 转换为浮点数（范围：-1.0到1.0）
rc_can->data.joy.ch_l_x = (float)ch_l_x / 32768.0f;
rc_can->data.joy.ch_l_y = (float)ch_l_y / 32768.0f;
rc_can->data.joy.ch_r_x = (float)ch_r_x / 32768.0f;
rc_can->data.joy.ch_r_y = (float)ch_r_y / 32768.0f;
break;
case RC_CAN_DATA_SWITCH:
if (BSP_CAN_GetMessage(rc_can->param.can, rc_can->param.sw_id, &msg,
BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
return DEVICE_ERR;
}
// 解包数据
rc_can->data.sw.sw_l = (RC_CAN_SW_t)msg.data[0];
rc_can->data.sw.sw_r = (RC_CAN_SW_t)msg.data[1];
int16_t ch_res = (int16_t)((msg.data[3] << 8) | msg.data[2]);
rc_can->data.sw.ch_res = (float)ch_res / 32768.0f;
break;
case RC_CAN_DATA_MOUSE:
if (BSP_CAN_GetMessage(rc_can->param.can, rc_can->param.mouse_id, &msg,
BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
return DEVICE_ERR;
}
// 解包数据
int16_t x = (int16_t)((msg.data[1] << 8) | msg.data[0]);
int16_t y = (int16_t)((msg.data[3] << 8) | msg.data[2]);
int16_t z = (int16_t)((msg.data[5] << 8) | msg.data[4]);
rc_can->data.mouse.x = (float)x;
rc_can->data.mouse.y = (float)y;
rc_can->data.mouse.z = (float)z;
rc_can->data.mouse.mouse_l = (msg.data[6] & 0x01) ? true : false;
rc_can->data.mouse.mouse_r = (msg.data[7] & 0x01) ? true : false;
break;
case RC_CAN_DATA_KEYBOARD:
if (BSP_CAN_GetMessage(rc_can->param.can, rc_can->param.keyboard_id, &msg,
BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
return DEVICE_ERR;
}
if (msg.dlc < 2) {
return DEVICE_ERR;
}
// 解包数据
rc_can->data.keyboard.key_value =
(uint16_t)((msg.data[1] << 8) | msg.data[0]);
for (int i = 0; i < 6 && (i + 2) < msg.dlc; i++) {
rc_can->data.keyboard.keys[i] = (RC_CAN_Key_t)(msg.data[2 + i]);
}
// 清空未使用的按键位置
for (int i = (msg.dlc > 2 ? msg.dlc - 2 : 0); i < 6; i++) {
rc_can->data.keyboard.keys[i] = RC_CAN_KEY_NONE;
}
break;
default:
return DEVICE_ERR;
}
// 更新header状态
rc_can->header.online = true;
rc_can->header.last_online_time = BSP_TIME_Get_us();
return DEVICE_OK;
}
/* Private functions -------------------------------------------------------- */
/**
* @brief 验证RC_CAN参数
* @param param 参数指针
* @return DEVICE_OK 成功，其他值失败
*/
static int8_t RC_CAN_ValidateParams(const RC_CAN_Param_t *param) {
if (param == NULL) {
return DEVICE_ERR_NULL;
}
// 检查CAN总线有效性
if (param->can >= BSP_CAN_NUM) {
return DEVICE_ERR;
}
// 检查工作模式有效性
if (param->mode != RC_CAN_MODE_MASTER && param->mode != RC_CAN_MODE_SLAVE) {
return DEVICE_ERR;
}
// 检查CAN ID是否重复
if (param->joy_id == param->sw_id || param->joy_id == param->mouse_id ||
param->joy_id == param->keyboard_id || param->sw_id == param->mouse_id ||
param->sw_id == param->keyboard_id ||
param->mouse_id == param->keyboard_id) {
return DEVICE_ERR;
}
return DEVICE_OK;
}
/**
* @brief 注册CAN ID
* @param rc_can RC_CAN结构体指针
* @return DEVICE_OK 成功，其他值失败
*/
static int8_t RC_CAN_RegisterIds(RC_CAN_t *rc_can) {
if (BSP_CAN_RegisterId(rc_can->param.can, rc_can->param.joy_id, 0) !=
BSP_OK) {
return DEVICE_ERR;
}
if (BSP_CAN_RegisterId(rc_can->param.can, rc_can->param.sw_id, 0) != BSP_OK) {
return DEVICE_ERR;
}
if (BSP_CAN_RegisterId(rc_can->param.can, rc_can->param.mouse_id, 0) !=
BSP_OK) {
return DEVICE_ERR;
}
if (BSP_CAN_RegisterId(rc_can->param.can, rc_can->param.keyboard_id, 0) !=
BSP_OK) {
return DEVICE_ERR;
}
return DEVICE_OK;
}
int8_t RC_CAN_OFFLINE(RC_CAN_t *rc_can){
if (rc_can == NULL) {
return DEVICE_ERR_NULL;
}
rc_can->header.online = false;
return DEVICE_OK;
}
/* USER CODE BEGIN */
/* USER CODE END */

157
assets/User_code/device/rc_can.h Normal file
View File

@ -0,0 +1,157 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "bsp/can.h"
#include "device/device.h"
#include <stdint.h>
#include <stdbool.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef enum {
RC_CAN_SW_ERR = 0,
RC_CAN_SW_UP = 1,
RC_CAN_SW_MID = 3,
RC_CAN_SW_DOWN = 2,
} RC_CAN_SW_t;
typedef enum {
RC_CAN_MODE_MASTER = 0, // 主机模式
RC_CAN_MODE_SLAVE = 1, // 从机模式
} RC_CAN_Mode_t;
typedef enum {
RC_CAN_DATA_JOYSTICK = 0,
RC_CAN_DATA_SWITCH,
RC_CAN_DATA_MOUSE,
RC_CAN_DATA_KEYBOARD
} RC_CAN_DataType_t;
typedef enum {
RC_CAN_KEY_NONE = 0xFF, // 无按键
RC_CAN_KEY_W = 0,
RC_CAN_KEY_S,
RC_CAN_KEY_A,
RC_CAN_KEY_D,
RC_CAN_KEY_SHIFT,
RC_CAN_KEY_CTRL,
RC_CAN_KEY_Q,
RC_CAN_KEY_E,
RC_CAN_KEY_R,
RC_CAN_KEY_F,
RC_CAN_KEY_G,
RC_CAN_KEY_Z,
RC_CAN_KEY_X,
RC_CAN_KEY_C,
RC_CAN_KEY_V,
RC_CAN_KEY_B,
RC_CAN_KEY_NUM,
} RC_CAN_Key_t;
// 遥杆数据包
typedef struct {
float ch_l_x;
float ch_l_y;
float ch_r_x;
float ch_r_y;
} RC_CAN_JoyData_t;
// 拨杆数据包
typedef struct {
RC_CAN_SW_t sw_l; // 左拨杆状态
RC_CAN_SW_t sw_r; // 右拨杆状态
float ch_res; // 第五通道
} RC_CAN_SwitchData_t;
// 鼠标数据包
typedef struct {
float x; // 鼠标X轴移动
float y; // 鼠标Y轴移动
float z; // 鼠标Z轴(滚轮)
bool mouse_l; // 鼠标左键
bool mouse_r; // 鼠标右键
} RC_CAN_MouseData_t;
// 键盘数据包
typedef struct {
uint16_t key_value; // 键盘按键位映射
RC_CAN_Key_t keys[16];
} RC_CAN_KeyboardData_t;
typedef struct {
RC_CAN_JoyData_t joy;
RC_CAN_SwitchData_t sw;
RC_CAN_MouseData_t mouse;
RC_CAN_KeyboardData_t keyboard;
} RC_CAN_Data_t;
// RC_CAN 参数结构
typedef struct {
BSP_CAN_t can; // 使用的CAN总线
RC_CAN_Mode_t mode; // 工作模式
uint16_t joy_id; // 遥杆CAN ID
uint16_t sw_id; // 拨杆CAN ID
uint16_t mouse_id; // 鼠标CAN ID
uint16_t keyboard_id; // 键盘CAN ID
} RC_CAN_Param_t;
// RC_CAN 主结构
typedef struct {
DEVICE_Header_t header;
RC_CAN_Param_t param;
RC_CAN_Data_t data;
} RC_CAN_t;
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 初始化RC CAN发送模块
* @param rc_can RC_CAN结构体指针
* @param param 初始化参数
* @return DEVICE_OK 成功，其他值失败
*/
int8_t RC_CAN_Init(RC_CAN_t *rc_can, RC_CAN_Param_t *param);
/**
* @brief 更新并发送数据到CAN总线
* @param rc_can RC_CAN结构体指针
* @param data_type 数据类型
* @return DEVICE_OK 成功，其他值失败
*/
int8_t RC_CAN_SendData(RC_CAN_t *rc_can, RC_CAN_DataType_t data_type);
/**
* @brief 接收并更新CAN数据
* @param rc_can RC_CAN结构体指针
* @param data_type 数据类型
* @return DEVICE_OK 成功，其他值失败
*/
int8_t RC_CAN_Update(RC_CAN_t *rc_can , RC_CAN_DataType_t data_type);
int8_t RC_CAN_OFFLINE(RC_CAN_t *rc_can);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif