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

code reformatting

Browse source 
rename biquadFilterApplyDF2 back to biquadFilterApply
add new value for dynamic notch mode
fix COLIBRI_RACE/i2c_bst.c
This commit is contained in:
rav 2017-05-13 02:22:38 +02:00
parent cd5307188e
commit a2453d1980
12 changed files with 76 additions and 67 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -24,6 +24,11 @@
#include "common/maths.h"
#include "common/utils.h"
#define M_LN2_FLOAT 0.69314718055994530942f
#define M_PI_FLOAT 3.14159265358979323846f
#define BIQUAD_BANDWIDTH 1.9f /* bandwidth in octaves */
#define BIQUAD_Q 1.0f / sqrtf(2.0f) /* quality factor - butterworth*/
// NULL filter
float nullFilterApply(void *filter, float input)
@ -145,7 +150,7 @@ void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refre
}
/* Computes a biquadFilter_t filter on a sample (slightly less precise than df2 but works in dynamic mode) */
float biquadFilterApply(biquadFilter_t *filter, float input)
float biquadFilterApplyDF1(biquadFilter_t *filter, float input)
{
/* compute result */
const float result = filter->b0 * input + filter->b1 * filter->x1 + filter->b2 * filter->x2 - filter->a1 * filter->y1 - filter->a2 * filter->y2;
@ -162,7 +167,7 @@ float biquadFilterApply(biquadFilter_t *filter, float input)
}
/* Computes a biquadFilter_t filter in direct form 2 on a sample (higher precision but can't handle changes in coefficients */
float biquadFilterApplyDF2(biquadFilter_t *filter, float input)
float biquadFilterApply(biquadFilter_t *filter, float input)
{
const float result = filter->b0 * input + filter->d1;
filter->d1 = filter->b1 * input - filter->a1 * result + filter->d2;

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

@ -23,11 +23,6 @@
#define MAX_FIR_DENOISE_WINDOW_SIZE 120
#endif
#define M_LN2_FLOAT 0.69314718055994530942f
#define M_PI_FLOAT 3.14159265358979323846f
#define BIQUAD_BANDWIDTH 1.9f /* bandwidth in octaves */
#define BIQUAD_Q 1.0f / sqrtf(2.0f) /* quality factor - butterworth*/
typedef struct pt1Filter_s {
float state;
float k;
@ -79,8 +74,8 @@ float nullFilterApply(void *filter, float input);
void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
float biquadFilterApply(biquadFilter_t *filter, float input);
float biquadFilterApplyDF2(biquadFilter_t *filter, float input);
float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff);
// not exactly correct, but very very close and much much faster

5
src/main/fc/fc_msp.c
View file

@ -135,7 +135,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXGOV, "GOVERNOR", 18 },
{ BOXOSD, "OSD SW", 19 },
{ BOXTELEMETRY, "TELEMETRY", 20 },
{ BOXDYNAMICFILTER, "DYNAMIC FILTER", 21 },
{ BOXGTUNE, "GTUNE", 21 },
{ BOXSONAR, "SONAR", 22 },
{ BOXSERVO1, "SERVO1", 23 },
{ BOXSERVO2, "SERVO2", 24 },
@ -146,6 +146,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOX3DDISABLESWITCH, "DISABLE 3D SWITCH", 29},
{ BOXFPVANGLEMIX, "FPV ANGLE MIX", 30},
{ BOXBLACKBOXERASE, "BLACKBOX ERASE (>30s)", 31 },
{ BOXDYNAMICFILTER, "DYNAMIC FILTER", 32 },
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF }
};
@ -422,7 +423,7 @@ static uint32_t packFlightModeFlags(void)
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGOV)) << BOXGOV |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXDYNAMICFILTER)) << BOXDYNAMICFILTER |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGTUNE)) << BOXGTUNE |
IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR |
IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX |

3
src/main/fc/rc_controls.h
View file

