Browse Source

AP_BattMonitor: use enum class For Battery Failsafe

c415-sdk
Peter Barker 4 years ago committed by Peter Barker
parent
commit
00a8a8fe8e
  1. 12
      libraries/AP_BattMonitor/AP_BattMonitor.cpp
  2. 12
      libraries/AP_BattMonitor/AP_BattMonitor.h
  3. 12
      libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp
  4. 2
      libraries/AP_BattMonitor/AP_BattMonitor_Backend.h
  5. 4
      libraries/AP_BattMonitor/AP_BattMonitor_Generator.cpp
  6. 2
      libraries/AP_BattMonitor/AP_BattMonitor_Generator.h

12
libraries/AP_BattMonitor/AP_BattMonitor.cpp

@ -393,7 +393,7 @@ void AP_BattMonitor::check_failsafes(void) @@ -393,7 +393,7 @@ void AP_BattMonitor::check_failsafes(void)
continue;
}
const BatteryFailsafe type = drivers[i]->update_failsafes();
const Failsafe type = drivers[i]->update_failsafes();
if (type <= state[i].failsafe) {
continue;
}
@ -401,13 +401,13 @@ void AP_BattMonitor::check_failsafes(void) @@ -401,13 +401,13 @@ void AP_BattMonitor::check_failsafes(void)
int8_t action = 0;
const char *type_str = nullptr;
switch (type) {
case AP_BattMonitor::BatteryFailsafe_None:
case Failsafe::None:
continue; // should not have been called in this case
case AP_BattMonitor::BatteryFailsafe_Low:
case Failsafe::Low:
action = _params[i]._failsafe_low_action;
type_str = "low";
break;
case AP_BattMonitor::BatteryFailsafe_Critical:
case Failsafe::Critical:
action = _params[i]._failsafe_critical_action;
type_str = "critical";
break;
@ -551,7 +551,7 @@ void AP_BattMonitor::checkPoweringOff(void) @@ -551,7 +551,7 @@ void AP_BattMonitor::checkPoweringOff(void)
bool AP_BattMonitor::reset_remaining(uint16_t battery_mask, float percentage)
{
bool ret = true;
BatteryFailsafe highest_failsafe = BatteryFailsafe_None;
Failsafe highest_failsafe = Failsafe::None;
for (uint8_t i = 0; i < _num_instances; i++) {
if ((1U<<i) & battery_mask) {
ret &= drivers[i]->reset_remaining(percentage);
@ -562,7 +562,7 @@ bool AP_BattMonitor::reset_remaining(uint16_t battery_mask, float percentage) @@ -562,7 +562,7 @@ bool AP_BattMonitor::reset_remaining(uint16_t battery_mask, float percentage)
}
// If all backends are not in failsafe then set overall failsafe state
if (highest_failsafe == BatteryFailsafe_None) {
if (highest_failsafe == Failsafe::None) {
_highest_failsafe_priority = INT8_MAX;
_has_triggered_failsafe = false;
// and reset notify flag

12
libraries/AP_BattMonitor/AP_BattMonitor.h

@ -58,10 +58,10 @@ class AP_BattMonitor @@ -58,10 +58,10 @@ class AP_BattMonitor
public:
// battery failsafes must be defined in levels of severity so that vehicles wont fall backwards
enum BatteryFailsafe {
BatteryFailsafe_None = 0,
BatteryFailsafe_Low,
BatteryFailsafe_Critical
enum class Failsafe : uint8_t {
None = 0,
Low,
Critical
};
// Battery monitor driver types
@ -117,7 +117,7 @@ public: @@ -117,7 +117,7 @@ public:
uint32_t temperature_time; // timestamp of the last received temperature message
float voltage_resting_estimate; // voltage with sag removed based on current and resistance estimate in Volt
float resistance; // resistance, in Ohms, calculated by comparing resting voltage vs in flight voltage
BatteryFailsafe failsafe; // stage failsafe the battery is in
Failsafe failsafe; // stage failsafe the battery is in
bool healthy; // battery monitor is communicating correctly
bool is_powering_off; // true when power button commands power off
bool powerOffNotified; // only send powering off notification once
@ -228,7 +228,7 @@ private: @@ -228,7 +228,7 @@ private:
void convert_params(void);
/// returns the failsafe state of the battery
BatteryFailsafe check_failsafe(const uint8_t instance);
Failsafe check_failsafe(const uint8_t instance);
void check_failsafes(void); // checks all batteries failsafes
battery_failsafe_handler_fn_t _battery_failsafe_handler_fn;

12
libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp

@ -98,7 +98,7 @@ float AP_BattMonitor_Backend::voltage_resting_estimate() const @@ -98,7 +98,7 @@ float AP_BattMonitor_Backend::voltage_resting_estimate() const
return MAX(_state.voltage, _state.voltage_resting_estimate);
}
AP_BattMonitor::BatteryFailsafe AP_BattMonitor_Backend::update_failsafes(void)
AP_BattMonitor::Failsafe AP_BattMonitor_Backend::update_failsafes(void)
{
const uint32_t now = AP_HAL::millis();
@ -111,7 +111,7 @@ AP_BattMonitor::BatteryFailsafe AP_BattMonitor_Backend::update_failsafes(void) @@ -111,7 +111,7 @@ AP_BattMonitor::BatteryFailsafe AP_BattMonitor_Backend::update_failsafes(void)
_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;
return AP_BattMonitor::Failsafe::Critical;
}
} else {
// acceptable voltage so reset timer
@ -119,7 +119,7 @@ AP_BattMonitor::BatteryFailsafe AP_BattMonitor_Backend::update_failsafes(void) @@ -119,7 +119,7 @@ AP_BattMonitor::BatteryFailsafe AP_BattMonitor_Backend::update_failsafes(void)
}
if (critical_capacity) {
return AP_BattMonitor::BatteryFailsafe_Critical;
return AP_BattMonitor::Failsafe::Critical;
}
if (low_voltage) {
@ -128,7 +128,7 @@ AP_BattMonitor::BatteryFailsafe AP_BattMonitor_Backend::update_failsafes(void) @@ -128,7 +128,7 @@ AP_BattMonitor::BatteryFailsafe AP_BattMonitor_Backend::update_failsafes(void)
_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;
return AP_BattMonitor::Failsafe::Low;
}
} else {
// acceptable voltage so reset timer
@ -136,11 +136,11 @@ AP_BattMonitor::BatteryFailsafe AP_BattMonitor_Backend::update_failsafes(void) @@ -136,11 +136,11 @@ AP_BattMonitor::BatteryFailsafe AP_BattMonitor_Backend::update_failsafes(void)
}
if (low_capacity) {
return AP_BattMonitor::BatteryFailsafe_Low;
return AP_BattMonitor::Failsafe::Low;
}
// if we've gotten this far then battery is ok
return AP_BattMonitor::BatteryFailsafe_None;
return AP_BattMonitor::Failsafe::None;
}
static bool update_check(size_t buflen, char *buffer, bool failed, const char *message)

2
libraries/AP_BattMonitor/AP_BattMonitor_Backend.h

@ -57,7 +57,7 @@ public: @@ -57,7 +57,7 @@ public:
void update_resistance_estimate();
// updates failsafe timers, and returns what failsafes are active
virtual AP_BattMonitor::BatteryFailsafe update_failsafes(void);
virtual AP_BattMonitor::Failsafe update_failsafes(void);
// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
bool arming_checks(char * buffer, size_t buflen) const;

4
libraries/AP_BattMonitor/AP_BattMonitor_Generator.cpp

@ -143,9 +143,9 @@ void AP_BattMonitor_Generator_Elec::read() @@ -143,9 +143,9 @@ void AP_BattMonitor_Generator_Elec::read()
_state.last_time_micros = AP_HAL::micros();
}
AP_BattMonitor::BatteryFailsafe AP_BattMonitor_Generator_Elec::update_failsafes()
AP_BattMonitor::Failsafe AP_BattMonitor_Generator_Elec::update_failsafes()
{
AP_BattMonitor::BatteryFailsafe failsafe = AP_BattMonitor::BatteryFailsafe::BatteryFailsafe_None;
AP_BattMonitor::Failsafe failsafe = AP_BattMonitor::Failsafe::None;
AP_Generator *generator = AP::generator();

2
libraries/AP_BattMonitor/AP_BattMonitor_Generator.h

@ -25,7 +25,7 @@ public: @@ -25,7 +25,7 @@ public:
bool has_consumed_energy(void) const override;
// Override backend update_failsafes. No point in failsafing twice so generator failsafes are only updated from the electrical instance of the generator drivers
AP_BattMonitor::BatteryFailsafe update_failsafes() override;
AP_BattMonitor::Failsafe update_failsafes() override;
};
// Sub class for generator fuel

Loading…
Cancel
Save