diff --git a/assets/User_code/device/motor_dm.c b/assets/User_code/device/motor_dm.c new file mode 100644 index 0000000..b394d59 --- /dev/null +++ b/assets/User_code/device/motor_dm.c @@ -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<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; +} diff --git a/assets/User_code/device/motor_dm.h b/assets/User_code/device/motor_dm.h new file mode 100644 index 0000000..20e91d4 --- /dev/null +++ b/assets/User_code/device/motor_dm.h @@ -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 \ No newline at end of file diff --git a/assets/User_code/device/rc_can.c b/assets/User_code/device/rc_can.c new file mode 100644 index 0000000..77d9733 --- /dev/null +++ b/assets/User_code/device/rc_can.c @@ -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 */ diff --git a/assets/User_code/device/rc_can.h b/assets/User_code/device/rc_can.h new file mode 100644 index 0000000..e5c6d90 --- /dev/null +++ b/assets/User_code/device/rc_can.h @@ -0,0 +1,157 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "bsp/can.h" +#include "device/device.h" +#include +#include + +/* 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