From a8468efaa0ce47447c790fa4dc0b87a7a1709998 Mon Sep 17 00:00:00 2001
From: Ian Burwell <iburwell@greensightag.com>
Date: Wed, 15 May 2024 18:17:30 -0400
Subject: [PATCH] AP_BattMonitor: Add architecture to shutdown batteries

---
 libraries/AP_BattMonitor/AP_BattMonitor.cpp   | 55 +++++++++++++++++++
 libraries/AP_BattMonitor/AP_BattMonitor.h     |  9 +++
 .../AP_BattMonitor/AP_BattMonitor_Backend.h   |  7 ++-
 libraries/GCS_MAVLink/GCS_Common.cpp          | 21 ++++++-
 4 files changed, 90 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp
index 8c766b4e362f9b..b5a262b72db3d6 100644
--- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp
+++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp
@@ -1164,6 +1164,61 @@ bool AP_BattMonitor::handle_scripting(uint8_t idx, const BattMonitorScript_State
 }
 #endif
 
+// returns true if the battery has shutdown functionality
+bool AP_BattMonitor::can_shutdown(uint8_t instance) const {
+    if (instance >= _num_instances || drivers[instance] == nullptr) {
+        return false;
+    }
+
+    return drivers[instance]->can_shutdown();
+}
+
+// returns true if all connected batteries have shutdown functionality
+bool AP_BattMonitor::can_shutdown() const {
+    if(_num_instances == 0 || _num_instances >= AP_BATT_MONITOR_MAX_INSTANCES){
+        return false;
+    }
+
+    for (uint8_t i=0; i< _num_instances; i++) {
+        if (configured_type(i) != Type::NONE && !can_shutdown(i)) {
+            return false;
+        }
+    }
+
+    return true;
+}
+
+// attempts to shut down a battery (if supported)
+void AP_BattMonitor::shutdown(uint8_t instance){
+    if (instance < _num_instances && drivers[instance] != nullptr) {
+        drivers[instance]->shutdown();
+    }
+}
+
+// attempts to shut down all batteries that support doing so
+void AP_BattMonitor::shutdown(){
+    if(_num_instances == 0 || _num_instances >= AP_BATT_MONITOR_MAX_INSTANCES){
+        return;
+    }
+
+    for (uint8_t i=0; i< _num_instances; i++) {
+        // skip the primary battery for now
+        if(i == AP_BATT_PRIMARY_INSTANCE) {
+            continue;
+        }
+
+        if (configured_type(i) != Type::NONE) {
+            shutdown(i);
+        }
+    }
+
+    // shutdown the primary battery last
+    if (configured_type(AP_BATT_PRIMARY_INSTANCE) != Type::NONE && can_shutdown(AP_BATT_PRIMARY_INSTANCE)) {
+        shutdown(AP_BATT_PRIMARY_INSTANCE);
+    }
+}
+
+
 namespace AP {
 
 AP_BattMonitor &battery()
diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h
index f03e9976320bec..9ba6877e5dbb9a 100644
--- a/libraries/AP_BattMonitor/AP_BattMonitor.h
+++ b/libraries/AP_BattMonitor/AP_BattMonitor.h
@@ -273,6 +273,15 @@ class AP_BattMonitor
     // sends powering off mavlink broadcasts and sets notify flag
     void checkPoweringOff(void);
 
+    // returns true if all connected batteries can be shutdown by the AP
+    bool can_shutdown(void) const;
+    // returns true if the battery has the capability to be shutdown by the AP
+    bool can_shutdown(uint8_t instance) const;
+    // attempts to shut down all batteries that support doing so
+    void shutdown(void);
+    // attempts to shut down a battery (if supported)
+    void shutdown(uint8_t instance);
+
     // reset battery remaining percentage
     bool reset_remaining_mask(uint16_t battery_mask, float percentage);
     bool reset_remaining(uint8_t instance, float percentage) { return reset_remaining_mask(1U<<instance, percentage);}
diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h
index a33c316d70d7bc..b0dd297661f031 100644
--- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h
+++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h
@@ -85,13 +85,18 @@ class AP_BattMonitor_Backend
     // return mavlink fault bitmask (see MAV_BATTERY_FAULT enum)
     virtual uint32_t get_mavlink_fault_bitmask() const { return 0; }
 
-    // logging functions 
+    // logging functions
     void Log_Write_BAT(const uint8_t instance, const uint64_t time_us) const;
     void Log_Write_BCL(const uint8_t instance, const uint64_t time_us) const;
 
     // set desired MPPT powered state (enabled/disabled)
     virtual void mppt_set_powered_state(bool power_on) {};
 
+    // returns true if the battery can be shutdown with shutdown()
+    virtual bool can_shutdown() { return false; };
+    // shuts the battery down if supported
+    virtual void shutdown() {};
+
     // Update an ESC telemetry channel's power information
     void update_esc_telem_outbound();
 
diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
index f86a06480bf1fd..21c4813a022db3 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -3309,7 +3309,7 @@ MAV_RESULT GCS_MAVLINK::handle_preflight_reboot(const mavlink_command_int_t &pac
 #endif
     }
 
-    // refuse reboot when armed:
+    // refuse shutdown/reboot when armed:
     if (hal.util->get_soft_armed()) {
         /// but allow it if forced:
         const uint32_t magic_force_reboot_value = 20190226;
@@ -3318,6 +3318,25 @@ MAV_RESULT GCS_MAVLINK::handle_preflight_reboot(const mavlink_command_int_t &pac
         }
     }
 
+#if AP_BATTERY_ENABLED
+    // Check if shutdown is requested
+    if(is_equal(packet.param1, 2.0f)){
+        // If the battery monitor in use doesnt support shutdown
+        if(!AP::battery().can_shutdown())
+            return MAV_RESULT_FAILED;
+
+        // send ack before we shutdown
+        mavlink_msg_command_ack_send(chan, packet.command, MAV_RESULT_ACCEPTED,
+                                 0, 0,
+                                 msg.sysid,
+                                 msg.compid);
+
+        AP::battery().shutdown();
+
+        return MAV_RESULT_FAILED;
+    }
+#endif
+
     if (!(is_equal(packet.param1, 1.0f) || is_equal(packet.param1, 3.0f))) {
         // param1 must be 1 or 3 - 1 being reboot, 3 being reboot-to-bootloader
         return MAV_RESULT_UNSUPPORTED;