diff --git a/boards/bitcraze/crazyflie/syslink/syslink_main.cpp b/boards/bitcraze/crazyflie/syslink/syslink_main.cpp index 38c6e7a95c..cdd2a9269d 100644 --- a/boards/bitcraze/crazyflie/syslink/syslink_main.cpp +++ b/boards/bitcraze/crazyflie/syslink/syslink_main.cpp @@ -404,7 +404,8 @@ Syslink::handle_message(syslink_message_t *msg) memcpy(&vbat, &msg->data[1], sizeof(float)); //memcpy(&iset, &msg->data[5], sizeof(float)); - _battery.updateBatteryStatus(t, vbat, -1, true); + _battery.setConnected(true); + _battery.updateBatteryStatus(t, vbat, -1); // 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 3a4c919ae1..b2dd632ea9 100644 --- a/src/drivers/power_monitor/ina226/ina226.cpp +++ b/src/drivers/power_monitor/ina226/ina226.cpp @@ -83,11 +83,11 @@ INA226::INA226(const I2CSPIDriverConfig &config, int battery_index) : _power_lsb = 25 * _current_lsb; // 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, - false + 0.0 ); } @@ -228,11 +228,11 @@ INA226::collect() _bus_voltage = _power = _current = _shunt = 0; } + _battery.setConnected(success); _battery.updateBatteryStatus( hrt_absolute_time(), (float) _bus_voltage * INA226_VSCALE, - (float) _current * _current_lsb, - success + (float) _current * _current_lsb ); perf_end(_sample_perf); @@ -296,11 +296,11 @@ INA226::RunImpl() ScheduleDelayed(INA226_CONVERSION_INTERVAL); } else { + _battery.setConnected(false); _battery.updateBatteryStatus( hrt_absolute_time(), 0.0f, - 0.0f, - false + 0.0f ); if (init() != PX4_OK) { diff --git a/src/drivers/power_monitor/ina228/ina228.cpp b/src/drivers/power_monitor/ina228/ina228.cpp index ef6b1ae04c..1d328cad06 100644 --- a/src/drivers/power_monitor/ina228/ina228.cpp +++ b/src/drivers/power_monitor/ina228/ina228.cpp @@ -85,11 +85,11 @@ INA228::INA228(const I2CSPIDriverConfig &config, int battery_index) : _power_lsb = 3.2f * _current_lsb; // 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, - false + 0.0 ); } @@ -309,11 +309,11 @@ INA228::collect() _bus_voltage = _power = _current = _shunt = 0; } + _battery.setConnected(success); _battery.updateBatteryStatus( hrt_absolute_time(), (float) _bus_voltage * INA228_VSCALE, - (float) _current * _current_lsb, - success + (float) _current * _current_lsb ); perf_end(_sample_perf); @@ -377,11 +377,11 @@ INA228::RunImpl() ScheduleDelayed(INA228_CONVERSION_INTERVAL); } else { + _battery.setConnected(false); _battery.updateBatteryStatus( hrt_absolute_time(), 0.0f, - 0.0f, - false + 0.0f ); if (init() != PX4_OK) { diff --git a/src/drivers/power_monitor/ina238/ina238.cpp b/src/drivers/power_monitor/ina238/ina238.cpp index 4f74585624..002f91ba5f 100644 --- a/src/drivers/power_monitor/ina238/ina238.cpp +++ b/src/drivers/power_monitor/ina238/ina238.cpp @@ -68,11 +68,11 @@ INA238::INA238(const I2CSPIDriverConfig &config, int battery_index) : _current_lsb = _max_current / INA238_DN_MAX; // 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, - false + 0.0 ); } @@ -196,11 +196,11 @@ int INA238::collect() bus_voltage = current = 0; } + _battery.setConnected(success); _battery.updateBatteryStatus( hrt_absolute_time(), (float) bus_voltage * INA238_VSCALE, - (float) current * _current_lsb, - success + (float) current * _current_lsb ); perf_end(_sample_perf); @@ -255,11 +255,11 @@ void INA238::RunImpl() ScheduleDelayed(INA238_CONVERSION_INTERVAL); } else { + _battery.setConnected(false); _battery.updateBatteryStatus( hrt_absolute_time(), 0.0f, - 0.0f, - false + 0.0f ); if (init() != PX4_OK) { diff --git a/src/drivers/power_monitor/voxlpm/voxlpm.cpp b/src/drivers/power_monitor/voxlpm/voxlpm.cpp index e06d77fff4..63aeadd368 100644 --- a/src/drivers/power_monitor/voxlpm/voxlpm.cpp +++ b/src/drivers/power_monitor/voxlpm/voxlpm.cpp @@ -71,11 +71,11 @@ VOXLPM::init() int ret = PX4_ERROR; if (_ch_type == VOXLPM_CH_TYPE_VBATT) { + _battery.setConnected(false); _battery.updateBatteryStatus( hrt_absolute_time(), 0.0, - 0.0, - false + 0.0 ); } @@ -343,10 +343,11 @@ VOXLPM::measure() if (ret == PX4_OK) { switch (_ch_type) { case VOXLPM_CH_TYPE_VBATT: { + + _battery.setConnected(true); _battery.updateBatteryStatus(tnow, _voltage, - _amperage, - true); + _amperage); } // fallthrough @@ -369,10 +370,10 @@ VOXLPM::measure() switch (_ch_type) { case VOXLPM_CH_TYPE_VBATT: { + _battery.setConnected(true); _battery.updateBatteryStatus(tnow, 0.0, - 0.0, - true); + 0.0); } break; diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index c5de2b7e15..121d8925e0 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -97,7 +97,7 @@ 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, bool connected) +void Battery::updateBatteryStatus(const hrt_abstime ×tamp, float voltage_v, float current_a) { if (!_battery_initialized) { _voltage_filter_v.reset(voltage_v); @@ -110,7 +110,7 @@ void Battery::updateBatteryStatus(const hrt_abstime ×tamp, float voltage_v, estimateStateOfCharge(_voltage_filter_v.getState(), _current_filter_a.getState()); computeScale(); - if (connected && _battery_initialized) { + if (_connected && _battery_initialized) { _warning = determineWarning(_state_of_charge); } @@ -118,7 +118,7 @@ void Battery::updateBatteryStatus(const hrt_abstime ×tamp, float voltage_v, _battery_initialized = true; } else { - connected = false; + _connected = false; } battery_status_s battery_status{}; @@ -133,7 +133,7 @@ void Battery::updateBatteryStatus(const hrt_abstime ×tamp, float voltage_v, battery_status.time_remaining_s = computeRemainingTime(current_a); battery_status.temperature = NAN; battery_status.cell_count = _params.n_cells; - battery_status.connected = connected; + battery_status.connected = _connected; battery_status.source = _source; battery_status.priority = _priority; battery_status.capacity = _params.capacity > 0.f ? static_cast(_params.capacity) : 0; diff --git a/src/lib/battery/battery.h b/src/lib/battery/battery.h index a4fb60a56f..4c6626688a 100644 --- a/src/lib/battery/battery.h +++ b/src/lib/battery/battery.h @@ -86,6 +86,7 @@ public: float full_cell_voltage() { return _params.v_charged; } void setPriority(const uint8_t priority) { _priority = priority; } + void setConnected(const bool connected) { _connected = connected; } /** * Update current battery status message. @@ -94,7 +95,7 @@ public: * @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, bool connected); + void updateBatteryStatus(const hrt_abstime ×tamp, float voltage_v, float current_a); protected: struct { @@ -138,6 +139,7 @@ private: uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)}; uORB::PublicationMulti _battery_status_pub{ORB_ID(battery_status)}; + bool _connected{false}; const uint8_t _source{}; uint8_t _priority{0}; bool _battery_initialized{false}; diff --git a/src/modules/battery_status/analog_battery.cpp b/src/modules/battery_status/analog_battery.cpp index 8ccd5ef02c..877f52153c 100644 --- a/src/modules/battery_status/analog_battery.cpp +++ b/src/modules/battery_status/analog_battery.cpp @@ -80,8 +80,8 @@ AnalogBattery::updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw, bool connected = voltage_v > BOARD_ADC_OPEN_CIRCUIT_V && (BOARD_ADC_OPEN_CIRCUIT_V <= BOARD_VALID_UV || is_valid()); - - Battery::updateBatteryStatus(timestamp, voltage_v, current_a, connected); + Battery::setConnected(connected); + Battery::updateBatteryStatus(timestamp, voltage_v, current_a); } bool AnalogBattery::is_valid() diff --git a/src/modules/esc_battery/EscBattery.cpp b/src/modules/esc_battery/EscBattery.cpp index 17163a482a..3e970c4a79 100644 --- a/src/modules/esc_battery/EscBattery.cpp +++ b/src/modules/esc_battery/EscBattery.cpp @@ -99,13 +99,11 @@ EscBattery::Run() average_voltage_v /= esc_status.esc_count; - const bool connected = true; - + _battery.setConnected(true); _battery.updateBatteryStatus( esc_status.timestamp, average_voltage_v, - total_current_a, - connected); + total_current_a); } } diff --git a/src/modules/simulator/battery_simulator/BatterySimulator.cpp b/src/modules/simulator/battery_simulator/BatterySimulator.cpp index 8b10b643e3..64d03a8bd7 100644 --- a/src/modules/simulator/battery_simulator/BatterySimulator.cpp +++ b/src/modules/simulator/battery_simulator/BatterySimulator.cpp @@ -99,7 +99,8 @@ 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.setConnected(true); + _battery.updateBatteryStatus(now_us, vbatt, ibatt); perf_end(_loop_perf); }