Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-11 17:13:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

AP_BattMonitor: Move failsafe checks to the backend

Browse Source
This commit is contained in:
Michael du Breuil 2018-09-12 14:18:06 -07:00 committed by Francisco Ferreira
parent 5192376c85
commit 1bb4f07365
3 changed files with 80 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

76
libraries/AP_BattMonitor/AP_BattMonitor.cpp
View File

@ -254,9 +254,8 @@ float AP_BattMonitor::voltage(uint8_t instance) const
/// this will always be greater than or equal to the raw voltage
float AP_BattMonitor::voltage_resting_estimate(uint8_t instance) const
{
if (instance < _num_instances) {
// resting voltage should always be greater than or equal to the raw voltage
return MAX(state[instance].voltage, state[instance].voltage_resting_estimate);
if (instance < _num_instances && drivers[instance] != nullptr) {
return drivers[instance]->voltage_resting_estimate();
} else {
return 0.0f;
}
@ -313,7 +312,11 @@ void AP_BattMonitor::check_failsafes(void)
{
if (hal.util->get_soft_armed()) {
for (uint8_t i = 0; i < _num_instances; i++) {
const BatteryFailsafe type = check_failsafe(i);
if (drivers[i] == nullptr) {
continue;
}
const BatteryFailsafe type = drivers[i]->update_failsafes();
if (type <= state[i].failsafe) {
continue;
}
@ -361,71 +364,6 @@ void AP_BattMonitor::check_failsafes(void)
}
}
// returns the failsafe state of the battery
AP_BattMonitor::BatteryFailsafe AP_BattMonitor::check_failsafe(const uint8_t instance)
{
// exit immediately if no monitors setup
if (_num_instances == 0 || instance >= _num_instances) {
return BatteryFailsafe_None;
}
const uint32_t now = AP_HAL::millis();
// use voltage or sag compensated voltage
float voltage_used;
switch (_params[instance].failsafe_voltage_source()) {
case AP_BattMonitor_Params::BattMonitor_LowVoltageSource_Raw:
default:
voltage_used = state[instance].voltage;
break;
case AP_BattMonitor_Params::BattMonitor_LowVoltageSource_SagCompensated:
voltage_used = voltage_resting_estimate(instance);
break;
}
// check critical battery levels
if ((voltage_used > 0) && (_params[instance]._critical_voltage > 0) && (voltage_used < _params[instance]._critical_voltage)) {
// this is the first time our voltage has dropped below minimum so start timer
if (state[instance].critical_voltage_start_ms == 0) {
state[instance].critical_voltage_start_ms = now;
} else if (_params[instance]._low_voltage_timeout > 0 &&
now - state[instance].critical_voltage_start_ms > uint32_t(_params[instance]._low_voltage_timeout)*1000U) {
return BatteryFailsafe_Critical;
}
} else {
// acceptable voltage so reset timer
state[instance].critical_voltage_start_ms = 0;
}
// check capacity if current monitoring is enabled
if (has_current(instance) && (_params[instance]._critical_capacity > 0) &&
((_params[instance]._pack_capacity - state[instance].consumed_mah) < _params[instance]._critical_capacity)) {
return BatteryFailsafe_Critical;
}
if ((voltage_used > 0) && (_params[instance]._low_voltage > 0) && (voltage_used < _params[instance]._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 = now;
} else if (_params[instance]._low_voltage_timeout > 0 &&
now - state[instance].low_voltage_start_ms > uint32_t(_params[instance]._low_voltage_timeout)*1000U) {
return BatteryFailsafe_Low;
}
} else {
// acceptable voltage so reset timer
state[instance].low_voltage_start_ms = 0;
}
// check capacity if current monitoring is enabled
if (has_current(instance) && (_params[instance]._low_capacity > 0) &&
((_params[instance]._pack_capacity - state[instance].consumed_mah) < _params[instance]._low_capacity)) {
return BatteryFailsafe_Low;
}
// if we've gotten this far then battery is ok
return BatteryFailsafe_None;
}
// return true if any battery is pushing too much power
bool AP_BattMonitor::overpower_detected() const
{

66
libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp
View File

@ -91,3 +91,69 @@ void AP_BattMonitor_Backend::update_resistance_estimate()
// update estimated voltage without sag
_state.voltage_resting_estimate = _state.voltage + _state.current_amps * _state.resistance;
}
float AP_BattMonitor_Backend::voltage_resting_estimate() const
{
// resting voltage should always be greater than or equal to the raw voltage
return MAX(_state.voltage, _state.voltage_resting_estimate);
}
AP_BattMonitor::BatteryFailsafe AP_BattMonitor_Backend::update_failsafes(void)
{
const uint32_t now = AP_HAL::millis();
// use voltage or sag compensated voltage
float voltage_used;
switch (_params.failsafe_voltage_source()) {
case AP_BattMonitor_Params::BattMonitor_LowVoltageSource_Raw:
default:
voltage_used = _state.voltage;
break;
case AP_BattMonitor_Params::BattMonitor_LowVoltageSource_SagCompensated:
voltage_used = voltage_resting_estimate();
break;
}
// check critical battery levels
if ((voltage_used > 0) && (_params._critical_voltage > 0) && (voltage_used < _params._critical_voltage)) {
// this is the first time our voltage has dropped below minimum so start timer
if (_state.critical_voltage_start_ms == 0) {
_state.critical_voltage_start_ms = now;
} else if (_params._low_voltage_timeout > 0 &&
now - _state.critical_voltage_start_ms > uint32_t(_params._low_voltage_timeout)*1000U) {
return AP_BattMonitor::BatteryFailsafe_Critical;
}
} else {
// acceptable voltage so reset timer
_state.critical_voltage_start_ms = 0;
}
// check capacity if current monitoring is enabled
if (has_current() && (_params._critical_capacity > 0) &&
((_params._pack_capacity - _state.consumed_mah) < _params._critical_capacity)) {
return AP_BattMonitor::BatteryFailsafe_Critical;
}
if ((voltage_used > 0) && (_params._low_voltage > 0) && (voltage_used < _params._low_voltage)) {
// this is the first time our voltage has dropped below minimum so start timer
if (_state.low_voltage_start_ms == 0) {
_state.low_voltage_start_ms = now;
} else if (_params._low_voltage_timeout > 0 &&
now - _state.low_voltage_start_ms > uint32_t(_params._low_voltage_timeout)*1000U) {
return AP_BattMonitor::BatteryFailsafe_Low;
}
} else {
// acceptable voltage so reset timer
_state.low_voltage_start_ms = 0;
}
// check capacity if current monitoring is enabled
if (has_current() && (_params._low_capacity > 0) &&
((_params._pack_capacity - _state.consumed_mah) < _params._low_capacity)) {
return AP_BattMonitor::BatteryFailsafe_Low;
}
// if we've gotten this far then battery is ok
return AP_BattMonitor::BatteryFailsafe_None;
}

7
libraries/AP_BattMonitor/AP_BattMonitor_Backend.h
View File

@ -46,9 +46,16 @@ public:
/// capacity_remaining_pct - returns the % battery capacity remaining (0 ~ 100)
uint8_t capacity_remaining_pct() const;
/// get voltage with sag removed (based on battery current draw and resistance)
/// this will always be greater than or equal to the raw voltage
float voltage_resting_estimate() const;
// update battery resistance estimate and voltage_resting_estimate
void update_resistance_estimate();
// updates failsafe timers, and returns what failsafes are active
AP_BattMonitor::BatteryFailsafe update_failsafes(void);
protected:
AP_BattMonitor &_mon; // reference to front-end
AP_BattMonitor::BattMonitor_State &_state; // reference to this instances state (held in the front-end)