【待测试】使用cpsid i / cpsie i（通过 PRIMASK 保存/恢复）保护临界区

RBcore/drv_interface.c

@@ -278,7 +278,7 @@ void Drv_InterfaceInit(void)
 	const osThreadAttr_t motor_task_attributes = {
 		.name = MODULE_NAME_MOTOR,
 		.stack_size = THREAD_DEFAULT_STACK,
-		.priority = (osPriority_t) osPriorityRealtime1,
+		.priority = (osPriority_t) osPriorityRealtime2,
 	};
 	(void)osThreadNew(MotorTask, NULL, &motor_task_attributes);
 

RBcore/include/msg_center.h

@@ -109,6 +109,16 @@ int MsgCenter_Send(const Msg_t *pstMsg);
  */
 int MsgCenter_SendTo(const char *pszDstName, uint32_t uiMsgID, const void *pData, uint32_t uiDataLen);
 
+/**
+ * @brief 发送消息给指定模块（ISR安全版本，无mutex，关中断+自旋锁保护）
+ * @param pszDstName 目标模块名称
+ * @param uiMsgID 消息ID
+ * @param pData 数据指针（可为NULL）
+ * @param uiDataLen 数据长度
+ * @return 0成功，非0失败
+ */
+int MsgCenter_SendFromISR(const char *pszDstName, uint32_t uiMsgID, const void *pData, uint32_t uiDataLen);
+
 /**
  * @brief 处理当前模块的消息（在模块线程中调用）
  * @param uiModuleID 模块ID

RBcore/msg_center.c

@@ -45,7 +45,11 @@
 typedef struct {
     MsgModule_t     stModule;
     rd_ringbuf_t    *pstRingBuf;    // 消息队列（ringbuffer）
-    rd_mutex_t      stMutex;        // 互斥锁（保护ringbuffer）
+#if defined(USE_FREERTOS)
+    rd_mutex_t      stMutex;        // FreeRTOS互斥锁（任务上下文保护ringbuffer）
+#endif
+    volatile uint8_t bSpinLock;     // 自旋锁（ISR安全，始终可用）
+    uint32_t        uiPrimask;      // 关中断前的PRIMASK快照（裸机用）
     rd_sem_t        stSem;          // 信号量（阻塞等待）
 } MsgModuleEntry_t;
 
@@ -69,6 +73,28 @@ static uint32_t g_uiNextModuleID = 1;  // 从1开始，0表示无效
     ((uiModuleID) > 0 && (uiModuleID) <= g_uiModuleCount ? \
      (uiModuleID) - 1 : -1)
 
+// 写入保护宏：FreeRTOS 用 mutex，裸机用关中断+自旋锁
+#if defined(USE_FREERTOS)
+#define MSG_CENTER_LOCK(pEntry)     Rd_MutexLock(&(pEntry)->stMutex, __func__, NULL)
+#define MSG_CENTER_UNLOCK(pEntry)   Rd_MutexUnlock(&(pEntry)->stMutex, __func__, NULL)
+#define MSG_CENTER_LOCK_INIT(pEntry) ({ \
+    (pEntry)->bSpinLock = 0; \
+    Rd_MutexInit(&(pEntry)->stMutex); \
+})
+#define MSG_CENTER_LOCK_DESTROY(pEntry) Rd_MutexDestroy(&(pEntry)->stMutex)
+#else
+#define MSG_CENTER_LOCK(pEntry)     do { \
+    (pEntry)->uiPrimask = rd_irq_save(); \
+    while (ATOMIC_EXCH(&(pEntry)->bSpinLock, 1, uint8_t, ATOMIC_ORDER_ACQUIRE)) {} \
+    } while(0)
+#define MSG_CENTER_UNLOCK(pEntry)   do { \
+    ATOMIC_STORE(&(pEntry)->bSpinLock, 0, uint8_t, ATOMIC_ORDER_RELEASE); \
+    rd_irq_restore((pEntry)->uiPrimask); \
+    } while(0)
+#define MSG_CENTER_LOCK_INIT(pEntry)    ({ (pEntry)->bSpinLock = 0; RD_SUCCESS; })
+#define MSG_CENTER_LOCK_DESTROY(pEntry) ((void)(pEntry))
+#endif
+
 /*****************************************************************************
  函 数 名  : MsgCenter_Init
  功能描述  : 初始化消息中心
@@ -129,8 +155,8 @@ uint32_t MsgCenter_Register(const char *pszName, MsgHandler_t pfHandler)
         return 0;
     }
 
-    // 初始化互斥锁
+    // 初始化写入锁
-    if (Rd_MutexInit(&g_astModules[uiIndex].stMutex) != RD_SUCCESS)
+    if (MSG_CENTER_LOCK_INIT(&g_astModules[uiIndex]) != RD_SUCCESS)
     {
         rd_RingbufferDestroy(g_astModules[uiIndex].pstRingBuf);
         return 0;
@@ -139,7 +165,7 @@ uint32_t MsgCenter_Register(const char *pszName, MsgHandler_t pfHandler)
     // 初始化信号量（初始值为0，表示无消息）
     if (Rd_SemInit(&g_astModules[uiIndex].stSem, 0, 0) != RD_SUCCESS)
     {
-        Rd_MutexDestroy(&g_astModules[uiIndex].stMutex);
+        MSG_CENTER_LOCK_DESTROY(&g_astModules[uiIndex]);
         rd_RingbufferDestroy(g_astModules[uiIndex].pstRingBuf);
         return 0;
     }
@@ -194,14 +220,13 @@ int MsgCenter_Send(const Msg_t *pstMsg)
         return RD_FAILURE;
     }
 
-    // 加锁保护ringbuffer
+    // 加锁保护ringbuffer（FreeRTOS: mutex, 裸机: 关中断+自旋锁）
-    Rd_MutexLock(&g_astModules[iDstIndex].stMutex, __func__, NULL);
+    MSG_CENTER_LOCK(&g_astModules[iDstIndex]);
 
-    // 写入ringbuffer
     int iRet = rd_RingbufferPut(g_astModules[iDstIndex].pstRingBuf,
                                  (const char *)pstMsg, sizeof(Msg_t));
 
-    Rd_MutexUnlock(&g_astModules[iDstIndex].stMutex, __func__, NULL);
+    MSG_CENTER_UNLOCK(&g_astModules[iDstIndex]);
 
     if (iRet > 0)
     {
@@ -256,6 +281,60 @@ int MsgCenter_SendTo(const char *pszDstName, uint32_t uiMsgID, const void *pData
     return MsgCenter_Send(&stMsg);
 }
 
+int MsgCenter_SendFromISR(const char *pszDstName, uint32_t uiMsgID, const void *pData, uint32_t uiDataLen)
+{
+    if (NULL == pszDstName)
+    {
+        return RD_NULL;
+    }
+
+    uint32_t uiDstModuleID = MsgCenter_FindModule(pszDstName);
+    if (0 == uiDstModuleID)
+    {
+        return RD_FAILURE;
+    }
+
+    int iDstIndex = FIND_MODULE_INDEX(uiDstModuleID);
+    if (iDstIndex < 0 || NULL == g_astModules[iDstIndex].pstRingBuf)
+    {
+        return RD_FAILURE;
+    }
+
+    Msg_t stMsg;
+    RD_MEMSET(&stMsg, 0, sizeof(Msg_t));
+    stMsg.m_uiDstModule = uiDstModuleID;
+    stMsg.m_uiMsgID = uiMsgID;
+
+    if (NULL != pData && uiDataLen > 0)
+    {
+        if (uiDataLen > MSG_CENTER_MAX_DATA_SIZE)
+        {
+            uiDataLen = MSG_CENTER_MAX_DATA_SIZE;
+        }
+        RD_MEMCPY(stMsg.m_aucData, pData, uiDataLen);
+        stMsg.m_uiDataLen = uiDataLen;
+    }
+
+    // ISR安全：始终使用关中断+自旋锁（不依赖FreeRTOS mutex）
+    volatile uint8_t *pLock = &g_astModules[iDstIndex].bSpinLock;
+    uint32_t primask = rd_irq_save();
+    while (ATOMIC_EXCH(pLock, 1, uint8_t, ATOMIC_ORDER_ACQUIRE)) {}
+
+    int iRet = rd_RingbufferPut(g_astModules[iDstIndex].pstRingBuf,
+                                 (const char *)&stMsg, sizeof(Msg_t));
+
+    ATOMIC_STORE(pLock, 0, uint8_t, ATOMIC_ORDER_RELEASE);
+    rd_irq_restore(primask);
+
+    if (iRet > 0)
+    {
+        Rd_SemPost(&g_astModules[iDstIndex].stSem, __func__, NULL);
+        return RD_SUCCESS;
+    }
+
+    return RD_FAILURE;
+}
+
 /*****************************************************************************
  函 数 名  : MsgCenter_ProcessOne
  功能描述  : 从ringbuffer读取一条消息
@@ -267,12 +346,12 @@ int MsgCenter_SendTo(const char *pszDstName, uint32_t uiMsgID, const void *pData
 static int MsgCenter_ProcessOne(int iIndex, Msg_t *pstMsg)
 {
     // 加锁保护ringbuffer
-    Rd_MutexLock(&g_astModules[iIndex].stMutex, __func__, NULL);
+    MSG_CENTER_LOCK(&g_astModules[iIndex]);
 
     int iRet = rd_RingbufferGet(g_astModules[iIndex].pstRingBuf,
                                  (char *)pstMsg, sizeof(Msg_t));
 
-    Rd_MutexUnlock(&g_astModules[iIndex].stMutex, __func__, NULL);
+    MSG_CENTER_UNLOCK(&g_astModules[iIndex]);
 
     return (iRet > 0) ? 1 : 0;
 }

library/common/include/common.h

@@ -41,6 +41,22 @@
     #define ATOMIC_ORDER_RELEASE 0
 #endif
 
+/* 中断控制（CMSIS 标准接口，ISR 安全） */
+#if defined(__arm__) || defined(__aarch64__)
+    #include "../../../Drivers/CMSIS/Include/cmsis_compiler.h"
+    static inline uint32_t rd_irq_save(void) {
+        uint32_t primask = __get_PRIMASK();
+        __disable_irq();
+        return primask;
+    }
+    static inline void rd_irq_restore(uint32_t primask) {
+        __set_PRIMASK(primask);
+    }
+#else
+    static inline uint32_t rd_irq_save(void) { return 0; }
+    static inline void rd_irq_restore(uint32_t primask) { (void)primask; }
+#endif
+
 /* 跨平台弱符号宏定义 */
 #if defined(_MSC_VER) || defined(WIN32)
     /* Microsoft Visual C++ */

