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

Dynamic notch bandwidth back to Q

Browse source 
+ simplified frequency estimation
This commit is contained in:
KarateBrot 2021-06-11 00:16:21 +02:00
parent 12a4016ff0
commit 1ae31fd3d5
7 changed files with 34 additions and 58 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/blackbox/blackbox.c
View file

@ -1422,7 +1422,7 @@ static bool blackboxWriteSysinfo(void)
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_max_hz", "%d", gyroConfig()->dyn_notch_max_hz); BLACKBOX_PRINT_HEADER_LINE("dyn_notch_max_hz", "%d", gyroConfig()->dyn_notch_max_hz);
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_count", "%d", gyroConfig()->dyn_notch_count); BLACKBOX_PRINT_HEADER_LINE("dyn_notch_count", "%d", gyroConfig()->dyn_notch_count);
BLACKBOX_PRINT_HEADER_LINE("dyn_notch_bandwidth_hz", "%d", gyroConfig()->dyn_notch_bandwidth_hz); 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); BLACKBOX_PRINT_HEADER_LINE("dyn_notch_min_hz", "%d", gyroConfig()->dyn_notch_min_hz);
#endif #endif
#ifdef USE_DSHOT_TELEMETRY #ifdef USE_DSHOT_TELEMETRY

2
src/main/cli/settings.c
View file

@ -662,7 +662,7 @@ const clivalue_t valueTable[] = {
#endif #endif
#if defined(USE_GYRO_DATA_ANALYSE) #if defined(USE_GYRO_DATA_ANALYSE)
{ "dyn_notch_count", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 1, DYN_NOTCH_COUNT_MAX }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_count) }, { "dyn_notch_count", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 1, DYN_NOTCH_COUNT_MAX }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_count) },
{ "dyn_notch_bandwidth_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_bandwidth_hz) }, { "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, 250 }, 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) },
{ "dyn_notch_max_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 200, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_max_hz) }, { "dyn_notch_max_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 200, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_max_hz) },
#endif #endif

8
src/main/cms/cms_menu_imu.c
View file

@ -757,7 +757,7 @@ static CMS_Menu cmsx_menuFilterGlobal = {
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
static uint16_t dynFiltNotchMaxHz; static uint16_t dynFiltNotchMaxHz;
static uint8_t dynFiltCount; static uint8_t dynFiltCount;
static uint16_t dynFiltBandwidthHz; static uint16_t dynFiltNotchQ;
static uint16_t dynFiltNotchMinHz; static uint16_t dynFiltNotchMinHz;
#endif #endif
#ifdef USE_DYN_LPF #ifdef USE_DYN_LPF
@ -776,7 +776,7 @@ static const void *cmsx_menuDynFilt_onEnter(displayPort_t *pDisp)
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
dynFiltNotchMaxHz = gyroConfig()->dyn_notch_max_hz; dynFiltNotchMaxHz = gyroConfig()->dyn_notch_max_hz;
dynFiltCount = gyroConfig()->dyn_notch_count; dynFiltCount = gyroConfig()->dyn_notch_count;
dynFiltBandwidthHz = gyroConfig()->dyn_notch_bandwidth_hz; dynFiltNotchQ = gyroConfig()->dyn_notch_q;
dynFiltNotchMinHz = gyroConfig()->dyn_notch_min_hz; dynFiltNotchMinHz = gyroConfig()->dyn_notch_min_hz;
#endif #endif
#ifdef USE_DYN_LPF #ifdef USE_DYN_LPF
@ -800,7 +800,7 @@ static const void *cmsx_menuDynFilt_onExit(displayPort_t *pDisp, const OSD_Entry
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
gyroConfigMutable()->dyn_notch_max_hz = dynFiltNotchMaxHz; gyroConfigMutable()->dyn_notch_max_hz = dynFiltNotchMaxHz;
gyroConfigMutable()->dyn_notch_count = dynFiltCount; gyroConfigMutable()->dyn_notch_count = dynFiltCount;
gyroConfigMutable()->dyn_notch_bandwidth_hz = dynFiltBandwidthHz; gyroConfigMutable()->dyn_notch_q = dynFiltNotchQ;
gyroConfigMutable()->dyn_notch_min_hz = dynFiltNotchMinHz; gyroConfigMutable()->dyn_notch_min_hz = dynFiltNotchMinHz;
#endif #endif
#ifdef USE_DYN_LPF #ifdef USE_DYN_LPF
@ -822,7 +822,7 @@ static const OSD_Entry cmsx_menuDynFiltEntries[] =
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
{ "NOTCH COUNT", OME_UINT8, NULL, &(OSD_UINT8_t) { &dynFiltCount, 1, DYN_NOTCH_COUNT_MAX, 1 }, 0 }, { "NOTCH COUNT", OME_UINT8, NULL, &(OSD_UINT8_t) { &dynFiltCount, 1, DYN_NOTCH_COUNT_MAX, 1 }, 0 },
{ "NOTCH WIDTH HZ", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltBandwidthHz, 1, 1000, 1 }, 0 }, { "NOTCH Q", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchQ, 1, 1000, 1 }, 0 },
{ "NOTCH MIN HZ", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchMinHz, 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 }, { "NOTCH MAX HZ", OME_UINT16, NULL, &(OSD_UINT16_t) { &dynFiltNotchMaxHz, 0, 1000, 1 }, 0 },
#endif #endif

