From 4c6686a26a84d367458ccb92151dc9283d929ed8 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sat, 20 Sep 2025 00:39:04 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AEcan?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/bsp/can.c | 114 +++++++++++----------------------- User/bsp/can.h | 13 +++- User/device/motor_lk.c | 2 +- User/device/motor_lz.c | 2 +- User/module/balance_chassis.c | 20 +++--- 5 files changed, 63 insertions(+), 88 deletions(-) diff --git a/User/bsp/can.c b/User/bsp/can.c index e3265de..5db21cb 100644 --- a/User/bsp/can.c +++ b/User/bsp/can.c @@ -7,8 +7,6 @@ /* Private define ----------------------------------------------------------- */ #define CAN_QUEUE_MUTEX_TIMEOUT 100 /* 队列互斥锁超时时间(ms) */ -#define CAN_TX_SEMAPHORE_TIMEOUT 1000 /* 发送信号量超时时间(ms) */ -#define CAN_TX_MAILBOX_NUM 3 /* CAN发送邮箱数量 */ /* Private macro ------------------------------------------------------------ */ /* Private typedef ---------------------------------------------------------- */ @@ -23,7 +21,6 @@ typedef struct BSP_CAN_QueueNode { /* Private variables -------------------------------------------------------- */ static BSP_CAN_QueueNode_t *queue_list = NULL; static osMutexId_t queue_mutex = NULL; -static osSemaphoreId_t tx_semaphore[BSP_CAN_NUM] = {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解析器 */ @@ -218,11 +215,6 @@ static void BSP_CAN_RxFifo1Callback(void) { void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) { BSP_CAN_t bsp_can = CAN_Get(hcan); if (bsp_can != BSP_CAN_ERR) { - // 释放发送信号量 - if (tx_semaphore[bsp_can] != NULL) { - osSemaphoreRelease(tx_semaphore[bsp_can]); - } - if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX0_CPLT_CB]) CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX0_CPLT_CB](); } @@ -231,11 +223,6 @@ void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) { void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef *hcan) { BSP_CAN_t bsp_can = CAN_Get(hcan); if (bsp_can != BSP_CAN_ERR) { - // 释放发送信号量 - if (tx_semaphore[bsp_can] != NULL) { - osSemaphoreRelease(tx_semaphore[bsp_can]); - } - if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX1_CPLT_CB]) CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX1_CPLT_CB](); } @@ -244,11 +231,6 @@ void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef *hcan) { void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan) { BSP_CAN_t bsp_can = CAN_Get(hcan); if (bsp_can != BSP_CAN_ERR) { - // 释放发送信号量 - if (tx_semaphore[bsp_can] != NULL) { - osSemaphoreRelease(tx_semaphore[bsp_can]); - } - if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX2_CPLT_CB]) CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX2_CPLT_CB](); } @@ -257,11 +239,6 @@ void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan) { void HAL_CAN_TxMailbox0AbortCallback(CAN_HandleTypeDef *hcan) { BSP_CAN_t bsp_can = CAN_Get(hcan); if (bsp_can != BSP_CAN_ERR) { - // 释放发送信号量（发送中止也要释放） - if (tx_semaphore[bsp_can] != NULL) { - osSemaphoreRelease(tx_semaphore[bsp_can]); - } - if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX0_ABORT_CB]) CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX0_ABORT_CB](); } @@ -270,11 +247,6 @@ void HAL_CAN_TxMailbox0AbortCallback(CAN_HandleTypeDef *hcan) { void HAL_CAN_TxMailbox1AbortCallback(CAN_HandleTypeDef *hcan) { BSP_CAN_t bsp_can = CAN_Get(hcan); if (bsp_can != BSP_CAN_ERR) { - // 释放发送信号量（发送中止也要释放） - if (tx_semaphore[bsp_can] != NULL) { - osSemaphoreRelease(tx_semaphore[bsp_can]); - } - if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX1_ABORT_CB]) CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX1_ABORT_CB](); } @@ -283,11 +255,6 @@ void HAL_CAN_TxMailbox1AbortCallback(CAN_HandleTypeDef *hcan) { void HAL_CAN_TxMailbox2AbortCallback(CAN_HandleTypeDef *hcan) { BSP_CAN_t bsp_can = CAN_Get(hcan); if (bsp_can != BSP_CAN_ERR) { - // 释放发送信号量（发送中止也要释放） - if (tx_semaphore[bsp_can] != NULL) { - osSemaphoreRelease(tx_semaphore[bsp_can]); - } - if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX2_ABORT_CB]) CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX2_ABORT_CB](); } @@ -368,25 +335,6 @@ int8_t BSP_CAN_Init(void) { return BSP_ERR; } - // 创建发送信号量，每个CAN通道有3个发送邮箱 - for (int i = 0; i < BSP_CAN_NUM; i++) { - tx_semaphore[i] = osSemaphoreNew(CAN_TX_MAILBOX_NUM, CAN_TX_MAILBOX_NUM, NULL); - if (tx_semaphore[i] == NULL) { - // 清理已创建的信号量 - for (int j = 0; j < i; j++) { - if (tx_semaphore[j] != NULL) { - osSemaphoreDelete(tx_semaphore[j]); - tx_semaphore[j] = NULL; - } - } - if (queue_mutex != NULL) { - osMutexDelete(queue_mutex); - queue_mutex = NULL; - } - return BSP_ERR; - } - } - // 先设置初始化标志，以便后续回调注册能通过检查 inited = true; @@ -445,14 +393,6 @@ int8_t BSP_CAN_DeInit(void) { osMutexRelease(queue_mutex); } - // 删除发送信号量 - for (int i = 0; i < BSP_CAN_NUM; i++) { - if (tx_semaphore[i] != NULL) { - osSemaphoreDelete(tx_semaphore[i]); - tx_semaphore[i] = NULL; - } - } - // 删除互斥锁 if (queue_mutex != NULL) { osMutexDelete(queue_mutex); @@ -518,20 +458,8 @@ int8_t BSP_CAN_Transmit(BSP_CAN_t can, BSP_CAN_Format_t format, return BSP_ERR; } - // 获取发送信号量，确保有可用的发送邮箱 - if (tx_semaphore[can] == NULL) { - return BSP_ERR; - } - - osStatus_t sem_status = osSemaphoreAcquire(tx_semaphore[can], CAN_TX_SEMAPHORE_TIMEOUT); - if (sem_status != osOK) { - return BSP_ERR_TIMEOUT; // 获取信号量超时，表示发送邮箱已满 - } - CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle(can); if (hcan == NULL) { - // 如果获取句柄失败，需要释放信号量 - osSemaphoreRelease(tx_semaphore[can]); return BSP_ERR_NULL; } @@ -560,8 +488,6 @@ int8_t BSP_CAN_Transmit(BSP_CAN_t can, BSP_CAN_Format_t format, header.RTR = CAN_RTR_REMOTE; break; default: - // 如果格式错误，需要释放信号量 - osSemaphoreRelease(tx_semaphore[can]); return BSP_ERR; } @@ -571,12 +497,9 @@ int8_t BSP_CAN_Transmit(BSP_CAN_t can, BSP_CAN_Format_t format, HAL_StatusTypeDef result = HAL_CAN_AddTxMessage(hcan, &header, data, &mailbox); if (result != HAL_OK) { - // 如果发送失败，需要释放信号量 - osSemaphoreRelease(tx_semaphore[can]); return BSP_ERR; } - // 发送成功，信号量将在发送完成回调中释放 return BSP_OK; } @@ -697,5 +620,40 @@ 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_WaitForEmptyMailbox(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; + } + + uint32_t start_time = HAL_GetTick(); + + while (1) { + // 检查是否有空闲的发送邮箱 + if (HAL_CAN_GetTxMailboxesFreeLevel(hcan) > 0) { + return BSP_OK; + } + + // 检查超时 + if (timeout != osWaitForever) { + uint32_t elapsed = HAL_GetTick() - start_time; + if (elapsed >= timeout) { + return BSP_ERR_TIMEOUT; + } + } + + // 短暂延时，避免过度占用CPU + osDelay(1); + } +} + /* USER CAN FUNCTIONS BEGIN */ -/* USER CAN FUNCTIONS END */ \ No newline at end of file +/* USER CAN FUNCTIONS END */ diff --git a/User/bsp/can.h b/User/bsp/can.h index d4f14d6..11e486d 100644 --- a/User/bsp/can.h +++ b/User/bsp/can.h @@ -225,9 +225,20 @@ int8_t BSP_CAN_UnregisterIdParser(void); */ uint32_t BSP_CAN_ParseId(uint32_t original_id, BSP_CAN_FrameType_t frame_type); +/** + * @brief 等待CAN发送邮箱为空 + * @param can CAN通道 + * @param timeout 超时时间(ms)，可以使用BSP_CAN_TIMEOUT_FOREVER表示永久等待 + * @retval BSP_OK 成功找到空邮箱 + * @retval BSP_ERR_TIMEOUT 超时 + * @retval BSP_ERR_INITED 未初始化 + * @retval BSP_ERR 其他错误 + */ +int8_t BSP_CAN_WaitForEmptyMailbox(BSP_CAN_t can, uint32_t timeout); + /* USER CAN FUNCTIONS BEGIN */ /* USER CAN FUNCTIONS END */ #ifdef __cplusplus } -#endif \ No newline at end of file +#endif diff --git a/User/device/motor_lk.c b/User/device/motor_lk.c index 2523ca0..b797ec4 100644 --- a/User/device/motor_lk.c +++ b/User/device/motor_lk.c @@ -237,7 +237,7 @@ 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_WaitForEmptyMailbox(param->can, 1); return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; } diff --git a/User/device/motor_lz.c b/User/device/motor_lz.c index 8da3b55..1db9c26 100644 --- a/User/device/motor_lz.c +++ b/User/device/motor_lz.c @@ -376,7 +376,7 @@ 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_WaitForEmptyMailbox(param->can, 1); return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8); } diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 4564596..8c1ef46 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -216,14 +216,20 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd){ switch (c->mode) { case CHASSIS_MODE_RELAX: // 放松模式，电机不输出 - MOTOR_LK_Relax(&c->param->wheel_motors[0]); - MOTOR_LK_Relax(&c->param->wheel_motors[1]); - // MOTOR_LK_Relax(&c->param->wheel_motors[0]); - MOTOR_LZ_Relax(&c->param->joint_motors[0]); - MOTOR_LZ_Relax(&c->param->joint_motors[1]); - MOTOR_LZ_Relax(&c->param->joint_motors[2]); - MOTOR_LZ_Relax(&c->param->joint_motors[3]); + // BSP_CAN_WaitForEmptyMailbox(BSP_CAN_1, 10); + MOTOR_LZ_Relax(&c->param->joint_motors[0]); + // BSP_CAN_WaitForEmptyMailbox(BSP_CAN_1, 10); + MOTOR_LZ_Relax(&c->param->joint_motors[1]); + // BSP_CAN_WaitForEmptyMailbox(BSP_CAN_1, 10); + MOTOR_LZ_Relax(&c->param->joint_motors[2]); + // BSP_CAN_WaitForEmptyMailbox(BSP_CAN_1, 10); + MOTOR_LZ_Relax(&c->param->joint_motors[3]); + BSP_TIME_Delay_us(200); // 等待CAN总线空闲，确保前面的命令发送完成 + // BSP_CAN_WaitForEmptyMailbox(BSP_CAN_1, 10); + MOTOR_LK_Relax(&c->param->wheel_motors[0]); + // BSP_CAN_WaitForEmptyMailbox(BSP_CAN_1, 10); + MOTOR_LK_Relax(&c->param->wheel_motors[1]); // 更新VMC正解算用于状态估计 VMC_ForwardSolve(&c->vmc_[0], c->feedback.joint[0].rotor_abs_angle, c->feedback.joint[1].rotor_abs_angle,