diff --git a/src/main/flight/gyroanalyse.c b/src/main/flight/gyroanalyse.c index 749c263cf..c643f8de2 100644 --- a/src/main/flight/gyroanalyse.c +++ b/src/main/flight/gyroanalyse.c @@ -118,16 +118,9 @@ static uint16_t FAST_DATA_ZERO_INIT dynNotchMaxFFT; static float FAST_DATA_ZERO_INIT smoothFactor; 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 - static bool gyroAnalyseInitialized; - if (gyroAnalyseInitialized) { - return; - } - gyroAnalyseInitialized = true; -#endif - + // initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later dynNotchBandwidthHz = gyroConfig()->dyn_notch_bandwidth_hz; dynNotchMinHz = gyroConfig()->dyn_notch_min_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++) { 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->maxSampleCountRcp = 1.0f / state->maxSampleCount; diff --git a/src/main/flight/gyroanalyse.h b/src/main/flight/gyroanalyse.h index 9b0a7f1df..b5e424627 100644 --- a/src/main/flight/gyroanalyse.h +++ b/src/main/flight/gyroanalyse.h @@ -42,7 +42,7 @@ typedef struct gyroAnalyseState_s { } 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 gyroDataAnalyse(gyroAnalyseState_t *state); uint16_t getMaxFFT(void); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 93ecbab10..1c1a9da44 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -134,8 +134,8 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig) gyroConfig->dyn_notch_min_hz = 150; gyroConfig->gyro_filter_debug_axis = FD_ROLL; gyroConfig->dyn_lpf_curve_expo = 5; - gyroConfig->simplified_gyro_filter = false; - gyroConfig->simplified_gyro_filter_multiplier = SIMPLIFIED_TUNING_DEFAULT; + gyroConfig->simplified_gyro_filter = false; + gyroConfig->simplified_gyro_filter_multiplier = SIMPLIFIED_TUNING_DEFAULT; } #ifdef USE_GYRO_DATA_ANALYSE diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index b18094edf..c7997c07c 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -166,11 +166,11 @@ enum { }; typedef struct gyroConfig_s { - uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. - uint8_t gyro_hardware_lpf; // gyro DLPF setting + uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. + uint8_t gyro_hardware_lpf; // gyro DLPF setting - uint8_t gyro_high_fsr; - uint8_t gyro_to_use; + uint8_t gyro_high_fsr; + uint8_t gyro_to_use; uint16_t gyro_lowpass_hz; uint16_t gyro_lowpass2_hz; @@ -179,15 +179,15 @@ typedef struct gyroConfig_s { uint16_t gyro_soft_notch_cutoff_1; uint16_t gyro_soft_notch_hz_2; uint16_t gyro_soft_notch_cutoff_2; - int16_t gyro_offset_yaw; - uint8_t checkOverflow; + int16_t gyro_offset_yaw; + uint8_t checkOverflow; // Lowpass primary/secondary - uint8_t gyro_lowpass_type; - uint8_t gyro_lowpass2_type; + uint8_t gyro_lowpass_type; + uint8_t gyro_lowpass2_type; - uint8_t yaw_spin_recovery; - int16_t yaw_spin_threshold; + uint8_t yaw_spin_recovery; + int16_t yaw_spin_threshold; uint16_t gyroCalibrationDuration; // Gyro calibration duration in 1/100 second @@ -195,16 +195,16 @@ typedef struct gyroConfig_s { uint16_t dyn_lpf_gyro_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_min_hz; - uint8_t gyro_filter_debug_axis; + uint8_t gyro_filter_debug_axis; uint8_t gyrosDetected; // What gyros should detection be attempted for on startup. Automatically set on first startup. uint8_t dyn_lpf_curve_expo; // set the curve for dynamic gyro lowpass filter - uint8_t simplified_gyro_filter; - uint8_t simplified_gyro_filter_multiplier; + uint8_t simplified_gyro_filter; + uint8_t simplified_gyro_filter_multiplier; } gyroConfig_t; PG_DECLARE(gyroConfig_t, gyroConfig); diff --git a/src/main/sensors/gyro_init.c b/src/main/sensors/gyro_init.c index c41ab1c71..c9380643b 100644 --- a/src/main/sensors/gyro_init.c +++ b/src/main/sensors/gyro_init.c @@ -264,7 +264,7 @@ void gyroInitFilters(void) dynLpfFilterInit(); #endif #ifdef USE_GYRO_DATA_ANALYSE - gyroDataAnalyseStateInit(&gyro.gyroAnalyseState, gyro.targetLooptime); + gyroDataAnalyseInit(&gyro.gyroAnalyseState, gyro.targetLooptime); #endif }