diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_NeoDesign.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_NeoDesign.cpp index ae3be521d6..c122e97159 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_NeoDesign.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_NeoDesign.cpp @@ -19,15 +19,17 @@ AP_BattMonitor_SMBus_NeoDesign::AP_BattMonitor_SMBus_NeoDesign(AP_BattMonitor &m void AP_BattMonitor_SMBus_NeoDesign::timer() { uint16_t data; - uint32_t tnow = AP_HAL::micros(); - // Get the cell count once, it's not likely to change in flight if (_cell_count == 0) { - if (read_word(BATTMONITOR_ND_CELL_COUNT, data)) { - _cell_count = MIN(data, max_cell_count); // never read in more cells then we can store - } else { + if (!read_word(BATTMONITOR_ND_CELL_COUNT, data)) { return; // something wrong, don't try anything else } + // constrain maximum cellcount in case of i2c corruption + if (data > max_cell_count) { + _cell_count = max_cell_count; + } else { + _cell_count = data; + } } bool read_all_cells = true; @@ -40,6 +42,8 @@ void AP_BattMonitor_SMBus_NeoDesign::timer() } } + const uint32_t tnow = AP_HAL::micros(); + if (read_all_cells && (_cell_count > 0)) { uint32_t summed = 0; for (int i = 0; i < _cell_count; i++) {