diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index 4cf1d808e3..133358bda1 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -588,6 +588,31 @@ bool AP_BattMonitor::reset_remaining_mask(uint16_t battery_mask, float percentag return ret; } +// Returns the mavlink charge state. The following mavlink charge states are not used +// MAV_BATTERY_CHARGE_STATE_EMERGENCY , MAV_BATTERY_CHARGE_STATE_FAILED +// MAV_BATTERY_CHARGE_STATE_UNHEALTHY, MAV_BATTERY_CHARGE_STATE_CHARGING +MAV_BATTERY_CHARGE_STATE AP_BattMonitor::get_mavlink_charge_state(const uint8_t instance) const +{ + if (instance >= _num_instances) { + return MAV_BATTERY_CHARGE_STATE_UNDEFINED; + } + + switch (state[instance].failsafe) { + + case Failsafe::None: + return MAV_BATTERY_CHARGE_STATE_OK; + + case Failsafe::Low: + return MAV_BATTERY_CHARGE_STATE_LOW; + + case Failsafe::Critical: + return MAV_BATTERY_CHARGE_STATE_CRITICAL; + } + + // Should not reach this + return MAV_BATTERY_CHARGE_STATE_UNDEFINED; +} + namespace AP { AP_BattMonitor &battery() diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index 49e74452fe..a95bcf7d8f 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -214,6 +214,9 @@ public: bool reset_remaining_mask(uint16_t battery_mask, float percentage); bool reset_remaining(uint8_t instance, float percentage) { return reset_remaining_mask(1U<