Updated command dispatcher to be a library

src/Makefile

@@ -81,7 +81,9 @@ export OBJS :=                                              	\
 ../src/drivers/photodiode/photodiode_driver.o               	\
 ../src/drivers/rtc/rtc_driver.o                                 \
 ../src/drivers/watchdog/watchdog_driver.o                     	\
-../src/drivers/at86rf215/at86rf215.o								\
+../src/drivers/at86rf215/at86rf215.o							\
+																\
+../src/libraries/cmd_dispatcher/cmd_dispatcher.o 			  	\
                                                             	\
 ../src/tasks/heartbeat/heartbeat_main.o                     	\
                                                             	\
@@ -101,9 +103,6 @@ export OBJS :=                                              	\
 ../src/tasks/task_manager/task_manager_main.o               	\
 ../src/tasks/task_manager/task_manager_task.o               	\
                                                             	\
-../src/tasks/command_dispatcher/command_dispatcher_main.o   	\
-../src/tasks/command_dispatcher/command_dispatcher_task.o   	\
-                                                            	\
 ../src/tasks/shell/shell_main.o                             	\
 ../src/tasks/shell/shell_helpers.o                          	\
 ../src/tasks/shell/shell_commands.o                         	\
@@ -135,6 +134,8 @@ export EXTRA_VPATH := \
 ../../src/drivers/rtc \
 ../../src/drivers/photodiode \
 ../../src/drivers/watchdog \
+../../src/libraries \
+../../src/libraries/cmd_dispatcher \
 ../../src/tasks \
 ../../src/tasks/watchdog \
 ../../src/tasks/heartbeat \

src/globals.h

