From b965923c082ec1768641d968512aa4680975d5b4 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 25 Nov 2021 10:53:31 +0100 Subject: [PATCH] battery: fetch throttle value inside of class --- .../bitcraze/crazyflie/syslink/syslink_main.cpp | 2 +- src/drivers/power_monitor/ina226/ina226.cpp | 11 +++-------- src/drivers/power_monitor/ina226/ina226.h | 5 ----- src/drivers/power_monitor/ina228/ina228.cpp | 11 +++-------- src/drivers/power_monitor/ina228/ina228.h | 5 ----- src/drivers/power_monitor/ina238/ina238.cpp | 11 +++-------- src/drivers/power_monitor/ina238/ina238.h | 5 ----- src/drivers/power_monitor/voxlpm/voxlpm.cpp | 11 +++-------- src/drivers/power_monitor/voxlpm/voxlpm.hpp | 4 ---- src/lib/battery/battery.cpp | 17 ++++++++++++----- src/lib/battery/battery.h | 8 +++++--- src/modules/battery_status/analog_battery.cpp | 4 ++-- src/modules/battery_status/analog_battery.h | 3 +-- src/modules/battery_status/battery_status.cpp | 10 ++-------- src/modules/esc_battery/EscBattery.cpp | 6 +----- src/modules/esc_battery/EscBattery.hpp | 4 +--- .../battery_simulator/BatterySimulator.cpp | 2 +- 17 files changed, 38 insertions(+), 81 deletions(-) diff --git a/boards/bitcraze/crazyflie/syslink/syslink_main.cpp b/boards/bitcraze/crazyflie/syslink/syslink_main.cpp index f6993aa8b0..1ea66ca5f9 100644 --- a/boards/bitcraze/crazyflie/syslink/syslink_main.cpp +++ b/boards/bitcraze/crazyflie/syslink/syslink_main.cpp @@ -405,7 +405,7 @@ Syslink::handle_message(syslink_message_t *msg) //memcpy(&iset, &msg->data[5], sizeof(float)); _battery.updateBatteryStatus(t, vbat, -1, true, - battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0, 0); + battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0); // Update battery charge state diff --git a/src/drivers/power_monitor/ina226/ina226.cpp b/src/drivers/power_monitor/ina226/ina226.cpp index 706d452705..2ad6ea4b06 100644 --- a/src/drivers/power_monitor/ina226/ina226.cpp +++ b/src/drivers/power_monitor/ina226/ina226.cpp @@ -89,8 +89,7 @@ INA226::INA226(const I2CSPIDriverConfig &config, int battery_index) : 0.0, false, battery_status_s::BATTERY_SOURCE_POWER_MODULE, - 0, - 0.0 + 0 ); } @@ -231,16 +230,13 @@ INA226::collect() _bus_voltage = _power = _current = _shunt = 0; } - _actuators_sub.copy(&_actuator_controls); - _battery.updateBatteryStatus( hrt_absolute_time(), (float) _bus_voltage * INA226_VSCALE, (float) _current * _current_lsb, success, battery_status_s::BATTERY_SOURCE_POWER_MODULE, - 0, - _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] + 0 ); perf_end(_sample_perf); @@ -310,8 +306,7 @@ INA226::RunImpl() 0.0f, false, battery_status_s::BATTERY_SOURCE_POWER_MODULE, - 0, - 0.0f + 0 ); if (init() != PX4_OK) { diff --git a/src/drivers/power_monitor/ina226/ina226.h b/src/drivers/power_monitor/ina226/ina226.h index 78034a6cf1..a402a079ab 100644 --- a/src/drivers/power_monitor/ina226/ina226.h +++ b/src/drivers/power_monitor/ina226/ina226.h @@ -45,9 +45,7 @@ #include #include #include -#include #include -#include #include #include @@ -202,10 +200,7 @@ private: float _current_lsb{_max_current / DN_MAX}; float _power_lsb{25.0f * _current_lsb}; - actuator_controls_s _actuator_controls{}; - Battery _battery; - uORB::Subscription _actuators_sub{ORB_ID(actuator_controls_0)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; int read(uint8_t address, int16_t &data); diff --git a/src/drivers/power_monitor/ina228/ina228.cpp b/src/drivers/power_monitor/ina228/ina228.cpp index d6540887c0..71d9a19379 100644 --- a/src/drivers/power_monitor/ina228/ina228.cpp +++ b/src/drivers/power_monitor/ina228/ina228.cpp @@ -91,8 +91,7 @@ INA228::INA228(const I2CSPIDriverConfig &config, int battery_index) : 0.0, false, battery_status_s::BATTERY_SOURCE_POWER_MODULE, - 0, - 0.0 + 0 ); } @@ -312,16 +311,13 @@ INA228::collect() _bus_voltage = _power = _current = _shunt = 0; } - _actuators_sub.copy(&_actuator_controls); - _battery.updateBatteryStatus( hrt_absolute_time(), (float) _bus_voltage * INA228_VSCALE, (float) _current * _current_lsb, success, battery_status_s::BATTERY_SOURCE_POWER_MODULE, - 0, - _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] + 0 ); perf_end(_sample_perf); @@ -391,8 +387,7 @@ INA228::RunImpl() 0.0f, false, battery_status_s::BATTERY_SOURCE_POWER_MODULE, - 0, - 0.0f + 0 ); if (init() != PX4_OK) { diff --git a/src/drivers/power_monitor/ina228/ina228.h b/src/drivers/power_monitor/ina228/ina228.h index 5629ab858e..d010d5e4b9 100644 --- a/src/drivers/power_monitor/ina228/ina228.h +++ b/src/drivers/power_monitor/ina228/ina228.h @@ -45,9 +45,7 @@ #include #include #include -#include #include -#include #include #include @@ -352,10 +350,7 @@ private: float _current_lsb{_max_current / DN_MAX}; float _power_lsb{25.0f * _current_lsb}; - actuator_controls_s _actuator_controls{}; - Battery _battery; - uORB::Subscription _actuators_sub{ORB_ID(actuator_controls_0)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; int read(uint8_t address, int16_t &data); diff --git a/src/drivers/power_monitor/ina238/ina238.cpp b/src/drivers/power_monitor/ina238/ina238.cpp index ad37040f66..3d6211521f 100644 --- a/src/drivers/power_monitor/ina238/ina238.cpp +++ b/src/drivers/power_monitor/ina238/ina238.cpp @@ -74,8 +74,7 @@ INA238::INA238(const I2CSPIDriverConfig &config, int battery_index) : 0.0, false, battery_status_s::BATTERY_SOURCE_POWER_MODULE, - 0, - 0.0 + 0 ); } @@ -199,16 +198,13 @@ int INA238::collect() bus_voltage = current = 0; } - _actuators_sub.copy(&_actuator_controls); - _battery.updateBatteryStatus( hrt_absolute_time(), (float) bus_voltage * INA238_VSCALE, (float) current * _current_lsb, success, battery_status_s::BATTERY_SOURCE_POWER_MODULE, - 0, - _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] + 0 ); perf_end(_sample_perf); @@ -269,8 +265,7 @@ void INA238::RunImpl() 0.0f, false, battery_status_s::BATTERY_SOURCE_POWER_MODULE, - 0, - 0.0f + 0 ); if (init() != PX4_OK) { diff --git a/src/drivers/power_monitor/ina238/ina238.h b/src/drivers/power_monitor/ina238/ina238.h index 0de43edb62..a9108c258e 100644 --- a/src/drivers/power_monitor/ina238/ina238.h +++ b/src/drivers/power_monitor/ina238/ina238.h @@ -41,9 +41,7 @@ #include #include #include -#include #include -#include #include #include @@ -338,10 +336,7 @@ private: float _current_lsb; int16_t _range; - actuator_controls_s _actuator_controls{}; - Battery _battery; - uORB::Subscription _actuators_sub{ORB_ID(actuator_controls_0)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; int read(uint8_t address, uint16_t &data); diff --git a/src/drivers/power_monitor/voxlpm/voxlpm.cpp b/src/drivers/power_monitor/voxlpm/voxlpm.cpp index ab4d8ed676..6827446da8 100644 --- a/src/drivers/power_monitor/voxlpm/voxlpm.cpp +++ b/src/drivers/power_monitor/voxlpm/voxlpm.cpp @@ -77,8 +77,7 @@ VOXLPM::init() 0.0, false, battery_status_s::BATTERY_SOURCE_POWER_MODULE, - 0, - 0.0 + 0 ); } @@ -346,15 +345,12 @@ VOXLPM::measure() if (ret == PX4_OK) { switch (_ch_type) { case VOXLPM_CH_TYPE_VBATT: { - _actuators_sub.copy(&_actuator_controls); - _battery.updateBatteryStatus(tnow, _voltage, _amperage, true, battery_status_s::BATTERY_SOURCE_POWER_MODULE, - 0, - _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE]); + 0); } // fallthrough @@ -382,8 +378,7 @@ VOXLPM::measure() 0.0, true, battery_status_s::BATTERY_SOURCE_POWER_MODULE, - 0, - 0.0); + 0); } break; diff --git a/src/drivers/power_monitor/voxlpm/voxlpm.hpp b/src/drivers/power_monitor/voxlpm/voxlpm.hpp index afae6a6f0e..e03326d622 100644 --- a/src/drivers/power_monitor/voxlpm/voxlpm.hpp +++ b/src/drivers/power_monitor/voxlpm/voxlpm.hpp @@ -88,9 +88,7 @@ #include #include -#include #include -#include #include #include #include @@ -267,8 +265,6 @@ private: int16_t _cal{0}; Battery _battery; - uORB::Subscription _actuators_sub{ORB_ID(actuator_controls_0)}; - actuator_controls_s _actuator_controls{}; uint8_t read_reg(uint8_t addr); int read_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len); diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index 79b54f6aec..b1bab046fc 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -97,19 +97,17 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us) } void Battery::updateBatteryStatus(const hrt_abstime ×tamp, float voltage_v, float current_a, bool connected, - int source, int priority, float throttle_normalized) + int source, int priority) { if (!_battery_initialized) { _voltage_filter_v.reset(voltage_v); _current_filter_a.reset(current_a); - _throttle_filter.reset(throttle_normalized); } _voltage_filter_v.update(voltage_v); _current_filter_a.update(current_a); - _throttle_filter.update(throttle_normalized); sumDischarged(timestamp, current_a); - estimateStateOfCharge(_voltage_filter_v.getState(), _current_filter_a.getState(), _throttle_filter.getState()); + estimateStateOfCharge(_voltage_filter_v.getState(), _current_filter_a.getState()); computeScale(); if (connected && _battery_initialized) { @@ -169,7 +167,7 @@ void Battery::sumDischarged(const hrt_abstime ×tamp, float current_a) _last_timestamp = timestamp; } -void Battery::estimateStateOfCharge(const float voltage_v, const float current_a, const float throttle) +void Battery::estimateStateOfCharge(const float voltage_v, const float current_a) { // remaining battery capacity based on voltage float cell_voltage = voltage_v / _params.n_cells; @@ -179,6 +177,15 @@ void Battery::estimateStateOfCharge(const float voltage_v, const float current_a cell_voltage += _params.r_internal * current_a; } else { + actuator_controls_s actuator_controls{}; + _actuator_controls_0_sub.copy(&actuator_controls); + const float throttle = actuator_controls.control[actuator_controls_s::INDEX_THROTTLE]; + _throttle_filter.update(throttle); + + if (!_battery_initialized) { + _throttle_filter.reset(throttle); + } + // assume linear relation between throttle and voltage drop cell_voltage += throttle * _params.v_load_drop; } diff --git a/src/lib/battery/battery.h b/src/lib/battery/battery.h index 402cf1bfb4..41ad4888d5 100644 --- a/src/lib/battery/battery.h +++ b/src/lib/battery/battery.h @@ -54,6 +54,8 @@ #include #include #include +#include +#include #include /** @@ -91,10 +93,9 @@ public: * @param timestamp: Time at which the ADC was read (use hrt_absolute_time()) * @param source: Source type in relation to BAT%d_SOURCE param. * @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(const hrt_abstime ×tamp, float voltage_v, float current_a, bool connected, - int source, int priority, float throttle_normalized); + int source, int priority); protected: struct { @@ -130,11 +131,12 @@ protected: private: void sumDischarged(const hrt_abstime ×tamp, float current_a); - void estimateStateOfCharge(const float voltage_v, const float current_a, const float throttle); + void estimateStateOfCharge(const float voltage_v, const float current_a); uint8_t determineWarning(float state_of_charge); void computeScale(); float computeRemainingTime(float current_a); + uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)}; uORB::PublicationMulti _battery_status_pub{ORB_ID(battery_status)}; bool _battery_initialized{false}; diff --git a/src/modules/battery_status/analog_battery.cpp b/src/modules/battery_status/analog_battery.cpp index ac9fafd0a2..8d10725eb4 100644 --- a/src/modules/battery_status/analog_battery.cpp +++ b/src/modules/battery_status/analog_battery.cpp @@ -71,7 +71,7 @@ AnalogBattery::AnalogBattery(int index, ModuleParams *parent, const int sample_i void AnalogBattery::updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw, float current_raw, - int source, int priority, float throttle_normalized) + int source, int priority) { float voltage_v = voltage_raw * _analog_params.v_div; float current_a = (current_raw - _analog_params.v_offs_cur) * _analog_params.a_per_v; @@ -81,7 +81,7 @@ AnalogBattery::updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw, Battery::updateBatteryStatus(timestamp, voltage_v, current_a, connected, - source, priority, throttle_normalized); + source, priority); } bool AnalogBattery::is_valid() diff --git a/src/modules/battery_status/analog_battery.h b/src/modules/battery_status/analog_battery.h index fd18319f19..f51af72748 100644 --- a/src/modules/battery_status/analog_battery.h +++ b/src/modules/battery_status/analog_battery.h @@ -49,10 +49,9 @@ public: * @param timestamp Time at which the ADC was read (use hrt_absolute_time()) * @param source The source as defined by param BAT%d_SOURCE * @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 updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw, float current_raw, - int source, int priority, float throttle_normalized); + int source, int priority); /** * Whether the ADC channel for the voltage of this battery is valid. diff --git a/src/modules/battery_status/battery_status.cpp b/src/modules/battery_status/battery_status.cpp index 85c5cac863..e7ba2bdef0 100644 --- a/src/modules/battery_status/battery_status.cpp +++ b/src/modules/battery_status/battery_status.cpp @@ -56,10 +56,9 @@ #include #include #include -#include +#include #include #include -#include #include #include #include @@ -100,7 +99,6 @@ public: private: void Run() override; - uORB::Subscription _actuator_ctrl_0_sub{ORB_ID(actuator_controls_0)}; /**< attitude controls sub */ uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; /**< notification of parameter updates */ uORB::SubscriptionCallbackWorkItem _adc_report_sub{this, ORB_ID(adc_report)}; @@ -220,16 +218,12 @@ BatteryStatus::adc_poll() for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) { - actuator_controls_s ctrl{}; - _actuator_ctrl_0_sub.copy(&ctrl); - _analogBatteries[b]->updateBatteryStatusADC( hrt_absolute_time(), bat_voltage_adc_readings[b], bat_current_adc_readings[b], battery_status_s::BATTERY_SOURCE_POWER_MODULE, - b, - ctrl.control[actuator_controls_s::INDEX_THROTTLE] + b ); } } diff --git a/src/modules/esc_battery/EscBattery.cpp b/src/modules/esc_battery/EscBattery.cpp index 9ad04b0c45..9dee8ac49f 100644 --- a/src/modules/esc_battery/EscBattery.cpp +++ b/src/modules/esc_battery/EscBattery.cpp @@ -102,17 +102,13 @@ EscBattery::Run() const bool connected = true; const int priority = 0; - actuator_controls_s ctrl; - _actuator_ctrl_0_sub.copy(&ctrl); - _battery.updateBatteryStatus( esc_status.timestamp, average_voltage_v, total_current_a, connected, battery_status_s::BATTERY_SOURCE_ESCS, - priority, - ctrl.control[actuator_controls_s::INDEX_THROTTLE]); + priority); } } diff --git a/src/modules/esc_battery/EscBattery.hpp b/src/modules/esc_battery/EscBattery.hpp index dc0ae3ac11..2cb6745579 100644 --- a/src/modules/esc_battery/EscBattery.hpp +++ b/src/modules/esc_battery/EscBattery.hpp @@ -40,11 +40,10 @@ #include #include #include -#include +#include #include #include #include -#include #include using namespace time_literals; @@ -72,7 +71,6 @@ private: void parameters_updated(); uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - uORB::Subscription _actuator_ctrl_0_sub{ORB_ID(actuator_controls_0)}; uORB::SubscriptionCallbackWorkItem _esc_status_sub{this, ORB_ID(esc_status)}; static constexpr uint32_t ESC_BATTERY_INTERVAL_US = 20_ms; // assume higher frequency esc feedback than 50Hz diff --git a/src/modules/simulator/battery_simulator/BatterySimulator.cpp b/src/modules/simulator/battery_simulator/BatterySimulator.cpp index 4954696a6d..9a1b0ac11d 100644 --- a/src/modules/simulator/battery_simulator/BatterySimulator.cpp +++ b/src/modules/simulator/battery_simulator/BatterySimulator.cpp @@ -99,7 +99,7 @@ void BatterySimulator::Run() float vbatt = math::gradual(_battery_percentage, 0.f, 1.f, _battery.empty_cell_voltage(), _battery.full_cell_voltage()); vbatt *= _battery.cell_count(); - _battery.updateBatteryStatus(now_us, vbatt, ibatt, true, battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0, 0.f); + _battery.updateBatteryStatus(now_us, vbatt, ibatt, true, battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0); perf_end(_loop_perf); }