diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index 1c9509a54f4c..bb1c3dec6a82 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -49,9 +49,7 @@ using namespace time_literals; Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us) : ModuleParams(parent), - _index(index < 1 || index > 9 ? 1 : index), - _warning(battery_status_s::BATTERY_WARNING_NONE), - _last_timestamp(0) + _index(index < 1 || index > 9 ? 1 : index) { const float expected_filter_dt = static_cast(sample_interval_us) / 1_s; _voltage_filter_v.setParameters(expected_filter_dt, 1.f); @@ -105,8 +103,7 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us) updateParams(); } -void -Battery::reset() +void Battery::reset() { memset(&_battery_status, 0, sizeof(_battery_status)); _battery_status.current_a = -1.f; @@ -121,13 +118,10 @@ Battery::reset() _battery_status.id = (uint8_t) _index; } -void -Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a, - bool connected, int source, int priority, - float throttle_normalized) +void Battery::updateBatteryStatus(const hrt_abstime ×tamp, float voltage_v, float current_a, bool connected, + int source, int priority, float throttle_normalized) { reset(); - _battery_status.timestamp = timestamp; if (!_battery_initialized) { _voltage_filter_v.reset(voltage_v); @@ -169,24 +163,18 @@ Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float curre } } - _battery_status.timestamp = timestamp; - - const bool should_publish = (source == _params.source); - - if (should_publish) { - _battery_status_pub.publish(_battery_status); + if (source == _params.source) { publish(); } } -void -Battery::publish() +void Battery::publish() { + _battery_status.timestamp = hrt_absolute_time(); _battery_status_pub.publish(_battery_status); } -void -Battery::sumDischarged(hrt_abstime timestamp, float current_a) +void Battery::sumDischarged(const hrt_abstime ×tamp, float current_a) { // Not a valid measurement if (current_a < 0.f) { @@ -207,8 +195,7 @@ Battery::sumDischarged(hrt_abstime timestamp, float current_a) _last_timestamp = timestamp; } -void -Battery::estimateRemaining(const float voltage_v, const float current_a, const float throttle) +void Battery::estimateRemaining(const float voltage_v, const float current_a, const float throttle) { // remaining battery capacity based on voltage float cell_voltage = voltage_v / _params.n_cells; @@ -246,8 +233,7 @@ Battery::estimateRemaining(const float voltage_v, const float current_a, const f } } -void -Battery::determineWarning(bool connected) +void Battery::determineWarning(bool connected) { if (connected) { // propagate warning state only if the state is higher, otherwise remain in current warning state @@ -266,8 +252,7 @@ Battery::determineWarning(bool connected) } } -void -Battery::computeScale() +void Battery::computeScale() { const float voltage_range = (_params.v_charged - _params.v_empty); diff --git a/src/lib/battery/battery.h b/src/lib/battery/battery.h index a0cc1fb70360..0b8bd6645e43 100644 --- a/src/lib/battery/battery.h +++ b/src/lib/battery/battery.h @@ -42,17 +42,18 @@ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include #include #include + +#include +#include +#include + +#include +#include #include +#include +#include /** * BatteryBase is a base class for any type of battery. @@ -96,7 +97,7 @@ class Battery : public ModuleParams * @param priority: The brick number -1. The term priority refers to the Vn connection on the LTC4417 * @param throttle_normalized: Throttle of the vehicle, between 0 and 1 */ - void updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a, bool connected, + void updateBatteryStatus(const hrt_abstime ×tamp, float voltage_v, float current_a, bool connected, int source, int priority, float throttle_normalized); /** @@ -126,7 +127,7 @@ class Battery : public ModuleParams param_t v_load_drop_old; param_t r_internal_old; param_t source_old; - } _param_handles; + } _param_handles{}; struct { float v_empty; @@ -149,9 +150,9 @@ class Battery : public ModuleParams float v_load_drop_old; float r_internal_old; int source_old; - } _params; + } _params{}; - battery_status_s _battery_status; + battery_status_s _battery_status{}; const int _index; @@ -192,25 +193,25 @@ class Battery : public ModuleParams } } - bool isFloatEqual(float a, float b) { return fabsf(a - b) > FLT_EPSILON; } + bool isFloatEqual(float a, float b) const { return fabsf(a - b) > FLT_EPSILON; } private: - void sumDischarged(hrt_abstime timestamp, float current_a); + void sumDischarged(const hrt_abstime ×tamp, float current_a); void estimateRemaining(const float voltage_v, const float current_a, const float throttle); void determineWarning(bool connected); void computeScale(); uORB::PublicationMulti _battery_status_pub{ORB_ID(battery_status)}; - bool _battery_initialized = false; + bool _battery_initialized{false}; AlphaFilter _voltage_filter_v; AlphaFilter _current_filter_a; AlphaFilter _throttle_filter; - float _discharged_mah = 0.f; - float _discharged_mah_loop = 0.f; - float _remaining_voltage = -1.f; ///< normalized battery charge level remaining based on voltage - float _remaining = -1.f; ///< normalized battery charge level, selected based on config param - float _scale = 1.f; - uint8_t _warning; - hrt_abstime _last_timestamp; + float _discharged_mah{0.f}; + float _discharged_mah_loop{0.f}; + float _remaining_voltage{-1.f}; ///< normalized battery charge level remaining based on voltage + float _remaining{-1.f}; ///< normalized battery charge level, selected based on config param + float _scale{1.f}; + uint8_t _warning{battery_status_s::BATTERY_WARNING_NONE}; + hrt_abstime _last_timestamp{0}; }; diff --git a/src/modules/battery_status/battery_status.cpp b/src/modules/battery_status/battery_status.cpp index b7df0bae5608..a3aaf1b5b2a5 100644 --- a/src/modules/battery_status/battery_status.cpp +++ b/src/modules/battery_status/battery_status.cpp @@ -57,6 +57,7 @@ #include #include #include +#include #include #include #include @@ -101,7 +102,7 @@ class BatteryStatus : public ModuleBase, public ModuleParams, pub uORB::Subscription _actuator_ctrl_0_sub{ORB_ID(actuator_controls_0)}; /**< attitude controls sub */ uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */ - uORB::Subscription _adc_report_sub{ORB_ID(adc_report)}; + uORB::SubscriptionCallbackWorkItem _adc_report_sub{this, ORB_ID(adc_report)}; static constexpr uint32_t SAMPLE_FREQUENCY_HZ = 100; static constexpr uint32_t SAMPLE_INTERVAL_US = 1_s / SAMPLE_FREQUENCY_HZ; @@ -280,9 +281,7 @@ BatteryStatus::task_spawn(int argc, char *argv[]) bool BatteryStatus::init() { - ScheduleOnInterval(SAMPLE_INTERVAL_US); - - return true; + return _adc_report_sub.registerCallback(); } int BatteryStatus::custom_command(int argc, char *argv[])