Implement PDU 24A

Core/Inc/cerberus_conf.h

@@ -75,5 +75,9 @@
 #define CANID_LV_MONITOR       0x503
 #define CANID_PEDALS_ACCEL_MSG 0x504
 #define CANID_PEDALS_BRAKE_MSG 0x505
+#define CANID_MOTOR_CONT_CURR  0x8000
+#define CANID_BATT_FANS_CURR   0xC000
+#define CANID_PUMPS_CURR       0xE000
+#define CANID_LV_BOARDS_CURR   0xF000
 // Reserved for MPU debug message, see yaml for format
 #define CANID_EXTRA_MSG 0x701

Core/Inc/fault.h

@@ -24,6 +24,10 @@ typedef enum {
 	BSPD_PREFAULT = 0x1000,
 	LV_MONITOR_FAULT = 0x2000,
 	RTDS_FAULT = 0x4000,
+	MOTOR_CONTROLLER_CURRENT_FAULT = 0x8000,
+	BATTBOX_FANS_CURRENT_FAULT = 0xC000,
+	PUMPS_CURRENT_FAULT = 0xE000,
+	LV_BOARDS_CURRENT_FAULT = 0xF000,
 	MAX_FAULTS
 } fault_code_t;
 

Core/Inc/pdu.h

@@ -3,6 +3,7 @@
 
 #include "cmsis_os.h"
 #include "pca9539.h"
+#include "INA226.h"
 #include <stdbool.h>
 #include <stdint.h>
 
@@ -13,6 +14,10 @@ typedef struct {
 	osMutexId_t *mutex;
 	pca9539_t *shutdown_expander;
 	pca9539_t *ctrl_expander;
+	ina226_t *motor_controller_current_sensor;
+	ina226_t *battbox_fans_current_sensor;
+	ina226_t *pumps_current_sensor;
+	ina226_t *lv_boards_current_sensor;
 } pdu_t;
 
 /* Creates a new PDU interface */