library/ringbuffer/ringbuffer.c

@@ -78,7 +78,6 @@ void rd_RingbufferInit(rd_ringbuf_t *rb, char *pool, int size)
 int rd_RingbufferSpaceLen(rd_ringbuf_t *rb)
 {
     if (!rb) return -1;
-    int iRet = 0;
 
     unsigned short wi = ATOMIC_LOAD(&rb->m_sWriteIndex, unsigned short, ATOMIC_ORDER_ACQUIRE);
     unsigned short ri = ATOMIC_LOAD(&rb->m_sReadIndex, unsigned short, ATOMIC_ORDER_RELAXED);
@@ -86,19 +85,9 @@ int rd_RingbufferSpaceLen(rd_ringbuf_t *rb)
     unsigned char rb_m = ATOMIC_LOAD(&rb->m_bReadMirror, unsigned char, ATOMIC_ORDER_RELAXED);
 
     if (wb == rb_m) {
-    	iRet = rb->m_iBufsize - (wi - ri);
+    	return rb->m_iBufsize - (wi - ri);
     } else {
-    	iRet =  ri - wi;
+    	return ri - wi;
-    }
-
-    if (iRet >= 0)
-    {
-    	return iRet;
-    }
-    else
-    {
-    	log_e("wb = %d, rb_m = %d, wi = %d, ri = %d", wb, rb_m, wi, ri);
-    	return 0;
     }
 }
 
