Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 06:15:16 +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/maths.h"
#include "common/utils.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 // NULL filter
float nullFilterApply(void *filter, float input) 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) */ /* 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 */ /* compute result */
const float result = filter->b0 * input + filter->b1 * filter->x1 + filter->b2 * filter->x2 - filter->a1 * filter->y1 - filter->a2 * filter->y2; 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 */ /* 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; const float result = filter->b0 * input + filter->d1;
filter->d1 = filter->b1 * input - filter->a1 * result + filter->d2; 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 #define MAX_FIR_DENOISE_WINDOW_SIZE 120
#endif #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 { typedef struct pt1Filter_s {
float state; float state;
float k; float k;
@ -79,8 +74,8 @@ float nullFilterApply(void *filter, float input);
void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate); 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 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); 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 biquadFilterApply(biquadFilter_t *filter, float input);
float biquadFilterApplyDF2(biquadFilter_t *filter, float input);
float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff); float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff);
// not exactly correct, but very very close and much much faster // 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 }, { BOXGOV, "GOVERNOR", 18 },
{ BOXOSD, "OSD SW", 19 }, { BOXOSD, "OSD SW", 19 },
{ BOXTELEMETRY, "TELEMETRY", 20 }, { BOXTELEMETRY, "TELEMETRY", 20 },
{ BOXDYNAMICFILTER, "DYNAMIC FILTER", 21 }, { BOXGTUNE, "GTUNE", 21 },
{ BOXSONAR, "SONAR", 22 }, { BOXSONAR, "SONAR", 22 },
{ BOXSERVO1, "SERVO1", 23 }, { BOXSERVO1, "SERVO1", 23 },
{ BOXSERVO2, "SERVO2", 24 }, { BOXSERVO2, "SERVO2", 24 },
@ -146,6 +146,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOX3DDISABLESWITCH, "DISABLE 3D SWITCH", 29}, { BOX3DDISABLESWITCH, "DISABLE 3D SWITCH", 29},
{ BOXFPVANGLEMIX, "FPV ANGLE MIX", 30}, { BOXFPVANGLEMIX, "FPV ANGLE MIX", 30},
{ BOXBLACKBOXERASE, "BLACKBOX ERASE (>30s)", 31 }, { BOXBLACKBOXERASE, "BLACKBOX ERASE (>30s)", 31 },
{ BOXDYNAMICFILTER, "DYNAMIC FILTER", 32 },
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF } { 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(BOXGOV)) << BOXGOV |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY | 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(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR |
IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM | IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX |

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

@ -43,7 +43,7 @@ typedef enum {
BOXGOV, BOXGOV,
BOXOSD, BOXOSD,
BOXTELEMETRY, BOXTELEMETRY,
BOXDYNAMICFILTER, BOXGTUNE,
BOXSONAR, BOXSONAR,
BOXSERVO1, BOXSERVO1,
BOXSERVO2, BOXSERVO2,
@ -54,6 +54,7 @@ typedef enum {
BOX3DDISABLESWITCH, BOX3DDISABLESWITCH,
BOXFPVANGLEMIX, BOXFPVANGLEMIX,
BOXBLACKBOXERASE, BOXBLACKBOXERASE,
BOXDYNAMICFILTER,
CHECKBOX_ITEM_COUNT CHECKBOX_ITEM_COUNT
} boxId_e; } 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) { if (pidProfile->dterm_notch_hz == 0 || pidProfile->dterm_notch_hz > pidFrequencyNyquist) {
dtermNotchFilterApplyFn = nullFilterApply; dtermNotchFilterApplyFn = nullFilterApply;
} else { } else {
dtermNotchFilterApplyFn = (filterApplyFnPtr)biquadFilterApplyDF2; dtermNotchFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
const float notchQ = filterGetNotchQ(pidProfile->dterm_notch_hz, pidProfile->dterm_notch_cutoff); const float notchQ = filterGetNotchQ(pidProfile->dterm_notch_hz, pidProfile->dterm_notch_cutoff);
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
dtermFilterNotch[axis] = &biquadFilterNotch[axis]; dtermFilterNotch[axis] = &biquadFilterNotch[axis];
@ -194,7 +194,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
} }
break; break;
case FILTER_BIQUAD: case FILTER_BIQUAD:
dtermLpfApplyFn = (filterApplyFnPtr)biquadFilterApplyDF2; dtermLpfApplyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
dtermFilterLpf[axis] = &biquadFilter[axis]; dtermFilterLpf[axis] = &biquadFilter[axis];
biquadFilterInitLPF(dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); 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; servoFilterIsSet = true;
} }
servo[servoIdx] = lrintf(biquadFilterApplyDF2(&servoFilter[servoIdx], (float)servo[servoIdx])); servo[servoIdx] = lrintf(biquadFilterApply(&servoFilter[servoIdx], (float)servo[servoIdx]));
// Sanity check // Sanity check
servo[servoIdx] = constrain(servo[servoIdx], servoParams(servoIdx)->min, servoParams(servoIdx)->max); 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) { if (accLpfCutHz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { 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); const uint16_t iBatSample = adcGetChannel(ADC_CURRENT);
currentMeterADCState.amperageLatest = currentMeterADCToCentiamps(iBatSample); currentMeterADCState.amperageLatest = currentMeterADCToCentiamps(iBatSample);
currentMeterADCState.amperage = currentMeterADCToCentiamps(biquadFilterApplyDF2(&adciBatFilter, iBatSample)); currentMeterADCState.amperage = currentMeterADCToCentiamps(biquadFilterApply(&adciBatFilter, iBatSample));
updateCurrentmAhDrawnState(&currentMeterADCState.mahDrawnState, currentMeterADCState.amperageLatest, lastUpdateAt); 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 if (lpfHz && lpfHz <= gyroFrequencyNyquist) { // Initialisation needs to happen once samplingrate is known
switch (gyroConfig()->gyro_soft_lpf_type) { switch (gyroConfig()->gyro_soft_lpf_type) {
case FILTER_BIQUAD: case FILTER_BIQUAD:
gyroSensor->softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApplyDF2; gyroSensor->softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
gyroSensor->softLpfFilterPtr[axis] = &gyroSensor->softLpfFilter.gyroFilterLpfState[axis]; gyroSensor->softLpfFilterPtr[axis] = &gyroSensor->softLpfFilter.gyroFilterLpfState[axis];
biquadFilterInitLPF(&gyroSensor->softLpfFilter.gyroFilterLpfState[axis], lpfHz, gyro.targetLooptime); 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; gyroSensor->notchFilter1ApplyFn = nullFilterApply;
const uint32_t gyroFrequencyNyquist = (1.0f / (gyro.targetLooptime * 0.000001f)) / 2; // No rounding needed const uint32_t gyroFrequencyNyquist = (1.0f / (gyro.targetLooptime * 0.000001f)) / 2; // No rounding needed
if (notchHz && notchHz <= gyroFrequencyNyquist) { if (notchHz && notchHz <= gyroFrequencyNyquist) {
gyroSensor->notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApplyDF2; gyroSensor->notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply;
const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz); const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz);
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
biquadFilterInit(&gyroSensor->notchFilter1[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH); 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; gyroSensor->notchFilter2ApplyFn = nullFilterApply;
const uint32_t gyroFrequencyNyquist = (1.0f / (gyro.targetLooptime * 0.000001f)) / 2; // No rounding needed const uint32_t gyroFrequencyNyquist = (1.0f / (gyro.targetLooptime * 0.000001f)) / 2; // No rounding needed
if (notchHz && notchHz <= gyroFrequencyNyquist) { if (notchHz && notchHz <= gyroFrequencyNyquist) {
gyroSensor->notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApplyDF2; gyroSensor->notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz); const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz);
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
biquadFilterInit(&gyroSensor->notchFilter2[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH); 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) 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 const float notchQ = filterGetNotchQ(400, 390); //just any init value
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
biquadFilterInit(&gyroSensor->notchFilterDyn[axis], 400, gyro.targetLooptime, notchQ, FILTER_NOTCH); 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_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 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 uint16_t samplingFrequency; // gyro rate
static uint8_t fftBinCount; static uint8_t fftBinCount;
static float fftResolution; // hz per bin 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 // Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window
static float hanningWindow[FFT_WINDOW_SIZE]; static float hanningWindow[FFT_WINDOW_SIZE];
void initHanning() { void initHanning()
{
for (int i = 0; i < FFT_WINDOW_SIZE; i++) { for (int i = 0; i < FFT_WINDOW_SIZE; i++) {
hanningWindow[i] = (0.5 - 0.5 * cosf(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1))); 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 axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
for (int i = 0; i < FFT_WINDOW_SIZE; i++) { for (int i = 0; i < FFT_WINDOW_SIZE; i++) {
gyroData[axis][i] = 0; 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); 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 // initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later
samplingFrequency = 1000000 / targetLooptimeUs; samplingFrequency = 1000000 / targetLooptimeUs;
fftSamplingScale = samplingFrequency / FFT_SAMPLING_RATE; fftSamplingScale = samplingFrequency / FFT_SAMPLING_RATE;
@ -125,18 +131,21 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs) {
} }
// used in OSD // used in OSD
const gyroFftData_t *gyroFftData(int axis) { const gyroFftData_t *gyroFftData(int axis)
{
return &fftResult[axis]; return &fftResult[axis];
} }
bool isDynamicFilterActive(void) { bool isDynamicFilterActive(void)
{
return (IS_RC_MODE_ACTIVE(BOXDYNAMICFILTER) || feature(FEATURE_DYNAMIC_FILTER)); return (IS_RC_MODE_ACTIVE(BOXDYNAMICFILTER) || feature(FEATURE_DYNAMIC_FILTER));
} }
/* /*
* Collect gyro data, to be analysed in gyroDataAnalyseUpdate function * 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()) { if (!isDynamicFilterActive()) {
return; return;
} }
@ -154,7 +163,7 @@ void gyroDataAnalyse(const gyroDev_t *gyroDev, biquadFilter_t *notchFilterDyn) {
//calculate mean value of accumulated samples //calculate mean value of accumulated samples
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
float sample = fftAcc[axis] / fftSamplingScale; float sample = fftAcc[axis] / fftSamplingScale;
sample = biquadFilterApplyDF2(&fftGyroFilter[axis], sample); sample = biquadFilterApply(&fftGyroFilter[axis], sample);
gyroData[axis][fftIdx] = sample; gyroData[axis][fftIdx] = sample;
if (axis == 0) if (axis == 0)
DEBUG_SET(DEBUG_FFT, 2, lrintf(sample * gyroDev->scale)); 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 * 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 axis = 0;
static int step = 0; static int step = 0;
arm_cfft_instance_f32 * Sint = &(fftInstance.Sint); 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); arm_bitreversal_32((uint32_t*) fftData, Sint->bitRevLength, Sint->pBitRevTable);
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
step++; step++;
//break; // fall through
} }
case STEP_STAGE_RFFT_F32: case STEP_STAGE_RFFT_F32:
{ {
@ -240,7 +250,7 @@ void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn) {
arm_cmplx_mag_f32(rfftData, fftData, fftBinCount); arm_cmplx_mag_f32(rfftData, fftData, fftBinCount);
DEBUG_SET(DEBUG_FFT_TIME, 2, micros() - startTime); DEBUG_SET(DEBUG_FFT_TIME, 2, micros() - startTime);
step++; step++;
//break; // fall through
} }
case STEP_CALC_FREQUENCIES: 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 // don't go below the minimal cutoff frequency + 10 and don't jump around too much
float centerFreq; float centerFreq;
centerFreq = constrain(fftMeanIndex * fftResolution, DYN_NOTCH_MIN_CUTOFF + 10, fftMaxFreq); 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); centerFreq = constrain(centerFreq, DYN_NOTCH_MIN_CUTOFF + 10, fftMaxFreq);
fftResult[axis].centerFreq = centerFreq; fftResult[axis].centerFreq = centerFreq;
if (axis == 0) { if (axis == 0) {
@ -291,6 +301,7 @@ void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn) {
axis = (axis + 1) % 3; axis = (axis + 1) % 3;
step++; step++;
// fall through
} }
case STEP_HANNING: case STEP_HANNING:
{ {

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

@ -153,7 +153,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 = biquadFilterApplyDF2(&state->filter, rawSample); uint16_t filteredSample = biquadFilterApply(&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);
@ -210,7 +210,7 @@ void voltageMeterESCRefresh(void)
#ifdef USE_ESC_SENSOR #ifdef USE_ESC_SENSOR
escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED); escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
voltageMeterESCState.voltageUnfiltered = escData->dataAge <= ESC_BATTERY_AGE_MAX ? escData->voltage / 10 : 0; 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 #endif
} }

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

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