190 moving logic and functions around, simplifying complex code

Core/Inc/dti.h

@@ -35,10 +35,6 @@
 	0x496 /* Throttle signal, Brake signal, IO, Drive enable */
 #define DTI_QUEUE_SIZE 5
 
-#define TIRE_DIAMETER 16 /* inches */
-#define GEAR_RATIO    47 / 13.0 /* unitless */
-#define POLE_PAIRS    10 /* unitless */
-
 typedef struct {
 	int32_t rpm; /* SCALE: 1         UNITS: Rotations per Minute   */
 	int16_t duty_cycle; /* SCALE: 10        UNITS: Percentage             */
@@ -57,6 +53,8 @@ typedef struct {
 // TODO: Expand GET interface
 int32_t dti_get_rpm(dti_t *dti);
 
+float dti_get_mph(dti_t *dti);
+
 uint16_t dti_get_input_voltage(dti_t *dti);
 
 /* Utilities for Decoding CAN message */

Core/Inc/pdu.h

@@ -41,6 +41,13 @@ typedef enum {
 
 int8_t read_fuses(pdu_t *pdu, bool status[MAX_FUSES]);
 
+/**
+ * @brief Read the state of the TSMS signal.
+ * 
+ * @param pdu Struct representing the PDU.
+ * @param status Pointer to location in memory where value will be read to.
+ * @return int8_t Error code.
+ */
 int8_t read_tsms_sense(pdu_t *pdu, bool *status);
 
 /* Functions to Read Status of Various Stages of Shutdown Loop */

Core/Inc/steeringio.h

@@ -36,12 +36,6 @@ typedef struct {
 	bool debounced_buttons[MAX_STEERING_BUTTONS];
 } steeringio_t;
 
-/* Utilities for Decoding CAN message */
-extern osThreadId_t steeringio_router_handle;
-extern const osThreadAttr_t steeringio_router_attributes;
-extern osMessageQueueId_t steeringio_router_queue;
-void vSteeringIORouter(void *pv_params);
-
 /* Creates a new Steering Wheel interface */
 steeringio_t *steeringio_init();
 

Core/Src/dti.c

@@ -14,6 +14,7 @@
 #include "emrax.h"
 #include "fault.h"
 #include "c_utils.h"
+#include <math.h>
 #include <assert.h>
 #include <stdlib.h>
 #include <string.h>
@@ -252,6 +253,18 @@ int32_t dti_get_rpm(dti_t *mc)
 	return rpm;
 }
 
+float dti_get_mph(dti_t *mc)
+{
+	/* Convert RPM to MPH */
+	// rpm * gear ratio = wheel rpm
+	// tire diamter (in) to miles --> tire diamter miles
+	// wheel rpm * 60 --> wheel rph
+	// tire diamter miles * pi --> tire circumference
+	// rph * wheel circumference miles --> mph
+	return (dti_get_rpm(mc) / (GEAR_RATIO)) * 60 *
+	       (TIRE_DIAMETER / 63360.0) * M_PI;
+}
+
 /* Inbound Task-specific Info */
 osThreadId_t dti_router_handle;
 const osThreadAttr_t dti_router_attributes = {

Core/Src/fault.c

@@ -48,7 +48,8 @@ void vFaultHandler(void *pv_params)
 			msg.len = 8;
 			uint8_t msg_data[8];
 			memcpy(msg_data, &fault_id, sizeof(fault_id));
-			memcpy(msg_data + sizeof(fault_id), &defcon, sizeof(defcon));
+			memcpy(msg_data + sizeof(fault_id), &defcon,
+			       sizeof(defcon));
 			memcpy(msg.data, msg_data, msg.len);
 			queue_can_msg(msg);
 			printf("\r\nFault Handler! Diagnostic Info:\t%s\r\n\r\n",

Core/Src/main.c

@@ -201,8 +201,11 @@ int main(void)
 
   /* USER CODE BEGIN RTOS_QUEUES */
   brakelight_signal = osMessageQueueNew(BRAKELIGHT_QUEUE_SIZE, sizeof(bool), NULL);
+  assert(brakelight_signal);
   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);
   /* USER CODE END RTOS_QUEUES */

Core/Src/monitor.c

@@ -69,53 +69,23 @@ void vLVMonitor(void *pv_params)
 	}
 }
 
-osThreadId_t temp_monitor_handle;
-const osThreadAttr_t temp_monitor_attributes = {
-	.name = "TempMonitor",
-	.stack_size = 32 * 8,
-	.priority = (osPriority_t)osPriorityHigh1,
-};
-
 bool get_brake_state()
 {
 	return brake_state;
 }
 
-void vTempMonitor(void *pv_params)
+/**
+ * @brief Return the adjusted pedal value based on its offset and maximum value. Clamps negative values to 0.
+ * 
+ * @param raw the raw pedal value
+ * @param offset the offset for the pedal
+ * @param max the maximum value of the pedal
+ */
+uint16_t adjust_pedal_val(uint32_t raw, int32_t offset, int32_t max)
 {
-	fault_data_t fault_data = { .id = ONBOARD_TEMP_FAULT,
-				    .severity = DEFCON5 };
-	can_msg_t temp_msg = { .id = CANID_TEMP_SENSOR,
-			       .len = 4,
-			       .data = { 0 } };
-
-	mpu_t *mpu = (mpu_t *)pv_params;
-
-	for (;;) {
-		/* Take measurement */
-		uint16_t temp = 0;
-		uint16_t humidity = 0;
-		if (read_temp_sensor(mpu, &temp, &humidity)) {
-			fault_data.diag = "Failed to get temp";
-			queue_fault(&fault_data);
-		}
-
-		serial_print("MPU Board Temperature:\t%d\r\n", temp);
-
-		temp_msg.data[0] = temp & 0xFF;
-		temp_msg.data[1] = (temp >> 8) & 0xFF;
-		temp_msg.data[2] = humidity & 0xFF;
-		temp_msg.data[3] = (humidity >> 8) & 0xFF;
-
-		/* Send CAN message */
-		if (queue_can_msg(temp_msg)) {
-			fault_data.diag = "Failed to send CAN message";
-			queue_fault(&fault_data);
-		}
-
-		/* Yield to other tasks */
-		osDelay(TEMP_SENS_SAMPLE_DELAY);
-	}
+	return (int16_t)raw - offset <= 0 ?
+		       0 :
+		       (uint16_t)(raw - offset) * 100 / (max - offset);
 }
 
 void eval_pedal_fault(uint16_t accel_1, uint16_t accel_2,
@@ -160,15 +130,9 @@ void eval_pedal_fault(uint16_t accel_1, uint16_t accel_2,
 
 	/* Normalize pedal values */
 	uint16_t accel_1_norm =
-		accel_1 - ACCEL1_OFFSET <= 0 ?
-			0 :
-			(uint16_t)(accel_1 - ACCEL1_OFFSET) * 100 /
-				(ACCEL1_MAX_VAL - ACCEL1_OFFSET);
+		adjust_pedal_val(accel_1, ACCEL1_OFFSET, ACCEL1_MAX_VAL);
 	uint16_t accel_2_norm =
-		accel_2 - ACCEL2_OFFSET <= 0 ?
-			0 :
-			(uint16_t)(accel_2 - ACCEL2_OFFSET) * 100 /
-				(ACCEL2_MAX_VAL - ACCEL2_OFFSET);
+		adjust_pedal_val(accel_2, ACCEL1_OFFSET, ACCEL1_MAX_VAL);
 
 	/* Fault - difference between pedal sensing values */
 	if ((abs(accel_1_norm - accel_2_norm) > PEDAL_DIFF_THRESH) &&
@@ -190,20 +154,6 @@ void eval_pedal_fault(uint16_t accel_1, uint16_t accel_2,
 	}
 }
 
-/**
- * @brief Return the adjusted pedal value based on its offset and maximum value. Clamps negative values to 0.
- * 
- * @param raw the raw pedal value
- * @param offset the offset for the pedal
- * @param max the maximum value of the pedal
- */
-uint16_t adjust_pedal_val(uint32_t raw, int32_t offset, int32_t max)
-{
-	return (int16_t)raw - offset <= 0 ?
-		       0 :
-		       (uint16_t)(raw - offset) * 100 / (max - offset);
-}
-
 osThreadId_t pedals_monitor_handle;
 const osThreadAttr_t pedals_monitor_attributes = {
 	.name = "PedalMonitor",
@@ -448,8 +398,6 @@ void vFusingMonitor(void *pv_params)
 		uint8_t fuse_2;
 	} fuse_data;
 
-	bool tsms_status = false;
-
 	for (;;) {
 		fuse_buf = 0;
 
@@ -477,12 +425,9 @@ void vFusingMonitor(void *pv_params)
 			queue_fault(&fault_data);
 		}
 
-		/* If we got a reliable TSMS reading, handle transition to and out of ACTIVE*/
-		if (!read_tsms_sense(pdu, &tsms_status)) {
-			tsms = tsms_status;
-			if (get_func_state() == ACTIVE && tsms == 0) {
-				set_home_mode();
-			}
+		/* If the TSMS is off, the car should no longer be active. */
+		if (!get_tsms() && get_func_state() == ACTIVE) {
+			set_home_mode();
 		}
 
 		osDelay(FUSES_SAMPLE_DELAY);
@@ -548,7 +493,7 @@ void vShutdownMonitor(void *pv_params)
 osThreadId steeringio_buttons_monitor_handle;
 const osThreadAttr_t steeringio_buttons_monitor_attributes = {
 	.name = "SteeringIOButtonsMonitor",
-	.stack_size = 200 * 8,
+	.stack_size = 400,
 	.priority = (osPriority_t)osPriorityNormal,
 };
 
@@ -624,4 +569,48 @@ void vBrakelightMonitor(void *pv_params)
 			write_brakelight(pdu, state);
 		}
 	}
-}
+}
+
+osThreadId_t temp_monitor_handle;
+const osThreadAttr_t temp_monitor_attributes = {
+	.name = "TempMonitor",
+	.stack_size = 32 * 8,
+	.priority = (osPriority_t)osPriorityHigh1,
+};
+
+void vTempMonitor(void *pv_params)
+{
+	fault_data_t fault_data = { .id = ONBOARD_TEMP_FAULT,
+				    .severity = DEFCON5 };
+	can_msg_t temp_msg = { .id = CANID_TEMP_SENSOR,
+			       .len = 4,
+			       .data = { 0 } };
+
+	mpu_t *mpu = (mpu_t *)pv_params;
+
+	for (;;) {
+		/* Take measurement */
+		uint16_t temp = 0;
+		uint16_t humidity = 0;
+		if (read_temp_sensor(mpu, &temp, &humidity)) {
+			fault_data.diag = "Failed to get temp";
+			queue_fault(&fault_data);
+		}
+
+		serial_print("MPU Board Temperature:\t%d\r\n", temp);
+
+		temp_msg.data[0] = temp & 0xFF;
+		temp_msg.data[1] = (temp >> 8) & 0xFF;
+		temp_msg.data[2] = humidity & 0xFF;
+		temp_msg.data[3] = (humidity >> 8) & 0xFF;
+
+		/* Send CAN message */
+		if (queue_can_msg(temp_msg)) {
+			fault_data.diag = "Failed to send CAN message";
+			queue_fault(&fault_data);
+		}
+
+		/* Yield to other tasks */
+		osDelay(TEMP_SENS_SAMPLE_DELAY);
+	}
+}

