Got Pedals task working and improved serial_print

Core/Src/can_handler.c

@@ -113,7 +113,7 @@ void vRouteCanIncoming(void* pv_params)
 		/* Wait until new CAN message comes into queue */
 		status = osMessageQueueGet(can_inbound_queue, &message, NULL, 0U);
 		if (status != osOK) {
-			fault_data.diag = "Init Failed";
+			fault_data.diag = "CAN Router Init Failed";
 			queue_fault(&fault_data);
 		} else {
 			callback = getFunction(message->id);

Core/Src/main.c

@@ -24,6 +24,7 @@
 /* USER CODE BEGIN Includes */
 #include <stdio.h>
 #include "sht30.h"
+#include "lsm6dso.h"
 #include "monitor.h"
 #include "queues.h"
 #include "fault.h"
@@ -186,7 +187,12 @@ int main(void)
   onboard_temp_queue = osMessageQueueNew(ONBOARD_TEMP_QUEUE_SIZE, sizeof(onboard_temp_t), NULL);
   imu_queue = osMessageQueueNew(IMU_QUEUE_SIZE, sizeof(imu_data_t), NULL);
 
-  pedal_params_t pedal_params = {.accel_adc1 = &hadc1, .accel_adc2 = &hadc2, .brake_adc = &hadc3};
+  // A bit sloppy since I don't free this, but it will be allocated for lifetime
+  pedal_params_t *pedal_params = malloc(sizeof(pedal_params_t));
+  pedal_params->accel_adc1 = &hadc1;
+  pedal_params->accel_adc2 = &hadc2;
+  pedal_params->brake_adc = &hadc3;
+
   pedal_data_queue = osMessageQueueNew(PEDAL_DATA_QUEUE_SIZE, sizeof(pedals_t), NULL);
   /* USER CODE END RTOS_QUEUES */
 
@@ -195,15 +201,15 @@ int main(void)
   defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes);
 
   /* USER CODE BEGIN RTOS_THREADS */
-  temp_monitor_handle = osThreadNew(vTempMonitor, &hi2c1, &temp_monitor_attributes);
-  //watchdog_monitor_handle = osThreadNew(vWatchdogMonitor, GPIOB, &watchdog_monitor_attributes);
-  imu_monitor_handle = osThreadNew(vIMUMonitor, &hi2c1, &imu_monitor_attributes);
+  //temp_monitor_handle = osThreadNew(vTempMonitor, &hi2c1, &temp_monitor_attributes);
+  watchdog_monitor_handle = osThreadNew(vWatchdogMonitor, GPIOB, &watchdog_monitor_attributes);
+  //imu_monitor_handle = osThreadNew(vIMUMonitor, &hi2c1, &imu_monitor_attributes);
   serial_monitor_handle = osThreadNew(vSerialMonitor, NULL, &serial_monitor_attributes);
-  fault_handle = osThreadNew(vFaultHandler, NULL, &fault_handle_attributes);
-  pedals_monitor_handle = osThreadNew(vPedalsMonitor, &pedal_params, &pedals_monitor_attributes);
+  //fault_handle = osThreadNew(vFaultHandler, NULL, &fault_handle_attributes);
+  pedals_monitor_handle = osThreadNew(vPedalsMonitor, pedal_params, &pedals_monitor_attributes);
   //route_can_incoming_handle = osThreadNew(vRouteCanIncoming, &hcan1, &route_can_incoming_attributes);
   can_dispatch_handle = osThreadNew(vCanDispatch, &hcan1, &can_dispatch_attributes);
-  sm_director_handle = osThreadNew(vStateMachineDirector, NULL, &sm_director_attributes);
+  //sm_director_handle = osThreadNew(vStateMachineDirector, NULL, &sm_director_attributes);
 
   /* USER CODE END RTOS_THREADS */
 
