From 472b1c3dd01de68a082f53c9951809662e592baa Mon Sep 17 00:00:00 2001 From: Hans Christian Olaussen <41271048+klutvott123@users.noreply.github.com> Date: Fri, 2 Apr 2021 22:19:37 +0200 Subject: [PATCH 1/2] Allow re-initialising dynamic notch --- src/main/flight/gyroanalyse.c | 16 ++-------------- src/main/flight/gyroanalyse.h | 2 +- src/main/sensors/gyro_init.c | 2 +- 3 files changed, 4 insertions(+), 16 deletions(-) diff --git a/src/main/flight/gyroanalyse.c b/src/main/flight/gyroanalyse.c index 749c263cf8..c643f8de20 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 9b0a7f1df4..b5e4246276 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_init.c b/src/main/sensors/gyro_init.c index 9fda10993d..6c6ac20e03 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 } From 242d5df8279ae6b807aa272b117767c0408496cb Mon Sep 17 00:00:00 2001 From: Hans Christian Olaussen <41271048+klutvott123@users.noreply.github.com> Date: Sat, 3 Apr 2021 20:54:28 +0200 Subject: [PATCH 2/2] tabs and spaces --- src/main/sensors/gyro.c | 4 ++-- src/main/sensors/gyro.h | 28 ++++++++++++++-------------- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 93ecbab10f..1c1a9da44d 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 b18094edf0..c7997c07cb 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);