mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-24 00:35:39 +03:00
Merge pull request #9327 from ctzsnooze/DN_Simplify
Simplify and improve Dynamic Notch
This commit is contained in:
Michael Keller
GitHub
commit
99dd2f5a27
9 changed files with 165 additions and 161 deletions
|
|
@ -1373,7 +1373,7 @@ static bool blackboxWriteSysinfo(void)
|
|||
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1,
|
||||
gyroConfig()->gyro_soft_notch_cutoff_2);
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_range", "%d", gyroConfig()->dyn_notch_range);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_max_hz", "%d", gyroConfig()->dyn_notch_max_hz);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_width_percent", "%d", gyroConfig()->dyn_notch_width_percent);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_q", "%d", gyroConfig()->dyn_notch_q);
|
||||
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_min_hz", "%d", gyroConfig()->dyn_notch_min_hz);
|
||||
|
|
|
|||
|
|
@ -408,12 +408,6 @@ static const char * const lookupTableRcSmoothingDerivativeType[] = {
|
|||
};
|
||||
#endif // USE_RC_SMOOTHING_FILTER
|
||||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
static const char * const lookupTableDynamicFilterRange[] = {
|
||||
"HIGH", "MEDIUM", "LOW", "AUTO"
|
||||
};
|
||||
#endif // USE_GYRO_DATA_ANALYSE
|
||||
|
||||
#ifdef USE_VTX_COMMON
|
||||
static const char * const lookupTableVtxLowPowerDisarm[] = {
|
||||
"OFF", "ON", "UNTIL_FIRST_ARM"
|
||||
|
|
@ -588,9 +582,6 @@ const lookupTableEntry_t lookupTables[] = {
|
|||
LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingInputType),
|
||||
LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingDerivativeType),
|
||||
#endif // USE_RC_SMOOTHING_FILTER
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
LOOKUP_TABLE_ENTRY(lookupTableDynamicFilterRange),
|
||||
#endif // USE_GYRO_DATA_ANALYSE
|
||||
#ifdef USE_VTX_COMMON
|
||||
LOOKUP_TABLE_ENTRY(lookupTableVtxLowPowerDisarm),
|
||||
#endif
|
||||
|
|
@ -658,10 +649,10 @@ const clivalue_t valueTable[] = {
|
|||
{ "gyro_to_use", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_to_use) },
|
||||
#endif
|
||||
#if defined(USE_GYRO_DATA_ANALYSE)
|
||||
{ "dyn_notch_range", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DYNAMIC_FILTER_RANGE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_range) },
|
||||
{ "dyn_notch_max_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 200, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_max_hz) },
|
||||
{ "dyn_notch_width_percent", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_width_percent) },
|
||||
{ "dyn_notch_q", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_q) },
|
||||
{ "dyn_notch_min_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 60, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_min_hz) },
|
||||
{ "dyn_notch_min_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 60, 250 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_min_hz) },
|
||||
#endif
|
||||
#ifdef USE_DYN_LPF
|
||||
{ "dyn_lpf_gyro_min_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_lpf_gyro_min_hz) },
|
||||
|
|
|
|||
|
|
@ -114,9 +114,6 @@ typedef enum {
|
|||
TABLE_RC_SMOOTHING_INPUT_TYPE,
|
||||
TABLE_RC_SMOOTHING_DERIVATIVE_TYPE,
|
||||
#endif // USE_RC_SMOOTHING_FILTER
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
TABLE_DYNAMIC_FILTER_RANGE,
|
||||
#endif // USE_GYRO_DATA_ANALYSE
|
||||
#ifdef USE_VTX_COMMON
|
||||
TABLE_VTX_LOW_POWER_DISARM,
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -77,12 +77,6 @@ static const char * const osdTableThrottleLimitType[] = {
|
|||
"OFF", "SCALE", "CLIP"
|
||||
};
|
||||
|
||||
#if defined(USE_GYRO_DATA_ANALYSE) && defined(USE_EXTENDED_CMS_MENUS)
|
||||
static const char * const osdTableDynNotchRangeType[] = {
|
||||
"HIGH", "MED", "LOW", "AUTO"
|
||||
};
|
||||
#endif
|
||||
|
||||
#ifdef USE_MULTI_GYRO
|
||||
static const char * const osdTableGyroToUse[] = {
|
||||
"FIRST", "SECOND", "BOTH"
|
||||
|
|
@ -570,7 +564,7 @@ static CMS_Menu cmsx_menuFilterGlobal = {
|
|||
#if (defined(USE_GYRO_DATA_ANALYSE) || defined(USE_DYN_LPF)) && defined(USE_EXTENDED_CMS_MENUS)
|
||||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
static uint8_t dynFiltNotchRange;
|
||||
static uint16_t dynFiltNotchMaxHz;
|
||||
static uint8_t dynFiltWidthPercent;
|
||||
static uint16_t dynFiltNotchQ;
|
||||
static uint16_t dynFiltNotchMinHz;
|
||||
|
|
@ -586,7 +580,7 @@ static uint8_t dynFiltDtermExpo;
|
|||
static const void *cmsx_menuDynFilt_onEnter(void)
|
||||
{
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
dynFiltNotchRange = gyroConfig()->dyn_notch_range;
|
||||
dynFiltNotchMaxHz = gyroConfig()->dyn_notch_max_hz;
|
||||
dynFiltWidthPercent = gyroConfig()->dyn_notch_width_percent;
|
||||
dynFiltNotchQ = gyroConfig()->dyn_notch_q;
|
||||
dynFiltNotchMinHz = gyroConfig()->dyn_notch_min_hz;
|
||||
|
|
@ -608,7 +602,7 @@ static const void *cmsx_menuDynFilt_onExit(const OSD_Entry *self)
|
|||
UNUSED(self);
|
||||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
gyroConfigMutable()->dyn_notch_range = dynFiltNotchRange;
|
||||
gyroConfigMutable()->dyn_notch_max_hz = dynFiltNotchMaxHz;
|
||||
gyroConfigMutable()->dyn_notch_width_percent = dynFiltWidthPercent;
|
||||
gyroConfigMutable()->dyn_notch_q = dynFiltNotchQ;
|
||||
gyroConfigMutable()->dyn_notch_min_hz = dynFiltNotchMinHz;
|
||||
|
|
@ -630,10 +624,10 @@ static const OSD_Entry cmsx_menuDynFiltEntries[] =
|
|||
{ "-- DYN FILT --", OME_Label, NULL, NULL, 0 },
|
||||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
{ "NOTCH RANGE", OME_TAB, NULL, &(OSD_TAB_t) { &dynFiltNotchRange, 3, osdTableDynNotchRangeType}, 0 },
|
||||
{ "NOTCH WIDTH %", OME_UINT8, NULL, &(OSD_UINT8_t) { &dynFiltWidthPercent, 0, 20, 1 }, 0 },
|
||||
{ "NOTCH Q", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchQ, 0, 1000, 1 }, 0 },
|
||||
{ "NOTCH MIN HZ", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchMinHz, 0, 1000, 1 }, 0 },
|
||||
{ "NOTCH MAX HZ", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchMaxHz, 0, 1000, 1 }, 0 },
|
||||
#endif
|
||||
|
||||
#ifdef USE_DYN_LPF
|
||||
|
|
|
|||
|
|
@ -44,31 +44,60 @@
|
|||
|
||||
#include "gyroanalyse.h"
|
||||
|
||||
// The FFT splits the frequency domain into an number of bins
|
||||
// A sampling frequency of 1000 and max frequency of 500 at a window size of 32 gives 16 frequency bins each 31.25Hz wide
|
||||
// Eg [0,31), [31,62), [62, 93) etc
|
||||
// for gyro loop >= 4KHz, sample rate 2000 defines FFT range to 1000Hz, 16 bins each 62.5 Hz wide
|
||||
// NB FFT_WINDOW_SIZE is set to 32 in gyroanalyse.h
|
||||
#define FFT_BIN_COUNT (FFT_WINDOW_SIZE / 2)
|
||||
// smoothing frequency for FFT centre frequency
|
||||
#define DYN_NOTCH_SMOOTH_FREQ_HZ 50
|
||||
// we need 4 steps for each axis
|
||||
#define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4)
|
||||
// FFT_WINDOW_SIZE defaults to 32 (gyroanalyse.h)
|
||||
// We get 16 frequency bins from 32 consecutive data values
|
||||
// Bin 0 is DC and can't be used.
|
||||
// Only bins 1 to 15 are usable.
|
||||
|
||||
// A gyro sample is collected every gyro loop
|
||||
// maxSampleCount recent gyro values are accumulated and averaged
|
||||
// to ensure that 32 samples are collected at the right rate for the required FFT bandwidth
|
||||
|
||||
// For an 8k gyro loop, at default 600hz max, 6 sequential gyro data points are averaged, FFT runs 1333Hz.
|
||||
// Upper limit of FFT is half that frequency, eg 666Hz by default.
|
||||
// At 8k, if user sets a max of 300Hz, int(8000/600) = 13, fftSamplingRateHz = 615Hz, range 307Hz
|
||||
// Note that lower max requires more samples to be averaged, increasing precision but taking longer to get enough samples.
|
||||
// For Bosch at 3200Hz gyro, max of 600, int(3200/1200) = 2, fftSamplingRateHz = 1600, range to 800hz
|
||||
// For Bosch on XClass, better to set a max of 300, int(3200/600) = 5, fftSamplingRateHz = 640, range to 320Hz
|
||||
//
|
||||
// When sampleCount reaches maxSampleCount, the averaged gyro value is put into the circular buffer of 32 samples
|
||||
// At 8k, with 600Hz max, maxSampleCount = 6, this happens every 6 * 0.125us, or every 0.75ms
|
||||
// Hence to completely replace all 32 samples of the FFT input buffer with clean new data takes 24ms
|
||||
|
||||
// The FFT code is split into steps. It takes 4 gyro loops to calculate the FFT for one axis
|
||||
// (gyroDataAnalyseUpdate has 8 steps, but only four breaks)
|
||||
// Since there are three axes, it takes 12 gyro loops to completely update all axes.
|
||||
// At 8k, any one axis gets updated at 8000 / 12 or 666hz or every 1.5ms
|
||||
// In this time, 2 points in the FFT buffer will have changed.
|
||||
// At 4k, it takes twice as long to update an axis, i.e. each axis updates only every 3ms
|
||||
// Four points in the buffer will have changed in that time, and each point will be the average of three samples.
|
||||
// Hence output jitter at 4k is about four times worse than at 8k. At 2k output jitter is quite bad.
|
||||
|
||||
// The Hanning window step loads gyro data (32 data points) for one axis from the circular buffer into fftData[i]
|
||||
// and applies the hanning window to the edge values
|
||||
// Calculation steps 1 and 2 then calculate the fft output (32 data points) and put that back into the same fftData[i] array.
|
||||
// We then use fftData[i] array for frequency centre calculations for that axis
|
||||
|
||||
// Each FFT output bin has width fftSamplingRateHz/32, ie 41.65Hz per bin at 1333Hz
|
||||
// Usable bandwidth is half this, ie 666Hz if fftSamplingRateHz is 1333Hz, i.e. bin 1 is 41.65hz, bin 2 83.3hz etc
|
||||
|
||||
#define DYN_NOTCH_SMOOTH_HZ 4
|
||||
#define FFT_BIN_COUNT (FFT_WINDOW_SIZE / 2) // 16
|
||||
#define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * 4) // 4 steps per axis
|
||||
#define DYN_NOTCH_OSD_MIN_THROTTLE 20
|
||||
|
||||
static uint16_t FAST_RAM_ZERO_INIT fftSamplingRateHz;
|
||||
static float FAST_RAM_ZERO_INIT fftResolution;
|
||||
static uint8_t FAST_RAM_ZERO_INIT fftStartBin;
|
||||
static uint16_t FAST_RAM_ZERO_INIT dynNotchMaxCtrHz;
|
||||
static uint8_t dynamicFilterRange;
|
||||
static float FAST_RAM_ZERO_INIT dynNotchQ;
|
||||
static float FAST_RAM_ZERO_INIT dynNotch1Ctr;
|
||||
static float FAST_RAM_ZERO_INIT dynNotch2Ctr;
|
||||
static uint16_t FAST_RAM_ZERO_INIT dynNotchMinHz;
|
||||
static bool FAST_RAM dualNotch = true;
|
||||
static uint16_t FAST_RAM_ZERO_INIT dynNotchMaxFFT;
|
||||
|
||||
static uint16_t FAST_RAM_ZERO_INIT dynNotchMaxHz;
|
||||
static bool FAST_RAM dualNotch = true;
|
||||
static uint16_t FAST_RAM_ZERO_INIT dynNotchMaxFFT;
|
||||
static float FAST_RAM_ZERO_INIT smoothFactor;
|
||||
static uint8_t FAST_RAM_ZERO_INIT samples;
|
||||
// Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window
|
||||
static FAST_RAM_ZERO_INIT float hanningWindow[FFT_WINDOW_SIZE];
|
||||
|
||||
|
|
@ -82,43 +111,30 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
|
|||
gyroAnalyseInitialized = true;
|
||||
#endif
|
||||
|
||||
dynamicFilterRange = gyroConfig()->dyn_notch_range;
|
||||
fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_LOW;
|
||||
dynNotch1Ctr = 1 - gyroConfig()->dyn_notch_width_percent / 100.0f;
|
||||
dynNotch2Ctr = 1 + gyroConfig()->dyn_notch_width_percent / 100.0f;
|
||||
dynNotchQ = gyroConfig()->dyn_notch_q / 100.0f;
|
||||
dynNotchMinHz = gyroConfig()->dyn_notch_min_hz;
|
||||
dynNotchMaxHz = MAX(2 * dynNotchMinHz, gyroConfig()->dyn_notch_max_hz);
|
||||
|
||||
if (gyroConfig()->dyn_notch_width_percent == 0) {
|
||||
dualNotch = false;
|
||||
}
|
||||
|
||||
if (dynamicFilterRange == DYN_NOTCH_RANGE_AUTO) {
|
||||
if (gyroConfig()->dyn_lpf_gyro_max_hz > 333) {
|
||||
fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_MEDIUM;
|
||||
}
|
||||
if (gyroConfig()->dyn_lpf_gyro_max_hz > 610) {
|
||||
fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_HIGH;
|
||||
}
|
||||
} else {
|
||||
if (dynamicFilterRange == DYN_NOTCH_RANGE_HIGH) {
|
||||
fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_HIGH;
|
||||
}
|
||||
else if (dynamicFilterRange == DYN_NOTCH_RANGE_MEDIUM) {
|
||||
fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_MEDIUM;
|
||||
}
|
||||
}
|
||||
// If we get at least 3 samples then use the default FFT sample frequency
|
||||
// otherwise we need to calculate a FFT sample frequency to ensure we get 3 samples (gyro loops < 4K)
|
||||
const int gyroLoopRateHz = lrintf((1.0f / targetLooptimeUs) * 1e6f);
|
||||
|
||||
fftSamplingRateHz = MIN((gyroLoopRateHz / 3), fftSamplingRateHz);
|
||||
samples = MAX(1, gyroLoopRateHz / (2 * dynNotchMaxHz)); //600hz, 8k looptime, 13.333
|
||||
|
||||
fftResolution = (float)fftSamplingRateHz / FFT_WINDOW_SIZE;
|
||||
fftSamplingRateHz = gyroLoopRateHz / samples;
|
||||
// eg 8k, user max 600hz, int(8000/1200) = 6 (6.666), fftSamplingRateHz = 1333hz, range 666Hz
|
||||
// eg 4k, user max 600hz, int(4000/1200) = 3 (3.333), fftSamplingRateHz = 1333hz, range 666Hz
|
||||
// eg 2k, user max 600hz, int(2000/1200) = 1 (1.666) fftSamplingRateHz = 2000hz, range 1000Hz
|
||||
// eg 2k, user max 400hz, int(2000/800) = 2 (2.5) fftSamplingRateHz = 1000hz, range 500Hz
|
||||
// eg 1k, user max 600hz, int(1000/1200) = 1 (max(1,0.8333)) fftSamplingRateHz = 1000hz, range 500Hz
|
||||
// the upper limit of DN is always going to be Nyquist
|
||||
|
||||
fftStartBin = dynNotchMinHz / lrintf(fftResolution);
|
||||
|
||||
dynNotchMaxCtrHz = fftSamplingRateHz / 2; //Nyquist
|
||||
fftResolution = (float)fftSamplingRateHz / FFT_WINDOW_SIZE; // 41.65hz per bin for medium
|
||||
fftStartBin = MAX(2, dynNotchMinHz / lrintf(fftResolution)); // can't use bin 0 because it is DC.
|
||||
smoothFactor = 2 * M_PIf * DYN_NOTCH_SMOOTH_HZ / (gyroLoopRateHz / 12); // minimum PT1 k value
|
||||
|
||||
for (int i = 0; i < FFT_WINDOW_SIZE; i++) {
|
||||
hanningWindow[i] = (0.5f - 0.5f * cos_approx(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1)));
|
||||
|
|
@ -128,24 +144,13 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
|
|||
void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs)
|
||||
{
|
||||
// initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later
|
||||
// *** can this next line be removed ??? ***
|
||||
gyroDataAnalyseInit(targetLooptimeUs);
|
||||
|
||||
const uint16_t samplingFrequency = 1000000 / targetLooptimeUs;
|
||||
state->maxSampleCount = samplingFrequency / fftSamplingRateHz;
|
||||
state->maxSampleCountRcp = 1.f / state->maxSampleCount;
|
||||
|
||||
state->maxSampleCount = samples;
|
||||
state->maxSampleCountRcp = 1.0f / state->maxSampleCount;
|
||||
arm_rfft_fast_init_f32(&state->fftInstance, FFT_WINDOW_SIZE);
|
||||
|
||||
// recalculation of filters takes 4 calls per axis => each filter gets updated every DYN_NOTCH_CALC_TICKS calls
|
||||
// at 4khz gyro loop rate this means 4khz / 4 / 3 = 333Hz => update every 3ms
|
||||
// for gyro rate > 16kHz, we have update frequency of 1kHz => 1ms
|
||||
const float looptime = MAX(1000000u / fftSamplingRateHz, targetLooptimeUs * DYN_NOTCH_CALC_TICKS);
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
// any init value
|
||||
state->centerFreq[axis] = dynNotchMaxCtrHz;
|
||||
state->prevCenterFreq[axis] = dynNotchMaxCtrHz;
|
||||
biquadFilterInitLPF(&state->detectedFrequencyFilter[axis], DYN_NOTCH_SMOOTH_FREQ_HZ, looptime);
|
||||
state->centerFreq[axis] = dynNotchMaxHz;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -162,10 +167,9 @@ static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *not
|
|||
void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2)
|
||||
{
|
||||
// samples should have been pushed by `gyroDataAnalysePush`
|
||||
// if gyro sampling is > 1kHz, accumulate multiple samples
|
||||
// if gyro sampling is > 1kHz, accumulate and average multiple gyro samples
|
||||
state->sampleCount++;
|
||||
|
||||
// this runs at 1kHz
|
||||
if (state->sampleCount == state->maxSampleCount) {
|
||||
state->sampleCount = 0;
|
||||
|
||||
|
|
@ -183,6 +187,9 @@ void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn,
|
|||
state->circularBufferIdx = (state->circularBufferIdx + 1) % FFT_WINDOW_SIZE;
|
||||
|
||||
// We need DYN_NOTCH_CALC_TICKS tick to update all axis with newly sampled value
|
||||
// recalculation of filters takes 4 calls per axis => each filter gets updated every DYN_NOTCH_CALC_TICKS calls
|
||||
// at 4kHz gyro loop rate this means 8kHz / 4 / 3 = 666Hz => update every 1.5ms
|
||||
// at 4kHz gyro loop rate this means 4kHz / 4 / 3 = 333Hz => update every 3ms
|
||||
state->updateTicks = DYN_NOTCH_CALC_TICKS;
|
||||
}
|
||||
|
||||
|
|
@ -200,7 +207,7 @@ void arm_radix8_butterfly_f32(float32_t *pSrc, uint16_t fftLen, const float32_t
|
|||
void arm_bitreversal_32(uint32_t *pSrc, const uint16_t bitRevLen, const uint16_t *pBitRevTable);
|
||||
|
||||
/*
|
||||
* Analyse last gyro data from the last FFT_WINDOW_SIZE milliseconds
|
||||
* Analyse gyro data
|
||||
*/
|
||||
static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2)
|
||||
{
|
||||
|
|
@ -241,6 +248,7 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
|
|||
break;
|
||||
}
|
||||
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
|
||||
|
||||
break;
|
||||
}
|
||||
case STEP_BITREVERSAL:
|
||||
|
|
@ -257,6 +265,7 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
|
|||
// this does not work in place => fftData AND rfftData needed
|
||||
stage_rfft_f32(&state->fftInstance, state->fftData, state->rfftData);
|
||||
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
|
||||
|
||||
break;
|
||||
}
|
||||
case STEP_ARM_CMPLX_MAG_F32:
|
||||
|
|
@ -269,62 +278,76 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
|
|||
}
|
||||
case STEP_CALC_FREQUENCIES:
|
||||
{
|
||||
bool fftIncreased = false;
|
||||
float dataMax = 0;
|
||||
uint8_t binStart = 0;
|
||||
// identify max bin and max/min heights
|
||||
float dataMax = 0.0f;
|
||||
float dataMin = 1.0f;
|
||||
uint8_t binMax = 0;
|
||||
//for bins after initial decline, identify start bin and max bin
|
||||
float dataMinHi = 1.0f;
|
||||
for (int i = fftStartBin; i < FFT_BIN_COUNT; i++) {
|
||||
if (fftIncreased || (state->fftData[i] > state->fftData[i - 1])) {
|
||||
if (!fftIncreased) {
|
||||
binStart = i; // first up-step bin
|
||||
fftIncreased = true;
|
||||
}
|
||||
if (state->fftData[i] > state->fftData[i - 1]) { // bin height increased
|
||||
if (state->fftData[i] > dataMax) {
|
||||
dataMax = state->fftData[i];
|
||||
binMax = i; // tallest bin
|
||||
binMax = i; // tallest bin so far
|
||||
}
|
||||
}
|
||||
}
|
||||
if (binMax == 0) { // no bin increase, hold prev max bin, dataMin = 1 dataMax = 0, ie move slow
|
||||
binMax = lrintf(state->centerFreq[state->updateAxis] / fftResolution);
|
||||
} else { // there was a max, find min
|
||||
for (int i = binMax - 1; i > 1; i--) { // look for min below max
|
||||
dataMin = state->fftData[i];
|
||||
if (state->fftData[i - 1] > state->fftData[i]) { // up step below this one
|
||||
break;
|
||||
}
|
||||
}
|
||||
for (int i = binMax + 1; i < (FFT_BIN_COUNT - 1); i++) { // // look for min above max
|
||||
dataMinHi = state->fftData[i];
|
||||
if (state->fftData[i] < state->fftData[i + 1]) { // up step above this one
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
dataMin = fminf(dataMin, dataMinHi);
|
||||
|
||||
// accumulate fftSum and fftWeightedSum from peak bin, and shoulder bins either side of peak
|
||||
float cubedData = state->fftData[binMax] * state->fftData[binMax] * state->fftData[binMax];
|
||||
float fftSum = cubedData;
|
||||
float fftWeightedSum = cubedData * (binMax + 1);
|
||||
// accumulate upper shoulder
|
||||
for (int i = binMax; i < FFT_BIN_COUNT - 1; i++) {
|
||||
if (state->fftData[i] > state->fftData[i + 1]) {
|
||||
cubedData = state->fftData[i] * state->fftData[i] * state->fftData[i];
|
||||
fftSum += cubedData;
|
||||
fftWeightedSum += cubedData * (i + 1);
|
||||
} else {
|
||||
break;
|
||||
}
|
||||
float squaredData = state->fftData[binMax] * state->fftData[binMax];
|
||||
float fftSum = squaredData;
|
||||
float fftWeightedSum = squaredData * binMax;
|
||||
|
||||
// accumulate upper shoulder unless it would be FFT_BIN_COUNT
|
||||
uint8_t shoulderBin = binMax + 1;
|
||||
if (shoulderBin < FFT_BIN_COUNT) {
|
||||
squaredData = state->fftData[shoulderBin] * state->fftData[shoulderBin];
|
||||
fftSum += squaredData;
|
||||
fftWeightedSum += squaredData * shoulderBin;
|
||||
}
|
||||
// accumulate lower shoulder
|
||||
for (int i = binMax; i > binStart + 1; i--) {
|
||||
if (state->fftData[i] > state->fftData[i - 1]) {
|
||||
cubedData = state->fftData[i] * state->fftData[i] * state->fftData[i];
|
||||
fftSum += cubedData;
|
||||
fftWeightedSum += cubedData * (i + 1);
|
||||
} else {
|
||||
break;
|
||||
}
|
||||
|
||||
// accumulate lower shoulder unless lower shoulder would be bin 0 (DC)
|
||||
if (binMax > 1) {
|
||||
shoulderBin = binMax - 1;
|
||||
squaredData = state->fftData[shoulderBin] * state->fftData[shoulderBin];
|
||||
fftSum += squaredData;
|
||||
fftWeightedSum += squaredData * shoulderBin;
|
||||
}
|
||||
// get weighted center of relevant frequency range (this way we have a better resolution than 31.25Hz)
|
||||
float centerFreq = dynNotchMaxCtrHz;
|
||||
|
||||
// get centerFreq in Hz from weighted bins
|
||||
float centerFreq = dynNotchMaxHz;
|
||||
float fftMeanIndex = 0;
|
||||
// idx was shifted by 1 to start at 1, not 0
|
||||
if (fftSum > 0) {
|
||||
fftMeanIndex = (fftWeightedSum / fftSum) - 1;
|
||||
// the index points at the center frequency of each bin so index 0 is actually 16.125Hz
|
||||
fftMeanIndex = (fftWeightedSum / fftSum);
|
||||
centerFreq = fftMeanIndex * fftResolution;
|
||||
// In theory, the index points to the centre frequency of the bin.
|
||||
// at 1333hz, bin widths are 41.65Hz, so bin 2 has the range 83,3Hz to 124,95Hz
|
||||
// Rav feels that maybe centerFreq = (fftMeanIndex + 0.5) * fftResolution; is better
|
||||
// empirical checking shows that not adding 0.5 works better
|
||||
} else {
|
||||
centerFreq = state->prevCenterFreq[state->updateAxis];
|
||||
centerFreq = state->centerFreq[state->updateAxis];
|
||||
}
|
||||
centerFreq = fmax(centerFreq, dynNotchMinHz);
|
||||
centerFreq = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], centerFreq);
|
||||
state->prevCenterFreq[state->updateAxis] = state->centerFreq[state->updateAxis];
|
||||
state->centerFreq[state->updateAxis] = centerFreq;
|
||||
centerFreq = constrainf(centerFreq, dynNotchMinHz, dynNotchMaxHz);
|
||||
|
||||
// PT1 style dynamic smoothing moves rapidly towards big peaks and slowly away, up to 8x faster
|
||||
float dynamicFactor = constrainf(dataMax / dataMin, 1.0f, 8.0f);
|
||||
state->centerFreq[state->updateAxis] = state->centerFreq[state->updateAxis] + smoothFactor * dynamicFactor * (centerFreq - state->centerFreq[state->updateAxis]);
|
||||
|
||||
if(calculateThrottlePercentAbs() > DYN_NOTCH_OSD_MIN_THROTTLE) {
|
||||
dynNotchMaxFFT = MAX(dynNotchMaxFFT, state->centerFreq[state->updateAxis]);
|
||||
|
|
@ -333,26 +356,25 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
|
|||
if (state->updateAxis == 0) {
|
||||
DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100));
|
||||
DEBUG_SET(DEBUG_FFT_FREQ, 0, state->centerFreq[state->updateAxis]);
|
||||
DEBUG_SET(DEBUG_FFT_FREQ, 1, lrintf(dynamicFactor * 100));
|
||||
DEBUG_SET(DEBUG_DYN_LPF, 1, state->centerFreq[state->updateAxis]);
|
||||
}
|
||||
if (state->updateAxis == 1) {
|
||||
DEBUG_SET(DEBUG_FFT_FREQ, 1, state->centerFreq[state->updateAxis]);
|
||||
}
|
||||
// Debug FFT_Freq carries raw gyro, gyro after first filter set, FFT centre for roll and for pitch
|
||||
// if (state->updateAxis == 1) {
|
||||
// DEBUG_SET(DEBUG_FFT_FREQ, 1, state->centerFreq[state->updateAxis]);
|
||||
// }
|
||||
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
|
||||
|
||||
break;
|
||||
}
|
||||
case STEP_UPDATE_FILTERS:
|
||||
{
|
||||
// 7us
|
||||
// calculate cutoffFreq and notch Q, update notch filter =1.8+((A2-150)*0.004)
|
||||
if (state->prevCenterFreq[state->updateAxis] != state->centerFreq[state->updateAxis]) {
|
||||
if (dualNotch) {
|
||||
biquadFilterUpdate(¬chFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch1Ctr, gyro.targetLooptime, dynNotchQ, FILTER_NOTCH);
|
||||
biquadFilterUpdate(¬chFilterDyn2[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch2Ctr, gyro.targetLooptime, dynNotchQ, FILTER_NOTCH);
|
||||
} else {
|
||||
biquadFilterUpdate(¬chFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis], gyro.targetLooptime, dynNotchQ, FILTER_NOTCH);
|
||||
}
|
||||
// calculate cutoffFreq and notch Q, update notch filter
|
||||
if (dualNotch) {
|
||||
biquadFilterUpdate(¬chFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch1Ctr, gyro.targetLooptime, dynNotchQ, FILTER_NOTCH);
|
||||
biquadFilterUpdate(¬chFilterDyn2[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch2Ctr, gyro.targetLooptime, dynNotchQ, FILTER_NOTCH);
|
||||
} else {
|
||||
biquadFilterUpdate(¬chFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis], gyro.targetLooptime, dynNotchQ, FILTER_NOTCH);
|
||||
}
|
||||
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
|
||||
|
||||
|
|
@ -363,8 +385,7 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
|
|||
case STEP_HANNING:
|
||||
{
|
||||
// 5us
|
||||
// apply hanning window to gyro samples and store result in fftData
|
||||
// hanning starts and ends with 0, could be skipped for minor speed improvement
|
||||
// apply hanning window to gyro samples and store result in fftData[i] to be used in step 1 and 2 and 3
|
||||
const uint8_t ringBufIdx = FFT_WINDOW_SIZE - state->circularBufferIdx;
|
||||
arm_mult_f32(&state->downsampledGyroData[state->updateAxis][state->circularBufferIdx], &hanningWindow[0], &state->fftData[0], ringBufIdx);
|
||||
if (state->circularBufferIdx > 0) {
|
||||
|
|
|
|||
|
|
@ -24,8 +24,6 @@
|
|||
|
||||
#include "common/filter.h"
|
||||
|
||||
|
||||
// max for F3 targets
|
||||
#define FFT_WINDOW_SIZE 32
|
||||
|
||||
typedef struct gyroAnalyseState_s {
|
||||
|
|
@ -48,9 +46,8 @@ typedef struct gyroAnalyseState_s {
|
|||
float fftData[FFT_WINDOW_SIZE];
|
||||
float rfftData[FFT_WINDOW_SIZE];
|
||||
|
||||
biquadFilter_t detectedFrequencyFilter[XYZ_AXIS_COUNT];
|
||||
uint16_t centerFreq[XYZ_AXIS_COUNT];
|
||||
uint16_t prevCenterFreq[XYZ_AXIS_COUNT];
|
||||
float centerFreq[XYZ_AXIS_COUNT];
|
||||
|
||||
} gyroAnalyseState_t;
|
||||
|
||||
STATIC_ASSERT(FFT_WINDOW_SIZE <= (uint8_t) -1, window_size_greater_than_underlying_type);
|
||||
|
|
|
|||
|
|
@ -1686,7 +1686,7 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
|
|||
#endif
|
||||
// Added in MSP API 1.42
|
||||
#if defined(USE_GYRO_DATA_ANALYSE)
|
||||
sbufWriteU8(dst, gyroConfig()->dyn_notch_range);
|
||||
sbufWriteU8(dst, 0); // DEPRECATED 1.43: dyn_notch_range
|
||||
sbufWriteU8(dst, gyroConfig()->dyn_notch_width_percent);
|
||||
sbufWriteU16(dst, gyroConfig()->dyn_notch_q);
|
||||
sbufWriteU16(dst, gyroConfig()->dyn_notch_min_hz);
|
||||
|
|
@ -1696,7 +1696,6 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
|
|||
sbufWriteU16(dst, 0);
|
||||
sbufWriteU16(dst, 0);
|
||||
#endif
|
||||
|
||||
#if defined(USE_RPM_FILTER)
|
||||
sbufWriteU8(dst, rpmFilterConfig()->gyro_rpm_notch_harmonics);
|
||||
sbufWriteU8(dst, rpmFilterConfig()->gyro_rpm_notch_min);
|
||||
|
|
@ -1704,6 +1703,12 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
|
|||
sbufWriteU8(dst, 0);
|
||||
sbufWriteU8(dst, 0);
|
||||
#endif
|
||||
#if defined(USE_GYRO_DATA_ANALYSE)
|
||||
// Added in MSP API 1.43
|
||||
sbufWriteU16(dst, gyroConfig()->dyn_notch_max_hz);
|
||||
#else
|
||||
sbufWriteU16(dst, 0);
|
||||
#endif
|
||||
|
||||
break;
|
||||
case MSP_PID_ADVANCED:
|
||||
|
|
@ -2497,7 +2502,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
|
|||
if (sbufBytesRemaining(src) >= 8) {
|
||||
// Added in MSP API 1.42
|
||||
#if defined(USE_GYRO_DATA_ANALYSE)
|
||||
gyroConfigMutable()->dyn_notch_range = sbufReadU8(src);
|
||||
sbufReadU8(src); // DEPRECATED: dyn_notch_range
|
||||
gyroConfigMutable()->dyn_notch_width_percent = sbufReadU8(src);
|
||||
gyroConfigMutable()->dyn_notch_q = sbufReadU16(src);
|
||||
gyroConfigMutable()->dyn_notch_min_hz = sbufReadU16(src);
|
||||
|
|
@ -2507,7 +2512,6 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
|
|||
sbufReadU16(src);
|
||||
sbufReadU16(src);
|
||||
#endif
|
||||
|
||||
#if defined(USE_RPM_FILTER)
|
||||
rpmFilterConfigMutable()->gyro_rpm_notch_harmonics = sbufReadU8(src);
|
||||
rpmFilterConfigMutable()->gyro_rpm_notch_min = sbufReadU8(src);
|
||||
|
|
@ -2516,6 +2520,15 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
|
|||
sbufReadU8(src);
|
||||
#endif
|
||||
}
|
||||
if (sbufBytesRemaining(src) >= 1) {
|
||||
#if defined(USE_GYRO_DATA_ANALYSE)
|
||||
// Added in MSP API 1.43
|
||||
gyroConfigMutable()->dyn_notch_max_hz = sbufReadU16(src);
|
||||
#else
|
||||
sbufReadU16(src);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
// reinitialize the gyro filters with the new values
|
||||
validateAndFixGyroConfig();
|
||||
|
|
|
|||
|
|
@ -157,7 +157,7 @@ static void gyroInitLowpassFilterLpf(int slot, int type, uint16_t lpfHz);
|
|||
#define GYRO_OVERFLOW_TRIGGER_THRESHOLD 31980 // 97.5% full scale (1950dps for 2000dps gyro)
|
||||
#define GYRO_OVERFLOW_RESET_THRESHOLD 30340 // 92.5% full scale (1850dps for 2000dps gyro)
|
||||
|
||||
PG_REGISTER_WITH_RESET_FN(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 7);
|
||||
PG_REGISTER_WITH_RESET_FN(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 8);
|
||||
|
||||
#ifndef GYRO_CONFIG_USE_GYRO_DEFAULT
|
||||
#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1
|
||||
|
|
@ -193,7 +193,7 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
|
|||
gyroConfig->yaw_spin_threshold = 1950;
|
||||
gyroConfig->dyn_lpf_gyro_min_hz = 200;
|
||||
gyroConfig->dyn_lpf_gyro_max_hz = 500;
|
||||
gyroConfig->dyn_notch_range = DYN_NOTCH_RANGE_MEDIUM;
|
||||
gyroConfig->dyn_notch_max_hz = 600;
|
||||
gyroConfig->dyn_notch_width_percent = 8;
|
||||
gyroConfig->dyn_notch_q = 120;
|
||||
gyroConfig->dyn_notch_min_hz = 150;
|
||||
|
|
|
|||
|
|
@ -82,17 +82,6 @@ enum {
|
|||
GYRO_OVERFLOW_CHECK_ALL_AXES
|
||||
};
|
||||
|
||||
enum {
|
||||
DYN_NOTCH_RANGE_HIGH = 0,
|
||||
DYN_NOTCH_RANGE_MEDIUM,
|
||||
DYN_NOTCH_RANGE_LOW,
|
||||
DYN_NOTCH_RANGE_AUTO
|
||||
};
|
||||
|
||||
#define DYN_NOTCH_RANGE_HZ_HIGH 2000
|
||||
#define DYN_NOTCH_RANGE_HZ_MEDIUM 1333
|
||||
#define DYN_NOTCH_RANGE_HZ_LOW 1000
|
||||
|
||||
enum {
|
||||
DYN_LPF_NONE = 0,
|
||||
DYN_LPF_PT1,
|
||||
|
|
@ -144,13 +133,15 @@ typedef struct gyroConfig_s {
|
|||
int16_t yaw_spin_threshold;
|
||||
|
||||
uint16_t gyroCalibrationDuration; // Gyro calibration duration in 1/100 second
|
||||
|
||||
|
||||
uint16_t dyn_lpf_gyro_min_hz;
|
||||
uint16_t dyn_lpf_gyro_max_hz;
|
||||
uint8_t dyn_notch_range; // ignore any FFT bin below this threshold
|
||||
|
||||
uint16_t dyn_notch_max_hz;
|
||||
uint8_t dyn_notch_width_percent;
|
||||
uint16_t dyn_notch_q;
|
||||
uint16_t dyn_notch_min_hz;
|
||||
|
||||
uint8_t gyro_filter_debug_axis;
|
||||
} gyroConfig_t;
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue