Refactor

src/main/CMakeLists.txt

@@ -321,6 +321,8 @@ main_sources(COMMON_SRC
     flight/rpm_filter.h
     flight/dynamic_gyro_notch.c
     flight/dynamic_gyro_notch.h
+    flight/dynamic_lpf.c
+    flight/dynamic_lpf.h
 
     io/beeper.c
     io/beeper.h

src/main/build/debug.h

@@ -79,5 +79,6 @@ typedef enum {
     DEBUG_SPM_VS600,    // Smartport master VS600 VTX
     DEBUG_SPM_VARIO,    // Smartport master variometer
     DEBUG_PCF8574,
+    DEBUG_DYNAMIC_GYRO_LPF,
     DEBUG_COUNT
 } debugType_e;

src/main/fc/fc_tasks.c

@@ -294,7 +294,7 @@ void taskUpdateAux(timeUs_t currentTimeUs)
 {
     UNUSED(currentTimeUs);
     updatePIDCoefficients();
-    gyroUpdateDynamicLpf();
+    dynamicLpfGyroTask();
 }
 
 void fcTasksInit(void)

src/main/fc/settings.yaml

@@ -84,7 +84,7 @@ tables:
       "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE",
       "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
       "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY",
-      "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574"]
+      "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
   - name: aux_operator

src/main/flight/dynamic_lpf.c

@@ -0,0 +1,50 @@
+/*
+ * 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 "flight/dynamic_lpf.h"
+#include "sensors/gyro.h"
+#include "flight/mixer.h"
+#include "fc/rc_controls.h"
+#include "build/debug.h"
+
+static float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo) {
+    const float expof = expo / 10.0f;
+    static float curve;
+    curve = throttle * (1 - throttle) * expof + throttle;
+    return (dynLpfMax - dynLpfMin) * curve + dynLpfMin;
+}
+
+void dynamicLpfGyroTask(void) {
+
+    if (!gyroConfig()->useDynamicLpf) {
+        return;
+    }
+
+    const float throttle = scaleRangef((float) rcCommand[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle, 0.0f, 1.0f);
+    const float cutoffFreq = dynLpfCutoffFreq(throttle, gyroConfig()->gyroDynamicLpfMinHz, gyroConfig()->gyroDynamicLpfMaxHz, gyroConfig()->gyroDynamicLpfCurveExpo);
+
+    DEBUG_SET(DEBUG_DYNAMIC_GYRO_LPF, 0, cutoffFreq);
+
+    gyroUpdateDynamicLpf(cutoffFreq);
+}

src/main/flight/dynamic_lpf.h

@@ -0,0 +1,29 @@
+/*
+ * 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 <stdint.h>
+
+void dynamicLpfGyroTask(void);

src/main/sensors/gyro.c

@@ -73,7 +73,6 @@ FILE_COMPILE_FOR_SPEED
 #include "flight/gyroanalyse.h"
 #include "flight/rpm_filter.h"
 #include "flight/dynamic_gyro_notch.h"
-#include "flight/mixer.h"
 
 #ifdef USE_HARDWARE_REVISION_DETECTION
 #include "hardware_revision.h"
@@ -517,21 +516,7 @@ bool gyroSyncCheckUpdate(void)
     return gyroDev[0].intStatusFn(&gyroDev[0]);
 }
 
-static float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo) {
-    const float expof = expo / 10.0f;
-    static float curve;
-    curve = throttle * (1 - throttle) * expof + throttle;
-    return (dynLpfMax - dynLpfMin) * curve + dynLpfMin;
-}
-
-void gyroUpdateDynamicLpf(void) {
-    if (!gyroConfig()->useDynamicLpf) {
-        return;
-    }
-
-    const float throttle = scaleRangef((float) rcCommand[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle, 0.0f, 1.0f);
-    const uint16_t cutoffFreq = dynLpfCutoffFreq(throttle, gyroConfig()->gyroDynamicLpfMinHz, gyroConfig()->gyroDynamicLpfMaxHz, gyroConfig()->gyroDynamicLpfCurveExpo);
-
+void gyroUpdateDynamicLpf(float cutoffFreq) {
     if (gyroConfig()->gyro_soft_lpf_type == FILTER_PT1) {
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
             pt1FilterUpdateCutoff(&gyroLpfState[axis].pt1, cutoffFreq);

src/main/sensors/gyro.h

@@ -91,4 +91,4 @@ bool gyroReadTemperature(void);
 int16_t gyroGetTemperature(void);
 int16_t gyroRateDps(int axis);
 bool gyroSyncCheckUpdate(void);
-void gyroUpdateDynamicLpf(void);
+void gyroUpdateDynamicLpf(float cutoffFreq);