tested

Author: Peyton-McKee

Core/Inc/cerberus_conf.h

@@ -21,12 +21,17 @@
 #define FAULT_HANDLE_DELAY
 
 /* Pedal tuning */
-#define PEDALS_SAMPLE_DELAY 10 /* ms */
-#define MAX_APPS1_VOLTS	    3.02 /* volts */
-#define MIN_APPS1_VOLTS	    1.75 /* volts */
-#define MAX_APPS2_VOLTS	    2.28 /* volts */
-#define MIN_APPS2_VOLTS	    1.01 /* volts */
-#define PEDAL_BRAKE_THRESH  0.12 /* percentage */
+#define PEDALS_SAMPLE_DELAY	10 /* ms */
+#define MAX_APPS1_VOLTS		3.03 /* volts */
+#define MIN_APPS1_VOLTS		1.81 /* volts */
+#define MAX_APPS2_VOLTS		2.28 /* volts */
+#define MIN_APPS2_VOLTS		1.07 /* volts */
+#define PEDAL_BRAKE_THRESH	0.20 /* percentage */
+#define PEDAL_HARD_BRAKE_THRESH 0.50 /* percentage */
+
+/* brake sensor thresholds */
+#define BRAKE_SENSOR_IRREGULAR_HIGH 4.5
+#define BRAKE_SENSOR_IRREGULAR_LOW  0.5
 
 /* Torque Tuning */
 #define MAX_TORQUE 220 /* Nm */
@@ -36,7 +41,7 @@
 #define ACCELERATION_THRESHOLD 0.25
 
 /* Maximum AC braking current */
-#define MAX_REGEN_CURRENT 300
+#define MAX_REGEN_CURRENT 250
 
 #define STEERING_WHEEL_DEBOUNCE 10 /* ms */
 

Core/Inc/dti.h

@@ -154,4 +154,14 @@ osStatus_t dti_get_motor_temp(dti_t *mc, uint16_t *motorTemp);
  */
 osStatus_t dti_get_controller_temp(dti_t *mc, uint16_t *controllerTemp);
 
+/**
+ * @brief Record the currents from DTI
+ *
+ * @param mc Pointer to DTI struct
+ * @param msg CAN message containing currents data
+ */
+osStatus_t dti_record_currents(dti_t *mc, can_msg_t msg);
+
+osStatus_t dti_get_dc_current(dti_t *mc, int16_t *dc_current);
+
 #endif

Core/Src/can_handler.c

@@ -33,7 +33,7 @@ static uint16_t id_list_1[4] = {
 static uint16_t id_list_2[4] = { DIAL_CANID_IO, CONTROL_CANID_FANBATTBOX,
 				 CONTROL_CANID_PUMP, CONTROL_CANID_RADFAN };
 
-static uint16_t id_list_3[4] = { BMS_CANID_CELL_TEMPS };
+static uint16_t id_list_3[4] = { BMS_CANID_CELL_TEMPS, DTI_CANID_CURRENTS };
 
 void init_can1(CAN_HandleTypeDef *hcan)
 {
@@ -187,6 +187,8 @@ void vCanReceive(void *pv_params)
 				break;
 			case CONTROL_CANID_RADFAN:
 				control_radfan_record(msg);
+			case DTI_CANID_CURRENTS:
+				dti_record_currents(mc, msg);
 			default:
 				break;
 			}

Core/Src/dti.c

@@ -335,3 +335,29 @@ osStatus_t dti_get_controller_temp(dti_t *mc, uint16_t *controllerTemp)
 
 	return osMutexRelease(mc->mutex);
 }