@@ -44,10 +44,12 @@ typedef enum {
     // Error Responses
     ERROR_READ_FAILED,
     ERROR_WRITE_FAILED,
+    ERROR_BAD_ARGS, 
     ERROR_SPI_TRANSFER_FAILED,
     ERROR_I2C_FAILED,
     ERROR_TASK_DISABLED,
     ERROR_BAD_TARGET,
+    ERROR_QUEUE_ERROR,
     ERROR_SANITY_CHECK_FAILED,
     ERROR_PROCESSING_FAILED,
     ERROR_NOT_READY,

src/libraries/cmd_dispatcher/cmd_dispatcher.c

@@ -0,0 +1,85 @@
+#include "cmd_dispatcher.h"
+#include "globals.h"
+#include "queue.h"
+#include "stdint.h"
+
+#define CMD_LOG_SZ 16 // The number of commands to keep in the log buffer for debugging purposes
+
+
+static cmd_record_t cmd_log_buffer[CMD_LOG_SZ]; 
+static uint8_t log_index = 0; // Index to keep track of where to log the next command in the buffer
+
+/**
+ * \fn log_command
+ * 
+ * \brief Log a command in the command log buffer for telemetry purposes
+ * \param[in] p_cmd a pointer to the command struct to be logged
+ */
+void log_command(const command_t *p_cmd) {
+
+    cmd_log_buffer[log_index].target = p_cmd->target;
+    cmd_log_buffer[log_index].data_type = p_cmd->data_type;
+    cmd_log_buffer[log_index].operation = p_cmd->operation;
+
+    log_index = (log_index + 1) % CMD_LOG_SZ; // Increment index and wrap around if it exceeds buffer size
+}
+
+
+/**
+ * \fn get_n_logs
+ *
+ * \brief Get the last n logged commands from the command log buffer
+ *
+ * \param[in] n the number of recent commands to retrieve
+ * \param[out] logs an array to store the retrieved command logs, must be at least size n
+ * 
+ * \return status_t, whether retrieval was sucessful
+ */
+status_t get_n_logs(uint8_t n, cmd_record_t *logs) {
+
+    if (n > CMD_LOG_SZ) {
+        return ERROR_BAD_ARGS; // Cannot retrieve more logs than the buffer size
+    }
+
+    for (uint8_t i = 0; i < n; i++) {
+        logs[i] = cmd_log_buffer[log_index];
+        log_index = (log_index == 0) ? (CMD_LOG_SZ - 1) : (log_index - 1); // Move to the previous log, wrap around if necessary
+    }
+
+    return SUCCESS;
+}
+
+
+/**
+ * \fn dispatch_command
+ * 
+ * \brief Forward command to the appropriate task for execution and 
+ * log the command for telemetry
+ * 
+ * \param[in] p_cmd a pointer to the command struct to be dispatched
+ * 
+ * \return status_t, whether the forwarding was successful or not
+ */
+status_t enqueue_command(command_t *p_cmd) {
+
+    pvdx_task_t *target = p_cmd->target;
+
+    // Log cmd into buffer for telemetry
+    log_command(p_cmd); 
+
+    // Check if target is valid and enabled
+    if (target == NULL) {
+        return ERROR_BAD_TARGET;
+    }
+    if (!target->enabled) {
+        return ERROR_TASK_DISABLED;
+    }
+
+    // now forward cmd to task command queue
+    if (xQueueSendToBack(p_cmd->target->command_queue, p_cmd, 0) != pdTRUE) {
+        return ERROR_QUEUE_ERROR; 
+    }
+
+    return SUCCESS;
+} 
+

src/libraries/cmd_dispatcher/cmd_dispatcher.h

@@ -0,0 +1,15 @@
+#ifndef CMD_DISPATCHER_H
+#define CMD_DISPATCHER_H
+
+#include "globals.h"
+
+typedef struct {
+    pvdx_task_t *target;
+    command_data_type_t data_type;
+    operation_t operation; 
+} cmd_record_t;
+
+status_t get_n_logs(uint8_t n, cmd_record_t *logs);
+status_t enqueue_command(command_t *p_cmd);
+
+#endif // CMD_DISPATCHER_H

src/tasks/adcs/adcs_main.c

@@ -9,11 +9,10 @@
  */
 
 #include "adcs_task.h"
-#include "command_dispatcher_task.h"
 #include "globals.h"
 #include "logging.h"
 #include "watchdog_task.h"
-
+#include "cmd_dispatcher.h"
 // ADCS Task memory structures
 adcs_task_memory_t adcs_mem;
 
@@ -106,7 +105,9 @@ void main_adcs(void *pvParameters) {
 
         // Check in with the watchdog task
         if (should_checkin(current_task)) {
-            enqueue_command(&cmd_checkin);
+            while (enqueue_command(&cmd_checkin) != SUCCESS) {
+                warning("adcs: Failed to enqueue watchdog checkin command\n");
+            }
             debug("adcs: Enqueued watchdog checkin command\n");
         }
     }

src/tasks/display/display_main.c

@@ -7,8 +7,8 @@
  * Authors: Tanish Makadia, Ignacio Blancas Rodriguez, Siddharta Laloux
  */
 
-#include "command_dispatcher_task.h"
 #include "display_task.h"
+#include "cmd_dispatcher.h"
 
 // Display Task memory structures
 display_task_memory_t display_mem;
@@ -58,7 +58,9 @@ void main_display(void *pvParameters) {
         {
             // TODO: Add logic for blocking on the result of the display_image command
             command_t display_image_command = get_display_image_command(IMAGE_BUFFER_PVDX);
-            enqueue_command(&display_image_command);
+            while (enqueue_command(&display_image_command) != SUCCESS) {
+                warning("display: Failed to enqueue display image command\n");
+            }
 
             if (display_image_command.result != SUCCESS) {
                 warning("display: Failed to display image. Error code: %d\n", display_image_command.result);
@@ -68,7 +70,9 @@ void main_display(void *pvParameters) {
             // Set the display buffer to the second image
             // TODO: Add logic for blocking on the result of the display_image command
             command_t display_image_command = get_display_image_command(IMAGE_BUFFER_BROWNLOGO);
-            enqueue_command(&display_image_command);
+            while (enqueue_command(&display_image_command) != SUCCESS) {
+                warning("display: Failed to enqueue display image command\n");
+            }
 
             if (display_image_command.result != SUCCESS) {
                 warning("display: Failed to display image. Error code: %d\n", display_image_command.result);
@@ -77,7 +81,10 @@ void main_display(void *pvParameters) {
 
         // Check in with the watchdog task
         if (should_checkin(current_task)) {
-            enqueue_command(&cmd_checkin);
+            while (enqueue_command(&cmd_checkin) != SUCCESS) {
+                warning("display: Failed to enqueue watchdog checkin command\n");
+            }
+
             debug("display: Enqueued watchdog checkin command\n");
         }
     }