diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index fa5617cb46..5621bc5214 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -14,6 +14,7 @@ #include #include #include +#include extern const AP_HAL::HAL& hal; @@ -244,6 +245,8 @@ AP_BattMonitor::read() } check_failsafes(); + + checkPoweringOff(); } // healthy - returns true if monitor is functioning @@ -464,6 +467,29 @@ bool AP_BattMonitor::arming_checks(size_t buflen, char *buffer) const return true; } +// Check's each smart battery instance for its powering off state and broadcasts notifications +void AP_BattMonitor::checkPoweringOff(void) +{ + for (uint8_t i = 0; i < _num_instances; i++) { + if (state[i].is_powering_off && !state[i].powerOffNotified) { + // Set the AP_Notify flag, which plays the power off tones + AP_Notify::flags.powering_off = true; + + // Send a Mavlink broadcast announcing the shutdown + mavlink_message_t msg; + mavlink_command_long_t cmd_msg; + cmd_msg.command = MAV_CMD_POWER_OFF_INITIATED; + cmd_msg.param1 = i+1; + mavlink_msg_command_long_encode(mavlink_system.sysid, MAV_COMP_ID_ALL, &msg, &cmd_msg); + GCS_MAVLINK::send_to_components(&msg); + gcs().send_text(MAV_SEVERITY_WARNING, "Vehicle %d battery %d is powering off", mavlink_system.sysid, i+1); + + // only send this once + state[i].powerOffNotified = true; + } + } +} + namespace AP { AP_BattMonitor &battery() diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index 78716b08eb..7a99702b53 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -78,6 +78,8 @@ public: float resistance; // resistance, in Ohms, calculated by comparing resting voltage vs in flight voltage BatteryFailsafe 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 }; // Return the number of battery monitor instances @@ -168,6 +170,9 @@ public: // returns false if we fail arming checks, in which case the buffer will be populated with a failure message bool arming_checks(size_t buflen, char *buffer) const; + // sends powering off mavlink broadcasts and sets notify flag + void checkPoweringOff(void); + static const struct AP_Param::GroupInfo var_info[]; protected: diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp index d57d4df697..e333bf066d 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp @@ -1,7 +1,6 @@ #include #include #include -#include #include "AP_BattMonitor.h" #include "AP_BattMonitor_SMBus_Solo.h" #include @@ -80,16 +79,14 @@ void AP_BattMonitor_SMBus_Solo::timer() bool pressed = (buff[1] >> 3) & 0x01; if (_button_press_count >= BATTMONITOR_SMBUS_SOLO_BUTTON_DEBOUNCE) { - // battery will power off - AP_Notify::flags.powering_off = true; + // vehicle will power off, set state flag + _state.is_powering_off = true; } else if (pressed) { // battery will power off if the button is held _button_press_count++; - } else { // button released, reset counters _button_press_count = 0; - AP_Notify::flags.powering_off = false; } }