190 data collection task, remove unnecessary queues, optimize RTOS pr…

…iorities and delays

Core/Inc/cerberus_conf.h

@@ -24,7 +24,7 @@
 #define FAULT_HANDLE_DELAY
 
 /* Pedal tuning */
-#define PEDALS_SAMPLE_DELAY 20 /* ms */
+#define PEDALS_SAMPLE_DELAY 10 /* ms */
 #define ACCEL1_OFFSET	    980
 #define ACCEL1_MAX_VAL	    1866
 #define ACCEL2_OFFSET	    1780

Core/Inc/dti.h

@@ -33,7 +33,6 @@
 #define DTI_CANID_ID_IQ 0x476 /* Id, Iq values */
 #define DTI_CANID_SIGNALS \
 	0x496 /* Throttle signal, Brake signal, IO, Drive enable */
-#define DTI_QUEUE_SIZE 5
 
 #define NEW_DTI_MSG_FLAG 1U
 

Core/Inc/monitor.h

@@ -5,12 +5,15 @@
 #include "stm32f4xx_hal.h"
 #include "stdbool.h"
 
-/**
- * @brief Task monitoring the voltage level of the low voltage batteries.
- */
-void vLVMonitor(void *pv_params);
-extern osThreadId lv_monitor_handle;
-extern const osThreadAttr_t lv_monitor_attributes;
+typedef struct {
+	uint16_t brake_value;
+	uint16_t accelerator_value; /* 0-100 */
+} pedals_t;
+
+pedals_t get_pedal_data();
+bool get_brake_state();
+
+bool get_tsms_reading();
 
 /* Defining Temperature Monitor Task */
 void vTempMonitor(void *pv_params);
@@ -32,23 +35,14 @@ void vIMUMonitor(void *pv_params);
 extern osThreadId_t imu_monitor_handle;
 extern const osThreadAttr_t imu_monitor_attributes;
 
-/* Task for Monitoring the TSMS sense on PDU  CTRL expander*/
-void vTsmsMonitor(void *pv_params);
-extern osThreadId_t tsms_monitor_handle;
-extern const osThreadAttr_t tsms_monitor_attributes;
-
-/* Task for Monitoring the Fuses on PDU */
-void vFusingMonitor(void *pv_params);
-extern osThreadId_t fusing_monitor_handle;
-extern const osThreadAttr_t fusing_monitor_attributes;
-
 /* Task for Monitoring the Shutdown Loop */
 void vShutdownMonitor(void *pv_params);
 extern osThreadId_t shutdown_monitor_handle;
 extern const osThreadAttr_t shutdown_monitor_attributes;
 
-void vSteeringIOButtonsMonitor(void *pv_params);
-extern osThreadId_t steeringio_buttons_monitor_handle;
-extern const osThreadAttr_t steeringio_buttons_monitor_attributes;
+/* Task for collecting data from the car */
+void vDataCollection(void *pv_params);
+extern osThreadId_t data_collection_thread;
+extern const osThreadAttr_t data_collection_attributes;
 
 #endif // MONITOR_H

Core/Inc/queues.h

@@ -4,7 +4,6 @@
 #include "cerberus_conf.h"
 #include "cmsis_os.h"
 
-#define BRAKELIGHT_QUEUE_SIZE	8
 #define ONBOARD_TEMP_QUEUE_SIZE 8
 #define PEDAL_DATA_QUEUE_SIZE	1
 #define IMU_QUEUE_SIZE		8