@@ -131,27 +120,46 @@ int rd_RingbufferPut(rd_ringbuf_t *rb, const char *ptr, int length)
 {
     if (!rb || !ptr || length <= 0) return RD_INVALUE;
 
-    int space = rd_RingbufferSpaceLen(rb);
+    unsigned short wi, ri;
-    if (space == 0) return 0;
+    unsigned char wb, rb_m;
-    if (space < length) length = space;
+    int space, tail;
+    unsigned int primask;
 
-    unsigned short wi = ATOMIC_LOAD(&rb->m_sWriteIndex, unsigned short, ATOMIC_ORDER_RELAXED);
+    __asm__ volatile ("mrs %0, primask" : "=r"(primask));
-    unsigned char wb = ATOMIC_LOAD(&rb->m_bWriteMirror, unsigned char, ATOMIC_ORDER_RELAXED);
+    __asm__ volatile ("cpsid i" ::: "memory");
+
+    wi = ATOMIC_LOAD(&rb->m_sWriteIndex, unsigned short, ATOMIC_ORDER_RELAXED);
+    ri = ATOMIC_LOAD(&rb->m_sReadIndex, unsigned short, ATOMIC_ORDER_RELAXED);
+    wb = ATOMIC_LOAD(&rb->m_bWriteMirror, unsigned char, ATOMIC_ORDER_RELAXED);
+    rb_m = ATOMIC_LOAD(&rb->m_bReadMirror, unsigned char, ATOMIC_ORDER_RELAXED);
+
+    if (wb == rb_m) {
+        space = rb->m_iBufsize - (int)(unsigned short)(wi - ri);
+    } else {
+        space = (int)(unsigned short)(ri - wi);
+    }
 
-    /* 写入不跨越尾部 */
+    if (space <= 0) {
-    if (rb->m_iBufsize - wi > length) {
+        __asm__ volatile ("msr primask, %0" :: "r"(primask) : "memory");
+        return 0;
+    }
+    if (space < length) length = space;
+
+    tail = rb->m_iBufsize - wi;
+    if (tail > length) {
         RD_MEMCPY(&rb->m_pcBufPtr[wi], ptr, length);
         wi += length;
     } else {
-        int first = rb->m_iBufsize - wi;
+        RD_MEMCPY(&rb->m_pcBufPtr[wi], ptr, tail);
-        RD_MEMCPY(&rb->m_pcBufPtr[wi], ptr, first);
+        RD_MEMCPY(&rb->m_pcBufPtr[0], ptr + tail, length - tail);
-        RD_MEMCPY(&rb->m_pcBufPtr[0], ptr + first, length - first);
+        wb ^= 1;
-        wb ^= 1;                     /* 翻转镜像位 */
+        wi = length - tail;
-        wi = length - first;
     }
 
     ATOMIC_STORE(&rb->m_sWriteIndex, wi, unsigned short, ATOMIC_ORDER_RELEASE);
     ATOMIC_STORE(&rb->m_bWriteMirror, wb, unsigned char, ATOMIC_ORDER_RELEASE);
+
+    __asm__ volatile ("msr primask, %0" :: "r"(primask) : "memory");
     return length;
 }
 
@@ -168,6 +176,10 @@ int rd_RingbufferPutForce(rd_ringbuf_t *rb, const char *ptr, int length)
 {
     if (!rb || !ptr || length <= 0) return RD_INVALUE;
 
+    unsigned int primask;
+    __asm__ volatile ("mrs %0, primask" : "=r"(primask));
+    __asm__ volatile ("cpsid i" ::: "memory");
+
     int space = rd_RingbufferSpaceLen(rb);
     if (length > rb->m_iBufsize) {
         ptr += length - rb->m_iBufsize;
@@ -177,18 +189,18 @@ int rd_RingbufferPutForce(rd_ringbuf_t *rb, const char *ptr, int length)
     unsigned short wi = ATOMIC_LOAD(&rb->m_sWriteIndex, unsigned short, ATOMIC_ORDER_RELAXED);
     unsigned char wb = ATOMIC_LOAD(&rb->m_bWriteMirror, unsigned char, ATOMIC_ORDER_RELAXED);
 
-    if (rb->m_iBufsize - wi > length) {
+    int tail = rb->m_iBufsize - wi;
+    if (tail > length) {
         RD_MEMCPY(&rb->m_pcBufPtr[wi], ptr, length);
         wi += length;
         if (length > space) {
             ATOMIC_STORE(&rb->m_sReadIndex, wi, unsigned short, ATOMIC_ORDER_RELEASE);
         }
     } else {
-        int first = rb->m_iBufsize - wi;
+        RD_MEMCPY(&rb->m_pcBufPtr[wi], ptr, tail);
-        RD_MEMCPY(&rb->m_pcBufPtr[wi], ptr, first);
+        RD_MEMCPY(&rb->m_pcBufPtr[0], ptr + tail, length - tail);
-        RD_MEMCPY(&rb->m_pcBufPtr[0], ptr + first, length - first);
         wb ^= 1;
-        wi = length - first;
+        wi = length - tail;
         if (length > space) {
             unsigned short ri = ATOMIC_LOAD(&rb->m_sReadIndex, unsigned short, ATOMIC_ORDER_RELAXED);
             if (wi <= ri) wb ^= 1;
@@ -198,6 +210,8 @@ int rd_RingbufferPutForce(rd_ringbuf_t *rb, const char *ptr, int length)
 
     ATOMIC_STORE(&rb->m_sWriteIndex, wi, unsigned short, ATOMIC_ORDER_RELEASE);
     ATOMIC_STORE(&rb->m_bWriteMirror, wb, unsigned char, ATOMIC_ORDER_RELEASE);
+
+    __asm__ volatile ("msr primask, %0" :: "r"(primask) : "memory");
     return length;
 }
 
@@ -299,7 +313,15 @@ int rd_RingbufferPeekLinear(rd_ringbuf_t *rb, char *out, int length)
 int rd_RingbufferPutchar(rd_ringbuf_t *rb, const char ch)
 {
     if (!rb) return RD_INVALUE;
-    if (!rd_RingbufferSpaceLen(rb)) return 0;
+
+    unsigned int primask;
+    __asm__ volatile ("mrs %0, primask" : "=r"(primask));
+    __asm__ volatile ("cpsid i" ::: "memory");
+
+    if (rd_RingbufferSpaceLen(rb) == 0) {
+        __asm__ volatile ("msr primask, %0" :: "r"(primask) : "memory");
+        return 0;
+    }
 
     unsigned short wi = ATOMIC_LOAD(&rb->m_sWriteIndex, unsigned short, ATOMIC_ORDER_RELAXED);
     unsigned char wb = ATOMIC_LOAD(&rb->m_bWriteMirror, unsigned char, ATOMIC_ORDER_RELAXED);
@@ -314,6 +336,8 @@ int rd_RingbufferPutchar(rd_ringbuf_t *rb, const char ch)
 
     ATOMIC_STORE(&rb->m_sWriteIndex, wi, unsigned short, ATOMIC_ORDER_RELEASE);
     ATOMIC_STORE(&rb->m_bWriteMirror, wb, unsigned char, ATOMIC_ORDER_RELEASE);
+
+    __asm__ volatile ("msr primask, %0" :: "r"(primask) : "memory");
     return 1;
 }
 
@@ -329,6 +353,10 @@ int rd_RingbufferPutcharForce(rd_ringbuf_t *rb, const char ch)
 {
     if (!rb) return RD_INVALUE;
 
+    unsigned int primask;
+    __asm__ volatile ("mrs %0, primask" : "=r"(primask));
+    __asm__ volatile ("cpsid i" ::: "memory");
+
     enum ERingBufState old_state = rd_RingbufferStatus(rb);
     unsigned short wi = ATOMIC_LOAD(&rb->m_sWriteIndex, unsigned short, ATOMIC_ORDER_RELAXED);
     unsigned char wb = ATOMIC_LOAD(&rb->m_bWriteMirror, unsigned char, ATOMIC_ORDER_RELAXED);
@@ -352,6 +380,8 @@ int rd_RingbufferPutcharForce(rd_ringbuf_t *rb, const char ch)
 
     ATOMIC_STORE(&rb->m_sWriteIndex, wi, unsigned short, ATOMIC_ORDER_RELEASE);
     ATOMIC_STORE(&rb->m_bWriteMirror, wb, unsigned char, ATOMIC_ORDER_RELEASE);
+
+    __asm__ volatile ("msr primask, %0" :: "r"(primask) : "memory");
     return 1;
 }
 

project/paint_robot_new/paint_robot_new.c

@@ -757,7 +757,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
 					g_stIV.Left_Motor_Err = uiFailtCode[1];
 					MsgCenter_SendTo(MODULE_NAME_DAEMON, DAEMON_SET_LEFT_MOTOR, NULL, 0);
 				}
-				else
+				else if (uiFailtCode[0] == 2)
 				{
 					g_stIV.Right_Motor_Err = uiFailtCode[1];
 					MsgCenter_SendTo(MODULE_NAME_DAEMON, DAEMON_SET_RIGHT_MOTOR, NULL, 0);
@@ -866,9 +866,12 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
 					log_a("Motor Error ! error code: left [%d] right [%d]", g_stIV.Left_Motor_Err, g_stIV.Right_Motor_Err);
 				}
 				
-				MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_POWER_DISABLE, NULL, 0);
 				MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0);
+				if (g_stMK32.IsOnline == 0)
+				{
 					g_Is_All_Button_Reset = 0;
+					MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_POWER_DISABLE, NULL, 0);
+				}
 				break;
 			}
 

project/paint_robot_new/paint_robot_new_motors.c

@@ -243,6 +243,7 @@ void MotorTask(void *argument)
     	{
     		static int LS_Motor_Config_Count = 0;
     		static int iStatusQueryCycle = 0;
+    		static int iLastiCount = 0;
 
 			if (LS_Motor_Config_Count <= 200)
 			{
@@ -253,10 +254,17 @@ void MotorTask(void *argument)
 				Rd_Delay(4);
 			}
 			
+			int iCurrentCount = Rd_GetTime();
+			if (iCurrentCount - iLastiCount >= 1000)
+			{
+				log_e("Send motor timeout (%d) !", iCurrentCount - iLastiCount);
+			}
+
 			MotorMgr_SetTargetSpeed(g_apstMotors[0], g_aiMotorSpeed[0]);
 			Rd_Delay(4);
 			MotorMgr_SetTargetSpeed(g_apstMotors[1], g_aiMotorSpeed[1]);
 			Rd_Delay(4);
+			iLastiCount = Rd_GetTime();
 
 			if (++iStatusQueryCycle >= 10)
 			{