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..4b454ad68b44 100644 --- a/src/modules/battery_status/analog_battery.cpp +++ b/src/modules/battery_status/analog_battery.cpp @@ -71,16 +71,48 @@ 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; +#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_filter.update(fmaxf(voltage_v, 0.f), dt); + voltage_v = _voltage_filter.getState(); +#endif + +#ifdef BOARD_BATTERY_ADC_CURRENT_FILTER_S + _current_filter.update(fmaxf(current_a, 0.f), dt); + 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..ce1f7057eb2c 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 @@ class AnalogBattery : public Battery 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; +#endif + +#ifdef BOARD_BATTERY_ADC_CURRENT_FILTER_S + AlphaFilter _current_filter; +#endif + void updateTopics(); };