diff --git a/boards/ark/fpv/src/board_config.h b/boards/ark/fpv/src/board_config.h index 081dbb5669..7b3c54183e 100644 --- a/boards/ark/fpv/src/board_config.h +++ b/boards/ark/fpv/src/board_config.h @@ -176,6 +176,9 @@ #define BOARD_BATTERY1_V_DIV (21.0f) // (20k + 1k) / 1k = 21 +#define BOARD_BATTERY_ADC_VOLTAGE_FILTER_S 0.075f +#define BOARD_BATTERY_ADC_CURRENT_FILTER_S 0.125f + #define ADC_SCALED_PAYLOAD_SENSE ADC_SCALED_12V_CHANNEL /* HW has to large of R termination on ADC todo:change when HW value is chosen */ diff --git a/src/lib/mathlib/math/filter/AlphaFilter.hpp b/src/lib/mathlib/math/filter/AlphaFilter.hpp index 8536f33976..b7a6e86953 100644 --- a/src/lib/mathlib/math/filter/AlphaFilter.hpp +++ b/src/lib/mathlib/math/filter/AlphaFilter.hpp @@ -62,8 +62,8 @@ public: * * Both parameters have to be provided in the same units. * - * @param sample_interval interval between two samples - * @param time_constant filter time constant determining convergence + * @param sample_interval interval between two samples in seconds + * @param time_constant filter time constant determining convergence in seconds */ void setParameters(float sample_interval, float time_constant) { diff --git a/src/modules/battery_status/analog_battery.cpp b/src/modules/battery_status/analog_battery.cpp index d46e5de290..8353b5d5d3 100644 --- a/src/modules/battery_status/analog_battery.cpp +++ b/src/modules/battery_status/analog_battery.cpp @@ -76,11 +76,29 @@ AnalogBattery::AnalogBattery(int index, ModuleParams *parent, const int sample_i void AnalogBattery::updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw, float current_raw) { - const float voltage_v = voltage_raw * _analog_params.v_div; + float voltage_v = voltage_raw * _analog_params.v_div; const bool connected = voltage_v > BOARD_ADC_OPEN_CIRCUIT_V && (BOARD_ADC_OPEN_CIRCUIT_V <= BOARD_VALID_UV || is_valid()); float current_a = (current_raw - _analog_params.v_offs_cur) * _analog_params.a_per_v; +#if defined(BOARD_BATTERY_ADC_VOLTAGE_FILTER_S) || defined(BOARD_BATTERY_ADC_CURRENT_FILTER_S) + + if (_last_timestamp == 0) { + _last_timestamp = timestamp; + } + + const float dt = (timestamp - _last_timestamp) / 1e6f; + _last_timestamp = timestamp; +#endif + +#ifdef BOARD_BATTERY_ADC_VOLTAGE_FILTER_S + voltage_v = _voltage_filter.update(fmaxf(voltage_v, 0.f), dt); +#endif + +#ifdef BOARD_BATTERY_ADC_CURRENT_FILTER_S + current_a = _current_filter.update(fmaxf(current_a, 0.f), dt); +#endif + // Overwrite the measured current if current overwrite is defined and vehicle is unarmed if (_analog_params.i_overwrite > 0) { updateTopics(); diff --git a/src/modules/battery_status/analog_battery.h b/src/modules/battery_status/analog_battery.h index b532f54d0d..1678f0aa01 100644 --- a/src/modules/battery_status/analog_battery.h +++ b/src/modules/battery_status/analog_battery.h @@ -34,6 +34,7 @@ #pragma once #include +#include #include #include @@ -96,5 +97,17 @@ private: uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; uint8_t _arming_state{0}; +#if defined(BOARD_BATTERY_ADC_VOLTAGE_FILTER_S) || defined(BOARD_BATTERY_ADC_CURRENT_FILTER_S) + hrt_abstime _last_timestamp {0}; +#endif + +#ifdef BOARD_BATTERY_ADC_VOLTAGE_FILTER_S + AlphaFilter _voltage_filter {BOARD_BATTERY_ADC_VOLTAGE_FILTER_S}; +#endif + +#ifdef BOARD_BATTERY_ADC_CURRENT_FILTER_S + AlphaFilter _current_filter {BOARD_BATTERY_ADC_CURRENT_FILTER_S}; +#endif + void updateTopics(); };