From 38d4139f06eb5ce09a424a3a8b89cf862bd09968 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 31 Aug 2025 23:52:43 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E5=A4=8Dcan=E5=8F=91=E9=80=81?= =?UTF-8?q?=E6=97=A0=E9=99=90=E5=88=B6=E5=AF=BC=E8=87=B4mailbox=E7=88=86?= =?UTF-8?q?=E7=82=B8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- MDK-ARM/DevC.uvprojx | 2 +- User/bsp/can.c | 90 +++++++++++++++++++++++++++++++++-- User/device/motor_lk.c | 2 +- User/device/motor_lz.c | 4 +- User/module/balance_chassis.c | 19 +++++--- User/task/user_task.c | 2 +- 6 files changed, 105 insertions(+), 14 deletions(-) diff --git a/MDK-ARM/DevC.uvprojx b/MDK-ARM/DevC.uvprojx index b914c63..974948a 100644 --- a/MDK-ARM/DevC.uvprojx +++ b/MDK-ARM/DevC.uvprojx @@ -314,7 +314,7 @@ 1 - 4 + 2 0 0 1 diff --git a/User/bsp/can.c b/User/bsp/can.c index f68144b..e3265de 100644 --- a/User/bsp/can.c +++ b/User/bsp/can.c @@ -7,6 +7,8 @@ /* 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 ---------------------------------------------------------- */ @@ -21,6 +23,7 @@ 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解析器 */ @@ -215,6 +218,11 @@ 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](); } @@ -223,6 +231,11 @@ 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](); } @@ -231,6 +244,11 @@ 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](); } @@ -239,6 +257,11 @@ 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](); } @@ -247,6 +270,11 @@ 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](); } @@ -255,6 +283,11 @@ 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](); } @@ -335,6 +368,25 @@ 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; @@ -356,7 +408,8 @@ int8_t BSP_CAN_Init(void) { // 注册CAN1回调函数 BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_RX_FIFO0_MSG_PENDING_CB, BSP_CAN_RxFifo0Callback); - HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING); + HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING | + CAN_IT_TX_MAILBOX_EMPTY); // 激活发送邮箱空中断 // 初始化 CAN2 - 使用 FIFO1 can1_filter.FilterBank = 14; @@ -366,7 +419,8 @@ int8_t BSP_CAN_Init(void) { // 注册CAN2回调函数 BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_RX_FIFO1_MSG_PENDING_CB, BSP_CAN_RxFifo1Callback); - 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; @@ -391,6 +445,14 @@ 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); @@ -456,8 +518,20 @@ 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; } @@ -486,6 +560,8 @@ 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; } @@ -493,7 +569,15 @@ int8_t BSP_CAN_Transmit(BSP_CAN_t can, BSP_CAN_Format_t format, header.TransmitGlobalTime = DISABLE; HAL_StatusTypeDef result = HAL_CAN_AddTxMessage(hcan, &header, data, &mailbox); - return (result == HAL_OK) ? BSP_OK : BSP_ERR; + + if (result != HAL_OK) { + // 如果发送失败，需要释放信号量 + osSemaphoreRelease(tx_semaphore[can]); + return BSP_ERR; + } + + // 发送成功，信号量将在发送完成回调中释放 + return BSP_OK; } int8_t BSP_CAN_TransmitStdDataFrame(BSP_CAN_t can, BSP_CAN_StdDataFrame_t *frame) { diff --git a/User/device/motor_lk.c b/User/device/motor_lk.c index 745513f..d145868 100644 --- a/User/device/motor_lk.c +++ b/User/device/motor_lk.c @@ -305,7 +305,7 @@ MOTOR_LK_t* MOTOR_LK_GetMotor(MOTOR_LK_Param_t *param) { } int8_t MOTOR_LK_Relax(MOTOR_LK_Param_t *param) { - return MOTOR_LK_SetOutput(param, 0.01f); + return MOTOR_LK_SetOutput(param, 0.0f); } int8_t MOTOR_LK_Offine(MOTOR_LK_Param_t *param) { diff --git a/User/device/motor_lz.c b/User/device/motor_lz.c index 0d88dd4..45f7f76 100644 --- a/User/device/motor_lz.c +++ b/User/device/motor_lz.c @@ -27,7 +27,7 @@ #define LZ_RAW_VALUE_MAX (65535) /* 16位原始值最大值 */ #define LZ_TEMP_SCALE (10.0f) /* 温度缩放因子 */ -#define LZ_MAX_RECOVER_DIFF_RAD (0.3f) +#define LZ_MAX_RECOVER_DIFF_RAD (0.4f) #define MOTOR_TX_BUF_SIZE (8) #define MOTOR_RX_BUF_SIZE (8) @@ -36,7 +36,7 @@ MOTOR_LZ_MotionParam_t lz_recover_param = { .target_angle = 0.0f, .target_velocity = 0.0f, - .kp = 5.0f, + .kp = 20.0f, .kd = 1.0f, .torque = 0.0f, }; diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 7d70956..9a95a35 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -99,12 +99,19 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd){ switch (c->mode) { case CHASSIS_MODE_RELAX: // 放松模式，电机不输出 - for (int i = 0; i < 4; i++) { - MOTOR_LZ_Relax(&c->param->joint_motors[i]); - } - for (int i = 0; i < 2; i++) { - MOTOR_LK_Relax(&c->param->wheel_motors[i]); // 改为Relax以保持反馈 - } + // for (int i = 0; i < 4; i++) { + // MOTOR_LZ_Relax(&c->param->joint_motors[i]); + // } + // for (int i = 0; i < 2; i++) { + // MOTOR_LK_Relax(&c->param->wheel_motors[i]); // 改为Relax以保持反馈 + // } + MOTOR_LZ_Relax(&c->param->joint_motors[0]); + MOTOR_LZ_Relax(&c->param->joint_motors[1]); + MOTOR_LZ_Relax(&c->param->joint_motors[2]); + BSP_TIME_Delay_us(100); // 确保有足够时间发送CAN消息 + MOTOR_LZ_Relax(&c->param->joint_motors[3]); + MOTOR_LK_Relax(&c->param->wheel_motors[0]); + MOTOR_LK_Relax(&c->param->wheel_motors[1]); break; case CHASSIS_MODE_RECOVER: diff --git a/User/task/user_task.c b/User/task/user_task.c index 70b4bee..65aa632 100644 --- a/User/task/user_task.c +++ b/User/task/user_task.c @@ -27,5 +27,5 @@ const osThreadAttr_t attr_atti_esti = { const osThreadAttr_t attr_ctrl_chassis = { .name = "ctrl_chassis", .priority = osPriorityNormal, - .stack_size = 512 * 8, + .stack_size = 512 * 4, }; \ No newline at end of file