From fdc40880d0ab8c8b7d30dacdc646d335fe4908a7 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 1 Dec 2021 18:34:26 +0100 Subject: [PATCH] battery: separate out publishing from updating to allow smart battery drivers to use the battery class and filling in additional information in case it makes sense. --- .../bitcraze/crazyflie/syslink/syslink_main.cpp | 2 +- src/drivers/power_monitor/ina226/ina226.cpp | 6 +++--- src/drivers/power_monitor/ina228/ina228.cpp | 6 +++--- src/drivers/power_monitor/ina238/ina238.cpp | 6 +++--- src/drivers/power_monitor/voxlpm/voxlpm.cpp | 6 +++--- src/lib/battery/battery.cpp | 15 ++++++++++++++- src/lib/battery/battery.h | 10 ++++++++++ src/modules/battery_status/analog_battery.cpp | 2 +- src/modules/esc_battery/EscBattery.cpp | 2 +- .../battery_simulator/BatterySimulator.cpp | 2 +- 10 files changed, 40 insertions(+), 17 deletions(-) diff --git a/boards/bitcraze/crazyflie/syslink/syslink_main.cpp b/boards/bitcraze/crazyflie/syslink/syslink_main.cpp index bd9460f59b..d2faf8d2d9 100644 --- a/boards/bitcraze/crazyflie/syslink/syslink_main.cpp +++ b/boards/bitcraze/crazyflie/syslink/syslink_main.cpp @@ -406,7 +406,7 @@ Syslink::handle_message(syslink_message_t *msg) _battery.setConnected(true); _battery.updateVoltage(vbat); - _battery.updateBatteryStatus(t); + _battery.updateAndPublishBatteryStatus(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 6a11300fd5..956e359b47 100644 --- a/src/drivers/power_monitor/ina226/ina226.cpp +++ b/src/drivers/power_monitor/ina226/ina226.cpp @@ -86,7 +86,7 @@ INA226::INA226(const I2CSPIDriverConfig &config, int battery_index) : _battery.setConnected(false); _battery.updateVoltage(0.f); _battery.updateCurrent(0.f); - _battery.updateBatteryStatus(hrt_absolute_time()); + _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); } INA226::~INA226() @@ -229,7 +229,7 @@ INA226::collect() _battery.setConnected(success); _battery.updateVoltage(static_cast(_bus_voltage * INA226_VSCALE)); _battery.updateCurrent(static_cast(_current * _current_lsb)); - _battery.updateBatteryStatus(hrt_absolute_time()); + _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); perf_end(_sample_perf); @@ -295,7 +295,7 @@ INA226::RunImpl() _battery.setConnected(false); _battery.updateVoltage(0.f); _battery.updateCurrent(0.f); - _battery.updateBatteryStatus(hrt_absolute_time()); + _battery.updateAndPublishBatteryStatus(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 7e31d4eff6..8edd8acd9d 100644 --- a/src/drivers/power_monitor/ina228/ina228.cpp +++ b/src/drivers/power_monitor/ina228/ina228.cpp @@ -88,7 +88,7 @@ INA228::INA228(const I2CSPIDriverConfig &config, int battery_index) : _battery.setConnected(false); _battery.updateVoltage(0.f); _battery.updateCurrent(0.f); - _battery.updateBatteryStatus(hrt_absolute_time()); + _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); } INA228::~INA228() @@ -310,7 +310,7 @@ INA228::collect() _battery.setConnected(success); _battery.updateVoltage(static_cast(_bus_voltage * INA228_VSCALE)); _battery.updateCurrent(static_cast(_current * _current_lsb)); - _battery.updateBatteryStatus(hrt_absolute_time()); + _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); perf_end(_sample_perf); @@ -376,7 +376,7 @@ INA228::RunImpl() _battery.setConnected(false); _battery.updateVoltage(0.f); _battery.updateCurrent(0.f); - _battery.updateBatteryStatus(hrt_absolute_time()); + _battery.updateAndPublishBatteryStatus(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 0f241b5cd3..d3210088b3 100644 --- a/src/drivers/power_monitor/ina238/ina238.cpp +++ b/src/drivers/power_monitor/ina238/ina238.cpp @@ -71,7 +71,7 @@ INA238::INA238(const I2CSPIDriverConfig &config, int battery_index) : _battery.setConnected(false); _battery.updateVoltage(0.f); _battery.updateCurrent(0.f); - _battery.updateBatteryStatus(hrt_absolute_time()); + _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); } INA238::~INA238() @@ -197,7 +197,7 @@ int INA238::collect() _battery.setConnected(success); _battery.updateVoltage(static_cast(bus_voltage * INA238_VSCALE)); _battery.updateCurrent(static_cast(current * _current_lsb)); - _battery.updateBatteryStatus(hrt_absolute_time()); + _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); perf_end(_sample_perf); @@ -254,7 +254,7 @@ void INA238::RunImpl() _battery.setConnected(false); _battery.updateVoltage(0.f); _battery.updateCurrent(0.f); - _battery.updateBatteryStatus(hrt_absolute_time()); + _battery.updateAndPublishBatteryStatus(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 aeb471ae15..91270538b3 100644 --- a/src/drivers/power_monitor/voxlpm/voxlpm.cpp +++ b/src/drivers/power_monitor/voxlpm/voxlpm.cpp @@ -74,7 +74,7 @@ VOXLPM::init() _battery.setConnected(false); _battery.updateVoltage(0.f); _battery.updateCurrent(0.f); - _battery.updateBatteryStatus(hrt_absolute_time()); + _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); } /* do I2C init, it will probe the bus for two possible configurations, LTC2946 or INA231 */ @@ -345,7 +345,7 @@ VOXLPM::measure() _battery.setConnected(true); _battery.updateVoltage(_voltage); _battery.updateCurrent(_amperage); - _battery.updateBatteryStatus(tnow); + _battery.updateAndPublishBatteryStatus(tnow); } // fallthrough @@ -371,7 +371,7 @@ VOXLPM::measure() _battery.setConnected(true); _battery.updateVoltage(0.f); _battery.updateCurrent(0.f); - _battery.updateBatteryStatus(tnow); + _battery.updateAndPublishBatteryStatus(tnow); } break; diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index e545c0c3b1..012a8596c3 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -130,7 +130,10 @@ void Battery::updateBatteryStatus(const hrt_abstime ×tamp) } else { _connected = false; } +} +battery_status_s Battery::getBatteryStatus() +{ battery_status_s battery_status{}; battery_status.voltage_v = _voltage_v; battery_status.voltage_filtered_v = _voltage_filter_v.getState(); @@ -149,13 +152,23 @@ void Battery::updateBatteryStatus(const hrt_abstime ×tamp) battery_status.capacity = _params.capacity > 0.f ? static_cast(_params.capacity) : 0; battery_status.id = static_cast(_index); battery_status.warning = _warning; + battery_status.timestamp = hrt_absolute_time(); + return battery_status; +} +void Battery::publishBatteryStatus(const battery_status_s &battery_status) +{ if (_source == _params.source) { - battery_status.timestamp = hrt_absolute_time(); _battery_status_pub.publish(battery_status); } } +void Battery::updateAndPublishBatteryStatus(const hrt_abstime ×tamp) +{ + updateBatteryStatus(timestamp); + publishBatteryStatus(getBatteryStatus()); +} + void Battery::sumDischarged(const hrt_abstime ×tamp, float current_a) { // Not a valid measurement diff --git a/src/lib/battery/battery.h b/src/lib/battery/battery.h index b2bb9c3c4f..abb28c6745 100644 --- a/src/lib/battery/battery.h +++ b/src/lib/battery/battery.h @@ -97,6 +97,16 @@ public: */ void updateBatteryStatus(const hrt_abstime ×tamp); + battery_status_s getBatteryStatus(); + void publishBatteryStatus(const battery_status_s &battery_status); + + /** + * Convenience function for combined update and publication + * @see updateBatteryStatus() + * @see publishBatteryStatus() + */ + void updateAndPublishBatteryStatus(const hrt_abstime ×tamp); + protected: struct { param_t v_empty; diff --git a/src/modules/battery_status/analog_battery.cpp b/src/modules/battery_status/analog_battery.cpp index 46f25e067b..82a99667c0 100644 --- a/src/modules/battery_status/analog_battery.cpp +++ b/src/modules/battery_status/analog_battery.cpp @@ -83,7 +83,7 @@ AnalogBattery::updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw, Battery::setConnected(connected); Battery::updateVoltage(voltage_v); Battery::updateCurrent(current_a); - Battery::updateBatteryStatus(timestamp); + Battery::updateAndPublishBatteryStatus(timestamp); } bool AnalogBattery::is_valid() diff --git a/src/modules/esc_battery/EscBattery.cpp b/src/modules/esc_battery/EscBattery.cpp index 1cc9e722f1..33795f5f77 100644 --- a/src/modules/esc_battery/EscBattery.cpp +++ b/src/modules/esc_battery/EscBattery.cpp @@ -102,7 +102,7 @@ EscBattery::Run() _battery.setConnected(true); _battery.updateVoltage(average_voltage_v); _battery.updateCurrent(total_current_a); - _battery.updateBatteryStatus(esc_status.timestamp); + _battery.updateAndPublishBatteryStatus(esc_status.timestamp); } } diff --git a/src/modules/simulator/battery_simulator/BatterySimulator.cpp b/src/modules/simulator/battery_simulator/BatterySimulator.cpp index 4c50f49218..7f946b8c6c 100644 --- a/src/modules/simulator/battery_simulator/BatterySimulator.cpp +++ b/src/modules/simulator/battery_simulator/BatterySimulator.cpp @@ -102,7 +102,7 @@ void BatterySimulator::Run() _battery.setConnected(true); _battery.updateVoltage(vbatt); _battery.updateCurrent(ibatt); - _battery.updateBatteryStatus(now_us); + _battery.updateAndPublishBatteryStatus(now_us); perf_end(_loop_perf); }