diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 0e3cf097be..000d04e491 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -301,6 +301,7 @@ private: bool _primary_pwm_device; ///< true if we are the default PWM output bool _lockdown_override; ///< allow to override the safety lockdown + bool _armed; ///< wether the system is armed float _battery_amp_per_volt; ///< current sensor amps/volt float _battery_amp_bias; ///< current sensor bias @@ -537,6 +538,7 @@ PX4IO::PX4IO(device::Device *interface) : _servorail_status{}, _primary_pwm_device(false), _lockdown_override(false), + _armed(false), _battery_amp_per_volt(90.0f / 5.0f), // this matches the 3DR current sensor _battery_amp_bias(0), _battery_mamphour_total(0), @@ -1348,6 +1350,8 @@ PX4IO::io_set_arming_state() clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } + _armed = armed.armed; + if (armed.lockdown && !_lockdown_override) { set |= PX4IO_P_SETUP_ARMING_LOCKDOWN; _lockdown_override = true; @@ -1683,7 +1687,7 @@ PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt) float current_a = ibatt * (3.3f / 4096.0f) * _battery_amp_per_volt; current_a += _battery_amp_bias; - _battery.updateBatteryStatus(timestamp, voltage_v, current_a, _last_throttle, &battery_status); + _battery.updateBatteryStatus(timestamp, voltage_v, current_a, _last_throttle, _armed, &battery_status); /* the announced battery status would conflict with the simulated battery status in HIL */ if (!(_pub_blocked)) { diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index c4bca1eab8..ddb25ece7f 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1705,7 +1705,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) actuator_controls_s ctrl; orb_copy(ORB_ID(actuator_controls_0), _actuator_ctrl_0_sub, &ctrl); _battery.updateBatteryStatus(t, bat_voltage_v, bat_current_a, ctrl.control[actuator_controls_s::INDEX_THROTTLE], - &_battery_status); + _armed, &_battery_status); /* announce the battery status if needed, just publish else */ if (_battery_pub != nullptr) { diff --git a/src/modules/systemlib/battery.cpp b/src/modules/systemlib/battery.cpp index ad57b4cd5f..859de86dd4 100644 --- a/src/modules/systemlib/battery.cpp +++ b/src/modules/systemlib/battery.cpp @@ -71,7 +71,7 @@ Battery::reset(battery_status_s *battery_status) { memset(battery_status, 0, sizeof(*battery_status)); battery_status->current_a = -1.0f; - battery_status->remaining = 0.0f; + battery_status->remaining = 1.0f; battery_status->cell_count = _param_n_cells.get(); // TODO: check if it is sane to reset warning to NONE battery_status->warning = battery_status_s::BATTERY_WARNING_NONE; @@ -79,13 +79,13 @@ Battery::reset(battery_status_s *battery_status) void Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a, float throttle_normalized, - battery_status_s *battery_status) + bool armed, battery_status_s *battery_status) { reset(battery_status); battery_status->timestamp = timestamp; filterVoltage(voltage_v); sumDischarged(timestamp, current_a); - estimateRemaining(voltage_v, throttle_normalized); + estimateRemaining(voltage_v, throttle_normalized, armed); determineWarning(); if (_voltage_filtered_v > 2.1f) { @@ -134,10 +134,13 @@ Battery::sumDischarged(hrt_abstime timestamp, float current_a) } void -Battery::estimateRemaining(float voltage_v, float throttle_normalized) +Battery::estimateRemaining(float voltage_v, float throttle_normalized, bool armed) { + // assume 10% voltage drop of the full drop range with motors idle + const float thr = (armed) ? ((fabsf(throttle_normalized) + 0.1f) / 1.1f) : 0.0f; + // remaining charge estimate based on voltage and internal resistance (drop under load) - const float bat_v_empty_dynamic = _param_v_empty.get() - (_param_v_load_drop.get() * fabsf(throttle_normalized)); + const float bat_v_empty_dynamic = _param_v_empty.get() - (_param_v_load_drop.get() * thr); // the range from full to empty is the same for batteries under load and without load, // since the voltage drop applies to both the full and empty state diff --git a/src/modules/systemlib/battery.h b/src/modules/systemlib/battery.h index 421252bb46..2b10c2df03 100644 --- a/src/modules/systemlib/battery.h +++ b/src/modules/systemlib/battery.h @@ -88,12 +88,12 @@ public: * @param throttle_normalized: throttle from 0 to 1 */ void updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a, float throttle_normalized, - battery_status_s *status); + bool armed, battery_status_s *status); private: void filterVoltage(float voltage_v); void sumDischarged(hrt_abstime timestamp, float current_a); - void estimateRemaining(float voltage_v, float throttle_normalized); + void estimateRemaining(float voltage_v, float throttle_normalized, bool armed); void determineWarning(); control::BlockParamFloat _param_v_empty;