AP_BattMonitor: TIBQ supports sleep

Co-authored-by: Huibean <huibean.luo@gmail.com>

libraries/AP_BattMonitor/AP_BattMonitor.cpp

@@ -1290,6 +1290,16 @@ bool AP_BattMonitor::handle_scripting(uint8_t idx, const BattMonitorScript_State
 }
 #endif
 
+// set battery BMS sleep timeout in seconds
+// set to zero to disable sleep
+void AP_BattMonitor::set_sleep_timeout(uint8_t instance, uint16_t timeout_sec)
+{
+    if (instance >= _num_instances || drivers[instance] == nullptr) {
+        return;
+    }
+    drivers[instance]->set_sleep_timeout(timeout_sec);
+}
+
 namespace AP {
 
 AP_BattMonitor &battery()

libraries/AP_BattMonitor/AP_BattMonitor.h

@@ -284,6 +284,11 @@ public:
     bool handle_scripting(uint8_t idx, const struct BattMonitorScript_State &state);
 #endif
 
+    // set battery BMS sleep timeout in seconds
+    // set to zero to disable sleep
+    void set_sleep_timeout(uint16_t timeout_sec) { return set_sleep_timeout(AP_BATT_PRIMARY_INSTANCE, timeout_sec); }
+    void set_sleep_timeout(uint8_t instance, uint16_t timeout_sec);
+
 protected:
 
     /// parameters

libraries/AP_BattMonitor/AP_BattMonitor_Backend.h

@@ -108,6 +108,10 @@ public:
     virtual bool handle_scripting(const BattMonitorScript_State &battmon_state) { return false; }
 #endif
 
+    // set battery BMS sleep timeout in seconds
+    // set to zero to disable sleep
+    virtual void set_sleep_timeout(uint16_t timeout_sec) {}
+
 protected:
     AP_BattMonitor                      &_mon;      // reference to front-end
     AP_BattMonitor::BattMonitor_State   &_state;    // reference to this instances state (held in the front-end)

libraries/AP_BattMonitor/AP_BattMonitor_TIBQ76952.cpp

@@ -690,6 +690,7 @@ void AP_BattMonitor_TIBQ76952::timer(void)
     // read data from device
     if (read_voltage_current_temperature()) {
         read_charging_state();
+        check_sleep_timeout();
     }
 }
 
@@ -890,6 +891,35 @@ void AP_BattMonitor_TIBQ76952::read_charging_state()
     _state.charging_state = new_state;
 }
 
+// check if the BMS should sleep
+void AP_BattMonitor_TIBQ76952::check_sleep_timeout()
+{
+    // exit immediately if sleep mode is disabled
+    if (sleep_timeout_sec <= 0) {
+        return;
+    }
+
+    // update activity time if not idle
+    uint32_t now_ms = AP_HAL::millis();
+    if (_state.charging_state != AP_BattMonitor::ChargingState::IDLE) {
+        activity_timer_ms = now_ms;
+        return;
+    }
+
+    // check for timeout
+    if (now_ms - activity_timer_ms > sleep_timeout_sec * 1000) {
+        // reset activity counter to avoid resending sleep commands in case BMS decides not to sleep
+        activity_timer_ms = now_ms;
+
+        // send debug message
+        Debug("BQ76952: sleep timeout");
+
+        // sleep mode commands must be sent twice
+        sub_command(TIBQ769x2_DEEPSLEEP);
+        sub_command(TIBQ769x2_DEEPSLEEP);
+    }
+}
+
 // read bytes from a register. returns true on success
 bool AP_BattMonitor_TIBQ76952::read_register(uint8_t reg_addr, uint8_t *reg_data, uint8_t len) const
 {

libraries/AP_BattMonitor/AP_BattMonitor_TIBQ76952.h

@@ -31,6 +31,10 @@ public:
     // set desired powered state (enabled/disabled) by enabling/disabling discharge FET
     void set_powered_state(bool power_on) override;
 
+    // set battery BMS sleep timeout in seconds
+    // set to zero to disable sleep
+    void set_sleep_timeout(uint16_t timeout_sec) override { sleep_timeout_sec = timeout_sec; }
+
     static const struct AP_Param::GroupInfo var_info[];
 
 protected:
@@ -61,6 +65,9 @@ protected:
     // read charging state (e.g. idle, charging, discharging)
     void read_charging_state();
 
+    // check if the BMS should sleep
+    void check_sleep_timeout();
+
     // read bytes from a register. returns true on success
     bool read_register(uint8_t reg_addr, uint8_t *reg_data, uint8_t len) const;
 
@@ -101,7 +108,7 @@ protected:
     };
 
     // parameters
-    AP_Enum<ConfigUpdateType> cfg_update;
+    AP_Enum<ConfigUpdateType> cfg_update;   // config update (0:disabled, 1:write once, 2:check and update)
 
     // internal variables
     AP_HAL::I2CDevice *dev; // I2C device
@@ -125,6 +132,8 @@ protected:
     HAL_Semaphore accumulate_sem;   // semaphore for accumulate structure
     uint32_t last_read_time_ms;     // timestamp of last read
     bool bms_fault;         // true if BMS reports some kind of failure or fault
+    uint16_t sleep_timeout_sec = 30;    // battery BMS sleep timeout in seconds
+    uint32_t activity_timer_ms; // timestamp of last activity, used to determine if sleep mode
 };
 
 #endif // AP_BATTERY_TIBQ76952_ENABLED