Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_BattMonitor: check actual battery instances instead of max possible instances

This commit is contained in:
Tom Pittenger 2016-06-02 15:02:20 -07:00
parent 8939c5308f
commit a1564bd337
1 changed files with 22 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
libraries/AP_BattMonitor/AP_BattMonitor.cpp
View File

@ -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; i<AP_BATT_MONITOR_MAX_INSTANCES; i++) {
for (uint8_t i=0; i<_num_instances; i++) {
if (drivers[i] != NULL && _monitoring[i] != BattMonitor_TYPE_NONE) {
drivers[i]->read();
}
@ -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; i++) {
if (drivers[i] != NULL) {
return _BattMonitor_STATE(i).voltage;
}
}
// if we somehow got here, return zero
return 0.0f;
}
/// current_amps - returns the instantaneous current draw in amperes
float AP_BattMonitor::current_amps(uint8_t instance) const {
if (instance < AP_BATT_MONITOR_MAX_INSTANCES) {
if (instance < _num_instances) {
return _BattMonitor_STATE(instance).current_amps;
} else {
return 0.0f;
@ -276,7 +250,7 @@ float AP_BattMonitor::current_amps(uint8_t instance) const {
/// current_total_mah - returns total current drawn since start-up in amp-hours
float AP_BattMonitor::current_total_mah(uint8_t instance) const {
if (instance < AP_BATT_MONITOR_MAX_INSTANCES) {
if (instance < _num_instances) {
return _BattMonitor_STATE(instance).current_total_mah;
} else {
return 0.0f;
@ -286,10 +260,10 @@ float AP_BattMonitor::current_total_mah(uint8_t instance) const {
/// capacity_remaining_pct - returns the % battery capacity remaining (0 ~ 100)
uint8_t AP_BattMonitor::capacity_remaining_pct(uint8_t instance) const
{
if (_num_instances == 0 || instance >= 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]);
}