Sending messages from VC properly without proper filtering (#1284)

### Changelist 
telem files and seperation of can files in tasks

### Testing Done
tested on hardware

### Resolved Tickets
n/a

---------

Co-authored-by: larakawasme <[email protected]>
Co-authored-by: Edwin <[email protected]>

Pipfile

@@ -21,17 +21,15 @@ python-can = "*"
 rich = "*"
 PyGithub = "*"
 black = "*"
-littlefs-python = "*"
-flask = "*"
-Definitions = "*"
-flask_cors = "*"
+flask-socketio = "*"
 pandas = "*"
-flask_socketio = "*"
 pytest = "*"
 asammdf = "*"
+python-dateutil = "*"
 requests = "*"
 python-dotenv = "*"
 influxdb-client = "*"
+flask = "*"
 
 [dev-packages]
 setuptools = "*"

firmware/boot/bootloader.c

@@ -255,7 +255,9 @@ _Noreturn void bootloader_runCanTxTask(void)
 {
     for (;;)
     {
-        io_can_transmitMsgFromQueue();
+        CanMsg tx_msg;
+        io_can_popTxMsgFromQueue(&tx_msg);
+        io_can_transmitMsgFromQueue(&tx_msg);
     }
 }
 

firmware/cmake/telem.proto

@@ -0,0 +1,16 @@
+// Tracksight Message
+
+syntax = "proto3";
+
+message TelemMessage {
+    int32 can_id = 1;
+    int32 message_0 = 2;
+    int32 message_1 = 3;
+    int32 message_2 = 4;
+    int32 message_3 = 5;
+    int32 message_4 = 6;
+    int32 message_5 = 7;
+    int32 message_6 = 8;
+    int32 message_7 = 9;
+    int32 time_stamp = 10;
+}

firmware/quadruna/BMS/src/tasks.c

@@ -457,7 +457,9 @@ _Noreturn void tasks_runCanTx(void)
 
     for (;;)
     {
-        io_can_transmitMsgFromQueue();
+        CanMsg tx_msg;
+        io_can_popTxMsgFromQueue(&tx_msg);
+        io_can_transmitMsgFromQueue(&tx_msg);
     }
 }
 

firmware/quadruna/CRIT/src/tasks.c

@@ -495,7 +495,9 @@ _Noreturn void tasks_runCanTx(void)
     // Setup tasks.
     for (;;)
     {
-        io_can_transmitMsgFromQueue();
+        CanMsg tx_msg;
+        io_can_popTxMsgFromQueue(&tx_msg);
+        io_can_transmitMsgFromQueue(&tx_msg);
     }
 }
 

firmware/quadruna/FSM/src/tasks.c

@@ -319,7 +319,9 @@ _Noreturn void tasks_runCanTx(void)
 
     for (;;)
     {
-        io_can_transmitMsgFromQueue();
+        CanMsg tx_msg;
+        io_can_popTxMsgFromQueue(&tx_msg);
+        io_can_transmitMsgFromQueue(&tx_msg);
     }
 }
 

firmware/quadruna/RSM/src/tasks.c

@@ -291,7 +291,9 @@ _Noreturn void tasks_runCanTx(void)
 
     for (;;)
     {
-        io_can_transmitMsgFromQueue();
+        CanMsg tx_msg;
+        io_can_popTxMsgFromQueue(&tx_msg);
+        io_can_transmitMsgFromQueue(&tx_msg);
     }
 }
 

firmware/quadruna/VC/proto/telem.proto

@@ -1,10 +1,16 @@
 // Tracksight Message
 
 syntax = "proto3";
-import "nanopb.proto";
 
 message TelemMessage {
     int32 can_id = 1;
-    repeated int32 message = 2 [(nanopb).max_count = 8];
-    int32 time_stamp = 3;
+    int32 message_0 = 2;
+    int32 message_1 = 3;
+    int32 message_2 = 4;
+    int32 message_3 = 5;
+    int32 message_4 = 6;
+    int32 message_5 = 7;
+    int32 message_6 = 8;
+    int32 message_7 = 9;
+    int32 time_stamp = 10;
 }

firmware/quadruna/VC/src/cubemx/Src/main.c

@@ -137,6 +137,18 @@ const osThreadAttr_t TaskLogging_attributes = {
     .stack_size = sizeof(TaskLoggingBuffer),
     .priority   = (osPriority_t)osPriorityLow,
 };
