修can

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 */
+/* USER CAN FUNCTIONS END */

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
+#endif

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

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

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,