diff --git a/include/ACU_Constants.h b/include/ACU_Constants.h index f07bad3d..451ab8ae 100644 --- a/include/ACU_Constants.h +++ b/include/ACU_Constants.h @@ -24,7 +24,7 @@ namespace ACUSystems constexpr const volt VOLTAGE_DIFF_TO_INIT_CB = 0.02; // differential with lowest cell voltage to enable cell balancing for a cell constexpr const celsius BALANCE_TEMP_LIMIT_C = 50.0; constexpr const celsius BALANCE_ENABLE_TEMP_THRESH_C = 35.0; // Celsius - constexpr const volt TS_ISOLATION_VOLTAGE = 50; // Volts + constexpr const volt TS_ISOLATION_VOLTAGE = 100; // Volts } namespace ACUInterfaces { @@ -70,7 +70,7 @@ namespace ACUInterfaces { constexpr const size_t TEENSY_OK_PIN = 3; // > Needs to stay HIGH while wd_kick_pin flips to keep BMS_OK high constexpr const size_t WD_KICK_PIN = 4; // > Needs to flip at 100 Hz to keep BMS_OK high - constexpr const size_t SW_NOT_OK_PIN = 5; // should be HIGH by default, and then set LOW after traversing state machine + constexpr const size_t SW_NOT_OK_PIN = 5; // should be HIGH by default, and then set LOW after traversing state machine constexpr const size_t N_FAULTED_STATE_PIN = 6; // > Input to Safety Light, true when teensy is not in FAULT state constexpr const size_t BSPD_CURRENT_PIN = 15; @@ -100,7 +100,6 @@ namespace ACUInterfaces { constexpr const float BIT_RESOLUTION = 4095.0F; } - namespace ACUConstants { constexpr size_t NUM_CELLS = 126; @@ -119,42 +118,42 @@ namespace ACUConstants /* Task Times */ constexpr uint32_t TICK_SM_PERIOD_US = 1000UL; // 1 000 us = 1000 Hz constexpr uint32_t TICK_SM_PRIORITY = 9; - constexpr uint32_t KICK_WATCHDOG_PERIOD_US = 5000UL; // 5000 us = 200 Hz + constexpr uint32_t KICK_WATCHDOG_PERIOD_US = 4000UL; // 10 000 us = 100 Hz constexpr uint32_t WATCHDOG_PRIORITY = 1; - constexpr uint32_t SAMPLE_BMS_PERIOD_US = 10000UL; // 10 000 us = 100 Hz (since we are reading by group) + constexpr uint32_t SAMPLE_BMS_PERIOD_US = 20000UL; // 5 000 us = 200 Hz (since we are reading by group) constexpr uint32_t SAMPLE_BMS_PRIORITY = 2; - constexpr uint32_t EVAL_ACC_PERIOD_US = 20000UL; // 20 000 us = 50 Hz + constexpr uint32_t EVAL_ACC_PERIOD_US = 20000UL; // 20 000 us = 50 Hz (problem for soc if this is running faster than voltage) constexpr uint32_t EVAL_ACC_PRIORITY = 10; constexpr uint32_t WRITE_CELL_BALANCE_PERIOD_US = 100000UL; // 100 000 us = 10 Hz constexpr uint32_t WRITE_CELL_BALANCE_PRIORITY = 15; constexpr uint32_t ALL_DATA_ETHERNET_PERIOD_US = 100000UL; // 100 000 us = 10 Hz constexpr uint32_t ALL_DATA_ETHERNET_PRIORITY = 5; - constexpr uint32_t CORE_DATA_ETHERNET_PERIOD_US = 4000UL; // 20 000 us = 50 Hz + constexpr uint32_t CORE_DATA_ETHERNET_PERIOD_US = 8000UL; // 8 000 us = 125 Hz constexpr uint32_t CORE_DATA_ETHERNET_PRIORITY = 4; - constexpr uint32_t SAMPLE_ADC_PRIORITY = 20; - constexpr uint32_t SAMPLE_ADC_PERIOD_US = 5000UL; // 10 000 us = 100 Hz + constexpr uint32_t SAMPLE_ADC_PRIORITY = 11; + constexpr uint32_t SAMPLE_ADC_PERIOD_US = 1000UL; // 1 000 us = 1000 Hz constexpr uint32_t CCU_SEND_PERIOD_US = 100000UL; // 100 000 us = 10 Hz - constexpr uint32_t CCU_SEND_PRIORITY = 11; + constexpr uint32_t CCU_SEND_PRIORITY = 14; constexpr uint32_t ACU_OK_CAN_PERIOD_US = 50000UL; // 50 000 us = 20 Hz constexpr uint32_t ACU_OK_CAN_PRIORITY = 3; constexpr uint32_t CCU_SEND_A_PERIOD_US = 100000UL; // 100 000 us = 10 Hz constexpr uint32_t CCU_SEND_A_PRIORITY = 12; constexpr uint32_t CCU_SEND_B_PERIOD_US = 100000UL; // 100 000 us = 10 Hz constexpr uint32_t CCU_SEND_B_PRIORITY = 13; - constexpr uint32_t EM_MEASUREMENT_SEND_PERIOD_US = 4000UL; // 10 000 us = 100 Hz + constexpr uint32_t EM_MEASUREMENT_SEND_PERIOD_US = 4000UL; // 4 000 us = 250 Hz constexpr uint32_t EM_MEASUREMENT_SEND_PRIORITY = 6; - constexpr uint32_t SEND_CAN_PERIOD_US = 4000UL; // 10 000 us = 100 Hz + constexpr uint32_t SEND_CAN_PERIOD_US = 4000UL; // 40 000 us = 250 Hz constexpr uint32_t SEND_CAN_PRIORITY = 8; - constexpr uint32_t RECV_CAN_PERIOD_US = 10000UL; // 10 000 us = 100 Hz + constexpr uint32_t RECV_CAN_PERIOD_US = 50000UL; // 50 000 us = 20 Hz constexpr uint32_t RECV_CAN_PRIORITY = 7; constexpr uint32_t DEBUG_PRINT_PERIOD_US = 250000UL; // 250 000 us = 4 Hz constexpr uint32_t DEBUG_PRINT_PRIORITY = 20; - constexpr uint32_t IDLE_SAMPLE_PERIOD_US = 200UL; // 1 000 us = 100 Hz + constexpr uint32_t IDLE_SAMPLE_PERIOD_US = 1000UL; // 1 000 us = 1000 Hz constexpr uint32_t IDLE_SAMPLE_PRIORITY = 0; /* Message Interface */ diff --git a/lib/interfaces/include/ADCInterface.h b/lib/interfaces/include/ADCInterface.h index 60cfd447..cb9093b7 100644 --- a/lib/interfaces/include/ADCInterface.h +++ b/lib/interfaces/include/ADCInterface.h @@ -14,7 +14,7 @@ namespace adc_default_parameters constexpr const float TEENSY41_MAX_DIGITAL_READ_VOLTAGE_THRESH = 2.8F; constexpr const float SHUTDOWN_VOLTAGE_DIGITAL_THRESHOLD = 12.0F; - constexpr const uint32_t IMD_STARTUP_TIME = 2000; + constexpr const uint32_t IMD_STARTUP_TIME = 10000; constexpr const float TEENSY41_MAX_INPUT_VOLTAGE = 3.3F; constexpr int MAX114X_VERSION = 8; diff --git a/lib/interfaces/include/BMSDriverGroup.h b/lib/interfaces/include/BMSDriverGroup.h index 93e00a3f..8eab49da 100644 --- a/lib/interfaces/include/BMSDriverGroup.h +++ b/lib/interfaces/include/BMSDriverGroup.h @@ -10,6 +10,7 @@ #include #include "etl/optional.h" #include +#include #include "etl/singleton.h" @@ -29,7 +30,7 @@ enum class SPIState_e IDLE = 0, WAIT_WRITE_COMPLETE = 1, WAIT_POLL_ADC_COMPLETE = 2, - START_CONVERSION = 3, + START_CONVERSIONS = 3, WAIT_CONVERSION = 4, WAIT_READ_COMPLETE = 5, }; @@ -95,8 +96,8 @@ namespace bms_driver_defaults constexpr const uint16_t OVER_VOLTAGE_THRESHOLD = 2625; // 4.2V (datasheet formula) Comparison Voltage = VOV • 16 • 100μV constexpr const uint16_t GPIO_ENABLE = 0x1F; constexpr const uint16_t CRC15_POLY = 0x4599; // Used for calculating the PEC table for LTC6811 - constexpr const uint16_t CV_ADC_CONVERSION_TIME_US = 2000; - constexpr const uint16_t GPIO_ADC_CONVERSION_TIME_US = 2000; + constexpr const uint16_t CV_ADC_CONVERSION_TIME_US = 1200; + constexpr const uint16_t GPIO_ADC_CONVERSION_TIME_US = 1200; constexpr const float CV_ADC_LSB_VOLTAGE = 0.0001f; // Cell voltage ADC resolution: 100μV per LSB (1/10000 V) } @@ -281,6 +282,8 @@ class BMSDriverGroup */ const char* get_current_read_group_name(); + const char* get_spi_state_name(); + /** * @brief Get validity status for all chips from last read * @return Const reference to validity data array (no copy overhead) @@ -324,11 +327,20 @@ class BMSDriverGroup return _config; } + /** + * @brief Runs atomic fetch and clear so it does everything in one atomic operation on the new voltage for fresh flag + * @return true if the voltage data is fresh (only happens once per good cycle) + * @return false if the voltage data is not fresh + */ + bool check_clear_voltage_ready() { + return _new_voltage_data_ready.exchange(false, std::memory_order_acquire); + } + private: ReadGroup_e _current_read_group = ReadGroup_e::CV_GROUP_A; - SPIState_e _spi_state = SPIState_e::IDLE; + SPIState_e _spi_state = SPIState_e::START_CONVERSIONS; /** * PEC: @@ -390,6 +402,8 @@ class BMSDriverGroup */ void _start_cell_voltage_ADC_conversion(); + void _init_adc_conversion(); + /** * Writes command to start GPIO ADC conversion * @post packaged data transfered over SPI, need to delay before we can read @@ -523,6 +537,9 @@ class BMSDriverGroup array _rx_read_buffer; array _tx_write_buffer; array _rx_write_buffer; + + // Says if voltage data is fresh for the state of charge estimator + std::atomic _new_voltage_data_ready{false}; }; template diff --git a/lib/interfaces/include/BMSDriverGroup.tpp b/lib/interfaces/include/BMSDriverGroup.tpp index f897a445..38e57cf9 100644 --- a/lib/interfaces/include/BMSDriverGroup.tpp +++ b/lib/interfaces/include/BMSDriverGroup.tpp @@ -65,15 +65,17 @@ void BMSDriverGroup::init() _spi_event.setContext(this); _spi_event.attachImmediate([](EventResponderRef ref) { - SPI1.endTransaction(); static_cast(ref.getContext())->_dma_callback(); }); + + _new_voltage_data_ready = false; } template void BMSDriverGroup::_dma_callback() { - ltc_spi_interface::write_and_delay_high(_chip_select[_current_cs_index], 2); + SPI1.endTransaction(); + ltc_spi_interface::delay_and_write_high(_chip_select[_current_cs_index], 5); // reset dma_busy var ltc_spi_interface::set_dma_idle(); @@ -81,9 +83,21 @@ void BMSDriverGroup::_dma_callback() _tx_read_buffer.fill(0); _tx_write_buffer.fill(0); - if (_spi_state == SPIState_e::WAIT_POLL_ADC_COMPLETE) + if (_spi_state == SPIState_e::WAIT_WRITE_COMPLETE) + { + _spi_state = SPIState_e::IDLE; + return; + } + + if (_spi_state == SPIState_e::START_CONVERSIONS) { + if (_current_cs_index + 1 < num_chip_selects) + { + _current_cs_index++; + return; + } _conversion_timer = 0; + _current_cs_index = 0; _spi_state = SPIState_e::WAIT_CONVERSION; return; } @@ -123,6 +137,18 @@ void BMSDriverGroup::_dma_callback() _bms_data.max_cell_voltage = _max_min_reference.max_cell_voltage; _max_min_reference.min_cell_voltage = ref_max_min_defaults::MIN_CELL_VOLTAGE; _max_min_reference.max_cell_voltage = ref_max_min_defaults::MAX_CELL_VOLTAGE; + + bool all_cv_valid = true; + for (const auto &p : _bms_data.valid_read_packets) { + if (!(p.valid_read_cells_1_to_3 && p.valid_read_cells_4_to_6 && p.valid_read_cells_7_to_9 && p.valid_read_cells_10_to_12)) { + all_cv_valid = false; + break; + } + } + + if (all_cv_valid) { + _new_voltage_data_ready = true; + } } if (_current_read_group == ReadGroup_e::AUX_GROUP_B) { @@ -137,7 +163,7 @@ void BMSDriverGroup::_dma_callback() _current_read_group = advance_read_group(_current_read_group); if (_current_read_group == ReadGroup_e::CV_GROUP_A || _current_read_group == ReadGroup_e::AUX_GROUP_A) { - _spi_state = SPIState_e::START_CONVERSION; + _spi_state = SPIState_e::START_CONVERSIONS; return; } } @@ -224,7 +250,10 @@ template typename BMSDriverGroup::BMSDriverData BMSDriverGroup::get_bms_data() { - return _bms_data; + noInterrupts(); + auto copy = _bms_data; + interrupts(); + return copy; } template @@ -236,18 +265,9 @@ void BMSDriverGroup::read_data() return; } - if (_spi_state == SPIState_e::START_CONVERSION) - { - if (_current_read_group == ReadGroup_e::CV_GROUP_A) - { - _start_cell_voltage_ADC_conversion(); - return; - } - if (_current_read_group == ReadGroup_e::AUX_GROUP_A) - { - _start_GPIO_ADC_conversion(); - return; - } + if (_spi_state == SPIState_e::START_CONVERSIONS) + { + _init_adc_conversion(); } if (_spi_state == SPIState_e::WAIT_CONVERSION && _conversion_timer > _config.cv_adc_conversion_time_us) @@ -327,7 +347,7 @@ void BMSDriverGroup::_read_data_through_ _rx_read_buffer.fill(0); _start_wakeup_protocol(_current_cs_index); SPI1.beginTransaction(SPISettings(500000, MSBFIRST, SPI_MODE3)); - ltc_spi_interface::write_and_delay_low(cs, 1); + ltc_spi_interface::write_and_delay_low(cs, 5); ltc_spi_interface::begin_transfer(_tx_read_buffer, _rx_read_buffer, _spi_event); // Update the SPI state @@ -346,6 +366,8 @@ void BMSDriverGroup::_process_broadcast_ // Clear the valid read packets buffer _bms_data.valid_read_packets.fill({}); + // Serial.println("PROCESSING READ RX BUFFER"); + for (size_t chip = 0; chip < num_chips / num_chip_selects; chip++) { size_t chip_index = chip + (_current_cs_index * (num_chips / num_chip_selects)); @@ -397,9 +419,21 @@ void BMSDriverGroup::_process_broadcast_ __builtin_unreachable(); } } + + // if (!current_group_valid) + // { + // Serial.print(get_current_read_group_name()); Serial.print(" "); + // Serial.print(chip); Serial.print(" "); + // Serial.print(_chip_select[_current_cs_index]); Serial.print(" "); + // for (int i = 0; i < data_size+4; i++) + // { + // Serial.print(_rx_read_buffer[i], HEX); Serial.print(" "); + // } + // Serial.println(); + // } if (!current_group_valid || (_current_read_group == ReadGroup_e::CV_GROUP_D && cells_per_chip == 9)) - { + { continue; } @@ -423,6 +457,17 @@ void BMSDriverGroup::_process_broadcast_ _load_auxillaries(_bms_data, _max_min_reference, spi_response, chip_index, start_index); } } + + // if (_current_read_group == ReadGroup_e::CV_GROUP_D) + // { + // Serial.print(get_current_read_group_name()); Serial.print(" "); + // Serial.print(_chip_select[_current_cs_index]); Serial.print(" "); + // for (int i = 0; i < data_size+4; i++) + // { + // Serial.print(_rx_read_buffer[i], HEX); Serial.print(" "); + // } + // Serial.println(); + // } } template @@ -483,7 +528,7 @@ void BMSDriverGroup::_read_data_through_ _rx_read_buffer.fill(0); SPI1.beginTransaction(SPISettings(500000, MSBFIRST, SPI_MODE3)); - ltc_spi_interface::write_and_delay_low(_chip_select_per_chip[_current_chip_address_index]); + ltc_spi_interface::write_and_delay_low(_chip_select_per_chip[_current_chip_address_index], 5); ltc_spi_interface::begin_transfer(_tx_read_buffer, _rx_read_buffer, _spi_event); // _start_wakeup_protocol(); @@ -539,7 +584,7 @@ void BMSDriverGroup::_load_cell_voltages { array data_in_cell_voltage; - uint8_t cell_global_offset = (chip_index / 2) * 21 + (chip_index % 2) * 12; + uint8_t cell_global_offset = (chip_index / 2) * 21 + (chip_index % 2) * num_chips; for (int cell_index = start_cell_index; cell_index < start_cell_index+3; cell_index++) { @@ -655,6 +700,8 @@ void BMSDriverGroup::write_configuration } write_configuration(_config.dcto_write, cb); + + // _spi_state = SPIState_e::WAIT_WRITE_COMPLETE; } template @@ -709,7 +756,13 @@ void BMSDriverGroup::_write_config_throu SPI1.beginTransaction(SPISettings(500000, MSBFIRST, SPI_MODE3)); ltc_spi_interface::write_and_delay_low(_chip_select[cs], 1); - ltc_spi_interface::begin_transfer(_tx_read_buffer, _rx_read_buffer, _spi_event); + for (int i = 0; i < cmd_and_data_buffer_size; i++) + { + SPI1.transfer(_tx_read_buffer[i]); + } + SPI1.endTransaction(); + ltc_spi_interface::delay_and_write_high(_chip_select[cs], 2); + // ltc_spi_interface::begin_transfer(_tx_read_buffer, _rx_read_buffer, _spi_event); } } @@ -757,7 +810,6 @@ void BMSDriverGroup::_start_cell_voltage { _start_ADC_conversion_through_address(cmd); } - _spi_state = SPIState_e::WAIT_POLL_ADC_COMPLETE; } template @@ -776,7 +828,6 @@ void BMSDriverGroup::_start_GPIO_ADC_con { _start_ADC_conversion_through_address(cmd); } - _spi_state = SPIState_e::WAIT_POLL_ADC_COMPLETE; } template @@ -791,14 +842,11 @@ void BMSDriverGroup::_start_ADC_conversi copy(cmd_and_pec.begin(), cmd_and_pec.end(), _tx_write_buffer.begin()); // Needs to be sent on each chip select line - for (size_t cs = 0; cs < num_chip_selects; cs++) - { - _start_wakeup_protocol(cs); + _start_wakeup_protocol(_current_cs_index); - SPI1.beginTransaction(SPISettings(500000, MSBFIRST, SPI_MODE3)); - ltc_spi_interface::write_and_delay_low(_chip_select[cs], 1); - ltc_spi_interface::begin_transfer(_tx_write_buffer, _rx_write_buffer, _spi_event); - } + SPI1.beginTransaction(SPISettings(500000, MSBFIRST, SPI_MODE3)); + ltc_spi_interface::write_and_delay_low(_chip_select[_current_cs_index], 2); + ltc_spi_interface::begin_transfer(_tx_write_buffer, _rx_write_buffer, _spi_event); } // UNUSED: LTC6811-2 ADDRESS MODE - REFERENCE ONLY @@ -818,6 +866,21 @@ void BMSDriverGroup::_start_ADC_conversi } } +template +void BMSDriverGroup::_init_adc_conversion() +{ + if (_current_read_group == ReadGroup_e::CV_GROUP_A) + { + _start_cell_voltage_ADC_conversion(); + return; + } + if (_current_read_group == ReadGroup_e::AUX_GROUP_A) + { + _start_GPIO_ADC_conversion(); + return; + } +} + /* -------------------- GETTER FUNCTIONS -------------------- */ @@ -890,13 +953,13 @@ const char* BMSDriverGroup::get_current_ switch (_current_read_group) { case ReadGroup_e::CV_GROUP_A: - return "GROUP_A"; + return "CV_GROUP_A"; case ReadGroup_e::CV_GROUP_B: - return "GROUP_B"; + return "CV_GROUP_B"; case ReadGroup_e::CV_GROUP_C: - return "GROUP_C"; + return "CV_GROUP_C"; case ReadGroup_e::CV_GROUP_D: - return "GROUP_D"; + return "CV_GROUP_D"; case ReadGroup_e::AUX_GROUP_A: return "AUX_A"; case ReadGroup_e::AUX_GROUP_B: @@ -906,6 +969,28 @@ const char* BMSDriverGroup::get_current_ } } +template +const char* BMSDriverGroup::get_spi_state_name() +{ + switch (_spi_state) + { + case SPIState_e::IDLE: + return "IDLE"; + case SPIState_e::WAIT_WRITE_COMPLETE: + return "WAIT_WRITE_COMPLETE"; + case SPIState_e::WAIT_POLL_ADC_COMPLETE: + return "WAIT_POLL_ADC_COMPLETE"; + case SPIState_e::START_CONVERSIONS: + return "START_CONVERSIONS"; + case SPIState_e::WAIT_CONVERSION: + return "WAIT_CONVERSION"; + case SPIState_e::WAIT_READ_COMPLETE: + return "WAIT_READ_COMPLETE"; + default: + return "UNKNOWN"; + } +} + template bool BMSDriverGroup::last_read_all_valid() { diff --git a/lib/interfaces/include/CCUInterface.h b/lib/interfaces/include/CCUInterface.h index b3417864..08396821 100644 --- a/lib/interfaces/include/CCUInterface.h +++ b/lib/interfaces/include/CCUInterface.h @@ -100,12 +100,14 @@ class CCUInterface _acu_all_data.core_data.max_board_temp = input.core_data.max_board_temp; _acu_all_data.core_data.min_cell_temp = input.core_data.min_cell_temp; _acu_all_data.core_data.max_cell_temp = input.core_data.max_cell_temp; + _acu_all_data.SoC = input.SoC; } void handle_enqueue_acu_status_CAN_message(); void handle_enqueue_acu_core_voltages_CAN_message(); void handle_enqueue_acu_voltages_CAN_message(); void handle_enqueue_acu_temps_CAN_message(); + void handle_enqueue_acu_SoC_CAN_message(); private: CCUCANInterfaceData_s _curr_data; diff --git a/lib/interfaces/include/LTCSPIInterface.h b/lib/interfaces/include/LTCSPIInterface.h index b9704758..e6590edb 100644 --- a/lib/interfaces/include/LTCSPIInterface.h +++ b/lib/interfaces/include/LTCSPIInterface.h @@ -45,6 +45,7 @@ namespace ltc_spi_interface { */ void write_and_delay_high(int cs, int delay_us); void write_and_delay_low(int cs, int delay_us); + void delay_and_write_high(int cs, int delay_us); } #include diff --git a/lib/interfaces/include/MAX114XInterface.h b/lib/interfaces/include/MAX114XInterface.h index 62c9093f..4ca12fcd 100644 --- a/lib/interfaces/include/MAX114XInterface.h +++ b/lib/interfaces/include/MAX114XInterface.h @@ -5,6 +5,8 @@ #include #include +using namespace std; + /** * Enum representing the different channel configurations in MAX114X ADCs (SINGLE, DIFFERENTIAL, or INV_DIFFERENTIAL) */ @@ -12,6 +14,7 @@ enum class CHANNEL_TYPE_e{ SINGLE, ///< single channel DIFFERENTIAL, ///< +- differential pair INV_DIFFERENTIAL, ///< -+ differential pair + NOT_USED, }; /** @@ -25,6 +28,9 @@ template class MAX114XInterface : public AnalogMultiSensor { public: + + constexpr static size_t buffer_size = 3; + /* Constructors */ /** * Constructs a MAX114X ADC interface of the specified ADC model, number of channels, and channel types. @@ -61,6 +67,11 @@ class MAX114XInterface : public AnalogMultiSensor */ void _sample() override; + /** + * Callback function for DMA SPI reads + */ + void _dma_callback(); + /** * Channel configuration is defined per channel pair (two physical channels). * This array stores the channel type for each pair of channels in the ADC. @@ -76,6 +87,12 @@ class MAX114XInterface : public AnalogMultiSensor const int _adc_not_shdn_pin; const int _spiSpeed; int _currentChannel; + + bool _dma_busy; + EventResponder _spi_event; + + array _tx_buf; + array _rx_buf; /** * The select bits for single-ended channels are all over the place and do not follow a logical mapping. diff --git a/lib/interfaces/include/MAX114XInterface.tpp b/lib/interfaces/include/MAX114XInterface.tpp index 1ec3ca92..9dbeb191 100644 --- a/lib/interfaces/include/MAX114XInterface.tpp +++ b/lib/interfaces/include/MAX114XInterface.tpp @@ -60,13 +60,58 @@ void MAX114XInterface::init() digitalWrite(_spiPinCS, HIGH); pinMode(_adc_not_shdn_pin, OUTPUT); digitalWrite(_adc_not_shdn_pin, HIGH); + + _dma_busy = false; + + _spi_event.setContext(this); + _spi_event.attachImmediate([](EventResponderRef ref) + { + SPI1.endTransaction(); + static_cast(ref.getContext())->_dma_callback(); + }); + + _tx_buf.fill(0); + _rx_buf.fill(0); } template void MAX114XInterface::tick() { + if (_dma_busy) + { + return; + } + _sample(); - this->_convert(); +} + +template +void MAX114XInterface::_dma_callback() +{ + digitalWrite(_spiPinCS, HIGH); + SPI.endTransaction(); + + // First two bits of b1 are filler + uint8_t b1 = _rx_buf[1]; + uint8_t b2 = _rx_buf[2]; + + uint16_t value = ((b1 & 0x3F) << 8) | (b2 & 0xFF); + + /* Stores return bytes (14 bit ADC conversion) in lastSample member of analog channel class corresponding to the channel. FOR DIFFERENTIAL: data for the pair is stored in the lower of the two channels. Ex: 1 & 2 are a differential pair, the object for channel 1 holds the return value. */ + MAX114XInterface::_channels[_currentChannel].lastSample = value; + + // Increments channel ID if the pair is differential or inverse differential + CHANNEL_TYPE_e channelType = _channelTypes[_currentChannel / 2]; + if (channelType == CHANNEL_TYPE_e::DIFFERENTIAL || channelType == CHANNEL_TYPE_e::INV_DIFFERENTIAL) + { + _currentChannel++; + MAX114XInterface::_channels[_currentChannel].lastSample = value; + } + _currentChannel++; + + _dma_busy = false; + + this -> _convert(); } template @@ -84,7 +129,7 @@ float MAX114XInterface::get_last_sampl template void MAX114XInterface::_sample() { - uint8_t command, b0, b1, b2; + uint8_t command; uint8_t selNum; // Resets loop after last channel is reached @@ -98,26 +143,34 @@ void MAX114XInterface::_sample() */ CHANNEL_TYPE_e channelType = _channelTypes[_currentChannel / 2]; - switch (channelType) { + switch (channelType) + { case CHANNEL_TYPE_e::SINGLE: - + { // The channel selection bits for single mode follows this array selNum = (_single_end_channel_to_select_map[_currentChannel]); break; - + } case CHANNEL_TYPE_e::DIFFERENTIAL: - + { // The channel selection bits for differential mode is the channel number halved and then truncated // The channelId is post-incremented so the sample() function does not send the same command byte for the other channel in the differential pair selNum = (_currentChannel / 2); break; - + } case CHANNEL_TYPE_e::INV_DIFFERENTIAL: - + { // The channel selection bits for inversed differential mode is the channel number halved, truncated, and with a 1 in the MSB // The channelId is post-incremented so the sample() function does not send the same command byte for the other channel in the differential pair selNum = ((_currentChannel / 2) | 0b100); break; + } + case CHANNEL_TYPE_e::NOT_USED: + { + // assumes that the channels not being used is in pairs - works for acu rev 10 application + _currentChannel += 2; + return; + } } /* Page 14 of datasheet @@ -130,30 +183,14 @@ void MAX114XInterface::_sample() (0x01 << 1) | // external clock mode (0x01); // ^ - // initialize SPI bus. REQUIRED: call SPI.begin() before this SPI.beginTransaction(SPISettings(_spiSpeed, MSBFIRST, SPI_MODE0)); digitalWrite(_spiPinCS, LOW); - b0 = SPI.transfer(command); - b1 = SPI.transfer(0x00); // dummy bytes to clock out data from the ADC - b2 = SPI.transfer(0x00); // ^ - - digitalWrite(_spiPinCS, HIGH); - - SPI.endTransaction(); + _tx_buf[0] = command; + _rx_buf.fill(0); + SPI.transfer(_tx_buf.data(), _rx_buf.data(), buffer_size, _spi_event); - // First two bits of b1 are filler - uint16_t value = ((b1 & 0x3F) << 8) | (b2 & 0xFF); - - /* Stores return bytes (14 bit ADC conversion) in lastSample member of analog channel class corresponding to the channel. FOR DIFFERENTIAL: data for the pair is stored in the lower of the two channels. Ex: 1 & 2 are a differential pair, the object for channel 1 holds the return value. */ - MAX114XInterface::_channels[_currentChannel].lastSample = value; - - // Increments channel ID if the pair is differential or inverse differential - if (channelType == CHANNEL_TYPE_e::DIFFERENTIAL || channelType == CHANNEL_TYPE_e::INV_DIFFERENTIAL) - { - _currentChannel++; - MAX114XInterface::_channels[_currentChannel].lastSample = value; - } - _currentChannel++; + // set the dma busy flag + _dma_busy = true; } \ No newline at end of file diff --git a/lib/interfaces/src/ADCInterface.cpp b/lib/interfaces/src/ADCInterface.cpp index 84a292c1..4833ce0a 100644 --- a/lib/interfaces/src/ADCInterface.cpp +++ b/lib/interfaces/src/ADCInterface.cpp @@ -33,7 +33,7 @@ bool ADCInterface::read_imd_ok(uint32_t curr_millis) if (_in_imd_startup_period) { if ((curr_millis - _init_millis) >= _adc_parameters.configs.imd_startup_time) - { // give 2 seconds for IMD to startup + { // give time for IMD to startup _in_imd_startup_period = false; } return true; diff --git a/lib/interfaces/src/CCUInterface.cpp b/lib/interfaces/src/CCUInterface.cpp index a1636c85..a896183e 100644 --- a/lib/interfaces/src/CCUInterface.cpp +++ b/lib/interfaces/src/CCUInterface.cpp @@ -54,6 +54,14 @@ void CCUInterface::handle_enqueue_acu_voltages_CAN_message() { CAN_util::enqueue_msg(&detailed_msg, &Pack_BMS_DETAILED_VOLTAGES_hytech, ACUCANInterfaceImpl::ccu_can_tx_buffer); } +void CCUInterface::handle_enqueue_acu_SoC_CAN_message() { + STATE_OF_CHARGE_t msg = {}; + msg.charge_percentage_ro = HYTECH_charge_percentage_ro_toS(_acu_all_data.SoC * 100); + msg.min_cell_voltage_est_ro = 0; + msg.charge_coulombs_ro = 0; + CAN_util::enqueue_msg(&msg, &Pack_STATE_OF_CHARGE_hytech, ACUCANInterfaceImpl::ccu_can_tx_buffer); +} + void CCUInterface::handle_enqueue_acu_temps_CAN_message() { BMS_DETAILED_TEMPS_t detailed_msg = {}; detailed_msg.ic_id = static_cast(_curr_data.detailed_temps_ic_id); diff --git a/lib/interfaces/src/EMInterface.cpp b/lib/interfaces/src/EMInterface.cpp index a4100a67..59f4c16b 100644 --- a/lib/interfaces/src/EMInterface.cpp +++ b/lib/interfaces/src/EMInterface.cpp @@ -4,8 +4,8 @@ void EMInterface::receive_EM_measurement_message(const CAN_message_t &msg, uint32_t curr_millis) { EM_MEASUREMENT_t em_msg; Unpack_EM_MEASUREMENT_hytech(&em_msg, &msg.buf[0], msg.len); - _em_data.em_voltage = HYTECH_em_current_ro_fromS(em_msg.em_current_ro); - _em_data.em_current = HYTECH_em_voltage_ro_fromS(em_msg.em_current_ro); + _em_data.em_voltage = HYTECH_em_voltage_ro_fromS(em_msg.em_voltage_ro); + _em_data.em_current = HYTECH_em_current_ro_fromS(em_msg.em_current_ro); _em_data.time_since_prev_msg_ms = curr_millis - _em_data.prev_time_stamp_ms; _em_data.prev_time_stamp_ms = curr_millis; } diff --git a/lib/interfaces/src/LTCSPIInterface.cpp b/lib/interfaces/src/LTCSPIInterface.cpp index b84dd829..70ece2a0 100644 --- a/lib/interfaces/src/LTCSPIInterface.cpp +++ b/lib/interfaces/src/LTCSPIInterface.cpp @@ -30,5 +30,11 @@ namespace ltc_spi_interface digitalWrite(cs, HIGH); delayMicroseconds(delay_us); } + + void delay_and_write_high(int cs, int delay_us) + { + delayMicroseconds(delay_us); + digitalWrite(cs, HIGH); + } } diff --git a/lib/shared_types/shared_types.h b/lib/shared_types/shared_types.h index 11136865..14cc1c3c 100644 --- a/lib/shared_types/shared_types.h +++ b/lib/shared_types/shared_types.h @@ -59,8 +59,13 @@ struct BMSCoreData_s { */ enum ReadGroup_e { - CV_GROUP_A = 0, CV_GROUP_B, CV_GROUP_C, CV_GROUP_D, - AUX_GROUP_A, AUX_GROUP_B, NUM_GROUPS + CV_GROUP_A = 0, + CV_GROUP_B, + CV_GROUP_C, + CV_GROUP_D, + AUX_GROUP_A, + AUX_GROUP_B, + NUM_GROUPS }; diff --git a/lib/state_machine/src/ACUStateMachine.cpp b/lib/state_machine/src/ACUStateMachine.cpp index 63afb6c9..aa3a429a 100644 --- a/lib/state_machine/src/ACUStateMachine.cpp +++ b/lib/state_machine/src/ACUStateMachine.cpp @@ -16,6 +16,12 @@ void ACUStateMachine::tick_state_machine(unsigned long current_millis) break; } + if (_has_bms_fault() || _has_imd_fault()) + { + _set_state(ACUState_e::FAULTED, current_millis); + break; + } + break; } case ACUState_e::WELDCHECK: diff --git a/lib/systems/include/ACUController.h b/lib/systems/include/ACUController.h index 656560a3..478f1542 100644 --- a/lib/systems/include/ACUController.h +++ b/lib/systems/include/ACUController.h @@ -2,25 +2,29 @@ #define ACUCONTROLLER_H #include +#include #include #include #include #include "etl/singleton.h" #include "SharedFirmwareTypes.h" #include "shared_types.h" +#include "SOCKalmanFilter.h" namespace acu_controller_default_parameters { - constexpr const size_t MAX_INVALID_PACKET_FAULT_COUNT = 1000000; // Same as voltage fault count + constexpr const size_t MAX_INVALID_PACKET_FAULT_COUNT = 1000; // Same as voltage fault count constexpr const time_ms MAX_VOLTAGE_FAULT_DUR = 1000; // At 15 Hz, we'll know if there is an error within 3 seconds of startup constexpr const time_ms MAX_TEMP_FAULT_DUR = 1000; - constexpr const time_ms MAX_INVALID_PACKET_FAULT_DUR = 500; // In cases in EMI, we will need more leniency with invalid packet faults + constexpr const time_ms MAX_INVALID_PACKET_FAULT_DUR = 1000; // In cases in EMI, we will need more leniency with invalid packet faults constexpr const float PACK_NOMINAL_CAPACITY_AH = 13.5; // nominal pack capacity in amp * hours constexpr const float PACK_MAX_VOLTAGE = 529.2; // from data sheet https://wiki.hytechracing.org/books/ht09-design/page/molicel-pack-investigation constexpr const float PACK_MIN_VOLTAGE = 378.0; // from data sheet^ but just assume 126 * 3.0V constexpr const float PACK_INTERNAL_RESISTANCE = 0.246; // Ohms (measured) + constexpr const float MIN_CELL_VOLTAGE_FOR_SOC = 2.5; // Volts } + struct ACUControllerData_s { time_ms last_time_uv_fault_not_present; @@ -31,6 +35,7 @@ struct ACUControllerData_s time_ms last_time_invalid_packet_present; time_ms prev_bms_time_stamp; time_ms prev_em_time_stamp; + time_ms first_zero_current_time_stamp; float SoC; bool has_fault; bool bms_ok; @@ -117,7 +122,7 @@ class ACUController * @post updates configuration bytes and sends configuration command * @param pack_current current flowing from the pack in amps (negative during discharge, positive during charge) */ - ACUControllerData_s evaluate_accumulator(time_ms current_millis, const BMSCoreData_s &bms_core_data, size_t max_consecutive_invalid_packet_count, float em_current, size_t num_of_voltage_cells); + ACUControllerData_s evaluate_accumulator(time_ms current_millis, const BMSCoreData_s &bms_core_data, size_t max_consecutive_invalid_packet_count, float em_current, size_t num_of_voltage_cells, bool voltage_is_fresh = false); /** * Calculate Cell Balancing values @@ -130,7 +135,7 @@ class ACUController /** * @return state of charge - float from 0.0 to 1.0, representing a percentage from 0 to 100% */ - float get_state_of_charge(float em_current, uint32_t delta_time_ms); + float get_state_of_charge(float em_current, uint32_t delta_time_ms, volt min_cell_voltage, time_ms current_millis, bool voltage_is_fresh); ACUControllerData_s get_status() const { return _acu_state; }; @@ -147,6 +152,10 @@ class ACUController private: + /** + * @brief Closest index that will represent the SoC of the minimum voltage on the cells + */ + float _get_soc_from_voltage(volt min_cell_voltage); /** * @pre data has been gathered @@ -157,7 +166,7 @@ class ACUController /** * @brief Update the BMS status (bms_ok) based on the time since the last fault not present */ - bool _check_bms_ok(time_ms current_millis); + bool _is_bms_ok(time_ms current_millis); /** * @pre voltage data has been gathered * @return boolean, true if there exists at least 1 voltage fault @@ -184,14 +193,33 @@ class ACUController */ ACUControllerData_s _acu_state = {}; - static constexpr uint32_t _bms_not_ok_hold_time_ms = 1000; + static constexpr uint32_t _bms_not_ok_hold_time_ms = 2000; static constexpr uint32_t _ms_to_hours = 3600000; + static constexpr float _ms_to_seconds = 1000.0f; + /** * @brief ACU Controller Parameters holder */ const ACUControllerParameters_s _acu_parameters = {}; + + + /** + * @brief Extended Kalman Filter for SoC tracking using current and voltage measurements from pack + * This will replace the coulomb counting method for SoC tracking and will be used to track SoC of the pack + */ + SOCKalmanFilter _soc_ekf; + bool _ekf_initialized = false; + + static constexpr size_t NUM_CELLS = 126; + + /** + * @brief Minimum current and voltage thresholds for the car to be considered stabilized + * + */ + static constexpr float STABILIZED_CURRENT_THRESH = 0.5f; // Absolute value threshold + static constexpr uint32_t MIN_STABILIZED_CURRENT_DURATION_MS = 1800000; // 30 minutes in milliseconds }; using ACUControllerInstance = etl::singleton; diff --git a/lib/systems/include/BMSFaultDataManager.h b/lib/systems/include/BMSFaultDataManager.h index a9a82bd7..69710ae7 100644 --- a/lib/systems/include/BMSFaultDataManager.h +++ b/lib/systems/include/BMSFaultDataManager.h @@ -25,7 +25,7 @@ class BMSFaultDataManager std::array chip_invalid_cmd_counts{}; }; - void update_from_valid_packets( const std::array& valid_read_packets); + void update_from_valid_packets(const std::array& valid_read_packets, const ReadGroup_e read_group); const BMSFaultData_s& get_fault_data() const; diff --git a/lib/systems/include/BMSFaultDataManager.tpp b/lib/systems/include/BMSFaultDataManager.tpp index f65e41c6..46c9f3e2 100644 --- a/lib/systems/include/BMSFaultDataManager.tpp +++ b/lib/systems/include/BMSFaultDataManager.tpp @@ -3,7 +3,8 @@ template void BMSFaultDataManager::update_from_valid_packets( - const std::array& valid_read_packets) + const std::array& valid_read_packets, + const ReadGroup_e read_group) { size_t num_total_bms_packets = num_chips * sizeof(BMSFaultCountData_s); std::array chip_max_invalid_cmd_counts = {}; @@ -12,21 +13,60 @@ void BMSFaultDataManager::update_from_valid_packets( for (size_t chip = 0; chip < valid_read_packets.size(); chip++) { - _bms_fault_data.chip_invalid_cmd_counts[chip].invalid_cell_1_to_3_count = (!valid_read_packets[chip].valid_read_cells_1_to_3) ? _bms_fault_data.chip_invalid_cmd_counts[chip].invalid_cell_1_to_3_count+1 : 0; - _bms_fault_data.chip_invalid_cmd_counts[chip].invalid_cell_4_to_6_count = (!valid_read_packets[chip].valid_read_cells_4_to_6) ? _bms_fault_data.chip_invalid_cmd_counts[chip].invalid_cell_4_to_6_count+1 : 0; - _bms_fault_data.chip_invalid_cmd_counts[chip].invalid_cell_7_to_9_count = (!valid_read_packets[chip].valid_read_cells_7_to_9) ? _bms_fault_data.chip_invalid_cmd_counts[chip].invalid_cell_7_to_9_count+1 : 0; - _bms_fault_data.chip_invalid_cmd_counts[chip].invalid_cell_10_to_12_count = (!valid_read_packets[chip].valid_read_cells_10_to_12) ? _bms_fault_data.chip_invalid_cmd_counts[chip].invalid_cell_10_to_12_count+1 : 0; - _bms_fault_data.chip_invalid_cmd_counts[chip].invalid_gpio_1_to_3_count = (!valid_read_packets[chip].valid_read_gpios_1_to_3) ? _bms_fault_data.chip_invalid_cmd_counts[chip].invalid_gpio_1_to_3_count+1 : 0; - _bms_fault_data.chip_invalid_cmd_counts[chip].invalid_gpio_4_to_6_count = (!valid_read_packets[chip].valid_read_gpios_4_to_6) ? _bms_fault_data.chip_invalid_cmd_counts[chip].invalid_gpio_4_to_6_count+1 : 0; - num_valid_packets += static_cast(valid_read_packets[chip].valid_read_cells_1_to_3 + valid_read_packets[chip].valid_read_cells_4_to_6 + valid_read_packets[chip].valid_read_cells_7_to_9 + - valid_read_packets[chip].valid_read_cells_10_to_12 + valid_read_packets[chip].valid_read_gpios_1_to_3 + valid_read_packets[chip].valid_read_gpios_4_to_6); + switch (read_group) + { + case ReadGroup_e::CV_GROUP_A: + { + _bms_fault_data.chip_invalid_cmd_counts[chip].invalid_cell_1_to_3_count = (!valid_read_packets[chip].valid_read_cells_1_to_3) ? _bms_fault_data.chip_invalid_cmd_counts[chip].invalid_cell_1_to_3_count+1 : 0; + break; + } + case ReadGroup_e::CV_GROUP_B: + { + _bms_fault_data.chip_invalid_cmd_counts[chip].invalid_cell_4_to_6_count = (!valid_read_packets[chip].valid_read_cells_4_to_6) ? _bms_fault_data.chip_invalid_cmd_counts[chip].invalid_cell_4_to_6_count+1 : 0; + break; + } + case ReadGroup_e::CV_GROUP_C: + { + _bms_fault_data.chip_invalid_cmd_counts[chip].invalid_cell_7_to_9_count = (!valid_read_packets[chip].valid_read_cells_7_to_9) ? _bms_fault_data.chip_invalid_cmd_counts[chip].invalid_cell_7_to_9_count+1 : 0; + break; + } + case ReadGroup_e::CV_GROUP_D: + { + _bms_fault_data.chip_invalid_cmd_counts[chip].invalid_cell_10_to_12_count = (!valid_read_packets[chip].valid_read_cells_10_to_12) ? _bms_fault_data.chip_invalid_cmd_counts[chip].invalid_cell_10_to_12_count+1 : 0; + break; + } + case ReadGroup_e::AUX_GROUP_A: + { + _bms_fault_data.chip_invalid_cmd_counts[chip].invalid_gpio_1_to_3_count = (!valid_read_packets[chip].valid_read_gpios_1_to_3) ? _bms_fault_data.chip_invalid_cmd_counts[chip].invalid_gpio_1_to_3_count+1 : 0; + break; + } + case ReadGroup_e::AUX_GROUP_B: + { + _bms_fault_data.chip_invalid_cmd_counts[chip].invalid_gpio_4_to_6_count = (!valid_read_packets[chip].valid_read_gpios_4_to_6) ? _bms_fault_data.chip_invalid_cmd_counts[chip].invalid_gpio_4_to_6_count+1 : 0; + break; + } + default: + { + break; + } + } + num_valid_packets += static_cast( + (_bms_fault_data.chip_invalid_cmd_counts[chip].invalid_cell_1_to_3_count == 0) + + (_bms_fault_data.chip_invalid_cmd_counts[chip].invalid_cell_4_to_6_count == 0) + + (_bms_fault_data.chip_invalid_cmd_counts[chip].invalid_cell_7_to_9_count == 0) + + (_bms_fault_data.chip_invalid_cmd_counts[chip].invalid_cell_10_to_12_count == 0) + + (_bms_fault_data.chip_invalid_cmd_counts[chip].invalid_gpio_1_to_3_count == 0) + + (_bms_fault_data.chip_invalid_cmd_counts[chip].invalid_gpio_4_to_6_count == 0) + ); - temp = {_bms_fault_data.chip_invalid_cmd_counts[chip].invalid_cell_1_to_3_count, + temp = { + _bms_fault_data.chip_invalid_cmd_counts[chip].invalid_cell_1_to_3_count, _bms_fault_data.chip_invalid_cmd_counts[chip].invalid_cell_4_to_6_count, _bms_fault_data.chip_invalid_cmd_counts[chip].invalid_cell_7_to_9_count, _bms_fault_data.chip_invalid_cmd_counts[chip].invalid_cell_10_to_12_count, _bms_fault_data.chip_invalid_cmd_counts[chip].invalid_gpio_1_to_3_count, - _bms_fault_data.chip_invalid_cmd_counts[chip].invalid_gpio_4_to_6_count}; + _bms_fault_data.chip_invalid_cmd_counts[chip].invalid_gpio_4_to_6_count + }; chip_max_invalid_cmd_counts[chip] = *etl::max_element(temp.begin(), temp.end()); _bms_fault_data.consecutive_invalid_packet_counts[chip] = chip_max_invalid_cmd_counts[chip]; } diff --git a/lib/systems/include/SOCKalmanFilter.h b/lib/systems/include/SOCKalmanFilter.h new file mode 100644 index 00000000..489411c0 --- /dev/null +++ b/lib/systems/include/SOCKalmanFilter.h @@ -0,0 +1,144 @@ +#ifndef SOC_KALMAN_FILTER_H +#define SOC_KALMAN_FILTER_H + +#include +#include +#include +#include +#include +#include "etl/singleton.h" +#include "SharedFirmwareTypes.h" +#include "shared_types.h" + +namespace soc_ekf_constants +{ + constexpr const float CAPACITY_AS = 48600.0f; // Pack capacity in Amp-seconds (13.5 Ah * 3600 s/h) + constexpr const float R0 = 0.00195238095f; // Internal series resistance (instantaneous voltage drop) + constexpr const float R1 = 0.00039047619f; // Polarization resistance to see slow voltage response - fix + constexpr const float TIME_CONSTANT = 20.0f; // time constant value - fix + constexpr const float C1 = TIME_CONSTANT / R1; // Polarization capacitance + + // EKF tuning parameters (update these to tune the EKF to track SoC better) + constexpr const float Q_SOC = 1e-5f; // process noise for SoC - fix + constexpr const float Q_V1 = 1e-6f; // process noise for V1 - fix + constexpr const float R_V1 = 0.1f; // measurement noise for V1 - fix + + constexpr const float MIN_SOC = 0.0f; + constexpr const float MAX_SOC = 1.0f; + constexpr const float MAX_V1_MAGNITUDE = 0.5f; + + // Initial/reset values for state and covariance + constexpr const float INITIAL_SOC = 0.5f; + constexpr const float INITIAL_V1 = 0.0f; + constexpr const float P_SOC_INITIAL = 0.01f; // Initial variance for SoC + constexpr const float P_V1_INITIAL = 0.1f; // Initial variance for V1 + constexpr const float P_CROSS_INITIAL = 0.0f; // Initial cross-covariance + + // Numerical differentiation step size + constexpr const float DOCV_DSOC_STEP = 0.01f; + // Minimum slope for dOCV/dSoC to prevent numerical instability + constexpr const float MIN_DOCV_DSOC_SLOPE = 0.1f; + constexpr const float MIN_INNOVATION_COV_THRESH = 1e-6f; + constexpr const float DIVIDER_CROSS_AVG = 2.0f; + + constexpr const float P_SOC_AFTER_REST = 0.001f; + constexpr const float P_V1_AFTER_REST = 0.001f; +} + +struct EKFState_s +{ + float soc; // State of charge state varying from 0.0 to 1.0 + float v1; // Polarization voltage to track lag in voltage from current change +}; + +class SOCKalmanFilter +{ +public: + SOCKalmanFilter(); + + /** + * @brief Set the EKF state appropriately based on the initial voltage + * This is called when the ACU starts and is the first reading done on the EKF + * @param initial_voltage // this is the voltage coming in from the cells on the pack (we take the minimum voltage for a per cell EKF) + */ + void init(float initial_voltage); + + /** + * @brief Used to update the state of our EKF at specified time intervals + * @param current // current going across the pack in amps + * @param voltage // minimum cell voltage across the pack + * @param dt // time elapsed since last update in seconds + * @param voltage_is_fresh // true if the voltage data is fresh (only happens once per good cycle) + */ + EKFState_s update(float current, float voltage, float dt, bool voltage_is_fresh); + + /** + * @brief Get the soc object + * @return float state of charge + */ + float get_soc() const { + return _state.soc; + } + + /** + * @brief Get current state + * @return Complete state vector + */ + EKFState_s get_state() const { + return _state; + } + + /** + * @brief Reset SoC estimate + * @param new_soc New SoC value + * @post SoC updated, uncertainty increased + */ + void reset_soc(float new_soc); + + // OCV Lookup Table + // Index 0 = 100% SoC (4.2V approx), Index 100 = 0% SoC (3.585V approx) + // fix tuned + static constexpr float VOLTAGE_LOOKUP_TABLE[101] = { + 4.181, 4.171, 4.161, 4.151, 4.141, 4.132, 4.122, 4.112, 4.102, 4.092, + 4.083, 4.073, 4.064, 4.055, 4.046, 4.037, 4.027, 4.018, 4.009, 4.000, + 3.991, 3.985, 3.980, 3.975, 3.969, 3.964, 3.959, 3.953, 3.948, 3.943, + 3.937, 3.931, 3.925, 3.919, 3.913, 3.907, 3.901, 3.895, 3.890, 3.884, + 3.878, 3.872, 3.867, 3.861, 3.856, 3.850, 3.845, 3.839, 3.834, 3.828, + 3.823, 3.820, 3.817, 3.815, 3.812, 3.809, 3.806, 3.804, 3.801, 3.798, + 3.796, 3.794, 3.793, 3.792, 3.790, 3.789, 3.787, 3.786, 3.785, 3.783, + 3.782, 3.779, 3.776, 3.774, 3.771, 3.768, 3.765, 3.763, 3.760, 3.757, + 3.755, 3.748, 3.742, 3.736, 3.730, 3.724, 3.718, 3.712, 3.706, 3.700, + 3.694, 3.657, 3.619, 3.581, 3.544, 3.506, 3.468, 3.431, 3.393, 3.356, + 3.318 + }; + +private: + /** + * @brief Get the OCV from the SoC estimate using linear interpolation of lookup table + * @param soc State of charge + * @return Open circuit voltage + */ + float _get_ocv_from_soc(float soc) const; + + /** + * @brief Get dOCV/dSoC for Jacobian in the EKF calculation + * @param soc State of charge + * @return Numerical derivative + */ + float _get_docv_dsoc(float soc) const; + + /** + * @brief Clamp state to physical limits + */ + void _clamp_state(); + +private: + EKFState_s _state; // The system state (SoC, V_polarization) + + // Covariance Matrix P (2x2) + // Tracks the uncertainty of our estimate. + // P[0][0] = var(SoC), P[1][1] = var(V1) + float _PMatrix[2][2]; +}; + +#endif \ No newline at end of file diff --git a/lib/systems/src/ACUController.cpp b/lib/systems/src/ACUController.cpp index b53a032f..82c1a211 100644 --- a/lib/systems/src/ACUController.cpp +++ b/lib/systems/src/ACUController.cpp @@ -10,13 +10,14 @@ void ACUController::init(time_ms system_start_time, volt pack_voltage) _acu_state.last_time_pack_uv_fault_not_present = system_start_time; _acu_state.last_time_invalid_packet_present = system_start_time; _acu_state.prev_bms_time_stamp = system_start_time; - _acu_state.SoC = (pack_voltage <= _acu_parameters.pack_specs.pack_min_voltage) ? 0.0f : ((pack_voltage - _acu_parameters.pack_specs.pack_min_voltage) / (_acu_parameters.pack_specs.pack_max_voltage - _acu_parameters.pack_specs.pack_min_voltage)); + _acu_state.SoC = -1; _acu_state.balancing_enabled = false; _acu_state.high_side_contactor_welded = false; _acu_state.low_side_contactor_welded = false; + _acu_state.bms_ok = true; } -ACUControllerData_s ACUController::evaluate_accumulator(time_ms current_millis, const BMSCoreData_s &input_state, size_t max_consecutive_invalid_packet_count, float em_current, size_t num_of_voltage_cells) +ACUControllerData_s ACUController::evaluate_accumulator(time_ms current_millis, const BMSCoreData_s &input_state, size_t max_consecutive_invalid_packet_count, float em_current, size_t num_of_voltage_cells, bool voltage_is_fresh) { // _acu_state.charging_enabled = input_state.charging_enabled; @@ -25,8 +26,9 @@ ACUControllerData_s ACUController::evaluate_accumulator(time_ms current_millis, { // meaning that at least one of the packets is invalid has_invalid_packet = true; } - _acu_state.SoC = get_state_of_charge(em_current, current_millis - _acu_state.prev_bms_time_stamp); - + + volt min_cell_voltage = input_state.min_cell_voltage; + _acu_state.SoC = get_state_of_charge(em_current, current_millis - _acu_state.prev_bms_time_stamp, min_cell_voltage, current_millis, voltage_is_fresh); // Cell balancing calculations bool previously_balancing = _acu_state.balancing_enabled; @@ -95,7 +97,7 @@ ACUControllerData_s ACUController::evaluate_accumulator(time_ms current_millis, _acu_state.has_fault = _check_faults(current_millis); // Determine if bms is ok - _acu_state.bms_ok = _check_bms_ok(current_millis); + _acu_state.bms_ok = _is_bms_ok(current_millis); return _acu_state; } @@ -117,34 +119,112 @@ void ACUController::calculate_cell_balance_statuses(bool* output, const volt* vo } } +float ACUController::_get_soc_from_voltage(volt min_cell_voltage) +{ + static constexpr size_t table_size = 101; + + if (min_cell_voltage >= SOCKalmanFilter::VOLTAGE_LOOKUP_TABLE[0]) + { + return 1.0f; + } + if (min_cell_voltage <= SOCKalmanFilter::VOLTAGE_LOOKUP_TABLE[table_size - 1]) + { + return 0.0f; + } -float ACUController::get_state_of_charge(float em_current, uint32_t delta_time_ms) + for (size_t i = 0; i < table_size - 1; i++) + { + if (min_cell_voltage <= SOCKalmanFilter::VOLTAGE_LOOKUP_TABLE[i] && min_cell_voltage > SOCKalmanFilter::VOLTAGE_LOOKUP_TABLE[i + 1]) //NOLINT + { + float v_high = SOCKalmanFilter::VOLTAGE_LOOKUP_TABLE[i]; //NOLINT + float v_low = SOCKalmanFilter::VOLTAGE_LOOKUP_TABLE[i + 1]; //NOLINT + float soc_high = (float)(table_size - 1 - i) / (table_size - 1); + float soc_low = (float)(table_size - 1 - (i + 1)) / (table_size - 1); + + return soc_low + (min_cell_voltage - v_low) / (v_high - v_low) * (soc_high - soc_low); + } + } + + return 0.0f; +} + +float ACUController::get_state_of_charge(float em_current, uint32_t delta_time_ms, volt min_cell_voltage, time_ms current_millis, bool voltage_is_fresh) { - float delta_ah = (em_current) * (static_cast(delta_time_ms) / _ms_to_hours); // amp hours - _acu_state.SoC += delta_ah / _acu_parameters.pack_specs.pack_nominal_capacity; // should be -= but EM inverted - if (_acu_state.SoC < 0.0) - _acu_state.SoC = 0; - if (_acu_state.SoC > 1.0) - _acu_state.SoC = 1; + if (!_ekf_initialized) + { + if (!voltage_is_fresh) + { + return _acu_state.SoC; + } + if (min_cell_voltage < acu_controller_default_parameters::MIN_CELL_VOLTAGE_FOR_SOC) + { + return 0.0f; + } + else + { + _soc_ekf.init(min_cell_voltage); + _ekf_initialized = true; + _acu_state.SoC = _soc_ekf.get_soc(); + return _acu_state.SoC; + } + } + + float dt = static_cast(delta_time_ms) / _ms_to_seconds; // in seconds + + // we will use coulomb counting for the normal implementation of getting state of charge + // whenever the car has been at rest (em voltage and em current at 0) for 30 mins, then we can correct the SoC to the voltage look up table value + // we will reset the soc with the voltage look up value + // we want to then start coulomb counting from this point, we also want to restart a 30 min timer, so we can set the start time to now + + bool is_stabilized = (fabs(em_current) <= STABILIZED_CURRENT_THRESH); + if (is_stabilized) + { + if (_acu_state.first_zero_current_time_stamp == 0) + { + _acu_state.first_zero_current_time_stamp = current_millis; + } + // we have another 0 current, so we need to see if we have rested for long enough + if ((current_millis - _acu_state.first_zero_current_time_stamp) >= MIN_STABILIZED_CURRENT_DURATION_MS) + { + if (voltage_is_fresh) + { + _acu_state.SoC = _get_soc_from_voltage(min_cell_voltage); + _soc_ekf.reset_soc(_acu_state.SoC); + + return _acu_state.SoC; + } + } + } + else + { + _acu_state.first_zero_current_time_stamp = 0; + } + + EKFState_s ekf_state = _soc_ekf.update(em_current, min_cell_voltage, dt, voltage_is_fresh); + _acu_state.SoC = ekf_state.soc; + return _acu_state.SoC; } -bool ACUController::_check_bms_ok(time_ms current_millis) +bool ACUController::_is_bms_ok(time_ms current_millis) { - if (_acu_state.has_fault) { - _acu_state.bms_ok = !_acu_state.has_fault; + if (_acu_state.has_fault) + { _acu_state.last_bms_not_ok_eval = current_millis; - } else if (!_acu_state.bms_ok && (current_millis - _acu_state.last_bms_not_ok_eval > _bms_not_ok_hold_time_ms)) { - _acu_state.bms_ok = true; + return false; + } + else if (!_acu_state.bms_ok && (current_millis - _acu_state.last_bms_not_ok_eval > _bms_not_ok_hold_time_ms)) + { + return true; } + return _acu_state.bms_ok; } bool ACUController::_check_faults(time_ms current_millis) { - return _check_voltage_faults(current_millis) || _check_temperature_faults(current_millis) || _check_invalid_packet_faults(current_millis); } diff --git a/lib/systems/src/SOCKalmanFilter.cpp b/lib/systems/src/SOCKalmanFilter.cpp new file mode 100644 index 00000000..6815ca80 --- /dev/null +++ b/lib/systems/src/SOCKalmanFilter.cpp @@ -0,0 +1,204 @@ +#include "SOCKalmanFilter.h" +#include + +SOCKalmanFilter::SOCKalmanFilter() + : _state{soc_ekf_constants::INITIAL_SOC, soc_ekf_constants::INITIAL_V1}, + _PMatrix{{soc_ekf_constants::P_SOC_INITIAL, soc_ekf_constants::P_CROSS_INITIAL}, + {soc_ekf_constants::P_CROSS_INITIAL, soc_ekf_constants::P_V1_INITIAL}} { +} + +void SOCKalmanFilter::init(float initial_voltage) { + static constexpr size_t table_size = 101; + + if (initial_voltage >= VOLTAGE_LOOKUP_TABLE[0]) { + _state.soc = 1.0f; + } else if (initial_voltage <= VOLTAGE_LOOKUP_TABLE[table_size - 1]) { + _state.soc = 0.0f; + } else { + for (size_t i = 0; i < table_size - 1; i++) { + if (initial_voltage <= VOLTAGE_LOOKUP_TABLE[i] && initial_voltage > VOLTAGE_LOOKUP_TABLE[i + 1]) { //NOLINT + float v_high = VOLTAGE_LOOKUP_TABLE[i]; //NOLINT + float v_low = VOLTAGE_LOOKUP_TABLE[i + 1]; //NOLINT + float soc_high = (float)(table_size - 1 - i) / (table_size - 1); + float soc_low = (float)(table_size - 1 - (i + 1)) / (table_size - 1); + + _state.soc = soc_low + (initial_voltage - v_low) / (v_high - v_low) * (soc_high - soc_low); + break; + } + } + + } + + _state.v1 = soc_ekf_constants::INITIAL_V1; + + _PMatrix[0][0] = soc_ekf_constants::P_SOC_INITIAL; + _PMatrix[0][1] = soc_ekf_constants::P_CROSS_INITIAL; + _PMatrix[1][0] = soc_ekf_constants::P_CROSS_INITIAL; + _PMatrix[1][1] = soc_ekf_constants::P_V1_INITIAL; +} + +EKFState_s SOCKalmanFilter::update(float current, float voltage, float dt, bool voltage_is_fresh) { + // If the time delta is too small, then we don't update the state + if (dt <= 0.0f) { + return _state; + } + + // Prediction + float soc_rate = -current / soc_ekf_constants::CAPACITY_AS; + _state.soc += soc_rate * dt; + + float decay_factor = expf(-dt / soc_ekf_constants::TIME_CONSTANT); + _state.v1 = _state.v1 * decay_factor + current * soc_ekf_constants::R1 * (1.0f - decay_factor); + + _clamp_state(); + + // Predict covariance (Formula: F * P * F^T + Q) + // F is the state transition matrix that shows how the state (SoC, V1) changes over time + // F = [1, 0; 0, decay_factor] - SoC stays constant and V1 decays exponentially + float F11 = decay_factor; + + // P is the covariance matrix that tracks confidence in the SoC and V1 estimates + float FP00 = _PMatrix[0][0]; + float FP01 = _PMatrix[0][1]; + float FP10 = F11 * _PMatrix[1][0]; + float FP11 = F11 * _PMatrix[1][1]; + + // Q is the process noise covariance that accounts for coulomb counting uncertainty and model mismatch + _PMatrix[0][0] = FP00 + soc_ekf_constants::Q_SOC; + _PMatrix[0][1] = FP01 * F11; + _PMatrix[1][0] = FP10; + _PMatrix[1][1] = FP11 * F11 + soc_ekf_constants::Q_V1; + + if (!voltage_is_fresh) { + return _state; + } + + // Update - corrects state estimation based on voltage measurement + float ocv = _get_ocv_from_soc(_state.soc); + float voltage_pred = ocv - current * soc_ekf_constants::R0 - _state.v1; + float innovation = voltage - voltage_pred; + + // H is the observation matrix that relates the state to measured voltage + // H = [dOCV/dSoC, -1] + float H0 = _get_docv_dsoc(_state.soc); + float H1 = -1.0f; + + float HP0 = H0 * _PMatrix[0][0] + H1 * _PMatrix[1][0]; + float HP1 = H0 * _PMatrix[0][1] + H1 * _PMatrix[1][1]; + // S is the innovation covariance that represents uncertainty in the voltage prediction + // R_V1 is the measurement sensor noise in the voltage sensor + float S = HP0 * H0 + HP1 * H1 + soc_ekf_constants::R_V1; + + // If the innovation covariance is too small, then we don't update the state + if (S <= soc_ekf_constants::MIN_INNOVATION_COV_THRESH) { + return _state; + } + + // K is the Kalman gain that shows how much we should update the state to optimally blend predict vs measurement + float K0 = (_PMatrix[0][0] * H0 + _PMatrix[0][1] * H1) / S; + float K1 = (_PMatrix[1][0] * H0 + _PMatrix[1][1] * H1) / S; + + // Apply correction: adjust SoC and V1 based on voltage error + _state.soc += K0 * innovation; + _state.v1 += K1 * innovation; + _clamp_state(); + + // (I - K*H): reduces uncertainty after incorporating measurement + float I_KH_00 = 1.0f - K0 * H0; + float I_KH_01 = -K0 * H1; + float I_KH_10 = -K1 * H0; + float I_KH_11 = 1.0f - K1 * H1; + + // Formula: P = (I-K*H) * P * (I-K*H)^T + K*R*K^T - This is the Joseph Form which is much more stable + // Calculate temp = (I - K*H) * P + float temp_00 = I_KH_00 * _PMatrix[0][0] + I_KH_01 * _PMatrix[1][0]; + float temp_01 = I_KH_00 * _PMatrix[0][1] + I_KH_01 * _PMatrix[1][1]; + float temp_10 = I_KH_10 * _PMatrix[0][0] + I_KH_11 * _PMatrix[1][0]; + float temp_11 = I_KH_10 * _PMatrix[0][1] + I_KH_11 * _PMatrix[1][1]; + + // Calculate term1 = temp * (I - K*H)^T + float term1_00 = temp_00 * I_KH_00 + temp_01 * I_KH_01; + float term1_01 = temp_00 * I_KH_10 + temp_01 * I_KH_11; + float term1_10 = temp_10 * I_KH_00 + temp_11 * I_KH_01; + float term1_11 = temp_10 * I_KH_10 + temp_11 * I_KH_11; + + // Calculate term2 = K * R * K^T (Note: R is just soc_ekf_constants::R_V1) + float term2_00 = K0 * soc_ekf_constants::R_V1 * K0; + float term2_01 = K0 * soc_ekf_constants::R_V1 * K1; + float term2_10 = K1 * soc_ekf_constants::R_V1 * K0; + float term2_11 = K1 * soc_ekf_constants::R_V1 * K1; + + //Add term1 and term2 to get the new P matrix + _PMatrix[0][0] = term1_00 + term2_00; + _PMatrix[0][1] = term1_01 + term2_01; + _PMatrix[1][0] = term1_10 + term2_10; + _PMatrix[1][1] = term1_11 + term2_11; + + + float p_cross_avg = (_PMatrix[0][1] + _PMatrix[1][0]) / soc_ekf_constants::DIVIDER_CROSS_AVG; + _PMatrix[0][1] = p_cross_avg; + _PMatrix[1][0] = p_cross_avg; + + return _state; +} + +void SOCKalmanFilter::_clamp_state() { + if (_state.soc < soc_ekf_constants::MIN_SOC) { + _state.soc = soc_ekf_constants::MIN_SOC; + } + if (_state.soc > soc_ekf_constants::MAX_SOC) { + _state.soc = soc_ekf_constants::MAX_SOC; + } + if (_state.v1 < -soc_ekf_constants::MAX_V1_MAGNITUDE) { + _state.v1 = -soc_ekf_constants::MAX_V1_MAGNITUDE; + } + if (_state.v1 > soc_ekf_constants::MAX_V1_MAGNITUDE) { + _state.v1 = soc_ekf_constants::MAX_V1_MAGNITUDE; + } +} + +float SOCKalmanFilter::_get_ocv_from_soc(float soc) const { + static constexpr size_t table_size = 101; + + if (soc >= 1.0f) { + return VOLTAGE_LOOKUP_TABLE[0]; + } + if (soc <= 0.0f) { + return VOLTAGE_LOOKUP_TABLE[table_size - 1]; + } + + float index_float = (1.0f - soc) * 100.0f; + size_t idx_low = (size_t)index_float; + size_t idx_high = idx_low + 1; + + if (idx_high >= table_size) { + idx_high = table_size - 1; + idx_low = idx_high - 1; + } + + float fraction = index_float - (float)idx_low; + return VOLTAGE_LOOKUP_TABLE[idx_low] + fraction * (VOLTAGE_LOOKUP_TABLE[idx_high] - VOLTAGE_LOOKUP_TABLE[idx_low]); //NOLINT +} + +float SOCKalmanFilter::_get_docv_dsoc(float soc) const { + float soc_plus = fminf(soc + soc_ekf_constants::DOCV_DSOC_STEP, soc_ekf_constants::MAX_SOC); + float soc_minus = fmaxf(soc - soc_ekf_constants::DOCV_DSOC_STEP, soc_ekf_constants::MIN_SOC); + float ocv_plus = _get_ocv_from_soc(soc_plus); + float ocv_minus = _get_ocv_from_soc(soc_minus); + float slope = (ocv_plus - ocv_minus) / (soc_plus - soc_minus); + + if (slope < soc_ekf_constants::MIN_DOCV_DSOC_SLOPE) { + slope = soc_ekf_constants::MIN_DOCV_DSOC_SLOPE; + } + + return slope; +} + +void SOCKalmanFilter::reset_soc(float new_soc) { + _state.soc = fmaxf(soc_ekf_constants::MIN_SOC, fminf(soc_ekf_constants::MAX_SOC, new_soc)); + _state.v1 = soc_ekf_constants::INITIAL_V1; + _PMatrix[0][0] = soc_ekf_constants::P_SOC_AFTER_REST; + _PMatrix[0][1] = soc_ekf_constants::P_CROSS_INITIAL; + _PMatrix[1][0] = soc_ekf_constants::P_CROSS_INITIAL; + _PMatrix[1][1] = soc_ekf_constants::P_V1_AFTER_REST; +} \ No newline at end of file diff --git a/src/ACU_InterfaceTasks.cpp b/src/ACU_InterfaceTasks.cpp index b5cff49e..a452ff70 100644 --- a/src/ACU_InterfaceTasks.cpp +++ b/src/ACU_InterfaceTasks.cpp @@ -81,31 +81,6 @@ void initialize_all_interfaces() ACUInterfaces::SW_NOT_OK_PIN}); WatchdogInstance::instance().init(); - /* Fault Latch Manager */ - FaultLatchManagerInstance::create(); - FaultLatchManagerInstance::instance().set_shdn_out_latched(true); // Start shdn out latch cleared - - /* BMS Driver */ - BMSDriverInstance_t::create(ACUConstants::CS, ACUConstants::CS_PER_CHIP, ACUConstants::ADDR); - BMSDriverInstance_t::instance().init(); - /* Get Initial Pack Voltage for SoC and SoH Approximations */ - BMSDriverInstance_t::instance().read_data(); - - BMSFaultDataManagerInstance_t::create(); - // BMSFaultDataManagerInstance_t::instance().update_from_valid_packets(data.valid_read_packets); - /* Ethernet Interface */ - ACUEthernetInterfaceInstance::create(); - ACUEthernetInterfaceInstance::instance().init_ethernet_device(); - - /* CCU Interface */ - CCUInterfaceInstance::create(sys_time::hal_millis()); - - /* VCR Interface */ - VCRInterfaceInstance::create(sys_time::hal_millis()); - - /* EM Interface */ - EMInterfaceInstance::create(sys_time::hal_millis()); - /* ADC Interface */ ADCInterfaceInstance::create( ADCPinout_s {ACUInterfaces::IMD_OK_PIN, ACUInterfaces::PRECHARGE_PIN, @@ -154,15 +129,40 @@ void initialize_all_interfaces() ACUInterfaces::SHUNT_CURRENT_N_OFFSET, ACUInterfaces::TS_OUT_FILTERED_OFFSET, ACUInterfaces::PACK_OUT_FILTERED_OFFSET}, - MAX114XChannels_s {CHANNEL_TYPE_e::INV_DIFFERENTIAL, + MAX114XChannels_s {CHANNEL_TYPE_e::NOT_USED, CHANNEL_TYPE_e::SINGLE, - CHANNEL_TYPE_e::DIFFERENTIAL, - CHANNEL_TYPE_e::SINGLE}, + CHANNEL_TYPE_e::NOT_USED, + CHANNEL_TYPE_e::NOT_USED}, ACUInterfaces::ADC0_SPEED, ACUInterfaces::BIT_RESOLUTION ); ADCInterfaceInstance::instance().init(sys_time::hal_millis()); + /* Fault Latch Manager */ + FaultLatchManagerInstance::create(); + FaultLatchManagerInstance::instance().set_shdn_out_latched(true); // Start shdn out latch cleared + + /* BMS Driver */ + BMSDriverInstance_t::create(ACUConstants::CS, ACUConstants::CS_PER_CHIP, ACUConstants::ADDR); + BMSDriverInstance_t::instance().init(); + /* Get Initial Pack Voltage for SoC and SoH Approximations */ + BMSDriverInstance_t::instance().read_data(); + + BMSFaultDataManagerInstance_t::create(); + + /* Ethernet Interface */ + ACUEthernetInterfaceInstance::create(); + ACUEthernetInterfaceInstance::instance().init_ethernet_device(); + + /* CCU Interface */ + CCUInterfaceInstance::create(sys_time::hal_millis()); + + /* VCR Interface */ + VCRInterfaceInstance::create(sys_time::hal_millis()); + + /* EM Interface */ + EMInterfaceInstance::create(sys_time::hal_millis()); + /* CAN Interfaces Construct */ CANInterfacesInstance::create(CCUInterfaceInstance::instance(), EMInterfaceInstance::instance()); } @@ -176,15 +176,18 @@ HT_TASK::TaskResponse run_kick_watchdog(const unsigned long &sysMicros, const HT HT_TASK::TaskResponse sample_bms_data(const unsigned long &sysMicros, const HT_TASK::TaskInfo &taskInfo) { auto start = sys_time::hal_micros(); + // Serial.print("PREVIOUS SPI STATE: "); Serial.println(BMSDriverInstance_t::instance().get_spi_state_name()); BMSDriverInstance_t::instance().read_data(); auto data = BMSDriverInstance_t::instance().get_bms_data(); - BMSFaultDataManagerInstance_t::instance().update_from_valid_packets(data.valid_read_packets); - print_bms_data(data); + BMSFaultDataManagerInstance_t::instance().update_from_valid_packets(data.valid_read_packets, BMSDriverInstance_t::instance().get_current_read_group()); + // Serial.print("CURRENT READ GROUP: "); Serial.println(BMSDriverInstance_t::instance().get_current_read_group_name()); + // Serial.print("CURRENT SPI STATE: "); Serial.println(BMSDriverInstance_t::instance().get_spi_state_name()); + // print_bms_data(data); - auto end = sys_time::hal_micros(); - auto diff = end - start; - - Serial.println(diff); + // Serial.println(); + // auto end = sys_time::hal_micros(); + // auto diff = end - start; + // Serial.println(diff); return HT_TASK::TaskResponse::YIELD; } @@ -268,6 +271,7 @@ HT_TASK::TaskResponse enqueue_ACU_core_CAN_data(const unsigned long& sysMicros, CCUInterfaceInstance::instance().set_ACU_data(data); CCUInterfaceInstance::instance().handle_enqueue_acu_status_CAN_message(); CCUInterfaceInstance::instance().handle_enqueue_acu_core_voltages_CAN_message(); + CCUInterfaceInstance::instance().handle_enqueue_acu_SoC_CAN_message(); return HT_TASK::TaskResponse::YIELD; } @@ -307,24 +311,24 @@ HT_TASK::TaskResponse idle_sample_interfaces(const unsigned long& sysMicros, con template void print_bms_data(bms_data data) { - // Serial.print("Total Voltage: "); - // Serial.print(data.total_voltage, 4); - // Serial.println("V"); - - // Serial.print("Minimum Voltage: "); - // Serial.print(data.min_cell_voltage, 4); - // Serial.print("V\tLocation of Minimum Voltage: "); - // Serial.println(data.min_cell_voltage_id); - - // Serial.print("Maximum Voltage: "); - // Serial.print(data.max_cell_voltage, 4); - // Serial.print("V\tLocation of Maximum Voltage: "); - // Serial.println(data.max_cell_voltage_id); - - // Serial.print("Average Voltage: "); - // Serial.print(data.total_voltage / ACUConstants::NUM_CELLS, 4); - // Serial.println("V"); - // Serial.println(); + Serial.print("Total Voltage: "); + Serial.print(data.total_voltage, 4); + Serial.println("V"); + + Serial.print("Minimum Voltage: "); + Serial.print(data.min_cell_voltage, 4); + Serial.print("V\tLocation of Minimum Voltage: "); + Serial.println(data.min_cell_voltage_id); + + Serial.print("Maximum Voltage: "); + Serial.print(data.max_cell_voltage, 4); + Serial.print("V\tLocation of Maximum Voltage: "); + Serial.println(data.max_cell_voltage_id); + + Serial.print("Average Voltage: "); + Serial.print(data.total_voltage / ACUConstants::NUM_CELLS, 4); + Serial.println("V"); + Serial.println(); // size_t chip_index = 1; // for (auto chip_voltages : data.voltages) @@ -384,9 +388,9 @@ void print_bms_data(bms_data data) // } // Serial.println(); - Serial.print("Number of Global Faults: "); + // Serial.print("Number of Global Faults: "); auto faults = BMSFaultDataManagerInstance_t::instance().get_fault_data(); - Serial.println(faults.max_consecutive_invalid_packet_count); + // Serial.println(faults.max_consecutive_invalid_packet_count); Serial.print("Valid Packet Rate: "); Serial.println(faults.valid_packet_rate); @@ -394,31 +398,19 @@ void print_bms_data(bms_data data) for (size_t c = 0; c < ACUConstants::NUM_CHIPS; c++) { ValidPacketData_s v = data.valid_read_packets[c]; - Serial.print("CHIP #"); Serial.print(c); Serial.print(": "); + Serial.print("CHIP #"); Serial.print(c); Serial.print(":\t"); Serial.print(v.valid_read_cells_1_to_3); Serial.print(" "); Serial.print(v.valid_read_cells_4_to_6); Serial.print(" "); Serial.print(v.valid_read_cells_7_to_9); Serial.print(" "); Serial.print(v.valid_read_cells_10_to_12); Serial.print(" "); Serial.print(v.valid_read_gpios_1_to_3); Serial.print(" "); - Serial.print(v.valid_read_gpios_4_to_6); Serial.println(); - } - - Serial.println("Number of Consecutive Faults Per Chip: "); - for (size_t c = 0; c < ACUConstants::NUM_CHIPS; c++) { - Serial.print("CHIP #"); Serial.print(c); Serial.print(": "); - - Serial.print(faults.chip_invalid_cmd_counts[c].invalid_cell_1_to_3_count); - Serial.print(" "); - Serial.print(faults.chip_invalid_cmd_counts[c].invalid_cell_4_to_6_count); - Serial.print(" "); - Serial.print(faults.chip_invalid_cmd_counts[c].invalid_cell_7_to_9_count); - Serial.print(" "); - Serial.print(faults.chip_invalid_cmd_counts[c].invalid_cell_10_to_12_count); - Serial.print(" "); - Serial.print(faults.chip_invalid_cmd_counts[c].invalid_gpio_1_to_3_count); - Serial.print(" "); - Serial.print(faults.chip_invalid_cmd_counts[c].invalid_gpio_4_to_6_count); - Serial.println(); + Serial.print(v.valid_read_gpios_4_to_6); Serial.print("\t"); + Serial.print(faults.chip_invalid_cmd_counts[c].invalid_cell_1_to_3_count); Serial.print(" "); + Serial.print(faults.chip_invalid_cmd_counts[c].invalid_cell_4_to_6_count); Serial.print(" "); + Serial.print(faults.chip_invalid_cmd_counts[c].invalid_cell_7_to_9_count); Serial.print(" "); + Serial.print(faults.chip_invalid_cmd_counts[c].invalid_cell_10_to_12_count); Serial.print(" "); + Serial.print(faults.chip_invalid_cmd_counts[c].invalid_gpio_1_to_3_count); Serial.print(" "); + Serial.print(faults.chip_invalid_cmd_counts[c].invalid_gpio_4_to_6_count); Serial.println(); } Serial.println(); @@ -489,39 +481,39 @@ HT_TASK::TaskResponse debug_print(const unsigned long &sysMicros, const HT_TASK: Serial.println("V"); Serial.println(); - Serial.print("Is charging enabled: "); Serial.print(ACUControllerInstance::instance().get_status().balancing_enabled ? "YES" : "NO"); Serial.println(" Balancing status : "); - for(bool status : check_and_get_balancing_status()) { - Serial.print(status); - Serial.print(" "); - } - Serial.println(); + // Serial.print("Is charging enabled: "); Serial.print(ACUControllerInstance::instance().get_status().balancing_enabled ? "YES" : "NO"); Serial.println(" Balancing status : "); + // for(bool status : check_and_get_balancing_status()) { + // Serial.print(status); + // Serial.print(" "); + // } + // Serial.println(); Serial.print("Number of Global Faults: "); auto faults = BMSFaultDataManagerInstance_t::instance().get_fault_data(); Serial.println(faults.max_consecutive_invalid_packet_count); Serial.print("Valid Packet Rate: "); Serial.println(faults.valid_packet_rate); - Serial.println("Number of Consecutive Faults Per Chip: "); - for (size_t c = 0; c < ACUConstants::NUM_CHIPS; c++) { - Serial.print("CHIP "); - Serial.print(c); - Serial.print(": "); - Serial.print(faults.consecutive_invalid_packet_counts[c]); - Serial.print(" "); + // Serial.println("Number of Consecutive Faults Per Chip: "); + // for (size_t c = 0; c < ACUConstants::NUM_CHIPS; c++) { + // Serial.print("CHIP "); + // Serial.print(c); + // Serial.print(": "); + // Serial.print(faults.consecutive_invalid_packet_counts[c]); + // Serial.print(" "); - Serial.print(faults.chip_invalid_cmd_counts[c].invalid_cell_1_to_3_count); - Serial.print(" "); - Serial.print(faults.chip_invalid_cmd_counts[c].invalid_cell_4_to_6_count); - Serial.print(" "); - Serial.print(faults.chip_invalid_cmd_counts[c].invalid_cell_7_to_9_count); - Serial.print(" "); - Serial.print(faults.chip_invalid_cmd_counts[c].invalid_cell_10_to_12_count); - Serial.print(" "); - Serial.print(faults.chip_invalid_cmd_counts[c].invalid_gpio_1_to_3_count); - Serial.print(" "); - Serial.print(faults.chip_invalid_cmd_counts[c].invalid_gpio_4_to_6_count); - Serial.print("\t"); - Serial.print(" "); - } + // Serial.print(faults.chip_invalid_cmd_counts[c].invalid_cell_1_to_3_count); + // Serial.print(" "); + // Serial.print(faults.chip_invalid_cmd_counts[c].invalid_cell_4_to_6_count); + // Serial.print(" "); + // Serial.print(faults.chip_invalid_cmd_counts[c].invalid_cell_7_to_9_count); + // Serial.print(" "); + // Serial.print(faults.chip_invalid_cmd_counts[c].invalid_cell_10_to_12_count); + // Serial.print(" "); + // Serial.print(faults.chip_invalid_cmd_counts[c].invalid_gpio_1_to_3_count); + // Serial.print(" "); + // Serial.print(faults.chip_invalid_cmd_counts[c].invalid_gpio_4_to_6_count); + // Serial.print("\t"); + // Serial.print(" "); + // } Serial.println(); Serial.println("\nMAX114X Output:"); diff --git a/src/ACU_SystemTasks.cpp b/src/ACU_SystemTasks.cpp index 88591d4d..1a802909 100644 --- a/src/ACU_SystemTasks.cpp +++ b/src/ACU_SystemTasks.cpp @@ -77,8 +77,9 @@ HT_TASK::TaskResponse evaluate_accumulator(const unsigned long &sysMicros, const sys_time::hal_millis(), BMSDriverInstance_t::instance().get_bms_core_data(), BMSFaultDataManagerInstance_t::instance().get_fault_data().max_consecutive_invalid_packet_count, - EMInterfaceInstance::instance().get_latest_data(sys_time::hal_millis()).em_current, - ACUConstants::NUM_CELLS + ADCInterfaceInstance::instance().read_shunt_current(), + ACUConstants::NUM_CELLS, + BMSDriverInstance_t::instance().check_clear_voltage_ready() ); return HT_TASK::TaskResponse::YIELD; } diff --git a/src/main.cpp b/src/main.cpp index 924caa96..bfe8d68b 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -57,7 +57,7 @@ void setup() scheduler.schedule(write_cell_balancing_config_task); scheduler.schedule(send_all_data_ethernet_task); - scheduler.schedule(send_core_data_ethernet_task); // waiting on update on drivebrain + scheduler.schedule(send_core_data_ethernet_task); scheduler.schedule(send_CAN_task); scheduler.schedule(enqueue_CCU_core_CAN_task); diff --git a/test/test_driver.cpp b/test/test_driver.cpp index 4095de18..d38c328a 100644 --- a/test/test_driver.cpp +++ b/test/test_driver.cpp @@ -3,6 +3,7 @@ #include "test_systems/test_acu_controller.h" #include "test_systems/test_acu_state_machine.h" // #include "test_interfaces/test_adc_interface.h" +#include "test_systems/test_soc_kalman.h" int main(int argc, char **argv) { testing::InitGoogleMock(&argc, argv); diff --git a/test/test_systems/test_soc_kalman.h b/test/test_systems/test_soc_kalman.h new file mode 100644 index 00000000..827bca68 --- /dev/null +++ b/test/test_systems/test_soc_kalman.h @@ -0,0 +1,82 @@ +#include "gtest/gtest.h" +#include +#include "SOCKalmanFilter.h" + +float coulomb_count(float initial_soc, float current, float dt) { + return initial_soc - (current * dt / soc_ekf_constants::CAPACITY_AS); +} + +TEST(SOCKalmanFilterTesting, initialization_from_voltage) { + SOCKalmanFilter ekf; + ekf.init(4.181f); + float soc = ekf.get_soc(); + EXPECT_NEAR(soc, 1.0f, 0.01f); + + SOCKalmanFilter ekf2; + ekf2.init(3.823f); + float soc2 = ekf2.get_soc(); + EXPECT_NEAR(soc2, 0.5f, 0.1f); + + SOCKalmanFilter ekf3; + ekf3.init(3.318f); + float soc3 = ekf3.get_soc(); + EXPECT_NEAR(soc3, 0.0f, 0.01f); +} + +TEST(SOCKalmanFilterTesting, v1_starts_at_0) { + SOCKalmanFilter ekf; + ekf.init(3.7f); + EKFState_s state = ekf.get_state(); + EXPECT_NEAR(state.v1, 0.0f, 0.01f); +} + +TEST(SOCKalmanFilterTesting, coulomb_counting_accuracy) { + SOCKalmanFilter ekf; + ekf.init(3.7f); + float start_soc = ekf.get_soc(); + + float current = 10.0f; + float dt = 100.0f; + float start_ocv = 3.7f; + + float ir_drop = current * soc_ekf_constants::R0; + float input_voltage = start_ocv - ir_drop; + + ekf.update(current, input_voltage, dt, true); + + float expected_delta = (current * dt) / soc_ekf_constants::CAPACITY_AS; + + EXPECT_NEAR(ekf.get_soc(), start_soc - expected_delta, 0.02f); +} + +TEST(SOCKalmanFilterTesting, v1_dynamics_time_constant) { + SOCKalmanFilter ekf; + float start_voltage = 3.7f; + ekf.init(start_voltage); + + float current = 100.0f; + float dt = soc_ekf_constants::TIME_CONSTANT; + float max_v1_possible = current * soc_ekf_constants::R1; + float expected_v1 = max_v1_possible * 0.6321f; + + float ir_drop = current * soc_ekf_constants::R0; + float input_voltage = start_voltage - ir_drop - expected_v1; + + + + EKFState_s state = ekf.update(current, input_voltage, dt, true); + + EXPECT_NEAR(state.v1, expected_v1, 0.05f); +} + +TEST(SOCKalmanFilterTesting, safety_clamping_bounds) { + SOCKalmanFilter ekf; + + ekf.init(4.2f); + ekf.update(-100.0f, 4.5f, 3600.0f, true); + EXPECT_LE(ekf.get_soc(), 1.0f); + + ekf.init(3.0f); + ekf.update(100.0f, 2.0f, 3600.0f, true); + EXPECT_GE(ekf.get_soc(), 0.0f); +} \ No newline at end of file