diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 873c750320..1946ba4381 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -150,7 +150,7 @@ static const char * const featureNames[] = { "SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM", "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "OSD", "UNUSED", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE", - "SDCARD", "VTX", "RX_SPI", "SOFTSPI", "ESC_SENSOR", "ANTI_GRAVITY", NULL + "SDCARD", "VTX", "RX_SPI", "SOFTSPI", "ESC_SENSOR", "ANTI_GRAVITY", "DYNAMIC_FILTER", NULL }; // sync this with rxFailsafeChannelMode_e diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 7ed9931c88..eb585addaa 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -204,7 +204,10 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = { "STACK", "ESC_SENSOR_RPM", "ESC_SENSOR_TMP", - "ALTITUDE" + "ALTITUDE", + "FFT", + "FFT_TIME", + "FFT_FREQ" }; #ifdef OSD diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index e994472858..b66d2dcfd6 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -96,15 +96,14 @@ typedef struct gyroSensor_s { biquadFilter_t notchFilter1[XYZ_AXIS_COUNT]; filterApplyFnPtr notchFilter2ApplyFn; biquadFilter_t notchFilter2[XYZ_AXIS_COUNT]; + filterApplyFnPtr notchFilterDynApplyFn; + biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT]; } gyroSensor_t; static gyroSensor_t gyroSensor0; static void gyroInitSensorFilters(gyroSensor_t *gyroSensor); -static filterApplyFnPtr notchFilterDynApplyFn; -biquadFilter_t *notchFilterDyn[3]; - #define DEBUG_GYRO_CALIBRATION 3 #ifdef STM32F10X @@ -410,7 +409,6 @@ void gyroInitFilterNotch1(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t n void gyroInitFilterNotch2(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t notchCutoffHz) { - gyroSensor->notchFilter2ApplyFn = nullFilterApply; const uint32_t gyroFrequencyNyquist = (1.0f / (gyro.targetLooptime * 0.000001f)) / 2; // No rounding needed if (notchHz && notchHz <= gyroFrequencyNyquist) { @@ -422,30 +420,26 @@ 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 + 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); + } +} + static void gyroInitSensorFilters(gyroSensor_t *gyroSensor) { gyroInitFilterLpf(gyroSensor, gyroConfig()->gyro_soft_lpf_hz); gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1); gyroInitFilterNotch2(gyroSensor, gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2); -} - -void gyroInitFilterDynamicNotch() -{ - static biquadFilter_t gyroFilterNotch[XYZ_AXIS_COUNT]; - - notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApply; //must be this function, do not change - - const float notchQ = filterGetNotchQ(400, 390); //just any init value - for (int axis = 0; axis < 3; axis++) { - notchFilterDyn[axis] = &gyroFilterNotch[axis]; - biquadFilterInit(notchFilterDyn[axis], 400, gyro.targetLooptime, notchQ, FILTER_NOTCH); - } + gyroInitFilterDynamicNotch(gyroSensor); } void gyroInitFilters(void) { gyroInitSensorFilters(&gyroSensor0); - gyroInitFilterDynamicNotch(); } bool isGyroSensorCalibrationComplete(const gyroSensor_t *gyroSensor) @@ -545,6 +539,10 @@ void gyroUpdateSensor(gyroSensor_t *gyroSensor) return; } +#ifdef USE_GYRO_DATA_ANALYSE + gyroDataAnalyse(&gyroSensor->gyroDev, gyroSensor->notchFilterDyn); +#endif + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { // scale gyro output to degrees per second float gyroADCf = (float)gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale; @@ -555,7 +553,7 @@ void gyroUpdateSensor(gyroSensor_t *gyroSensor) DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf)); // store raw data if (isDynamicFilterActive()) - gyroADCf = notchFilterDynApplyFn(notchFilterDyn[axis], gyroADCf); + gyroADCf = gyroSensor->notchFilterDynApplyFn(&gyroSensor->notchFilterDyn[axis], gyroADCf); if (axis == 0) DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); // store data after dynamic notch @@ -572,10 +570,6 @@ void gyroUpdateSensor(gyroSensor_t *gyroSensor) gyro.gyroADCf[axis] = gyroADCf; } - -#ifdef USE_GYRO_DATA_ANALYSE - gyroDataAnalyse(&gyroSensor->gyroDev, &gyro); -#endif } void gyroUpdate(void) diff --git a/src/main/sensors/gyroanalyse.c b/src/main/sensors/gyroanalyse.c index ed8a66d391..bfc05c96d0 100644 --- a/src/main/sensors/gyroanalyse.c +++ b/src/main/sensors/gyroanalyse.c @@ -53,7 +53,7 @@ #define FFT_SAMPLING_RATE 1000 // allows analysis up to 500Hz which is more than motors create #define FFT_BPF_HZ 200 // use a bandpass on gyro data to ignore extreme low and extreme high frequencies #define DYN_NOTCH_WIDTH 100 // just an orientation and start value -#define DYN_NOTCH_CHANGERATE 40 // lower cut does not improve the performance much, higher cut makes it worse... +#define DYN_NOTCH_CHANGERATE 60 // lower cut does not improve the performance much, higher cut makes it worse... #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) @@ -118,7 +118,7 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs) { // at 4khz gyro loop rate this means 4khz / 4 / 3 = 333Hz => update every 3ms float looptime = targetLooptimeUs * 4 * 3; for (int axis = 0; axis < 3; ++axis) { - fftResult[axis].centerFreq = 200; + fftResult[axis].centerFreq = 200; // any init value biquadFilterInitLPF(&fftFreqFilter[axis], DYN_NOTCH_CHANGERATE, looptime); biquadFilterInit(&fftGyroFilter[axis], FFT_BPF_HZ, 1000000 / FFT_SAMPLING_RATE, BIQUAD_Q, FILTER_BPF); } @@ -136,15 +136,14 @@ bool isDynamicFilterActive(void) { /* * Collect gyro data, to be analysed in gyroDataAnalyseUpdate function */ -void gyroDataAnalyse(const gyroDev_t *gyroDev, const gyro_t *gyro) { +void gyroDataAnalyse(const gyroDev_t *gyroDev, biquadFilter_t *notchFilterDyn) { if (!isDynamicFilterActive()) { return; } - UNUSED(gyro); // if gyro sampling is > 1kHz, accumulate multiple samples for (int axis = 0; axis < 3; ++axis) { - fftAcc[axis] += gyroDev->gyroADC[axis] * gyroDev->scale; + fftAcc[axis] += gyroDev->gyroADC[axis]; } fftAccCount++; @@ -158,7 +157,7 @@ void gyroDataAnalyse(const gyroDev_t *gyroDev, const gyro_t *gyro) { sample = biquadFilterApplyDF2(&fftGyroFilter[axis], sample); gyroData[axis][fftIdx] = sample; if (axis == 0) - DEBUG_SET(DEBUG_FFT, 2, lrintf(sample)); + DEBUG_SET(DEBUG_FFT, 2, lrintf(sample * gyroDev->scale)); fftAcc[axis] = 0; } @@ -166,7 +165,7 @@ void gyroDataAnalyse(const gyroDev_t *gyroDev, const gyro_t *gyro) { } // calculate FFT and update filters - gyroDataAnalyseUpdate(); + gyroDataAnalyseUpdate(notchFilterDyn); } void stage_rfft_f32(arm_rfft_fast_instance_f32 * S, float32_t * p, float32_t * pOut); @@ -189,7 +188,7 @@ typedef enum { /* * Analyse last gyro data from the last FFT_WINDOW_SIZE milliseconds */ -void gyroDataAnalyseUpdate() { +void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn) { static int axis = 0; static int step = 0; arm_cfft_instance_f32 * Sint = &(fftInstance.Sint); @@ -295,7 +294,7 @@ void gyroDataAnalyseUpdate() { // calculate new filter coefficients float cutoffFreq = constrain(fftResult[axis].centerFreq - DYN_NOTCH_WIDTH, DYN_NOTCH_MIN_CUTOFF, DYN_NOTCH_MAX_CUTOFF); float notchQ = filterGetNotchQApprox(fftResult[axis].centerFreq, cutoffFreq); - biquadFilterUpdate(notchFilterDyn[axis], fftResult[axis].centerFreq, gyro.targetLooptime, notchQ, FILTER_NOTCH); + biquadFilterUpdate(¬chFilterDyn[axis], fftResult[axis].centerFreq, gyro.targetLooptime, notchQ, FILTER_NOTCH); DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime); axis = (axis + 1) % 3; diff --git a/src/main/sensors/gyroanalyse.h b/src/main/sensors/gyroanalyse.h index 0bca01dc4a..08ba8817bb 100644 --- a/src/main/sensors/gyroanalyse.h +++ b/src/main/sensors/gyroanalyse.h @@ -30,9 +30,6 @@ typedef struct gyroFftData_s { void gyroDataAnalyseInit(uint32_t targetLooptime); const gyroFftData_t *gyroFftData(int axis); struct gyroDev_s; -struct gyro_s; -void gyroDataAnalyse(const struct gyroDev_s *gyroDev, const struct gyro_s *gyro); -void gyroDataAnalyseUpdate(); +void gyroDataAnalyse(const struct gyroDev_s *gyroDev, biquadFilter_t *notchFilterDyn); +void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn); bool isDynamicFilterActive(); - -extern biquadFilter_t *notchFilterDyn[3];