From c22c35f2adcc24d44b6881bd46fadc528aa1bbd6 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 1 Jul 2026 14:42:05 +0900 Subject: [PATCH] AP_BattMonitor: TIBQ supports sleep Co-authored-by: Huibean --- libraries/AP_BattMonitor/AP_BattMonitor.cpp | 10 +++++++ libraries/AP_BattMonitor/AP_BattMonitor.h | 5 ++++ .../AP_BattMonitor/AP_BattMonitor_Backend.h | 4 +++ .../AP_BattMonitor_TIBQ76952.cpp | 30 +++++++++++++++++++ .../AP_BattMonitor/AP_BattMonitor_TIBQ76952.h | 11 ++++++- 5 files changed, 59 insertions(+), 1 deletion(-) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index 0601a759da7..61554c8d417 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/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() diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index 1aedb543b5e..ace6e15042e 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/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 diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h index 41a581ef1f0..1db877f7144 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h +++ b/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) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_TIBQ76952.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_TIBQ76952.cpp index fdea9a7c26b..4ad3ae14ddb 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_TIBQ76952.cpp +++ b/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 { diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_TIBQ76952.h b/libraries/AP_BattMonitor/AP_BattMonitor_TIBQ76952.h index f087aba695e..e65247035c9 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_TIBQ76952.h +++ b/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 cfg_update; + AP_Enum 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