AP_BattMonitor_SMBus_TIBQ: Added new TIBQ battery monitor

ArduPilot · Jun 13, 2024 · 6bc529a · 6bc529a
1 parent a8468ef
commit 6bc529a
Show file tree

Hide file tree

Showing 7 changed files with 96 additions and 9 deletions.
diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/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]);

diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/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;
@@ -115,6 +117,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);

diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Generic.h b/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

diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_TIBQ.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_TIBQ.cpp
@@ -0,0 +1,48 @@
+#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();
+}
+
+
+void 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");
+    } else {
+        _state.is_powering_off = true;
+    }
+}
+
+#endif // AP_BATTERY_SMBUS_TIBQ_ENABLED
diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_TIBQ.h b/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
+    void shutdown() override;
+
+    bool _exit_emshut;
+};
+
+#endif // AP_BATTERY_SMBUS_TIBQ_ENABLED
diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_config.h b/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
diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -3321,19 +3321,13 @@ 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 the battery monitor in use doesn't 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;
+        return MAV_RESULT_ACCEPTED;
     }
 #endif