64
src/main/flight/gyroanalyse.c
View file

@ -111,17 +111,17 @@ static float FAST_DATA_ZERO_INIT sdftResolutionHz;
static uint8_t FAST_DATA_ZERO_INIT sdftStartBin; static uint8_t FAST_DATA_ZERO_INIT sdftStartBin;
static uint8_t FAST_DATA_ZERO_INIT sdftEndBin; static uint8_t FAST_DATA_ZERO_INIT sdftEndBin;
static float FAST_DATA_ZERO_INIT sdftMeanSq; static float FAST_DATA_ZERO_INIT sdftMeanSq;
static uint16_t FAST_DATA_ZERO_INIT dynNotchBandwidthHz; static uint16_t FAST_DATA_ZERO_INIT dynNotchQ;
static uint16_t FAST_DATA_ZERO_INIT dynNotchMinHz; static uint16_t FAST_DATA_ZERO_INIT dynNotchMinHz;
static uint16_t FAST_DATA_ZERO_INIT dynNotchMaxHz; static uint16_t FAST_DATA_ZERO_INIT dynNotchMaxHz;
static uint16_t FAST_DATA_ZERO_INIT dynNotchMaxFFT; static uint16_t FAST_DATA_ZERO_INIT dynNotchMaxFFT;
static float FAST_DATA_ZERO_INIT smoothFactor; static float FAST_DATA_ZERO_INIT gain;
static uint8_t FAST_DATA_ZERO_INIT numSamples; static uint8_t FAST_DATA_ZERO_INIT numSamples;
void gyroDataAnalyseInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs) void gyroDataAnalyseInit(gyroAnalyseState_t *state, 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
dynNotchBandwidthHz = gyroConfig()->dyn_notch_bandwidth_hz; dynNotchQ = gyroConfig()->dyn_notch_q / 100.0f;
dynNotchMinHz = gyroConfig()->dyn_notch_min_hz; dynNotchMinHz = gyroConfig()->dyn_notch_min_hz;
dynNotchMaxHz = MAX(2 * dynNotchMinHz, gyroConfig()->dyn_notch_max_hz); dynNotchMaxHz = MAX(2 * dynNotchMinHz, gyroConfig()->dyn_notch_max_hz);
@ -140,7 +140,7 @@ void gyroDataAnalyseInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs)
sdftResolutionHz = (float)sdftSampleRateHz / SDFT_SAMPLE_SIZE; // 13.3hz per bin at 8k sdftResolutionHz = (float)sdftSampleRateHz / SDFT_SAMPLE_SIZE; // 13.3hz per bin at 8k
sdftStartBin = MAX(2, lrintf(dynNotchMinHz / sdftResolutionHz + 0.5f)); // can't use bin 0 because it is DC. sdftStartBin = MAX(2, lrintf(dynNotchMinHz / sdftResolutionHz + 0.5f)); // can't use bin 0 because it is DC.
sdftEndBin = MIN(SDFT_BIN_COUNT - 1, lrintf(dynNotchMaxHz / sdftResolutionHz + 0.5f)); // can't use more than SDFT_BIN_COUNT bins. sdftEndBin = MIN(SDFT_BIN_COUNT - 1, lrintf(dynNotchMaxHz / sdftResolutionHz + 0.5f)); // can't use more than SDFT_BIN_COUNT bins.
smoothFactor = pt1FilterGain(DYN_NOTCH_SMOOTH_HZ, DYN_NOTCH_CALC_TICKS / (float)targetLoopRateHz); // minimum PT1 k value gain = pt1FilterGain(DYN_NOTCH_SMOOTH_HZ, DYN_NOTCH_CALC_TICKS / (float)targetLoopRateHz); // minimum PT1 k value
for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
sdftInit(&sdft[axis], sdftStartBin, sdftEndBin, numSamples); sdftInit(&sdft[axis], sdftStartBin, sdftEndBin, numSamples);
@ -283,45 +283,28 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
// Only update state->centerFreq if there is a peak (ignore void peaks) and if peak is above noise floor // Only update state->centerFreq if there is a peak (ignore void peaks) and if peak is above noise floor
if (peaks[p].bin != 0 && peaks[p].value > sdftMeanSq) { if (peaks[p].bin != 0 && peaks[p].value > sdftMeanSq) {
// accumulate sdftSum and sdftWeightedSum from peak bin, and shoulder bins either side of peak float meanBin = peaks[p].bin;
float squaredData = peaks[p].value; // peak data already squared (see sdftWinSq)
float sdftSum = squaredData;
float sdftWeightedSum = squaredData * peaks[p].bin;
// accumulate upper shoulder unless it would be sdftEndBin // Height of peak bin (y1) and shoulder bins (y0, y2)
uint8_t shoulderBin = peaks[p].bin + 1; const float y0 = sdftData[peaks[p].bin - 1];
if (shoulderBin < sdftEndBin) { const float y1 = sdftData[peaks[p].bin];
squaredData = sdftData[shoulderBin]; // sdftData already squared (see sdftWinSq) const float y2 = sdftData[peaks[p].bin + 1];
sdftSum += squaredData;
sdftWeightedSum += squaredData * shoulderBin; // Estimate true peak position aka. meanBin (fit parabola y(x) over y0, y1 and y2, solve dy/dx=0 for x)
const float denom = 2.0f * (y0 - 2 * y1 + y2);
if (denom != 0.0f) {
meanBin += (y0 - y2) / denom;
} }
// accumulate lower shoulder unless lower shoulder would be bin 0 (DC) // Convert bin to frequency: freq = bin * binResoultion (bin 0 is 0Hz)
if (peaks[p].bin > 1) { const float centerFreq = constrainf(meanBin * sdftResolutionHz, dynNotchMinHz, dynNotchMaxHz);
shoulderBin = peaks[p].bin - 1;
squaredData = sdftData[shoulderBin]; // sdftData already squared (see sdftWinSq)
sdftSum += squaredData;
sdftWeightedSum += squaredData * shoulderBin;
}
// get centerFreq in Hz from weighted bins // PT1 style smoothing moves notch center freqs rapidly towards big peaks and slowly away, up to 8x faster
float centerFreq = dynNotchMaxHz; // DYN_NOTCH_SMOOTH_HZ = 4 & gainMultiplier = 1 .. 8 => PT1 -3dB cutoff frequency = 4Hz .. 41Hz
float sdftMeanBin = 0; const float gainMultiplier = constrainf(peaks[p].value / sdftMeanSq, 1.0f, 8.0f);
if (sdftSum > 0) { // Finally update notch center frequency p on current axis
sdftMeanBin = (sdftWeightedSum / sdftSum); state->centerFreq[state->updateAxis][p] += gain * gainMultiplier * (centerFreq - state->centerFreq[state->updateAxis][p]);
centerFreq = sdftMeanBin * sdftResolutionHz;
centerFreq = constrainf(centerFreq, dynNotchMinHz, dynNotchMaxHz);
// In theory, the index points to the centre frequency of the bin.
// at 1333hz, bin widths are 13.3Hz, so bin 2 (26.7Hz) has the range 20Hz to 33.3Hz
// Rav feels that maybe centerFreq = (sdftMeanBin + 0.5) * sdftResolutionHz is better
// empirical checking shows that not adding 0.5 works better
// PT1 style dynamic smoothing moves rapidly towards big peaks and slowly away, up to 8x faster
// DYN_NOTCH_SMOOTH_HZ = 4 & dynamicFactor = 1 .. 8 => PT1 -3dB cutoff frequency = 4Hz .. 41Hz
const float dynamicFactor = constrainf(peaks[p].value / sdftMeanSq, 1.0f, 8.0f);
state->centerFreq[state->updateAxis][p] += smoothFactor * dynamicFactor * (centerFreq - state->centerFreq[state->updateAxis][p]);
}
} }
} }
@ -347,10 +330,7 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) { for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) {
// Only update notch filter coefficients if the corresponding peak got its center frequency updated in the previous step // Only update notch filter coefficients if the corresponding peak got its center frequency updated in the previous step
if (peaks[p].bin != 0 && peaks[p].value > sdftMeanSq) { if (peaks[p].bin != 0 && peaks[p].value > sdftMeanSq) {
// Choose notch Q in such a way that notch bandwidth stays constant (improves prop wash handling) biquadFilterUpdate(&gyro.notchFilterDyn[state->updateAxis][p], state->centerFreq[state->updateAxis][p], gyro.targetLooptime, dynNotchQ, FILTER_NOTCH);
float dynamicQ = state->centerFreq[state->updateAxis][p] / (float)dynNotchBandwidthHz;
dynamicQ = constrainf(dynamicQ, 2.0f, 10.0f);
biquadFilterUpdate(&gyro.notchFilterDyn[state->updateAxis][p], state->centerFreq[state->updateAxis][p], gyro.targetLooptime, dynamicQ, FILTER_NOTCH);
} }
} }