+
+osStatus_t dti_record_currents(dti_t *mc, can_msg_t msg)
+{
+	osStatus_t stat = osMutexAcquire(mc->mutex, osWaitForever);
+	if (stat)
+		return stat;
+
+	int16_t ac_current = (msg.data[0] << 8) + (msg.data[1]) / 10;
+	int16_t dc_current = (msg.data[2] << 8) + (msg.data[3]) / 10;
+
+	mc->ac_current = ac_current;
+	mc->dc_current = dc_current;
+
+	return osMutexRelease(mc->mutex);
+}
+
+osStatus_t dti_get_dc_current(dti_t *mc, int16_t *dc_current)
+{
+	osStatus_t stat = osMutexAcquire(mc->mutex, osWaitForever);
+	if (stat)
+		return stat;
+
+	memcpy(dc_current, &mc->dc_current, sizeof(mc->dc_current));
+
+	return osMutexRelease(mc->mutex);
+}

Core/Src/fault.c

@@ -42,8 +42,6 @@ static void clear_fault(void *args)
 	    *fault_id < MAX_CRITICAL_FAULT) {
 		set_ready_mode();
 	}
-
-	free(fault_id);
 }
 
 /**

Core/Src/pdu.c

@@ -188,10 +188,10 @@ pdu_t *init_pdu(I2C_HandleTypeDef *hi2c, ADC_HandleTypeDef *pump_sensors_adc)
 	/* Initialize Pumps Current Sensor */
 	pdu->pumps_current_sensor = malloc(sizeof(ina226_t));
 	assert(pdu->pumps_current_sensor);
-	if (init_ina(pdu, pdu->pumps_current_sensor, PUMPS_CURRENT_SENSOR_ADDR,
-		     0.01f, 2.0f)) {
-		return NULL;
-	}
+	// if (init_ina(pdu, pdu->pumps_current_sensor, PUMPS_CURRENT_SENSOR_ADDR,
+	// 	     0.01f, 2.0f)) {
+	// 	return NULL;
+	// }
 
 	/* Initialize LV Boards Current Sensor */
 	pdu->lv_boards_current_sensor = malloc(sizeof(ina226_t));
