AP_BattMonitor: Add powering off mavlink broadcast

libraries/AP_BattMonitor/AP_BattMonitor.cpp

@@ -14,6 +14,7 @@
 #include <AP_Vehicle/AP_Vehicle_Type.h>
 #include <AP_Logger/AP_Logger.h>
 #include <GCS_MAVLink/GCS.h>
+#include <AP_Notify/AP_Notify.h>
 
 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()

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:

libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp

@@ -1,7 +1,6 @@
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Common/AP_Common.h>
 #include <AP_Math/AP_Math.h>
-#include <AP_Notify/AP_Notify.h>
 #include "AP_BattMonitor.h"
 #include "AP_BattMonitor_SMBus_Solo.h"
 #include <utility>
@@ -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;
         }
     }