10
src/main/msp/msp.c
View file

@ -1780,7 +1780,7 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
#if defined(USE_GYRO_DATA_ANALYSE) #if defined(USE_GYRO_DATA_ANALYSE)
sbufWriteU8(dst, 0); // DEPRECATED 1.43: dyn_notch_range sbufWriteU8(dst, 0); // DEPRECATED 1.43: dyn_notch_range
sbufWriteU8(dst, 0); // DEPRECATED 1.44: dyn_notch_width_percent sbufWriteU8(dst, 0); // DEPRECATED 1.44: dyn_notch_width_percent
sbufWriteU16(dst, 0); // DEPRECATED 1.44: dyn_notch_q sbufWriteU16(dst, gyroConfig()->dyn_notch_q);
sbufWriteU16(dst, gyroConfig()->dyn_notch_min_hz); sbufWriteU16(dst, gyroConfig()->dyn_notch_min_hz);
#else #else
sbufWriteU8(dst, 0); sbufWriteU8(dst, 0);
@ -1809,10 +1809,8 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
#endif #endif
#if defined(USE_GYRO_DATA_ANALYSE) #if defined(USE_GYRO_DATA_ANALYSE)
sbufWriteU8(dst, gyroConfig()->dyn_notch_count); sbufWriteU8(dst, gyroConfig()->dyn_notch_count);
sbufWriteU16(dst, gyroConfig()->dyn_notch_bandwidth_hz);
#else #else
sbufWriteU8(dst, 0); sbufWriteU8(dst, 0);
sbufWriteU16(dst, 0);
#endif #endif
break; break;
@ -2656,7 +2654,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
#if defined(USE_GYRO_DATA_ANALYSE) #if defined(USE_GYRO_DATA_ANALYSE)
sbufReadU8(src); // DEPRECATED 1.43: dyn_notch_range sbufReadU8(src); // DEPRECATED 1.43: dyn_notch_range
sbufReadU8(src); // DEPRECATED 1.44: dyn_notch_width_percent sbufReadU8(src); // DEPRECATED 1.44: dyn_notch_width_percent
sbufReadU16(src); // DEPRECATED 1.44: dyn_notch_q gyroConfigMutable()->dyn_notch_q = sbufReadU16(src);
gyroConfigMutable()->dyn_notch_min_hz = sbufReadU16(src); gyroConfigMutable()->dyn_notch_min_hz = sbufReadU16(src);
#else #else
sbufReadU8(src); sbufReadU8(src);
@ -2680,7 +2678,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
sbufReadU16(src); sbufReadU16(src);
#endif #endif
} }
if (sbufBytesRemaining(src) >= 4) { if (sbufBytesRemaining(src) >= 2) {
// Added in MSP API 1.44 // Added in MSP API 1.44
#if defined(USE_DYN_LPF) #if defined(USE_DYN_LPF)
currentPidProfile->dyn_lpf_curve_expo = sbufReadU8(src); currentPidProfile->dyn_lpf_curve_expo = sbufReadU8(src);
@ -2689,10 +2687,8 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
#endif #endif
#if defined(USE_GYRO_DATA_ANALYSE) #if defined(USE_GYRO_DATA_ANALYSE)
gyroConfigMutable()->dyn_notch_count = sbufReadU8(src); gyroConfigMutable()->dyn_notch_count = sbufReadU8(src);
gyroConfigMutable()->dyn_notch_bandwidth_hz = sbufReadU16(src);
#else #else
sbufReadU8(src); sbufReadU8(src);
sbufReadU16(src);
#endif #endif
} }

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