@@ -546,12 +546,11 @@ int8_t read_all_current(pdu_t *pdu, float *motor_controller_current,
 {
 	// setting to 0 since current sensor is nonfunctional
 	*battbox_fans_current = 0;
+	*pumps_current = 0;
 
 	if (read_current(pdu, pdu->motor_controller_current_sensor,
 			 motor_controller_current))
 		return -1;
-	if (read_current(pdu, pdu->pumps_current_sensor, pumps_current))
-		return -1;
 	if (read_current(pdu, pdu->lv_boards_current_sensor, lv_boards_current))
 		return -1;
 	return 0;

Core/Src/pedals.c

@@ -43,9 +43,11 @@ static bool launch_control_enabled = false;
 #define MAX_VOLTS_UNSCALED 5.0
 
 #define PEDAL_DIFF_THRESH 0.20 /* percentage */
-#define PEDAL_FAULT_TIME  90 /* ms */
+#define PEDAL_FAULT_TIME  95 /* ms */
+#define BRAKE_FAULT_TIME  300 /* ms */
 
-#define APPS_THRESHOLD_BUF 0.45
+#define APPS_THRESHOLD_BUF  0.45
+#define BRAKE_THRESHOLD_BUF 0.25
 enum { ACCELPIN_1, ACCELPIN_2, BRAKEPIN_1, BRAKEPIN_2 };
 
 /* Factor for converting MPH to KMH */
@@ -254,6 +256,44 @@ bool calc_pedal_faults(float accel1, float accel2, float accel1_norm,
 	return false;
 }
 
+/**
+ * @brief Determine if there has been a brake sensor fault
+ * 
+ * @param brake1 sensor 1 voltage reading (out of 5V)
+ * @param accel2 sensor 2 voltage reading (out of 5V)
+ */
+bool calc_brake_faults(float brake1, float brake2)
+{
+	/* oc = Open Circuit */
+	static nertimer_t oc_fault_timer;
+
+	/* sc = Short Circuit */
+	static nertimer_t sc_fault_timer;
+
+	/* EV3.5.4: For analog acceleration control signals, this error checking must detect open circuit, short to 
+	ground and short to sensor power. */
+
+	/* Pedal open circuit fault */
+	bool open_circuit =
+		brake1 > BRAKE_SENSOR_IRREGULAR_HIGH + BRAKE_THRESHOLD_BUF ||
+		brake2 > BRAKE_SENSOR_IRREGULAR_HIGH + BRAKE_THRESHOLD_BUF;
+	debounce(open_circuit, &oc_fault_timer, BRAKE_FAULT_TIME,
+		 &pedal_fault_cb, "Brake open circuit fault - max brake value");
+
+	/* Pedal short circuit to gnd */
+	bool short_circuit =
+		brake1 < BRAKE_SENSOR_IRREGULAR_LOW - BRAKE_THRESHOLD_BUF ||
+		brake2 < BRAKE_SENSOR_IRREGULAR_LOW - BRAKE_THRESHOLD_BUF;
+	debounce(short_circuit, &sc_fault_timer, BRAKE_FAULT_TIME,
+		 &pedal_fault_cb,
+		 "Brake grounded circuit fault - 0 brake value");
+
+	if (open_circuit || short_circuit) {
+		return true;
+	}
+	return false;
+}
+
 /**
  * @brief Function to send raw pedal data over CAN.
  * 
@@ -295,20 +335,29 @@ void send_pedal_data(void *arg)
 /**
  * @brief Determine if power to the motor controller should be disabled based on brake and accelerator pedal travel.
  * 
+ * Will queue fault
+ * 
  * @param accel_val Percent travel of the accelerator pedal from 0-1
- * @param brake_val Brake pressure sensor reading
+ * @param brake_val Brake pressure sensor reading, 0-1
  * @return bool True for prefault conditions met, false for no prefault
  */
-bool calc_bspd_prefault(float accel_val, float brake_val)
+bool calc_bspd_prefault(float accel_val, float brake_val, float dc_current)
 {
-	static fault_data_t fault_data = { .fault_id = BSPD_PREFAULT,
-					   .diag = "BSPD prefault triggered" };
+	static const fault_data_t fault_data = {
+		.fault_id = BSPD_PREFAULT, .diag = "BSPD prefault triggered"
+	};
 	static bool motor_disabled = false;
 
 	/* EV.4.7: If brakes are engaged and APPS signals more than 25% pedal travel, disable power
 	to the motor(s). Re-enable when accelerator has less than 5% pedal travel. */
 
-	if (brake_val > PEDAL_BRAKE_THRESH && accel_val > 0.25) {
+	if (brake_val > PEDAL_HARD_BRAKE_THRESH && accel_val > 0.25) {
+		motor_disabled = true;
+		queue_fault(&fault_data);
+	}
+
+	// prevent a fault
+	if (brake_val > PEDAL_HARD_BRAKE_THRESH && dc_current > 10) {
 		motor_disabled = true;
 		queue_fault(&fault_data);
 	}
@@ -596,6 +645,8 @@ void vProcessPedals(void *pv_params)
 
 		float accel1_volts = adc_to_volts(adc_data[ACCELPIN_1]);
 		float accel2_volts = adc_to_volts(adc_data[ACCELPIN_2]);
+		float brake1_volts = adc_to_volts(adc_data[BRAKEPIN_1]);
+		float brake2_volts = adc_to_volts(adc_data[BRAKEPIN_2]);
 
 		// printf("accel1 volts %f\n", accel1_volts);
 		// printf("accel2 volts %f\n", accel2_volts);
@@ -606,8 +657,10 @@ void vProcessPedals(void *pv_params)
 		float accel2_norm = pedal_percent_pressed(
 			accel2_volts, MIN_APPS2_VOLTS, MAX_APPS2_VOLTS);
 
-		bool possible_faults = calc_pedal_faults(
+		bool possible_pedal_faults = calc_pedal_faults(
 			accel1_volts, accel2_volts, accel1_norm, accel2_norm);
+		bool possible_brake_faults =
+			calc_brake_faults(brake1_volts, brake2_volts);
 
 		/* same for brake values */
 		float brake_avg =
@@ -630,10 +683,13 @@ void vProcessPedals(void *pv_params)
 		osMutexRelease(brake_state_mut);
 		write_brakelight(pdu, brake_pressed);
 
-		if (calc_bspd_prefault(accel_value, brake_value)) {
+		int16_t dc_current;
+		dti_get_dc_current(mc, &dc_current);
+
+		if (calc_bspd_prefault(accel_value, brake_value, dc_current)) {
 			/* Prefault triggered */
-			// osDelay(delay_time);
 			// dti_set_torque(0);
+			// osDelay(delay_time);
 			// continue;
 		}
 

Core/Src/state_machine.c

@@ -19,6 +19,7 @@
 
 /* Internal State of Vehicle */
 static state_t cerberus_state;
+static osTimerId_t reverse_sound_timer;
 
 typedef struct {
 	enum { FUNCTIONAL, NERO } id;
@@ -82,6 +83,14 @@ nero_state_t get_nero_state()
 	return cerberus_state.nero;
 }
 
+void sound_reverse_callback(void *pdu)
+{
+	static bool sound = false;
+
+	write_rtds(pdu, sound);
+	sound = !sound;
+}
+
 static int transition_functional_state(func_state_t new_state, pdu_t *pdu,
 				       dti_t *mc, mpu_t *mpu)
 {
@@ -97,6 +106,8 @@ static int transition_functional_state(func_state_t new_state, pdu_t *pdu,
 
 	/* Make sure wheels are not spinning before changing modes */
 	bool brake_state;
+	osTimerStop(reverse_sound_timer);
+	write_rtds(pdu, false);
 
 	/* Catching state transitions */
 	switch (new_state) {
@@ -110,6 +121,7 @@ static int transition_functional_state(func_state_t new_state, pdu_t *pdu,
 		printf("Reverse is disabled.");
 		return 4;
 #endif
+		osTimerStart(reverse_sound_timer, pdMS_TO_TICKS(500));
 	case F_PIT:
 	case F_PERFORMANCE:
 	case F_EFFICIENCY:
@@ -331,6 +343,8 @@ void vStateMachineDirector(void *pv_params)
 
 	/* Write to GPIO expander to set initial state */
 	write_fault(mpu, false);
+	reverse_sound_timer =
+		osTimerNew(sound_reverse_callback, osTimerPeriodic, pdu, NULL);
 
 	for (;;) {
 		if (osMessageQueueGet(state_trans_queue, &new_state_req, NULL,

Core/Src/steeringio.c

@@ -35,25 +35,32 @@ void buttons_update(can_msg_t msg, dti_t *mc)
 		break;
 	case BUTTON_ESC:
 		printf("Esc button pressed \n");
-		set_home_mode();
+		if (fabs(dti_get_mph(mc)) < 1) {
+			set_home_mode();
+		}
 		break;
 	case BUTTON_UP:
 		printf("Up button pressed \n");
-		if (get_func_state() == F_EFFICIENCY || (dti_get_mph(mc) <= 0 && get_func_state() == F_PERFORMANCE)) {
+		if (get_func_state() == F_EFFICIENCY ||
+		    (dti_get_mph(mc) <= 0 && get_brake_state() &&
+		     get_func_state() == F_PERFORMANCE)) {
 			increase_regen_limit();
 		}
 		decrement_nero_index();
 		break;
 	case BUTTON_DOWN:
 		printf("Down button pressed \n");
-		if (get_func_state() == F_EFFICIENCY || (dti_get_mph(mc) <= 0 && get_func_state() == F_PERFORMANCE)) {
+		if (get_func_state() == F_EFFICIENCY ||
+		    (dti_get_mph(mc) <= 0 && get_brake_state() &&
+		     get_func_state() == F_PERFORMANCE)) {
 			decrease_regen_limit();
 		}
 		increment_nero_index();
 		break;
 	case BUTTON_ENTER:
 		printf("Enter button pressed \n");
-		if (get_func_state() == F_PERFORMANCE) {
+		if (get_func_state() == F_PERFORMANCE &&
+		    fabs(dti_get_mph(mc)) < 1) {
 			toggle_launch_control();
 		}
 		select_nero_index();