rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #10662 from klutvott123/dynamic-notch-update-fix 

Allow re-initialising dynamic notch
This commit is contained in:
Michael Keller 2021-05-18 00:53:35 +12:00 committed by GitHub
parent dfcd11bd69 242d5df827
commit adc20088b5
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 20 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/main/flight/gyroanalyse.c
View File

@ -118,16 +118,9 @@ static uint16_t FAST_DATA_ZERO_INIT dynNotchMaxFFT;
static float FAST_DATA_ZERO_INIT smoothFactor; static float FAST_DATA_ZERO_INIT smoothFactor;
static uint8_t FAST_DATA_ZERO_INIT numSamples; static uint8_t FAST_DATA_ZERO_INIT numSamples;
void gyroDataAnalyseInit(uint32_t targetLooptimeUs) void gyroDataAnalyseInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs)
{ {
#ifdef USE_MULTI_GYRO // initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later
static bool gyroAnalyseInitialized;
if (gyroAnalyseInitialized) {
return;
}
gyroAnalyseInitialized = true;
#endif
dynNotchBandwidthHz = gyroConfig()->dyn_notch_bandwidth_hz; dynNotchBandwidthHz = gyroConfig()->dyn_notch_bandwidth_hz;
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);
@ -152,12 +145,7 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
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);
} }
}
void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs)
{
// initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later
gyroDataAnalyseInit(targetLooptimeUs);
state->maxSampleCount = numSamples; state->maxSampleCount = numSamples;
state->maxSampleCountRcp = 1.0f / state->maxSampleCount; state->maxSampleCountRcp = 1.0f / state->maxSampleCount;

2
src/main/flight/gyroanalyse.h
View File

@ -42,7 +42,7 @@ typedef struct gyroAnalyseState_s {
} gyroAnalyseState_t; } gyroAnalyseState_t;
void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs); void gyroDataAnalyseInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs);
void gyroDataAnalysePush(gyroAnalyseState_t *state, const uint8_t axis, const float sample); void gyroDataAnalysePush(gyroAnalyseState_t *state, const uint8_t axis, const float sample);
void gyroDataAnalyse(gyroAnalyseState_t *state); void gyroDataAnalyse(gyroAnalyseState_t *state);
uint16_t getMaxFFT(void); uint16_t getMaxFFT(void);

2
src/main/sensors/gyro_init.c
View File

@ -264,7 +264,7 @@ void gyroInitFilters(void)
dynLpfFilterInit(); dynLpfFilterInit();
#endif #endif
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
gyroDataAnalyseStateInit(&gyro.gyroAnalyseState, gyro.targetLooptime); gyroDataAnalyseInit(&gyro.gyroAnalyseState, gyro.targetLooptime);
#endif #endif
} }