Filter setup via EzTune

src/main/CMakeLists.txt

@@ -331,6 +331,8 @@ main_sources(COMMON_SRC
     flight/secondary_dynamic_gyro_notch.h
     flight/dynamic_lpf.c
     flight/dynamic_lpf.h
+    flight/ez_tune.c
+    flight/ez_tune.h
 
     io/beeper.c
     io/beeper.h

src/main/config/parameter_group_ids.h

@@ -119,7 +119,8 @@
 #define PG_UNUSED_1 1029
 #define PG_POWER_LIMITS_CONFIG 1030
 #define PG_OSD_COMMON_CONFIG 1031
-#define PG_INAV_END 1031
+#define PG_EZ_TUNE 1032
+#define PG_INAV_END PG_EZ_TUNE
 
 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047

src/main/fc/fc_init.c

@@ -96,6 +96,7 @@
 #include "flight/power_limits.h"
 #include "flight/rpm_filter.h"
 #include "flight/servos.h"
+#include "flight/ez_tune.h"
 
 #include "io/asyncfatfs/asyncfatfs.h"
 #include "io/beeper.h"
@@ -509,6 +510,10 @@ void init(void)
     owInit();
 #endif
 
+#ifdef USE_EZ_TUNE
+    ezTuneUpdate();
+#endif
+
     if (!sensorsAutodetect()) {
         // if gyro was not detected due to whatever reason, we give up now.
         failureMode(FAILURE_MISSING_ACC);

src/main/fc/fc_msp.c

@@ -3164,6 +3164,8 @@ static bool mspSettingInfoCommand(sbuf_t *dst, sbuf_t *src)
         sbufWriteU8(dst, 0);
         sbufWriteU8(dst, 0);
         break;
+    case EZ_TUNE_VALUE:
+        FALLTHROUGH;
     case PROFILE_VALUE:
         FALLTHROUGH;
     case CONTROL_RATE_VALUE:

src/main/fc/settings.c

@@ -237,6 +237,8 @@ static uint16_t getValueOffset(const setting_t *value)
         return value->offset + sizeof(pidProfile_t) * getConfigProfile();
     case CONTROL_RATE_VALUE:
         return value->offset + sizeof(controlRateConfig_t) * getConfigProfile();
+	case EZ_TUNE_VALUE:
+		return value->offset + sizeof(ezTuneSettings_t) * getConfigProfile();
     case BATTERY_CONFIG_VALUE:
         return value->offset + sizeof(batteryProfile_t) * getConfigBatteryProfile();
     }

src/main/fc/settings.h

@@ -34,6 +34,7 @@ typedef enum {
     PROFILE_VALUE = (1 << SETTING_SECTION_OFFSET),
     CONTROL_RATE_VALUE = (2 << SETTING_SECTION_OFFSET), // 0x20
     BATTERY_CONFIG_VALUE = (3 << SETTING_SECTION_OFFSET),
+    EZ_TUNE_VALUE = (4 << SETTING_SECTION_OFFSET)
 } setting_section_e;
 
 typedef enum {

src/main/fc/settings.yaml

@@ -1477,6 +1477,23 @@ groups:
         min: 0
         max: 99
 
+  - name: PG_EZ_TUNE
+    headers: ["flight/ez_tune.h"]
+    type: ezTuneSettings_t
+    value_type: EZ_TUNE_VALUE
+    members:
+      - name: ez_tune_enabled
+        description: "Enables EzTune feature"
+        default_value: OFF
+        field: enabled
+        type: bool
+      - name: ez_tune_filter_hz
+        description: "EzTune filter cutoff frequency"
+        default_value: 110
+        field: filterHz
+        min: 10
+        max: 300
+
   - name: PG_RPM_FILTER_CONFIG
     headers: ["flight/rpm_filter.h"]
     condition: USE_RPM_FILTER

src/main/flight/ez_tune.c

@@ -0,0 +1,91 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include "fc/config.h"
+#include "config/config_reset.h"
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
+#include "flight/ez_tune.h"
+
+#include "fc/settings.h"
+#include "flight/pid.h"
+#include "sensors/gyro.h"
+
+PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(ezTuneSettings_t, ezTune, PG_EZ_TUNE, 0);
+
+PG_RESET_TEMPLATE(ezTuneSettings_t, ezTune,
+    .enabled = SETTING_EZ_TUNE_ENABLED_DEFAULT,
+    .filterHz = SETTING_EZ_TUNE_FILTER_HZ_DEFAULT,
+);
+
+static float computePt1FilterDelayMs(uint8_t filterHz) {
+    return 1.0f / (2.0f * M_PIf * filterHz);
+}
+
+/**
+ * Update INAV settings based on current EZTune settings
+ * This has to be called every time control profile is changed, or EZTune settings are changed
+ * FIXME call on profile change
+ * FIXME call on EZTune settings change
+ */
+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;
+
+        //Set main gyro filter
+        gyroConfigMutable()->gyro_main_lpf_hz = ezTune()->filterHz;
+        gyroConfigMutable()->gyro_main_lpf_type = FILTER_PT1;
+
+        //Set anti-aliasing filter
+        gyroConfigMutable()->gyro_anti_aliasing_lpf_hz = MIN(ezTune()->filterHz * 2, 250);
+        gyroConfigMutable()->gyro_anti_aliasing_lpf_type = FILTER_PT1;
+
+        //Enable dynamic notch
+        gyroConfigMutable()->dynamicGyroNotchEnabled = 1;
+        gyroConfigMutable()->dynamicGyroNotchQ = 250;
+        gyroConfigMutable()->dynamicGyroNotchMinHz = MAX(ezTune()->filterHz * 0.667f, SETTING_DYNAMIC_GYRO_NOTCH_MIN_HZ_DEFAULT);
+        gyroConfigMutable()->dynamicGyroNotchMode = DYNAMIC_NOTCH_MODE_3D;
+
+        //Make sure Kalman filter is enabled
+        gyroConfigMutable()->kalmanEnabled = 1;
+        if (ezTune()->filterHz < 150) {
+            gyroConfigMutable()->kalman_q = 200;
+        } else {
+            gyroConfigMutable()->kalman_q = scaleRangef(ezTune()->filterHz, 150, 300, 200, 400);
+        }
+
+        
+
+    }
+}

src/main/flight/ez_tune.h

@@ -0,0 +1,36 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+#include "config/parameter_group.h"
+
+typedef struct ezTuneSettings_s {
+    uint8_t enabled;
+    uint16_t filterHz;
+} ezTuneSettings_t;
+
+PG_DECLARE_PROFILE(ezTuneSettings_t, ezTune);
+
+void ezTuneUpdate(void);

src/main/target/common.h

@@ -192,3 +192,5 @@
 #define USE_HOTT_TEXTMODE
 
 #endif
+
+#define USE_EZ_TUNE