Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-25 01:05:27 +03:00
Code Issues Wiki Activity

Merge pull request #7566 from klutvott123/battery-pt1-filter

Browse source 
Change battery filters from biquad to pt1
This commit is contained in:
Michael Keller 2019-02-13 21:47:24 +13:00 • committed by GitHub
commit dbd4fe9b3d
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
6 changed files with 15 additions and 14 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/common/filter.c
View file

@ -44,7 +44,7 @@ FAST_CODE float nullFilterApply(filter_t *filter, float input)
// PT1 Low Pass filter // 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); float RC = 1 / ( 2 * M_PI_FLOAT * f_cut);
return dT / (RC + dT); return dT / (RC + dT);

2
src/main/common/filter.h
View file

@ -76,7 +76,7 @@ float filterGetNotchQ(float centerFreq, float cutoffFreq);
void laggedMovingAverageInit(laggedMovingAverage_t *filter, uint16_t windowSize, float *buf); void laggedMovingAverageInit(laggedMovingAverage_t *filter, uint16_t windowSize, float *buf);
float laggedMovingAverageUpdate(laggedMovingAverage_t *filter, float input); 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 pt1FilterInit(pt1Filter_t *filter, float k);
void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k); void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k);
float pt1FilterApply(pt1Filter_t *filter, float input); float pt1FilterApply(pt1Filter_t *filter, float input);

1
src/main/common/maths.h
View file

@ -57,6 +57,7 @@
#define Q12 (1 << 12) #define Q12 (1 << 12)
#define HZ_TO_INTERVAL(x) (1.0f / (x))
#define HZ_TO_INTERVAL_US(x) (1000000 / (x)) #define HZ_TO_INTERVAL_US(x) (1000000 / (x))
typedef int32_t fix12_t; typedef int32_t fix12_t;

6
src/main/sensors/current.c
View file

@ -88,7 +88,7 @@ void currentMeterReset(currentMeter_t *meter)
// ADC/Virtual shared // ADC/Virtual shared
// //
static biquadFilter_t adciBatFilter; static pt1Filter_t adciBatFilter;
#ifndef CURRENT_METER_SCALE_DEFAULT #ifndef CURRENT_METER_SCALE_DEFAULT
#define CURRENT_METER_SCALE_DEFAULT 400 // for Allegro ACS758LCB-100U (40mV/A) #define CURRENT_METER_SCALE_DEFAULT 400 // for Allegro ACS758LCB-100U (40mV/A)
@ -141,7 +141,7 @@ currentMeterADCState_t currentMeterADCState;
void currentMeterADCInit(void) void currentMeterADCInit(void)
{ {
memset(&currentMeterADCState, 0, sizeof(currentMeterADCState_t)); memset(&currentMeterADCState, 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) void currentMeterADCRefresh(int32_t lastUpdateAt)
@ -149,7 +149,7 @@ void currentMeterADCRefresh(int32_t lastUpdateAt)
#ifdef USE_ADC #ifdef USE_ADC
const uint16_t iBatSample = adcGetChannel(ADC_CURRENT); const uint16_t iBatSample = adcGetChannel(ADC_CURRENT);
currentMeterADCState.amperageLatest = currentMeterADCToCentiamps(iBatSample); currentMeterADCState.amperageLatest = currentMeterADCToCentiamps(iBatSample);
currentMeterADCState.amperage = currentMeterADCToCentiamps(biquadFilterApply(&adciBatFilter, iBatSample)); currentMeterADCState.amperage = currentMeterADCToCentiamps(pt1FilterApply(&adciBatFilter, iBatSample));
updateCurrentmAhDrawnState(&currentMeterADCState.mahDrawnState, currentMeterADCState.amperageLatest, lastUpdateAt); updateCurrentmAhDrawnState(&currentMeterADCState.mahDrawnState, currentMeterADCState.amperageLatest, lastUpdateAt);
#else #else

12
src/main/sensors/voltage.c
View file

@ -106,7 +106,7 @@ void voltageMeterReset(voltageMeter_t *meter)
typedef struct voltageMeterADCState_s { typedef struct voltageMeterADCState_s {
uint16_t voltageFiltered; // battery voltage in 0.01V steps (filtered) uint16_t voltageFiltered; // battery voltage in 0.01V steps (filtered)
uint16_t voltageUnfiltered; // battery voltage in 0.01V steps (unfiltered) uint16_t voltageUnfiltered; // battery voltage in 0.01V steps (unfiltered)
biquadFilter_t filter; pt1Filter_t filter;
} voltageMeterADCState_t; } voltageMeterADCState_t;
extern voltageMeterADCState_t voltageMeterADCStates[MAX_VOLTAGE_SENSOR_ADC]; extern voltageMeterADCState_t voltageMeterADCStates[MAX_VOLTAGE_SENSOR_ADC];
@ -164,7 +164,7 @@ void voltageMeterADCRefresh(void)
uint8_t channel = voltageMeterAdcChannelMap[i]; uint8_t channel = voltageMeterAdcChannelMap[i];
uint16_t rawSample = adcGetChannel(channel); 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. // always calculate the latest voltage, see getLatestVoltage() which does the calculation on demand.
state->voltageFiltered = voltageAdcToVoltage(filteredSample, config); state->voltageFiltered = voltageAdcToVoltage(filteredSample, config);
@ -194,7 +194,7 @@ void voltageMeterADCInit(void)
voltageMeterADCState_t *state = &voltageMeterADCStates[i]; voltageMeterADCState_t *state = &voltageMeterADCStates[i];
memset(state, 0, sizeof(voltageMeterADCState_t)); 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 { typedef struct voltageMeterESCState_s {
uint16_t voltageFiltered; // battery voltage in 0.01V steps (filtered) uint16_t voltageFiltered; // battery voltage in 0.01V steps (filtered)
uint16_t voltageUnfiltered; // battery voltage in 0.01V steps (unfiltered) uint16_t voltageUnfiltered; // battery voltage in 0.01V steps (unfiltered)
biquadFilter_t filter; pt1Filter_t filter;
} voltageMeterESCState_t; } voltageMeterESCState_t;
static voltageMeterESCState_t voltageMeterESCState; static voltageMeterESCState_t voltageMeterESCState;
@ -218,7 +218,7 @@ void voltageMeterESCInit(void)
{ {
#ifdef USE_ESC_SENSOR #ifdef USE_ESC_SENSOR
memset(&voltageMeterESCState, 0, sizeof(voltageMeterESCState_t)); 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 #endif
} }
@ -228,7 +228,7 @@ void voltageMeterESCRefresh(void)
escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED); escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
if (escData) { if (escData) {
voltageMeterESCState.voltageUnfiltered = escData->dataAge <= ESC_BATTERY_AGE_MAX ? escData->voltage : 0; 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 #endif
} }

6
src/test/unit/common_filter_unittest.cc
View file

@ -41,15 +41,15 @@ TEST(FilterUnittest, TestPt1FilterInit)
TEST(FilterUnittest, TestPt1FilterGain) 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 // 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) TEST(FilterUnittest, TestPt1FilterApply)
{ {
pt1Filter_t filter; pt1Filter_t filter;
pt1FilterInit(&filter, pt1FilterGain(100, 31.25f)); pt1FilterInit(&filter, pt1FilterGain(100.0f, 31.25f));
EXPECT_EQ(0, filter.state); EXPECT_EQ(0, filter.state);
pt1FilterApply(&filter, 1800.0f); pt1FilterApply(&filter, 1800.0f);