+/* Definitions for TaskTelem */
+osThreadId_t         TaskTelemHandle;
+uint32_t             TaskTelemBuffer[512];
+osStaticThreadDef_t  TaskTelemControlBlock;
+const osThreadAttr_t TaskTelem_attributes = {
+    .name       = "TaskTelem",
+    .cb_mem     = &TaskTelemControlBlock,
+    .cb_size    = sizeof(TaskTelemControlBlock),
+    .stack_mem  = &TaskTelemBuffer[0],
+    .stack_size = sizeof(TaskTelemBuffer),
+    .priority   = (osPriority_t)osPriorityLow,
+};
 /* USER CODE BEGIN PV */
 Gpio sd_present = {
     .pin  = GPIO_PIN_8,
@@ -166,6 +178,7 @@ void        RunCanRxTask(void *argument);
 void        RunTask1kHz(void *argument);
 void        RunTask1Hz(void *argument);
 void        RunTaskLogging(void *argument);
+void        RunTaskTelem(void *argument);
 
 /* USER CODE BEGIN PFP */
 
@@ -259,6 +272,9 @@ int main(void)
     /* creation of TaskLogging */
     TaskLoggingHandle = osThreadNew(RunTaskLogging, NULL, &TaskLogging_attributes);
 
+    /* creation of TaskTelem */
+    TaskTelemHandle = osThreadNew(RunTaskTelem, NULL, &TaskTelem_attributes);
+
     /* USER CODE BEGIN RTOS_THREADS */
     /* add threads, ... */
     /* USER CODE END RTOS_THREADS */
@@ -1189,14 +1205,25 @@ void RunTask1Hz(void *argument)
 void RunTaskLogging(void *argument)
 {
     /* USER CODE BEGIN RunTaskLogging */
-    /* Infinite loop */
-    for (;;)
-    {
-        tasks_runLogging();
-    }
+
+    tasks_runLogging();
     /* USER CODE END RunTaskLogging */
 }
 
+/* USER CODE BEGIN Header_RunTaskTelem */
+/**
+ * @brief Function implementing the TaskTelem thread.
+ * @param argument: Not used
+ * @retval None
+ */
+/* USER CODE END Header_RunTaskTelem */
+void RunTaskTelem(void *argument)
+{
+    /* USER CODE BEGIN RunTaskTelem */
+    tasks_runTelem();
+    /* USER CODE END RunTaskTelem */
+}
+
 /**
  * @brief  Period elapsed callback in non blocking mode
  * @note   This function is called  when TIM6 interrupt took place, inside

firmware/quadruna/VC/src/cubemx/VC.ioc

@@ -93,7 +93,7 @@ FDCAN1.StdFiltersNbr=1
 FDCAN1.TxFifoQueueElmtsNbr=1
 FREERTOS.FootprintOK=true
 FREERTOS.IPParameters=Tasks01,FootprintOK
-FREERTOS.Tasks01=Task100Hz,40,8096,RunTask100Hz,Default,NULL,Static,Task100HzBuffer,Task100HzControlBlock;TaskCanTx,16,512,RunCanTxTask,Default,NULL,Static,canTxTaskBuffer,canTxTaskControlBlock;TaskCanRx,16,512,RunCanRxTask,Default,NULL,Static,canRxTaskBuffer,canRxTaskControlBlock;Task1kHz,48,512,RunTask1kHz,Default,NULL,Static,Task1kHzBuffer,Task1kHzControlBlock;Task1Hz,32,512,RunTask1Hz,Default,NULL,Static,Task1HzBuffer,Task1HzControlBlock;TaskLogging,8,1024,RunTaskLogging,Default,NULL,Static,TaskLoggingBuffer,TaskLoggingControlBlock
+FREERTOS.Tasks01=Task100Hz,40,8096,RunTask100Hz,Default,NULL,Static,Task100HzBuffer,Task100HzControlBlock;TaskCanTx,16,512,RunCanTxTask,Default,NULL,Static,canTxTaskBuffer,canTxTaskControlBlock;TaskCanRx,16,512,RunCanRxTask,Default,NULL,Static,canRxTaskBuffer,canRxTaskControlBlock;Task1kHz,48,512,RunTask1kHz,Default,NULL,Static,Task1kHzBuffer,Task1kHzControlBlock;Task1Hz,32,512,RunTask1Hz,Default,NULL,Static,Task1HzBuffer,Task1HzControlBlock;TaskLogging,8,1024,RunTaskLogging,Default,NULL,Static,TaskLoggingBuffer,TaskLoggingControlBlock;TaskTelem,8,512,RunTaskTelem,Default,NULL,Static,TaskTelemBuffer,TaskTelemControlBlock
 File.Version=6
 GPIO.groupedBy=Group By Peripherals
 I2C1.IPParameters=Timing

firmware/quadruna/VC/src/cubemx/VC.ioc.md5

@@ -1 +1 @@
-43ef53a33ef962205a8b320f5fbc6ded
+b3531f304282bafc01eb3f14081ed474

firmware/quadruna/VC/src/io/io_telemMessage.c

@@ -3,57 +3,109 @@
 #include "pb_encode.h"
 #include "pb_decode.h"
 #include "io_time.h"
+#include "cmsis_os.h"
+#include "queue.h"
+#include "io_log.h"
 
 // create the truth table for now to decide which amount of things to use
 // create or grab the constants for the different modem and pins and such
-
 // Private Globals
 
-#define CAN_DATA_LENGTH 8
+#define CAN_DATA_LENGTH 12
 #define UART_LENGTH 1
-#define QUEUE_SIZE 12
-#define QUEUE_BYTES 4 * QUEUE_SIZE // this is all temp
-
+#define QUEUE_SIZE 50
 static bool         modem_900_choice;
 static const Modem *modem = NULL;
+#define QUEUE_BYTES CAN_DATA_LENGTH *QUEUE_SIZE
+
+static bool               proto_status;
+static uint8_t            proto_msg_length;
+static StaticQueue_t      queue_control_block;
+static uint8_t            queue_buf[QUEUE_BYTES];
+static osMessageQueueId_t message_queue_id;
+static uint8_t            proto_out_length;
 
-static bool    proto_status;
-static uint8_t proto_msg_length;
-static uint8_t proto_buffer[QUEUE_SIZE]; // TODO: verify that this is the needed size (most likely can be smaller)
-TelemMessage   t_message = TelemMessage_init_zero;
+TelemMessage t_message = TelemMessage_init_zero;
+
+static const osMessageQueueAttr_t queue_attr = {
+    .name      = "Telem Message Queue",
+    .attr_bits = 0,
+    .cb_mem    = &queue_control_block,
+    .cb_size   = sizeof(StaticQueue_t),
+    .mq_mem    = queue_buf,
+    .mq_size   = QUEUE_BYTES,
+};
 
 void io_telemMessage_init(const Modem *m)
 {
     modem_900_choice = true; // if false, then using the 2.4GHz,
     modem            = m;
+    message_queue_id = osMessageQueueNew(CAN_DATA_LENGTH, QUEUE_SIZE, &queue_attr);
 }
 
-bool io_telemMessage_broadcast(CanMsg *rx_msg)
+bool io_telemMessage_pushMsgtoQueue(CanMsg *rx_msg)
 {
+    uint8_t proto_buffer[QUEUE_SIZE] = { 0 };
+
+    // filter messages
+    if (rx_msg->std_id != 111)
+    {
+        return false;
+    }
     // send it over the correct UART functionality
     pb_ostream_t stream = pb_ostream_from_buffer(proto_buffer, sizeof(proto_buffer));
 
     // filling in fields
-    t_message.can_id = (int32_t)(rx_msg->std_id);
-    for (uint8_t i = 0; i < CAN_DATA_LENGTH; i++)
+    if (rx_msg->dlc > 8)
+        return false;
+    t_message.can_id    = (int32_t)(rx_msg->std_id);
+    t_message.message_0 = rx_msg->data[0];
+    t_message.message_1 = rx_msg->data[1];
+    t_message.message_2 = rx_msg->data[2];
+    t_message.message_3 = rx_msg->data[3];
+    t_message.message_4 = rx_msg->data[4];
+    t_message.message_5 = rx_msg->data[5];
+    t_message.message_6 = rx_msg->data[6];
+    t_message.message_7 = rx_msg->data[7];
+
+    // encoding message
+
+    proto_status                         = pb_encode(&stream, TelemMessage_fields, &t_message);
+    proto_msg_length                     = (uint8_t)stream.bytes_written;
+    proto_buffer[49]                     = proto_msg_length;
+    static uint32_t telem_overflow_count = 0;
+    osStatus_t      s                    = osMessageQueuePut(message_queue_id, &proto_buffer, 0U, 0U);
+    if (s != osOK)
+    {
+        telem_overflow_count++;
+        LOG_WARN("queue problem");
+    }
+    else
     {
-        t_message.message[i] = rx_msg->data[i];
+        LOG_INFO("proto pushed to queue");
     }
-    t_message.time_stamp = (int32_t)io_time_getCurrentMs();
+    return true;
+}
 
-    // encoding message
-    proto_status     = pb_encode(&stream, TelemMessage_fields, &t_message);
-    proto_msg_length = (uint8_t)stream.bytes_written;
+bool io_telemMessage_broadcastMsgFromQueue(void)
+{
+    uint8_t    proto_out[QUEUE_SIZE] = { 0 };
+    uint8_t    zero_test             = 0;
+    osStatus_t status                = osMessageQueueGet(message_queue_id, &proto_out, NULL, osWaitForever);
+    proto_out_length                 = proto_out[49];
+    proto_out[49]                    = 0;
 
-    if (modem_900_choice == true)
+    LOG_INFO("proto popped and on to uart");
+    if (modem_900_choice)
     {
-        hw_uart_transmitPoll(modem->modem900M, &proto_msg_length, UART_LENGTH, UART_LENGTH);
-        hw_uart_transmitPoll(modem->modem900M, proto_buffer, (uint8_t)sizeof(proto_buffer), UART_LENGTH);
+        hw_uart_transmitPoll(modem->modem900M, &proto_out_length, UART_LENGTH, UART_LENGTH);
+        hw_uart_transmitPoll(modem->modem900M, proto_out, (uint8_t)sizeof(proto_out), 100);
+        // hw_uart_transmitPoll(modem->modem900M, &zero_test, UART_LENGT/H, UART_LENGTH);
     }
     else
     {
         hw_uart_transmitPoll(modem->modem2_4G, &proto_msg_length, UART_LENGTH, UART_LENGTH);
-        hw_uart_transmitPoll(modem->modem2_4G, proto_buffer, (uint8_t)sizeof(proto_buffer), UART_LENGTH);
+        hw_uart_transmitPoll(modem->modem2_4G, proto_out, (uint8_t)sizeof(proto_out), 100);
     }
     return true;
 }

firmware/quadruna/VC/src/io/io_telemMessage.h

@@ -21,7 +21,11 @@ EMPTY_STRUCT(Modem);
 void io_telemMessage_init(const Modem *m);
 
 /**
- * Serializes the can msg and sends it over UART to the appropriate Modem
+ * Serializes the can msg and pushes it to the queue
  */
+bool io_telemMessage_pushMsgtoQueue(CanMsg *rx_msg);
 
-bool io_telemMessage_broadcast(CanMsg *rx_msg);
+/**
+ * Pops message from the queue and passes over uart
+ */
+bool io_telemMessage_broadcastMsgFromQueue(void);

firmware/quadruna/VC/src/tasks.c

@@ -70,8 +70,6 @@ static void canRxCallback(CanMsg *rx_msg)
         io_canLogging_loggingQueuePush(rx_msg); // push to logging queue
         read_count++;
     }
