fix zqueue在中断中调用失败的BUG

zqueue.hpp

@@ -1,6 +1,6 @@
 #pragma once
-#include "cmsis_os.h"
 #include "basic/zlog.h"
+#include "cmsis_os.h"
 namespace iflytop {
 template <typename T>
 class ZQueue {
@@ -30,8 +30,14 @@ class ZQueue {
 
  private:
   bool _send(uint8_t *data, int32_t len, int32_t overtime) {
-    BaseType_t xStatus = xQueueSend(xQueue, data, overtime);
-    return xStatus == pdPASS;
+    if (xPortIsInsideInterrupt()) {
+      BaseType_t xHigherPriorityTaskWoken = pdFALSE;
+      BaseType_t xStatus                  = xQueueSendToFrontFromISR(xQueue, data, &xHigherPriorityTaskWoken);
+      return xStatus == pdPASS;
+    } else {
+      BaseType_t xStatus = xQueueSend(xQueue, data, overtime);
+      return xStatus == pdPASS;
+    }
   }
   bool _receive(uint8_t *data, int32_t overtime) {
     BaseType_t xStatus = xQueueReceive(xQueue, data, overtime);