Regen Braking (#188)

* switch to pca9539 (untested) (#175)

* switch to pca9539 (untested)

* update pdu to improved api

* fix addr

* new reg name

* disable shutdown

* removed debugging stuff

* bump

---------

Co-authored-by: Scott A <[email protected]>

* Feature/pedal tune (#180)

* prepped for pedal tuning + added break check when going to drive mode

* added Scott's implementation for bspd pre check

* added defcon 5 fault

* fuck

* remoced torque send

* pedals calibrated

* fix bit

* im stupid

* new pedals

* added debug

* poop farty

* Auto stash before merge of "feature/pedal-tune" and "origin/feature/pedal-tune"

* move tsms status

* reset queuue

* fixed stuff

---------

Co-authored-by: Jack Rubacha <[email protected]>

* Finally get mph working! (#183)

* dti back and mph calcs

* queue fixes and got a backwards erpm reading

* use correct units

* fix up mph, tested recieve and send, not validated connection

---------

Co-authored-by: Scott A <[email protected]>

* Monitor fixes (#181)

* add lv monitor again

* remove bad line

* fix missed line in rebase

* fix up math

* regen fix issue

* add dti router, disable preemption

* regen breaking ported from 17A, get voltage from DTI

* it builds

* new regen function, get ccl from bms, brake consts, adj_pedal_val func

* fixing some calcs, went back to torque pass by reference

* regen only when car is moving, adjust range of max regen, documetation

* ccl to little endian

* rewriting entire codebase in rust

* misc fixes

* comment out torque accel limit

* fix send torque

* fix send torque

* add accel, make vital improvements

* Bms fault fixes (#182)

* fix bms fault, get rid of write fan battbox

* add note

* add torque.py

* add max brake ac current, new accel pedal algo, makes forward accel more sensitive

* fix bspd prefault

* adjust value for proper scale

* fix mph, bspd prefault

* fix calcs

* fix nero

* more torque work

* Accel pedal regen cleanup, dti_set_brake_current adjusted to use current instead of torque, cleanup and docs, bumb embedded base

* override tsms, ccl fixed, adjusting torque and regen calcs

* messages successfully sent

* acceleration is steps of 1% travel

* fixed commands to not overwrite eachother

* regen working, comment out bspd prefault

* formatting

* merge failures fixed

* remove garbage and merge garbage, change send_regen_braking function signature, add docs

* remove bad comments, change func signature to uint16, edit documentation

* debugging, changing more func signatures

* change DTI functions to not use floats

* formatting

---------

Co-authored-by: Jack Rubacha <[email protected]>
Co-authored-by: dyldonahue <[email protected]>

Core/Inc/bms.h

@@ -4,11 +4,13 @@
 
 #include "cmsis_os.h"
 
-#define BMS_DCL_MSG 0x156 /*BMS MONITOR WATCHDOG*/
+#define BMS_DCL_MSG	   0x156 /*BMS MONITOR WATCHDOG*/
+#define BMS_CURR_LIMIT_MSG 0x176
 
 typedef struct {
 	osTimerId bms_monitor_timer;
 	uint16_t dcl;
+	uint16_t ccl;
 } bms_t;
 
 extern bms_t *bms;

Core/Inc/cerberus_conf.h

@@ -6,10 +6,19 @@
 #define SHUTDOWN_MONITOR_DELAY 500 /* ms */
 #define NERO_DELAY_TIME	       100 /* ms*/
 #define LV_READ_DELAY	       1000
+#define YELLOW_LED_BLINK_DELAY 500 /* ms */
+#define TEMP_SENS_SAMPLE_DELAY 200 /* ms */
+#define IMU_SAMPLE_DELAY       500 /* ms */
+#define FUSES_SAMPLE_DELAY     1000 /* ms */
+#define SHUTDOWN_MONITOR_DELAY 500 /* ms */
+#define NERO_DELAY_TIME	       100 /* ms*/
+#define LV_READ_DELAY	       1000
 #define SERIAL_MONITOR_DELAY
 #define CAN_ROUTER_DELAY
 #define CAN_DISPATCH_DELAY    5
 #define BMS_CAN_MONITOR_DELAY 4000
+#define CAN_DISPATCH_DELAY    5
+#define BMS_CAN_MONITOR_DELAY 4000
 #define STATE_MACHINE_DELAY
 #define TORQUE_CALC_DELAY
 #define FAULT_HANDLE_DELAY
@@ -23,7 +32,7 @@
 #define PEDAL_BRAKE_THRESH  650
 
 /* Torque Tuning */
-#define MAX_TORQUE 220 /* Nm * 10 */
+#define MAX_TORQUE 220 /* Nm */
 
 // DEBUGGING: Start with a high debounce period, lower it if it is too slow
 // NOTE: SteeringIOMonitor updates every 25 ms. Previous value of 25 ms doesn't

Core/Inc/dti.h

@@ -19,12 +19,22 @@
 /* Message IDs from DTI CAN Datasheet */
 #define DTI_CANID_ERPM	      0x416 /* ERPM, Duty, Input Voltage */
 #define DTI_CANID_CURRENTS    0x436 /* AC Current, DC Current */
+#define DTI_CANID_ERPM	      0x416 /* ERPM, Duty, Input Voltage */
+#define DTI_CANID_CURRENTS    0x436 /* AC Current, DC Current */
 #define DTI_CANID_TEMPS_FAULT 0x456 /* Controller Temp, Motor Temp, Faults */
 #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 TIRE_DIAMETER	16 /* inches */
+#define GEAR_RATIO	47 / 13.0 /* unitless */
+#define POLE_PAIRS	10 /* unitless */
+#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 TIRE_DIAMETER 16 /* inches */
 #define GEAR_RATIO    47 / 13.0 /* unitless */
 #define POLE_PAIRS    10 /* unitless */
@@ -47,6 +57,8 @@ typedef struct {
 // TODO: Expand GET interface
 int32_t dti_get_rpm(dti_t *dti);
 
+uint16_t dti_get_input_voltage(dti_t *dti);
+
 /* Utilities for Decoding CAN message */
 extern osThreadId_t dti_router_handle;
 extern const osThreadAttr_t dti_router_attributes;
@@ -61,14 +73,19 @@ dti_t *dti_init();
  */
 void dti_set_torque(int16_t torque);
 
-// TODO: Regen interface
-// void dti_set_regen(int16_t regen);
+/**
+ * @brief Set the brake AC current target for regenerative braking. Only positive values are accepted by the DTI.
+ * 
+ * @param current_target The desired AC current to do regenerative braking at. Must be positive. This argument must be the actual value to set multiplied by 10.
+ */
+void dti_set_regen(uint16_t current_target);
 
-/*
- * SCALE: 10
- * UNITS: Amps
+/**
+ * @brief Send a CAN message containing the AC current target for regenerative braking.
+ * 
+ * @param brake_current AC current target for regenerative braking. The actual value sent to the motor controller must be multiplied by 10.
  */
-void dti_set_brake_current(int16_t brake_current);
+void dti_send_brake_current(uint16_t brake_current);
 
 /*
  * SCALE: 10

Core/Src/bms.c

@@ -56,12 +56,8 @@ void vBMSCANMonitor(void *pv_params)
 		if (osOK == osMessageQueueGet(bms_monitor_queue,
 					      &msg_from_queue, NULL,
 					      osWaitForever)) {
-			/*TO-DO: fix duration (ticks)*/
 			osTimerStart(bms->bms_monitor_timer,
 				     BMS_CAN_MONITOR_DELAY);
-			bms->dcl = (uint16_t)((msg_from_queue.data[1] << 8) &
-					      msg_from_queue.data[0]);
-			//serial_print("BMS DCL %d", bms->dcl);
 		}
 	}
 }

Core/Src/dti.c

@@ -24,7 +24,6 @@
 #define CAN_QUEUE_SIZE 5 /* messages */
 #define SAMPLES	       20
 static osMutexAttr_t dti_mutex_attributes;
-// osMessageQueueId_t dti_router_queue;
 
 dti_t *dti_init()
 {
@@ -64,11 +63,35 @@ void dti_set_torque(int16_t torque)
 
 	int16_t ac_current = (((float)average / EMRAX_KT) * 10); /* times 10 */
 
-	//serial_print("Commanded Current: %d \r\n", ac_current);
+	// serial_print("Commanded Current: %d \r\n", ac_current);
 
 	dti_set_current(ac_current);
 }
 
+void dti_set_regen(uint16_t current_target)
+{
+	/* Simple moving average to smooth change in braking target */
+
+	// Static variables for the buffer and index
+	static uint16_t buffer[SAMPLES] = { 0 };
+	static int index = 0;
+
+	// Add the new value to the buffer
+	buffer[index] = current_target;
+
+	// Increment the index, wrapping around if necessary
+	index = (index + 1) % SAMPLES;
+
+	// Calculate the average of the buffer
+	uint16_t sum = 0;
+	for (int i = 0; i < SAMPLES; ++i) {
+		sum += buffer[i];
+	}
+	uint16_t average = sum / SAMPLES;
+
+	dti_send_brake_current(average);
+}
+
 void dti_set_current(int16_t current)
 {
 	can_msg_t msg = { .id = 0x036, .len = 2, .data = { 0 } };
@@ -77,8 +100,6 @@ void dti_set_current(int16_t current)
 
 	//endian_swap(&current, sizeof(current));
 	//memcpy(msg.data, &current, msg.len);
-	serial_print("current: %d\r\n", current);
-
 	int8_t msb = (int8_t)((current >> 8) & 0xFF);
 	int8_t lsb = (uint8_t)(current & 0xFF);
 
@@ -88,14 +109,17 @@ void dti_set_current(int16_t current)
 	queue_can_msg(msg);
 }
 
-void dti_set_brake_current(int16_t brake_current)
+void dti_send_brake_current(uint16_t brake_current)
 {
-	can_msg_t msg = { .id = 0x056, .len = 2, .data = { 0 } };
+	can_msg_t msg = { .id = 0x056,
+			  .len = 8,
+			  .data = { 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0 } };
 
 	/* convert to big endian */
 	endian_swap(&brake_current, sizeof(brake_current));
+
 	/* Send CAN message */
-	memcpy(&msg.data, &brake_current, msg.len);
+	memcpy(&msg.data, &brake_current, 2);
 	queue_can_msg(msg);
 }
 

Core/Src/monitor.c

@@ -56,7 +56,6 @@ void vLVMonitor(void *pv_params)
 		// get final voltage
 		v_int = (uint32_t)(v_dec * 10.0);
 
-		// endian_swap(&v_int, sizeof(v_int));
 		memcpy(msg.data, &v_int, msg.len);
 		if (queue_can_msg(msg)) {
 			fault_data.diag =
@@ -196,10 +195,23 @@ 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);
+}
+
 void vPedalsMonitor(void *pv_params)
 {
 	const uint8_t num_samples = 10;
-	static const uint8_t buffer_size = 10;
 	enum { ACCELPIN_2, ACCELPIN_1, BRAKEPIN_1, BRAKEPIN_2 };
 
 	static pedals_t sensor_data;
@@ -226,7 +238,6 @@ void vPedalsMonitor(void *pv_params)
 	mpu_t *mpu = (mpu_t *)pv_params;
 
 	uint8_t counter = 0;
-	static int index = 0;
 
 	for (;;) {
 		read_pedals(mpu, adc_data);
@@ -235,69 +246,30 @@ void vPedalsMonitor(void *pv_params)
 		eval_pedal_fault(adc_data[ACCELPIN_1], adc_data[ACCELPIN_2],
 				 &diff_timer_accelerator, &sc_timer_accelerator,
 				 &oc_timer_accelerator, &fault_data);
-		// eval_pedal_fault(adc_data[BRAKEPIN_1], adc_data[BRAKEPIN_1], &diff_timer_brake, &sc_timer_brake, &oc_timer_brake, &fault_data);
 
 		/* Offset adjusted per pedal sensor, clamp to be above 0 */
-		uint16_t accel_val1 =
-			(int16_t)adc_data[ACCELPIN_1] - ACCEL1_OFFSET <= 0 ?
-				0 :
-				(uint16_t)(adc_data[ACCELPIN_1] -
-					   ACCEL1_OFFSET) *
-					100 / (ACCEL1_MAX_VAL - ACCEL1_OFFSET);
-		// printf("Accel 1: %d\r\n", max_pedal1);
-		uint16_t accel_val2 =
-			(int16_t)adc_data[ACCELPIN_2] - ACCEL2_OFFSET <= 0 ?
-				0 :
-				(uint16_t)(adc_data[ACCELPIN_2] -
-					   ACCEL2_OFFSET) *
-					100 / (ACCEL2_MAX_VAL - ACCEL2_OFFSET);
-		// printf("Accel 2: %d\r\n",max_pedal2);
+		uint16_t accel_val1 = adjust_pedal_val(
+			adc_data[ACCELPIN_1], ACCEL1_OFFSET, ACCEL1_MAX_VAL);
+		uint16_t accel_val2 = adjust_pedal_val(
+			adc_data[ACCELPIN_2], ACCEL2_OFFSET, ACCEL2_MAX_VAL);
 
 		uint16_t accel_val = (uint16_t)(accel_val1 + accel_val2) / 2;
-		// printf("Avg Pedal Val: %d\r\n\n", accel_val);
-
-		/* Raw ADC for tuning */
-		// printf("Accel 1: %ld\r\n", adc_data[ACCELPIN_1]);
-		// printf("Accel 2: %ld\r\n", adc_data[ACCELPIN_2]);
-
-		/* Brakelight Control */
-		// printf("Brake 1: %ld\r\n", adc_data[BRAKEPIN_1]);
-		// printf("Brake 2: %ld\r\n", adc_data[BRAKEPIN_2]);
-
-		static float buffer[10] = { 0 };
 
-		uint16_t brake_avg =
-			(adc_data[BRAKEPIN_1] + adc_data[BRAKEPIN_2]) / 2;
-		// Add the new value to the buffer
-		buffer[index] = brake_avg;
-
-		// Increment the index, wrapping around if necessary
-		index = (index + 1) % buffer_size;
-
-		// Calculate the average of the buffer
-		float sum = 0.0;
-		for (int i = 0; i < buffer_size; ++i) {
-			sum += buffer[i];
-		}
-		float average_brake = sum / buffer_size;
-
-		is_braking = average_brake > PEDAL_BRAKE_THRESH;
+		is_braking = ((adc_data[BRAKEPIN_1] + adc_data[BRAKEPIN_2]) /
+			      2) > PEDAL_BRAKE_THRESH;
 		brake_state = is_braking;
 
 		osMessageQueuePut(brakelight_signal, &is_braking, 0U, 0U);
-		// osMessageQueueReset(break_state_queue);
-		// osMessageQueuePut(break_state_queue, &is_braking, 0U, 0U);
 
 		/* Low Pass Filter */
 		sensor_data.accelerator_value =
-			(sensor_data.accelerator_value + (accel_val)) /
-			num_samples;
+			(sensor_data.accelerator_value + (accel_val)) / 2;
 		sensor_data.brake_value =
-			average_brake /
-			10.0; // still divide by 10 since we multiple by 10 on other end
+			(sensor_data.brake_value +
+			 (adc_data[BRAKEPIN_1] + adc_data[BRAKEPIN_2]) / 2) /
+			num_samples;
 
 		/* Publish to Onboard Pedals Queue */
-		// printf("Accel pedal queue %d",  sensor_data.accelerator_value);
 		osStatus_t check = osMessageQueuePut(pedal_data_queue,
 						     &sensor_data, 0U, 0U);
 
@@ -429,8 +401,6 @@ void vTsmsMonitor(void *pv_params)
 	for (;;) {
 		/* If we got a reliable TSMS reading, handle transition to and out of ACTIVE*/
 		if (!read_tsms_sense(pdu, &tsms_status)) {
-			printf("Checking pdu");
-
 			// Timer has not been started, and there is a change in TSMS, so start the timer
 			if (tsms != tsms_status &&
 			    !is_timer_active(&tsms_debounce_timer)) {
@@ -477,6 +447,8 @@ void vFusingMonitor(void *pv_params)
 		uint8_t fuse_2;
 	} fuse_data;
 
+	bool tsms_status = false;
+
 	for (;;) {
 		fuse_buf = 0;
 
@@ -506,6 +478,14 @@ 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();
+			}
+		}
+
 		osDelay(FUSES_SAMPLE_DELAY);
 	}
 }

Core/Src/state_machine.c

@@ -6,7 +6,9 @@
 #include "queues.h"
 #include "monitor.h"
 #include "serial_monitor.h"
-#include <assert.h>
+#include "nero.h"
+#include "pdu.h"
+#include "queues.h"
 #include <stdbool.h>
 #include <stdio.h>
 
@@ -91,7 +93,6 @@ static int transition_drive_state(drive_state_t new_state)
 		if (!get_brake_state()) {
 			return 0;
 		}
-		serial_print("CALLING RTDS");
 		sound_rtds(pdu);
 	}
 

Core/Src/steeringio.c

@@ -106,7 +106,6 @@ static void debounce_cb(void *arg)
 	steeringio_t *wheel = args->wheel;
 
 	if (wheel->raw_buttons[button]) {
-		serial_print("%d index pressed \r\n", button);
 		switch (button) {
 		case STEERING_PADDLE_LEFT:
 			paddle_left_cb();
@@ -198,7 +197,6 @@ void vSteeringIORouter(void *pv_params)
 		status = osMessageQueueGet(steeringio_router_queue, &message,
 					   NULL, osWaitForever);
 		if (status == osOK) {
-			printf("joe mama");
 			steeringio_update(wheel, message.data);
 		}
 	}