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

MDK-ARM/DevC.uvprojx

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

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

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

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

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++) {
+            // for (int i = 0; i < 4; i++) {
-                MOTOR_LZ_Relax(&c->param->joint_motors[i]);
+            //     MOTOR_LZ_Relax(&c->param->joint_motors[i]);
-            }
+            // }
-            for (int i = 0; i < 2; i++) {
+            // for (int i = 0; i < 2; i++) {
-                MOTOR_LK_Relax(&c->param->wheel_motors[i]); // 改为Relax以保持反馈
+            //     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:

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