-
-    // TODO all telemetry here
 }
 
 SdCard                 sd  = { .hsd = &hsd1, .timeout = 1000 };
@@ -528,7 +526,18 @@ _Noreturn void tasks_runCanTx(void)
 
     for (;;)
     {
-        io_can_transmitMsgFromQueue();
+        CanMsg tx_msg;
+        io_can_popTxMsgFromQueue(&tx_msg);
+        io_telemMessage_pushMsgtoQueue(&tx_msg);
+        io_can_transmitMsgFromQueue(&tx_msg);
+    }
+}
+
+_Noreturn void tasks_runTelem(void)
+{
+    for (;;)
+    {
+        io_telemMessage_broadcastMsgFromQueue();
     }
 }
 
@@ -540,7 +549,7 @@ _Noreturn void tasks_runCanRx(void)
     {
         CanMsg rx_msg;
         io_can_popRxMsgFromQueue(&rx_msg);
-        //        io_telemMessage_broadcast(&rx_msg);
+        io_telemMessage_pushMsgtoQueue(&rx_msg);
         JsonCanMsg jsoncan_rx_msg;
         io_jsoncan_copyFromCanMsg(&rx_msg, &jsoncan_rx_msg);
         io_canRx_updateRxTableWithMessage(&jsoncan_rx_msg);

firmware/quadruna/VC/src/tasks.h

@@ -7,4 +7,5 @@ _Noreturn void tasks_run100Hz(void);
 _Noreturn void tasks_run1kHz(void);
 _Noreturn void tasks_runCanTx(void);
 _Noreturn void tasks_runCanRx(void);
-_Noreturn void tasks_runLogging(void);
+_Noreturn void tasks_runTelem(void);
+_Noreturn void tasks_runLogging(void);