From e70d70468aa9060994e9bcea9f0a315cfef78526 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 1 Dec 2021 18:16:59 +0100 Subject: [PATCH] battery: pass voltage and current by setter --- .../crazyflie/syslink/syslink_main.cpp | 3 +- src/drivers/power_monitor/ina226/ina226.cpp | 24 ++++++---------- src/drivers/power_monitor/ina228/ina228.cpp | 24 ++++++---------- src/drivers/power_monitor/ina238/ina238.cpp | 24 ++++++---------- src/drivers/power_monitor/voxlpm/voxlpm.cpp | 20 ++++++------- src/lib/battery/battery.cpp | 28 +++++++++++++------ src/lib/battery/battery.h | 8 ++++-- src/modules/battery_status/analog_battery.cpp | 4 ++- src/modules/esc_battery/EscBattery.cpp | 7 ++--- .../battery_simulator/BatterySimulator.cpp | 4 ++- 10 files changed, 71 insertions(+), 75 deletions(-) diff --git a/boards/bitcraze/crazyflie/syslink/syslink_main.cpp b/boards/bitcraze/crazyflie/syslink/syslink_main.cpp index cdd2a9269d..bd9460f59b 100644 --- a/boards/bitcraze/crazyflie/syslink/syslink_main.cpp +++ b/boards/bitcraze/crazyflie/syslink/syslink_main.cpp @@ -405,7 +405,8 @@ Syslink::handle_message(syslink_message_t *msg) //memcpy(&iset, &msg->data[5], sizeof(float)); _battery.setConnected(true); - _battery.updateBatteryStatus(t, vbat, -1); + _battery.updateVoltage(vbat); + _battery.updateBatteryStatus(t); // Update battery charge state if (charging) { diff --git a/src/drivers/power_monitor/ina226/ina226.cpp b/src/drivers/power_monitor/ina226/ina226.cpp index b2dd632ea9..6a11300fd5 100644 --- a/src/drivers/power_monitor/ina226/ina226.cpp +++ b/src/drivers/power_monitor/ina226/ina226.cpp @@ -84,11 +84,9 @@ INA226::INA226(const I2CSPIDriverConfig &config, int battery_index) : // We need to publish immediately, to guarantee that the first instance of the driver publishes to uORB instance 0 _battery.setConnected(false); - _battery.updateBatteryStatus( - hrt_absolute_time(), - 0.0, - 0.0 - ); + _battery.updateVoltage(0.f); + _battery.updateCurrent(0.f); + _battery.updateBatteryStatus(hrt_absolute_time()); } INA226::~INA226() @@ -229,11 +227,9 @@ INA226::collect() } _battery.setConnected(success); - _battery.updateBatteryStatus( - hrt_absolute_time(), - (float) _bus_voltage * INA226_VSCALE, - (float) _current * _current_lsb - ); + _battery.updateVoltage(static_cast(_bus_voltage * INA226_VSCALE)); + _battery.updateCurrent(static_cast(_current * _current_lsb)); + _battery.updateBatteryStatus(hrt_absolute_time()); perf_end(_sample_perf); @@ -297,11 +293,9 @@ INA226::RunImpl() } else { _battery.setConnected(false); - _battery.updateBatteryStatus( - hrt_absolute_time(), - 0.0f, - 0.0f - ); + _battery.updateVoltage(0.f); + _battery.updateCurrent(0.f); + _battery.updateBatteryStatus(hrt_absolute_time()); if (init() != PX4_OK) { ScheduleDelayed(INA226_INIT_RETRY_INTERVAL_US); diff --git a/src/drivers/power_monitor/ina228/ina228.cpp b/src/drivers/power_monitor/ina228/ina228.cpp index 1d328cad06..7e31d4eff6 100644 --- a/src/drivers/power_monitor/ina228/ina228.cpp +++ b/src/drivers/power_monitor/ina228/ina228.cpp @@ -86,11 +86,9 @@ INA228::INA228(const I2CSPIDriverConfig &config, int battery_index) : // We need to publish immediately, to guarantee that the first instance of the driver publishes to uORB instance 0 _battery.setConnected(false); - _battery.updateBatteryStatus( - hrt_absolute_time(), - 0.0, - 0.0 - ); + _battery.updateVoltage(0.f); + _battery.updateCurrent(0.f); + _battery.updateBatteryStatus(hrt_absolute_time()); } INA228::~INA228() @@ -310,11 +308,9 @@ INA228::collect() } _battery.setConnected(success); - _battery.updateBatteryStatus( - hrt_absolute_time(), - (float) _bus_voltage * INA228_VSCALE, - (float) _current * _current_lsb - ); + _battery.updateVoltage(static_cast(_bus_voltage * INA228_VSCALE)); + _battery.updateCurrent(static_cast(_current * _current_lsb)); + _battery.updateBatteryStatus(hrt_absolute_time()); perf_end(_sample_perf); @@ -378,11 +374,9 @@ INA228::RunImpl() } else { _battery.setConnected(false); - _battery.updateBatteryStatus( - hrt_absolute_time(), - 0.0f, - 0.0f - ); + _battery.updateVoltage(0.f); + _battery.updateCurrent(0.f); + _battery.updateBatteryStatus(hrt_absolute_time()); if (init() != PX4_OK) { ScheduleDelayed(INA228_INIT_RETRY_INTERVAL_US); diff --git a/src/drivers/power_monitor/ina238/ina238.cpp b/src/drivers/power_monitor/ina238/ina238.cpp index 002f91ba5f..0f241b5cd3 100644 --- a/src/drivers/power_monitor/ina238/ina238.cpp +++ b/src/drivers/power_monitor/ina238/ina238.cpp @@ -69,11 +69,9 @@ INA238::INA238(const I2CSPIDriverConfig &config, int battery_index) : // We need to publish immediately, to guarantee that the first instance of the driver publishes to uORB instance 0 _battery.setConnected(false); - _battery.updateBatteryStatus( - hrt_absolute_time(), - 0.0, - 0.0 - ); + _battery.updateVoltage(0.f); + _battery.updateCurrent(0.f); + _battery.updateBatteryStatus(hrt_absolute_time()); } INA238::~INA238() @@ -197,11 +195,9 @@ int INA238::collect() } _battery.setConnected(success); - _battery.updateBatteryStatus( - hrt_absolute_time(), - (float) bus_voltage * INA238_VSCALE, - (float) current * _current_lsb - ); + _battery.updateVoltage(static_cast(bus_voltage * INA238_VSCALE)); + _battery.updateCurrent(static_cast(current * _current_lsb)); + _battery.updateBatteryStatus(hrt_absolute_time()); perf_end(_sample_perf); @@ -256,11 +252,9 @@ void INA238::RunImpl() } else { _battery.setConnected(false); - _battery.updateBatteryStatus( - hrt_absolute_time(), - 0.0f, - 0.0f - ); + _battery.updateVoltage(0.f); + _battery.updateCurrent(0.f); + _battery.updateBatteryStatus(hrt_absolute_time()); if (init() != PX4_OK) { ScheduleDelayed(INA238_INIT_RETRY_INTERVAL_US); diff --git a/src/drivers/power_monitor/voxlpm/voxlpm.cpp b/src/drivers/power_monitor/voxlpm/voxlpm.cpp index 63aeadd368..aeb471ae15 100644 --- a/src/drivers/power_monitor/voxlpm/voxlpm.cpp +++ b/src/drivers/power_monitor/voxlpm/voxlpm.cpp @@ -72,11 +72,9 @@ VOXLPM::init() if (_ch_type == VOXLPM_CH_TYPE_VBATT) { _battery.setConnected(false); - _battery.updateBatteryStatus( - hrt_absolute_time(), - 0.0, - 0.0 - ); + _battery.updateVoltage(0.f); + _battery.updateCurrent(0.f); + _battery.updateBatteryStatus(hrt_absolute_time()); } /* do I2C init, it will probe the bus for two possible configurations, LTC2946 or INA231 */ @@ -345,9 +343,9 @@ VOXLPM::measure() case VOXLPM_CH_TYPE_VBATT: { _battery.setConnected(true); - _battery.updateBatteryStatus(tnow, - _voltage, - _amperage); + _battery.updateVoltage(_voltage); + _battery.updateCurrent(_amperage); + _battery.updateBatteryStatus(tnow); } // fallthrough @@ -371,9 +369,9 @@ VOXLPM::measure() switch (_ch_type) { case VOXLPM_CH_TYPE_VBATT: { _battery.setConnected(true); - _battery.updateBatteryStatus(tnow, - 0.0, - 0.0); + _battery.updateVoltage(0.f); + _battery.updateCurrent(0.f); + _battery.updateBatteryStatus(tnow); } break; diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index 121d8925e0..e545c0c3b1 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -97,16 +97,26 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us, updateParams(); } -void Battery::updateBatteryStatus(const hrt_abstime ×tamp, float voltage_v, float current_a) +void Battery::updateVoltage(const float voltage_v) +{ + _voltage_v = voltage_v; + _voltage_filter_v.update(voltage_v); +} + +void Battery::updateCurrent(const float current_a) +{ + _current_a = current_a; + _current_filter_a.update(current_a); +} + +void Battery::updateBatteryStatus(const hrt_abstime ×tamp) { if (!_battery_initialized) { - _voltage_filter_v.reset(voltage_v); - _current_filter_a.reset(current_a); + _voltage_filter_v.reset(_voltage_v); + _current_filter_a.reset(_current_a); } - _voltage_filter_v.update(voltage_v); - _current_filter_a.update(current_a); - sumDischarged(timestamp, current_a); + sumDischarged(timestamp, _current_a); estimateStateOfCharge(_voltage_filter_v.getState(), _current_filter_a.getState()); computeScale(); @@ -122,15 +132,15 @@ void Battery::updateBatteryStatus(const hrt_abstime ×tamp, float voltage_v, } battery_status_s battery_status{}; - battery_status.voltage_v = voltage_v; + battery_status.voltage_v = _voltage_v; battery_status.voltage_filtered_v = _voltage_filter_v.getState(); - battery_status.current_a = current_a; + battery_status.current_a = _current_a; battery_status.current_filtered_a = _current_filter_a.getState(); battery_status.current_average_a = _current_average_filter_a.getState(); battery_status.discharged_mah = _discharged_mah; battery_status.remaining = _state_of_charge; battery_status.scale = _scale; - battery_status.time_remaining_s = computeRemainingTime(current_a); + battery_status.time_remaining_s = computeRemainingTime(_current_a); battery_status.temperature = NAN; battery_status.cell_count = _params.n_cells; battery_status.connected = _connected; diff --git a/src/lib/battery/battery.h b/src/lib/battery/battery.h index 4c6626688a..b2bb9c3c4f 100644 --- a/src/lib/battery/battery.h +++ b/src/lib/battery/battery.h @@ -87,15 +87,15 @@ public: void setPriority(const uint8_t priority) { _priority = priority; } void setConnected(const bool connected) { _connected = connected; } + void updateVoltage(const float voltage_v); + void updateCurrent(const float current_a); /** * Update current battery status message. * - * @param voltage_raw: Battery voltage, in Volts - * @param current_raw: Battery current, in Amps * @param timestamp: Time at which the ADC was read (use hrt_absolute_time()) */ - void updateBatteryStatus(const hrt_abstime ×tamp, float voltage_v, float current_a); + void updateBatteryStatus(const hrt_abstime ×tamp); protected: struct { @@ -143,7 +143,9 @@ private: const uint8_t _source{}; uint8_t _priority{0}; bool _battery_initialized{false}; + float _voltage_v{0.f}; AlphaFilter _voltage_filter_v; + float _current_a{-1}; AlphaFilter _current_filter_a; AlphaFilter _current_average_filter_a; AlphaFilter _throttle_filter; diff --git a/src/modules/battery_status/analog_battery.cpp b/src/modules/battery_status/analog_battery.cpp index 877f52153c..46f25e067b 100644 --- a/src/modules/battery_status/analog_battery.cpp +++ b/src/modules/battery_status/analog_battery.cpp @@ -81,7 +81,9 @@ AnalogBattery::updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw, (BOARD_ADC_OPEN_CIRCUIT_V <= BOARD_VALID_UV || is_valid()); Battery::setConnected(connected); - Battery::updateBatteryStatus(timestamp, voltage_v, current_a); + Battery::updateVoltage(voltage_v); + Battery::updateCurrent(current_a); + Battery::updateBatteryStatus(timestamp); } bool AnalogBattery::is_valid() diff --git a/src/modules/esc_battery/EscBattery.cpp b/src/modules/esc_battery/EscBattery.cpp index 3e970c4a79..1cc9e722f1 100644 --- a/src/modules/esc_battery/EscBattery.cpp +++ b/src/modules/esc_battery/EscBattery.cpp @@ -100,10 +100,9 @@ EscBattery::Run() average_voltage_v /= esc_status.esc_count; _battery.setConnected(true); - _battery.updateBatteryStatus( - esc_status.timestamp, - average_voltage_v, - total_current_a); + _battery.updateVoltage(average_voltage_v); + _battery.updateCurrent(total_current_a); + _battery.updateBatteryStatus(esc_status.timestamp); } } diff --git a/src/modules/simulator/battery_simulator/BatterySimulator.cpp b/src/modules/simulator/battery_simulator/BatterySimulator.cpp index 64d03a8bd7..4c50f49218 100644 --- a/src/modules/simulator/battery_simulator/BatterySimulator.cpp +++ b/src/modules/simulator/battery_simulator/BatterySimulator.cpp @@ -100,7 +100,9 @@ void BatterySimulator::Run() vbatt *= _battery.cell_count(); _battery.setConnected(true); - _battery.updateBatteryStatus(now_us, vbatt, ibatt); + _battery.updateVoltage(vbatt); + _battery.updateCurrent(ibatt); + _battery.updateBatteryStatus(now_us); perf_end(_loop_perf); }