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 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;

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

@ -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);

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

@ -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

28
src/main/sensors/gyro.h
View File

@ -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);

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

@ -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
}