Imrove processing

iNavFlight · Oct 20, 2023 · 927597c · 927597c
1 parent 3f5b320
commit 927597c
Showing 1 changed file with 5 additions and 7 deletions.
diff --git a/src/main/flight/ez_tune.c b/src/main/flight/ez_tune.c
@@ -54,7 +54,7 @@ PG_RESET_TEMPLATE(ezTuneSettings_t, ezTune,
 #define EZ_TUNE_YAW_SCALE 0.5f
 
 static float computePt1FilterDelayMs(uint8_t filterHz) {
-    return 1.0f / (2.0f * M_PIf * filterHz);
+    return 1000.0f / (2.0f * M_PIf * filterHz);
 }
 
 static float getYawPidScale(float input) {
@@ -71,10 +71,6 @@ void ezTuneUpdate(void) {
     if (ezTune()->enabled) {
 
         // Setup filtering
-        //Enable Smith predictor
-        pidProfileMutable()->smithPredictorDelay = computePt1FilterDelayMs(ezTune()->filterHz)
-            + computePt1FilterDelayMs(gyroConfig()->gyro_anti_aliasing_lpf_hz);
-
         //Set Dterm LPF
         pidProfileMutable()->dterm_lpf_hz = MAX(ezTune()->filterHz - 5, 50);
         pidProfileMutable()->dterm_lpf_type = FILTER_PT2;
@@ -84,10 +80,12 @@ void ezTuneUpdate(void) {
         gyroConfigMutable()->gyro_main_lpf_type = FILTER_PT1;
 
         //Set anti-aliasing filter
-        const int gyroNyquist = 1000000 / TASK_GYRO_LOOPTIME;
-        gyroConfigMutable()->gyro_anti_aliasing_lpf_hz = MIN(MIN(ezTune()->filterHz * 2, 250), (int)(gyroNyquist * 0.5f));
+        gyroConfigMutable()->gyro_anti_aliasing_lpf_hz = SETTING_GYRO_ANTI_ALIASING_LPF_HZ_DEFAULT;
         gyroConfigMutable()->gyro_anti_aliasing_lpf_type = FILTER_PT1;
 
+        //Enable Smith predictor
+        pidProfileMutable()->smithPredictorDelay = computePt1FilterDelayMs(ezTune()->filterHz);
+
 #ifdef USE_DYNAMIC_FILTERS
         //Enable dynamic notch
         gyroConfigMutable()->dynamicGyroNotchEnabled = 1;