yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

Use filtered throttle for battery estimation instead of raw.

Browse Source
This commit is contained in:
nanthony21
2018-03-10 19:09:56 -06:00
committed by Matthias Grob
parent 036d920070
commit d3e9ee88a0
2 changed files with 18 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
src/modules/systemlib/battery.cpp
View File

@@ -71,9 +71,10 @@ Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float curre
reset(battery_status); reset(battery_status);
battery_status->timestamp = timestamp; battery_status->timestamp = timestamp;
filterVoltage(voltage_v); filterVoltage(voltage_v);
filterThrottle(throttle_normalized);
filterCurrent(current_a); filterCurrent(current_a);
sumDischarged(timestamp, current_a); sumDischarged(timestamp, current_a);
estimateRemaining(_voltage_filtered_v, _current_filtered_a, throttle_normalized, armed); estimateRemaining(_voltage_filtered_v, _current_filtered_a, _throttle_filtered, armed);
computeScale(); computeScale();
if (_battery_initialized) { if (_battery_initialized) {
@@ -126,6 +127,17 @@ Battery::filterCurrent(float current_a)
} }
} }
void Battery::filterThrottle(float throttle){
if (!_battery_initialized) {
_throttle_filtered = throttle;
}
const float filtered_next = _throttle_filtered * 0.99f + throttle * 0.01f;
if (PX4_ISFINITE(filtered_next)){
_throttle_filtered = filtered_next;
}
}
void void
Battery::sumDischarged(hrt_abstime timestamp, float current_a) Battery::sumDischarged(hrt_abstime timestamp, float current_a)
@@ -150,7 +162,7 @@ Battery::sumDischarged(hrt_abstime timestamp, float current_a)
} }
void void
Battery::estimateRemaining(float voltage_v, float current_a, float throttle_normalized, bool armed) Battery::estimateRemaining(float voltage_v, float current_a, float throttle, bool armed)
{ {
// remaining battery capacity based on voltage // remaining battery capacity based on voltage
float cell_voltage = voltage_v / _n_cells.get(); float cell_voltage = voltage_v / _n_cells.get();
@@ -161,7 +173,7 @@ Battery::estimateRemaining(float voltage_v, float current_a, float throttle_norm
} else { } else {
// assume linear relation between throttle and voltage drop // assume linear relation between throttle and voltage drop
cell_voltage += throttle_normalized * _v_load_drop.get(); cell_voltage += throttle * _v_load_drop.get();
} }
_remaining_voltage = math::gradual(cell_voltage, _v_empty.get(), _v_charged.get(), 0.f, 1.f); _remaining_voltage = math::gradual(cell_voltage, _v_empty.get(), _v_charged.get(), 0.f, 1.f);

4
src/modules/systemlib/battery.h
View File

@@ -88,9 +88,10 @@ public:
private: private:
void filterVoltage(float voltage_v); void filterVoltage(float voltage_v);
void filterThrottle(float throttle);
void filterCurrent(float current_a); void filterCurrent(float current_a);
void sumDischarged(hrt_abstime timestamp, float current_a); void sumDischarged(hrt_abstime timestamp, float current_a);
void estimateRemaining(float voltage_v, float current_a, float throttle_normalized, bool armed); void estimateRemaining(float voltage_v, float current_a, float throttle, bool armed);
void determineWarning(bool connected); void determineWarning(bool connected);
void computeScale(); void computeScale();
@@ -108,6 +109,7 @@ private:
bool _battery_initialized = false; bool _battery_initialized = false;
float _voltage_filtered_v = -1.f; float _voltage_filtered_v = -1.f;
float _throttle_filtered = -1.f;
float _current_filtered_a = -1.f; float _current_filtered_a = -1.f;
float _discharged_mah = 0.f; float _discharged_mah = 0.f;
float _discharged_mah_loop = 0.f; float _discharged_mah_loop = 0.f;