@@ -725,9 +731,29 @@ void StartDefaultTask(void *argument)
 {
   /* USER CODE BEGIN 5 */
   int i = 0;
+  uint8_t data;
+  HAL_StatusTypeDef err;
   /* Infinite loop */
   for(;;) {
-    serial_print("Default Task:\t%d\r\n", i);
+    /* Testing getting data from I2C devices */
+    //serial_print("Register Address\tContents\r\n");
+    //serial_print("----------------\t--------\r\n");
+    //for (uint8_t reg = 0x00; reg <= 0x7E; reg++) {
+      //uint8_t reg = 0x0F;
+      //  err = HAL_I2C_Mem_Read(&hi2c1, 0x6A, reg, I2C_MEMADD_SIZE_8BIT, &data, 1, HAL_MAX_DELAY);
+      //  if (err)
+      //    serial_print("0x%02X\t\t\tErr: %d!\r\n", reg, err);
+      //  else
+      //    serial_print("0x%02X\t\t\t0x%02X\r\n", reg, data);;
+    //}
+
+    /* Basics of manually getting ADC reading (no DMA) */
+    //HAL_ADC_Start(&hadc1);
+    //HAL_StatusTypeDef err = HAL_ADC_PollForConversion(&hadc1, HAL_MAX_DELAY);
+    //if (!err)
+    //  serial_print("%d\r\n", HAL_ADC_GetValue(&hadc1));
+
+    /* Toggle LED at certain frequency */
     HAL_GPIO_TogglePin(GPIOC, LED_1_Pin); // Toggle on LED2
     i++;
     osDelay(YELLOW_LED_BLINK_DELAY);

Core/Src/monitor.c

@@ -33,14 +33,14 @@ void vTempMonitor(void* pv_params)
 	hi2c1				   = (I2C_HandleTypeDef*)pv_params;
 	temp_sensor.i2c_handle = hi2c1;
 
-	if (sht30_init(&temp_sensor)) {
-		fault_data.diag = "Init Failed";
-		queue_fault(&fault_data);
-	}
+	//if (sht30_init(&temp_sensor)) {
+	//	fault_data.diag = "Temp Monitor Init Failed";
+	//	queue_fault(&fault_data);
+	//}
 
 	for (;;) {
 		/* Take measurement */
-		serial_print("Temp Sensor Task\r\n");
+		//serial_print("Temp Sensor Task\r\n");
 		//if (sht30_get_temp_humid(&temp_sensor)) {
 		//	fault_data.diag = "Failed to get temp";
 		//	queue_fault(&fault_data);
@@ -69,7 +69,7 @@ osThreadId_t watchdog_monitor_handle;
 const osThreadAttr_t watchdog_monitor_attributes = {
 	.name		= "WatchdogMonitor",
 	.stack_size = 128 * 4,
-	.priority	= (osPriority_t)osPriorityNormal1,
+	.priority	= (osPriority_t)osPriorityNormal,
 };
 
 void vWatchdogMonitor(void* pv_params)
@@ -98,7 +98,8 @@ void vPedalsMonitor(void* pv_params)
 {
 	const uint8_t num_samples = 10;
 	enum { ACCELPIN_1, ACCELPIN_2, BRAKEPIN_1, BRAKEPIN_2 };
-	const uint16_t delayTime  = 2; /* ms */
+	const uint16_t delay_time  = 15; /* ms */
+	const uint16_t adc_sample_time = 2; /* ms */
 	const uint8_t can_msg_len = 4;	/* bytes */
 
 	nertimer_t diff_timer;
@@ -116,20 +117,40 @@ void vPedalsMonitor(void* pv_params)
 
 	uint16_t adc_data[4];
 
-    //HAL_ADC_Start(params->accel_adc1);
-	//HAL_ADC_Start(params->accel_adc2);
-	//HAL_ADC_Start(params->brake_adc);
-
 	for (;;) {
 		serial_print("Pedals Task\r\n");
+
 		/*
 		 * Get the value from the adc at the brake and accelerator
 		 * pin addresses and average them to the sensor data value
 		 */
-		//adc_data[ACCELPIN_1] = HAL_ADC_PollForConversion(params->accel_adc1, HAL_MAX_DELAY);
-		//adc_data[ACCELPIN_2] = HAL_ADC_PollForConversion(params->accel_adc2, HAL_MAX_DELAY);
-		//adc_data[BRAKEPIN_1] = HAL_ADC_PollForConversion(params->brake_adc, HAL_MAX_DELAY);
-		//adc_data[BRAKEPIN_2] = HAL_ADC_PollForConversion(params->brake_adc, HAL_MAX_DELAY);
+		HAL_ADC_Start(params->accel_adc1);
+		HAL_ADC_Start(params->accel_adc2);
+		HAL_ADC_Start(params->brake_adc);
+
+		/* Yield to other tasks */
+		osDelay(delay_time);
+
+		//uint32_t adc_err;
+
+		//adc_errs = HAL_ADC_PollForConversion(params->accel_adc1, HAL_MAX_DELAY);
+		//adc_err <<= 2;
+
+		HAL_StatusTypeDef err = HAL_ADC_PollForConversion(params->accel_adc1, HAL_MAX_DELAY);
+		if (!err)
+		  serial_print("Accel 1: %d\r\n", HAL_ADC_GetValue(params->accel_adc1));
+
+		err = HAL_ADC_PollForConversion(params->accel_adc2, HAL_MAX_DELAY);
+		if (!err)
+		  serial_print("Accel 2: %d\r\n", HAL_ADC_GetValue(params->accel_adc2));
+
+		err = HAL_ADC_PollForConversion(params->brake_adc, HAL_MAX_DELAY);
+		if (!err)
+		  serial_print("Brake 1: %d\r\n", HAL_ADC_GetValue(params->brake_adc));
+
+		//err = HAL_ADC_PollForConversion(params->brake_adc, HAL_MAX_DELAY);
+		//if (!err)
+		  //serial_print("Brake 2: %d\r\n", HAL_ADC_GetValue(params->brake_adc));
 
 		/* Evaluate accelerator faults */
 		//if (is_timer_expired(&oc_timer))
@@ -167,17 +188,14 @@ void vPedalsMonitor(void* pv_params)
 		//	  / num_samples;
 
 		/* Publish to Onboard Pedals Queue */
-		//osMessageQueuePut(pedal_data_queue, &sensor_data, 0U, 0U);
+		osMessageQueuePut(pedal_data_queue, &sensor_data, 0U, 0U);
 
 		/* Send CAN message */
 		//memcpy(pedal_msg.data, &sensor_data, can_msg_len);
 		//if (queue_can_msg(pedal_msg)) {
 		//	fault_data.diag = "Failed to send CAN message";
 		//	queue_fault(&fault_data);
 		//}
-
-		/* Yield to other tasks */
-		osDelay(delayTime);
   }
 }
 
@@ -215,13 +233,13 @@ void vIMUMonitor(void *pv_params)
 	imu.i2c_handle = hi2c1;
 
 	/* Initialize IMU */
-	if (lsm6dso_init(&imu, hi2c1)) {
-		fault_data.diag = "Init Failed";
-		queue_fault(&fault_data);
-	}
+	//if (lsm6dso_init(&imu, hi2c1)) {
+	//	fault_data.diag = "IMU Monitor Init Failed";
+	//	queue_fault(&fault_data);
+	//}
 
 	for(;;) {
-		serial_print("IMU Task\r\n");
+		//serial_print("IMU Task\r\n");
 		/* Take measurement */
 		//if (lsm6dso_read_accel(&imu)) {
 		//	fault_data.diag = "Failed to get IMU acceleration";

Core/Src/serial_monitor.c

@@ -17,20 +17,28 @@ const osThreadAttr_t serial_monitor_attributes;
 int serial_print(const char* format, ...)
 {
 	va_list arg;
-	char temp[PRINTF_BUFFER_LEN];
-	size_t len;
-	char* buffer = temp;
+	char *buffer = malloc(sizeof(char) * PRINTF_BUFFER_LEN);
+	if (buffer == NULL)
+		return -1;
 
 	/* Format Variadic Args into string */
 	va_start(arg, format);
-	len = vsnprintf(temp, PRINTF_BUFFER_LEN, format, arg);
+	size_t len = vsnprintf(buffer, PRINTF_BUFFER_LEN, format, arg);
 	va_end(arg);
 
 	/* Check to make sure we don't overflow buffer */
-	if (len > PRINTF_BUFFER_LEN - 1)
-		return -1;
+	if (len > PRINTF_BUFFER_LEN - 1) {
+		free(buffer);
+		return -2;
+	}
+
+	/* If string can't be queued */
+	osStatus_t stat = osMessageQueuePut(printf_queue, &buffer, 0U, 0U);
+	if (stat) {
+		free(buffer);
+		return -3;
+	}
 
-	osMessageQueuePut(printf_queue, &buffer, 0U, 0U);
 	return 0;
 }
 
@@ -48,6 +56,7 @@ void vSerialMonitor(void* pv_params)
 			// TODO: Trigger fault ?
 		} else {
 			printf(message);
+			free(message);
 		}
 	}
 }