fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 05:07:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

battery: use AlphaFilter class

Browse Source 
Replace the filtered value calculations with calls to the class.
This will allow time abstraction.
This commit is contained in:
Matthias Grob
2020-03-31 19:55:25 +02:00
parent cd8850b43b
commit 4dc9ca69ab
2 changed files with 24 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/battery/battery.cpp
+19 -51
View File
@@ -51,6 +51,10 @@ Battery::Battery(int index, ModuleParams *parent) :
_warning(battery_status_s::BATTERY_WARNING_NONE),
_last_timestamp(0)
{
_voltage_filter_v.setParameters(.01f, 1.f);
_current_filter_a.setParameters(.01f, .5f);
_throttle_filter.setParameters(.01f, 1.f);
if (index > 9 || index < 1) {
PX4_ERR("Battery index must be between 1 and 9 (inclusive). Received %d. Defaulting to 1.", index);
}
@@ -126,24 +130,31 @@ Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float curre
{
reset();
_battery_status.timestamp = timestamp;
filterVoltage(voltage_v);
filterThrottle(throttle_normalized);
filterCurrent(current_a);
if (!_battery_initialized) {
_voltage_filter_v.reset(voltage_v);
_current_filter_a.reset(current_a);
_throttle_filter.reset(throttle_normalized);
}
_voltage_filter_v.update(voltage_v);
_current_filter_a.update(current_a);
_throttle_filter.update(throttle_normalized);
sumDischarged(timestamp, current_a);
estimateRemaining(_voltage_filtered_v, _current_filtered_a, _throttle_filtered);
estimateRemaining(_voltage_filter_v.getState(), _current_filter_a.getState(), _throttle_filter.getState());
computeScale();
if (_battery_initialized) {
determineWarning(connected);
}
if (_voltage_filtered_v > 2.1f) {
if (_voltage_filter_v.getState() > 2.1f) {
_battery_initialized = true;
_battery_status.voltage_v = voltage_v;
_battery_status.voltage_filtered_v = _voltage_filtered_v;
_battery_status.voltage_filtered_v = _voltage_filter_v.getState();
_battery_status.scale = _scale;
_battery_status.current_a = current_a;
_battery_status.current_filtered_a = _current_filtered_a;
_battery_status.current_filtered_a = _current_filter_a.getState();
_battery_status.discharged_mah = _discharged_mah;
_battery_status.warning = _warning;
_battery_status.remaining = _remaining;
@@ -175,49 +186,6 @@ Battery::publish()
orb_publish_auto(ORB_ID(battery_status), &_orb_advert, &_battery_status, &_orb_instance, ORB_PRIO_DEFAULT);
}
void
Battery::filterVoltage(float voltage_v)
{
if (!_battery_initialized) {
_voltage_filtered_v = voltage_v;
}
// TODO: inspect that filter performance
const float filtered_next = _voltage_filtered_v * 0.99f + voltage_v * 0.01f;
if (PX4_ISFINITE(filtered_next)) {
_voltage_filtered_v = filtered_next;
}
}
void
Battery::filterCurrent(float current_a)
{
if (!_battery_initialized) {
_current_filtered_a = current_a;
}
// ADC poll is at 100Hz, this will perform a low pass over approx 500ms
const float filtered_next = _current_filtered_a * 0.98f + current_a * 0.02f;
if (PX4_ISFINITE(filtered_next)) {
_current_filtered_a = filtered_next;
}
}
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
Battery::sumDischarged(hrt_abstime timestamp, float current_a)
{
@@ -241,7 +209,7 @@ Battery::sumDischarged(hrt_abstime timestamp, float current_a)
}
void
Battery::estimateRemaining(float voltage_v, float current_a, float throttle)
Battery::estimateRemaining(const float voltage_v, const float current_a, const float throttle)
{
// remaining battery capacity based on voltage
float cell_voltage = voltage_v / _params.n_cells;
src/lib/battery/battery.h
+5 -7
View File
@@ -53,6 +53,7 @@
#include <px4_platform_common/board_common.h>
#include <math.h>
#include <float.h>
#include <lib/ecl/EKF/AlphaFilter.hpp>
/**
* BatteryBase is a base class for any type of battery.
@@ -212,18 +213,15 @@ protected:
}
private:
void filterVoltage(float voltage_v);
void filterThrottle(float throttle);
void filterCurrent(float current_a);
void sumDischarged(hrt_abstime timestamp, float current_a);
void estimateRemaining(float voltage_v, float current_a, float throttle);
void estimateRemaining(const float voltage_v, const float current_a, const float throttle);
void determineWarning(bool connected);
void computeScale();
bool _battery_initialized = false;
float _voltage_filtered_v = -1.f;
float _throttle_filtered = -1.f;
float _current_filtered_a = -1.f;
AlphaFilter<float> _voltage_filter_v;
AlphaFilter<float> _current_filter_a;
AlphaFilter<float> _throttle_filter;
float _discharged_mah = 0.f;
float _discharged_mah_loop = 0.f;
float _remaining_voltage = -1.f; ///< normalized battery charge level remaining based on voltage