@ -129,8 +129,8 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
gyroConfig->dyn_lpf_gyro_min_hz = DYN_LPF_GYRO_MIN_HZ_DEFAULT; gyroConfig->dyn_lpf_gyro_min_hz = DYN_LPF_GYRO_MIN_HZ_DEFAULT;
gyroConfig->dyn_lpf_gyro_max_hz = DYN_LPF_GYRO_MAX_HZ_DEFAULT; gyroConfig->dyn_lpf_gyro_max_hz = DYN_LPF_GYRO_MAX_HZ_DEFAULT;
gyroConfig->dyn_notch_max_hz = 600; gyroConfig->dyn_notch_max_hz = 600;
gyroConfig->dyn_notch_count = 1; gyroConfig->dyn_notch_count = 3;
gyroConfig->dyn_notch_bandwidth_hz = 45; gyroConfig->dyn_notch_q = 300;
gyroConfig->dyn_notch_min_hz = 150; gyroConfig->dyn_notch_min_hz = 150;
gyroConfig->gyro_filter_debug_axis = FD_ROLL; gyroConfig->gyro_filter_debug_axis = FD_ROLL;
gyroConfig->dyn_lpf_curve_expo = 5; gyroConfig->dyn_lpf_curve_expo = 5;

2
src/main/sensors/gyro.h
View file

@ -200,7 +200,7 @@ typedef struct gyroConfig_s {
uint16_t dyn_notch_max_hz; uint16_t dyn_notch_max_hz;
uint8_t dyn_notch_count; uint8_t dyn_notch_count;
uint16_t dyn_notch_bandwidth_hz; uint16_t dyn_notch_q;
uint16_t dyn_notch_min_hz; uint16_t dyn_notch_min_hz;
uint8_t gyro_filter_debug_axis; uint8_t gyro_filter_debug_axis;