diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp index 6b30aee470..3130a8bdf3 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp @@ -256,15 +256,22 @@ void AP_BattMonitor_DroneCAN::read() } } +// Return true if the DroneCAN state of charge should be used. +// Return false if state of charge should be calculated locally by counting mah. +bool AP_BattMonitor_DroneCAN::use_CAN_SoC() const +{ + // a UAVCAN battery monitor may not be able to supply a state of charge. If it can't then + // the user can set the option to use current integration in the backend instead. + // SOC of 127 is used as an invalid SOC flag ie system configuration errors or SOC estimation unavailable + return !(option_is_set(AP_BattMonitor_Params::Options::Ignore_UAVCAN_SoC) || + _mppt.is_detected || + (_soc == 127)); +} + /// capacity_remaining_pct - returns true if the percentage is valid and writes to percentage argument bool AP_BattMonitor_DroneCAN::capacity_remaining_pct(uint8_t &percentage) const { - if ((uint32_t(_params._options.get()) & uint32_t(AP_BattMonitor_Params::Options::Ignore_UAVCAN_SoC)) || - _mppt.is_detected || - _soc == 127) { - // a UAVCAN battery monitor may not be able to supply a state of charge. If it can't then - // the user can set the option to use current integration in the backend instead. - // SOC of 127 is used as an invalid SOC flag ie system configuration errors or SOC estimation unavailable + if (!use_CAN_SoC()) { return AP_BattMonitor_Backend::capacity_remaining_pct(percentage); } @@ -277,6 +284,27 @@ bool AP_BattMonitor_DroneCAN::capacity_remaining_pct(uint8_t &percentage) const return true; } +// reset remaining percentage to given value +bool AP_BattMonitor_DroneCAN::reset_remaining(float percentage) +{ + if (use_CAN_SoC()) { + // Cannot reset external state of charge + return false; + } + + WITH_SEMAPHORE(_sem_battmon); + + if (!AP_BattMonitor_Backend::reset_remaining(percentage)) { + // Base class reset failed + return false; + } + + // Reset interim state that is used internally, this is then copied back to the main state in the read() call + _interim_state.consumed_mah = _state.consumed_mah; + _interim_state.consumed_wh = _state.consumed_wh; + return true; +} + /// get_cycle_count - return true if cycle count can be provided and fills in cycles argument bool AP_BattMonitor_DroneCAN::get_cycle_count(uint16_t &cycles) const { diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h index 9c5c56bb73..543aeb3b6d 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h @@ -54,6 +54,9 @@ public: void mppt_set_powered_state(bool power_on) override; + // reset remaining percentage to given value + bool reset_remaining(float percentage) override; + private: void handle_battery_info(const uavcan_equipment_power_BatteryInfo &msg); void handle_battery_info_aux(const ardupilot_equipment_power_BatteryInfoAux &msg); @@ -79,6 +82,10 @@ private: static const char* mppt_fault_string(const MPPT_FaultFlags fault); #endif + // Return true if the DroneCAN state of charge should be used. + // Return false if state of charge should be calculated locally by counting mah. + bool use_CAN_SoC() const; + AP_BattMonitor::BattMonitor_State _interim_state; BattMonitor_DroneCAN_Type _type;