AP_BattMonitor: Adds smart battery shutdown framework and TI BQ battery monitor

libraries/AP_BattMonitor/AP_BattMonitor.cpp

@@ -10,6 +10,7 @@
 #include "AP_BattMonitor_SMBus_Generic.h"
 #include "AP_BattMonitor_SMBus_Maxell.h"
 #include "AP_BattMonitor_SMBus_Rotoye.h"
+#include "AP_BattMonitor_SMBus_TIBQ.h"
 #include "AP_BattMonitor_Bebop.h"
 #include "AP_BattMonitor_ESC.h"
 #include "AP_BattMonitor_SMBus_SUI.h"
@@ -516,6 +517,11 @@ AP_BattMonitor::init()
                 drivers[instance] = NEW_NOTHROW AP_BattMonitor_SMBus_Rotoye(*this, state[instance], _params[instance]);
                 break;
 #endif
+#if AP_BATTERY_SMBUS_TIBQ_ENABLED
+            case Type::TIBQ:
+                drivers[instance] = NEW_NOTHROW AP_BattMonitor_SMBus_TIBQ(*this, state[instance], _params[instance]);
+                break;
+#endif
 #if AP_BATTERY_SMBUS_NEODESIGN_ENABLED
             case Type::NeoDesign:
                 drivers[instance] = NEW_NOTHROW AP_BattMonitor_SMBus_NeoDesign(*this, state[instance], _params[instance]);
@@ -1176,6 +1182,68 @@ 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 >= AP_BATT_MONITOR_MAX_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) {
+        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)
+bool AP_BattMonitor::shutdown(uint8_t instance)
+{
+    if (instance >= AP_BATT_MONITOR_MAX_INSTANCES || drivers[instance] == nullptr) {
+        return false;
+    }
+
+    if (configured_type(instance) == Type::NONE) {
+        return true;
+    }
+
+    return drivers[instance]->shutdown();
+}
+
+// Attempts to shut down all batteries that support doing so
+bool AP_BattMonitor::shutdown()
+{
+    if (!can_shutdown() || _num_instances == 0) {
+        return false;
+    }
+
+    for (uint8_t i=0; i< _num_instances; i++) {
+        // Save primary battery for last
+        if (i == AP_BATT_PRIMARY_INSTANCE) {
+            continue;
+        }
+
+        if (!shutdown(i)) {
+            return false;
+        }
+    }
+
+    return shutdown(AP_BATT_PRIMARY_INSTANCE);
+}
+
+
 namespace AP {
 
 AP_BattMonitor &battery()

libraries/AP_BattMonitor/AP_BattMonitor.h

@@ -40,6 +40,7 @@ class AP_BattMonitor_SMBus_Solo;
 class AP_BattMonitor_SMBus_Generic;
 class AP_BattMonitor_SMBus_Maxell;
 class AP_BattMonitor_SMBus_Rotoye;
+class AP_BattMonitor_SMBus_TIBQ;
 class AP_BattMonitor_DroneCAN;
 class AP_BattMonitor_Generator;
 class AP_BattMonitor_INA2XX;
@@ -60,6 +61,7 @@ class AP_BattMonitor
     friend class AP_BattMonitor_SMBus_Generic;
     friend class AP_BattMonitor_SMBus_Maxell;
     friend class AP_BattMonitor_SMBus_Rotoye;
+    friend class AP_BattMonitor_SMBus_TIBQ;
     friend class AP_BattMonitor_DroneCAN;
     friend class AP_BattMonitor_Sum;
     friend class AP_BattMonitor_FuelFlow;
@@ -116,6 +118,7 @@ class AP_BattMonitor
         EFI                            = 27,
         AD7091R5                       = 28,
         Scripting                      = 29,
+        TIBQ                           = 30,
     };
 
     FUNCTOR_TYPEDEF(battery_failsafe_handler_fn_t, void, const char *, const int8_t);
@@ -275,6 +278,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
+    bool shutdown(void);
+    // attempts to shut down a battery (if supported)
+    bool 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);}

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 bool shutdown() { return false; };
+
     // Update an ESC telemetry channel's power information
     void update_esc_telem_outbound();
 

libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp

@@ -21,7 +21,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = {
     // @Param: MONITOR
     // @DisplayName: Battery monitoring
     // @Description: Controls enabling monitoring of the battery's voltage and current
-    // @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Generic,8:DroneCAN-BatteryInfo,9:ESC,10:Sum Of Selected Monitors,11:FuelFlow,12:FuelLevelPWM,13:SMBUS-SUI3,14:SMBUS-SUI6,15:NeoDesign,16:SMBus-Maxell,17:Generator-Elec,18:Generator-Fuel,19:Rotoye,20:MPPT,21:INA2XX,22:LTC2946,23:Torqeedo,24:FuelLevelAnalog,25:Synthetic Current and Analog Voltage,26:INA239_SPI,27:EFI,28:AD7091R5,29:Scripting
+    // @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Generic,8:DroneCAN-BatteryInfo,9:ESC,10:Sum Of Selected Monitors,11:FuelFlow,12:FuelLevelPWM,13:SMBUS-SUI3,14:SMBUS-SUI6,15:NeoDesign,16:SMBus-Maxell,17:Generator-Elec,18:Generator-Fuel,19:Rotoye,20:MPPT,21:INA2XX,22:LTC2946,23:Torqeedo,24:FuelLevelAnalog,25:Synthetic Current and Analog Voltage,26:INA239_SPI,27:EFI,28:AD7091R5,29:Scripting,30:TI BQ40Z
     // @User: Standard
     // @RebootRequired: True
     AP_GROUPINFO_FLAGS("MONITOR", 1, AP_BattMonitor_Params, _type, int8_t(AP_BattMonitor::Type::NONE), AP_PARAM_FLAG_ENABLE),

libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Generic.h

@@ -19,9 +19,13 @@ class AP_BattMonitor_SMBus_Generic : public AP_BattMonitor_SMBus
                              AP_BattMonitor::BattMonitor_State &mon_state,
                              AP_BattMonitor_Params &params);
 
