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

改can

Browse Source
This commit is contained in:
Robofish 2025-10-03 22:12:28 +08:00
parent ad9898bdc4
commit 8ee3b8a298
12 changed files with 744 additions and 725 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1026
Core/Src/stm32f4xx_it.c
View File

File diff suppressed because it is too large Load Diff

291
User/bsp/can.c
View File

@ -37,16 +37,21 @@ static osMutexId_t queue_mutex = NULL;
static void (*CAN_Callback[BSP_CAN_NUM][BSP_CAN_CB_NUM])(void);
static bool inited = false;
static BSP_CAN_IdParser_t id_parser = NULL; /* ID解析器 */
static BSP_CAN_TxQueue_t tx_queues[BSP_CAN_NUM]; /* 每个CAN的发送队列 */
/* Private function prototypes ---------------------------------------------- */
static BSP_CAN_t CAN_Get(CAN_HandleTypeDef *hcan);
static osMessageQueueId_t BSP_CAN_FindQueue(BSP_CAN_t can, uint32_t can_id);
static int8_t BSP_CAN_CreateIdQueue(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size);
static int8_t BSP_CAN_DeleteIdQueue(BSP_CAN_t can, uint32_t can_id);
static void BSP_CAN_RxFifo0Callback(void);
static void BSP_CAN_RxFifo1Callback(void);
static void BSP_CAN_TxCompleteCallback(void);
static BSP_CAN_FrameType_t BSP_CAN_GetFrameType(CAN_RxHeaderTypeDef *header);
static uint32_t BSP_CAN_DefaultIdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type);
static void BSP_CAN_TxQueueInit(BSP_CAN_t can);
static bool BSP_CAN_TxQueuePush(BSP_CAN_t can, BSP_CAN_TxMessage_t *msg);
static bool BSP_CAN_TxQueuePop(BSP_CAN_t can, BSP_CAN_TxMessage_t *msg);
static bool BSP_CAN_TxQueueIsEmpty(BSP_CAN_t can);
/* Private functions -------------------------------------------------------- */
/* USER FUNCTION BEGIN */
@ -121,29 +126,7 @@ static int8_t BSP_CAN_CreateIdQueue(BSP_CAN_t can, uint32_t can_id, uint8_t queu
return BSP_OK;
}
/**
* @brief 删除指定CAN ID的消息队列
* @note 内部函数，已包含互斥锁保护
*/
static int8_t BSP_CAN_DeleteIdQueue(BSP_CAN_t can, uint32_t can_id) {
if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) != osOK) {
return BSP_ERR_TIMEOUT;
}
BSP_CAN_QueueNode_t **current = &queue_list;
while (*current != NULL) {
if ((*current)->can == can && (*current)->can_id == can_id) {
BSP_CAN_QueueNode_t *to_delete = *current;
*current = (*current)->next;
osMessageQueueDelete(to_delete->queue);
BSP_Free(to_delete);
osMutexRelease(queue_mutex);
return BSP_OK;
}
current = &(*current)->next;
}
osMutexRelease(queue_mutex);
return BSP_ERR; // 未找到
}
/**
* @brief 获取帧类型
*/
@ -163,6 +146,106 @@ static uint32_t BSP_CAN_DefaultIdParser(uint32_t original_id, BSP_CAN_FrameType_
return original_id;
}
/**
* @brief 初始化发送队列
*/
static void BSP_CAN_TxQueueInit(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return;
tx_queues[can].head = 0;
tx_queues[can].tail = 0;
}
/**
* @brief 向发送队列添加消息（无锁）
*/
static bool BSP_CAN_TxQueuePush(BSP_CAN_t can, BSP_CAN_TxMessage_t *msg) {
if (can >= BSP_CAN_NUM || msg == NULL) return false;
BSP_CAN_TxQueue_t *queue = &tx_queues[can];
uint32_t next_head = (queue->head + 1) % BSP_CAN_TX_QUEUE_SIZE;
// 队列满
if (next_head == queue->tail) {
return false;
}
// 复制消息
queue->buffer[queue->head] = *msg;
// 更新头指针（原子操作）
queue->head = next_head;
return true;
}
/**
* @brief 从发送队列取出消息（无锁）
*/
static bool BSP_CAN_TxQueuePop(BSP_CAN_t can, BSP_CAN_TxMessage_t *msg) {
if (can >= BSP_CAN_NUM || msg == NULL) return false;
BSP_CAN_TxQueue_t *queue = &tx_queues[can];
// 队列空
if (queue->head == queue->tail) {
return false;
}
// 复制消息
*msg = queue->buffer[queue->tail];
// 更新尾指针（原子操作）
queue->tail = (queue->tail + 1) % BSP_CAN_TX_QUEUE_SIZE;
return true;
}
/**
* @brief 检查发送队列是否为空
*/
static bool BSP_CAN_TxQueueIsEmpty(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return true;
return tx_queues[can].head == tx_queues[can].tail;
}
/**
* @brief 处理所有CAN实例的发送队列
*/
static void BSP_CAN_TxCompleteCallback(void) {
// 处理所有CAN实例的发送队列
for (int i = 0; i < BSP_CAN_NUM; i++) {
BSP_CAN_t can = (BSP_CAN_t)i;
CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle(can);
if (hcan == NULL) continue;
BSP_CAN_TxMessage_t msg;
uint32_t mailbox;
// 尝试发送队列中的消息
while (!BSP_CAN_TxQueueIsEmpty(can)) {
// 检查是否有空闲邮箱
if (HAL_CAN_GetTxMailboxesFreeLevel(hcan) == 0) {
break; // 没有空闲邮箱，等待下次中断
}
// 从队列中取出消息
if (!BSP_CAN_TxQueuePop(can, &msg)) {
break;
}
// 发送消息
if (HAL_CAN_AddTxMessage(hcan, &msg.header, msg.data, &mailbox) != HAL_OK) {
// 发送失败，消息已经从队列中移除，直接丢弃
break;
}
}
}
}
/**
* @brief FIFO0接收处理函数
*/
@ -347,7 +430,12 @@ int8_t BSP_CAN_Init(void) {
// 清零回调函数数组
memset(CAN_Callback, 0, sizeof(CAN_Callback));
// 初始化发送队列
for (int i = 0; i < BSP_CAN_NUM; i++) {
BSP_CAN_TxQueueInit((BSP_CAN_t)i);
}
// 初始化ID解析器为默认解析器
id_parser = BSP_CAN_DefaultIdParser;
@ -377,6 +465,9 @@ int8_t BSP_CAN_Init(void) {
// 自动注册CAN1接收回调函数
BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_RX_FIFO0_MSG_PENDING_CB, BSP_CAN_RxFifo0Callback);
BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_TX_MAILBOX0_CPLT_CB, BSP_CAN_TxCompleteCallback);
BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_TX_MAILBOX1_CPLT_CB, BSP_CAN_TxCompleteCallback);
BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_TX_MAILBOX2_CPLT_CB, BSP_CAN_TxCompleteCallback);
// 激活CAN1中断
HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING |
@ -390,48 +481,19 @@ int8_t BSP_CAN_Init(void) {
// 自动注册CAN2接收回调函数
BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_RX_FIFO1_MSG_PENDING_CB, BSP_CAN_RxFifo1Callback);
BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_TX_MAILBOX0_CPLT_CB, BSP_CAN_TxCompleteCallback);
BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_TX_MAILBOX1_CPLT_CB, BSP_CAN_TxCompleteCallback);
BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_TX_MAILBOX2_CPLT_CB, BSP_CAN_TxCompleteCallback);
// 激活CAN2中断
HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING);
HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING |
CAN_IT_TX_MAILBOX_EMPTY); // 激活发送邮箱空中断
inited = true;
return BSP_OK;
}
int8_t BSP_CAN_DeInit(void) {
if (!inited) {
return BSP_ERR;
}
// 删除所有队列
if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) == osOK) {
BSP_CAN_QueueNode_t *current = queue_list;
while (current != NULL) {
BSP_CAN_QueueNode_t *next = current->next;
osMessageQueueDelete(current->queue);
BSP_Free(current);
current = next;
}
queue_list = NULL;
osMutexRelease(queue_mutex);
}
// 删除互斥锁
if (queue_mutex != NULL) {
osMutexDelete(queue_mutex);
queue_mutex = NULL;
}
// 清零回调函数数组
memset(CAN_Callback, 0, sizeof(CAN_Callback));
// 重置ID解析器
id_parser = NULL;
inited = false;
return BSP_OK;
}
CAN_HandleTypeDef *BSP_CAN_GetHandle(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) {
@ -487,44 +549,58 @@ int8_t BSP_CAN_Transmit(BSP_CAN_t can, BSP_CAN_Format_t format,
return BSP_ERR_NULL;
}
CAN_TxHeaderTypeDef header = {0};
uint32_t mailbox;
// 准备发送消息
BSP_CAN_TxMessage_t tx_msg = {0};
switch (format) {
case BSP_CAN_FORMAT_STD_DATA:
header.StdId = id;
header.IDE = CAN_ID_STD;
header.RTR = CAN_RTR_DATA;
tx_msg.header.StdId = id;
tx_msg.header.IDE = CAN_ID_STD;
tx_msg.header.RTR = CAN_RTR_DATA;
break;
case BSP_CAN_FORMAT_EXT_DATA:
header.ExtId = id;
header.IDE = CAN_ID_EXT;
header.RTR = CAN_RTR_DATA;
tx_msg.header.ExtId = id;
tx_msg.header.IDE = CAN_ID_EXT;
tx_msg.header.RTR = CAN_RTR_DATA;
break;
case BSP_CAN_FORMAT_STD_REMOTE:
header.StdId = id;
header.IDE = CAN_ID_STD;
header.RTR = CAN_RTR_REMOTE;
tx_msg.header.StdId = id;
tx_msg.header.IDE = CAN_ID_STD;
tx_msg.header.RTR = CAN_RTR_REMOTE;
break;
case BSP_CAN_FORMAT_EXT_REMOTE:
header.ExtId = id;
header.IDE = CAN_ID_EXT;
header.RTR = CAN_RTR_REMOTE;
tx_msg.header.ExtId = id;
tx_msg.header.IDE = CAN_ID_EXT;
tx_msg.header.RTR = CAN_RTR_REMOTE;
break;
default:
return BSP_ERR;
}
header.DLC = dlc;
header.TransmitGlobalTime = DISABLE;
tx_msg.header.DLC = dlc;
tx_msg.header.TransmitGlobalTime = DISABLE;
HAL_StatusTypeDef result = HAL_CAN_AddTxMessage(hcan, &header, data, &mailbox);
if (result != HAL_OK) {
return BSP_ERR;
// 复制数据
if (data != NULL && dlc > 0) {
memcpy(tx_msg.data, data, dlc);
}
return BSP_OK;
// 尝试直接发送到邮箱
uint32_t mailbox;
if (HAL_CAN_GetTxMailboxesFreeLevel(hcan) > 0) {
HAL_StatusTypeDef result = HAL_CAN_AddTxMessage(hcan, &tx_msg.header, tx_msg.data, &mailbox);
if (result == HAL_OK) {
return BSP_OK; // 发送成功
}
}
// 邮箱满，尝试放入队列
if (BSP_CAN_TxQueuePush(can, &tx_msg)) {
return BSP_OK; // 成功放入队列
}
// 队列也满，丢弃数据
return BSP_ERR; // 数据丢弃
}
int8_t BSP_CAN_TransmitStdDataFrame(BSP_CAN_t can, BSP_CAN_StdDataFrame_t *frame) {
@ -556,12 +632,6 @@ int8_t BSP_CAN_RegisterId(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size) {
return BSP_CAN_CreateIdQueue(can, can_id, queue_size);
}
int8_t BSP_CAN_UnregisterIdQueue(BSP_CAN_t can, uint32_t can_id) {
if (!inited) {
return BSP_ERR_INITED;
}
return BSP_CAN_DeleteIdQueue(can, can_id);
}
int8_t BSP_CAN_GetMessage(BSP_CAN_t can, uint32_t can_id, BSP_CAN_Message_t *msg, uint32_t timeout) {
if (!inited) {
@ -628,15 +698,6 @@ int8_t BSP_CAN_RegisterIdParser(BSP_CAN_IdParser_t parser) {
return BSP_OK;
}
int8_t BSP_CAN_UnregisterIdParser(void) {
if (!inited) {
return BSP_ERR_INITED;
}
id_parser = BSP_CAN_DefaultIdParser;
return BSP_OK;
}
uint32_t BSP_CAN_ParseId(uint32_t original_id, BSP_CAN_FrameType_t frame_type) {
if (id_parser != NULL) {
return id_parser(original_id, frame_type);
@ -644,42 +705,4 @@ uint32_t BSP_CAN_ParseId(uint32_t original_id, BSP_CAN_FrameType_t frame_type) {
return BSP_CAN_DefaultIdParser(original_id, frame_type);
}
int8_t BSP_CAN_WaitTxMailboxEmpty(BSP_CAN_t can, uint32_t timeout) {
if (!inited) {
return BSP_ERR_INITED;
}
if (can >= BSP_CAN_NUM) {
return BSP_ERR;
}
CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle(can);
if (hcan == NULL) {
return BSP_ERR_NULL;
}
uint32_t start_time = HAL_GetTick();
// 如果超时时间为0，立即检查并返回
if (timeout == 0) {
uint32_t free_level = HAL_CAN_GetTxMailboxesFreeLevel(hcan);
return (free_level > 0) ? BSP_OK : BSP_ERR_TIMEOUT;
}
// 等待至少有一个邮箱空闲
while (true) {
uint32_t free_level = HAL_CAN_GetTxMailboxesFreeLevel(hcan);
if (free_level > 0) {
return BSP_OK;
}
// 检查超时
if (timeout != BSP_CAN_TIMEOUT_FOREVER) {
uint32_t elapsed = HAL_GetTick() - start_time;
if (elapsed >= timeout) {
return BSP_ERR_TIMEOUT;
}
}
osDelay(1);
}
}

46
User/bsp/can.h
View File

@ -21,6 +21,7 @@ extern "C" {
#define BSP_CAN_DEFAULT_QUEUE_SIZE 10
#define BSP_CAN_TIMEOUT_IMMEDIATE 0
#define BSP_CAN_TIMEOUT_FOREVER osWaitForever
#define BSP_CAN_TX_QUEUE_SIZE 32 /* 发送队列大小 */
/* USER DEFINE BEGIN */
@ -102,6 +103,19 @@ typedef struct {
/* ID解析回调函数类型 */
typedef uint32_t (*BSP_CAN_IdParser_t)(uint32_t original_id, BSP_CAN_FrameType_t frame_type);
/* CAN发送消息结构体 */
typedef struct {
CAN_TxHeaderTypeDef header; /* 发送头 */
uint8_t data[BSP_CAN_MAX_DLC]; /* 数据 */
} BSP_CAN_TxMessage_t;
/* 无锁环形队列结构体 */
typedef struct {
BSP_CAN_TxMessage_t buffer[BSP_CAN_TX_QUEUE_SIZE]; /* 缓冲区 */
volatile uint32_t head; /* 队列头 */
volatile uint32_t tail; /* 队列尾 */
} BSP_CAN_TxQueue_t;
/* USER STRUCT BEGIN */
/* USER STRUCT END */
@ -114,12 +128,6 @@ typedef uint32_t (*BSP_CAN_IdParser_t)(uint32_t original_id, BSP_CAN_FrameType_t
*/
int8_t BSP_CAN_Init(void);
/**
* @brief 反初始化 CAN 模块
* @return BSP_OK 成功，其他值失败
*/
int8_t BSP_CAN_DeInit(void);
/**
* @brief 获取 CAN 句柄
* @param can CAN 枚举
@ -173,13 +181,20 @@ int8_t BSP_CAN_TransmitExtDataFrame(BSP_CAN_t can, BSP_CAN_ExtDataFrame_t *frame
*/
int8_t BSP_CAN_TransmitRemoteFrame(BSP_CAN_t can, BSP_CAN_RemoteFrame_t *frame);
/**
* @brief 等待CAN发送邮箱空闲
* @brief 获取发送队列中待发送消息数量
* @param can CAN 枚举
* @return 队列中消息数量，-1表示错误
*/
int32_t BSP_CAN_GetTxQueueCount(BSP_CAN_t can);
/**
* @brief 清空发送队列
* @param can CAN 枚举
* @param timeout 超时时间（毫秒），0为立即返回，osWaitForever为永久等待
* @return BSP_OK 成功，其他值失败
*/
int8_t BSP_CAN_WaitTxMailboxEmpty(BSP_CAN_t can, uint32_t timeout);
int8_t BSP_CAN_FlushTxQueue(BSP_CAN_t can);
/**
* @brief 注册 CAN ID 接收队列
@ -190,13 +205,7 @@ int8_t BSP_CAN_WaitTxMailboxEmpty(BSP_CAN_t can, uint32_t timeout);
*/
int8_t BSP_CAN_RegisterId(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size);
/**
* @brief 注销 CAN ID 接收队列
* @param can CAN 枚举
* @param can_id 解析后的CAN ID
* @return BSP_OK 成功，其他值失败
*/
int8_t BSP_CAN_UnregisterIdQueue(BSP_CAN_t can, uint32_t can_id);
/**
* @brief 获取 CAN 消息
@ -231,11 +240,6 @@ int8_t BSP_CAN_FlushQueue(BSP_CAN_t can, uint32_t can_id);
*/
int8_t BSP_CAN_RegisterIdParser(BSP_CAN_IdParser_t parser);
/**
* @brief 注销ID解析器
* @return BSP_OK 成功，其他值失败
*/
int8_t BSP_CAN_UnregisterIdParser(void);
/**
* @brief 解析CAN ID

4
User/component/user_math.c
View File

@ -51,8 +51,8 @@ inline void ResetMoveVector(MoveVector_t *mv) { memset(mv, 0, sizeof(*mv)); }
* \brief 计算循环值的误差，用于没有负数值，并在一定范围内变化的值
* 例如编码器：相差1.5PI其实等于相差-0.5PI
*
* \param sp 设定值
* \param fb 反馈值
* \param sp 被操作的值
* \param fb 变化量
* \param range 被操作的值变化范围，正数时起效
*
* \return 函数运行结果

3
User/device/device_config.yaml
View File

@ -12,6 +12,9 @@ dr16:
motor:
bsp_config: {}
enabled: true
motor_dm:
bsp_config: {}
enabled: true
motor_lk:
bsp_config: {}
enabled: true

1
User/device/dm_imu.c
View File

@ -166,7 +166,6 @@ int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid) {
.dlc = 4,
};
memcpy(frame.data, tx_data, 4);
BSP_CAN_WaitTxMailboxEmpty(imu->param.can, 1); // 等待发送邮箱空闲
int8_t result = BSP_CAN_TransmitStdDataFrame(imu->param.can, &frame);
return (result == BSP_OK) ? DEVICE_OK : DEVICE_ERR;
}

6
User/device/dr16.c
View File

@ -141,13 +141,13 @@ int8_t DR16_ParseData(DR16_t *dr16){
uint16_t key_value = dr16->raw_data.key;
// 解析键盘位映射（W-B键，位0-15）
for (int i = CMD_KEY_W; i <= CMD_KEY_B; i++) {
for (int i = DR16_KEY_W; i <= DR16_KEY_B; i++) {
dr16->data.keyboard.key[i] = (key_value & (1 << i)) != 0;
}
// 解析鼠标点击
dr16->data.keyboard.key[CMD_L_CLICK] = dr16->data.mouse.l_click;
dr16->data.keyboard.key[CMD_R_CLICK] = dr16->data.mouse.r_click;
dr16->data.keyboard.key[DR16_L_CLICK] = dr16->data.mouse.l_click;
dr16->data.keyboard.key[DR16_R_CLICK] = dr16->data.mouse.r_click;
// 解析第五通道
dr16->data.ch_res = 2.0f * ((float)dr16->raw_data.res - DR16_CH_VALUE_MID) / full_range;

49
User/device/dr16.h
View File

@ -38,33 +38,33 @@ typedef struct __packed {
} DR16_RawData_t;
typedef enum {
CMD_SW_ERR = 0,
CMD_SW_UP = 1,
CMD_SW_MID = 3,
CMD_SW_DOWN = 2,
DR16_SW_ERR = 0,
DR16_SW_UP = 1,
DR16_SW_MID = 3,
DR16_SW_DOWN = 2,
} DR16_SwitchPos_t;
/* 键盘按键值 */
typedef enum {
CMD_KEY_W = 0,
CMD_KEY_S,
CMD_KEY_A,
CMD_KEY_D,
CMD_KEY_SHIFT,
CMD_KEY_CTRL,
CMD_KEY_Q,
CMD_KEY_E,
CMD_KEY_R,
CMD_KEY_F,
CMD_KEY_G,
CMD_KEY_Z,
CMD_KEY_X,
CMD_KEY_C,
CMD_KEY_V,
CMD_KEY_B,
CMD_L_CLICK,
CMD_R_CLICK,
CMD_KEY_NUM,
DR16_KEY_W = 0,
DR16_KEY_S,
DR16_KEY_A,
DR16_KEY_D,
DR16_KEY_SHIFT,
DR16_KEY_CTRL,
DR16_KEY_Q,
DR16_KEY_E,
DR16_KEY_R,
DR16_KEY_F,
DR16_KEY_G,
DR16_KEY_Z,
DR16_KEY_X,
DR16_KEY_C,
DR16_KEY_V,
DR16_KEY_B,
DR16_L_CLICK,
DR16_R_CLICK,
DR16_KEY_NUM,
} DR16_Key_t;
typedef struct {
@ -87,14 +87,13 @@ typedef struct {
} mouse; /* 鼠标值 */
union {
bool key[CMD_KEY_NUM]; /* 键盘按键值 */
bool key[DR16_KEY_NUM]; /* 键盘按键值 */
uint16_t value; /* 键盘按键值的位映射 */
} keyboard;
uint16_t res; /* 保留，未启用 */
} DR16_Data_t;
typedef struct {
DEVICE_Header_t header;
DR16_RawData_t raw_data;

6
User/device/motor_dm.c
View File

@ -133,7 +133,7 @@ static int8_t MOTOR_DM_SendMITCmd(MOTOR_DM_t *motor, MOTOR_MIT_Output_t *output)
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;
}
@ -163,7 +163,6 @@ static int8_t MOTOR_DM_SendPosVelCmd(MOTOR_DM_t *motor, float pos, float vel) {
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;
}
@ -190,7 +189,6 @@ static int8_t MOTOR_DM_SendVelCmd(MOTOR_DM_t *motor, float vel) {
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;
}
@ -460,7 +458,7 @@ int8_t MOTOR_DM_Enable(MOTOR_DM_Param_t *param){
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;
}

3
User/device/motor_lk.c
View File

@ -253,7 +253,6 @@ int8_t MOTOR_LK_SetOutput(MOTOR_LK_Param_t *param, float value) {
tx_frame.data[5] = (uint8_t)((torque_control >> 8) & 0xFF);
tx_frame.data[6] = 0x00;
tx_frame.data[7] = 0x00;
BSP_CAN_WaitTxMailboxEmpty(param->can, 1); // 等待发送邮箱空闲
return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
@ -279,7 +278,6 @@ int8_t MOTOR_LK_MotorOn(MOTOR_LK_Param_t *param) {
tx_frame.data[5] = 0x00;
tx_frame.data[6] = 0x00;
tx_frame.data[7] = 0x00;
BSP_CAN_WaitTxMailboxEmpty(param->can, 1); // 等待发送邮箱空闲
return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
@ -299,7 +297,6 @@ int8_t MOTOR_LK_MotorOff(MOTOR_LK_Param_t *param) {
tx_frame.data[5] = 0x00;
tx_frame.data[6] = 0x00;
tx_frame.data[7] = 0x00;
BSP_CAN_WaitTxMailboxEmpty(param->can, 1); // 等待发送邮箱空闲
return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}

12
User/device/motor_lz.c
View File

@ -134,7 +134,6 @@ static int8_t MOTOR_LZ_SendExtFrame(BSP_CAN_t can, uint32_t ext_id, uint8_t *dat
} else {
memset(tx_frame.data, 0, dlc);
}
BSP_CAN_WaitTxMailboxEmpty(can, 1); // 等待发送邮箱空闲
return BSP_CAN_TransmitExtDataFrame(can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
@ -244,15 +243,6 @@ int8_t MOTOR_LZ_Init(void) {
return BSP_CAN_RegisterIdParser(MOTOR_LZ_IdParser) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
/**
* @brief 反初始化灵足电机驱动系统
* @return 设备状态码
*/
int8_t MOTOR_LZ_DeInit(void) {
// 注销ID解析器
return BSP_CAN_UnregisterIdParser() == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
int8_t MOTOR_LZ_Register(MOTOR_LZ_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
@ -369,7 +359,6 @@ int8_t MOTOR_LZ_MotionControl(MOTOR_LZ_Param_t *param, MOTOR_LZ_MotionParam_t *m
uint16_t raw_kd = MOTOR_LZ_FloatToRawPositive(send_param.kd, LZ_KD_MAX);
data[6] = (raw_kd >> 8) & 0xFF;
data[7] = raw_kd & 0xFF;
BSP_CAN_WaitTxMailboxEmpty(param->can, 1); // 等待发送邮箱空闲
return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
}
@ -378,7 +367,6 @@ int8_t MOTOR_LZ_Enable(MOTOR_LZ_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
// 构建扩展ID - 使能命令
// 格式: 0x300FF7X, 其中X是motor_id的低4位
uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_ENABLE, param->host_id, param->motor_id);
// 数据区清零

22
User/device/motor_rm.c
View File

@ -139,16 +139,22 @@ static void Motor_RM_Decode(MOTOR_RM_t *motor, BSP_CAN_Message_t *msg) {
motor->feedback.rotor_speed = rotor_speed;
motor->feedback.torque_current = torque_current;
}
if (motor->motor.reverse) {
while (motor->feedback.rotor_abs_angle < 0) {
motor->feedback.rotor_abs_angle += M_2PI;
}
while (motor->feedback.rotor_abs_angle >= M_2PI) {
motor->feedback.rotor_abs_angle -= M_2PI;
}
motor->feedback.rotor_abs_angle = M_2PI - motor->feedback.rotor_abs_angle;
motor->feedback.rotor_speed = -motor->feedback.rotor_speed;
motor->feedback.torque_current = -motor->feedback.torque_current;
}
motor->feedback.temp = msg->data[6];
motor->motor.feedback.rotor_abs_angle = motor->feedback.rotor_abs_angle;
motor->motor.feedback.rotor_speed = motor->feedback.rotor_speed;
motor->motor.feedback.torque_current = motor->feedback.torque_current;
motor->motor.feedback.temp = motor->feedback.temp;
}
/* Exported functions ------------------------------------------------------- */
int8_t MOTOR_RM_Register(MOTOR_RM_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
if (MOTOR_RM_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR;
@ -197,7 +203,7 @@ int8_t MOTOR_RM_Update(MOTOR_RM_Param_t *param) {
motor->motor.header.online = true;
motor->motor.header.last_online_time = BSP_TIME_Get();
Motor_RM_Decode(motor, &rx_msg);
motor->motor.feedback = motor->feedback;
return DEVICE_OK;
}
}
@ -227,6 +233,9 @@ int8_t MOTOR_RM_SetOutput(MOTOR_RM_Param_t *param, float value) {
if (manager == NULL) return DEVICE_ERR_NO_DEV;
if (value > 1.0f) value = 1.0f;
if (value < -1.0f) value = -1.0f;
if (param->reverse){
value = -value;
}
MOTOR_RM_t *motor = MOTOR_RM_GetMotor(param);
if (motor == NULL) return DEVICE_ERR_NO_DEV;
int8_t logical_index = MOTOR_RM_GetLogicalIndex(param->id, param->module);
@ -282,7 +291,6 @@ int8_t MOTOR_RM_Ctrl(MOTOR_RM_Param_t *param) {
default:
return DEVICE_ERR;
}
BSP_CAN_WaitTxMailboxEmpty(param->can, 1); // 等待发送邮箱空闲
return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}