fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 16:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

battery: improve remaining flight time estimation

Browse Source 
makes the remaining flight time estimation based on SoC and it's derivative  s.t. it also works without setting battery capacity
This commit is contained in:
chfriedrich98
2025-03-26 12:11:18 +01:00
parent 917daf379e
commit 2309036f63
3 changed files with 46 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/battery/battery.cpp
+26 -18
View File
@@ -57,6 +57,8 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us,
_current_average_filter_a.setParameters(expected_filter_dt, 50.f);
_ocv_filter_v.setParameters(expected_filter_dt, 1.f);
_cell_voltage_filter_v.setParameters(expected_filter_dt, 1.f);
_soc_derivative_filter.setParameters(expected_filter_dt, 50.f);
_flight_time_filter.setParameters(expected_filter_dt, 50.f);
if (index > 9 || index < 1) {
PX4_ERR("Battery index must be between 1 and 9 (inclusive). Received %d. Defaulting to 1.", index);
@@ -92,6 +94,7 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us,
_param_handles.emergen_thr = param_find("BAT_EMERGEN_THR");
_param_handles.bat_avrg_current = param_find("BAT_AVRG_CURRENT");
_param_handles.bat_avrg_soc = param_find("BAT_AVRG_SOC");
updateParams();
}
@@ -153,7 +156,7 @@ battery_status_s Battery::getBatteryStatus()
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(_state_of_charge);
battery_status.temperature = _temperature_c;
battery_status.cell_count = _params.n_cells;
battery_status.connected = _connected;
@@ -345,10 +348,11 @@ void Battery::computeScale()
}
}
float Battery::computeRemainingTime(float current_a)
float Battery::computeRemainingTime(const float state_of_charge)
{
float time_remaining_s = NAN;
bool reset_current_avg_filter = false;
bool reset_soc_derivative_filter = false;
hrt_abstime timestamp = hrt_absolute_time();
if (_vehicle_status_sub.updated()) {
vehicle_status_s vehicle_status;
@@ -357,7 +361,7 @@ float Battery::computeRemainingTime(float current_a)
_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && !_vehicle_status_is_fw) {
reset_current_avg_filter = true;
reset_soc_derivative_filter = true;
}
_vehicle_status_is_fw = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
@@ -367,28 +371,31 @@ float Battery::computeRemainingTime(float current_a)
_flight_phase_estimation_sub.update();
// reset filter if not feasible, negative or we did a VTOL transition to FW mode
if (!PX4_ISFINITE(_current_average_filter_a.getState()) || _current_average_filter_a.getState() < FLT_EPSILON
|| reset_current_avg_filter) {
_current_average_filter_a.reset(_params.bat_avrg_current);
if (!PX4_ISFINITE(_soc_derivative_filter.getState()) || _soc_derivative_filter.getState() < FLT_EPSILON
|| reset_soc_derivative_filter) {
_soc_derivative_filter.reset(_params.bat_avrg_soc);
}
if (_armed && PX4_ISFINITE(current_a)) {
const float dt = math::constrain(timestamp - _last_flight_time_update, 1_ms, 5000_ms) * 1e-6f;
if (_armed && _state_of_charge < 0.99f && PX4_ISFINITE(dt)) {
// For FW only update when we are in level flight
if (!_vehicle_status_is_fw || ((hrt_absolute_time() - _flight_phase_estimation_sub.get().timestamp) < 2_s
if (!_vehicle_status_is_fw || ((timestamp - _flight_phase_estimation_sub.get().timestamp) < 2_s
&& _flight_phase_estimation_sub.get().flight_phase == flight_phase_estimation_s::FLIGHT_PHASE_LEVEL)) {
// only update with positive numbers
_current_average_filter_a.update(fmaxf(current_a, 0.f));
_soc_derivative_filter.update((_prev_state_of_charge - _state_of_charge) / dt);
}
} else {
_soc_derivative_filter.reset(_params.bat_avrg_soc);
_flight_time_filter.reset(_state_of_charge / fmaxf(_soc_derivative_filter.getState(), FLT_EPSILON));
}
// Remaining time estimation only possible with capacity
if (_params.capacity > 0.f) {
const float remaining_capacity_mah = _state_of_charge * _params.capacity;
const float current_ma = fmaxf(_current_average_filter_a.getState() * 1e3f, FLT_EPSILON);
time_remaining_s = remaining_capacity_mah / current_ma * 3600.f;
}
time_remaining_s = _state_of_charge / fmaxf(_soc_derivative_filter.getState(), FLT_EPSILON);
_flight_time_filter.update(time_remaining_s);
_last_flight_time_update = timestamp;
_prev_state_of_charge = state_of_charge;
return time_remaining_s;
return _flight_time_filter.getState();
}
void Battery::updateParams()
@@ -404,6 +411,7 @@ void Battery::updateParams()
param_get(_param_handles.crit_thr, &_params.crit_thr);
param_get(_param_handles.emergen_thr, &_params.emergen_thr);
param_get(_param_handles.bat_avrg_current, &_params.bat_avrg_current);
param_get(_param_handles.bat_avrg_soc, &_params.bat_avrg_soc);
if (n_cells != _params.n_cells) {
_internal_resistance_initialized = false;
src/lib/battery/battery.h
+7 -1
View File
@@ -124,6 +124,7 @@ protected:
param_t emergen_thr;
param_t source;
param_t bat_avrg_current;
param_t bat_avrg_soc;
} _param_handles{};
struct {
@@ -137,6 +138,7 @@ protected:
float emergen_thr;
int32_t source;
float bat_avrg_current;
float bat_avrg_soc;
} _params{};
const int _index;
@@ -151,7 +153,7 @@ private:
uint8_t determineWarning(float state_of_charge);
uint16_t determineFaults();
void computeScale();
float computeRemainingTime(float current_a);
float computeRemainingTime(const float state_of_charge);
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::SubscriptionData<flight_phase_estimation_s> _flight_phase_estimation_sub{ORB_ID(flight_phase_estimation)};
@@ -166,6 +168,8 @@ private:
float _voltage_v{0.f};
AlphaFilter<float> _ocv_filter_v;
AlphaFilter<float> _cell_voltage_filter_v;
AlphaFilter<float> _soc_derivative_filter;
AlphaFilter<float> _flight_time_filter;
float _current_a{-1};
AlphaFilter<float>
_current_average_filter_a; ///< averaging filter for current. For FW, it is the current in level flight.
@@ -174,12 +178,14 @@ private:
float _discharged_mah_loop{0.f};
float _state_of_charge_volt_based{-1.f}; // [0,1]
float _state_of_charge{-1.f}; // [0,1]
float _prev_state_of_charge{-1.f}; // [0, 1]
float _scale{1.f};
uint8_t _warning{battery_status_s::WARNING_NONE};
hrt_abstime _last_timestamp{0};
bool _armed{false};
bool _vehicle_status_is_fw{false};
hrt_abstime _last_unconnected_timestamp{0};
hrt_abstime _last_flight_time_update{0};
// Internal Resistance estimation
void updateInternalResistanceEstimation(const float voltage_v, const float current_a);
src/lib/battery/module.yaml
+13
View File
@@ -178,3 +178,16 @@ parameters:
decimal: 2
increment: 0.1
default: 15
BAT_AVRG_SOC:
description:
short: Expected average SoC derivative during flight.
long: |
This value is used to initialize the in-flight average SoC derivative,
which in turn is used for estimating remaining flight time and RTL triggering.
type: float
min: 0
max: 100
decimal: 4
increment: 0.0001
default: 0.001