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

修复can发送无限制导致mailbox爆炸

Browse Source
This commit is contained in:
Robofish 2025-08-31 23:52:43 +08:00
parent 9574929545
commit 38d4139f06
6 changed files with 105 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
MDK-ARM/DevC.uvprojx
View File

@ -314,7 +314,7 @@
</ArmAdsMisc>
<Cads>
<interw>1</interw>
<Optim>4</Optim>
<Optim>2</Optim>
<oTime>0</oTime>
<SplitLS>0</SplitLS>
<OneElfS>1</OneElfS>

90
User/bsp/can.c
View File

@ -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) {

2
User/device/motor_lk.c
View File

@ -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) {

4
User/device/motor_lz.c
View File

@ -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,
};

19
User/module/balance_chassis.c
View File

@ -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:

2
User/task/user_task.c
View File

@ -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,
};