task

lhr-solar · Apr 1, 2024 · 4097730 · 4097730
1 parent 4689d96
commit 4097730
Show file tree

Hide file tree

Showing 3 changed files with 42 additions and 12 deletions.
diff --git a/Apps/Inc/Print_Queue.h b/Apps/Inc/Print_Queue.h
@@ -27,6 +27,8 @@ void PQ_Init(void);
 bool PQ_Write(char *data, uint32_t len);
 bool PQ_Read(char *data, uint32_t len);
 void PQ_Flush(void);
+void PQ_WaitForFlush(void);
+uint32_t PQ_GetNumWaiting(void);
 
 /**
  * @brief threadsafe wrapper for printf(). DO NOT USE DIRECTLY -- just use printf
@@ -56,3 +58,10 @@ void RTOS_BPS_Blocking_Printf(const char *format, ...);
 #endif  // BPS_ENABLE_PRINT_OUTPUT
 
 #endif
+
+/**
+ * TODO:
+ * Error handling on fifo full
+ * Test everything
+ * Add back in CAN print mirroring
+ */
diff --git a/Apps/Src/Print_Queue.c b/Apps/Src/Print_Queue.c
@@ -19,10 +19,10 @@ static PQFifo_t PQFifo;
 static OS_MEM  PQPrintfPool;
 static char    PQPrintfBuffers[PQ_PRINTF_BUFFER_COUNT][PQ_PRINTF_BUFFER_SIZE];
 
-static BPS_OS_SEM PQ_SignalFlush;
+static BPS_OS_SEM PQ_SignalFlush_Sem4;
 static BPS_OS_MUTEX PQ_Mutex;
 
-void PQ_Init() {
+void PQ_Init(void) {
     // setup fifo
     PQFifo = PQFifo_new();
 
@@ -36,31 +36,45 @@ void PQ_Init() {
                 &err);
     assertOSError(err);
 
-    RTOS_BPS_SemCreate(&PQ_SignalFlush, "PQ_SignalFlush", 0);
+    RTOS_BPS_SemCreate(&PQ_SignalFlush_Sem4, "PQ_SignalFlush", 0);
     RTOS_BPS_MutexCreate(&PQ_Mutex, "PQ_Mutex");
 }
 
 bool PQ_Write(char *data, uint32_t len) {
-    RTOS_BPS_MutexPend(&PQ_Mutex, OS_OPT_PEND_BLOCKING);
+    if (OSIntNestingCtr == 0) { // not in ISR -- ok to pend
+        RTOS_BPS_MutexPend(&PQ_Mutex, OS_OPT_PEND_BLOCKING);
+    }
     bool status = PQFifo_put(&PQFifo, data, (int)len);
-    RTOS_BPS_MutexPost(&PQ_Mutex, OS_OPT_POST_NONE);
+    if (OSIntNestingCtr == 0) {
+        RTOS_BPS_MutexPost(&PQ_Mutex, OS_OPT_POST_NONE);
+    }
     return status;
 }
 
 bool PQ_Read(char *data, uint32_t len) {
-    RTOS_BPS_MutexPend(&PQ_Mutex, OS_OPT_PEND_BLOCKING);
+    if (OSIntNestingCtr == 0) { // not in ISR -- ok to pend
+        RTOS_BPS_MutexPend(&PQ_Mutex, OS_OPT_PEND_BLOCKING);
+    }
     bool status = PQFifo_get(&PQFifo, data, (int)len);
-    RTOS_BPS_MutexPost(&PQ_Mutex, OS_OPT_POST_NONE);
+    if (OSIntNestingCtr == 0) {
+        RTOS_BPS_MutexPost(&PQ_Mutex, OS_OPT_POST_NONE);
+    }
     return status;
 }
 
+uint32_t PQ_GetNumWaiting(void) {
+    return (uint32_t)PQFifo_len(&PQFifo);
+}
+
 void PQ_Flush(void) {
-    RTOS_BPS_SemPost(&PQ_SignalFlush, OS_OPT_POST_ALL);
+    RTOS_BPS_SemPost(&PQ_SignalFlush_Sem4, OS_OPT_POST_ALL | OS_OPT_POST_NO_SCHED);
 }
 
+void PQ_WaitForFlush(void) {
+    RTOS_BPS_SemPend(&PQ_SignalFlush_Sem4, OS_OPT_PEND_BLOCKING);
+}
 
 int _printf_internal(const char *format, ...) {
-    // TODO: make this work from ISR
     BPS_OS_ERR err;
 
     va_list args;

diff --git a/Tasks/Src/Task_Print.c b/Tasks/Src/Task_Print.c
@@ -7,9 +7,16 @@
 void Task_Print(void *p_arg) {
     (void)p_arg;
 
+    uint32_t pending;
+    char c;
 
-    while(1) {
-        // BLOCKING =====================
-
+    while (1) {
+        PQ_WaitForFlush();
+
+        pending = PQ_GetNumWaiting();   // could be outdated, but will always be less than actual
+        for (uint32_t i = 0; i < pending; i++) {
+            PQ_Read(&c, 1);
+            BSP_UART_Write(&c, 1, UART_USB);
+        }
     }
 }