Core/Src/state_machine.c

@@ -118,18 +118,16 @@ static int transition_functional_state(func_state_t new_state)
 		write_pump(pdu, false);
 		write_fault(pdu, true);
 		transition_drive_state(OFF);
-		printf("READY\r\n");
+		serial_print("READY\r\n");
 		break;
 	case ACTIVE:
-		serial_print("BRAKE: %d\n", get_brake_state());
 		if (!get_tsms() || !get_brake_state())
 			return 1; // Cannot go into active if tsms is off
 		/* Turn on high power peripherals */
 		// write_fan_battbox(pdu, true);
 		write_pump(pdu, true);
 		write_fault(pdu, true);
-		sound_rtds(pdu); // TEMPORARY
-		printf("ACTIVE STATE\r\n");
+		serial_print("ACTIVE STATE\r\n");
 		break;
 	case FAULTED:
 		/* Turn off high power peripherals */
@@ -142,7 +140,7 @@ static int transition_functional_state(func_state_t new_state)
 		transition_drive_state(OFF);
 		cerberus_state.nero =
 			(nero_state_t){ .nero_index = OFF, .home_mode = true };
-		printf("FAULTED\r\n");
+		serial_print("FAULTED\r\n");
 		break;
 	default:
 		// Do Nothing

Core/Src/steeringio.c