@@ -21,15 +20,6 @@ typedef struct {
 
 extern osMessageQueueId_t imu_queue;
 
-typedef struct {
-	uint16_t brake_value;
-	uint16_t accelerator_value; /* 0-100 */
-} pedals_t;
-
-extern osMessageQueueId_t pedal_data_queue;
-
 extern osMessageQueueId_t tsms_data_queue;
 
-bool get_brake_state();
-
 #endif // QUEUES_H

Core/Src/bms.c

@@ -8,7 +8,7 @@
 #include <stdlib.h>
 #include "stdio.h"
 
-#define CAN_QUEUE_SIZE 5
+#define BMS_QUEUE_SIZE 5
 
 osMessageQueueId_t bms_monitor_queue;
 
@@ -20,7 +20,7 @@ osThreadId_t bms_monitor_handle;
 const osThreadAttr_t bms_monitor_attributes = {
 	.name = "BMSCANMonitor",
 	.stack_size = 64 * 8,
-	.priority = (osPriority_t)osPriorityHigh2
+	.priority = (osPriority_t)osPriorityHigh
 };
 
 void bms_fault_callback()
@@ -43,7 +43,7 @@ void bms_init()
 		osTimerNew(&bms_fault_callback, osTimerOnce, NULL, NULL);
 
 	bms_monitor_queue =
-		osMessageQueueNew(CAN_QUEUE_SIZE, sizeof(can_msg_t), NULL);
+		osMessageQueueNew(BMS_QUEUE_SIZE, sizeof(can_msg_t), NULL);
 	assert(bms_monitor_queue);
 }
 

Core/Src/can_handler.c

@@ -110,7 +110,7 @@ osThreadId_t can_dispatch_handle;
 const osThreadAttr_t can_dispatch_attributes = {
 	.name = "CanDispatch",
 	.stack_size = 128 * 8,
-	.priority = (osPriority_t)osPriorityRealtime,
+	.priority = (osPriority_t)osPriorityRealtime6,
 };
 
 void vCanDispatch(void *pv_params)

Core/Src/control.c

@@ -12,6 +12,8 @@
 #include "pdu.h"
 #include "queues.h"
 #include "control.h"