@ -43,7 +43,7 @@ typedef enum {
BOXGOV,
BOXOSD,
BOXTELEMETRY,
BOXDYNAMICFILTER,
BOXGTUNE,
BOXSONAR,
BOXSERVO1,
BOXSERVO2,
@ -54,6 +54,7 @@ typedef enum {
BOX3DDISABLESWITCH,
BOXFPVANGLEMIX,
BOXBLACKBOXERASE,
BOXDYNAMICFILTER,
CHECKBOX_ITEM_COUNT
} boxId_e;

4
src/main/flight/pid.c
View file

@ -171,7 +171,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
if (pidProfile->dterm_notch_hz == 0 || pidProfile->dterm_notch_hz > pidFrequencyNyquist) {
dtermNotchFilterApplyFn = nullFilterApply;
} else {
dtermNotchFilterApplyFn = (filterApplyFnPtr)biquadFilterApplyDF2;
dtermNotchFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
const float notchQ = filterGetNotchQ(pidProfile->dterm_notch_hz, pidProfile->dterm_notch_cutoff);
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
dtermFilterNotch[axis] = &biquadFilterNotch[axis];
@ -194,7 +194,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
}
break;
case FILTER_BIQUAD:
dtermLpfApplyFn = (filterApplyFnPtr)biquadFilterApplyDF2;
dtermLpfApplyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
dtermFilterLpf[axis] = &biquadFilter[axis];
biquadFilterInitLPF(dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);

2
src/main/flight/servos.c
View file

@ -514,7 +514,7 @@ static void filterServos(void)
servoFilterIsSet = true;
}
servo[servoIdx] = lrintf(biquadFilterApplyDF2(&servoFilter[servoIdx], (float)servo[servoIdx]));
servo[servoIdx] = lrintf(biquadFilterApply(&servoFilter[servoIdx], (float)servo[servoIdx]));
// Sanity check
servo[servoIdx] = constrain(servo[servoIdx], servoParams(servoIdx)->min, servoParams(servoIdx)->max);
}

2
src/main/sensors/acceleration.c
View file

@ -465,7 +465,7 @@ void accUpdate(rollAndPitchTrims_t *rollAndPitchTrims)
if (accLpfCutHz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
acc.accSmooth[axis] = lrintf(biquadFilterApplyDF2(&accFilter[axis], (float)acc.accSmooth[axis]));
acc.accSmooth[axis] = lrintf(biquadFilterApply(&accFilter[axis], (float)acc.accSmooth[axis]));
}
}

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

@ -135,7 +135,7 @@ void currentMeterADCRefresh(int32_t lastUpdateAt)
{
const uint16_t iBatSample = adcGetChannel(ADC_CURRENT);
currentMeterADCState.amperageLatest = currentMeterADCToCentiamps(iBatSample);
currentMeterADCState.amperage = currentMeterADCToCentiamps(biquadFilterApplyDF2(&adciBatFilter, iBatSample));
currentMeterADCState.amperage = currentMeterADCToCentiamps(biquadFilterApply(&adciBatFilter, iBatSample));
updateCurrentmAhDrawnState(&currentMeterADCState.mahDrawnState, currentMeterADCState.amperageLatest, lastUpdateAt);
}

8
src/main/sensors/gyro.c
View file