@@ -175,29 +175,3 @@ void steeringio_update(steeringio_t *wheel, uint8_t wheel_data[])
 	}
 	osMutexRelease(wheel->button_mutex);
 }
-
-/* Inbound Task-specific Info */
-osThreadId_t steeringio_router_handle;
-const osThreadAttr_t steeringio_router_attributes = {
-	.name = "SteeringIORouter",
-	.stack_size = 128 * 8,
-	.priority = (osPriority_t)osPriorityHigh1
-};
-
-void vSteeringIORouter(void *pv_params)
-{
-	button_data_t message;
-	osStatus_t status;
-	// fault_data_t fault_data = { .id = STEERINGIO_ROUTING_FAULT, .severity = DEFCON2 };
-
-	steeringio_t *wheel = (steeringio_t *)pv_params;
-
-	for (;;) {
-		/* Wait until new CAN message comes into queue */
-		status = osMessageQueueGet(steeringio_router_queue, &message,
-					   NULL, osWaitForever);
-		if (status == osOK) {
-			steeringio_update(wheel, message.data);
-		}
-	}
-}

Core/Src/torque.c

@@ -17,7 +17,6 @@
 #include "serial_monitor.h"
 #include "state_machine.h"
 #include "torque.h"
-#include <math.h>
 #include <string.h>
 #include <stdio.h>
 #include "bms.h"
@@ -40,17 +39,6 @@ static void linear_accel_to_torque(float accel)
 	dti_set_torque(torque);
 }
 
-static float rpm_to_mph(int32_t rpm)
-{
-	/* Convert RPM to MPH */
-	// rpm * gear ratio = wheel rpm
-	// tire diamter (in) to miles --> tire diamter miles
-	// wheel rpm * 60 --> wheel rph
-	// tire diamter miles * pi --> tire circumference
-	// rph * wheel circumference miles --> mph
-	return (rpm / (GEAR_RATIO)) * 60 * (TIRE_DIAMETER / 63360.0) * M_PI;
-}
-
 // static void limit_accel_to_torque(float mph, float accel, uint16_t* torque)
 // {
 // 		static uint16_t torque_accumulator[ACCUMULATOR_SIZE];
@@ -191,8 +179,7 @@ void vCalcTorque(void *pv_params)
 
 		/* If we receive a new message within the time frame, calc new torque */
 		if (stat == osOK) {
-			int32_t rpm = dti_get_rpm(mc);
-			mph = rpm_to_mph(rpm);
+			mph = dti_get_mph(mc);
 			set_mph(mph);
 
 			func_state_t func_state = get_func_state();