Feature/ControlSystem

Core/Inc/can_handler.h

@@ -3,6 +3,13 @@
 
 #include "can.h"
 #include "cmsis_os.h"
+#include "control.h"
+#include "dti.h"
+
+typedef struct {
+	dti_t *mc;
+	control_t *control;
+} can_receive_t;
 
 /**
  * @brief Callback to be called when a message is received on CAN line 1.

Core/Inc/control.h

@@ -0,0 +1,39 @@
+#ifndef CONTROL_H
+#define CONTROL_H
+
+#include "pdu.h"
+#include "can.h"
+
+#define CONTROL_CANID_FANBATTBOX 0x4A1
+#define CONTROL_CANID_PUMP	 0x4A0
+
+#define MOTOR_TEMP_LIMIT 50
+
+extern osThreadId_t control_handle;
+extern const osThreadAttr_t control_attributes;
+
+typedef struct {
+	bool fanBattBoxState;
+	bool pumpState0;
+	bool pumpState1;
+} control_t;
+
+typedef struct {
+	pdu_t *pdu;
+	control_t *control;
+} control_args_t;
+
+typedef struct {
+	bool state;
+	control_t *control;
+} set_pump_state_t;
+
+void vControl(void *param);
+
+void setPumpState(void *params);
+
+void control_fanbattbox_record(control_t *control, can_msg_t msg);
+
+void control_pump_record(control_t *control, can_msg_t msg);
+
+#endif

Core/Inc/dti.h

@@ -12,7 +12,8 @@
 #ifndef DTI_H
 #define DTI_H
 
-#include "can_handler.h"
+#include "can.h"
+#include "cmsis_os.h"
 
 /* Message IDs from DTI CAN Datasheet */
 #define DTI_CANID_ERPM	      0x416 /* ERPM, Duty, Input Voltage */
@@ -131,6 +132,19 @@ void dti_set_relative_current(int16_t relative_current);
  */
 void dti_set_drive_enable(bool drive_enable);
 
+/**
+ * @brief Record the moter temperature in the DTI
+ * 
+ * @param mc Pointer to DTI struct 
+ * @param msg CAN message containing temperature data
+ */
+void dti_record_motor_temp(dti_t *mc, can_msg_t msg);
+
+/**
+ * @brief gets the current motor temperature from the DTI
+ */
+uint16_t dti_get_motor_temp();
+
 /**
  * @brief gets the current mph from the DTI
  */

Core/Src/can_handler.c

@@ -9,6 +9,7 @@
 #include "dti.h"
 #include "fault.h"
 #include "steeringio.h"
+#include "control.h"
 
 #define CAN_MSG_QUEUE_SIZE 50 /* messages */
 
@@ -22,8 +23,8 @@ static osMessageQueueId_t can_inbound_queue;
 can_t *can1;
 
 /* Relevant Info for Initializing CAN 1 */
