diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..01728a6 --- /dev/null +++ b/.clang-format @@ -0,0 +1,18 @@ +--- +BasedOnStyle: WebKit +AlignAfterOpenBracket: Align +AlignConsecutiveMacros: 'true' +AlignConsecutiveAssignments: 'true' +AlignOperands: 'true' +AlignTrailingComments: 'true' +AllowShortBlocksOnASingleLine: 'false' +AllowShortFunctionsOnASingleLine: InlineOnly +AllowShortLoopsOnASingleLine: 'false' +ColumnLimit: '100' +IndentWidth: '4' +SpaceAfterLogicalNot: 'false' +SpaceBeforeAssignmentOperators: 'true' +SpaceBeforeRangeBasedForLoopColon: 'true' +TabWidth: '4' +UseTab: Always +--- diff --git a/.github/workflows/build-check.yml b/.github/workflows/build-check.yml new file mode 100644 index 0000000..2cb67d6 --- /dev/null +++ b/.github/workflows/build-check.yml @@ -0,0 +1,20 @@ +name: Build +on: [push] +jobs: + run-build: + runs-on: ubuntu-latest + container: + image: nwdepatie/ner-gcc-arm:latest + timeout-minutes: 10 + steps: + - name: Checkout code + uses: actions/checkout@v3 + with: + submodules: recursive + + - name: Execute Make + run: | + if ! make; then + echo "The application has failed to build." + exit 1 # This will cause the workflow to fail + fi diff --git a/Core/Inc/analyzer.h b/Core/Inc/analyzer.h index 2b8fc5b..b5b0200 100644 --- a/Core/Inc/analyzer.h +++ b/Core/Inc/analyzer.h @@ -5,193 +5,32 @@ #include "datastructs.h" #include "segment.h" -//We want to make sure we aren't doing useless analysis on the same set of data since we are backfilling segment data +/* We want to make sure we aren't doing useless analysis on the same set of data since we are + * backfilling segment data */ #define ANALYSIS_INTERVAL VOLTAGE_WAIT_TIME - - //#define MAX_SIZE_OF_HIST_QUEUE 300000U //bytes /** - * @brief Resizable linked list queue for performing historical analysis + * @brief Pushes in a new data point if we have waited long enough * + * @param data */ -class Analyzer -{ - private: - - Timer analysisTimer; - Timer ocvTimer; - - bool is_first_reading_ = true; - - /** - * @brief Mapping Cell temperature to the cell resistance based on the - * nominal cell resistance curve profile of the Samsung 186500 INR in the - * Orion BMS software utility app - * - * @note Units are in mOhms and indicies are in (degrees C)/5, stops at 65C - * @note Resistance should be *interpolated* from these values (i.e. if we are - * at 27C, we should take the resistance that is halfway between 25C and 30C) - */ - const float TEMP_TO_CELL_RES[14] = - { - 5.52, 4.84, 4.27, 3.68, 3.16, 2.74, 2.4, - 2.12, 1.98, 1.92, 1.90, 1.90, 1.90, 1.90 - }; - - /** - * @brief Mapping Cell temperatue to the discharge current limit based on the - * temperature discharge limit curve profile of the Samsung 186500 INR - * in the Orion BMS software utility app - * - * @note Units are in Amps and indicies are in (degrees C)/5, stops at 65C - * @note Limit should be *interpolated* from these values (i.e. if we are - * at 27C, we should take the limit that is halfway between 25C and 30C) - * - */ - const uint8_t TEMP_TO_DCL[14] = - { - 110, 125, 140, 140, 140, 140, - 140, 140, 140, 100, 60, 20, 0, 0 - }; - - /** - * @brief Mapping Cell temperatue to the charge current limit based on the - * temperature charge limit curve profile of the Samsung 186500 INR - * in the Orion BMS software utility app - * - * @note Units are in Amps and indicies are in (degrees C)/5, stops at 65C - * @note Limit should be *interpolated* from these values (i.e. if we are - * at 27C, we should take the limit that is halfway between 25C and 30C) - * - */ - const uint8_t TEMP_TO_CCL[14] = - { - 0, 25, 25, 25, 25, 25, 25, 25, - 20, 15, 10, 5, 1, 1 - }; - - /** - * @brief Lookup table for State of Charge - * - * @note each index covers 0.1V increase (voltage range is 2.9V - 4.2V, deltaV = 1.3V, currently 13 data points) - * @note values are unitless percentages that represent % charge - * - */ - const uint8_t STATE_OF_CHARGE_CURVE[18] = - { - 0, 0, 0, 0, 0, 0, 0, 0, 0, 6, 15, 24, 56, 74, 85, 95, 98, 100 - }; - - /** - * @brief Mapping the Relevant Thermistors for each cell based on cell # - * - */ - const std::vector RelevantThermMap[NUM_CELLS_PER_CHIP] = - { - {17,18}, - {17,18}, - {17,18,19,20}, - {19,20}, - {19,20,21,22,23}, - {21,22,23,24,25}, - {24,25}, - {24,25,26,27}, - {26,27} - }; - - /** - * @brief Mapping desired fan speed PWM to the cell temperature - * - * @note Units are in PWM out of 255 and indicies are in (degrees C)/5, stops at 65C - * @note Limit should be *interpolated* from these values (i.e. if we are - * at 27C, we should take the limit that is halfway between 25C and 30C) - * - */ - const uint8_t FAN_CURVE[16] = - { - 0, 0, 0, 0, 0, 0, 0, 0, 32, 64, - 128, 255, 255, 255, 255, 255 - }; - - /** - * @brief Selecting thermistors to ignore - * - * @note True / 1 will disable the thermistor - * - */ - const uint8_t THERM_DISABLE[8][11] = - { - {0,0,0,0,0,0,0,0,0,0,0}, - {0,0,0,0,0,0,0,0,0,0,0}, - {0,0,0,0,0,0,0,0,0,0,0}, - {0,0,0,0,0,0,0,0,0,0,0}, - {0,0,0,0,0,0,0,0,0,0,0}, - {0,0,0,0,0,0,0,0,0,0,0}, - {0,0,0,0,0,0,0,0,0,0,0}, - {0,0,0,0,0,0,0,0,0,0,0} - }; - - void calcPackVoltageStats(); - - void calcCellResistances(); - - void calcCellTemps(); - - void calcPackTemps(); - - void calcDCL(); - - void calcContDCL(); - - void calcCCL(); +void analyzer_push(AccumulatorData_t* data); - void calcContCCL(); - - void printData(); - - void calcOpenCellVoltage(); - - void disableTherms(); - - void calcStateOfCharge(); - - void highCurrThermCheck(); - - void diffCurrThermCheck(); - - - - public: - Analyzer(); - - ~Analyzer(); - - /** - * @brief Pushes in a new data point if we have waited long enough - * - * @param data - */ - void push(AccumulatorData_t *data); - - /** - * @brief Calculates the PWM required to drive the fans at the current moment in time - * - * @param bmsdata - * @return uint8_t - */ - uint8_t calcFanPWM(); - - /** - * @brief Pointer to the address of the most recent data point - * - */ - AccumulatorData_t *bmsdata; +/** + * @brief Calculates the PWM required to drive the fans at the current moment in time + * + * @param bmsdata + * @return uint8_t + */ +uint8_t analyzer_calc_fan_pwm(); - AccumulatorData_t *prevbmsdata; -}; +/** + * @brief Pointer to the address of the most recent data point + */ +AccumulatorData_t* bmsdata; -extern Analyzer analyzer; +AccumulatorData_t* prevbmsdata; #endif \ No newline at end of file diff --git a/Core/Inc/compute.h b/Core/Inc/compute.h index 660c7a8..a940e6d 100644 --- a/Core/Inc/compute.h +++ b/Core/Inc/compute.h @@ -6,194 +6,169 @@ #include "canMsgHandler.h" #include "stateMachine.h" -#define CURRENT_SENSOR_PIN_L A1 -#define CURRENT_SENSOR_PIN_H A0 -#define MEAS_5VREF_PIN A7 -#define FAULT_PIN 2 -#define CHARGE_SAFETY_RELAY 4 -#define CHARGE_DETECT 5 -#define CHARGER_BAUD 250000U -#define MC_BAUD 1000000U -#define MAX_ADC_RESOLUTION 1023 // 13 bit ADC - -class ComputeInterface -{ - private: - uint8_t fan_speed_; - bool is_charging_enabled_; - - enum - { - CHARGE_ENABLED, - CHARGE_DISABLED - }; - - /** - * @brief Determines state of the charger LEDs based on the battery charge percentage - * - * @param bms_data - * - * @return uint8_t Value to be used for setting LED bits for charger message - * - */ - uint8_t calcChargerLEDState(AccumulatorData_t *bms_data); - - public: - ComputeInterface(); - - ~ComputeInterface(); - - /** - * @brief sets safeguard bool to check whether charging is enabled or disabled - * - * @param is_enabled - */ - void enableCharging(bool enable_charging); - - /** - * @brief sends charger message - * - * @param voltage_to_set - * @param currentToSet - * - * @return Returns a fault if we are not able to communicate with charger - */ - FaultStatus_t sendChargingMessage(uint16_t voltage_to_set, AccumulatorData_t *bms_data); - - /** - * @brief Returns if charger interlock is engaged, indicating charger LV connector is plugged in - * - * @return true - * @return false - */ - bool chargerConnected(); - - /** - * @brief Handle any messages received from the charger - * - * @param msg - */ - static void chargerCallback(const CAN_message_t &msg); - - static void MCCallback(const CAN_message_t &msg); - - /** - * @brief Sets the desired fan speed - * - * @param new_fan_speed - */ - void setFanSpeed(uint8_t new_fan_speed); - - /** - * @brief Returns the pack current sensor reading - * - * @return int16_t - */ - int16_t getPackCurrent(); - - /** - * @brief sends max charge/discharge current to Motor Controller - * - * @param max_charge - * @param max_discharge - */ - void sendMCMsg(uint16_t max_charge, uint16_t max_discharge); - - /** - * @brief updates fault relay - * - * @param fault_state - */ - void setFault(FaultStatus_t fault_state); - - /** - * @brief sends acc status message - * - * @param voltage - * @param current - * @param ah - * @param soc - * @param health - * - * @return Returns a fault if we are not able to send - */ - void sendAccStatusMessage(uint16_t voltage, int16_t current, uint16_t ah, uint8_t soc, uint8_t health); - - /** - * @brief sends BMS status message - * - * @param bms_state - * @param fault_status - * @param tempAvg - * @param tempInternal - * - * @return Returns a fault if we are not able to send - */ - void sendBMSStatusMessage(int bms_state, uint32_t fault_status, int8_t avg_temp, int8_t internal_temp, bool balance); - - /** - * @brief sends shutdown control message - * - * @param mpe_state - * - * @return Returns a fault if we are not able to send - */ - void sendShutdownControlMessage(uint8_t mpe_state); - - /** - * @brief sends cell data message - * - * @param high_voltage - * @param low_voltage - * @param avg_voltage - * - * @return Returns a fault if we are not able to send - */ - void sendCellDataMessage(CriticalCellValue_t high_voltage, CriticalCellValue_t low_voltage, uint16_t avg_voltage); - - /** - * @brief sends cell voltage message - * - * @param cell_id - * @param instant_volt - * @param internal_res - * @param shunted - * @param open_voltage - * - * @return Returns a fault if we are not able to send - */ - void sendCellVoltageMessage(uint8_t cell_id, uint16_t instant_volt, uint16_t internal_res, uint8_t shunted, uint16_t open_voltage); - - /** - * @brief sends out the calculated values of currents - * - * @param discharge - * @param charge - * @param current - */ - void sendCurrentsStatus(uint16_t discharge, uint16_t charge, uint16_t current); - - /** - * @brief sends cell temperature message - * - * - * - * @return Returns a fault if we are not able to send - */ - void sendCellTemp(CriticalCellValue_t max_cell_temp, CriticalCellValue_t min_cell_temp, uint16_t avg_temp); - - /** - * @brief sends the average segment temperatures - * - * - * - * @return Returns a fault if we are not able to send - */ - void sendSegmentTemps(int8_t segmentTemps[NUM_SEGMENTS]); - - void sendDclPreFault(bool prefault); -}; - -extern ComputeInterface compute; - -#endif \ No newline at end of file +#define CURRENT_SENSOR_PIN_L A1 +#define CURRENT_SENSOR_PIN_H A0 +#define MEAS_5VREF_PIN A7 +#define FAULT_PIN 2 +#define CHARGE_SAFETY_RELAY 4 +#define CHARGE_DETECT 5 +#define CHARGER_BAUD 250000U +#define MC_BAUD 1000000U +#define MAX_ADC_RESOLUTION 1023 // 13 bit ADC + +/** + * @brief inits the compute interface + */ +void compute_init(); + +/** + * @brief sets safeguard bool to check whether charging is enabled or disabled + * + * @param is_enabled + */ +void compute_enable_charging(bool enable_charging); + +/** + * @brief sends charger message + * + * @param voltage_to_set + * @param currentToSet + * + * @return Returns a fault if we are not able to communicate with charger + */ +FaultStatus_t compute_send_charging_message(uint16_t voltage_to_set, AccumulatorData_t* bms_data); + +/** + * @brief Returns if charger interlock is engaged, indicating charger LV connector is plugged in + * + * @return true + * @return false + */ +bool compute_charger_connected(); + +/** + * @brief Handle any messages received from the charger + * + * @param msg + */ +static void compute_charger_callback(const CAN_message_t& msg); + +static void compute_mc_callback(const CAN_message_t& msg); + +/** + * @brief Sets the desired fan speed + * + * @param new_fan_speed + */ +void compute_set_fan_speed(uint8_t new_fan_speed); + +/** + * @brief Returns the pack current sensor reading + * + * @return int16_t + */ +int16_t compute_get_pack_current(); + +/** + * @brief sends max charge/discharge current to Motor Controller + * + * @param max_charge + * @param max_discharge + */ +void compute_send_mc_message(uint16_t max_charge, uint16_t max_discharge); + +/** + * @brief updates fault relay + * + * @param fault_state + */ +void compute_set_fault(FaultStatus_t fault_state); + +/** + * @brief sends acc status message + * + * @param voltage + * @param current + * @param ah + * @param soc + * @param health + * + * @return Returns a fault if we are not able to send + */ +void compute_send_acc_status_message(AccumulatorData_t* bmsdata); + +/** + * @brief sends BMS status message + * + * @param bms_state + * @param fault_status + * @param tempAvg + * @param tempInternal + * + * @return Returns a fault if we are not able to send + */ +void compute_send_bms_status_message(AccumulatorData_t* bmsdata, int bms_state, bool balance); + +/** + * @brief sends shutdown control message + * + * @param mpe_state + * + * @return Returns a fault if we are not able to send + */ +void compute_send_shutdown_ctrl_message(uint8_t mpe_state); + +/** + * @brief sends cell data message + * + * @param high_voltage + * @param low_voltage + * @param avg_voltage + * + * @return Returns a fault if we are not able to send + */ +void compute_send_cell_data_message(AccumulatorData_t* bmsdata); + +/** + * @brief sends cell voltage message + * + * @param cell_id + * @param instant_volt + * @param internal_res + * @param shunted + * @param open_voltage + * + * @return Returns a fault if we are not able to send + */ +void compute_send_cell_voltage_message(uint8_t cell_id, uint16_t instant_volt, + uint16_t internal_res, uint8_t shunted, + uint16_t open_voltage); + +/** + * @brief sends out the calculated values of currents + * + * @param discharge + * @param charge + * @param current + */ +void compute_send_current_message(AccumulatorData_t* bmsdata); + +/** + * @brief sends cell temperature message + * + * @return Returns a fault if we are not able to send + */ +void compute_send_cell_temp_message(AccumulatorData_t* bmsdata); + +/** + * @brief sends the average segment temperatures + * + * + * + * @return Returns a fault if we are not able to send + */ +void compute_send_segment_temp_message(AccumulatorData_t* bmsdata); + +void compute_send_dcl_prefault_message(bool prefault); + +#endif // COMPUTE_H diff --git a/Core/Inc/datastructs.h b/Core/Inc/datastructs.h index 4706e08..5bbcb59 100644 --- a/Core/Inc/datastructs.h +++ b/Core/Inc/datastructs.h @@ -1,28 +1,27 @@ #ifndef DATASTRUCTS_H #define DATASTRUCTS_H -#include #include "bmsConfig.h" +#include #include /** * @brief Individual chip data * @note stores thermistor values, voltage readings, and the discharge status */ -struct ChipData_t -{ - //These are retrieved from the initial LTC comms - uint16_t voltage_reading[NUM_CELLS_PER_CHIP]; //store voltage readings from each chip - int8_t thermistor_reading[NUM_THERMS_PER_CHIP]; //store all therm readings from each chip - int8_t thermistor_value[NUM_THERMS_PER_CHIP]; - FaultStatus_t error_reading; - - //These are calculated during the analysis of data - int8_t cell_temp[NUM_CELLS_PER_CHIP]; - float cell_resistance[NUM_CELLS_PER_CHIP]; - uint16_t open_cell_voltage[NUM_CELLS_PER_CHIP]; - - uint8_t bad_volt_diff_count[NUM_CELLS_PER_CHIP]; +struct ChipData_t { + /* These are retrieved from the initial LTC comms */ + uint16_t voltage_reading[NUM_CELLS_PER_CHIP]; /* store voltage readings from each chip */ + int8_t thermistor_reading[NUM_THERMS_PER_CHIP]; /* store all therm readings from each chip */ + int8_t thermistor_value[NUM_THERMS_PER_CHIP]; + FaultStatus_t error_reading; + + /* These are calculated during the analysis of data */ + int8_t cell_temp[NUM_CELLS_PER_CHIP]; + float cell_resistance[NUM_CELLS_PER_CHIP]; + uint16_t open_cell_voltage[NUM_CELLS_PER_CHIP]; + + uint8_t bad_volt_diff_count[NUM_CELLS_PER_CHIP]; }; /** @@ -30,42 +29,42 @@ struct ChipData_t * @note the values increase at powers of two to perform bitwise operations on a main fault code * to set or get the error codes */ -enum BMSFault_t -{ - FAULTS_CLEAR = 0x0, - - //Orion BMS faults - CELLS_NOT_BALANCING = 0x1, - CELL_VOLTAGE_TOO_HIGH = 0x2, - CELL_VOLTAGE_TOO_LOW = 0x4, - PACK_TOO_HOT = 0x8, - OPEN_WIRING_FAULT = 0x10, //cell tap wire is either weakly connected or not connected - INTERNAL_SOFTWARE_FAULT = 0x20, //general software fault - INTERNAL_THERMAL_ERROR = 0x40, //internal hardware fault reulting from too hot of a temperature reading from NERduino - INTERNAL_CELL_COMM_FAULT = 0x80, //this is due to an invalid CRC from retrieving values - CURRENT_SENSOR_FAULT = 0x100, - CHARGE_READING_MISMATCH = 0x200, //we detect that there is a charge voltage, but we are not supposed to be charging - LOW_CELL_VOLTAGE = 0x400, //voltage of a cell falls below 90 mV - WEAK_PACK_FAULT = 0x800, - EXTERNAL_CAN_FAULT = 0x1000, - DISCHARGE_LIMIT_ENFORCEMENT_FAULT = 0x2000, - CHARGER_SAFETY_RELAY = 0x4000, - BATTERY_THERMISTOR = 0x8000, - CHARGER_CAN_FAULT = 0x10000, - CHARGE_LIMIT_ENFORCEMENT_FAULT = 0x20000, - - MAX_FAULTS = 0x80000000 //Maximum allowable fault code +// clang-format off +enum BMSFault_t { + FAULTS_CLEAR = 0x0, + + /* Orion BMS faults */ + CELLS_NOT_BALANCING = 0x1, + CELL_VOLTAGE_TOO_HIGH = 0x2, + CELL_VOLTAGE_TOO_LOW = 0x4, + PACK_TOO_HOT = 0x8, + OPEN_WIRING_FAULT = 0x10, /* cell tap wire is either weakly connected or not connected */ + INTERNAL_SOFTWARE_FAULT = 0x20, /* general software fault */ + INTERNAL_THERMAL_ERROR = 0x40, /* internal hardware fault reulting from too hot of onboard temps */ + INTERNAL_CELL_COMM_FAULT = 0x80, /* this is due to an invalid CRC from retrieving values */ + CURRENT_SENSOR_FAULT = 0x100, + CHARGE_READING_MISMATCH = 0x200, /* charge voltage when not supposed to be charging*/ + LOW_CELL_VOLTAGE = 0x400, /* voltage of a cell falls below 90 mV */ + WEAK_PACK_FAULT = 0x800, + EXTERNAL_CAN_FAULT = 0x1000, + DISCHARGE_LIMIT_ENFORCEMENT_FAULT = 0x2000, + CHARGER_SAFETY_RELAY = 0x4000, + BATTERY_THERMISTOR = 0x8000, + CHARGER_CAN_FAULT = 0x10000, + CHARGE_LIMIT_ENFORCEMENT_FAULT = 0x20000, + + MAX_FAULTS = 0x80000000 /* Maximum allowable fault code */ }; +// clang-format on /** * @brief Stores critical values for the pack, and where that critical value can be found * */ -struct CriticalCellValue_t -{ - int32_t val; - uint8_t chipIndex; - uint8_t cellNum; +struct CriticalCellValue_t { + int32_t val; + uint8_t chipIndex; + uint8_t cellNum; }; /** @@ -74,62 +73,59 @@ struct CriticalCellValue_t */ #define ACCUMULATOR_FRAME_SIZE sizeof(AccumulatorData_t); -struct AccumulatorData_t -{ - /*Array of data from all chips in the system*/ - ChipData_t chip_data[NUM_CHIPS]; - - FaultStatus_t fault_status = NOT_FAULTED; +struct AccumulatorData_t { + /* Array of data from all chips in the system */ + ChipData_t chip_data[NUM_CHIPS]; - int16_t pack_current; // this value is multiplied by 10 to account for decimal precision - uint16_t pack_voltage; - uint16_t pack_ocv; - uint16_t pack_res; + FaultStatus_t fault_status = NOT_FAULTED; - uint16_t discharge_limit; - uint16_t charge_limit; - uint16_t cont_DCL; - uint16_t cont_CCL; - uint8_t soc; + int16_t pack_current; /* this value is multiplied by 10 to account for decimal precision */ + uint16_t pack_voltage; + uint16_t pack_ocv; + uint16_t pack_res; - int8_t segment_average_temps[NUM_SEGMENTS]; + uint16_t discharge_limit; + uint16_t charge_limit; + uint16_t cont_DCL; + uint16_t cont_CCL; + uint8_t soc; - /** - * @brief Note that this is a 32 bit integer, so there are 32 max possible fault codes - */ - uint32_t fault_code; + int8_t segment_average_temps[NUM_SEGMENTS]; - /*Max, min, and avg thermistor readings*/ - CriticalCellValue_t max_temp; - CriticalCellValue_t min_temp; - int8_t avg_temp; + /** + * @brief Note that this is a 32 bit integer, so there are 32 max possible fault codes + */ + uint32_t fault_code; - /*Max and min cell resistances*/ - CriticalCellValue_t max_res; - CriticalCellValue_t min_res; + /* Max, min, and avg thermistor readings */ + CriticalCellValue_t max_temp; + CriticalCellValue_t min_temp; + int8_t avg_temp; - /*Max, min, and avg voltage of the cells*/ - CriticalCellValue_t max_voltage; - CriticalCellValue_t min_voltage; - uint16_t avg_voltage; - uint16_t delt_voltage; + /* Max and min cell resistances */ + CriticalCellValue_t max_res; + CriticalCellValue_t min_res; - CriticalCellValue_t max_ocv; - CriticalCellValue_t min_ocv; - uint16_t avg_ocv; - uint16_t delt_ocv; + /* Max, min, and avg voltage of the cells */ + CriticalCellValue_t max_voltage; + CriticalCellValue_t min_voltage; + uint16_t avg_voltage; + uint16_t delt_voltage; - uint16_t boost_setting; + CriticalCellValue_t max_ocv; + CriticalCellValue_t min_ocv; + uint16_t avg_ocv; + uint16_t delt_ocv; - bool is_charger_connected; + uint16_t boost_setting; + bool is_charger_connected; }; /** * @brief Represents the state of the BMS fault timers */ -typedef enum -{ +typedef enum { BEFORE_TIMER_START, DURING_EVAL @@ -138,70 +134,59 @@ typedef enum /** * @brief Represents a timer that can be in one of three states */ -struct tristate_timer : public Timer -{ - +struct tristate_timer : public Timer { + TSTimerEvalState eval_state = BEFORE_TIMER_START; - int eval_length; - + int eval_length; }; /** * @brief Represents individual BMS states */ -typedef enum -{ - BOOT_STATE, /* State when BMS first starts up, used to initialize everything that needs configuring */ - READY_STATE, /* State when car is not on/BMS is not really doing anything */ - CHARGING_STATE, /* State when car is on and is charging (Filling battery) */ - FAULTED_STATE, /* State when BMS has detected a catastrophic fault and we need to hault operations */ - NUM_STATES +typedef enum { + BOOT_STATE, /* State when BMS first starts up, used to initialize everything that needs + configuring */ + READY_STATE, /* State when car is not on/BMS is not really doing anything */ + CHARGING_STATE, /* State when car is on and is charging (Filling battery) */ + FAULTED_STATE, /* State when BMS has detected a catastrophic fault and we need to hault + operations */ + NUM_STATES } BMSState_t; - /** * @brief Represents fault evaluation operators */ -typedef enum -{ - GT, /* fault if {data} greater than {threshold} */ - LT, /* fault if {data} less than {threshold} */ - GE, /* fault if {data} greater than or equal to {threshold} */ - LE, /* fault if {data} less than or equal to {threshold} */ - EQ, /* fault if {data} equal to {threshold} */ - NEQ, /* fault if {data} not equal to {threshold} */ - NOP /* no operation, use for single threshold faults */ +typedef enum { + GT, /* fault if {data} greater than {threshold} */ + LT, /* fault if {data} less than {threshold} */ + GE, /* fault if {data} greater than or equal to {threshold} */ + LE, /* fault if {data} less than or equal to {threshold} */ + EQ, /* fault if {data} equal to {threshold} */ + NEQ, /* fault if {data} not equal to {threshold} */ + NOP /* no operation, use for single threshold faults */ } FaultEvalType; -/** - * @brief Represents fault timer data - */ - - - /** * @brief Represents data to be packaged into a fault evaluation */ -struct fault_eval -{ - char* id; - tristate_timer timer; +struct fault_eval { + char* id; + tristate_timer timer; - int data_1; - FaultEvalType optype_1; - int lim_1; + int data_1; + FaultEvalType optype_1; + int lim_1; - int timeout; - int code; + int timeout; + int code; - int data_2 = 0; - FaultEvalType optype_2 = NOP; - int lim_2 = 0; - - bool is_faulted = false; + int data_2 = 0; + FaultEvalType optype_2 = NOP; + int lim_2 = 0; + bool is_faulted = false; }; #endif \ No newline at end of file diff --git a/Core/Inc/eepromdirectory.h b/Core/Inc/eepromdirectory.h new file mode 100644 index 0000000..87b1811 --- /dev/null +++ b/Core/Inc/eepromdirectory.h @@ -0,0 +1,90 @@ +#ifndef EEPROMDIRECTORY_H +#define EEPROMDIRECTORY_H + +#include "bmsConfig.h" + +/* index 0 = newest, index 4 = oldest */ +static uint32_t eeprom_faults[NUM_EEPROM_FAULTS]; + +struct eeprom_partition +{ + char *id; /* key */ + uint16_t size; /* bytes */ + uint16_t address; /* start address */ +}; + +struct eeprom_partition eeprom_data[NUM_EEPROM_ITEMS] +{ +/* ____________KEY________________ _BYTES_ */ + {.id = const_cast("ROOT"), .size = 1}, + {.id = const_cast("FAULTS"), .size = 21} + +}; + + +/** + * @brief partitions eeprom addresses given table of data and size + * + * + */ +void eepromInit(); + +/** + * @brief returns the starting address of the passed key + * + * @param key + * @return int + */ +int eepromGetIndex(char *key); + +/** + * @brief returns the key at the passed index + * + * + */ +char *eepromGetKey(int index); + + + +/** + * @brief fills passed data pointer with data from eeprom + * + * @note user is responsible for passing data of correct size + * @param key + * @param data + */ +void eepromReadData(char *key, void *data); + +void eepromReadData(uint8_t index, void *data); + +/** + * @brief loads eeprom with data from passed pointer + * + * @note user is responsible for passing data of correct size + * @param key + * @param data + */ +void eepromWriteData(char *key, void *data); + +void eepromWriteData(uint8_t index, void *data); + +/** + * @brief logs fault code in eeprom + * + * + * @param fault_code + */ +void logFault(uint32_t fault_code); +/** + * @brief reads all stored faults from eeprom + * + * + * @note this updates a static array of fault codes, should be called before accessing the array + * @note this function is blocking, and will take a few ms to complete. This is why it is kept seperate from logFault(), + * allwing the user more control as to when to use this + */ + +void getFaults(); + + +#endif \ No newline at end of file diff --git a/Core/Inc/segment.h b/Core/Inc/segment.h index 67a9515..f467cca 100644 --- a/Core/Inc/segment.h +++ b/Core/Inc/segment.h @@ -3,146 +3,61 @@ #include //#include <---------- REPLACE W/ NEW DRIVER -#include "datastructs.h" #include "bmsConfig.h" - -#define THERM_WAIT_TIME 500 //ms -#define VOLTAGE_WAIT_TIME 100 //ms -#define THERM_AVG 15 // Number of values to average - -#define MAX_VOLT_DELTA 2500 -#define MAX_VOLT_DELTA_COUNT 10 +#include "datastructs.h" /** - * @brief This class serves as the interface for all of the segment boards + * @brief Initializes the segments */ -class SegmentInterface -{ - private: - - Timer therm_timer; - Timer voltage_reading_timer; - Timer variance_timer; - - FaultStatus_t voltage_error = NOT_FAULTED; - FaultStatus_t therm_error = NOT_FAULTED; - - uint16_t therm_settle_time_ = 0; - - const uint32_t VOLT_TEMP_CONV[91] = - { - 44260, 43970, 43670, 43450, 43030, 42690, 42340, 41980, 41620, 41240, - 40890, 40460, 40040, 39580, 39130, 38660, 38210, 37710, 37210, 36190, - 35670, 35160, 34620, 34080, 33550, 32990, 32390, 31880, 31270, 30690, - 30160, 29590, 28990, 28450, 27880, 27270, 26740, 26080, 25610, 25000, - 24440, 23880, 23330, 22780, 22240, 21700, 21180, 20660, 20150, 19640, - 19140, 18650, 18170, 17700, 17230, 16780, 16330, 15890, 15470, 15030, - 14640, 14230, 13850, 13450, 13070, 12710, 11490, 11170, 10850, 10550, - 10250, 9960, 9670, 9400, 9130, 8870, 8620, 8370, 8130, 7900, - 0 - }; - - const int32_t VOLT_TEMP_CALIB_OFFSET = 0; - - ChipData_t *segment_data = nullptr; - - ChipData_t previous_data[NUM_CHIPS]; - - uint8_t local_config[NUM_CHIPS][6] = {}; - - uint16_t discharge_commands[NUM_CHIPS] = {}; - - void pullChipConfigurations(); - - void pushChipConfigurations(); - - void SelectTherm(uint8_t therm); - - FaultStatus_t pullThermistors(); - - FaultStatus_t pullVoltages(); - - int8_t steinhartEst(uint16_t V); - - void serializeI2CMsg(uint8_t data_to_write[][3], uint8_t comm_output[][6]); - - void configureDischarge(uint8_t chip, uint16_t cells); - - void disableGPIOPulldowns(); +void segment_init(); - public: - SegmentInterface(); - - ~SegmentInterface(); - /** - * @brief Initializes the segments - * - */ - void init(); - - /** - * @brief Pulls all cell data from the segments and returns all cell data - * - * @todo make sure that retrieving cell data doesn't block code too much - * - * @return int* - */ - void retrieveSegmentData(ChipData_t databuf[NUM_CHIPS]); - - /** - * @brief Enables/disables balancing for all cells - * - * @param balance_enable - */ - void enableBalancing(bool balance_enable); - - /** - * @brief Enables/disables balancing for a specific cell - * - * @param chip_num - * @param cell_num - * @param balance_enable - */ - void enableBalancing(uint8_t chip_num, uint8_t cell_num, bool balance_enable); - - /** - * @brief Sets each cell to whatever state is passed in the boolean config area - * - * @param discharge_config - */ - void configureBalancing(bool discharge_config[NUM_CHIPS][NUM_CELLS_PER_CHIP]); - - /** - * @brief Returns if a specific cell is balancing - * - * @param chip_num - * @return true - * @return false - */ - bool isBalancing(uint8_t chip_num, uint8_t cell_num); - - /** - * @brief Returns if any cells are balancing - * - * @return true - * @return false - */ - bool isBalancing(); - - void averagingThermCheck(); - - void standardDevThermCheck(); - - int8_t calcThermStandardDev(int16_t avg_temp); +/** + * @brief Pulls all cell data from the segments and returns all cell data + * + * @todo make sure that retrieving cell data doesn't block code too much + * + * @return int* + */ +void segment_retrieve_segment_data(ChipData_t databuf[NUM_CHIPS]); - int16_t calcAverage(); +/** + * @brief Enables/disables balancing for all cells + * + * @param balance_enable + */ +void segment_enable_balancing(bool balance_enable); - void varianceThermCheck(); +/** + * @brief Enables/disables balancing for a specific cell + * + * @param chip_num + * @param cell_num + * @param balance_enable + */ +void enable_balancing(uint8_t chip_num, uint8_t cell_num, bool balance_enable); - void discardNeutrals(); +/** + * @brief Sets each cell to whatever state is passed in the boolean config area + * + * @param discharge_config + */ +void segment_configure_balancing(bool discharge_config[NUM_CHIPS][NUM_CELLS_PER_CHIP]); -}; +/** + * @brief Returns if a specific cell is balancing + * + * @param chip_num + * @return true + * @return false + */ +bool segment_is_balancing(uint8_t chip_num, uint8_t cell_num); -extern SegmentInterface segment; +/** + * @brief Returns if any cells are balancing + * + * @return true + * @return false + */ +bool segment_is_balancing(); #endif \ No newline at end of file diff --git a/Core/Inc/stateMachine.h b/Core/Inc/stateMachine.h new file mode 100644 index 0000000..d8d169b --- /dev/null +++ b/Core/Inc/stateMachine.h @@ -0,0 +1,68 @@ +#ifndef BMS_STATES_H +#define BMS_STATES_H + +#include "datastructs.h" +#include "segment.h" +#include "compute.h" +#include "analyzer.h" + +/** +* @brief Returns if we want to balance cells during a particular frame +* +* @param bmsdata +* @return true +* @return false +*/ +bool sm_balancing_check(AccumulatorData_t *bmsdata); + +/** +* @brief Returns if we want to charge cells during a particular frame +* +* @param bmsdata +* @return true +* @return false +*/ +bool sm_charging_check(AccumulatorData_t *bmsdata); + + +/** +* @brief Returns any new faults or current faults that have come up +* @note Should be bitwise OR'ed with the current fault status +* +* @param accData +* @return uint32_t +*/ +uint32_t sm_fault_return(AccumulatorData_t *accData); + + /** + * @brief Used in parellel to faultReturn(), calculates each fault to append the fault status + * + * @param index + * @return fault_code + */ +uint32_t sm_fault_eval(fault_eval index); + +/** + * @brief Used to check for faults immedietly before reaching faulted state, allows for easier handling + * + * @param bmsdata + */ +void preFaultCheck(AccumulatorData_t *bmsdata); + +/** + * @brief handles the state machine, calls the appropriate handler function and runs every loop functions + * + * @param bmsdata + */ +void sm_handle_state(AccumulatorData_t *bmsdata); + +/** +* @brief Algorithm behind determining which cells we want to balance +* @note Directly interfaces with the segments +* +* @param bms_data +*/ +void sm_balance_cells(AccumulatorData_t *bms_data); +void sm_broadcast_current_limit(AccumulatorData_t *bmsdata); + +#endif //BMS_STATES_H \ No newline at end of file diff --git a/Core/Src/analyzer.c b/Core/Src/analyzer.c index 5737f27..00a3531 100644 --- a/Core/Src/analyzer.c +++ b/Core/Src/analyzer.c @@ -1,447 +1,527 @@ #include "analyzer.h" -Analyzer analyzer; +// clang-format off +/** + * @brief Mapping Cell temperature to the cell resistance based on the + * nominal cell resistance curve profile of the Samsung 186500 INR in the + * Orion BMS software utility app + * + * @note Units are in mOhms and indicies are in (degrees C)/5, stops at 65C + * @note Resistance should be *interpolated* from these values (i.e. if we are + * at 27C, we should take the resistance that is halfway between 25C and 30C) + */ +const float TEMP_TO_CELL_RES[14] = +{ + 5.52, 4.84, 4.27, 3.68, 3.16, 2.74, 2.4, + 2.12, 1.98, 1.92, 1.90, 1.90, 1.90, 1.90 +}; + +/** + * @brief Mapping Cell temperatue to the discharge current limit based on the + * temperature discharge limit curve profile of the Samsung 186500 INR + * in the Orion BMS software utility app + * + * @note Units are in Amps and indicies are in (degrees C)/5, stops at 65C + * @note Limit should be *interpolated* from these values (i.e. if we are + * at 27C, we should take the limit that is halfway between 25C and 30C) + * + */ +const uint8_t TEMP_TO_DCL[14] = +{ + 110, 125, 140, 140, 140, 140, + 140, 140, 140, 100, 60, 20, 0, 0 +}; + +/** + * @brief Mapping Cell temperatue to the charge current limit based on the + * temperature charge limit curve profile of the Samsung 186500 INR + * in the Orion BMS software utility app + * + * @note Units are in Amps and indicies are in (degrees C)/5, stops at 65C + * @note Limit should be *interpolated* from these values (i.e. if we are + * at 27C, we should take the limit that is halfway between 25C and 30C) + * + */ +const uint8_t TEMP_TO_CCL[14] = +{ + 0, 25, 25, 25, 25, 25, 25, 25, + 20, 15, 10, 5, 1, 1 +}; + +/** + * @brief Lookup table for State of Charge + * + * @note each index covers 0.1V increase (voltage range is 2.9V - 4.2V, deltaV = 1.3V, + * currently 13 data points) + * @note values are unitless percentages that represent % charge + * + */ +const uint8_t STATE_OF_CHARGE_CURVE[18] = +{ + 0, 0, 0, 0, 0, 0, 0, 0, 0, 6, 15, 24, 56, 74, 85, 95, 98, 100 +}; + +/** + * @brief Mapping the Relevant Thermistors for each cell based on cell # + * @note 0xFF indicates the end of the relevant therms + */ +const uint8_t RelevantThermMap[NUM_CELLS_PER_CHIP] = +{ + {17, 18, 0xFF, 0xFF, 0xFF}, + {17, 18, 0xFF, 0xFF, 0xFF}, + {17, 18, 19, 20, 0xFF}, + {19, 20, 0xFF, 0xFF, 0xFF}, + {19, 20, 21, 22, 23}, + {21, 22, 23, 24, 25}, + {24, 25, 0xFF, 0xFF, 0xFF}, + {24, 25, 26, 27, 0xFF}, + {26, 27, 0xFF, 0xFF, 0xFF} +}; + +/** + * @brief Mapping desired fan speed PWM to the cell temperature + * + * @note Units are in PWM out of 255 and indicies are in (degrees C)/5, stops at 65C + * @note Limit should be *interpolated* from these values (i.e. if we are + * at 27C, we should take the limit that is halfway between 25C and 30C) + */ +const uint8_t FAN_CURVE[16] = +{ + 0, 0, 0, 0, 0, 0, 0, 0, 32, 64, + 128, 255, 255, 255, 255, 255 +}; + +/** + * @brief Selecting thermistors to ignore + * + * @note True / 1 will disable the thermistor + */ +const uint8_t THERM_DISABLE[8][11] = +{ + {0,0,0,0,0,0,0,0,0,0,0}, + {0,0,0,0,0,0,0,0,0,0,0}, + {0,0,0,0,0,0,0,0,0,0,0}, + {0,0,0,0,0,0,0,0,0,0,0}, + {0,0,0,0,0,0,0,0,0,0,0}, + {0,0,0,0,0,0,0,0,0,0,0}, + {0,0,0,0,0,0,0,0,0,0,0}, + {0,0,0,0,0,0,0,0,0,0,0} +}; + +// clang-format on -Analyzer::Analyzer(){} +Timer analysisTimer; +Timer ocvTimer; -Analyzer::~Analyzer(){} +bool is_first_reading_ = true; -void Analyzer::push(AccumulatorData_t *data) +void push(AccumulatorData_t* data) { - if(prevbmsdata != nullptr) - delete prevbmsdata; - - prevbmsdata = bmsdata; - bmsdata = data; - - disableTherms(); - - highCurrThermCheck(); // = prev if curr > 50 - //diffCurrThermCheck(); // = prev if curr - prevcurr > 10 - //varianceThermCheck();// = prev if val > 5 deg difference - //standardDevThermCheck(); // = prev if std dev > 3 - //averagingThermCheck(); // matt shitty incrementing - - calcCellTemps(); - calcPackTemps(); - calcPackVoltageStats(); - calcOpenCellVoltage(); - calcCellResistances(); - calcDCL(); - calcContDCL(); - calcContCCL(); - calcStateOfCharge(); - - is_first_reading_ = false; + if (prevbmsdata != nullptr) + delete prevbmsdata; + + prevbmsdata = bmsdata; + bmsdata = data; + + disable_therms(); + + high_curr_therm_check(); /* = prev if curr > 50 */ + // diff_curr_therm_check(); /* = prev if curr - prevcurr > 10 */ + // variance_therm_check(); /* = prev if val > 5 deg difference */ + // standard_dev_therm_check(); /* = prev if std dev > 3 */ + // averaging_therm_check(); /* matt shitty incrementing */ + + calc_cell_temps(); + calc_pack_temps(); + calc_pack_voltage_stats(); + calc_open_cell_voltage(); + calc_cell_resistances(); + calc_dcl(); + calc_cont_dcl(); + calc_cont_ccl(); + calc_state_of_charge(); + + is_first_reading = false; } -void Analyzer::calcCellTemps() +void calc_cell_temps() { - for(uint8_t c = 0; c < NUM_CHIPS; c++) - { - for(uint8_t cell = 0; cell < NUM_CELLS_PER_CHIP; cell++) - { - int temp_sum = 0; - for(uint8_t therm = 0; therm < RelevantThermMap[cell].size(); therm++) - { - uint8_t thermNum = RelevantThermMap[cell][therm]; - temp_sum += bmsdata->chip_data[c].thermistor_value[thermNum]; - } - - //Takes the average temperature of all the relevant thermistors - bmsdata->chip_data[c].cell_temp[cell] = temp_sum / RelevantThermMap[cell].size(); - - //Cleansing value - if(bmsdata->chip_data[c].cell_temp[cell] > MAX_TEMP) - { - bmsdata->chip_data[c].cell_temp[cell] = MAX_TEMP; - } - } - } + for (uint8_t c = 0; c < NUM_CHIPS; c++) { + for (uint8_t cell = 0; cell < NUM_CELLS_PER_CHIP; cell++) { + int temp_sum = 0; + for (uint8_t therm = 0; therm < RelevantThermMap[cell].size(); therm++) { + uint8_t thermNum = RelevantThermMap[cell][therm]; + temp_sum += bmsdata->chip_data[c].thermistor_value[thermNum]; + } + + /* Takes the average temperature of all the relevant thermistors */ + bmsdata->chip_data[c].cell_temp[cell] = temp_sum / RelevantThermMap[cell].size(); + + /* Cleansing value */ + if (bmsdata->chip_data[c].cell_temp[cell] > MAX_TEMP) { + bmsdata->chip_data[c].cell_temp[cell] = MAX_TEMP; + } + } + } } -void Analyzer::calcPackTemps() +void calc_pack_temps() { - bmsdata->max_temp = {MIN_TEMP, 0, 0}; - bmsdata->min_temp = {MAX_TEMP, 0, 0}; - int total_temp = 0; - int total_seg_temp = 0; - for(uint8_t c = 0; c < NUM_CHIPS; c++) - { - for(uint8_t therm = 17; therm < 28; therm++) - { - // finds out the maximum cell temp and location - if (bmsdata->chip_data[c].thermistor_value[therm] > bmsdata->max_temp.val) - { - bmsdata->max_temp = {bmsdata->chip_data[c].thermistor_value[therm], c, therm}; - } - - // finds out the minimum cell temp and location - if (bmsdata->chip_data[c].thermistor_value[therm] < bmsdata->min_temp.val) - { - bmsdata->min_temp = {bmsdata->chip_data[c].thermistor_value[therm], c, therm}; - } - - total_temp += bmsdata->chip_data[c].thermistor_value[therm]; - total_seg_temp += bmsdata->chip_data[c].thermistor_value[therm]; - } - if (c % 2 == 0) { - bmsdata->segment_average_temps[c/2] = total_seg_temp / 22; - total_seg_temp = 0; - } - } - - // takes the average of all the cell temperatures - bmsdata->avg_temp = total_temp / 88; + bmsdata->max_temp = { MIN_TEMP, 0, 0 }; + bmsdata->min_temp = { MAX_TEMP, 0, 0 }; + int total_temp = 0; + int total_seg_temp = 0; + for (uint8_t c = 0; c < NUM_CHIPS; c++) { + for (uint8_t therm = 17; therm < 28; therm++) { + /* finds out the maximum cell temp and location */ + if (bmsdata->chip_data[c].thermistor_value[therm] > bmsdata->max_temp.val) { + bmsdata->max_temp = { bmsdata->chip_data[c].thermistor_value[therm], c, therm }; + } + + /* finds out the minimum cell temp and location */ + if (bmsdata->chip_data[c].thermistor_value[therm] < bmsdata->min_temp.val) { + bmsdata->min_temp = { bmsdata->chip_data[c].thermistor_value[therm], c, therm }; + } + + total_temp += bmsdata->chip_data[c].thermistor_value[therm]; + total_seg_temp += bmsdata->chip_data[c].thermistor_value[therm]; + } + if (c % 2 == 0) { + bmsdata->segment_average_temps[c / 2] = total_seg_temp / 22; + total_seg_temp = 0; + } + } + + /* takes the average of all the cell temperatures */ + bmsdata->avg_temp = total_temp / 88; } -void Analyzer::calcPackVoltageStats() { - bmsdata->max_voltage = {MIN_VOLT_MEAS, 0, 0}; - bmsdata->max_ocv = {MIN_VOLT_MEAS, 0, 0}; - bmsdata->min_voltage = {MAX_VOLT_MEAS, 0, 0}; - bmsdata->min_ocv = {MAX_VOLT_MEAS, 0, 0}; - uint32_t total_volt = 0; - uint32_t total_ocv = 0; - for(uint8_t c = 0; c < NUM_CHIPS; c++) - { - for(uint8_t cell = 0; cell < NUM_CELLS_PER_CHIP; cell++) - { - // fings out the maximum cell voltage and location - if (bmsdata->chip_data[c].voltage_reading[cell] > bmsdata->max_voltage.val) - { - bmsdata->max_voltage = {bmsdata->chip_data[c].voltage_reading[cell], c, cell}; - } - - if (bmsdata->chip_data[c].open_cell_voltage[cell] > bmsdata->max_ocv.val) - { - bmsdata->max_ocv = {bmsdata->chip_data[c].open_cell_voltage[cell], c, cell}; - } - - //finds out the minimum cell voltage and location - if (bmsdata->chip_data[c].voltage_reading[cell] < bmsdata->min_voltage.val) - { - bmsdata->min_voltage = {bmsdata->chip_data[c].voltage_reading[cell], c, cell}; - } - - if (bmsdata->chip_data[c].open_cell_voltage[cell] < bmsdata->min_ocv.val) - { - bmsdata->min_ocv = {bmsdata->chip_data[c].open_cell_voltage[cell], c, cell}; - } - - total_volt += bmsdata->chip_data[c].voltage_reading[cell]; - total_ocv += bmsdata->chip_data[c].open_cell_voltage[cell]; - } - } - - // calculate some voltage stats - bmsdata->avg_voltage = total_volt / (NUM_CELLS_PER_CHIP * NUM_CHIPS); - bmsdata->pack_voltage = total_volt / 1000; // convert to voltage * 10 - bmsdata->delt_voltage = bmsdata->max_voltage.val - bmsdata->min_voltage.val; - - bmsdata->avg_ocv = total_ocv / (NUM_CELLS_PER_CHIP * NUM_CHIPS); - bmsdata->pack_ocv = total_ocv / 1000; // convert to voltage * 10 - bmsdata->delt_ocv = bmsdata->max_ocv.val - bmsdata->min_ocv.val; +void calc_pack_voltage_stats() +{ + bmsdata->max_voltage = { MIN_VOLT_MEAS, 0, 0 }; + bmsdata->max_ocv = { MIN_VOLT_MEAS, 0, 0 }; + bmsdata->min_voltage = { MAX_VOLT_MEAS, 0, 0 }; + bmsdata->min_ocv = { MAX_VOLT_MEAS, 0, 0 }; + uint32_t total_volt = 0; + uint32_t total_ocv = 0; + + for (uint8_t c = 0; c < NUM_CHIPS; c++) { + for (uint8_t cell = 0; cell < NUM_CELLS_PER_CHIP; cell++) { + /* fings out the maximum cell voltage and location */ + if (bmsdata->chip_data[c].voltage_reading[cell] > bmsdata->max_voltage.val) { + bmsdata->max_voltage = { bmsdata->chip_data[c].voltage_reading[cell], c, cell }; + } + + if (bmsdata->chip_data[c].open_cell_voltage[cell] > bmsdata->max_ocv.val) { + bmsdata->max_ocv = { bmsdata->chip_data[c].open_cell_voltage[cell], c, cell }; + } + + /* finds out the minimum cell voltage and location */ + if (bmsdata->chip_data[c].voltage_reading[cell] < bmsdata->min_voltage.val) { + bmsdata->min_voltage = { bmsdata->chip_data[c].voltage_reading[cell], c, cell }; + } + + if (bmsdata->chip_data[c].open_cell_voltage[cell] < bmsdata->min_ocv.val) { + bmsdata->min_ocv = { bmsdata->chip_data[c].open_cell_voltage[cell], c, cell }; + } + + total_volt += bmsdata->chip_data[c].voltage_reading[cell]; + total_ocv += bmsdata->chip_data[c].open_cell_voltage[cell]; + } + } + + /* calculate some voltage stats */ + bmsdata->avg_voltage = total_volt / (NUM_CELLS_PER_CHIP * NUM_CHIPS); + bmsdata->pack_voltage = total_volt / 1000; /* convert to voltage * 10 */ + bmsdata->delt_voltage = bmsdata->max_voltage.val - bmsdata->min_voltage.val; + + bmsdata->avg_ocv = total_ocv / (NUM_CELLS_PER_CHIP * NUM_CHIPS); + bmsdata->pack_ocv = total_ocv / 1000; /* convert to voltage * 10 */ + bmsdata->delt_ocv = bmsdata->max_ocv.val - bmsdata->min_ocv.val; } -void Analyzer::calcCellResistances() +void calc_cell_resistances() { - for(uint8_t c = 0; c < NUM_CHIPS; c++) - { - for(uint8_t cell = 0; cell < NUM_CELLS_PER_CHIP; cell++) - { - uint8_t cell_temp = bmsdata->chip_data[c].cell_temp[cell]; - uint8_t resIndex = (cell_temp - MIN_TEMP) / 5; //resistance LUT increments by 5C for each index - - bmsdata->chip_data[c].cell_resistance[cell] = TEMP_TO_CELL_RES[resIndex]; - - //Linear interpolation to more accurately represent cell resistances in between increments of 5C - if(cell_temp != MAX_TEMP) - { - float interpolation = (TEMP_TO_CELL_RES[resIndex+1] - TEMP_TO_CELL_RES[resIndex]) / 5; - bmsdata->chip_data[c].cell_resistance[cell] += (interpolation * (cell_temp % 5)); - } - } - } + for (uint8_t c = 0; c < NUM_CHIPS; c++) { + for (uint8_t cell = 0; cell < NUM_CELLS_PER_CHIP; cell++) { + + uint8_t cell_temp = bmsdata->chip_data[c].cell_temp[cell]; + + /* resistance LUT increments by 5C for each index */ + uint8_t resIndex = (cell_temp - MIN_TEMP) / 5; + + bmsdata->chip_data[c].cell_resistance[cell] = TEMP_TO_CELL_RES[resIndex]; + + /* Linear interpolation to more accurately represent cell resistances in between + * increments of 5C */ + if (cell_temp != MAX_TEMP) { + float interpolation + = (TEMP_TO_CELL_RES[resIndex + 1] - TEMP_TO_CELL_RES[resIndex]) / 5; + bmsdata->chip_data[c].cell_resistance[cell] += (interpolation * (cell_temp % 5)); + } + } + } } -void Analyzer::calcDCL() +void calc_dcl() { - typedef enum - { - BEFORE_TIMER_START, - DURING_DCL_EVAL - }DCL_state; - - struct DCLeval - { - DCL_state state = BEFORE_TIMER_START; - Timer timer; - }; - - DCLeval dclEval; - int16_t current_limit = 0x7FFF; - - for(uint8_t c = 0; c < NUM_CHIPS; c++) - { - for(uint8_t cell = 0; cell < NUM_CELLS_PER_CHIP; cell++) - { - // Apply equation - uint16_t tmpDCL = (bmsdata->chip_data[c].open_cell_voltage[cell] - ((MIN_VOLT + VOLT_SAG_MARGIN) * 10000)) / (bmsdata->chip_data[c].cell_resistance[cell] * 10); - // Multiplying resistance by 10 to convert from mOhm to Ohm and then to Ohm * 10000 to account for the voltage units - - //Taking the minimum DCL of all the cells - if(tmpDCL < current_limit) current_limit = tmpDCL; - } - } - - // ceiling for current limit - if (current_limit > MAX_CELL_CURR) - { - bmsdata->discharge_limit = MAX_CELL_CURR; - } - - else if (dclEval.state == BEFORE_TIMER_START && current_limit < 5) // 5 is arbitrary @ matt adjust as needed - { - if (prevbmsdata == nullptr) - { - bmsdata->discharge_limit = current_limit; - return; - } - - bmsdata->discharge_limit = prevbmsdata->discharge_limit; - dclEval.state = DURING_DCL_EVAL; - dclEval.timer.startTimer(500); // 1 second is arbitrary @ matt adjust as needed - } - - else if (dclEval.state == DURING_DCL_EVAL) - { - if (dclEval.timer.isTimerExpired()) - { - bmsdata->discharge_limit = current_limit; - } - if (current_limit > 5) - { - bmsdata->discharge_limit = current_limit; - dclEval.state = BEFORE_TIMER_START; - dclEval.timer.cancelTimer(); - } - - else - { - bmsdata->discharge_limit = prevbmsdata->discharge_limit; - } - } - else - { - bmsdata->discharge_limit = current_limit; - } - - if (bmsdata->discharge_limit > DCDC_CURRENT_DRAW) - bmsdata->discharge_limit -= DCDC_CURRENT_DRAW; + typedef enum { BEFORE_TIMER_START, DURING_DCL_EVAL } DCL_state; + + struct DCLeval { + DCL_state state = BEFORE_TIMER_START; + Timer timer; + }; + + DCLeval dclEval; + int16_t current_limit = 0x7FFF; + + for (uint8_t c = 0; c < NUM_CHIPS; c++) { + for (uint8_t cell = 0; cell < NUM_CELLS_PER_CHIP; cell++) { + /* Apply equation */ + /* Multiplying resistance by 10 to convert from mOhm to Ohm and then to Ohm * 10000 to + * account for the voltage units */ + uint16_t tmpDCL = (bmsdata->chip_data[c].open_cell_voltage[cell] + - ((MIN_VOLT + VOLT_SAG_MARGIN) * 10000)) + / (bmsdata->chip_data[c].cell_resistance[cell] * 10); + + /* Taking the minimum DCL of all the cells */ + if (tmpDCL < current_limit) + current_limit = tmpDCL; + } + } + + /* ceiling for current limit */ + if (current_limit > MAX_CELL_CURR) { + bmsdata->discharge_limit = MAX_CELL_CURR; + } + + else if (dclEval.state == BEFORE_TIMER_START && current_limit < 5) { + if (prevbmsdata == nullptr) { + bmsdata->discharge_limit = current_limit; + return; + } + + bmsdata->discharge_limit = prevbmsdata->discharge_limit; + dclEval.state = DURING_DCL_EVAL; + dclEval.timer.startTimer(500); + } + + else if (dclEval.state == DURING_DCL_EVAL) { + if (dclEval.timer.isTimerExpired()) { + bmsdata->discharge_limit = current_limit; + } + if (current_limit > 5) { + bmsdata->discharge_limit = current_limit; + dclEval.state = BEFORE_TIMER_START; + dclEval.timer.cancelTimer(); + } + + else { + bmsdata->discharge_limit = prevbmsdata->discharge_limit; + } + } else { + bmsdata->discharge_limit = current_limit; + } + + if (bmsdata->discharge_limit > DCDC_CURRENT_DRAW) + bmsdata->discharge_limit -= DCDC_CURRENT_DRAW; } -void Analyzer::calcContDCL() +void calc_cont_dcl() { - uint8_t min_res_index = (bmsdata->min_temp.val - MIN_TEMP) / 5; //resistance LUT increments by 5C for each index - uint8_t max_res_index = (bmsdata->max_temp.val - MIN_TEMP) / 5; - - if (TEMP_TO_DCL[min_res_index] < TEMP_TO_DCL[max_res_index]) - { - bmsdata->cont_DCL = TEMP_TO_DCL[min_res_index]; - } - else - { - bmsdata->cont_DCL = TEMP_TO_DCL[max_res_index]; - } + uint8_t min_res_index = (bmsdata->min_temp.val - MIN_TEMP) + / 5; /* resistance LUT increments by 5C for each index */ + uint8_t max_res_index = (bmsdata->max_temp.val - MIN_TEMP) / 5; + + if (TEMP_TO_DCL[min_res_index] < TEMP_TO_DCL[max_res_index]) { + bmsdata->cont_DCL = TEMP_TO_DCL[min_res_index]; + } else { + bmsdata->cont_DCL = TEMP_TO_DCL[max_res_index]; + } } -void Analyzer::calcCCL() +void calcCCL() { - int16_t currentLimit = 0x7FFF; - - for (uint8_t c = 0; c < NUM_CHIPS; c++) - { - for (uint8_t cell = 0; cell < NUM_CELLS_PER_CHIP; cell++) - { - // Apply equation - uint16_t tmpCCL = ((MAX_VOLT * 10000) - (bmsdata->chip_data[c].open_cell_voltage[cell] + (VOLT_SAG_MARGIN * 10000))) / (bmsdata->chip_data[c].cell_resistance[cell] * 10); - // Multiplying resistance by 10 to convert from mOhm to Ohm and then to Ohm * 10000 to account for the voltage units - - //Taking the minimum CCL of all the cells - if (tmpCCL < currentLimit) currentLimit = tmpCCL; - } - } - - // ceiling for current limit - if (currentLimit > MAX_CHG_CELL_CURR) - { - bmsdata->charge_limit = MAX_CHG_CELL_CURR; - } - else - { - bmsdata->charge_limit = currentLimit; - } + int16_t currentLimit = 0x7FFF; + + for (uint8_t c = 0; c < NUM_CHIPS; c++) { + for (uint8_t cell = 0; cell < NUM_CELLS_PER_CHIP; cell++) { + /* Apply equation */ + uint16_t tmpCCL + = ((MAX_VOLT * 10000) + - (bmsdata->chip_data[c].open_cell_voltage[cell] + (VOLT_SAG_MARGIN * 10000))) + / (bmsdata->chip_data[c].cell_resistance[cell] * 10); + /* Multiplying resistance by 10 to convert from mOhm to Ohm and then to Ohm * 10000 to + * account for the voltage units */ + + /* Taking the minimum CCL of all the cells */ + if (tmpCCL < currentLimit) + currentLimit = tmpCCL; + } + } + + /* ceiling for current limit */ + if (currentLimit > MAX_CHG_CELL_CURR) { + bmsdata->charge_limit = MAX_CHG_CELL_CURR; + } else { + bmsdata->charge_limit = currentLimit; + } } - -void Analyzer::calcContCCL() +void calc_cont_ccl() { - uint8_t min_res_index = (bmsdata->min_temp.val - MIN_TEMP) / 5; //resistance LUT increments by 5C for each index - uint8_t max_res_index = (bmsdata->max_temp.val - MIN_TEMP) / 5; - - if (TEMP_TO_CCL[min_res_index] < TEMP_TO_CCL[max_res_index]) - { - bmsdata->cont_CCL = TEMP_TO_CCL[min_res_index]; - } - else - { - bmsdata->cont_CCL = TEMP_TO_CCL[max_res_index]; - } + uint8_t min_res_index = (bmsdata->min_temp.val - MIN_TEMP) + / 5; /* resistance LUT increments by 5C for each index */ + uint8_t max_res_index = (bmsdata->max_temp.val - MIN_TEMP) / 5; + + if (TEMP_TO_CCL[min_res_index] < TEMP_TO_CCL[max_res_index]) { + bmsdata->cont_CCL = TEMP_TO_CCL[min_res_index]; + } else { + bmsdata->cont_CCL = TEMP_TO_CCL[max_res_index]; + } } -void Analyzer::calcOpenCellVoltage() +void calc_open_cell_voltage() { - // if there is no previous data point, set inital open cell voltage to current reading - if (is_first_reading_) - { - for (uint8_t chip = 0; chip < NUM_CHIPS; chip++) - { - for (uint8_t cell = 0; cell < NUM_CELLS_PER_CHIP; cell++) - { - bmsdata->chip_data[chip].open_cell_voltage[cell] = bmsdata->chip_data[chip].voltage_reading[cell]; - } - } - return; - } - // If we are within the current threshold for open voltage measurments - else if (bmsdata->pack_current < (OCV_CURR_THRESH * 10) && bmsdata->pack_current > (-OCV_CURR_THRESH * 10)) - { - if (ocvTimer.isTimerExpired()) { - for (uint8_t chip = 0; chip < NUM_CHIPS; chip++) - { - for (uint8_t cell = 0; cell < NUM_CELLS_PER_CHIP; cell++) - { - // Sets open cell voltage to a moving average of OCV_AVG values - bmsdata->chip_data[chip].open_cell_voltage[cell] = (uint32_t(bmsdata->chip_data[chip].voltage_reading[cell]) + (uint32_t(prevbmsdata->chip_data[chip].open_cell_voltage[cell]) * (OCV_AVG - 1))) / OCV_AVG; - bmsdata->chip_data[chip].open_cell_voltage[cell] = bmsdata->chip_data[chip].voltage_reading[cell]; - - if (bmsdata->chip_data[chip].open_cell_voltage[cell] > MAX_VOLT * 10000) - { - bmsdata->chip_data[chip].open_cell_voltage[cell] = prevbmsdata->chip_data[chip].open_cell_voltage[cell]; - } - else if (bmsdata->chip_data[chip].open_cell_voltage[cell] < MIN_VOLT * 10000) - { - bmsdata->chip_data[chip].open_cell_voltage[cell] = prevbmsdata->chip_data[chip].open_cell_voltage[cell]; - } - } - } - return; - } - } - else - { - ocvTimer.startTimer(1000); - } - for (uint8_t chip = 0; chip < NUM_CHIPS; chip++) - { - for (uint8_t cell = 0; cell < NUM_CELLS_PER_CHIP; cell++) - { - // Set OCV to the previous/existing OCV - bmsdata->chip_data[chip].open_cell_voltage[cell] = prevbmsdata->chip_data[chip].open_cell_voltage[cell]; - } - } + /* if there is no previous data point, set inital open cell voltage to current reading */ + if (is_first_reading_) { + for (uint8_t chip = 0; chip < NUM_CHIPS; chip++) { + for (uint8_t cell = 0; cell < NUM_CELLS_PER_CHIP; cell++) { + bmsdata->chip_data[chip].open_cell_voltage[cell] + = bmsdata->chip_data[chip].voltage_reading[cell]; + } + } + return; + } + /* If we are within the current threshold for open voltage measurments */ + else if (bmsdata->pack_current < (OCV_CURR_THRESH * 10) + && bmsdata->pack_current > (-OCV_CURR_THRESH * 10)) { + if (ocvTimer.isTimerExpired()) { + for (uint8_t chip = 0; chip < NUM_CHIPS; chip++) { + for (uint8_t cell = 0; cell < NUM_CELLS_PER_CHIP; cell++) { + /* Sets open cell voltage to a moving average of OCV_AVG values */ + bmsdata->chip_data[chip].open_cell_voltage[cell] + = (uint32_t(bmsdata->chip_data[chip].voltage_reading[cell]) + + (uint32_t(prevbmsdata->chip_data[chip].open_cell_voltage[cell]) + * (OCV_AVG - 1))) + / OCV_AVG; + bmsdata->chip_data[chip].open_cell_voltage[cell] + = bmsdata->chip_data[chip].voltage_reading[cell]; + + if (bmsdata->chip_data[chip].open_cell_voltage[cell] > MAX_VOLT * 10000) { + bmsdata->chip_data[chip].open_cell_voltage[cell] + = prevbmsdata->chip_data[chip].open_cell_voltage[cell]; + } else if (bmsdata->chip_data[chip].open_cell_voltage[cell] + < MIN_VOLT * 10000) { + bmsdata->chip_data[chip].open_cell_voltage[cell] + = prevbmsdata->chip_data[chip].open_cell_voltage[cell]; + } + } + } + return; + } + } else { + ocvTimer.startTimer(1000); + } + for (uint8_t chip = 0; chip < NUM_CHIPS; chip++) { + for (uint8_t cell = 0; cell < NUM_CELLS_PER_CHIP; cell++) { + /* Set OCV to the previous/existing OCV */ + bmsdata->chip_data[chip].open_cell_voltage[cell] + = prevbmsdata->chip_data[chip].open_cell_voltage[cell]; + } + } } -uint8_t Analyzer::calcFanPWM() +uint8_t calcFanPWM() { - // Resistance LUT increments by 5C for each index, plus we account for negative minimum - uint8_t min_res_index = (bmsdata->max_temp.val - MIN_TEMP) / 5; - // Ints are roounded down, so this would be the value if rounded up - uint8_t max_res_index = (bmsdata->max_temp.val - MIN_TEMP) / 5 + 1; - // Determine how far into the 5C interval the temp is - uint8_t part_of_index = (bmsdata->max_temp.val - MIN_TEMP) % 5; - - // Uses fan LUT and finds low and upper end. Then takes average, weighted to how far into the interval the exact temp is - return ((FAN_CURVE[max_res_index] * part_of_index) + (FAN_CURVE[min_res_index] * (5 - part_of_index))) / (2 * 5); + /* Resistance LUT increments by 5C for each index, plus we account for negative minimum */ + uint8_t min_res_index = (bmsdata->max_temp.val - MIN_TEMP) / 5; + /* Ints are roounded down, so this would be the value if rounded up */ + uint8_t max_res_index = (bmsdata->max_temp.val - MIN_TEMP) / 5 + 1; + /* Determine how far into the 5C interval the temp is */ + uint8_t part_of_index = (bmsdata->max_temp.val - MIN_TEMP) % 5; + + /* Uses fan LUT and finds low and upper end. Then takes average, weighted to how far into the + * interval the exact temp is */ + return ((FAN_CURVE[max_res_index] * part_of_index) + + (FAN_CURVE[min_res_index] * (5 - part_of_index))) + / (2 * 5); } -void Analyzer::disableTherms() +void disable_therms() { - int8_t temp_rep_1 = 25; // Iniitalize to room temp (necessary to stabilize when the BMS first boots up/has null values) - //if (!is_first_reading_) temp_rep_1 = prevbmsdata->avg_temp; // Set to actual average temp of the pack - - for(uint8_t c = 0; c < NUM_CHIPS; c++) - { - for(uint8_t therm = 17; therm < 28; therm++) - { - // If 2D LUT shows therm should be disable - if (THERM_DISABLE[(c - 1) / 2][therm - 17]) - { - // Nullify thermistor by setting to pack average - bmsdata->chip_data[c].thermistor_value[therm] = temp_rep_1; - } - } - } + int8_t temp_rep_1 = 25; /* Iniitalize to room temp (necessary to stabilize when the BMS first + boots up/has null values) */ + // if (!is_first_reading_) temp_rep_1 = prevbmsdata->avg_temp; /* Set to actual average temp of + // the pack */ + + for (uint8_t c = 0; c < NUM_CHIPS; c++) { + for (uint8_t therm = 17; therm < 28; therm++) { + /* If 2D LUT shows therm should be disable */ + if (THERM_DISABLE[(c - 1) / 2][therm - 17]) { + /* Nullify thermistor by setting to pack average */ + bmsdata->chip_data[c].thermistor_value[therm] = temp_rep_1; + } + } + } } -void Analyzer::calcStateOfCharge() +void calc_state_of_charge() { - /* Spltting the delta voltage into 18 increments */ - const uint16_t increments = ((uint16_t)(MAX_VOLT*10000 - MIN_VOLT*10000) / ((MAX_VOLT - MIN_VOLT) * 10)); - - /* Retrieving a index of 0-18 */ - uint8_t index = ((bmsdata->min_ocv.val) - MIN_VOLT*10000) / increments; - - bmsdata->soc = STATE_OF_CHARGE_CURVE[index]; - - if (bmsdata->soc != 100) - { - float interpolation = (float)(STATE_OF_CHARGE_CURVE[index+1] - STATE_OF_CHARGE_CURVE[index]) / increments; - bmsdata->soc += (uint8_t)(interpolation * (((bmsdata->min_ocv.val) - (int32_t)(MIN_VOLT * 10000)) % increments)); - } - - if (bmsdata->soc < 0) - { - bmsdata->soc = 0; - } + /* Spltting the delta voltage into 18 increments */ + const uint16_t increments + = ((uint16_t)(MAX_VOLT * 10000 - MIN_VOLT * 10000) / ((MAX_VOLT - MIN_VOLT) * 10)); + + /* Retrieving a index of 0-18 */ + uint8_t index = ((bmsdata->min_ocv.val) - MIN_VOLT * 10000) / increments; + + bmsdata->soc = STATE_OF_CHARGE_CURVE[index]; + + if (bmsdata->soc != 100) { + float interpolation + = (float)(STATE_OF_CHARGE_CURVE[index + 1] - STATE_OF_CHARGE_CURVE[index]) / increments; + bmsdata->soc + += (uint8_t)(interpolation + * (((bmsdata->min_ocv.val) - (int32_t)(MIN_VOLT * 10000)) % increments)); + } + + if (bmsdata->soc < 0) { + bmsdata->soc = 0; + } } -void Analyzer::highCurrThermCheck() +void high_curr_therm_check() { - if (prevbmsdata == nullptr) - return; - - if (bmsdata->pack_current > 500) { - - for(uint8_t c = 0; c < NUM_CHIPS; c++) - { - for(uint8_t cell = 0; cell < NUM_CELLS_PER_CHIP; cell++) - { - bmsdata->chip_data[c].thermistor_reading[cell] = prevbmsdata->chip_data[c].thermistor_reading[cell]; - bmsdata->chip_data[c].thermistor_value[cell] = prevbmsdata->chip_data[c].thermistor_value[cell]; - } - } - } + if (prevbmsdata == nullptr) + return; + + if (bmsdata->pack_current > 500) { + + for (uint8_t c = 0; c < NUM_CHIPS; c++) { + for (uint8_t cell = 0; cell < NUM_CELLS_PER_CHIP; cell++) { + bmsdata->chip_data[c].thermistor_reading[cell] + = prevbmsdata->chip_data[c].thermistor_reading[cell]; + bmsdata->chip_data[c].thermistor_value[cell] + = prevbmsdata->chip_data[c].thermistor_value[cell]; + } + } + } } -void Analyzer::diffCurrThermCheck() +void diff_curr_therm_check() { - if (prevbmsdata == nullptr) - return; - - if (abs(bmsdata->pack_current - prevbmsdata->pack_current) > 100) { - for(uint8_t c = 0; c < NUM_CHIPS; c++) - { - for(uint8_t cell = 0; cell < NUM_CELLS_PER_CHIP; cell++) - { - bmsdata->chip_data[c].thermistor_reading[cell] = prevbmsdata->chip_data[c].thermistor_reading[cell]; - bmsdata->chip_data[c].thermistor_value[cell] = prevbmsdata->chip_data[c].thermistor_value[cell]; - } - } - } + if (prevbmsdata == nullptr) + return; + + if (abs(bmsdata->pack_current - prevbmsdata->pack_current) > 100) { + for (uint8_t c = 0; c < NUM_CHIPS; c++) { + for (uint8_t cell = 0; cell < NUM_CELLS_PER_CHIP; cell++) { + bmsdata->chip_data[c].thermistor_reading[cell] + = prevbmsdata->chip_data[c].thermistor_reading[cell]; + bmsdata->chip_data[c].thermistor_value[cell] + = prevbmsdata->chip_data[c].thermistor_value[cell]; + } + } + } } - - - - diff --git a/Core/Src/compute.c b/Core/Src/compute.c index 52b36b0..031f9d9 100644 --- a/Core/Src/compute.c +++ b/Core/Src/compute.c @@ -1,418 +1,391 @@ #include "compute.h" +uint8_t fan_speed; +bool is_charging_enabled; +enum { CHARGE_ENABLED, CHARGE_DISABLED }; + ComputeInterface compute; -ComputeInterface::ComputeInterface() +void compute_init() { - pinMode(CURRENT_SENSOR_PIN_H, INPUT); - pinMode(CURRENT_SENSOR_PIN_L, INPUT); - pinMode(MEAS_5VREF_PIN, INPUT); - pinMode(FAULT_PIN, OUTPUT); - pinMode(CHARGE_DETECT, INPUT); - initializeCAN(CANLINE_2, CHARGER_BAUD, &(this->chargerCallback)); - initializeCAN(CANLINE_1, MC_BAUD, &(this->MCCallback)); + // TODO UPDATE DRIVER HERE + pinMode(CURRENT_SENSOR_PIN_H, INPUT); + pinMode(CURRENT_SENSOR_PIN_L, INPUT); + pinMode(MEAS_5VREF_PIN, INPUT); + pinMode(FAULT_PIN, OUTPUT); + pinMode(CHARGE_DETECT, INPUT); + initializeCAN(CANLINE_2, CHARGER_BAUD, &(this->compute_charger_callback)); + initializeCAN(CANLINE_1, MC_BAUD, &(this->compute_mc_callback)); } -ComputeInterface::~ComputeInterface(){} - -void ComputeInterface::enableCharging(bool enable_charging) +void compute_enable_charging(bool enable_charging) { - is_charging_enabled_ = enable_charging; + is_charging_enabled_ = enable_charging; } -FaultStatus_t ComputeInterface::sendChargingMessage(uint16_t voltage_to_set, AccumulatorData_t *bms_data) +FaultStatus_t compute_send_charging_message(uint16_t voltage_to_set, AccumulatorData_t* bms_data) { - union - { - uint8_t msg[8] = {0, 0, 0, 0, 0, 0, 0xFF, 0xFF}; - - struct - { - uint8_t chargerControl; - uint16_t chargerVoltage; //Note the charger voltage sent over should be 10*desired voltage - uint16_t chargerCurrent; //Note the charge current sent over should be 10*desired current + 3200 - uint8_t chargerLEDs; - uint16_t reserved2_3; - } cfg; - } chargerMsg; - - uint16_t current_to_set = bms_data->charge_limit; - - if (!is_charging_enabled_) - { - chargerMsg.cfg.chargerControl = 0b101; - sendMessageCAN2(CANMSG_CHARGER, 8, chargerMsg.msg); - //return isCharging() ? FAULTED : NOT_FAULTED; //return a fault if we DO detect a voltage after we stop charging - return NOT_FAULTED; - } - - // equations taken from TSM2500 CAN protocol datasheet - chargerMsg.cfg.chargerControl = 0xFC; - chargerMsg.cfg.chargerVoltage = voltage_to_set * 10; - if (current_to_set > 10) - { - current_to_set = 10; - } - chargerMsg.cfg.chargerCurrent = current_to_set * 10 + 3200; - chargerMsg.cfg.chargerLEDs = calcChargerLEDState(bms_data); - chargerMsg.cfg.reserved2_3 = 0xFFFF; - - uint8_t msg[8] = {chargerMsg.cfg.chargerControl, static_cast(chargerMsg.cfg.chargerVoltage), chargerMsg.cfg.chargerVoltage >> 8, static_cast(chargerMsg.cfg.chargerCurrent), chargerMsg.cfg.chargerCurrent >> 8, chargerMsg.cfg.chargerLEDs, 0xFF, 0xFF}; - - sendMessageCAN2(CANMSG_CHARGER, 8, msg); - - //return isCharging() ? NOT_FAULTED : FAULTED; //return a fault if we DON'T detect a voltage after we begin charging - return NOT_FAULTED; + struct __attribute__((packed)) { + uint8_t charger_control; + uint16_t charger_voltage; // Note the charger voltage sent over should be 10*desired voltage + uint16_t charger_current; // Note the charge current sent over should be 10*desired current + // + 3200 + uint8_t charger_leds; + uint16_t reserved2_3; + } charger_msg; + + uint16_t current_to_set = bms_data->charge_limit; + + if (!is_charging_enabled_) { + charger_msg.charger_control = 0b101; + sendMessageCAN2(CANMSG_CHARGER, 8, charger_msg); + // return isCharging() ? FAULTED : NOT_FAULTED; //return a fault if we DO detect a voltage + // after we stop charging + return NOT_FAULTED; + } + + // equations taken from TSM2500 CAN protocol datasheet + charger_msg.charger_control = 0xFC; + charger_msg.charger_voltage = voltage_to_set * 10; + if (current_to_set > 10) { + current_to_set = 10; + } + charger_msg.charger_current = current_to_set * 10 + 3200; + charger_msg.charger_leds = calc_charger_led_state(bms_data); + charger_msg.reserved2_3 = 0xFFFF; + + unit8_t buf[8] = { 0 }; + memcpy(buf, &charger_msg, sizeof(charger_msg)); + + sendMessageCAN2(CANMSG_CHARGER, 8, buf); + + // return isCharging() ? NOT_FAULTED : FAULTED; //return a fault if we DON'T detect a voltage + // after we begin charging + return NOT_FAULTED; } -bool ComputeInterface::chargerConnected() +bool compute_charger_connected() { - return !(digitalRead(CHARGE_DETECT) == HIGH); + return !(digitalRead(CHARGE_DETECT) == HIGH); } -void ComputeInterface::chargerCallback(const CAN_message_t &msg) +void compute_charger_callback(const CAN_message_t& msg) { - return; + return; } -void ComputeInterface::setFanSpeed(uint8_t new_fan_speed) +void compute_set_fan_speed(uint8_t new_fan_speed) { - fan_speed_ = new_fan_speed; - //NERduino.setAMCDutyCycle(new_fan_speed); Replace + fan_speed_ = new_fan_speed; + // NERduino.setAMCDutyCycle(new_fan_speed); Replace } -void ComputeInterface::setFault(FaultStatus_t fault_state) +void compute_set_fault(FaultStatus_t fault_state) { - digitalWrite(FAULT_PIN, !fault_state); - if (FAULTED) digitalWrite(CHARGE_SAFETY_RELAY, HIGH); + digitalWrite(FAULT_PIN, !fault_state); + if (FAULTED) + digitalWrite(CHARGE_SAFETY_RELAY, HIGH); } -int16_t ComputeInterface::getPackCurrent() +int16_t compute_get_pack_current() { - static const float CURRENT_LOWCHANNEL_MAX = 75.0; //Amps - static const float CURRENT_LOWCHANNEL_MIN = -75.0; //Amps - // static const float CURRENT_SUPPLY_VOLTAGE = 5.038; - static const float CURRENT_ADC_RESOLUTION = 5.0 / MAX_ADC_RESOLUTION; - - static const float CURRENT_LOWCHANNEL_OFFSET = 2.517; // Calibrated with current = 0A - static const float CURRENT_HIGHCHANNEL_OFFSET = 2.520; // Calibrated with current = 0A - - static const float HIGHCHANNEL_GAIN = 1 / 0.004; // Calibrated with current = 5A, 10A, 20A - static const float LOWCHANNEL_GAIN = 1 / 0.0267; - - static const float REF5V_DIV = 19.02 / (19.08 + 19.02); // Resistive divider in kOhm - static const float REF5V_CONV = 1 / REF5V_DIV; // Converting from reading to real value - - float ref_5V = analogRead(MEAS_5VREF_PIN) * (3.3 / MAX_ADC_RESOLUTION) * REF5V_CONV; - int16_t high_current = 10 * (((5 / ref_5V) * (analogRead(CURRENT_SENSOR_PIN_L) * CURRENT_ADC_RESOLUTION)) - CURRENT_HIGHCHANNEL_OFFSET) * HIGHCHANNEL_GAIN; // Channel has a large range with low resolution - int16_t low_current = 10 * (((5 / ref_5V) * (analogRead(CURRENT_SENSOR_PIN_H) * CURRENT_ADC_RESOLUTION)) - CURRENT_LOWCHANNEL_OFFSET) * LOWCHANNEL_GAIN; // Channel has a small range with high resolution - - // Serial.print("High: "); - // Serial.println(-high_current); - // Serial.print("Low: "); - // Serial.println(-low_current); - // Serial.print("5V: "); - // Serial.println(ref_5V); - - // If the current is scoped within the range of the low channel, use the low channel - if(low_current < CURRENT_LOWCHANNEL_MAX - 5.0 || low_current > CURRENT_LOWCHANNEL_MIN + 5.0) - { - return -low_current; - } - - return -high_current; + static const float CURRENT_LOWCHANNEL_MAX = 75.0; // Amps + static const float CURRENT_LOWCHANNEL_MIN = -75.0; // Amps + // static const float CURRENT_SUPPLY_VOLTAGE = 5.038; + static const float CURRENT_ADC_RESOLUTION = 5.0 / MAX_ADC_RESOLUTION; + + static const float CURRENT_LOWCHANNEL_OFFSET = 2.517; // Calibrated with current = 0A + static const float CURRENT_HIGHCHANNEL_OFFSET = 2.520; // Calibrated with current = 0A + + static const float HIGHCHANNEL_GAIN = 1 / 0.004; // Calibrated with current = 5A, 10A, 20A + static const float LOWCHANNEL_GAIN = 1 / 0.0267; + + static const float REF5V_DIV = 19.02 / (19.08 + 19.02); // Resistive divider in kOhm + static const float REF5V_CONV = 1 / REF5V_DIV; // Converting from reading to real value + + float ref_5V = analogRead(MEAS_5VREF_PIN) * (3.3 / MAX_ADC_RESOLUTION) * REF5V_CONV; + int16_t high_current + = 10 + * (((5 / ref_5V) * (analogRead(CURRENT_SENSOR_PIN_L) * CURRENT_ADC_RESOLUTION)) + - CURRENT_HIGHCHANNEL_OFFSET) + * HIGHCHANNEL_GAIN; // Channel has a large range with low resolution + int16_t low_current + = 10 + * (((5 / ref_5V) * (analogRead(CURRENT_SENSOR_PIN_H) * CURRENT_ADC_RESOLUTION)) + - CURRENT_LOWCHANNEL_OFFSET) + * LOWCHANNEL_GAIN; // Channel has a small range with high resolution + + // Serial.print("High: "); + // Serial.println(-high_current); + // Serial.print("Low: "); + // Serial.println(-low_current); + // Serial.print("5V: "); + // Serial.println(ref_5V); + + // If the current is scoped within the range of the low channel, use the low channel + if (low_current < CURRENT_LOWCHANNEL_MAX - 5.0 || low_current > CURRENT_LOWCHANNEL_MIN + 5.0) { + return -low_current; + } + + return -high_current; } -void ComputeInterface::sendMCMsg(uint16_t user_max_charge, uint16_t user_max_discharge) +void compute_send_mc_message(uint16_t user_max_charge, uint16_t user_max_discharge) { - union - { - uint8_t msg[4] = {0,0,0,0}; - struct - { - uint16_t maxDischarge; - uint16_t maxCharge; + struct __attribute__((packed)) { + uint16_t maxDischarge; + uint16_t maxCharge; + + } mcMsg; + + mcMsg.maxCharge = user_max_charge; + mcMsg.maxDischarge = user_max_discharge; - }config; - }mcMsg; + unit8_t buf[4] = { 0 }; + memcpy(buf, &mcMsg, sizeof(mcMsg)); - mcMsg.config.maxCharge = user_max_charge; - mcMsg.config.maxDischarge = user_max_discharge; - sendMessageCAN1(CANMSG_BMSCURRENTLIMITS, 4, mcMsg.msg); + sendMessageCAN1(CANMSG_BMSCURRENTLIMITS, 4, buf); } -void ComputeInterface::sendAccStatusMessage(uint16_t voltage, int16_t current, uint16_t ah, uint8_t soc, uint8_t health) +void compute_send_acc_status_message(AccumulatorData_t* bmsdata) { - union - { - uint8_t msg[8] = {0, 0, 0, 0, 0, 0, 0, 0}; - - struct - { - uint16_t packVolt; - uint16_t pack_current; - uint16_t packAH; - uint8_t packSoC; - uint8_t packHealth; - } cfg; - } accStatusMsg; - - accStatusMsg.cfg.packVolt = __builtin_bswap16(voltage); - accStatusMsg.cfg.pack_current = __builtin_bswap16(static_cast(current)); // convert with 2s complement - accStatusMsg.cfg.packAH = __builtin_bswap16(ah); - accStatusMsg.cfg.packSoC = soc; - accStatusMsg.cfg.packHealth = health; - - sendMessageCAN1(CANMSG_BMSACCSTATUS, 8, accStatusMsg.msg); + + struct __attribute__((packed)) { + uint16_t packVolt; + uint16_t pack_current; + uint16_t pack_ah; + uint8_t pack_soc; + uint8_t pack_health; + } acc_status_msg; + + acc_status_msg.cfg.packVolt = __builtin_bswap16(bmsdata->pack_voltage); + acc_status_msg.cfg.pack_current = __builtin_bswap16( + static_cast(bmsdata->pack_current)); // convert with 2s complement + acc_status_msg.cfg.pack_ah = __builtin_bswap16(0); + acc_status_msg.cfg.pack_soc = bmsdata->soc; + acc_status_msg.cfg.pack_health = 0; + + unit8_t buf[8] = { 0 }; + memcpy(buf, &acc_status_msg, sizeof(acc_status_msg)); + sendMessageCAN1(CANMSG_BMSACCSTATUS, 8, buf); } -void ComputeInterface::sendBMSStatusMessage(int bms_state, uint32_t fault_status, int8_t avg_temp, int8_t internal_temp, bool balance) +void compute_send_bms_status_message(AccumulatorData_t* bmsdata, int bms_state, bool balance) { - union - { - uint8_t msg[8] = {0, 0, 0, 0, 0, 0, 0, 0}; - - struct - { - uint8_t state; - uint32_t fault; - int8_t temp_avg; - uint8_t temp_internal; - uint8_t balance; - } cfg; - } bmsStatusMsg; - - bmsStatusMsg.cfg.temp_avg = static_cast(avg_temp); - bmsStatusMsg.cfg.state = static_cast(bms_state); - bmsStatusMsg.cfg.fault = fault_status; - bmsStatusMsg.cfg.temp_internal = static_cast(internal_temp); - bmsStatusMsg.cfg.balance = static_cast(balance); - - uint8_t msg[8] = { - bmsStatusMsg.cfg.state, - (fault_status & 0xff000000), - (fault_status & 0x00ff0000), - (fault_status & 0x0000ff00), - (fault_status & 0x000000ff), - bmsStatusMsg.cfg.temp_avg, - bmsStatusMsg.cfg.balance - }; - - - sendMessageCAN1(CANMSG_BMSDTCSTATUS, 8, msg); + + struct __attribute__((packed)) { + uint8_t state; + uint32_t fault; + int8_t temp_avg; + uint8_t temp_internal; + uint8_t balance; + } bms_status_msg; + + bms_status_msg.temp_avg = static_cast(bms_data->avg_temp); + bms_status_msg.state = static_cast(bms_state); + bms_status_msg.fault = bmsdata->fault_code; + bms_status_msg.temp_internal = static_cast(0); + bms_status_msg.balance = static_cast(balance); + + /* uint8_t msg[8] = { + bms_status_msg.cfg.state, + (fault_status & 0xff000000), + (fault_status & 0x00ff0000), + (fault_status & 0x0000ff00), + (fault_status & 0x000000ff), + bms_status_msg.cfg.temp_avg, + bms_status_msg.cfg.balance + }; + */ + unit8_t buf[8] = { 0 }; + memcpy(buf, &bms_status_msg, sizeof(bmsStatusMsg)); + sendMessageCAN1(CANMSG_BMSDTCSTATUS, 8, buf); } -void ComputeInterface::sendShutdownControlMessage(uint8_t mpe_state) +void compute_send_shutdown_ctrl_message(uint8_t mpe_state) { - union - { - uint8_t msg[1] = {0}; - struct - { - uint8_t mpeState; + struct __attribute__((packed)) { + uint8_t mpeState; - } cfg; - } shutdownControlMsg; + } shutdownControlMsg; - shutdownControlMsg.cfg.mpeState = mpe_state; + shutdownControlMsg.mpeState = mpe_state; - sendMessageCAN1(0x03, 1, shutdownControlMsg.msg); + unit8_t buf[1] = { 0 }; + memcpy(buf, &compute_send_shutdown_ctrl_message, sizeof(compute_send_shutdown_ctrl_message)); + sendMessageCAN1(0x03, 1, buff); } -void ComputeInterface::sendCellDataMessage(CriticalCellValue_t high_voltage, CriticalCellValue_t low_voltage, uint16_t avg_voltage) +void compute_send_cell_data_message(AccumulatorData_t* bmsdata) { - union - { - uint8_t msg[8] = {0, 0, 0, 0, 0, 0, 0, 0}; - - struct - { - uint16_t highCellVoltage; - uint8_t highCellID; - uint16_t lowCellVoltage; - uint8_t lowCellID; - uint16_t voltAvg; - } cfg; - } cellDataMsg; - - cellDataMsg.cfg.highCellVoltage = high_voltage.val; - cellDataMsg.cfg.highCellID = (high_voltage.chipIndex << 4) | high_voltage.cellNum; - cellDataMsg.cfg.lowCellVoltage = low_voltage.val; - cellDataMsg.cfg.lowCellID = (low_voltage.chipIndex << 4) | low_voltage.cellNum; - cellDataMsg.cfg.voltAvg = avg_voltage; - - uint8_t msg[8] = { - ( cellDataMsg.cfg.highCellVoltage & 0x00ff), - ((cellDataMsg.cfg.highCellVoltage & 0xff00)>>8), - cellDataMsg.cfg.highCellID, - ( cellDataMsg.cfg.lowCellVoltage & 0x00ff), - ((cellDataMsg.cfg.lowCellVoltage & 0xff00)>>8), - cellDataMsg.cfg.lowCellID, - ( cellDataMsg.cfg.voltAvg & 0x00ff), - ((cellDataMsg.cfg.voltAvg & 0xff00)>>8) - }; - - sendMessageCAN1(CANMSG_BMSCELLDATA, 8, msg); + struct __attribute__((packed)) { + uint16_t high_cell_voltage; + uint8_t high_cell_id; + uint16_t low_cell_voltage; + uint8_t low_cell_id; + uint16_t volt_avg; + } cell_data_msg; + + cell_data_msg.high_cell_voltage = bmsdata->max_voltage.val; + cell_data_msg.high_cell_id = bmsdata->max_voltage.chipIndex; + cell_data_msg.low_cell_voltage = bmsdata->min_voltage.val; + // Chip number Cell number + cell_data_msg.low_cell_id + = (bmsdata->min_voltage.chipIndex << 4) | bmsdata->min_voltage.cellNum; + cell_data_msg.volt_avg = bmsdata->avg_voltage; + + /* uint8_t msg[8] = { + ( cell_data_msg.cfg.high_cell_voltage & 0x00ff), + ((cell_data_msg.cfg.high_cell_voltage & 0xff00)>>8), + cell_data_msg.cfg.high_cell_id, + ( cell_data_msg.cfg.low_cell_voltage & 0x00ff), + ((cell_data_msg.cfg.low_cell_voltage & 0xff00)>>8), + cell_data_msg.cfg.low_cell_id, + ( cell_data_msg.cfg.volt_avg & 0x00ff), + ((cell_data_msg.cfg.volt_avg & 0xff00)>>8) + }; + */ + unit8_t buf[8] = { 0 }; + memcpy(buf, &cell_data_msg, sizeof(cell_data_msg)); + sendMessageCAN1(CANMSG_BMSCELLDATA, 8, buf); } -void ComputeInterface::sendCellVoltageMessage(uint8_t cell_id, uint16_t instant_voltage, uint16_t internal_Res, uint8_t shunted, uint16_t open_voltage) +void compute_send_cell_voltage_message(uint8_t cell_id, uint16_t instant_voltage, + uint16_t internal_Res, uint8_t shunted, + uint16_t open_voltage) { - union - { - uint8_t msg[8] = {0, 0, 0, 0, 0, 0, 0, 0}; - - struct - { - uint8_t cellID; - uint16_t instantVoltage; - uint16_t internalResistance; - uint8_t shunted; - uint16_t openVoltage; - } cfg; - } cellVoltageMsg; - - cellVoltageMsg.cfg.cellID = cell_id; - cellVoltageMsg.cfg.instantVoltage = __builtin_bswap16(instant_voltage); - cellVoltageMsg.cfg.internalResistance = __builtin_bswap16(internal_Res); - cellVoltageMsg.cfg.shunted = shunted; - cellVoltageMsg.cfg.openVoltage = __builtin_bswap16(open_voltage); - - sendMessageCAN1(0x07, 8, cellVoltageMsg.msg); + struct __attribute__((packed)) { + uint8_t cellID; + uint16_t instantVoltage; + uint16_t internalResistance; + uint8_t shunted; + uint16_t openVoltage; + } cellVoltageMsg; + + cellVoltageMsg.cellID = cell_id; + cellVoltageMsg.instantVoltage = __builtin_bswap16(instant_voltage); + cellVoltageMsg.internalResistance = __builtin_bswap16(internal_Res); + cellVoltageMsg.shunted = shunted; + cellVoltageMsg.openVoltage = __builtin_bswap16(open_voltage); + + unit8_t buf[8] = { 0 }; + memcpy(0x07, &cellVoltageMsg, sizeof(cellVoltageMsg)); + + sendMessageCAN1(0x07, 8, buf); } -void ComputeInterface::sendCurrentsStatus(uint16_t discharge, uint16_t charge, uint16_t current) +void compute_send_current_message(AccumulatorData_t* bmsdata) { - union - { - uint8_t msg[6] = {0, 0, 0, 0, 0, 0}; - - struct - { - uint16_t DCL; - uint16_t CCL; - uint16_t packCurr; - } cfg; - } currentsStatusMsg; - - currentsStatusMsg.cfg.DCL = discharge; - currentsStatusMsg.cfg.CCL = charge; - currentsStatusMsg.cfg.packCurr = current; - - sendMessageCAN1(CANMSG_BMSCURRENTS, 8, currentsStatusMsg.msg); + struct __attribute__((packed)) { + uint16_t dcl; + uint16_t ccl; + uint16_t pack_curr; + } current_status_msg; + + current_status_msg.dcl = bmsdata->discharge_limit; + current_status_msg.ccl = bmsdata->charge_limit; + current_status_msg.pack_curr = bmsdata->pack_current; + + uint8_t buf[8] = { 0 }; + memcpy(buf, ¤t_status_msg, sizeof(current_status_msg)); + + sendMessageCAN1(CANMSG_BMSCURRENTS, 8, buf); } -void ComputeInterface::MCCallback(const CAN_message_t &msg) +void compute_mc_callback(const CAN_message_t& currentStatusMsg) { - return; + return; } -void ComputeInterface::sendCellTemp(CriticalCellValue_t max_cell_temp, CriticalCellValue_t min_cell_temp, uint16_t avg_temp) +void compute_send_cell_temp_message(AccumulatorData_t* bmsdata) { - union - { - uint8_t msg[8] = {0, 0, 0, 0, 0, 0, 0, 0}; - - struct - { - uint16_t maxCellTemp; - uint8_t maxCellID; - uint16_t minCellTemp; - uint8_t minCellID; - uint16_t averageTemp; - } cfg; - } cellTempMsg; - - cellTempMsg.cfg.maxCellTemp = max_cell_temp.val; - cellTempMsg.cfg.maxCellID = (max_cell_temp.chipIndex << 4) | (max_cell_temp.cellNum - 17); - cellTempMsg.cfg.minCellTemp = min_cell_temp.val; - cellTempMsg.cfg.minCellID = (min_cell_temp.chipIndex << 4) | (min_cell_temp.cellNum - 17); - cellTempMsg.cfg.averageTemp = avg_temp; - - uint8_t msg[8] = { - ( cellTempMsg.cfg.maxCellTemp & 0x00ff), - ((cellTempMsg.cfg.maxCellTemp & 0xff00)>>8), - cellTempMsg.cfg.maxCellID, - ( cellTempMsg.cfg.minCellTemp & 0x00ff), - ((cellTempMsg.cfg.minCellTemp & 0xff00)>>8), - cellTempMsg.cfg.minCellID, - ( cellTempMsg.cfg.averageTemp & 0x00ff), - ((cellTempMsg.cfg.averageTemp & 0xff00)>>8) - }; - - sendMessageCAN1(0x08, 8, msg); + + struct __attribute__((packed)) { + uint16_t max_cell_temp; + uint8_t max_cell_id; + uint16_t min_cell_temp; + uint8_t min_cell_id; + uint16_t average_temp; + } cell_temp_msg; + + cell_temp_msg.max_cell_temp = bmsdata->max_temp.val; + cell_temp_msg.max_cell_id + = (bmsdata->max_temp.chipIndex << 4) | (bmsdata->max_temp.cellNum - 17); + cell_temp_msg.min_cell_temp = bmsdata->min_temp.val; + cell_temp_msg.min_cell_id + = (bmsdata->min_temp.chipIndex << 4) | (bmsdata->min_temp.cellNum - 17); + cell_temp_msg.average_temp = bmsdata->avg_temp; + + /* + uint8_t msg[8] = { + ( cell_temp_msg.cfg.max_cell_temp & 0x00ff), + ((cell_temp_msg.cfg.max_cell_temp & 0xff00)>>8), + cell_temp_msg.cfg.max_cell_id, + ( cell_temp_msg.cfg.min_cell_temp & 0x00ff), + ((cell_temp_msg.cfg.min_cell_temp & 0xff00)>>8), + cell_temp_msg.cfg.min_cell_id, + ( cell_temp_msg.cfg.average_temp & 0x00ff), + ((cell_temp_msg.cfg.average_temp & 0xff00)>>8) + }; + */ + unit8_t buf[8] = { 0 }; + memcpy(buf, &cell_temp_msg, sizeof(cell_temp_msg)); + sendMessageCAN1(0x08, 8, buf); } -void ComputeInterface::sendSegmentTemps(int8_t segment_temps[NUM_SEGMENTS]) +void send_segment_temp_message(AccumulatorData_t* bmsdata) { - union - { - uint8_t msg[4] = {0, 0, 0, 0}; - - struct - { - int8_t segment1_average_temp; - int8_t segment2_average_temp; - int8_t segment3_average_temp; - int8_t segment4_average_temp; - } cfg; - } segmentTempsMsg; - - segmentTempsMsg.cfg.segment1_average_temp = segment_temps[0]; - segmentTempsMsg.cfg.segment2_average_temp = segment_temps[1]; - segmentTempsMsg.cfg.segment3_average_temp = segment_temps[2]; - segmentTempsMsg.cfg.segment4_average_temp = segment_temps[3]; - - sendMessageCAN1(0x09, 4, segmentTempsMsg.msg); + + struct __attribute__((packed)) { + int8_t segment1_average_temp; + int8_t segment2_average_temp; + int8_t segment3_average_temp; + int8_t segment4_average_temp; + } segment_temp_msg; + + segment_temp_msg.segment1_average_temp = bmsdata->segment_average_temps[0]; + segment_temp_msg.segment2_average_temp = bmsdata->segment_average_temps[1]; + segment_temp_msg.segment3_average_temp = bmsdata->segment_average_temps[2]; + segment_temp_msg.segment4_average_temp = bmsdata->segment_average_temps[3]; + unit8_t buff[4] = { 0 }; + memcpy(buff & segment_temp_msg, sizeof(segment_temp_msg)); + // Ask about these + sendMessageCAN1(0x09, 4, buff); } -uint8_t ComputeInterface::calcChargerLEDState(AccumulatorData_t *bms_data) +uint8_t calc_charger_led_state(AccumulatorData_t* bms_data) { - enum LED_state - { - RED_BLINKING = 0x00, - RED_CONSTANT = 0x01, - YELLOW_BLINKING = 0x02, - YELLOW_CONSTANT = 0x03, - GREEN_BLINKING = 0x04, - GREEN_CONSTANT = 0x05, - RED_GREEN_BLINKING = 0x06 - }; - - if((bms_data->soc < 80) && (bms_data->pack_current > .5 * 10)) - { - return RED_BLINKING; - } - else if((bms_data->soc < 80) && (bms_data->pack_current <= .5 * 10)) - { - return RED_CONSTANT; - } - else if((bms_data->soc >= 80 && bms_data->soc < 95) && (bms_data->pack_current > .5 * 10)) - { - return YELLOW_BLINKING; - } - else if((bms_data->soc >= 80 && bms_data->soc < 95) && (bms_data->pack_current <= .5 * 10)) - { - return YELLOW_CONSTANT; - } - else if((bms_data->soc >= 95) && (bms_data->pack_current > .5 * 10)) - { - return GREEN_BLINKING; - } - else if((bms_data->soc >= 95) && (bms_data->pack_current <= .5 * 10)) - { - return GREEN_CONSTANT; - } - else - { - return RED_GREEN_BLINKING; - } - + enum LED_state { + RED_BLINKING = 0x00, + RED_CONSTANT = 0x01, + YELLOW_BLINKING = 0x02, + YELLOW_CONSTANT = 0x03, + GREEN_BLINKING = 0x04, + GREEN_CONSTANT = 0x05, + RED_GREEN_BLINKING = 0x06 + }; + + if ((bms_data->soc < 80) && (bms_data->pack_current > .5 * 10)) { + return RED_BLINKING; + } else if ((bms_data->soc < 80) && (bms_data->pack_current <= .5 * 10)) { + return RED_CONSTANT; + } else if ((bms_data->soc >= 80 && bms_data->soc < 95) && (bms_data->pack_current > .5 * 10)) { + return YELLOW_BLINKING; + } else if ((bms_data->soc >= 80 && bms_data->soc < 95) && (bms_data->pack_current <= .5 * 10)) { + return YELLOW_CONSTANT; + } else if ((bms_data->soc >= 95) && (bms_data->pack_current > .5 * 10)) { + return GREEN_BLINKING; + } else if ((bms_data->soc >= 95) && (bms_data->pack_current <= .5 * 10)) { + return GREEN_CONSTANT; + } else { + return RED_GREEN_BLINKING; + } } -void ComputeInterface::sendDclPreFault(bool prefault) +void compute_send_dcl_prefault_message(bool prefault) { - uint8_t msg[1] = {prefault}; - sendMessageCAN1(0x500, 1, msg); -} \ No newline at end of file + uint8_t msg[1] = { prefault }; + sendMessageCAN1(0x500, 1, msg); +} diff --git a/Core/Src/eepromdirectory.c b/Core/Src/eepromdirectory.c new file mode 100644 index 0000000..19735df --- /dev/null +++ b/Core/Src/eepromdirectory.c @@ -0,0 +1,171 @@ +#include "eepromdirectory.h" +#include "general/include/m24c32.h" + +void eepromInit() +{ + // Initialize EEPROM addresses given data and length + + int i = 1; + int offset = 0; + + /* initialize root offset to zero */ + eeprom_data[0].address = EEPROM_ROOT_ADDR; + + /* continue through array, setting offsets */ + while (eeprom_data[i].id != NULL) + { + offset += eeprom_data[i-1].size; + eeprom_data[i].address = offset; + i++; + } + //Initialize first byte of faults partition to contain the address of the end of the partition so that the + //logFault function logs the first fault to the first 4 bytes in the partition. + eepromWriteData(const_cast("FAULTS"), eeprom_data[ eepromGetAddress(const_cast("FAULTS"))] + + eeprom_data[ eepromGetIndex(const_cast("FAULTS"))].size - 3, 1); +} + +uint16_t eepromGetAddress(char *key) +{ + /* find the index of the key in the eeprom_data array */ + int i = 0; + while (eeprom_data[i].id != NULL) + { + if (eeprom_data[i].id == key) + { + return eeprom_data[i].address; + } + + i++; + } + return -1; +} + +uint16_t eepromGetIndex(char *key) +{ + /* find the index of the key in the eeprom_data array */ + int i = 0; + while (eeprom_data[i].id != NULL) + { + if (eeprom_data[i].id == key) + { + return i; + } + + i++; + } + return -1; +} + +char* eepromGetKey(int index) +{ + /* find the key at the index in the eeprom_data array */ + int i = 0; + while (eeprom_data[i].id != NULL) + { + if (eeprom_data[i].address == index) + { + return eeprom_data[i].id; + } + + i++; + } + return NULL; +} + +bool eepromReadData(char *key, void *data, uint16_t size) +{ + if(!data) { + return false; + } + /* read data from eeprom given key and size */ + int address = eepromGetAddress(key); + eeprom_read(address, data, size); + +} + +bool eepromReadData(uint16_t address, void *data, uint16_t size) +{ + if(!data) { + return false; + } + /* read data from eeprom given index */ + eeprom_read(address, data, size); + //EEPROM.get(index, data); // TODO will need update with new eeprom driver +} + +bool eepromWriteData(char *key, void *data, uint16_t size) +{ + if(!data) { + return false; + } + /* write data to eeprom given key, offset, and size of data */ + int address = eepromGetAddress(key); + eeprom_write(address, data, size); +} + +bool eepromWriteData(uint16_t address, void *data, uint16_t size) +{ + if(!data) { + return false; + } + /* write data to eeprom given page, offset, and size of data */ + eeprom_write(address, data, size); +} + +void logFault(uint32_t fault_code) +{ + uint32_t fault = fault_code; + //The next address to write a fault to is located in the first byte of the FAULTS partition. + uint8_t *address; + eepromReadData(eepromGetAddress(const_cast("FAULTS")), address, 1); + + uint8_t startIndex = eeprom_data[eepromGetIndex(const_cast("FAULTS"))].address; + uint8_t size = eeprom_data[eepromGetIndex(const_cast("FAULTS"))].size; + + /* if the index is at the end of the partition, wrap around (currently store 5 faults, so max = 5 + offset) */ + if (*address == size + startIndex - 3) + { + /* first byte of partition is the index of the most recent fault, faults begin at second byte */ + *address = startIndex + 1; + } + else + { + *address += 4; + } + + /* write the fault code*/ + eepromWriteData(*address, fault, 4); + /* update first byte of faults partition*/ + eepromWriteData(startIndex, *address + 4, 1); +} + +void getFaults() +{ + uint8_t* address; + eepromReadData("FAULTS", address, 1); + + uint8_t startAddress = eeprom_data[eepromGetIndex(const_cast("FAULTS"))].address; + uint8_t size = eeprom_data[eepromGetIndex(const_cast("FAULTS"))].size; + + /* read and store the faults */ + + int currIter = 0; + while (currIter < NUM_EEPROM_FAULTS) + { + eepromReadData(*index, &eeprom_faults[currIter], 4); + currIter++; + + /* if the index is at the end of the partition, wrap around (5 faults * 4 bytes per fault + offset - 3 for start of fault) */ + if (*address == size + startAddress - 3) + { + /* first byte of partition is the index of the most recent fault, faults begin at second byte */ + *address = startAddress + 1; + } + + else + { + /* 4 bytes per fault */ + *address += 4; + } + } +} \ No newline at end of file diff --git a/Core/Src/main.c b/Core/Src/main.c index 82bcf4d..014651c 100644 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -4,14 +4,6 @@ * @file : main.c * @brief : Main program body ****************************************************************************** - * @attention - * - * Copyright (c) 2023 STMicroelectronics. - * All rights reserved. - * - * This software is licensed under terms that can be found in the LICENSE file - * in the root directory of this software component. - * If no LICENSE file comes with this software, it is provided AS-IS. * ****************************************************************************** */ @@ -22,7 +14,8 @@ /* Private includes ----------------------------------------------------------*/ /* USER CODE BEGIN Includes */ // #include -#include +// TODO: import and replace new watchdog library +//#include #include #include "segment.h" #include "compute.h" @@ -66,7 +59,7 @@ PCD_HandleTypeDef hpcd_USB_OTG_FS; /* USER CODE BEGIN PV */ WDT_T4 wdt; -AccumulatorData_t *prevAccData = nullptr; +AccumulatorData_t *prev_acc_data = nullptr; StateMachine stateMachine; /* USER CODE END PV */ @@ -88,40 +81,40 @@ static void MX_USB_OTG_FS_PCD_Init(void); /* USER CODE BEGIN 0 */ #ifdef DEBUG_STATS -const void printBMSStats(AccumulatorData_t *accData) +const void print_bms_stats(AccumulatorData_t *acc_data) { - static Timer debug_statTimer; + static Timer debug_stat_timer; static const uint16_t PRINT_STAT_WAIT = 500; //ms - if(!debug_statTimer.isTimerExpired()) return; + if(!debug_stat_timer.isTimerExpired()) return; Serial.print("Prev Fault: 0x"); Serial.println(stateMachine.previousFault, HEX); Serial.print("Current: "); - Serial.println((float)(accData->pack_current) / 10.0); + Serial.println((float)(acc_data->pack_current) / 10.0); Serial.print("Min, Max, Avg Temps: "); - Serial.print(accData->min_temp.val); + Serial.print(acc_data->min_temp.val); Serial.print(", "); - Serial.print(accData->max_temp.val); + Serial.print(acc_data->max_temp.val); Serial.print(", "); - Serial.println(accData->avg_temp); + Serial.println(acc_data->avg_temp); Serial.print("Min, Max, Avg, Delta Voltages: "); - Serial.print(accData->min_voltage.val); + Serial.print(acc_data->min_voltage.val); Serial.print(", "); - Serial.print(accData->max_voltage.val); + Serial.print(acc_data->max_voltage.val); Serial.print(", "); - Serial.print(accData->avg_voltage); + Serial.print(acc_data->avg_voltage); Serial.print(", "); - Serial.println(accData->delt_voltage); + Serial.println(acc_data->delt_voltage); Serial.print("DCL: "); - Serial.println(accData->discharge_limit); + Serial.println(acc_data->discharge_limit); Serial.print("CCL: "); - Serial.println(accData->charge_limit); + Serial.println(acc_data->charge_limit); Serial.print("SoC: "); - Serial.println(accData->soc); + Serial.println(acc_data->soc); Serial.print("Is Balancing?: "); Serial.println(segment.isBalancing()); @@ -137,7 +130,7 @@ const void printBMSStats(AccumulatorData_t *accData) { for(uint8_t cell = 0; cell < NUM_CELLS_PER_CHIP; cell++) { - Serial.print(accData->chip_data[c].voltage_reading[cell]); + Serial.print(acc_data->chip_data[c].voltage_reading[cell]); Serial.print("\t"); } Serial.println(); @@ -148,7 +141,7 @@ const void printBMSStats(AccumulatorData_t *accData) { for(uint8_t cell = 0; cell < NUM_CELLS_PER_CHIP; cell++) { - Serial.print(accData->chip_data[c].open_cell_voltage[cell]); + Serial.print(acc_data->chip_data[c].open_cell_voltage[cell]); Serial.print("\t"); } Serial.println(); @@ -159,7 +152,7 @@ const void printBMSStats(AccumulatorData_t *accData) { for(uint8_t cell = 17; cell < 28; cell++) { - Serial.print(accData->chip_data[c].thermistor_reading[cell]); + Serial.print(acc_data->chip_data[c].thermistor_reading[cell]); Serial.print("\t"); } Serial.println(); @@ -170,14 +163,14 @@ const void printBMSStats(AccumulatorData_t *accData) { for(uint8_t cell = 17; cell < 28; cell++) { - Serial.print(accData->chip_data[c].thermistor_value[cell]); + Serial.print(acc_data->chip_data[c].thermistor_value[cell]); Serial.print("\t"); } Serial.println(); } - debug_statTimer.startTimer(PRINT_STAT_WAIT); + debug_stat_timer.startTimer(PRINT_STAT_WAIT); } @@ -205,7 +198,7 @@ int main(void) config.timeout = 15; /* in seconds, 0->128 */ wdt.begin(config); // NERduino.begin(); - compute.setFault(NOT_FAULTED); + compute.compute_set_fault(NOT_FAULTED); segment.init(); /* USER CODE END Init */ @@ -233,22 +226,22 @@ int main(void) /* USER CODE BEGIN WHILE */ //Create a dynamically allocated structure - AccumulatorData_t *accData = new AccumulatorData_t; + AccumulatorData_t *acc_data = new AccumulatorData_t; - //accData->faultCode = FAULTS_CLEAR; + //acc_data->faultCode = FAULTS_CLEAR; //Collect all the segment data needed to perform analysis //Not state specific - segment.retrieveSegmentData(accData->chip_data); - accData->pack_current = compute.getPackCurrent(); + segment_retrieve_segment_data(acc_data->chip_data); + acc_data->pack_current = compute.compute_get_pack_current(); //Perform calculations on the data in the frame - analyzer.push(accData); + analyzer.push(acc_data); - stateMachine.handleState(accData); + stateMachine_sm_handle_state(acc_data); #ifdef DEBUG_STATS - printBMSStats(analyzer.bmsdata); + print_bms_stats(analyzer.bmsdata); #endif wdt.feed(); diff --git a/Core/Src/segment.c b/Core/Src/segment.c index a625047..e279aae 100644 --- a/Core/Src/segment.c +++ b/Core/Src/segment.c @@ -1,574 +1,570 @@ #include "segment.h" -SegmentInterface segment; +#define THERM_WAIT_TIME 500 /* ms */ +#define VOLTAGE_WAIT_TIME 100 /* ms */ +#define THERM_AVG 15 /* Number of values to average */ +#define MAX_VOLT_DELTA 2500 +#define MAX_VOLT_DELTA_COUNT 10 + uint8_t therm_avg_counter = 0; +Timer therm_timer; +Timer voltage_reading_timer; +Timer variance_timer; + +FaultStatus_t voltage_error = NOT_FAULTED; +FaultStatus_t therm_error = NOT_FAULTED; -SegmentInterface::SegmentInterface(){} +uint16_t therm_settle_time_ = 0; -SegmentInterface::~SegmentInterface(){} +const uint32_t VOLT_TEMP_CONV[91] = { 44260, 43970, 43670, 43450, 43030, 42690, 42340, 41980, 41620, + 41240, 40890, 40460, 40040, 39580, 39130, 38660, 38210, 37710, 37210, 36190, 35670, 35160, + 34620, 34080, 33550, 32990, 32390, 31880, 31270, 30690, 30160, 29590, 28990, 28450, 27880, + 27270, 26740, 26080, 25610, 25000, 24440, 23880, 23330, 22780, 22240, 21700, 21180, 20660, + 20150, 19640, 19140, 18650, 18170, 17700, 17230, 16780, 16330, 15890, 15470, 15030, 14640, + 14230, 13850, 13450, 13070, 12710, 11490, 11170, 10850, 10550, 10250, 9960, 9670, 9400, 9130, + 8870, 8620, 8370, 8130, 7900, 0 }; -void SegmentInterface::init() +const int32_t VOLT_TEMP_CALIB_OFFSET = 0; + +void segment_init() { - Serial.println("Initializing Segments..."); - - LTC6804_initialize(); - - //pullChipConfigurations(); - - for (int c = 0; c < NUM_CHIPS; c++) - { - local_config[c][0] = 0xF8; - local_config[c][1] = 0x19; // VUV = 0x619 = 1561 -> 2.4992V - local_config[c][2] = 0x06; // VOV = 0xA60 = 2656 -> 4.2496V - local_config[c][3] = 0xA6; - local_config[c][4] = 0x00; - local_config[c][5] = 0x00; - } - pushChipConfigurations(); + printf("Initializing Segments..."); + + LTC6804_initialize(); + + // pull_chip_configuration(); + + for (int c = 0; c < NUM_CHIPS; c++) { + local_config[c][0] = 0xF8; + local_config[c][1] = 0x19; /* VUV = 0x619 = 1561 -> 2.4992V */ + local_config[c][2] = 0x06; /* VOV = 0xA60 = 2656 -> 4.2496V */ + local_config[c][3] = 0xA6; + local_config[c][4] = 0x00; + local_config[c][5] = 0x00; + } + push_chip_configurations(); } -void SegmentInterface::retrieveSegmentData(ChipData_t databuf[NUM_CHIPS]) +void segment_retrieve_data(ChipData_t databuf[NUM_CHIPS]) { - segment_data = databuf; + segment_data = databuf; - /** - * Pull voltages and thermistors and indiacte if there was a problem during retrieval - */ - voltage_error = pullVoltages(); - therm_error = pullThermistors(); + /* Pull voltages and thermistors and indiacte if there was a problem during + * retrieval */ + voltage_error = pull_voltages(); + therm_error = pull_thermistors(); - /** - * Save the contents of the reading so that we can use it to fill in missing data - */ - memcpy(previous_data, segment_data, sizeof(ChipData_t)*NUM_CHIPS); + /* Save the contents of the reading so that we can use it to fill in missing + * data */ + memcpy(previous_data, segment_data, sizeof(ChipData_t) * NUM_CHIPS); - segment_data = nullptr; + segment_data = nullptr; } -void SegmentInterface::configureDischarge(uint8_t chip, uint16_t cells) +void configure_discharge(uint8_t chip, uint16_t cells) { - // chipConfigurations[chip][4] == chipConfigurations[Literally what chip you want][register] - // 4 and 5 are registers to discharge chips - local_config[chip][4] = uint8_t(cells & 0x00FF); - // Register 5 is split in half, so we maintain the upper half and add in the bottom half to discharge cells - local_config[chip][5] = (local_config[chip][5] & 0xF0) + uint8_t(cells >> 8); + /* + * chipConfigurations[chip][4] == chipConfigurations[Literally what chip you + * want][register] 4 and 5 are registers to discharge chips + */ + local_config[chip][4] = uint8_t(cells & 0x00FF); + + /* + * Register 5 is split in half, so we maintain the upper half and add in the + * bottom half to discharge cells + */ + local_config[chip][5] = (local_config[chip][5] & 0xF0) + uint8_t(cells >> 8); } -void SegmentInterface::enableBalancing(bool balance_enable) +void segment_enable_balancing(bool balance_enable) { - //Discharging all cells in series - static const uint16_t DICHARGE_ALL_COMMAND = 0xFFFF >> (16-NUM_CELLS_PER_CHIP); // Making the discharge command all 1's for all cells per chip - - if(balance_enable) - { - for(int c = 0; c < NUM_CHIPS; c++) - { - configureDischarge(c, DICHARGE_ALL_COMMAND); + /* + * Discharging all cells in series + * Making the discharge command all 1's for all cells per chip + */ + static const uint16_t DICHARGE_ALL_COMMAND = 0xFFFF >> (16 - NUM_CELLS_PER_CHIP); + + if (balance_enable) { + for (int c = 0; c < NUM_CHIPS; c++) { + configureDischarge(c, DICHARGE_ALL_COMMAND); discharge_commands[c] = DICHARGE_ALL_COMMAND; - } - pushChipConfigurations(); - } - else - { - for (int c = 0; c < NUM_CHIPS; c++) - { - configureDischarge(c, 0); + } + push_chip_configuration(); + } else { + for (int c = 0; c < NUM_CHIPS; c++) { + configureDischarge(c, 0); discharge_commands[c] = 0; - } - pushChipConfigurations(); - } + } + push_chip_configuration(); + } } -/** - * @todo Revisit after testing - */ -void SegmentInterface::enableBalancing(uint8_t chip_num, uint8_t cell_num, bool balance_enable) +// @todo Revisit after testing +void segment_enable_balancing(uint8_t chip_num, uint8_t cell_num, bool balance_enable) { - pullChipConfigurations(); + pull_chip_configurations(); - if(balance_enable) + if (balance_enable) discharge_commands[chip_num] |= (1 << cell_num); else discharge_commands[chip_num] &= ~(1 << cell_num); - configureDischarge(chip_num, discharge_commands[chip_num]); + configure_discharge(chip_num, discharge_commands[chip_num]); - pushChipConfigurations(); + push_chip_configurations(); } -void SegmentInterface::configureBalancing(bool discharge_config[NUM_CHIPS][NUM_CELLS_PER_CHIP]) +void segment_configure_balancing(bool discharge_config[NUM_CHIPS][NUM_CELLS_PER_CHIP]) { - for(int c = 0; c < NUM_CHIPS; c++) - { - for(int cell = 0; cell < NUM_CELLS_PER_CHIP; cell++) - { - if(discharge_config[c][cell]) + for (int c = 0; c < NUM_CHIPS; c++) { + for (int cell = 0; cell < NUM_CELLS_PER_CHIP; cell++) { + if (discharge_config[c][cell]) discharge_commands[c] |= 1 << cell; else discharge_commands[c] &= ~(1 << cell); - } + } - configureDischarge(c, discharge_commands[c]); - } - pushChipConfigurations(); + configure_discharge(c, discharge_commands[c]); + } + push_chip_configurations(); } -bool SegmentInterface::isBalancing(uint8_t chip_num, uint8_t cell_num) +bool segment_is_balancing(uint8_t chip_num, uint8_t cell_num) { - //If the cell is one of the first 8, check the 4th register - if(cell_num < 8) - { - return local_config[chip_num][4] & (1 << cell_num); - } - //If the cell number is greater than 8, check the 5th register - else - { - return local_config[chip_num][5] & (1 << (cell_num - 8)); - } - - return false; //default case -} + /* If the cell is one of the first 8, check the 4th register */ + if (cell_num < 8) { + return local_config[chip_num][4] & (1 << cell_num); + } + /* If the cell number is greater than 8, check the 5th register */ + else { + return local_config[chip_num][5] & (1 << (cell_num - 8)); + } -bool SegmentInterface::isBalancing() -{ - for(int c = 0; c < NUM_CHIPS; c++) - { - //Reading from the 4th config register - for(int cell = 0; cell < 8; cell++) - { - if(local_config[c][4] & (1 << cell)) return true; - } - - //Reading from the 5th config register - for(int cell = 0; cell < 4; cell++) - { - if(local_config[c][5] & (1 << (cell))) return true; - } - } - - return false; + return false; /* default case */ } -void SegmentInterface::pullChipConfigurations() +bool segment_is_balancing() { - uint8_t remote_config[NUM_CHIPS][8]; - LTC6804_rdcfg(NUM_CHIPS, remote_config); - - for (int chip = 0; chip < NUM_CHIPS; chip++) - { - for(int index = 0; index < 6; index++) - { - local_config[chip][index] = remote_config[chip][index]; - } - } + for (int c = 0; c < NUM_CHIPS; c++) { + /* Reading from the 4th config register */ + for (int cell = 0; cell < 8; cell++) { + if (local_config[c][4] & (1 << cell)) + return true; + } + + /* Reading from the 5th config register */ + for (int cell = 0; cell < 4; cell++) { + if (local_config[c][5] & (1 << (cell))) + return true; + } + } + + return false; } -void SegmentInterface::pushChipConfigurations() +void pull_chip_configurations() { - LTC6804_wrcfg(NUM_CHIPS, local_config); + uint8_t remote_config[NUM_CHIPS][8]; + LTC6804_rdcfg(NUM_CHIPS, remote_config); + + for (int chip = 0; chip < NUM_CHIPS; chip++) { + for (int index = 0; index < 6; index++) { + local_config[chip][index] = remote_config[chip][index]; + } + } } -FaultStatus_t SegmentInterface::pullVoltages() +void push_chip_configurations() { LTC6804_wrcfg(NUM_CHIPS, local_config); } + +FaultStatus_t pull_voltages() { - /** - * If we haven't waited long enough between pulling voltage data - * just copy over the contents of the last good reading and the fault status from the most recent attempt - */ - if(!voltage_reading_timer.isTimerExpired()) - { - for(uint8_t i=0; i MAX_VOLT_DELTA) - { - segment_data[i].voltage_reading[j] = previous_data[i].voltage_reading[j]; - segment_data[i].bad_volt_diff_count[j]++; - - if (segment_data[i].bad_volt_diff_count[j] > MAX_VOLT_DELTA_COUNT) - { - segment_data[i].bad_volt_diff_count[j] = 0; - segment_data[i].voltage_reading[j] = segment_voltages[i][j]; - } - } - else - { - segment_data[i].bad_volt_diff_count[j] = 0; - segment_data[i].voltage_reading[j] = segment_voltages[i][j]; - } - } - } - - /** - * Start the timer between readings if successful - */ - voltage_reading_timer.startTimer(VOLTAGE_WAIT_TIME); - return NOT_FAULTED; + /** + * If we haven't waited long enough between pulling voltage data + * just copy over the contents of the last good reading and the fault status + * from the most recent attempt + */ + if (!voltage_reading_timer.isTimerExpired()) { + for (uint8_t i = 0; i < NUM_CHIPS; i++) { + memcpy(segment_data[i].voltage_reading, previous_data[i].voltage_reading, + sizeof(segment_data[i].voltage_reading)); + } + return voltage_error; + } + + uint16_t segment_voltages[NUM_CHIPS][12]; + + push_chip_configurations(); + LTC6804_adcv(); + + /** + * If we received an incorrect PEC indicating a bad read + * copy over the data from the last good read and indicate an error + */ + if (LTC6804_rdcv(0, NUM_CHIPS, segment_voltages) == -1) { + for (uint8_t i = 0; i < NUM_CHIPS; i++) { + memcpy(segment_data[i].voltage_reading, previous_data[i].voltage_reading, + sizeof(segment_data[i].voltage_reading)); + } + return FAULTED; + } + + /* If the read was successful, copy the voltage data */ + for (uint8_t i = 0; i < NUM_CHIPS; i++) { + for (uint8_t j = 0; j < NUM_CELLS_PER_CHIP; j++) { + if (abs(segment_voltages[i][j] - previous_data[i].voltage_reading[j]) + > MAX_VOLT_DELTA) { + segment_data[i].voltage_reading[j] = previous_data[i].voltage_reading[j]; + segment_data[i].bad_volt_diff_count[j]++; + + if (segment_data[i].bad_volt_diff_count[j] > MAX_VOLT_DELTA_COUNT) { + segment_data[i].bad_volt_diff_count[j] = 0; + segment_data[i].voltage_reading[j] = segment_voltages[i][j]; + } + } else { + segment_data[i].bad_volt_diff_count[j] = 0; + segment_data[i].voltage_reading[j] = segment_voltages[i][j]; + } + } + } + + /* Start the timer between readings if successful */ + voltage_reading_timer.startTimer(VOLTAGE_WAIT_TIME); + return NOT_FAULTED; } -FaultStatus_t SegmentInterface::pullThermistors() +FaultStatus_t pull_thermistors() { - // If polled too soon, just copy existing values from memory - if (!therm_timer.isTimerExpired()) - { - for(uint8_t i=0; i 32) { return; } uint8_t i2c_write_data[NUM_CHIPS][3]; uint8_t comm_reg_data[NUM_CHIPS][6]; if (therm <= 8) { - // Turn off competing multiplexor (therms 9-16) - for(int chip = 0; chip < NUM_CHIPS; chip++) { - i2c_write_data[chip][0] = 0x92; - i2c_write_data[chip][1] = 0x00; - i2c_write_data[chip][2] = 0x00; + /* Turn off competing multiplexor (therms 9-16) */ + for (int chip = 0; chip < NUM_CHIPS; chip++) { + i2c_write_data[chip][0] = 0x92; + i2c_write_data[chip][1] = 0x00; + i2c_write_data[chip][2] = 0x00; } - serializeI2CMsg(i2c_write_data, comm_reg_data); - pushChipConfigurations(); + serialize_i2c_msg(i2c_write_data, comm_reg_data); + push_chip_configurations(); LTC6804_wrcomm(NUM_CHIPS, comm_reg_data); LTC6804_stcomm(24); - // Turn on desired thermistor - for(int chip = 0; chip < NUM_CHIPS; chip++) { - i2c_write_data[chip][0] = 0x90; - i2c_write_data[chip][1] = 0x08 + (therm - 1); - i2c_write_data[chip][2] = 0x00; + /* Turn on desired thermistor */ + for (int chip = 0; chip < NUM_CHIPS; chip++) { + i2c_write_data[chip][0] = 0x90; + i2c_write_data[chip][1] = 0x08 + (therm - 1); + i2c_write_data[chip][2] = 0x00; } - serializeI2CMsg(i2c_write_data, comm_reg_data); - pushChipConfigurations(); + serialize_i2c_msg(i2c_write_data, comm_reg_data); + push_chip_configurations(); LTC6804_wrcomm(NUM_CHIPS, comm_reg_data); LTC6804_stcomm(24); } else if (therm <= 16) { - // Turn off competing multiplexor (therms 1-8) - for(int chip = 0; chip < NUM_CHIPS; chip++) { - i2c_write_data[chip][0] = 0x90; - i2c_write_data[chip][1] = 0x00; - i2c_write_data[chip][2] = 0x00; + /* Turn off competing multiplexor (therms 1-8) */ + for (int chip = 0; chip < NUM_CHIPS; chip++) { + i2c_write_data[chip][0] = 0x90; + i2c_write_data[chip][1] = 0x00; + i2c_write_data[chip][2] = 0x00; } - serializeI2CMsg(i2c_write_data, comm_reg_data); - pushChipConfigurations(); + serialize_i2c_msg(i2c_write_data, comm_reg_data); + push_chip_configurations(); LTC6804_wrcomm(NUM_CHIPS, comm_reg_data); LTC6804_stcomm(24); - // Turn on desired thermistor - for(int chip = 0; chip < NUM_CHIPS; chip++) { - i2c_write_data[chip][0] = 0x92; - i2c_write_data[chip][1] = 0x08 + (therm - 9); - i2c_write_data[chip][2] = 0x00; + /* Turn on desired thermistor */ + for (int chip = 0; chip < NUM_CHIPS; chip++) { + i2c_write_data[chip][0] = 0x92; + i2c_write_data[chip][1] = 0x08 + (therm - 9); + i2c_write_data[chip][2] = 0x00; } - serializeI2CMsg(i2c_write_data, comm_reg_data); - pushChipConfigurations(); + serialize_i2c_msg(i2c_write_data, comm_reg_data); + push_chip_configuration(); LTC6804_wrcomm(NUM_CHIPS, comm_reg_data); LTC6804_stcomm(24); } else if (therm <= 24) { - // Turn off competing multiplexor (therms 25-32) - for(int chip = 0; chip < NUM_CHIPS; chip++) { - i2c_write_data[chip][0] = 0x96; - i2c_write_data[chip][1] = 0x00; - i2c_write_data[chip][2] = 0x00; + /* Turn off competing multiplexor (therms 25-32) */ + for (int chip = 0; chip < NUM_CHIPS; chip++) { + i2c_write_data[chip][0] = 0x96; + i2c_write_data[chip][1] = 0x00; + i2c_write_data[chip][2] = 0x00; } - serializeI2CMsg(i2c_write_data, comm_reg_data); - pushChipConfigurations(); + serialize_i2c_msg(i2c_write_data, comm_reg_data); + push_chip_configuration(); LTC6804_wrcomm(NUM_CHIPS, comm_reg_data); LTC6804_stcomm(24); - // Turn on desired thermistor - for(int chip = 0; chip < NUM_CHIPS; chip++) { - i2c_write_data[chip][0] = 0x94; - i2c_write_data[chip][1] = 0x08 + (therm - 17); - i2c_write_data[chip][2] = 0x00; + /* Turn on desired thermistor */ + for (int chip = 0; chip < NUM_CHIPS; chip++) { + i2c_write_data[chip][0] = 0x94; + i2c_write_data[chip][1] = 0x08 + (therm - 17); + i2c_write_data[chip][2] = 0x00; } - serializeI2CMsg(i2c_write_data, comm_reg_data); - pushChipConfigurations(); + serialize_i2c_msg(i2c_write_data, comm_reg_data); + push_chip_configuration(); LTC6804_wrcomm(NUM_CHIPS, comm_reg_data); LTC6804_stcomm(24); } else { - // Turn off competing multiplexor (therms 17-24) - for(int chip = 0; chip < NUM_CHIPS; chip++) { - i2c_write_data[chip][0] = 0x94; - i2c_write_data[chip][1] = 0x00; - i2c_write_data[chip][2] = 0x00; + /* Turn off competing multiplexor (therms 17-24) */ + for (int chip = 0; chip < NUM_CHIPS; chip++) { + i2c_write_data[chip][0] = 0x94; + i2c_write_data[chip][1] = 0x00; + i2c_write_data[chip][2] = 0x00; } - serializeI2CMsg(i2c_write_data, comm_reg_data); - pushChipConfigurations(); + serialize_i2c_msg(i2c_write_data, comm_reg_data); + push_chip_configuration(); LTC6804_wrcomm(NUM_CHIPS, comm_reg_data); LTC6804_stcomm(24); - // Turn on desired thermistor - for(int chip = 0; chip < NUM_CHIPS; chip++) { - i2c_write_data[chip][0] = 0x96; - i2c_write_data[chip][1] = 0x08 + (therm - 25); - i2c_write_data[chip][2] = 0x00; + /* Turn on desired thermistor */ + for (int chip = 0; chip < NUM_CHIPS; chip++) { + i2c_write_data[chip][0] = 0x96; + i2c_write_data[chip][1] = 0x08 + (therm - 25); + i2c_write_data[chip][2] = 0x00; } - serializeI2CMsg(i2c_write_data, comm_reg_data); - pushChipConfigurations(); + serialize_i2c_msg(i2c_write_data, comm_reg_data); + push_chip_configuration(); LTC6804_wrcomm(NUM_CHIPS, comm_reg_data); LTC6804_stcomm(24); } } -int8_t SegmentInterface::steinhartEst(uint16_t V) +int8_t steinhart_est(uint16_t V) { - int i = 0; - while (V < VOLT_TEMP_CONV[i]) i++; - return i + MIN_TEMP; + int i = 0; + while (V < VOLT_TEMP_CONV[i]) + i++; + return i + MIN_TEMP; } -void SegmentInterface::disableGPIOPulldowns() +void disable_gpio_pulldowns() { delay(1000); - // Turn OFF GPIO 1 & 2 pull downs - pullChipConfigurations(); - for (int c = 0; c < NUM_CHIPS; c++) - { + /* Turn OFF GPIO 1 & 2 pull downs */ + pull_chip_configuration(); + for (int c = 0; c < NUM_CHIPS; c++) { local_config[c][0] |= 0x18; } - pushChipConfigurations(); - - pullChipConfigurations(); - Serial.print("Chip CFG:\n"); - for (int c = 0; c < NUM_CHIPS; c++) - { - for (int byte = 0; byte < 6; byte++) - { - Serial.print(local_config[c][byte], HEX); - Serial.print("\t"); + push_chip_configuration(); + + pull_chip_configuration(); + printf("Chip CFG:\n"); + for (int c = 0; c < NUM_CHIPS; c++) { + for (int byte = 0; byte < 6; byte++) { + printf(local_config[c][byte], HEX); + printf("\t"); } - Serial.println(); + printf("\n"); } - Serial.println("Done"); + printf("Done\n"); } -void SegmentInterface::serializeI2CMsg(uint8_t data_to_write[][3], uint8_t comm_output[][6]) +void serialize_i2c_msg(uint8_t data_to_write[][3], uint8_t comm_output[][6]) { - for (int chip = 0; chip < NUM_CHIPS; chip++) - { - comm_output[chip][0] = 0x60 | (data_to_write[chip][0] >> 4); // START + high side of B0 - comm_output[chip][1] = (data_to_write[chip][0] << 4) | 0x00; // low side of B0 + ACK - comm_output[chip][2] = 0x00 | (data_to_write[chip][1] >> 4); // BLANK + high side of B1 - comm_output[chip][3] = (data_to_write[chip][1] << 4) | 0x00; // low side of B1 + ACK - comm_output[chip][4] = 0x00 | (data_to_write[chip][2] >> 4); // BLANK + high side of B2 - comm_output[chip][5] = (data_to_write[chip][2] << 4) | 0x09; // low side of B2 + STOP & NACK - } + for (int chip = 0; chip < NUM_CHIPS; chip++) { + comm_output[chip][0] = 0x60 | (data_to_write[chip][0] >> 4); /* START + high side of B0 */ + comm_output[chip][1] = (data_to_write[chip][0] << 4) | 0x00; /* low side of B0 + ACK */ + comm_output[chip][2] = 0x00 | (data_to_write[chip][1] >> 4); /* BLANK + high side of B1 */ + comm_output[chip][3] = (data_to_write[chip][1] << 4) | 0x00; /* low side of B1 + ACK */ + comm_output[chip][4] = 0x00 | (data_to_write[chip][2] >> 4); /* BLANK + high side of B2 */ + comm_output[chip][5] + = (data_to_write[chip][2] << 4) | 0x09; /* low side of B2 + STOP & NACK */ + } } -void SegmentInterface::averagingThermCheck() +void averaging_therm_check() { - for (int therm = 1; therm <= 16; therm++) - { - for (int c = 0; c < NUM_CHIPS; c++) - { - // Directly update for a set time from start up due to therm voltages needing to settle - if (therm_avg_counter < THERM_AVG * 10) - { - segment_data[c].thermistor_value[therm - 1] = segment_data[c].thermistor_reading[therm - 1]; - segment_data[c].thermistor_value[therm + 15] = segment_data[c].thermistor_reading[therm + 15]; - therm_avg_counter++; - } else - { - // We need to investigate this. Very sloppy - // Discard if reading is 33C - if (segment_data[c].thermistor_reading[therm - 1] != 33) - { - // If measured value is larger than current "averaged" value, increment value - if (segment_data[c].thermistor_reading[therm - 1] > segment_data[c].thermistor_value[therm - 1]) - { - segment_data[c].thermistor_value[therm - 1]++; - // If measured value is smaller than current "averaged" value, decrement value - } else if (segment_data[c].thermistor_reading[therm - 1] < segment_data[c].thermistor_value[therm - 1]) - { - segment_data[c].thermistor_value[therm - 1]--; - } - } - - // See comments above. Identical but for the upper 16 therms - if (segment_data[c].thermistor_reading[therm + 15] != 33) - { - if (segment_data[c].thermistor_reading[therm + 15] > segment_data[c].thermistor_value[therm + 15]) - { - segment_data[c].thermistor_value[therm + 15]++; - } else if (segment_data[c].thermistor_reading[therm + 15] < segment_data[c].thermistor_value[therm + 15]) - { - segment_data[c].thermistor_value[therm + 15]--; - } - } - } - } - } + for (int therm = 1; therm <= 16; therm++) { + for (int c = 0; c < NUM_CHIPS; c++) { + /* Directly update for a set time from start up due to therm voltages + * needing to settle */ + if (therm_avg_counter < THERM_AVG * 10) { + segment_data[c].thermistor_value[therm - 1] + = segment_data[c].thermistor_reading[therm - 1]; + segment_data[c].thermistor_value[therm + 15] + = segment_data[c].thermistor_reading[therm + 15]; + therm_avg_counter++; + } else { + /* We need to investigate this. Very sloppy */ + /* Discard if reading is 33C */ + if (segment_data[c].thermistor_reading[therm - 1] != 33) { + /* If measured value is larger than current "averaged" value, + * increment value */ + if (segment_data[c].thermistor_reading[therm - 1] + > segment_data[c].thermistor_value[therm - 1]) { + segment_data[c].thermistor_value[therm - 1]++; + /* If measured value is smaller than current "averaged" value, + * decrement value */ + } else if (segment_data[c].thermistor_reading[therm - 1] + < segment_data[c].thermistor_value[therm - 1]) { + segment_data[c].thermistor_value[therm - 1]--; + } + } + + /* See comments above. Identical but for the upper 16 therms */ + if (segment_data[c].thermistor_reading[therm + 15] != 33) { + if (segment_data[c].thermistor_reading[therm + 15] + > segment_data[c].thermistor_value[therm + 15]) { + segment_data[c].thermistor_value[therm + 15]++; + } else if (segment_data[c].thermistor_reading[therm + 15] + < segment_data[c].thermistor_value[therm + 15]) { + segment_data[c].thermistor_value[therm + 15]--; + } + } + } + } + } } -void SegmentInterface::standardDevThermCheck() +void standard_dev_therm_check() { - if (previous_data == nullptr) - return; - int16_t avg_temp = calcAverage(); - uint8_t standard_dev = calcThermStandardDev(avg_temp); - for(uint8_t c = 0; c < NUM_CHIPS; c++) - { - for(uint8_t therm = 17; therm < 28; therm++) - { - // If difference between thermistor and average is more than MAX_STANDARD_DEV set the therm to pack average - if (abs(segment_data[c].thermistor_value[therm] - avg_temp) > (MAX_STANDARD_DEV * standard_dev)) - { - // Nullify thermistor by setting to pack average - segment_data[c].thermistor_value[therm] = previous_data[c].thermistor_value[therm]; - } - - } - } + if (previous_data == nullptr) + return; + int16_t avg_temp = calc_average(); + uint8_t standard_dev = calc_therm_standard_dev(avg_temp); + for (uint8_t c = 0; c < NUM_CHIPS; c++) { + for (uint8_t therm = 17; therm < 28; therm++) { + /* + * If difference between thermistor and average is more than + * MAX_STANDARD_DEV set the therm to pack average + */ + if (abs(segment_data[c].thermistor_value[therm] - avg_temp) + > (MAX_STANDARD_DEV * standard_dev)) { + /* Nullify thermistor by setting to pack average */ + segment_data[c].thermistor_value[therm] = previous_data[c].thermistor_value[therm]; + } + } + } } -int8_t SegmentInterface::calcThermStandardDev(int16_t avg_temp) +int8_t calc_therm_standard_dev(int16_t avg_temp) { - uint16_t sum_diff_sqrd = 0; - for(uint8_t chip = 0; chip < NUM_CHIPS; chip++) - { - for(uint8_t therm = 17; therm < 28; therm++) - { - uint16_t sum_diff = abs(segment_data[chip].thermistor_value[therm] - avg_temp); - sum_diff_sqrd += sum_diff * sum_diff; - } - } - - uint8_t standard_dev = sqrt(sum_diff_sqrd / 88); - if(standard_dev < 8) - { - standard_dev = 8; - } - return standard_dev; + uint16_t sum_diff_sqrd = 0; + for (uint8_t chip = 0; chip < NUM_CHIPS; chip++) { + for (uint8_t therm = 17; therm < 28; therm++) { + uint16_t sum_diff = abs(segment_data[chip].thermistor_value[therm] - avg_temp); + sum_diff_sqrd += sum_diff * sum_diff; + } + } + + uint8_t standard_dev = sqrt(sum_diff_sqrd / 88); + if (standard_dev < 8) { + standard_dev = 8; + } + return standard_dev; } -int16_t SegmentInterface::calcAverage() +int16_t calc_average() { - int16_t avg = 0; - for(int chip = 0; chip < NUM_CHIPS; chip++) - { - for(int therm = 17; therm < 28; therm++) - { - avg += segment_data[chip].thermistor_value[therm]; - } - } - - avg = avg / (NUM_CHIPS * 11); - return avg; + int16_t avg = 0; + for (int chip = 0; chip < NUM_CHIPS; chip++) { + for (int therm = 17; therm < 28; therm++) { + avg += segment_data[chip].thermistor_value[therm]; + } + } + + avg = avg / (NUM_CHIPS * 11); + return avg; } -void SegmentInterface::varianceThermCheck() +void variance_therm_check() { - if (previous_data == nullptr) - { - variance_timer.startTimer(1000); - return; - } - - if (variance_timer.isTimerExpired()) - { - for(uint8_t c = 0; c < NUM_CHIPS; c++) - { - for(uint8_t therm = 17; therm < 28; therm++) - { - if (abs(segment_data[c].thermistor_reading[therm] - previous_data[c].thermistor_reading[therm]) > 5 && - (segment_data[c].thermistor_reading[therm] < 10 || segment_data[c].thermistor_reading[therm] > 30)) - { - segment_data[c].thermistor_reading[therm] = previous_data[c].thermistor_reading[therm]; - segment_data[c].thermistor_value[therm] = previous_data[c].thermistor_value[therm]; - } - } - } - - } + if (previous_data == nullptr) { + variance_timer.startTimer(1000); + return; + } + + if (variance_timer.isTimerExpired()) { + for (uint8_t c = 0; c < NUM_CHIPS; c++) { + for (uint8_t therm = 17; therm < 28; therm++) { + if (abs(segment_data[c].thermistor_reading[therm] + - previous_data[c].thermistor_reading[therm]) + > 5 + && (segment_data[c].thermistor_reading[therm] < 10 + || segment_data[c].thermistor_reading[therm] > 30)) { + segment_data[c].thermistor_reading[therm] + = previous_data[c].thermistor_reading[therm]; + segment_data[c].thermistor_value[therm] + = previous_data[c].thermistor_value[therm]; + } + } + } + } } -void SegmentInterface::discardNeutrals() +void discard_neutrals() { - for(uint8_t c = 0; c < NUM_CHIPS; c++) - { - for(uint8_t therm = 17; therm < 28; therm++) - { - if (segment_data[c].thermistor_reading[therm] == 33) - { - segment_data[c].thermistor_reading[therm] = 25; - segment_data[c].thermistor_value[therm] = 25; - } - } - } + for (uint8_t c = 0; c < NUM_CHIPS; c++) { + for (uint8_t therm = 17; therm < 28; therm++) { + if (segment_data[c].thermistor_reading[therm] == 33) { + segment_data[c].thermistor_reading[therm] = 25; + segment_data[c].thermistor_value[therm] = 25; + } + } + } } \ No newline at end of file diff --git a/Core/Src/stateMachine.c b/Core/Src/stateMachine.c new file mode 100644 index 0000000..a6a45ac --- /dev/null +++ b/Core/Src/stateMachine.c @@ -0,0 +1,417 @@ +#include "stateMachine.h" + +AccumulatorData_t* prevAccData; +uint32_t bms_fault = FAULTS_CLEAR; + +BMSState_t current_state = BOOT_STATE; +uint32_t previousFault = 0; + +tristate_timer over_curr_tmr; +tristate_timer over_chgcurr_tmr; +tristate_timer under_volt_tmr; +tristate_timer over_voltcharge_tmr; +tristate_timer overVolt_tmr; +tristate_timer lowCell_tmr; +tristate_timer highTemp_tmr; + +tristate_timer prefaultOverCurr_tmr; +tristate_timer prefaultLowCell_tmr; + +Timer charge_timeout; +tristate_timer charge_cut_off_time; + +Timer prefaultCANDelay1; // low cell +Timer prefaultCANDelay2; // dcl + +Timer can_msg_timer; + +bool entered_faulted = false; + +Timer charge_message_timer; +static const uint16_t CHARGE_MESSAGE_WAIT = 250; /* ms */ + +const bool valid_transition_from_to[NUM_STATES][NUM_STATES] = { + /* BOOT, READY, CHARGING, FAULTED */ + { true, true, false, true }, /* BOOT */ + { false, true, true, true }, /* READY */ + { false, true, true, true }, /* CHARGING */ + { true, false, false, true } /* FAULTED */ +}; + +typedef void (*HandlerFunction_t)(AccumulatorData_t* bmsdata); +typedef void (*InitFunction_t)(); + +const InitFunction_t init_LUT[NUM_STATES] + = { &init_boot, &init_ready, &init_charging, &init_faulted }; + +const HandlerFunction_t handler_LUT[NUM_STATES] + = { &handle_boot, &handle_ready, &handle_charging, &handle_faulted }; + +void init_boot() { return; } + +void handle_boot(AccumulatorData_t* bmsdata) +{ + prevAccData = nullptr; + segment.enableBalancing(false); + compute.compute_enable_charging(false); + + // bmsdata->fault_code = FAULTS_CLEAR; + + request_transition(READY_STATE); + return; +} + +void init_ready() +{ + segment.enableBalancing(false); + compute.compute_enable_charging(false); + return; +} + +void handle_ready(AccumulatorData_t* bmsdata) +{ + /* check for charger connection */ + if (compute.compute_charger_connected()) { + request_transition(CHARGING_STATE); + } else { + sm_broadcast_current_limit(bmsdata); + return; + } +} + +void init_charging() +{ + charge_timeout.cancelTimer(); + return; +} + +void handle_charging(AccumulatorData_t* bmsdata) +{ + if (!compute.compute_charger_connected()) { + request_transition(READY_STATE); + return; + } else { + /* Check if we should charge */ + if (sm_charging_check(analyzer.bmsdata)) { + digitalWrite(CHARGE_SAFETY_RELAY, HIGH); + compute.compute_enable_charging(true); + } else { + digitalWrite(CHARGE_SAFETY_RELAY, LOW); + compute.compute_enable_charging(false); + } + + /* Check if we should balance */ + if (statemachine_balancing_check(analyzer.bmsdata)) { + sm_balance_cells(analyzer.bmsdata); + } else { + segment.enableBalancing(false); + } + + /* Send CAN message, but not too often */ + if (charge_message_timer.isTimerExpired()) { + compute.compute_send_charging_message( + (MAX_CHARGE_VOLT * NUM_CELLS_PER_CHIP * NUM_CHIPS), analyzer.bmsdata); + charge_message_timer.startTimer(CHARGE_MESSAGE_WAIT); + } else { + digitalWrite(CHARGE_SAFETY_RELAY, LOW); + } + } +} + +void init_faulted() +{ + segment.enableBalancing(false); + compute.compute_enable_charging(false); + entered_faulted = true; + return; +} + +void handle_faulted(AccumulatorData_t* bmsdata) +{ + if (entered_faulted) { + entered_faulted = false; + previousFault = sm_fault_return(bmsdata); + } + + if (bmsdata->fault_code == FAULTS_CLEAR) { + compute.compute_set_fault(NOT_FAULTED); + request_transition(BOOT_STATE); + return; + } + + else { + compute.compute_set_fault(FAULTED); + digitalWrite(CHARGE_SAFETY_RELAY, LOW); + } + return; +} + +void sm_handle_state(AccumulatorData_t* bmsdata) +{ + preFaultCheck(bmsdata); + bmsdata->is_charger_connected = compute.compute_charger_connected(); + bmsdata->fault_code = sm_fault_return(bmsdata); + + if (bmsdata->fault_code != FAULTS_CLEAR) { + bmsdata->discharge_limit = 0; + request_transition(FAULTED_STATE); + } + + // TODO needs testing + handler_LUT[current_state](bmsdata); + + compute.compute_set_fan_speed(analyzer.calcFanPWM()); + sm_broadcast_current_limit(bmsdata); + + /* send relevant CAN msgs */ + // clang-format off + if (can_msg_timer.isTimerExpired()) + { + compute.compute_send_acc_status_message(analyzer.bmsdata); + compute.compute_send_current_message(analyzer.bmsdata); + compute.compute_send_bms_status_message(analyzer.bmsdata, current_state, segment.isBalancing()); + compute.compute_send_cell_temp_message(analyzer.bmsdata->max_temp, analyzer.bmsdata->min_temp, analyzer.bmsdata->avg_temp); + compute.compute_send_cell_data_message(analyzer.bmsdata); + compute.send_segment_temp_message(analyzer.bmsdata); + can_msg_timer.startTimer(CAN_MESSAGE_WAIT); + } + // clang-format on +} + +void request_transition(BMSState_t next_state) +{ + if (current_state == next_state) + return; + if (!valid_transition_from_to[current_state][next_state]) + return; + + init_LUT[next_state](); + current_state = next_state; +} + +uint32_t sm_fault_return(AccumulatorData_t* accData) +{ + /* FAULT CHECK (Check for fuckies) */ + + static struct fault_eval fault_table[8] = { + // clang-format off + + // ___________FAULT ID____________ __________TIMER___________ _____________DATA________________ __OPERATOR__ __________________________THRESHOLD____________________________ _______TIMER LENGTH_________ _____________FAULT CODE_________________ _______________DATA_____________________ ___OPERATOR___ ________THRESHOLD________ + {.id = "Discharge Current Limit", .timer = over_curr_tmr, .data_1 = accData->pack_current, .optype_1 = GT, .lim_1 = (accData->discharge_limit + DCDC_CURRENT_DRAW)*10*1.04, .timeout = OVER_CURR_TIME, .code = DISCHARGE_LIMIT_ENFORCEMENT_FAULT /* -----------------------------------UNUSED---------------------------------*/ }, + {.id = "Charge Current Limit", .timer = over_chgcurr_tmr, .data_1 = accData->pack_current, .optype_1 = GT, .lim_1 = (accData->charge_limit)*10, .timeout = OVER_CHG_CURR_TIME, .code = CHARGE_LIMIT_ENFORCEMENT_FAULT, .data_2 = accData->pack_current, .optype_2 = LT, .lim_2 = 0 }, + {.id = "Low Cell Voltage", .timer = under_volt_tmr, .data_1 = accData->min_voltage.val, .optype_1 = LT, .lim_1 = MIN_VOLT * 10000, .timeout = UNDER_VOLT_TIME, .code = CELL_VOLTAGE_TOO_LOW /* -----------------------------------UNUSED---------------------------------*/ }, + {.id = "High Cell Voltage", .timer = over_voltcharge_tmr, .data_1 = accData->max_voltage.val, .optype_1 = GT, .lim_1 = MAX_CHARGE_VOLT * 10000, .timeout = OVER_VOLT_TIME, .code = CELL_VOLTAGE_TOO_HIGH /* -----------------------------------UNUSED---------------------------------*/ }, + {.id = "High Cell Voltage", .timer = overVolt_tmr, .data_1 = accData->max_voltage.val, .optype_1 = GT, .lim_1 = MAX_VOLT * 10000, .timeout = OVER_VOLT_TIME, .code = CELL_VOLTAGE_TOO_HIGH, .data_2 = accData->is_charger_connected, .optype_2 = EQ, .lim_2 = false }, + {.id = "High Temp", .timer = highTemp_tmr, .data_1 = accData->max_temp.val, .optype_1 = GT, .lim_1 = MAX_CELL_TEMP, .timeout = LOW_CELL_TIME, .code = PACK_TOO_HOT /* -----------------------------------UNUSED---------------------------------*/ }, + {.id = "Extremely Low Voltage", .timer = lowCell_tmr, .data_1 = accData->min_voltage.val, .optype_1 = LT, .lim_1 = 900, .timeout = HIGH_TEMP_TIME, .code = LOW_CELL_VOLTAGE /* -----------------------------------UNUSED---------------------------------*/ }, + + NULL + // clang-format on + }; + + uint32_t fault_status = 0; + int incr = 0; + + while (&fault_table[incr] != NULL) { + fault_status |= sm_fault_eval(fault_table[incr]); + incr++; + } + + return fault_status; +} + +uint32_t sm_fault_eval(fault_eval index) +{ + bool condition1; + bool condition2; + + index.timer.eval_length = index.timeout; + + // clang-format off + switch (index.optype_1) + { + case GT: condition1 = index.data_1 > index.lim_1; break; + case LT: condition1 = index.data_1 < index.lim_1; break; + case GE: condition1 = index.data_1 >= index.lim_1; break; + case LE: condition1 = index.data_1 <= index.lim_1; break; + case EQ: condition1 = index.data_1 == index.lim_1; break; + case NEQ: condition1 = index.data_1 != index.lim_1; break; + case NOP: condition1 = true; break; + } + + switch (index.optype_2) + { + case GT: condition2 = index.data_2 > index.lim_2; break; + case LT: condition2 = index.data_2 < index.lim_2; break; + case GE: condition2 = index.data_2 >= index.lim_2; break; + case LE: condition2 = index.data_2 <= index.lim_2; break; + case EQ: condition2 = index.data_2 == index.lim_2; break; + case NEQ: condition2 = index.data_2 != index.lim_2; break; + case NOP: condition2 = true; break; + } + // clang-format on + + if (index.timer.eval_state == BEFORE_TIMER_START && condition1 && condition2) { + index.timer.startTimer(index.timer.eval_length); + index.timer.eval_state = DURING_EVAL; + } + + else if (index.timer.eval_state == DURING_EVAL) { + if (index.timer.isTimerExpired()) { + return index.code; + } + if (!(condition1 && condition2)) { + index.timer.cancelTimer(); + index.timer.eval_state = BEFORE_TIMER_START; + } + } + + return 0; +} + +bool sm_charging_check(AccumulatorData_t* bmsdata) +{ + if (!compute.compute_charger_connected()) + return false; + if (!charge_timeout.isTimerExpired()) + return false; + + if (bmsdata->max_voltage.val >= (MAX_CHARGE_VOLT * 10000) + && charge_cut_off_time.eval_state == BEFORE_TIMER_START) { + charge_cut_off_time.startTimer(5000); + charge_cut_off_time.eval_state = DURING_EVAL; + } else if (charge_cut_off_time.eval_state == DURING_EVAL) { + if (charge_cut_off_time.isTimerExpired()) { + charge_timeout.startTimer(CHARGE_TIMEOUT); + return false; + } + if (!(bmsdata->max_voltage.val >= (MAX_CHARGE_VOLT * 10000))) { + charge_cut_off_time.cancelTimer(); + charge_cut_off_time.eval_state = BEFORE_TIMER_START; + } + } + + return true; +} + +bool statemachine_balancing_check(AccumulatorData_t* bmsdata) +{ + if (!compute.compute_charger_connected()) + return false; + if (bmsdata->max_temp.val > MAX_CELL_TEMP_BAL) + return false; + if (bmsdata->max_voltage.val <= (BAL_MIN_V * 10000)) + return false; + if (bmsdata->delt_voltage <= (MAX_DELTA_V * 10000)) + return false; + + return true; +} + +void preFaultCheck(AccumulatorData_t* bmsdata) +{ + // prefault for Low Cell Voltage + if (prefaultLowCell_tmr.eval_state == BEFORE_TIMER_START + && bmsdata->min_voltage.val < MIN_VOLT * 10000) { + prefaultLowCell_tmr.startTimer(PRE_UNDER_VOLT_TIME); + prefaultLowCell_tmr.eval_state = DURING_EVAL; + } else if (prefaultLowCell_tmr.eval_state == DURING_EVAL) { + if (prefaultLowCell_tmr.isTimerExpired()) { + if (prefaultCANDelay1.isTimerExpired()) { + compute.sendDclPreFault(true); + prefaultCANDelay1.startTimer(CAN_MESSAGE_WAIT); + } + } + if (!(bmsdata->min_voltage.val < MIN_VOLT * 10000)) { + prefaultLowCell_tmr.cancelTimer(); + prefaultLowCell_tmr.eval_state = BEFORE_TIMER_START; + } + } + + /* prefault for DCL */ + if (prefaultOverCurr_tmr.eval_state == BEFORE_TIMER_START + && (bmsdata->pack_current) > ((bmsdata->discharge_limit + DCDC_CURRENT_DRAW) * 10 + * 1.04)) // *104% to account for current sensor +/-A + { + prefaultOverCurr_tmr.startTimer(PRE_OVER_CURR_TIME); + prefaultOverCurr_tmr.eval_state = DURING_EVAL; + } else if (prefaultOverCurr_tmr.eval_state == DURING_EVAL) { + if (prefaultOverCurr_tmr.isTimerExpired()) { + if (prefaultCANDelay2.isTimerExpired()) { + compute.sendDclPreFault(true); + prefaultCANDelay2.startTimer(CAN_MESSAGE_WAIT); + } + } + if (!((bmsdata->pack_current) + > ((bmsdata->discharge_limit + DCDC_CURRENT_DRAW) * 10 * 1.04))) { + prefaultOverCurr_tmr.cancelTimer(); + prefaultOverCurr_tmr.eval_state = BEFORE_TIMER_START; + } + } +} + +void sm_broadcast_current_limit(AccumulatorData_t* bmsdata) +{ + // States for Boosting State Machine + static enum { BOOST_STANDBY, BOOSTING, BOOST_RECHARGE } BoostState; + + static Timer boostTimer; + static Timer boostRechargeTimer; + + /* Transitioning out of boost */ + if (boostTimer.isTimerExpired() && BoostState == BOOSTING) { + BoostState = BOOST_RECHARGE; + boostRechargeTimer.startTimer(BOOST_RECHARGE_TIME); + } + /* Transition out of boost recharge */ + if (boostRechargeTimer.isTimerExpired() && BoostState == BOOST_RECHARGE) { + BoostState = BOOST_STANDBY; + } + /* Transition to boosting */ + if ((bmsdata->pack_current) > ((bmsdata->cont_DCL) * 10) && BoostState == BOOST_STANDBY) { + BoostState = BOOSTING; + boostTimer.startTimer(BOOST_TIME); + } + + /* Currently boosting */ + if (BoostState == BOOSTING || BoostState == BOOST_STANDBY) { + bmsdata->boost_setting + = min(bmsdata->discharge_limit, bmsdata->cont_DCL * CONTDCL_MULTIPLIER); + } + + /* Currently recharging boost */ + else { + bmsdata->boost_setting = min(bmsdata->cont_DCL, bmsdata->discharge_limit); + } +} + +void sm_balance_cells(AccumulatorData_t* bms_data) +{ + bool balanceConfig[NUM_CHIPS][NUM_CELLS_PER_CHIP]; + + /* For all cells of all the chips, figure out if we need to balance by comparing the difference + * in voltages */ + for (uint8_t chip = 0; chip < NUM_CHIPS; chip++) { + for (uint8_t cell = 0; cell < NUM_CELLS_PER_CHIP; cell++) { + uint16_t delta = bms_data->chip_data[chip].voltage_reading[cell] + - (uint16_t)bms_data->min_voltage.val; + if (delta > MAX_DELTA_V * 10000) + balanceConfig[chip][cell] = true; + else + balanceConfig[chip][cell] = false; + } + } + +#ifdef DEBUG_CHARGING + printf("Cell Balancing:"); + for (uint8_t c = 0; c < NUM_CHIPS; c++) { + for (uint8_t cell = 0; cell < NUM_CELLS_PER_CHIP; cell++) { + printf(balanceConfig[c][cell]); + printf("\t"); + } + printf("\n"); + } +#endif + + segment.configureBalancing(balanceConfig); +} diff --git a/README.md b/README.md index 7c45bbe..c445626 100644 --- a/README.md +++ b/README.md @@ -45,6 +45,9 @@ serial flash # Actual command is: # openocd -f interface/cmsis-dap.cfg -f target/stm32f4x.cfg -c "adapter speed 5000" -c "program ./build/cerberus.elf verify reset exit" + +# to autoformat code +clang-format -style=file -i whateverfile.c ``` ### Mounting Hardware to Docker Container in Windows > Very specific use case but nonetheless needed, also documented in the above confluence page, on macOS and Linux this happens by default when running privileged docker container