+protected:
+
+    // timer is protected to allow calling from a child class
+    virtual void timer(void) override;
+
 private:
 
-    void timer(void) override;
 
     // check if PEC supported with the version value in SpecificationInfo() function
     // returns true once PEC is confirmed as working or not working

libraries/AP_BattMonitor/AP_BattMonitor_SMBus_TIBQ.cpp

@@ -0,0 +1,50 @@
+#include "AP_BattMonitor_config.h"
+
+#if AP_BATTERY_SMBUS_TIBQ_ENABLED
+
+#include "AP_BattMonitor_SMBus_TIBQ.h"
+
+#include <AP_HAL/AP_HAL.h>
+#include <GCS_MAVLink/GCS.h>
+
+
+// Extention of AP_BattMonitor_SMBus_Generic to include TI's BQ40Z chip shutdown mechanism
+AP_BattMonitor_SMBus_TIBQ::AP_BattMonitor_SMBus_TIBQ(AP_BattMonitor &mon,
+                                                   AP_BattMonitor::BattMonitor_State &mon_state,
+                                                   AP_BattMonitor_Params &params)
+    : AP_BattMonitor_SMBus_Generic(mon, mon_state, params)
+{
+    _exit_emshut = true;
+}
+
+
+void AP_BattMonitor_SMBus_TIBQ::timer() {
+    if (_exit_emshut) {
+        // Exit EMERGENCY SHUTDOWN state in case it was engaged on the last poweroff:
+        uint8_t cmd[] = {0x00, 0xA7, 0x23};
+        if (_dev->transfer(cmd, 3, nullptr, 0)) {
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "BQ40Z bms exited shutdown");
+            _exit_emshut = false;
+        }
+    }
+
+    AP_BattMonitor_SMBus_Generic::timer();
+}
+
+
+bool AP_BattMonitor_SMBus_TIBQ::shutdown() {
+    // Semaphore is needed in case this is called from another thread
+    WITH_SEMAPHORE(_dev->get_semaphore());
+
+    uint8_t cmd[] = {0x00, 0x10, 0x00};
+    if (!_dev->transfer(cmd, 3, nullptr, 0)) {
+        GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Failed to shutdown TIBQ");
+        return false;
+    } else {
+        _state.is_powering_off = true;
+    }
+
+    return true;
+}
+
+#endif // AP_BATTERY_SMBUS_TIBQ_ENABLED

libraries/AP_BattMonitor/AP_BattMonitor_SMBus_TIBQ.h

@@ -0,0 +1,28 @@
+#pragma once
+
+#include "AP_BattMonitor_config.h"
+
+#if AP_BATTERY_SMBUS_TIBQ_ENABLED
+
+#include "AP_BattMonitor_SMBus_Generic.h"
+
+class AP_BattMonitor_SMBus_TIBQ : public AP_BattMonitor_SMBus_Generic
+{
+public:
+    // Constructor
+    AP_BattMonitor_SMBus_TIBQ(AP_BattMonitor &mon,
+                             AP_BattMonitor::BattMonitor_State &mon_state,
+                             AP_BattMonitor_Params &params);
+
+private:
+    void timer(void) override;
+
+    // returns true if the battery can be shutdown with shutdown()
+    bool can_shutdown() override { return true; };
+    // shuts the battery down if supported
+    bool shutdown() override;
+
+    bool _exit_emshut;
+};
+
+#endif // AP_BATTERY_SMBUS_TIBQ_ENABLED

libraries/AP_BattMonitor/AP_BattMonitor_config.h

@@ -116,6 +116,10 @@
 #define AP_BATTERY_SMBUS_ROTOYE_ENABLED AP_BATTERY_SMBUS_GENERIC_ENABLED
 #endif
 
+#ifndef AP_BATTERY_SMBUS_TIBQ_ENABLED
+#define AP_BATTERY_SMBUS_TIBQ_ENABLED AP_BATTERY_SMBUS_GENERIC_ENABLED
+#endif
+
 #ifndef AP_BATTERY_SCRIPTING_ENABLED
 #define AP_BATTERY_SCRIPTING_ENABLED (AP_SCRIPTING_ENABLED && AP_BATTERY_BACKEND_DEFAULT_ENABLED)
 #endif

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -3389,7 +3389,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;
@@ -3398,6 +3398,18 @@ 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 doesn't support shutdown
+        if (!AP::battery().shutdown()){
+            return MAV_RESULT_FAILED;
+        }
+
+        return MAV_RESULT_ACCEPTED;
+    }
+#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;