diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 9b729cd5c3..ef2d624512 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -44,7 +44,7 @@ FAST_CODE float nullFilterApply(filter_t *filter, float input) // PT1 Low Pass filter -float pt1FilterGain(uint16_t f_cut, float dT) +float pt1FilterGain(float f_cut, float dT) { float RC = 1 / ( 2 * M_PI_FLOAT * f_cut); return dT / (RC + dT); diff --git a/src/main/common/filter.h b/src/main/common/filter.h index f4de43d17d..5bc1a045b1 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -76,7 +76,7 @@ float filterGetNotchQ(float centerFreq, float cutoffFreq); void laggedMovingAverageInit(laggedMovingAverage_t *filter, uint16_t windowSize, float *buf); float laggedMovingAverageUpdate(laggedMovingAverage_t *filter, float input); -float pt1FilterGain(uint16_t f_cut, float dT); +float pt1FilterGain(float f_cut, float dT); void pt1FilterInit(pt1Filter_t *filter, float k); void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k); float pt1FilterApply(pt1Filter_t *filter, float input); diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 1a506789b8..1454b69a87 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -57,6 +57,7 @@ #define Q12 (1 << 12) +#define HZ_TO_INTERVAL(x) (1.0f / (x)) #define HZ_TO_INTERVAL_US(x) (1000000 / (x)) typedef int32_t fix12_t; diff --git a/src/main/sensors/current.c b/src/main/sensors/current.c index ae4c245b96..ad6dbcf541 100644 --- a/src/main/sensors/current.c +++ b/src/main/sensors/current.c @@ -88,7 +88,7 @@ void currentMeterReset(currentMeter_t *meter) // ADC/Virtual shared // -static biquadFilter_t adciBatFilter; +static pt1Filter_t adciBatFilter; #ifndef CURRENT_METER_SCALE_DEFAULT #define CURRENT_METER_SCALE_DEFAULT 400 // for Allegro ACS758LCB-100U (40mV/A) @@ -141,7 +141,7 @@ currentMeterADCState_t currentMeterADCState; void currentMeterADCInit(void) { memset(¤tMeterADCState, 0, sizeof(currentMeterADCState_t)); - biquadFilterInitLPF(&adciBatFilter, GET_BATTERY_LPF_FREQUENCY(batteryConfig()->ibatLpfPeriod), HZ_TO_INTERVAL_US(50)); + pt1FilterInit(&adciBatFilter, pt1FilterGain(GET_BATTERY_LPF_FREQUENCY(batteryConfig()->ibatLpfPeriod), HZ_TO_INTERVAL(50))); } void currentMeterADCRefresh(int32_t lastUpdateAt) @@ -149,7 +149,7 @@ void currentMeterADCRefresh(int32_t lastUpdateAt) #ifdef USE_ADC const uint16_t iBatSample = adcGetChannel(ADC_CURRENT); currentMeterADCState.amperageLatest = currentMeterADCToCentiamps(iBatSample); - currentMeterADCState.amperage = currentMeterADCToCentiamps(biquadFilterApply(&adciBatFilter, iBatSample)); + currentMeterADCState.amperage = currentMeterADCToCentiamps(pt1FilterApply(&adciBatFilter, iBatSample)); updateCurrentmAhDrawnState(¤tMeterADCState.mahDrawnState, currentMeterADCState.amperageLatest, lastUpdateAt); #else diff --git a/src/main/sensors/voltage.c b/src/main/sensors/voltage.c index 8baa4669ae..f9a0de560b 100644 --- a/src/main/sensors/voltage.c +++ b/src/main/sensors/voltage.c @@ -106,7 +106,7 @@ void voltageMeterReset(voltageMeter_t *meter) typedef struct voltageMeterADCState_s { uint16_t voltageFiltered; // battery voltage in 0.01V steps (filtered) uint16_t voltageUnfiltered; // battery voltage in 0.01V steps (unfiltered) - biquadFilter_t filter; + pt1Filter_t filter; } voltageMeterADCState_t; extern voltageMeterADCState_t voltageMeterADCStates[MAX_VOLTAGE_SENSOR_ADC]; @@ -164,7 +164,7 @@ void voltageMeterADCRefresh(void) uint8_t channel = voltageMeterAdcChannelMap[i]; uint16_t rawSample = adcGetChannel(channel); - uint16_t filteredSample = biquadFilterApply(&state->filter, rawSample); + uint16_t filteredSample = pt1FilterApply(&state->filter, rawSample); // always calculate the latest voltage, see getLatestVoltage() which does the calculation on demand. state->voltageFiltered = voltageAdcToVoltage(filteredSample, config); @@ -194,7 +194,7 @@ void voltageMeterADCInit(void) voltageMeterADCState_t *state = &voltageMeterADCStates[i]; memset(state, 0, sizeof(voltageMeterADCState_t)); - biquadFilterInitLPF(&state->filter, GET_BATTERY_LPF_FREQUENCY(batteryConfig()->vbatLpfPeriod), HZ_TO_INTERVAL_US(50)); + pt1FilterInit(&state->filter, pt1FilterGain(GET_BATTERY_LPF_FREQUENCY(batteryConfig()->vbatLpfPeriod), HZ_TO_INTERVAL(50))); } } @@ -206,7 +206,7 @@ void voltageMeterADCInit(void) typedef struct voltageMeterESCState_s { uint16_t voltageFiltered; // battery voltage in 0.01V steps (filtered) uint16_t voltageUnfiltered; // battery voltage in 0.01V steps (unfiltered) - biquadFilter_t filter; + pt1Filter_t filter; } voltageMeterESCState_t; static voltageMeterESCState_t voltageMeterESCState; @@ -218,7 +218,7 @@ void voltageMeterESCInit(void) { #ifdef USE_ESC_SENSOR memset(&voltageMeterESCState, 0, sizeof(voltageMeterESCState_t)); - biquadFilterInitLPF(&voltageMeterESCState.filter, GET_BATTERY_LPF_FREQUENCY(batteryConfig()->vbatLpfPeriod), HZ_TO_INTERVAL_US(50)); + pt1FilterInit(&voltageMeterESCState.filter, pt1FilterGain(GET_BATTERY_LPF_FREQUENCY(batteryConfig()->vbatLpfPeriod), HZ_TO_INTERVAL(50))); #endif } @@ -228,7 +228,7 @@ void voltageMeterESCRefresh(void) escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED); if (escData) { voltageMeterESCState.voltageUnfiltered = escData->dataAge <= ESC_BATTERY_AGE_MAX ? escData->voltage : 0; - voltageMeterESCState.voltageFiltered = biquadFilterApply(&voltageMeterESCState.filter, voltageMeterESCState.voltageUnfiltered); + voltageMeterESCState.voltageFiltered = pt1FilterApply(&voltageMeterESCState.filter, voltageMeterESCState.voltageUnfiltered); } #endif } diff --git a/src/test/unit/common_filter_unittest.cc b/src/test/unit/common_filter_unittest.cc index e8abf37965..08d6762b84 100644 --- a/src/test/unit/common_filter_unittest.cc +++ b/src/test/unit/common_filter_unittest.cc @@ -41,15 +41,15 @@ TEST(FilterUnittest, TestPt1FilterInit) TEST(FilterUnittest, TestPt1FilterGain) { - EXPECT_FLOAT_EQ(0.999949, pt1FilterGain(100, 31.25f)); + EXPECT_FLOAT_EQ(0.999949, pt1FilterGain(100.0f, 31.25f)); // handle cases over uint8_t boundary - EXPECT_FLOAT_EQ(0.99998301, pt1FilterGain(300, 31.25f)); + EXPECT_FLOAT_EQ(0.99998301, pt1FilterGain(300.0f, 31.25f)); } TEST(FilterUnittest, TestPt1FilterApply) { pt1Filter_t filter; - pt1FilterInit(&filter, pt1FilterGain(100, 31.25f)); + pt1FilterInit(&filter, pt1FilterGain(100.0f, 31.25f)); EXPECT_EQ(0, filter.state); pt1FilterApply(&filter, 1800.0f);