From a1564bd337b3302d499aa5e12a8a6c7e7afd3249 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Thu, 2 Jun 2016 15:02:20 -0700 Subject: [PATCH] AP_BattMonitor: check actual battery instances instead of max possible instances --- libraries/AP_BattMonitor/AP_BattMonitor.cpp | 73 +++++++-------------- 1 file changed, 22 insertions(+), 51 deletions(-) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index f4640a6259..ce6dece0bc 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -199,12 +199,7 @@ AP_BattMonitor::init() void AP_BattMonitor::read() { - // exit immediately if no monitors setup - if (_num_instances == 0) { - return; - } - - for (uint8_t i=0; iread(); } @@ -213,23 +208,21 @@ AP_BattMonitor::read() // healthy - returns true if monitor is functioning bool AP_BattMonitor::healthy(uint8_t instance) const { - return instance < AP_BATT_MONITOR_MAX_INSTANCES && _BattMonitor_STATE(instance).healthy; + return instance < _num_instances && _BattMonitor_STATE(instance).healthy; } bool AP_BattMonitor::is_powering_off(uint8_t instance) const { - return instance < AP_BATT_MONITOR_MAX_INSTANCES && _BattMonitor_STATE(instance).is_powering_off; + return instance < _num_instances && _BattMonitor_STATE(instance).is_powering_off; } /// has_current - returns true if battery monitor instance provides current info bool AP_BattMonitor::has_current(uint8_t instance) const { // check for analog voltage and current monitor or smbus monitor - if (instance < AP_BATT_MONITOR_MAX_INSTANCES) { - if (drivers[instance] != NULL) { - return (_monitoring[instance] == BattMonitor_TYPE_ANALOG_VOLTAGE_AND_CURRENT || - _monitoring[instance] == BattMonitor_TYPE_SMBUS || - _monitoring[instance] == BattMonitor_TYPE_BEBOP); - } + if (instance < _num_instances && drivers[instance] != NULL) { + return (_monitoring[instance] == BattMonitor_TYPE_ANALOG_VOLTAGE_AND_CURRENT || + _monitoring[instance] == BattMonitor_TYPE_SMBUS || + _monitoring[instance] == BattMonitor_TYPE_BEBOP); } // not monitoring current @@ -239,35 +232,16 @@ bool AP_BattMonitor::has_current(uint8_t instance) const /// voltage - returns battery voltage in volts float AP_BattMonitor::voltage(uint8_t instance) const { - if (instance < AP_BATT_MONITOR_MAX_INSTANCES) { + if (instance < _num_instances) { return _BattMonitor_STATE(instance).voltage; } else { return 0.0f; } } -// voltage2 - returns the voltage of the second battery (helper function to send 2nd voltage to GCS) -float AP_BattMonitor::voltage2() const -{ - // exit immediately if one or fewer monitors - if (_num_instances < 2) { - return 0.0f; - } - - // get next battery's voltage - for (uint8_t i=1; i= AP_BATT_MONITOR_MAX_INSTANCES || drivers[instance] == NULL) { - return 0; - } else { + if (instance < _num_instances && drivers[instance] != NULL) { return drivers[instance]->capacity_remaining_pct(); + } else { + return 0; } } @@ -297,32 +271,29 @@ uint8_t AP_BattMonitor::capacity_remaining_pct(uint8_t instance) const bool AP_BattMonitor::exhausted(uint8_t instance, float low_voltage, float min_capacity_mah) { // exit immediately if no monitors setup - if (_num_instances == 0 || instance >= AP_BATT_MONITOR_MAX_INSTANCES || drivers[instance] == NULL) { + if (_num_instances == 0 || instance >= _num_instances) { return false; } - // get current time - uint32_t tnow = AP_HAL::millis(); - // check voltage - if ((state[instance].voltage > 0.0f) && (low_voltage > 0) && (state[instance].voltage < low_voltage)) { + if ((state[instance].voltage > 0) && (low_voltage > 0) && (state[instance].voltage < low_voltage)) { // this is the first time our voltage has dropped below minimum so start timer if (state[instance].low_voltage_start_ms == 0) { - state[instance].low_voltage_start_ms = tnow; - }else if (tnow - state[instance].low_voltage_start_ms > AP_BATT_LOW_VOLT_TIMEOUT_MS) { + state[instance].low_voltage_start_ms = AP_HAL::millis(); + } else if (AP_HAL::millis() - state[instance].low_voltage_start_ms > AP_BATT_LOW_VOLT_TIMEOUT_MS) { return true; } - }else{ + } else { // acceptable voltage so reset timer state[instance].low_voltage_start_ms = 0; } // check capacity if current monitoring is enabled - if (has_current(instance) && (min_capacity_mah>0) && (_pack_capacity[instance] - state[instance].current_total_mah < min_capacity_mah)) { + if (has_current(instance) && (min_capacity_mah > 0) && (_pack_capacity[instance] - state[instance].current_total_mah < min_capacity_mah)) { return true; } - // if we've gotten this far battery is ok + // if we've gotten this far then battery is ok return false; } @@ -331,7 +302,7 @@ bool AP_BattMonitor::exhausted(uint8_t instance, float low_voltage, float min_ca bool AP_BattMonitor::overpower_detected() const { bool result = false; - for (int instance = 0; instance < AP_BATT_MONITOR_MAX_INSTANCES; instance++) { + for (int instance = 0; instance < _num_instances; instance++) { result |= overpower_detected(instance); } return result; @@ -339,7 +310,7 @@ bool AP_BattMonitor::overpower_detected() const bool AP_BattMonitor::overpower_detected(uint8_t instance) const { - if (instance < AP_BATT_MONITOR_MAX_INSTANCES && _watt_max[instance] > 0) { + if (instance < _num_instances && _watt_max[instance] > 0) { float power = _BattMonitor_STATE(instance).current_amps * _BattMonitor_STATE(instance).voltage; return _BattMonitor_STATE(instance).healthy && (power > _watt_max[instance]); }