@@ -81,6 +86,12 @@ typedef enum {
  */
 int8_t read_shutdown(pdu_t *pdu, bool status[MAX_SHUTDOWN_STAGES]);
 
+// Functions for reading current
+int8_t read_motor_controller_current(pdu_t *pdu, float *data);
+int8_t read_battbox_fans_current(pdu_t *pdu, float *data);
+int8_t read_pumps_current(pdu_t *pdu, float *data);
+int8_t read_lv_boards_current(pdu_t *pdu, float *data);
+
 /**
  * @brief Taskf for sounding RTDS.
  * 

Core/Src/monitor.c

@@ -27,6 +27,94 @@
 static bool tsms = false;
 osMutexId_t tsms_mutex;
 
+void read_motor_controller_current_data(pdu_t *pdu)
+{
+	fault_data_t fault_data = { .id = MOTOR_CONTROLLER_CURRENT_FAULT, .severity = DEFCON5 };
+	can_msg_t msg = { .id = CANID_MOTOR_CONT_CURR, .len = 4, .data = { 0 } };
+
+	float current;
+	if(read_motor_controller_current(pdu, &current))
+	{
+		fault_data.diag = "Failed to read motor controller current";
+		queue_fault(&fault_data);
+	}
+
+	uint32_t current_int = (uint32_t)(current * 10000); // idk how precise it needs to be
+
+	memcpy(msg.data, &current_int, msg.len);
+	if(queue_can_msg(msg))
+	{
+		fault_data.diag = "Failed to send motor controller current CAN message";
+		queue_fault(&fault_data);
+	}
+}
+
+void read_battbox_fans_current_data(pdu_t *pdu)
+{
+	fault_data_t fault_data = { .id = BATTBOX_FANS_CURRENT_FAULT, .severity = DEFCON5 };
+	can_msg_t msg = { .id = CANID_BATT_FANS_CURR, .len = 4, .data = { 0 } };
+
+	float current;
+	if(read_battbox_fans_current(pdu, &current))
+	{
+		fault_data.diag = "Failed to read battbox fans current";
+		queue_fault(&fault_data);
+	}
+
+	uint32_t current_int = (uint32_t)(current * 10000); // idk how precise it needs to be
+
+	memcpy(msg.data, &current_int, msg.len);
+	if(queue_can_msg(msg))
+	{
+		fault_data.diag = "Failed to send battbox fans current CAN message";
+		queue_fault(&fault_data);
+	}
+}
+
+void read_pumps_current_data(pdu_t *pdu)
+{
+	fault_data_t fault_data = { .id = PUMPS_CURRENT_FAULT, .severity = DEFCON5 };
+	can_msg_t msg = { .id = CANID_PUMPS_CURR, .len = 4, .data = { 0 } };
+
+	float current;
+	if(read_pumps_current(pdu, &current))
+	{
+		fault_data.diag = "Failed to read pumps current";
+		queue_fault(&fault_data);
+	}
+
+	uint32_t current_int = (uint32_t)(current * 10000); // idk how precise it needs to be
+
+	memcpy(msg.data, &current_int, msg.len);
+	if(queue_can_msg(msg))
+	{
+		fault_data.diag = "Failed to send pumps current CAN message";
+		queue_fault(&fault_data);
+	}
+}
+
+void read_lv_boards_current_data(pdu_t *pdu)
+{
+	fault_data_t fault_data = { .id = LV_BOARDS_CURRENT_FAULT, .severity = DEFCON5 };
+	can_msg_t msg = { .id = CANID_LV_BOARDS_CURR, .len = 4, .data = { 0 } };
+
+	float current;
+	if(read_lv_boards_current(pdu, &current))
+	{
+		fault_data.diag = "Failed to read lv boards current";
+		queue_fault(&fault_data);
+	}
+
+	uint32_t current_int = (uint32_t)(current * 10000); // idk how precise it needs to be
+
+	memcpy(msg.data, &current_int, msg.len);
+	if(queue_can_msg(msg))
+	{
+		fault_data.diag = "Failed to send lv boards current CAN message";
+		queue_fault(&fault_data);
+	}
+}
+
 /**
  * @brief Read the open cell voltage of the LV batteries and send a CAN message with the result.
  */
@@ -120,6 +208,10 @@ void vNonFunctionalDataCollection(void *pv_params)
 	for (;;) {
 		read_lv_sense(mpu);
 		read_fuse_data(pdu);
+		read_motor_controller_current_data(pdu);
+		read_battbox_fans_current_data(pdu);
+		read_pumps_current_data(pdu);
+		read_lv_boards_current_data(pdu);
 
 		/* delay for 1000 ms (1k ticks at 1000 Hz tickrate) */
 		osDelay(1000);

Core/Src/pdu.c

@@ -18,7 +18,39 @@
 #define CTRL_ADDR     PCA_I2C_ADDR_2
 #define RTDS_DURATION 1750 /* ms at 1kHz tick rate */
 
+#define MOTOR_CONTROLLER_CURRENT_SENSOR_ADDR 0x40 // add INA_I2C_ADDR to driver?
+#define BATTBOX_FANS_CURRENT_SENSOR_ADDR 	 0x42
+#define PUMPS_CURRENT_SENSOR_ADDR 			 0x44
+#define LV_BOARDS_CURRENT_SENSOR_ADDR		 0x45
+
 static osMutexAttr_t pdu_mutex_attributes;
+static I2C_HandleTypeDef *hi2c = NULL; // added this since i need it for wrapper functions (a similar change was made in mpu.c for lsm6dso PR im pretty sure). it's still a param in init_pdu tho
+
+// Wrapper for reading ina226 (current sensor) registers
+static inline int ina_read_reg(uint16_t dev_addr, uint8_t reg, uint16_t *data)
+{
+	uint8_t buff[2];
+	HAL_StatusTypeDef status;
+
+	status = HAL_I2C_Mem_Read(hi2c, dev_addr, reg, I2C_MEMADD_SIZE_16BIT, buff, 2, HAL_MAX_DELAY);
+	if(status != HAL_OK) {return -1;}
+
+	*data = (buff[0] << 8) | buff[1];
+	return 0;
+}
+
+// Wrapper for writing ina226 (current sensor) registers
+static inline int ina_write_reg(uint16_t dev_addr, uint8_t reg, uint16_t *data)
+{
+	uint8_t buff[2];
+	HAL_StatusTypeDef status;
+
+	status = HAL_I2C_Mem_Write(hi2c, dev_addr, reg, I2C_MEMADD_SIZE_16BIT, buff, 2, HAL_MAX_DELAY);
+	if(status != HAL_OK) {return -1;}
+
+	*data = (buff[0] << 8) | buff[1];
+	return 0;
+}
 
 static uint8_t sound_rtds(pdu_t *pdu)
 {
@@ -98,6 +130,58 @@ pdu_t *init_pdu(I2C_HandleTypeDef *hi2c)
 
 	pdu->hi2c = hi2c;
 
+	/* Initialize Motor Controller Current Sensor */
+	pdu->motor_controller_current_sensor = malloc(sizeof(ina226_t));
+	assert(pdu->motor_controller_current_sensor);
+	ina226_init(pdu->motor_controller_current_sensor, ina_write_reg, ina_read_reg, MOTOR_CONTROLLER_CURRENT_SENSOR_ADDR);
+	int status_init = ina226_calibrate(pdu->motor_controller_current_sensor, 0.01f, 3.0f);
+	if (status_init != 0)
+	{
+		printf("\n\rmotor controller current sensor init fail\n\r");
+		free(pdu->motor_controller_current_sensor);
+		free(pdu);
+		return NULL;
+	}
+
+	/* Initialize Battbox Fans Current Sensor */
+	pdu->battbox_fans_current_sensor = malloc(sizeof(ina226_t));
+	assert(pdu->battbox_fans_current_sensor);
+	ina226_init(pdu->battbox_fans_current_sensor, ina_write_reg, ina_read_reg, BATTBOX_FANS_CURRENT_SENSOR_ADDR);
+	status_init = ina226_calibrate(pdu->battbox_fans_current_sensor, 0.01f, 5.0f);
+	if (status_init != 0)
+	{
+		printf("\n\rbattbox fans current sensor init fail\n\r");
+		free(pdu->battbox_fans_current_sensor);
+		free(pdu);
+		return NULL;
+	}
+
+	/* Initialize Pumps Current Sensor */
+	pdu->pumps_current_sensor = malloc(sizeof(ina226_t));
+	assert(pdu->pumps_current_sensor);
+	ina226_init(pdu->pumps_current_sensor, ina_write_reg, ina_read_reg, PUMPS_CURRENT_SENSOR_ADDR);
+	status_init = ina226_calibrate(pdu->pumps_current_sensor, 0.01f, 2.0f);
+	if (status_init != 0)
+	{
+		printf("\n\rpumps current sensor init fail\n\r");
+		free(pdu->pumps_current_sensor);
+		free(pdu);
+		return NULL;
+	}
+
+	/* Initialize LV Boards Current Sensor */
+	pdu->lv_boards_current_sensor = malloc(sizeof(ina226_t));
+	assert(pdu->lv_boards_current_sensor);
+	ina226_init(pdu->lv_boards_current_sensor, ina_write_reg, ina_read_reg, LV_BOARDS_CURRENT_SENSOR_ADDR);
+	status_init = ina226_calibrate(pdu->lv_boards_current_sensor, 0.01f, 1.25f);
+	if (status_init != 0)
+	{
+		printf("\n\rlv boards current sensor init fail\n\r");
+		free(pdu->lv_boards_current_sensor);
+		free(pdu);
+		return NULL;
+	}
+
 	/* Initialize Shutdown GPIO Expander */
 	pdu->shutdown_expander = malloc(sizeof(pca9539_t));
 	assert(pdu->shutdown_expander);
@@ -370,3 +454,53 @@ int8_t read_shutdown(pdu_t *pdu, bool status[MAX_SHUTDOWN_STAGES])
 	osMutexRelease(pdu->mutex);
 	return 0;
 }
+
+
+static int8_t read_current(pdu_t *pdu, ina226_t *ina, float *data)
+{
+	if(!pdu || !ina || !data) return -1;
+
+	float current;
+
+	osStatus_t stat = osMutexAcquire(pdu->mutex, MUTEX_TIMEOUT);
+	if (stat) return stat;
+
+	I2C_HandleTypeDef *previous_hi2c = hi2c; // Saves current global hi2c to previous_hi2c
+	hi2c = pdu->hi2c; // Sets global hi2c to pdu's hi2c before ina226_read_current is called
+
+	int status = ina226_read_current(ina, &current);
+	hi2c = previous_hi2c; // After ina226_read_current is called, restores global hi2c to previous
+	if(status != 0)
+	{
+		osMutexRelease(pdu->mutex);
+		return status;
+	}
+
+	*data = current;
+	osMutexRelease(pdu->mutex);
+	return 0;
+}
+
+int8_t read_motor_controller_current(pdu_t *pdu, float *data)
+{
+	return read_current(pdu, pdu->motor_controller_current_sensor, data);
+}
+
+int8_t read_battbox_fans_current(pdu_t *pdu, float *data)
+{
+	return read_current(pdu, pdu->battbox_fans_current_sensor, data);
+}
+
+int8_t read_pumps_current(pdu_t *pdu, float *data)
+{
+	return read_current(pdu, pdu->pumps_current_sensor, data);
+}
+
+int8_t read_lv_boards_current(pdu_t *pdu, float *data)
+{
+	return read_current(pdu, pdu->lv_boards_current_sensor, data);
+}
+
+
+
+

Makefile

@@ -77,6 +77,7 @@ Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim.c \
 Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim_ex.c \
 Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c.c \
 Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c_ex.c \
+Drivers/Embedded-Base/general/src/INA226.c \
 Drivers/Embedded-Base/general/src/lsm6dso.c \
 Drivers/Embedded-Base/general/src/sht30.c \
 Drivers/Embedded-Base/general/src/max7314.c \