@ -369,7 +369,7 @@ void gyroInitFilterLpf(gyroSensor_t *gyroSensor, uint8_t lpfHz)
if (lpfHz && lpfHz <= gyroFrequencyNyquist) { // Initialisation needs to happen once samplingrate is known
switch (gyroConfig()->gyro_soft_lpf_type) {
case FILTER_BIQUAD:
gyroSensor->softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApplyDF2;
gyroSensor->softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = 0; axis < 3; axis++) {
gyroSensor->softLpfFilterPtr[axis] = &gyroSensor->softLpfFilter.gyroFilterLpfState[axis];
biquadFilterInitLPF(&gyroSensor->softLpfFilter.gyroFilterLpfState[axis], lpfHz, gyro.targetLooptime);
@ -399,7 +399,7 @@ void gyroInitFilterNotch1(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t n
gyroSensor->notchFilter1ApplyFn = nullFilterApply;
const uint32_t gyroFrequencyNyquist = (1.0f / (gyro.targetLooptime * 0.000001f)) / 2; // No rounding needed
if (notchHz && notchHz <= gyroFrequencyNyquist) {
gyroSensor->notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApplyDF2;
gyroSensor->notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply;
const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz);
for (int axis = 0; axis < 3; axis++) {
biquadFilterInit(&gyroSensor->notchFilter1[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH);
@ -412,7 +412,7 @@ void gyroInitFilterNotch2(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t n
gyroSensor->notchFilter2ApplyFn = nullFilterApply;
const uint32_t gyroFrequencyNyquist = (1.0f / (gyro.targetLooptime * 0.000001f)) / 2; // No rounding needed
if (notchHz && notchHz <= gyroFrequencyNyquist) {
gyroSensor->notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApplyDF2;
gyroSensor->notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz);
for (int axis = 0; axis < 3; axis++) {
biquadFilterInit(&gyroSensor->notchFilter2[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH);
@ -422,7 +422,7 @@ void gyroInitFilterNotch2(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t n
void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor)
{
gyroSensor->notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApply; // must be this function, not DF2
gyroSensor->notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
const float notchQ = filterGetNotchQ(400, 390); //just any init value
for (int axis = 0; axis < 3; axis++) {
biquadFilterInit(&gyroSensor->notchFilterDyn[axis], 400, gyro.targetLooptime, notchQ, FILTER_NOTCH);

35
src/main/sensors/gyroanalyse.c
View file

@ -57,6 +57,8 @@
#define DYN_NOTCH_MIN_CUTOFF 120 // don't cut too deep into low frequencies
#define DYN_NOTCH_MAX_CUTOFF 200 // don't go above this cutoff (better filtering with "constant" delay at higher center frequencies)
#define BIQUAD_Q 1.0f / sqrtf(2.0f) // quality factor - butterworth
static uint16_t samplingFrequency; // gyro rate
static uint8_t fftBinCount;
static float fftResolution; // hz per bin
@ -84,13 +86,15 @@ static biquadFilter_t fftFreqFilter[3];
// Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window
static float hanningWindow[FFT_WINDOW_SIZE];
void initHanning() {
void initHanning()
{
for (int i = 0; i < FFT_WINDOW_SIZE; i++) {
hanningWindow[i] = (0.5 - 0.5 * cosf(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1)));
}
}
void initGyroData() {
void initGyroData()
{
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
for (int i = 0; i < FFT_WINDOW_SIZE; i++) {
gyroData[axis][i] = 0;
@ -98,11 +102,13 @@ void initGyroData() {
}
}
static inline int fftFreqToBin(int freq) {
static inline int fftFreqToBin(int freq)
{
return ((FFT_WINDOW_SIZE / 2 - 1) * freq) / (fftMaxFreq);
}
void gyroDataAnalyseInit(uint32_t targetLooptimeUs) {
void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
{
// initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later
samplingFrequency = 1000000 / targetLooptimeUs;
fftSamplingScale = samplingFrequency / FFT_SAMPLING_RATE;
@ -125,18 +131,21 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs) {
}
// used in OSD
const gyroFftData_t *gyroFftData(int axis) {
const gyroFftData_t *gyroFftData(int axis)
{
return &fftResult[axis];
}
bool isDynamicFilterActive(void) {
bool isDynamicFilterActive(void)
{
return (IS_RC_MODE_ACTIVE(BOXDYNAMICFILTER) || feature(FEATURE_DYNAMIC_FILTER));
}
/*
* Collect gyro data, to be analysed in gyroDataAnalyseUpdate function
*/
void gyroDataAnalyse(const gyroDev_t *gyroDev, biquadFilter_t *notchFilterDyn) {
void gyroDataAnalyse(const gyroDev_t *gyroDev, biquadFilter_t *notchFilterDyn)
{
if (!isDynamicFilterActive()) {
return;
}
@ -154,7 +163,7 @@ void gyroDataAnalyse(const gyroDev_t *gyroDev, biquadFilter_t *notchFilterDyn) {
//calculate mean value of accumulated samples
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
float sample = fftAcc[axis] / fftSamplingScale;
sample = biquadFilterApplyDF2(&fftGyroFilter[axis], sample);
sample = biquadFilterApply(&fftGyroFilter[axis], sample);
gyroData[axis][fftIdx] = sample;
if (axis == 0)
DEBUG_SET(DEBUG_FFT, 2, lrintf(sample * gyroDev->scale));
@ -188,7 +197,8 @@ typedef enum {
/*
* Analyse last gyro data from the last FFT_WINDOW_SIZE milliseconds
*/
void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn) {
void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn)
{
static int axis = 0;
static int step = 0;
arm_cfft_instance_f32 * Sint = &(fftInstance.Sint);
@ -224,7 +234,7 @@ void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn) {
arm_bitreversal_32((uint32_t*) fftData, Sint->bitRevLength, Sint->pBitRevTable);
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
step++;
//break;
// fall through
}
case STEP_STAGE_RFFT_F32:
{
@ -240,7 +250,7 @@ void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn) {
arm_cmplx_mag_f32(rfftData, fftData, fftBinCount);
DEBUG_SET(DEBUG_FFT_TIME, 2, micros() - startTime);
step++;
//break;
// fall through
}
case STEP_CALC_FREQUENCIES:
{
@ -268,7 +278,7 @@ void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn) {
// don't go below the minimal cutoff frequency + 10 and don't jump around too much
float centerFreq;
centerFreq = constrain(fftMeanIndex * fftResolution, DYN_NOTCH_MIN_CUTOFF + 10, fftMaxFreq);
centerFreq = biquadFilterApplyDF2(&fftFreqFilter[axis], centerFreq);
centerFreq = biquadFilterApply(&fftFreqFilter[axis], centerFreq);
centerFreq = constrain(centerFreq, DYN_NOTCH_MIN_CUTOFF + 10, fftMaxFreq);
fftResult[axis].centerFreq = centerFreq;
if (axis == 0) {
@ -291,6 +301,7 @@ void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn) {
axis = (axis + 1) % 3;
step++;
// fall through
}
case STEP_HANNING:
{

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

@ -153,7 +153,7 @@ void voltageMeterADCRefresh(void)
uint8_t channel = voltageMeterAdcChannelMap[i];
uint16_t rawSample = adcGetChannel(channel);
uint16_t filteredSample = biquadFilterApplyDF2(&state->filter, rawSample);
uint16_t filteredSample = biquadFilterApply(&state->filter, rawSample);
// always calculate the latest voltage, see getLatestVoltage() which does the calculation on demand.
state->voltageFiltered = voltageAdcToVoltage(filteredSample, config);
@ -210,7 +210,7 @@ void voltageMeterESCRefresh(void)
#ifdef USE_ESC_SENSOR
escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
voltageMeterESCState.voltageUnfiltered = escData->dataAge <= ESC_BATTERY_AGE_MAX ? escData->voltage / 10 : 0;
voltageMeterESCState.voltageFiltered = biquadFilterApplyDF2(&voltageMeterESCState.filter, voltageMeterESCState.voltageUnfiltered);
voltageMeterESCState.voltageFiltered = biquadFilterApply(&voltageMeterESCState.filter, voltageMeterESCState.voltageUnfiltered);
#endif
}

62
src/main/target/COLIBRI_RACE/i2c_bst.c
View file

@ -151,38 +151,34 @@ typedef struct box_e {
// FIXME remove ;'s
static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXARM, "ARM", 0 },
{ BOXANGLE, "ANGLE", 1 },
{ BOXHORIZON, "HORIZON", 2 },
{ BOXBARO, "BARO", 3 },
{ BOXANTIGRAVITY, "ANTI GRAVITY", 4 },
{ BOXMAG, "MAG", 5 },
{ BOXHEADFREE, "HEADFREE", 6 },
{ BOXHEADADJ, "HEADADJ", 7 },
{ BOXCAMSTAB, "CAMSTAB", 8 },
{ BOXCAMTRIG, "CAMTRIG", 9 },
{ BOXGPSHOME, "GPS HOME", 10 },
{ BOXGPSHOLD, "GPS HOLD", 11 },
{ BOXPASSTHRU, "PASSTHRU", 12 },
{ BOXBEEPERON, "BEEPER", 13 },
{ BOXLEDMAX, "LEDMAX", 14 },
{ BOXLEDLOW, "LEDLOW", 15 },
{ BOXLLIGHTS, "LLIGHTS", 16 },
{ BOXCALIB, "CALIB", 17 },
{ BOXGOV, "GOVERNOR", 18 },
{ BOXOSD, "OSD SW", 19 },
{ BOXTELEMETRY, "TELEMETRY", 20 },
{ BOXDYNAMICFILTER, "DYNAMIC FILTER", 21 },
{ BOXSONAR, "SONAR", 22 },
{ BOXSERVO1, "SERVO1", 23 },
{ BOXSERVO2, "SERVO2", 24 },
{ BOXSERVO3, "SERVO3", 25 },
{ BOXBLACKBOX, "BLACKBOX", 26 },
{ BOXFAILSAFE, "FAILSAFE", 27 },
{ BOXAIRMODE, "AIR MODE", 28 },
{ BOX3DDISABLESWITCH, "DISABLE 3D SWITCH", 29},
{ BOXFPVANGLEMIX, "FPV ANGLE MIX", 30},
{ BOXBLACKBOXERASE, "BLACKBOX ERASE (>30s)", 31 },
{ BOXARM, "ARM;", 0 },
{ BOXANGLE, "ANGLE;", 1 },
{ BOXHORIZON, "HORIZON;", 2 },
{ BOXBARO, "BARO;", 3 },
//{ BOXVARIO, "VARIO;", 4 },
{ BOXMAG, "MAG;", 5 },
{ BOXHEADFREE, "HEADFREE;", 6 },
{ BOXHEADADJ, "HEADADJ;", 7 },
{ BOXCAMSTAB, "CAMSTAB;", 8 },
{ BOXCAMTRIG, "CAMTRIG;", 9 },
{ BOXGPSHOME, "GPS HOME;", 10 },
{ BOXGPSHOLD, "GPS HOLD;", 11 },
{ BOXPASSTHRU, "PASSTHRU;", 12 },
{ BOXBEEPERON, "BEEPER;", 13 },
{ BOXLEDMAX, "LEDMAX;", 14 },
{ BOXLEDLOW, "LEDLOW;", 15 },
{ BOXLLIGHTS, "LLIGHTS;", 16 },
{ BOXCALIB, "CALIB;", 17 },
{ BOXGOV, "GOVERNOR;", 18 },
{ BOXOSD, "OSD SW;", 19 },
{ BOXTELEMETRY, "TELEMETRY;", 20 },
{ BOXGTUNE, "GTUNE;", 21 },
{ BOXSONAR, "SONAR;", 22 },
{ BOXSERVO1, "SERVO1;", 23 },
{ BOXSERVO2, "SERVO2;", 24 },
{ BOXSERVO3, "SERVO3;", 25 },
{ BOXBLACKBOX, "BLACKBOX;", 26 },
{ BOXFAILSAFE, "FAILSAFE;", 27 },
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF }
};
@ -322,7 +318,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGOV)) << BOXGOV |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXDYNAMICFILTER)) << BOXDYNAMICFILTER |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGTUNE)) << BOXGTUNE |
IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR |
IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX |