diff --git a/boards/ark/fpv/src/board_config.h b/boards/ark/fpv/src/board_config.h index 081dbb56698f..7b3c54183ec9 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 8536f33976b4..b7a6e86953a8 100644 --- a/src/lib/mathlib/math/filter/AlphaFilter.hpp +++ b/src/lib/mathlib/math/filter/AlphaFilter.hpp @@ -62,8 +62,8 @@ class AlphaFilter * * 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 d46e5de29090..54f90c726929 100644 --- a/src/modules/battery_status/analog_battery.cpp +++ b/src/modules/battery_status/analog_battery.cpp @@ -71,16 +71,38 @@ AnalogBattery::AnalogBattery(int index, ModuleParams *parent, const int sample_i snprintf(param_name, sizeof(param_name), "BAT%d_I_OVERWRITE", index); _analog_param_handles.i_overwrite = param_find(param_name); + +#if defined(BOARD_BATTERY_ADC_VOLTAGE_FILTER_S) || defined(BOARD_BATTERY_ADC_CURRENT_FILTER_S) + const float expected_filter_dt = static_cast(sample_interval_us) / 1e6f; +#endif + +#ifdef BOARD_BATTERY_ADC_VOLTAGE_FILTER_S + _voltage_filter.setParameters(expected_filter_dt, BOARD_BATTERY_ADC_VOLTAGE_FILTER_S); +#endif + +#ifdef BOARD_BATTERY_ADC_CURRENT_FILTER_S + _current_filter.setParameters(expected_filter_dt, BOARD_BATTERY_ADC_CURRENT_FILTER_S); +#endif } 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; +#ifdef BOARD_BATTERY_ADC_VOLTAGE_FILTER_S + _voltage_filter.update(fmaxf(voltage_v, 0.f)); + voltage_v = _voltage_filter.getState(); +#endif + +#ifdef BOARD_BATTERY_ADC_CURRENT_FILTER_S + _current_filter.update(fmaxf(current_a, 0.f)); + current_a = _current_filter.getState(); +#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 b532f54d0de4..044e2f03cca8 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,13 @@ class AnalogBattery : public Battery uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; uint8_t _arming_state{0}; +#ifdef BOARD_BATTERY_ADC_VOLTAGE_FILTER_S + AlphaFilter _voltage_filter; +#endif + +#ifdef BOARD_BATTERY_ADC_CURRENT_FILTER_S + AlphaFilter _current_filter; +#endif + void updateTopics(); };