-static uint32_t id_list[] = { DTI_CANID_ERPM, DTI_CANID_CURRENTS, BMS_DCL_MSG,
-			      STEERING_CANID_IO };
+static uint32_t id_list[] = { DTI_CANID_ERPM, CONTROL_CANID_FANBATTBOX,
+			      BMS_DCL_MSG, STEERING_CANID_IO };
 
 void init_can1(CAN_HandleTypeDef *hcan)
 {
@@ -132,8 +133,8 @@ const osThreadAttr_t can_receive_attributes = {
 
 void vCanReceive(void *pv_params)
 {
-	dti_t *mc = (dti_t *)pv_params;
-	assert(mc);
+	can_receive_t *can_receive = (can_receive_t *)pv_params;
+	assert(can_receive);
 
 	can_msg_t msg;
 
@@ -145,14 +146,23 @@ void vCanReceive(void *pv_params)
 			switch (msg.id) {
 			/* Messages Relevant to Motor Controller */
 			case DTI_CANID_ERPM:
-				dti_record_rpm(mc, msg);
+				dti_record_rpm(can_receive->mc, msg);
+				break;
+			case DTI_CANID_TEMPS_FAULT:
+				dti_record_motor_temp(can_receive->mc, msg);
 				break;
 			case BMS_DCL_MSG:
 				handle_dcl_msg();
 				break;
 			case STEERING_CANID_IO:
 				steeringio_update(msg);
 				break;
+			case CONTROL_CANID_FANBATTBOX:
+				control_fanbattbox_record(can_receive->control,
+							  msg);
+				break;
+			case CONTROL_CANID_PUMP:
+				control_pump_record(can_receive->control, msg);
 			default:
 				break;
 			}

Core/Src/control.c

@@ -0,0 +1,62 @@
+#include <stdlib.h>
+
+#include "control.h"
+#include "cerb_utils.h"
+#include "dti.h"
+
+osThreadId_t control_handle;
+const osThreadAttr_t control_attributes = {
+	.name = "Control",
+	.stack_size = 128 * 8,
+	.priority = (osPriority_t)osPriorityRealtime,
+};
+
+void vControl(void *params)
+{
+	control_args_t *control_args = (control_args_t *)params;
+	control_t *control = control_args->control;
+
+	nertimer_t pumpTimer;
+
+	set_pump_state_t *set_pump = malloc(sizeof(set_pump_state_t));
+	set_pump->control = control;
+
+	for (;;) {
+		write_fan_battbox(control_args->pdu, control->fanBattBoxState);
+
+		uint16_t motorTemp = dti_get_motor_temp();
+
+		if (motorTemp > MOTOR_TEMP_LIMIT) {
+			set_pump->state = 1;
+			debounce(motorTemp > MOTOR_TEMP_LIMIT, &pumpTimer,
+				 10000, &setPumpState, &set_pump);
+		} else {
+			set_pump->state = 0;
+			debounce(motorTemp <= MOTOR_TEMP_LIMIT, &pumpTimer,
+				 10000, &setPumpState, &set_pump);
+		}
+
+		write_pump_0(control_args->pdu, control->pumpState0);
+		write_pump_1(control_args->pdu, control->pumpState1);
+
+		osDelay(1000);
+	}
+}
+
+void setPumpState(void *params)
+{
+	set_pump_state_t *set_pump_state = (set_pump_state_t *)params;
+	set_pump_state->control->pumpState0 = set_pump_state->state;
+	set_pump_state->control->pumpState1 = set_pump_state->state;
+}
+
+void control_fanbattbox_record(control_t *control, can_msg_t msg)
+{
+	control->fanBattBoxState = msg.data[0] > 0;
+}
+
+void control_pump_record(control_t *control, can_msg_t msg)
+{
+	control->pumpState0 = msg.data[0] > 0;
+	control->pumpState1 = msg.data[1] > 0;
+}

Core/Src/dti.c

@@ -17,13 +17,16 @@
 
 #include "emrax.h"
 #include "bms.h"
+#include "can_handler.h"
 
 #define CAN_QUEUE_SIZE 5 /* messages */
 #define SAMPLES	       20
 static osMutexAttr_t dti_mutex_attributes;
 
 static uint8_t mph = 0;
 
+static uint8_t motorTemp = 0;
+
 dti_t *dti_init()
 {
 	dti_t *mc = malloc(sizeof(dti_t));
@@ -284,6 +287,23 @@ void dti_record_rpm(dti_t *mc, can_msg_t msg)
 	mph = dti_get_mph(mc);
 }
 
+void dti_record_motor_temp(dti_t *mc, can_msg_t msg)
+{
+	uint16_t temp = (msg.data[0] << 8) + (msg.data[1]);
+
+	temp /= 10;
+
+	osMutexAcquire(*mc->mutex, osWaitForever);
+	mc->motor_temp = temp;
+	osMutexRelease(*mc->mutex);
+	motorTemp = temp;
+}
+
+uint16_t dti_get_motor_temp()
+{
+	return motorTemp;
+}
+
 uint8_t get_mph()
 {
 	return mph;

Core/Src/fault.c

@@ -7,6 +7,7 @@
 
 #include "cerberus_conf.h"
 #include "state_machine.h"
+#include "can_handler.h"
 
 #define FAULT_HANDLE_QUEUE_SIZE 16
 #define NUM_OF_FAULTS		18UL

Core/Src/main.c

@@ -37,6 +37,7 @@
 #include "dti.h"
 #include "steeringio.h"
 #include "pedals.h"
+#include "control.h"
 #include "monitor.h"
 #include "state_machine.h"
 /* USER CODE END Includes */
@@ -226,10 +227,24 @@ int main(void)
   // shutdown_monitor_handle = osThreadNew(vShutdownMonitor, pdu, &shutdown_monitor_attributes);
   // assert(shutdown_monitor_handle);
 
+  /* Control File Thread */
+  control_args_t *control_args = malloc(sizeof(control_args_t));
+  control_args->pdu = pdu;
+  control_args->control = malloc(sizeof(control_t));
+  control_args->control->fanBattBoxState = 0;
+  control_args->control->pumpState0 = 0;
+  control_args->control->pumpState1 = 0;
+  control_handle = osThreadNew(vControl, control_args, &control_attributes);
+  assert(control_handle);
+
   /* Messaging */
   can_dispatch_handle = osThreadNew(vCanDispatch, &hcan1, &can_dispatch_attributes);
   assert(can_dispatch_handle);
-  can_receive_thread = osThreadNew(vCanReceive, mc, &can_receive_attributes);
+
+  can_receive_t *can_receive = malloc(sizeof(can_receive));
+  can_receive->mc = mc;
+  can_receive->control = control_args->control;
+  can_receive_thread = osThreadNew(vCanReceive, can_receive, &can_receive_attributes);
   assert(can_receive_thread);
 
   /* Control Logic */
@@ -252,6 +267,7 @@ int main(void)
   sm_args->mpu = mpu;
   sm_director_handle = osThreadNew(vStateMachineDirector, sm_args, &sm_director_attributes);
   assert(sm_director_handle);
+
   /* USER CODE END RTOS_THREADS */
 
   /* USER CODE BEGIN RTOS_EVENTS */
@@ -828,7 +844,7 @@ void StartDefaultTask(void *argument)
     // refresh the external watchdog so the car doesnt fault
     pet_watchdog(mpu);
 
-    
+	
     /* Send NERO state data continuously */
     send_git_version_message();
     osDelay(500);

Core/Src/monitor.c

@@ -9,6 +9,7 @@
 #include "cerberus_conf.h"
 #include "fault.h"
 #include "state_machine.h"
+#include "can_handler.h"
 
 #define TSMS_DEBOUNCE_PERIOD 500 /* ms */
 

Core/Src/state_machine.c

@@ -8,6 +8,7 @@
 #include "cerb_utils.h"
 #include "monitor.h"
 #include "pedals.h"
+#include "can_handler.h"
 
 #define STATE_TRANS_QUEUE_SIZE 4
 

Makefile

@@ -82,6 +82,7 @@ Core/Src/state_machine.c \
 Core/Src/bms.c \
 Core/Src/pdu.c \
 Core/Src/mpu.c \
+Core/Src/control.c \
 Core/Src/steeringio.c \
 Core/Src/pedals.c \
 Core/Src/cerb_utils.c \