+#include "monitor.h"
+#include "cerberus_conf.h"
 
 osThreadId brakelight_control_thread;
 const osThreadAttr_t brakelight_monitor_attributes = {

Core/Src/fault.c

@@ -44,6 +44,7 @@ void vFaultHandler(void *pv_params)
 		while (osMessageQueueGet(fault_handle_queue, &fault_data, NULL,
 					 osWaitForever) == osOK) {
 			uint32_t fault_id = (uint32_t)fault_data.id;
+			if (fault_id == CAN_DISPATCH_FAULT) continue;
 			endian_swap(&fault_id, sizeof(fault_id));
 			uint8_t defcon = (uint8_t)fault_data.severity;
 

Core/Src/main.c

@@ -81,7 +81,6 @@ const osThreadAttr_t defaultTask_attributes = {
   .priority = (osPriority_t) osPriorityNormal,
 };
 /* USER CODE BEGIN PV */
-osMessageQueueId_t pedal_data_queue;
 osMessageQueueId_t imu_queue;
 osMessageQueueId_t dti_router_queue;
 osMessageQueueId_t tsms_data_queue;
@@ -173,10 +172,13 @@ int main(void)
 
   /* Create Interfaces to Represent Relevant Hardware */
   mpu_t *mpu  = init_mpu(&hi2c1, &hadc3, &hadc1, GPIOC, GPIOB);
+  assert(mpu);
   pdu_t *pdu  = init_pdu(&hi2c2);
   assert(pdu);
   dti_t *mc   = dti_init();
+  assert(mc);
   steeringio_t *wheel = steeringio_init();
+  assert(wheel);
   init_can1(&hcan1);
   bms_init();
 
@@ -202,8 +204,6 @@ int main(void)
   /* USER CODE BEGIN RTOS_QUEUES */
   imu_queue = osMessageQueueNew(IMU_QUEUE_SIZE, sizeof(imu_data_t), NULL);
   assert(imu_queue);
-  pedal_data_queue = osMessageQueueNew(PEDAL_DATA_QUEUE_SIZE, sizeof(pedals_t), NULL);
-  assert(pedal_data_queue);
 	dti_router_queue = osMessageQueueNew(DTI_QUEUE_SIZE, sizeof(can_msg_t), NULL);
 	assert(dti_router_queue);
   tsms_data_queue = osMessageQueueNew(TSMS_QUEUE_SIZE, sizeof(bool), NULL);
@@ -217,23 +217,22 @@ int main(void)
   /* USER CODE BEGIN RTOS_THREADS */
 
   /* Monitors */
-  lv_monitor_handle = osThreadNew(vLVMonitor, mpu, &lv_monitor_attributes);
-  assert(lv_monitor_handle);
+  pedals_monitor_handle = osThreadNew(vPedalsMonitor, mpu, &pedals_monitor_attributes);
+  assert(pedals_monitor_handle);
   // temp_monitor_handle = osThreadNew(vTempMonitor, mpu, &temp_monitor_attributes);
   // assert(temp_monitor_handle);
   //imu_monitor_handle = osThreadNew(vIMUMonitor, mpu, &imu_monitor_attributes);
   //assert(imu_monitor_handle);
-  steeringio_buttons_monitor_handle = osThreadNew(vSteeringIOButtonsMonitor, wheel, &steeringio_buttons_monitor_attributes);
-  assert(steeringio_buttons_monitor_handle);
-  pedals_monitor_handle = osThreadNew(vPedalsMonitor, mpu, &pedals_monitor_attributes);
-  assert(pedals_monitor_handle);
-  tsms_monitor_handle = osThreadNew(vTsmsMonitor, pdu, &tsms_monitor_attributes);
-  assert(tsms_monitor_handle);
-  fusing_monitor_handle = osThreadNew(vFusingMonitor, pdu, &fusing_monitor_attributes);
-  assert(fusing_monitor_handle);
   // shutdown_monitor_handle = osThreadNew(vShutdownMonitor, pdu, &shutdown_monitor_attributes);
   // assert(shutdown_monitor_handle);
 
+  uint32_t *data_collection_args = malloc(3 * sizeof(uint32_t));
+  data_collection_args[0] = (uint32_t) mpu;
+  data_collection_args[1] = (uint32_t) pdu;
+  data_collection_args[2] = (uint32_t) wheel;
+  data_collection_thread = osThreadNew(vDataCollection, data_collection_args, &data_collection_attributes);
+  assert(data_collection_thread);
+
   /* Data Processing */
   process_tsms_thread_id = osThreadNew(vProcessTSMS, NULL, &process_tsms_attributes);
   assert(process_tsms_thread_id);
@@ -255,15 +254,19 @@ int main(void)
   assert(process_pedals_thread);
   fault_handle = osThreadNew(vFaultHandler, NULL, &fault_handle_attributes);
   assert(fault_handle);
+
   uint32_t *state_machine_args = malloc(sizeof(uint32_t) * 2);
+
+  /* No clue why the memory can only be correctly free'd only after another malloc call. If you free above the malloc the thread does not init correctly working atleast in my experience maybe I am doing something wrong. */
+  free(data_collection_args);
+
   state_machine_args[0] = (uint32_t) pdu;
   state_machine_args[1] = (uint32_t) mc;
   sm_director_handle = osThreadNew(vStateMachineDirector, state_machine_args, &sm_director_attributes);
   assert(sm_director_handle);
   free(state_machine_args);
   brakelight_control_thread = osThreadNew(vBrakelightControl, pdu, &brakelight_monitor_attributes);;
   assert(brakelight_control_thread);
-
   /* USER CODE END RTOS_THREADS */
 
   /* USER CODE BEGIN RTOS_EVENTS */
@@ -720,23 +723,15 @@ static void MX_GPIO_Init(void)
 void StartDefaultTask(void *argument)
 {
   /* USER CODE BEGIN 5 */
-  int i = 0;
 
   /* Infinite loop */
   for(;;) {
 
     /* Pet watchdog */
     HAL_IWDG_Refresh(&hiwdg);
     /* Toggle LED at certain frequency */
-    HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_8); // I am not using MPU interface because I'm lazy
-
-    if (i % 2 == 1)
-     serial_print(".\r\n");
-    else
-     serial_print("..\r\n");
-
-    i++;
-
+    printf(".\r\n..\r\n");
+    HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_8);
     osDelay(500);
     //osDelay(YELLOW_LED_BLINK_DELAY);
   }