diff --git a/Core/Inc/cerberus_conf.h b/Core/Inc/cerberus_conf.h index 2480045e..66c31338 100644 --- a/Core/Inc/cerberus_conf.h +++ b/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 \ No newline at end of file diff --git a/Core/Inc/fault.h b/Core/Inc/fault.h index 0442dea3..c46f39c9 100644 --- a/Core/Inc/fault.h +++ b/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; diff --git a/Core/Inc/pdu.h b/Core/Inc/pdu.h index d4b006ac..c396534b 100644 --- a/Core/Inc/pdu.h +++ b/Core/Inc/pdu.h @@ -3,6 +3,7 @@ #include "cmsis_os.h" #include "pca9539.h" +#include "INA226.h" #include #include @@ -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. * diff --git a/Core/Src/monitor.c b/Core/Src/monitor.c index 1bfb3324..f6232c31 100644 --- a/Core/Src/monitor.c +++ b/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, ¤t)) + { + 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, ¤t_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, ¤t)) + { + 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, ¤t_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, ¤t)) + { + 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, ¤t_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, ¤t)) + { + 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, ¤t_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); diff --git a/Core/Src/pdu.c b/Core/Src/pdu.c index 419ae3fe..0d08fb45 100644 --- a/Core/Src/pdu.c +++ b/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, ¤t); + 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); +} + + + + diff --git a/Drivers/Embedded-Base b/Drivers/Embedded-Base index 20415dbe..83258635 160000 --- a/Drivers/Embedded-Base +++ b/Drivers/Embedded-Base @@ -1 +1 @@ -Subproject commit 20415dbef0a34e5ffce356a2f68d9d8ad8108e86 +Subproject commit 83258635fc749ac72adb4b52e61f4f1a42e1842f diff --git a/Makefile b/Makefile index 89335d9a..e80f4aa6 100644 --- a/Makefile +++ b/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 \