diff --git a/src/modules/systemlib/battery.cpp b/src/modules/systemlib/battery.cpp index c8a8785470..35e48a0493 100644 --- a/src/modules/systemlib/battery.cpp +++ b/src/modules/systemlib/battery.cpp @@ -51,8 +51,9 @@ Battery::Battery() : _param_low_thr(this, "LOW_THR"), _param_crit_thr(this, "CRIT_THR"), _voltage_filtered_v(-1.0f), - _throttle_filtered(0.0f), _discharged_mah(0.0f), + _remaining_voltage(1.0f), + _remaining_capacity(1.0f), _remaining(1.0f), _warning(battery_status_s::BATTERY_WARNING_NONE), _last_timestamp(0) @@ -84,7 +85,7 @@ Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float curre battery_status->timestamp = timestamp; filterVoltage(voltage_v); sumDischarged(timestamp, current_a); - estimateRemaining(_voltage_filtered_v, throttle_normalized); + estimateRemaining(voltage_v, throttle_normalized); determineWarning(); if (_voltage_filtered_v > 2.1f) { @@ -106,7 +107,7 @@ Battery::filterVoltage(float voltage_v) } // TODO: inspect that filter performance - const float filtered_next = _voltage_filtered_v * 0.999f + voltage_v * 0.001f; + const float filtered_next = _voltage_filtered_v * 0.99f + voltage_v * 0.01f; if (PX4_ISFINITE(filtered_next)) { _voltage_filtered_v = filtered_next; @@ -135,34 +136,45 @@ Battery::sumDischarged(hrt_abstime timestamp, float current_a) void Battery::estimateRemaining(float voltage_v, float throttle_normalized) { - // XXX this time constant needs to become tunable but really, the right fix are smart batteries. - const float filtered_next = _throttle_filtered * 0.97f + fabsf(throttle_normalized) * 0.03f; + // 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)); - if (PX4_ISFINITE(filtered_next)) { - _throttle_filtered = filtered_next; - } - - /* 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() * _throttle_filtered); - /* 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 - */ + // 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 const float voltage_range = (_param_v_full.get() - _param_v_empty.get()); - const float remaining_voltage = (voltage_v - (_param_n_cells.get() * bat_v_empty_dynamic)) + + // remaining battery capacity based on voltage + const float rvoltage = (voltage_v - (_param_n_cells.get() * bat_v_empty_dynamic)) / (_param_n_cells.get() * voltage_range); + const float rvoltage_filt = rvoltage * 0.99f + _remaining_voltage * 0.01f; + if (PX4_ISFINITE(rvoltage_filt)) { + _remaining_voltage = rvoltage_filt; + } + // remaining battery capacity based on used current integrated time + const float rcap = 1.0f - _discharged_mah / _param_capacity.get(); + const float rcap_filt = rcap * 0.99f + _remaining_capacity * 0.01f; + if (PX4_ISFINITE(rcap_filt)) { + _remaining_capacity = rcap_filt; + } + + // limit to sane values + _remaining_voltage = (_remaining_voltage < 0.0f) ? 0.0f : _remaining_voltage; + _remaining_voltage = (_remaining_voltage > 1.0f) ? 1.0f : _remaining_voltage; + + _remaining_capacity = (_remaining_capacity < 0.0f) ? 0.0f : _remaining_capacity; + _remaining_capacity = (_remaining_capacity > 1.0f) ? 1.0f : _remaining_capacity; + + // choose which quantity we're using for final reporting if (_param_capacity.get() > 0.0f) { - /* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */ - _remaining = fminf(remaining_voltage, 1.0f - _discharged_mah / _param_capacity.get()); + // if battery capacity is known, use discharged current for estimate, + // but don't show more than voltage estimate + _remaining = fminf(_remaining_voltage, _remaining_capacity); } else { - /* else use voltage */ - _remaining = remaining_voltage; + // else use voltage + _remaining = _remaining_voltage; } - - /* limit to sane values */ - _remaining = (_remaining < 0.0f) ? 0.0f : _remaining; - _remaining = (_remaining > 1.0f) ? 1.0f : _remaining; } void diff --git a/src/modules/systemlib/battery.h b/src/modules/systemlib/battery.h index 7a9e0759cf..421252bb46 100644 --- a/src/modules/systemlib/battery.h +++ b/src/modules/systemlib/battery.h @@ -105,9 +105,10 @@ private: control::BlockParamFloat _param_crit_thr; float _voltage_filtered_v; - float _throttle_filtered; float _discharged_mah; - float _remaining; + float _remaining_voltage; ///< normalized battery charge level remaining based on voltage + float _remaining_capacity; ///< normalized battery charge level remaining based on capacity + float _remaining; ///< normalized battery charge level, selected based on config param uint8_t _warning; hrt_abstime _last_timestamp; }; diff --git a/src/modules/systemlib/battery_params.c b/src/modules/systemlib/battery_params.c index 5f31419a51..dc8159123c 100644 --- a/src/modules/systemlib/battery_params.c +++ b/src/modules/systemlib/battery_params.c @@ -106,12 +106,12 @@ PARAM_DEFINE_FLOAT(BAT_CRIT_THR, 0.09f); * * @group Battery Calibration * @unit V - * @min 0.0 - * @max 1.5 + * @min 0.07 + * @max 0.5 * @decimal 2 * @increment 0.01 */ -PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f); +PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.25f); /** * Number of cells.