From 28448bd7e1259081b3ea5d23de0923b73b140401 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Thu, 19 Nov 2020 19:21:12 +0100
Subject: [PATCH 01/23] Precompute k for PT1 filter

---
 src/main/common/filter.c | 6 ++++--
 src/main/common/filter.h | 1 +
 2 files changed, 5 insertions(+), 2 deletions(-)

diff --git a/src/main/common/filter.c b/src/main/common/filter.c
index b3683c3edc..4a3c37aba5 100644
--- a/src/main/common/filter.c
+++ b/src/main/common/filter.c
@@ -54,6 +54,7 @@ void pt1FilterInitRC(pt1Filter_t *filter, float tau, float dT)
     filter->state = 0.0f;
     filter->RC = tau;
     filter->dT = dT;
+    filter->k = filter->dT / (filter->RC + filter->dT);
 }
 
 void pt1FilterInit(pt1Filter_t *filter, float f_cut, float dT)
@@ -71,7 +72,7 @@ float pt1FilterGetLastOutput(pt1Filter_t *filter) {
 
 float FAST_CODE NOINLINE pt1FilterApply(pt1Filter_t *filter, float input)
 {
-    filter->state = filter->state + filter->dT / (filter->RC + filter->dT) * (input - filter->state);
+    filter->state = filter->state + filter->k * (input - filter->state);
     return filter->state;
 }
 
@@ -90,7 +91,8 @@ float FAST_CODE NOINLINE pt1FilterApply4(pt1Filter_t *filter, float input, float
     }
 
     filter->dT = dT;    // cache latest dT for possible use in pt1FilterApply
-    filter->state = filter->state + dT / (filter->RC + dT) * (input - filter->state);
+    filter->k = filter->dT / (filter->RC + filter->dT);
+    filter->state = filter->state + filter->k * (input - filter->state);
     return filter->state;
 }
 
diff --git a/src/main/common/filter.h b/src/main/common/filter.h
index 258b13dbe1..e083f9bce7 100644
--- a/src/main/common/filter.h
+++ b/src/main/common/filter.h
@@ -25,6 +25,7 @@ typedef struct pt1Filter_s {
     float state;
     float RC;
     float dT;
+    float k;
 } pt1Filter_t;
 
 /* this holds the data required to update samples thru a filter */

From 1b4c517de8255e3007192c2f2335a0c9d0c685bf Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Thu, 19 Nov 2020 19:33:14 +0100
Subject: [PATCH 02/23] Parameter rename

---
 src/main/common/filter.c | 8 ++++----
 src/main/common/filter.h | 2 +-
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/main/common/filter.c b/src/main/common/filter.c
index 4a3c37aba5..1cdba06c19 100644
--- a/src/main/common/filter.c
+++ b/src/main/common/filter.c
@@ -54,7 +54,7 @@ void pt1FilterInitRC(pt1Filter_t *filter, float tau, float dT)
     filter->state = 0.0f;
     filter->RC = tau;
     filter->dT = dT;
-    filter->k = filter->dT / (filter->RC + filter->dT);
+    filter->alpha = filter->dT / (filter->RC + filter->dT);
 }
 
 void pt1FilterInit(pt1Filter_t *filter, float f_cut, float dT)
@@ -72,7 +72,7 @@ float pt1FilterGetLastOutput(pt1Filter_t *filter) {
 
 float FAST_CODE NOINLINE pt1FilterApply(pt1Filter_t *filter, float input)
 {
-    filter->state = filter->state + filter->k * (input - filter->state);
+    filter->state = filter->state + filter->alpha * (input - filter->state);
     return filter->state;
 }
 
@@ -91,8 +91,8 @@ float FAST_CODE NOINLINE pt1FilterApply4(pt1Filter_t *filter, float input, float
     }
 
     filter->dT = dT;    // cache latest dT for possible use in pt1FilterApply
-    filter->k = filter->dT / (filter->RC + filter->dT);
-    filter->state = filter->state + filter->k * (input - filter->state);
+    filter->alpha = filter->dT / (filter->RC + filter->dT);
+    filter->state = filter->state + filter->alpha * (input - filter->state);
     return filter->state;
 }
 
diff --git a/src/main/common/filter.h b/src/main/common/filter.h
index e083f9bce7..b0e782c5c9 100644
--- a/src/main/common/filter.h
+++ b/src/main/common/filter.h
@@ -25,7 +25,7 @@ typedef struct pt1Filter_s {
     float state;
     float RC;
     float dT;
-    float k;
+    float alpha;
 } pt1Filter_t;
 
 /* this holds the data required to update samples thru a filter */

From 3e2bfec8c6597addeedce83378d4d9f6a819b54d Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Thu, 19 Nov 2020 21:20:41 +0100
Subject: [PATCH 03/23] Settings for Dynamic LPF filter

---
 src/main/common/filter.c  | 15 +++++++++++++--
 src/main/common/filter.h  |  1 +
 src/main/fc/settings.yaml | 23 +++++++++++++++++++++++
 src/main/sensors/gyro.c   |  6 +++++-
 src/main/sensors/gyro.h   |  4 ++++
 5 files changed, 46 insertions(+), 3 deletions(-)

diff --git a/src/main/common/filter.c b/src/main/common/filter.c
index 1cdba06c19..4bc2f88425 100644
--- a/src/main/common/filter.c
+++ b/src/main/common/filter.c
@@ -48,6 +48,11 @@ float nullFilterApply4(void *filter, float input, float f_cut, float dt)
 
 // PT1 Low Pass filter
 
+static float pt1ComputeRC(const float f_cut)
+{
+    return 1.0f / (2.0f * M_PIf * f_cut);
+}
+
 // f_cut = cutoff frequency
 void pt1FilterInitRC(pt1Filter_t *filter, float tau, float dT)
 {
@@ -59,7 +64,7 @@ void pt1FilterInitRC(pt1Filter_t *filter, float tau, float dT)
 
 void pt1FilterInit(pt1Filter_t *filter, float f_cut, float dT)
 {
-    pt1FilterInitRC(filter, 1.0f / (2.0f * M_PIf * f_cut), dT);
+    pt1FilterInitRC(filter, pt1ComputeRC(f_cut), dT);
 }
 
 void pt1FilterSetTimeConstant(pt1Filter_t *filter, float tau) {
@@ -70,6 +75,12 @@ float pt1FilterGetLastOutput(pt1Filter_t *filter) {
     return filter->state;
 }
 
+void pt1FilterUpdateCutoff(pt1Filter_t *filter, float f_cut)
+{
+    filter->RC = pt1ComputeRC(f_cut);
+    filter->alpha = filter->dT / (filter->RC + filter->dT);
+}
+
 float FAST_CODE NOINLINE pt1FilterApply(pt1Filter_t *filter, float input)
 {
     filter->state = filter->state + filter->alpha * (input - filter->state);
@@ -87,7 +98,7 @@ float FAST_CODE NOINLINE pt1FilterApply4(pt1Filter_t *filter, float input, float
 {
     // Pre calculate and store RC
     if (!filter->RC) {
-        filter->RC = 1.0f / ( 2.0f * M_PIf * f_cut );
+        filter->RC = pt1ComputeRC(f_cut);
     }
 
     filter->dT = dT;    // cache latest dT for possible use in pt1FilterApply
diff --git a/src/main/common/filter.h b/src/main/common/filter.h
index b0e782c5c9..ed8500ecdd 100644
--- a/src/main/common/filter.h
+++ b/src/main/common/filter.h
@@ -65,6 +65,7 @@ float nullFilterApply4(void *filter, float input, float f_cut, float dt);
 void pt1FilterInit(pt1Filter_t *filter, float f_cut, float dT);
 void pt1FilterInitRC(pt1Filter_t *filter, float tau, float dT);
 void pt1FilterSetTimeConstant(pt1Filter_t *filter, float tau);
+void pt1FilterUpdateCutoff(pt1Filter_t *filter, float f_cut);
 float pt1FilterGetLastOutput(pt1Filter_t *filter);
 float pt1FilterApply(pt1Filter_t *filter, float input);
 float pt1FilterApply3(pt1Filter_t *filter, float input, float dT);
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 62c5496df7..79b9d007a0 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -214,6 +214,29 @@ groups:
         default_value: "`BIQUAD`"
         field: gyro_stage2_lowpass_type
         table: filter_type
+      - name: gyro_use_dyn_lpf
+        description: "Use Dynamic LPF instead of static gyro stage1 LPF."
+        default_value: "OFF"
+        field: useDynamicLpf
+        type: bool
+      - name: gyro_dyn_lpf_min_hz
+        description: "Minimum frequency of the gyro Dynamic LPF"
+        default_value: "200"
+        field: gyroDynamicLpfMinHz
+        min: 40
+        max: 400
+      - name: gyro_dyn_lpf_max_hz
+        description: "Maximum frequency of the gyro Dynamic LPF"
+        default_value: "500"
+        field: gyroDynamicLpfMaxHz
+        min: 40
+        max: 1000
+      - name: gyro_dyn_lpf_curve_expo
+        description: "Expo value for the throttle-to-frequency mapping for Dynamic LPF"
+        default_value: "5"
+        field: gyroDynamicLpFCurveExpo
+        min: 0
+        max: 10
       - name: dynamic_gyro_notch_enabled
         description: "Enable/disable dynamic gyro notch also known as Matrix Filter"
         default_value: "`OFF`"
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index d9cd15891d..0518d8f229 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -101,7 +101,7 @@ EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState;
 
 #endif
 
-PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 10);
+PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 11);
 
 PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .gyro_lpf = GYRO_LPF_42HZ,      // 42HZ value is defined for Invensense/TDK gyros
@@ -116,6 +116,10 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .gyro_notch_cutoff = 1,
     .gyro_stage2_lowpass_hz = 0,
     .gyro_stage2_lowpass_type = FILTER_BIQUAD,
+    .useDynamicLpf = 0,
+    .gyroDynamicLpfMinHz = 200,
+    .gyroDynamicLpfMaxHz = 500,
+    .gyroDynamicLpFCurveExpo = 5,
     .dynamicGyroNotchRange = DYN_NOTCH_RANGE_MEDIUM,
     .dynamicGyroNotchQ = 120,
     .dynamicGyroNotchMinHz = 150,
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index b709294b55..57e6c484ff 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -70,6 +70,10 @@ typedef struct gyroConfig_s {
     uint16_t gyro_notch_cutoff;
     uint16_t gyro_stage2_lowpass_hz;
     uint8_t gyro_stage2_lowpass_type;
+    uint8_t useDynamicLpf;
+    uint16_t gyroDynamicLpfMinHz;
+    uint16_t gyroDynamicLpfMaxHz;
+    uint8_t gyroDynamicLpFCurveExpo;
     uint8_t dynamicGyroNotchRange;
     uint16_t dynamicGyroNotchQ;
     uint16_t dynamicGyroNotchMinHz;

From cf1b17181f76a2461d46bd49f7ab81954cdf6ff9 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Fri, 20 Nov 2020 15:15:59 +0100
Subject: [PATCH 04/23] Dynamic update of the gyro stage1 LPF

---
 src/main/common/filter.c |  3 ---
 src/main/common/filter.h |  3 +++
 src/main/fc/fc_tasks.c   |  3 ++-
 src/main/sensors/gyro.c  | 39 +++++++++++++++++++++++++++++++++++++++
 src/main/sensors/gyro.h  |  1 +
 5 files changed, 45 insertions(+), 4 deletions(-)

diff --git a/src/main/common/filter.c b/src/main/common/filter.c
index 4bc2f88425..3a4c9797ef 100644
--- a/src/main/common/filter.c
+++ b/src/main/common/filter.c
@@ -28,9 +28,6 @@ FILE_COMPILE_FOR_SPEED
 #include "common/maths.h"
 #include "common/utils.h"
 
-#define BIQUAD_BANDWIDTH 1.9f     /* bandwidth in octaves */
-#define BIQUAD_Q 1.0f / sqrtf(2.0f)     /* quality factor - butterworth*/
-
 // NULL filter
 float nullFilterApply(void *filter, float input)
 {
diff --git a/src/main/common/filter.h b/src/main/common/filter.h
index ed8500ecdd..81c76cd0dc 100644
--- a/src/main/common/filter.h
+++ b/src/main/common/filter.h
@@ -59,6 +59,9 @@ typedef struct firFilter_s {
 typedef float (*filterApplyFnPtr)(void *filter, float input);
 typedef float (*filterApply4FnPtr)(void *filter, float input, float f_cut, float dt);
 
+#define BIQUAD_BANDWIDTH 1.9f     /* bandwidth in octaves */
+#define BIQUAD_Q 1.0f / sqrtf(2.0f)     /* quality factor - butterworth*/
+
 float nullFilterApply(void *filter, float input);
 float nullFilterApply4(void *filter, float input, float f_cut, float dt);
 
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index 190a299d9d..3a4517cf56 100755
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -294,6 +294,7 @@ void taskUpdateAux(timeUs_t currentTimeUs)
 {
     UNUSED(currentTimeUs);
     updatePIDCoefficients();
+    gyroUpdateDynamicLpf();
 }
 
 void fcTasksInit(void)
@@ -620,7 +621,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
     [TASK_AUX] = {
         .taskName = "AUX",
         .taskFunc = taskUpdateAux,
-        .desiredPeriod = TASK_PERIOD_HZ(TASK_AUX_RATE_HZ),          // 300Hz @3,33ms
+        .desiredPeriod = TASK_PERIOD_HZ(TASK_AUX_RATE_HZ),          // 100Hz @10ms
         .staticPriority = TASK_PRIORITY_HIGH,
     },
 };
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 0518d8f229..5006c3509d 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -59,6 +59,7 @@ FILE_COMPILE_FOR_SPEED
 
 #include "fc/config.h"
 #include "fc/runtime_config.h"
+#include "fc/rc_controls.h"
 
 #include "io/beeper.h"
 #include "io/statusindicator.h"
@@ -72,6 +73,7 @@ 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"
@@ -514,3 +516,40 @@ 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;
+}
+
+static float dynThrottle(float throttle) {
+    return throttle * (1 - (throttle * throttle) / 3.0f) * 1.5f;
+}
+
+void gyroUpdateDynamicLpf(void) {
+    if (!gyroConfig()->useDynamicLpf) {
+        return;
+    }
+
+    const float throttle = scaleRangef((float) rcCommand[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle, 0.0f, 1.0f);
+
+    uint16_t cutoffFreq;
+    if (gyroConfig()->gyroDynamicLpFCurveExpo > 0) {
+        cutoffFreq = dynLpfCutoffFreq(throttle, gyroConfig()->gyroDynamicLpfMinHz, gyroConfig()->gyroDynamicLpfMaxHz, gyroConfig()->gyroDynamicLpFCurveExpo);
+    } else {
+        cutoffFreq = fmax(dynThrottle(throttle) * gyroConfig()->gyroDynamicLpfMaxHz, gyroConfig()->gyroDynamicLpfMinHz);
+    }
+
+    if (gyroConfig()->gyro_soft_lpf_type == FILTER_PT1) {
+        const float gyroDt = getLooptime() * 1e-6f;
+        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+            pt1FilterUpdateCutoff(&gyroLpfState[axis].pt1, cutoffFreq);
+        }
+    } else if (gyroConfig()->gyro_soft_lpf_type == FILTER_BIQUAD) {
+        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+            biquadFilterUpdate(&gyroLpfState[axis].biquad, cutoffFreq, getLooptime(), BIQUAD_Q, FILTER_LPF);
+        }
+    }
+}
\ No newline at end of file
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index 57e6c484ff..b5bf66a4b2 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -91,3 +91,4 @@ bool gyroReadTemperature(void);
 int16_t gyroGetTemperature(void);
 int16_t gyroRateDps(int axis);
 bool gyroSyncCheckUpdate(void);
+void gyroUpdateDynamicLpf(void);

From 390987597634831791cd20a1f4b18add53a770a1 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Fri, 20 Nov 2020 16:00:58 +0100
Subject: [PATCH 05/23] Remove unused variable

---
 src/main/sensors/gyro.c | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 5006c3509d..7adadf2c10 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -543,7 +543,6 @@ void gyroUpdateDynamicLpf(void) {
     }
 
     if (gyroConfig()->gyro_soft_lpf_type == FILTER_PT1) {
-        const float gyroDt = getLooptime() * 1e-6f;
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
             pt1FilterUpdateCutoff(&gyroLpfState[axis].pt1, cutoffFreq);
         }

From fa077fa1868f7e3fa4aa57c61d086a5fe2901884 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sat, 28 Nov 2020 18:28:54 +0100
Subject: [PATCH 06/23] PID Progrsmming Framework pt1

---
 src/main/CMakeLists.txt                 |  2 ++
 src/main/programming/pid.c              | 38 +++++++++++++++++++++++++
 src/main/programming/pid.h              | 30 +++++++++++++++++++
 src/main/programming/programming_task.c |  2 ++
 4 files changed, 72 insertions(+)
 create mode 100644 src/main/programming/pid.c
 create mode 100644 src/main/programming/pid.h

diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt
index 0bd201a9fb..6bf6e2ec60 100644
--- a/src/main/CMakeLists.txt
+++ b/src/main/CMakeLists.txt
@@ -366,6 +366,8 @@ main_sources(COMMON_SRC
     programming/global_variables.h
     programming/programming_task.c
     programming/programming_task.h
+    programming/pid.c
+    programming/pid.h
 
     rx/crsf.c
     rx/crsf.h
diff --git a/src/main/programming/pid.c b/src/main/programming/pid.c
new file mode 100644
index 0000000000..1040980c1c
--- /dev/null
+++ b/src/main/programming/pid.c
@@ -0,0 +1,38 @@
+/*
+ * 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 "platform.h"
+
+FILE_COMPILE_FOR_SIZE
+
+#ifdef USE_PROGRAMMING_FRAMEWORK
+
+#include "programming/pid.h"
+
+void programmingPidUpdateTask(timeUs_t currentTimeUs)
+{
+    //Dummy
+}
+
+#endif
\ No newline at end of file
diff --git a/src/main/programming/pid.h b/src/main/programming/pid.h
new file mode 100644
index 0000000000..8869a0d6c0
--- /dev/null
+++ b/src/main/programming/pid.h
@@ -0,0 +1,30 @@
+/*
+ * 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"
+#include "common/time.h"
+
+void programmingPidUpdateTask(timeUs_t currentTimeUs);
\ No newline at end of file
diff --git a/src/main/programming/programming_task.c b/src/main/programming/programming_task.c
index ba26217fc3..19257cb736 100644
--- a/src/main/programming/programming_task.c
+++ b/src/main/programming/programming_task.c
@@ -27,7 +27,9 @@
 FILE_COMPILE_FOR_SIZE
 
 #include "programming/logic_condition.h"
+#include "programming/pid.h"
 
 void programmingFrameworkUpdateTask(timeUs_t currentTimeUs) {
+    programmingPidUpdateTask(currentTimeUs);
     logicConditionUpdateTask(currentTimeUs);
 }
\ No newline at end of file

From 4f3abed3ef07809f62f4e778d52f2d781d03040d Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Wed, 2 Dec 2020 17:26:56 +0100
Subject: [PATCH 07/23] Configuration

---
 src/main/config/parameter_group_ids.h |  3 +-
 src/main/programming/pid.c            | 41 +++++++++++++++++++++++++++
 src/main/programming/pid.h            | 24 +++++++++++++++-
 3 files changed, 66 insertions(+), 2 deletions(-)

diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h
index 47c1faeb05..6e526ea610 100644
--- a/src/main/config/parameter_group_ids.h
+++ b/src/main/config/parameter_group_ids.h
@@ -115,7 +115,8 @@
 #define PG_OSD_LAYOUTS_CONFIG 1025
 #define PG_SAFE_HOME_CONFIG 1026
 #define PG_DJI_OSD_CONFIG 1027
-#define PG_INAV_END 1027
+#define PG_PROGRAMMING_PID 1028
+#define PG_INAV_END 1028
 
 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047
diff --git a/src/main/programming/pid.c b/src/main/programming/pid.c
index 1040980c1c..9b09e2ea9d 100644
--- a/src/main/programming/pid.c
+++ b/src/main/programming/pid.c
@@ -28,11 +28,52 @@ FILE_COMPILE_FOR_SIZE
 
 #ifdef USE_PROGRAMMING_FRAMEWORK
 
+#include "config/config_reset.h"
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
 #include "programming/pid.h"
 
+EXTENDED_FASTRAM programmingPidState_t programmingPidState[MAX_PROGRAMMING_PID_COUNT];
+
+PG_REGISTER_ARRAY_WITH_RESET_FN(programmingPid_t, MAX_PROGRAMMING_PID_COUNT, programmingPids, PG_PROGRAMMING_PID, 1);
+
+void pgResetFn_programmingPids(programmingPid_t *instance)
+{
+    for (int i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
+        RESET_CONFIG(programmingPid_t, &instance[i],
+            .enabled = 0,
+            .setpoint = {
+                .type = LOGIC_CONDITION_OPERAND_TYPE_VALUE,
+                .value = 0
+            },
+            .measurement = {
+                .type = LOGIC_CONDITION_OPERAND_TYPE_VALUE,
+                .value = 0
+            },
+            .gains = {
+                .P = 0,
+                .I = 0,
+                .D = 0,
+                .FF = 0,
+            }
+        );
+    }
+}
+
 void programmingPidUpdateTask(timeUs_t currentTimeUs)
 {
+    UNUSED(currentTimeUs);
     //Dummy
 }
 
+void programmingPidInit(void)
+{
+    for (uint8_t i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
+        navPidInit(
+
+        );
+    }
+}
+
 #endif
\ No newline at end of file
diff --git a/src/main/programming/pid.h b/src/main/programming/pid.h
index 8869a0d6c0..810b9cfa3c 100644
--- a/src/main/programming/pid.h
+++ b/src/main/programming/pid.h
@@ -27,4 +27,26 @@
 #include "config/parameter_group.h"
 #include "common/time.h"
 
-void programmingPidUpdateTask(timeUs_t currentTimeUs);
\ No newline at end of file
+#include "programming/logic_condition.h"
+#include "common/axis.h"
+#include "flight/pid.h"
+#include "navigation/navigation.h"
+
+#define MAX_PROGRAMMING_PID_COUNT 2
+
+typedef struct programmingPid_s {
+    uint8_t enabled;
+    logicOperand_t setpoint;
+    logicOperand_t measurement;
+    pid8_t gains;
+} programmingPid_t;
+
+PG_DECLARE_ARRAY(programmingPid_t, MAX_PROGRAMMING_PID_COUNT, programmingPids);
+
+typedef struct programmingPidState_s {
+    pidController_t controller;
+    logicOperand_t setpoint;
+} programmingPidState_t;
+
+void programmingPidUpdateTask(timeUs_t currentTimeUs);
+void programmingPidInit(void);
\ No newline at end of file

From 3dd995bd328a07e5bf9fc5ea824f357b6bfe9816 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Thu, 3 Dec 2020 20:42:51 +0100
Subject: [PATCH 08/23] Initiate PID controllers

---
 src/main/navigation/navigation_private.h |  2 ++
 src/main/programming/pid.c               | 16 ++++++++++++++--
 2 files changed, 16 insertions(+), 2 deletions(-)

diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
index fcae1b7ded..f2b348d650 100755
--- a/src/main/navigation/navigation_private.h
+++ b/src/main/navigation/navigation_private.h
@@ -25,7 +25,9 @@
 #include "common/maths.h"
 #include "common/filter.h"
 #include "common/time.h"
+#include "common/vector.h"
 #include "fc/runtime_config.h"
+#include "navigation/navigation.h"
 
 #define MIN_POSITION_UPDATE_RATE_HZ         5       // Minimum position update rate at which XYZ controllers would be applied
 #define NAV_THROTTLE_CUTOFF_FREQENCY_HZ     4       // low-pass filter on throttle output
diff --git a/src/main/programming/pid.c b/src/main/programming/pid.c
index 9b09e2ea9d..a6882b8190 100644
--- a/src/main/programming/pid.c
+++ b/src/main/programming/pid.c
@@ -31,10 +31,12 @@ FILE_COMPILE_FOR_SIZE
 #include "config/config_reset.h"
 #include "config/parameter_group.h"
 #include "config/parameter_group_ids.h"
+#include "navigation/navigation_private.h"
 
 #include "programming/pid.h"
 
 EXTENDED_FASTRAM programmingPidState_t programmingPidState[MAX_PROGRAMMING_PID_COUNT];
+static bool pidsInitiated = false;
 
 PG_REGISTER_ARRAY_WITH_RESET_FN(programmingPid_t, MAX_PROGRAMMING_PID_COUNT, programmingPids, PG_PROGRAMMING_PID, 1);
 
@@ -64,14 +66,24 @@ void pgResetFn_programmingPids(programmingPid_t *instance)
 void programmingPidUpdateTask(timeUs_t currentTimeUs)
 {
     UNUSED(currentTimeUs);
-    //Dummy
+    
+    if (!pidsInitiated) {
+        programmingPidInit();
+        pidsInitiated = true;
+    }
+
 }
 
 void programmingPidInit(void)
 {
     for (uint8_t i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
         navPidInit(
-
+            &programmingPidState[i].controller,
+            programmingPids(i)->gains.P / 100.0f,
+            programmingPids(i)->gains.I / 100.0f,
+            programmingPids(i)->gains.D / 100.0f,
+            programmingPids(i)->gains.FF / 100.0f,
+            5.0f
         );
     }
 }

From ebf581c871e61c50d9d62bb34bf824008405d04a Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Thu, 3 Dec 2020 21:22:06 +0100
Subject: [PATCH 09/23] WIP

---
 src/main/fc/cli.c          | 112 +++++++++++++++++++++++++++++++++++++
 src/main/programming/pid.c |   1 +
 2 files changed, 113 insertions(+)

diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index 53c7fdb58c..172e0fb47f 100755
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -2001,6 +2001,111 @@ static void cliGvar(char *cmdline) {
     }
 }
 
+static void printPid(uint8_t dumpMask, const logicCondition_t *logicConditions, const logicCondition_t *defaultLogicConditions)
+{
+    const char *format = "pid %d %d %d %d %d %d %d %d %d %d";
+    for (uint32_t i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
+        const logicCondition_t logic = logicConditions[i];
+
+        bool equalsDefault = false;
+        if (defaultLogicConditions) {
+            logicCondition_t defaultValue = defaultLogicConditions[i];
+            equalsDefault =
+                logic.enabled == defaultValue.enabled &&
+                logic.activatorId == defaultValue.activatorId &&
+                logic.operation == defaultValue.operation &&
+                logic.operandA.type == defaultValue.operandA.type &&
+                logic.operandA.value == defaultValue.operandA.value &&
+                logic.operandB.type == defaultValue.operandB.type &&
+                logic.operandB.value == defaultValue.operandB.value &&
+                logic.flags == defaultValue.flags;
+
+            cliDefaultPrintLinef(dumpMask, equalsDefault, format,
+                i,
+                logic.enabled,
+                logic.activatorId,
+                logic.operation,
+                logic.operandA.type,
+                logic.operandA.value,
+                logic.operandB.type,
+                logic.operandB.value,
+                logic.flags
+            );
+        }
+        cliDumpPrintLinef(dumpMask, equalsDefault, format,
+            i,
+            logic.enabled,
+            logic.activatorId,
+            logic.operation,
+            logic.operandA.type,
+            logic.operandA.value,
+            logic.operandB.type,
+            logic.operandB.value,
+            logic.flags
+        );
+    }
+}
+
+static void cliPid(char *cmdline) {
+    char * saveptr;
+    int args[10], check = 0;
+    uint8_t len = strlen(cmdline);
+
+    if (len == 0) {
+        printLogic(DUMP_MASTER, programmingPids(0), NULL);
+    } else if (sl_strncasecmp(cmdline, "reset", 5) == 0) {
+        pgResetCopy(programmingPidsMutable(0), PG_LOGIC_CONDITIONS);
+    } else {
+        enum {
+            INDEX = 0,
+            ENABLED,
+            SETPOINT_TYPE,
+            SETPOINT_VALUE,
+            MEASUREMENT_TYPE,
+            MEASUREMENT_VALUE,
+            P_GAIN,
+            I_GAIN,
+            D_GAIN,
+            FF_GAIN,
+            ARGS_COUNT
+            };
+        char *ptr = strtok_r(cmdline, " ", &saveptr);
+        while (ptr != NULL && check < ARGS_COUNT) {
+            args[check++] = fastA2I(ptr);
+            ptr = strtok_r(NULL, " ", &saveptr);
+        }
+
+        if (ptr != NULL || check != ARGS_COUNT) {
+            cliShowParseError();
+            return;
+        }
+
+        int32_t i = args[INDEX];
+        if (
+            i >= 0 && i < MAX_LOGIC_CONDITIONS &&
+            args[ENABLED] >= 0 && args[ENABLED] <= 1 &&
+            args[ACTIVATOR_ID] >= -1 && args[ACTIVATOR_ID] < MAX_LOGIC_CONDITIONS &&
+            args[OPERATION] >= 0 && args[OPERATION] < LOGIC_CONDITION_LAST &&
+            args[OPERAND_A_TYPE] >= 0 && args[OPERAND_A_TYPE] < LOGIC_CONDITION_OPERAND_TYPE_LAST &&
+            args[OPERAND_A_VALUE] >= -1000000 && args[OPERAND_A_VALUE] <= 1000000 &&
+            args[OPERAND_B_TYPE] >= 0 && args[OPERAND_B_TYPE] < LOGIC_CONDITION_OPERAND_TYPE_LAST &&
+            args[OPERAND_B_VALUE] >= -1000000 && args[OPERAND_B_VALUE] <= 1000000 &&
+            args[FLAGS] >= 0 && args[FLAGS] <= 255
+
+        ) {
+            programmingPidsMutable(i)->enabled = args[ENABLED];
+            logicConditionsMutable(i)->setpoint.type = args[OPERAND_A_TYPE];
+            logicConditionsMutable(i)->setpoint.value = args[OPERAND_A_VALUE];
+            logicConditionsMutable(i)->measurement.type = args[OPERAND_B_TYPE];
+            logicConditionsMutable(i)->measurement.value = args[OPERAND_B_VALUE];
+
+            cliLogic("");
+        } else {
+            cliShowParseError();
+        }
+    }
+}
+
 #endif
 
 #ifdef USE_SDCARD
@@ -3315,6 +3420,9 @@ static void printConfig(const char *cmdline, bool doDiff)
 
         cliPrintHashLine("gvar");
         printGvar(dumpMask, globalVariableConfigs_CopyArray, globalVariableConfigs(0));
+
+        cliPrintHashLine("pid");
+        printLogic(dumpMask, programmingPids_CopyArray, programmingPids(0));
 #endif
 
         cliPrintHashLine("feature");
@@ -3569,6 +3677,10 @@ const clicmd_t cmdTable[] = {
     CLI_COMMAND_DEF("gvar", "configure global variables",
         "<gvar> <default> <min> <max>\r\n"
         "\treset\r\n", cliGvar),
+
+    CLI_COMMAND_DEF("pid", "configurable PID controllers",
+        "<#> <enabled> <setpoint type> <setpoint value> <measurement type> <measurement value> <P gain> <I gain> <D gain> <FF gain>\r\n"
+        "\treset\r\n", cliPid),
 #endif
     CLI_COMMAND_DEF("set", "change setting", "[<name>=<value>]", cliSet),
     CLI_COMMAND_DEF("smix", "servo mixer",
diff --git a/src/main/programming/pid.c b/src/main/programming/pid.c
index a6882b8190..eb680f6687 100644
--- a/src/main/programming/pid.c
+++ b/src/main/programming/pid.c
@@ -28,6 +28,7 @@ FILE_COMPILE_FOR_SIZE
 
 #ifdef USE_PROGRAMMING_FRAMEWORK
 
+#include "common/utils.h"
 #include "config/config_reset.h"
 #include "config/parameter_group.h"
 #include "config/parameter_group_ids.h"

From d32fe6dea50a512e40a02a913e1d2ac856937333 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Fri, 4 Dec 2020 20:38:34 +0100
Subject: [PATCH 10/23] Move PID gains to UINT16 instead of UINT8. Keep it
 compatible on the MSP level

---
 docs/Programming Framework.md            |  1 +
 src/main/cms/cms_menu_imu.c              |  2 +-
 src/main/fc/cli.c                        | 98 +++++++++++++-----------
 src/main/fc/fc_core.c                    |  3 +
 src/main/fc/fc_msp.c                     | 14 ++--
 src/main/flight/pid.h                    |  8 +-
 src/main/navigation/navigation.c         |  4 +
 src/main/navigation/navigation_private.h |  1 +
 src/main/programming/logic_condition.c   |  7 ++
 src/main/programming/logic_condition.h   |  1 +
 src/main/programming/pid.c               | 40 ++++++++--
 src/main/programming/pid.h               |  8 +-
 12 files changed, 122 insertions(+), 65 deletions(-)

diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md
index 2034bc0da1..fc11010f1f 100644
--- a/docs/Programming Framework.md	
+++ b/docs/Programming Framework.md	
@@ -80,6 +80,7 @@ IPF can be edited using INAV Configurator user interface, of via CLI
 | 3             | FLIGHT_MODE   | `value` points to flight modes table                  |
 | 4             | LC            | `value` points to other logic condition ID            |
 | 5             | GVAR          | Value stored in Global Variable indexed by `value`. `GVAR 1` means: value in GVAR 1 |
+| 5             | PID          | Output of a Programming PID indexed by `value`. `PID 1` means: value in PID 1 |
 
 #### FLIGHT
 
diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c
index 07e56539b7..68311250c7 100644
--- a/src/main/cms/cms_menu_imu.c
+++ b/src/main/cms/cms_menu_imu.c
@@ -55,7 +55,7 @@
 #define RPY_PIDFF_MAX 200
 #define OTHER_PIDDF_MAX 255
 
-#define PIDFF_ENTRY(label, ptr, max) OSD_UINT8_ENTRY(label, (&(const OSD_UINT8_t){ ptr, PIDFF_MIN, max, PIDFF_STEP }))
+#define PIDFF_ENTRY(label, ptr, max) OSD_UINT8_ENTRY(label, (&(const OSD_UINT16_t){ ptr, PIDFF_MIN, max, PIDFF_STEP }))
 #define RPY_PIDFF_ENTRY(label, ptr) PIDFF_ENTRY(label, ptr, RPY_PIDFF_MAX)
 #define OTHER_PIDFF_ENTRY(label, ptr) PIDFF_ENTRY(label, ptr, OTHER_PIDDF_MAX)
 
diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index 172e0fb47f..abffc91bbd 100755
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -44,6 +44,7 @@ extern uint8_t __config_end;
 #include "common/time.h"
 #include "common/typeconversion.h"
 #include "programming/global_variables.h"
+#include "programming/pid.h"
 
 #include "config/config_eeprom.h"
 #include "config/feature.h"
@@ -2001,47 +2002,50 @@ static void cliGvar(char *cmdline) {
     }
 }
 
-static void printPid(uint8_t dumpMask, const logicCondition_t *logicConditions, const logicCondition_t *defaultLogicConditions)
+static void printPid(uint8_t dumpMask, const programmingPid_t *programmingPids, const programmingPid_t *defaultProgrammingPids)
 {
     const char *format = "pid %d %d %d %d %d %d %d %d %d %d";
-    for (uint32_t i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
-        const logicCondition_t logic = logicConditions[i];
+    for (uint32_t i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
+        const programmingPid_t pid = programmingPids[i];
 
         bool equalsDefault = false;
-        if (defaultLogicConditions) {
-            logicCondition_t defaultValue = defaultLogicConditions[i];
+        if (defaultProgrammingPids) {
+            programmingPid_t defaultValue = defaultProgrammingPids[i];
             equalsDefault =
-                logic.enabled == defaultValue.enabled &&
-                logic.activatorId == defaultValue.activatorId &&
-                logic.operation == defaultValue.operation &&
-                logic.operandA.type == defaultValue.operandA.type &&
-                logic.operandA.value == defaultValue.operandA.value &&
-                logic.operandB.type == defaultValue.operandB.type &&
-                logic.operandB.value == defaultValue.operandB.value &&
-                logic.flags == defaultValue.flags;
+                pid.enabled == defaultValue.enabled &&
+                pid.setpoint.type == defaultValue.setpoint.type &&
+                pid.setpoint.value == defaultValue.setpoint.value &&
+                pid.measurement.type == defaultValue.measurement.type &&
+                pid.measurement.value == defaultValue.measurement.value &&
+                pid.gains.P == defaultValue.gains.P && 
+                pid.gains.I == defaultValue.gains.I && 
+                pid.gains.D == defaultValue.gains.D && 
+                pid.gains.FF == defaultValue.gains.FF;
 
             cliDefaultPrintLinef(dumpMask, equalsDefault, format,
                 i,
-                logic.enabled,
-                logic.activatorId,
-                logic.operation,
-                logic.operandA.type,
-                logic.operandA.value,
-                logic.operandB.type,
-                logic.operandB.value,
-                logic.flags
+                pid.enabled,
+                pid.setpoint.type,
+                pid.setpoint.value,
+                pid.measurement.type,
+                pid.measurement.value,
+                pid.gains.P,
+                pid.gains.I,
+                pid.gains.D,
+                pid.gains.FF
             );
         }
         cliDumpPrintLinef(dumpMask, equalsDefault, format,
             i,
-            logic.enabled,
-            logic.activatorId,
-            logic.operation,
-            logic.operandA.type,
-            logic.operandA.value,
-            logic.operandB.type,
-            logic.operandB.value,
-            logic.flags
+            pid.enabled,
+            pid.setpoint.type,
+            pid.setpoint.value,
+            pid.measurement.type,
+            pid.measurement.value,
+            pid.gains.P,
+            pid.gains.I,
+            pid.gains.D,
+            pid.gains.FF
         );
     }
 }
@@ -2052,7 +2056,7 @@ static void cliPid(char *cmdline) {
     uint8_t len = strlen(cmdline);
 
     if (len == 0) {
-        printLogic(DUMP_MASTER, programmingPids(0), NULL);
+        printPid(DUMP_MASTER, programmingPids(0), NULL);
     } else if (sl_strncasecmp(cmdline, "reset", 5) == 0) {
         pgResetCopy(programmingPidsMutable(0), PG_LOGIC_CONDITIONS);
     } else {
@@ -2082,24 +2086,28 @@ static void cliPid(char *cmdline) {
 
         int32_t i = args[INDEX];
         if (
-            i >= 0 && i < MAX_LOGIC_CONDITIONS &&
+            i >= 0 && i < MAX_PROGRAMMING_PID_COUNT &&
             args[ENABLED] >= 0 && args[ENABLED] <= 1 &&
-            args[ACTIVATOR_ID] >= -1 && args[ACTIVATOR_ID] < MAX_LOGIC_CONDITIONS &&
-            args[OPERATION] >= 0 && args[OPERATION] < LOGIC_CONDITION_LAST &&
-            args[OPERAND_A_TYPE] >= 0 && args[OPERAND_A_TYPE] < LOGIC_CONDITION_OPERAND_TYPE_LAST &&
-            args[OPERAND_A_VALUE] >= -1000000 && args[OPERAND_A_VALUE] <= 1000000 &&
-            args[OPERAND_B_TYPE] >= 0 && args[OPERAND_B_TYPE] < LOGIC_CONDITION_OPERAND_TYPE_LAST &&
-            args[OPERAND_B_VALUE] >= -1000000 && args[OPERAND_B_VALUE] <= 1000000 &&
-            args[FLAGS] >= 0 && args[FLAGS] <= 255
-
+            args[SETPOINT_TYPE] >= 0 && args[SETPOINT_TYPE] < LOGIC_CONDITION_OPERAND_TYPE_LAST &&
+            args[SETPOINT_VALUE] >= -1000000 && args[SETPOINT_VALUE] <= 1000000 &&
+            args[MEASUREMENT_TYPE] >= 0 && args[MEASUREMENT_TYPE] < LOGIC_CONDITION_OPERAND_TYPE_LAST &&
+            args[MEASUREMENT_VALUE] >= -1000000 && args[MEASUREMENT_VALUE] <= 1000000 &&
+            args[P_GAIN] >= 0 && args[P_GAIN] <= INT16_MAX &&
+            args[I_GAIN] >= 0 && args[I_GAIN] <= INT16_MAX &&
+            args[D_GAIN] >= 0 && args[D_GAIN] <= INT16_MAX &&
+            args[FF_GAIN] >= 0 && args[FF_GAIN] <= INT16_MAX
         ) {
             programmingPidsMutable(i)->enabled = args[ENABLED];
-            logicConditionsMutable(i)->setpoint.type = args[OPERAND_A_TYPE];
-            logicConditionsMutable(i)->setpoint.value = args[OPERAND_A_VALUE];
-            logicConditionsMutable(i)->measurement.type = args[OPERAND_B_TYPE];
-            logicConditionsMutable(i)->measurement.value = args[OPERAND_B_VALUE];
+            programmingPidsMutable(i)->setpoint.type = args[SETPOINT_TYPE];
+            programmingPidsMutable(i)->setpoint.value = args[SETPOINT_VALUE];
+            programmingPidsMutable(i)->measurement.type = args[MEASUREMENT_TYPE];
+            programmingPidsMutable(i)->measurement.value = args[MEASUREMENT_VALUE];
+            programmingPidsMutable(i)->gains.P = args[P_GAIN];
+            programmingPidsMutable(i)->gains.I = args[I_GAIN];
+            programmingPidsMutable(i)->gains.D = args[D_GAIN];
+            programmingPidsMutable(i)->gains.FF = args[FF_GAIN];
 
-            cliLogic("");
+            cliPid("");
         } else {
             cliShowParseError();
         }
@@ -3422,7 +3430,7 @@ static void printConfig(const char *cmdline, bool doDiff)
         printGvar(dumpMask, globalVariableConfigs_CopyArray, globalVariableConfigs(0));
 
         cliPrintHashLine("pid");
-        printLogic(dumpMask, programmingPids_CopyArray, programmingPids(0));
+        printPid(dumpMask, programmingPids_CopyArray, programmingPids(0));
 #endif
 
         cliPrintHashLine("feature");
diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index fb6d610337..71f758dc40 100755
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -89,6 +89,7 @@ FILE_COMPILE_FOR_SPEED
 #include "flight/failsafe.h"
 
 #include "config/feature.h"
+#include "programming/pid.h"
 
 // June 2013     V2.2-dev
 
@@ -390,6 +391,7 @@ void disarm(disarmReason_t disarmReason)
 
         statsOnDisarm();
         logicConditionReset();
+        programmingPidReset();
         beeper(BEEPER_DISARMING);      // emit disarm tone
     }
 }
@@ -480,6 +482,7 @@ void tryArm(void)
         //It is required to inform the mixer that arming was executed and it has to switch to the FORWARD direction
         ENABLE_STATE(SET_REVERSIBLE_MOTORS_FORWARD);
         logicConditionReset();
+        programmingPidReset();
         headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 
         resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 7e070167ee..342077a039 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -686,18 +686,18 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
 
     case MSP_PID:
         for (int i = 0; i < PID_ITEM_COUNT; i++) {
-            sbufWriteU8(dst, pidBank()->pid[i].P);
-            sbufWriteU8(dst, pidBank()->pid[i].I);
-            sbufWriteU8(dst, pidBank()->pid[i].D);
+            sbufWriteU8(dst, constrain(pidBank()->pid[i].P, 0, 255));
+            sbufWriteU8(dst, constrain(pidBank()->pid[i].I, 0, 255));
+            sbufWriteU8(dst, constrain(pidBank()->pid[i].D, 0, 255));
         }
         break;
 
     case MSP2_PID:
         for (int i = 0; i < PID_ITEM_COUNT; i++) {
-            sbufWriteU8(dst, pidBank()->pid[i].P);
-            sbufWriteU8(dst, pidBank()->pid[i].I);
-            sbufWriteU8(dst, pidBank()->pid[i].D);
-            sbufWriteU8(dst, pidBank()->pid[i].FF);
+            sbufWriteU8(dst, constrain(pidBank()->pid[i].P, 0, 255));
+            sbufWriteU8(dst, constrain(pidBank()->pid[i].I, 0, 255));
+            sbufWriteU8(dst, constrain(pidBank()->pid[i].D, 0, 255));
+            sbufWriteU8(dst, constrain(pidBank()->pid[i].FF, 0, 255));
         }
         break;
 
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index cc4b20ae59..fa8ed01405 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -81,10 +81,10 @@ typedef enum {
 } pidType_e;
 
 typedef struct pid8_s {
-    uint8_t P;
-    uint8_t I;
-    uint8_t D;
-    uint8_t FF;
+    uint16_t P;
+    uint16_t I;
+    uint16_t D;
+    uint16_t FF;
 } pid8_t;
 
 typedef struct pidBank_s {
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index ec8047f35e..0f9567bc0d 100755
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -1961,6 +1961,10 @@ float navPidApply3(
             pid->integrator = newIntegrator;
         }
     }
+    
+    if (pidFlags & PID_LIMIT_INTEGRATOR) {
+        pid->integrator = constrainf(pid->integrator, outMin, outMax);
+    } 
 
     return outValConstrained;
 }
diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
index f2b348d650..ceae9fd56c 100755
--- a/src/main/navigation/navigation_private.h
+++ b/src/main/navigation/navigation_private.h
@@ -95,6 +95,7 @@ typedef enum {
     PID_DTERM_FROM_ERROR            = 1 << 0,
     PID_ZERO_INTEGRATOR             = 1 << 1,
     PID_SHRINK_INTEGRATOR           = 1 << 2,
+    PID_LIMIT_INTEGRATOR            = 1 << 3,
 } pidControllerFlags_e;
 
 typedef struct {
diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c
index b9b35d810d..7d309132cd 100644
--- a/src/main/programming/logic_condition.c
+++ b/src/main/programming/logic_condition.c
@@ -30,6 +30,7 @@
 
 #include "programming/logic_condition.h"
 #include "programming/global_variables.h"
+#include "programming/pid.h"
 #include "common/utils.h"
 #include "rx/rx.h"
 #include "common/maths.h"
@@ -595,6 +596,12 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) {
             }
             break;
 
+        case LOGIC_CONDITION_OPERAND_TYPE_PID:
+            if (operand >= 0 && operand < MAX_PROGRAMMING_PID_COUNT) {
+                retVal = programmingPidGetOutput(operand);
+            }
+            break;
+
         default:
             break;
     }
diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h
index 75905c51ca..65e98713ca 100644
--- a/src/main/programming/logic_condition.h
+++ b/src/main/programming/logic_condition.h
@@ -77,6 +77,7 @@ typedef enum logicOperandType_s {
     LOGIC_CONDITION_OPERAND_TYPE_FLIGHT_MODE,
     LOGIC_CONDITION_OPERAND_TYPE_LC,    // Result of different LC and LC operand
     LOGIC_CONDITION_OPERAND_TYPE_GVAR,  // Value from a global variable
+    LOGIC_CONDITION_OPERAND_TYPE_PID,  // Value from a Programming PID
     LOGIC_CONDITION_OPERAND_TYPE_LAST
 } logicOperandType_e;
 
diff --git a/src/main/programming/pid.c b/src/main/programming/pid.c
index eb680f6687..91134f4096 100644
--- a/src/main/programming/pid.c
+++ b/src/main/programming/pid.c
@@ -35,6 +35,7 @@ FILE_COMPILE_FOR_SIZE
 #include "navigation/navigation_private.h"
 
 #include "programming/pid.h"
+#include "programming/logic_condition.h"
 
 EXTENDED_FASTRAM programmingPidState_t programmingPidState[MAX_PROGRAMMING_PID_COUNT];
 static bool pidsInitiated = false;
@@ -66,13 +67,31 @@ void pgResetFn_programmingPids(programmingPid_t *instance)
 
 void programmingPidUpdateTask(timeUs_t currentTimeUs)
 {
-    UNUSED(currentTimeUs);
+    static timeUs_t previousUpdateTimeUs;
+    const float dT = US2S(currentTimeUs - previousUpdateTimeUs);
     
     if (!pidsInitiated) {
         programmingPidInit();
         pidsInitiated = true;
     }
 
+    for (uint8_t i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
+        if (programmingPids(i)->enabled) {
+            const int setpoint = logicConditionGetOperandValue(programmingPids(i)->setpoint.type, programmingPids(i)->setpoint.value);
+            const int measurement = logicConditionGetOperandValue(programmingPids(i)->measurement.type, programmingPids(i)->measurement.value);
+
+            programmingPidState[i].output = navPidApply2(
+                &programmingPidState[i].controller,
+                setpoint,
+                measurement,
+                dT,
+                -1000,
+                1000,
+                PID_LIMIT_INTEGRATOR
+            );
+
+        }
+    }
 }
 
 void programmingPidInit(void)
@@ -80,13 +99,24 @@ void programmingPidInit(void)
     for (uint8_t i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
         navPidInit(
             &programmingPidState[i].controller,
-            programmingPids(i)->gains.P / 100.0f,
-            programmingPids(i)->gains.I / 100.0f,
-            programmingPids(i)->gains.D / 100.0f,
-            programmingPids(i)->gains.FF / 100.0f,
+            programmingPids(i)->gains.P / 1000.0f,
+            programmingPids(i)->gains.I / 1000.0f,
+            programmingPids(i)->gains.D / 1000.0f,
+            programmingPids(i)->gains.FF / 1000.0f,
             5.0f
         );
     }
 }
 
+int programmingPidGetOutput(uint8_t i) {
+    return programmingPidState[constrain(i, 0, MAX_PROGRAMMING_PID_COUNT)].output;
+}
+
+void programmingPidReset(void)
+{
+    for (uint8_t i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
+        navPidReset(&programmingPidState[i].controller);
+    }
+}
+
 #endif
\ No newline at end of file
diff --git a/src/main/programming/pid.h b/src/main/programming/pid.h
index 810b9cfa3c..f120f9f073 100644
--- a/src/main/programming/pid.h
+++ b/src/main/programming/pid.h
@@ -32,7 +32,7 @@
 #include "flight/pid.h"
 #include "navigation/navigation.h"
 
-#define MAX_PROGRAMMING_PID_COUNT 2
+#define MAX_PROGRAMMING_PID_COUNT 4
 
 typedef struct programmingPid_s {
     uint8_t enabled;
@@ -45,8 +45,10 @@ PG_DECLARE_ARRAY(programmingPid_t, MAX_PROGRAMMING_PID_COUNT, programmingPids);
 
 typedef struct programmingPidState_s {
     pidController_t controller;
-    logicOperand_t setpoint;
+    float output;
 } programmingPidState_t;
 
 void programmingPidUpdateTask(timeUs_t currentTimeUs);
-void programmingPidInit(void);
\ No newline at end of file
+void programmingPidInit(void);
+void programmingPidReset(void);
+int programmingPidGetOutput(uint8_t i);
\ No newline at end of file

From 97eea57e9e01a754df06d0d16c2e3c26fbb4c2a3 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Fri, 4 Dec 2020 21:20:02 +0100
Subject: [PATCH 11/23] Fix PID bank uint16 transition

---
 src/main/fc/rc_adjustments.c | 4 ++--
 src/main/flight/pid.c        | 2 +-
 src/main/flight/pid.h        | 2 +-
 3 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c
index d7ae44f9d8..434815b69c 100644
--- a/src/main/fc/rc_adjustments.c
+++ b/src/main/fc/rc_adjustments.c
@@ -360,9 +360,9 @@ static void applyAdjustmentManualRate(adjustmentFunction_e adjustmentFunction, u
     return applyAdjustmentU8(adjustmentFunction, val, delta, RC_ADJUSTMENT_MANUAL_RATE_MIN, RC_ADJUSTMENT_MANUAL_RATE_MAX);
 }
 
-static void applyAdjustmentPID(adjustmentFunction_e adjustmentFunction, uint8_t *val, int delta)
+static void applyAdjustmentPID(adjustmentFunction_e adjustmentFunction, uint16_t *val, int delta)
 {
-    applyAdjustmentU8(adjustmentFunction, val, delta, RC_ADJUSTMENT_PID_MIN, RC_ADJUSTMENT_PID_MAX);
+    applyAdjustmentU16(adjustmentFunction, val, delta, RC_ADJUSTMENT_PID_MIN, RC_ADJUSTMENT_PID_MAX);
 }
 
 static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta)
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 00cd8ac9c5..5e5f9d3b5a 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -1116,7 +1116,7 @@ pidBank_t * pidBankMutable(void) {
     return usedPidControllerType == PID_TYPE_PIFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc;
 }
 
-uint8_t * getD_FFRefByBank(pidBank_t *pidBank, pidIndex_e pidIndex)
+uint16_t * getD_FFRefByBank(pidBank_t *pidBank, pidIndex_e pidIndex)
 {
     if (pidIndexGetType(pidIndex) == PID_TYPE_PIFF) {
        return &pidBank->pid[pidIndex].FF;
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index fa8ed01405..7f83accab6 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -199,4 +199,4 @@ void autotuneUpdateState(void);
 void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRateDps, float reachedRateDps, float pidOutput);
 
 pidType_e pidIndexGetType(pidIndex_e pidIndex);
-uint8_t * getD_FFRefByBank(pidBank_t *pidBank, pidIndex_e pidIndex);
+uint16_t * getD_FFRefByBank(pidBank_t *pidBank, pidIndex_e pidIndex);

From 6dd4cf9b7946121582f0a5b59d5f19fd279a98dd Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Mon, 7 Dec 2020 09:56:42 +0100
Subject: [PATCH 12/23] Drop legacy curve

---
 docs/Settings.md          | 10 +++++++---
 src/main/fc/settings.yaml |  4 ++--
 src/main/sensors/gyro.c   | 14 ++------------
 src/main/sensors/gyro.h   |  2 +-
 4 files changed, 12 insertions(+), 18 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index 00999bf75a..986235b09b 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -137,6 +137,9 @@
 | gps_provider | UBLOX | Which GPS protocol to be used, note that UBLOX is 5Hz and UBLOX7 is 10Hz (M8N). |
 | gps_sbas_mode | NONE | Which SBAS mode to be used |
 | gps_ublox_use_galileo | OFF | Enable use of Galileo satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires M8N and Ublox firmware 3.x (or later) [OFF/ON]. |
+| gyro_dyn_lpf_curve_expo | 5 | Expo value for the throttle-to-frequency mapping for Dynamic LPF |
+| gyro_dyn_lpf_max_hz | 500 | Maximum frequency of the gyro Dynamic LPF |
+| gyro_dyn_lpf_min_hz | 200 | Minimum frequency of the gyro Dynamic LPF |
 | gyro_hardware_lpf | 42HZ | Hardware lowpass filter for gyro. Allowed values depend on the driver - For example MPU6050 allows 10HZ,20HZ,42HZ,98HZ,188HZ,256Hz (8khz mode). If you have to set gyro lpf below 42Hz generally means the frame is vibrating too much, and that should be fixed first. |
 | gyro_lpf_hz | 60 | Software-based filter to remove mechanical vibrations from the gyro signal. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value. |
 | gyro_lpf_type | BIQUAD | Specifies the type of the software LPF of the gyro signals. BIQUAD gives better filtering and more delay, PT1 less filtering and less delay, so use only on clean builds. |
@@ -146,6 +149,7 @@
 | gyro_stage2_lowpass_type | `BIQUAD` | Defines the type of stage 2 gyro LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. |
 | gyro_sync | OFF | This option enables gyro_sync feature. In this case the loop will be synced to gyro refresh rate. Loop will always wait for the newest gyro measurement. Maximum gyro refresh rate is determined by gyro_hardware_lpf |
 | gyro_to_use |  |  |
+| gyro_use_dyn_lpf | OFF | Use Dynamic LPF instead of static gyro stage1 LPF. |
 | has_flaps | OFF | Defines is UAV is capable of having flaps. If ON and AIRPLANE `platform_type` is used, **FLAPERON** flight mode will be available for the pilot |
 | heading_hold_rate_limit | 90 | This setting limits yaw rotation rate that HEADING_HOLD controller can request from PID inner loop controller. It is independent from manual yaw rate and used only when HEADING_HOLD flight mode is enabled by pilot, RTH or WAYPOINT modes. |
 | hott_alarm_sound_interval | 5 | Battery alarm delay in seconds for Hott telemetry |
@@ -277,8 +281,8 @@
 | nav_fw_max_thr | 1700 | Maximum throttle for flying wing in GPS assisted modes |
 | nav_fw_min_thr | 1200 | Minimum throttle for flying wing in GPS assisted modes |
 | nav_fw_pitch2thr | 10 | Amount of throttle applied related to pitch attitude in GPS assisted modes. Throttle = nav_fw_cruise_throttle - (nav_fw_pitch2thr * pitch_angle). (notice that pitch_angle is in degrees and is negative when climbing and positive when diving, and throttle value is constrained between nav_fw_min_thr and nav_fw_max_thr) |
-| nav_fw_pitch2thr_smoothing | 0 | How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh. |
-| nav_fw_pitch2thr_threshold | 0 | Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] |
+| nav_fw_pitch2thr_smoothing | 6 | How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh. |
+| nav_fw_pitch2thr_threshold | 50 | Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] |
 | nav_fw_pos_hdg_d | 0 | D gain of heading trajectory PID controller. (Fixedwing, rovers, boats) |
 | nav_fw_pos_hdg_i | 0 | I gain of heading trajectory PID controller. (Fixedwing, rovers, boats) |
 | nav_fw_pos_hdg_p | 60 | P gain of heading PID controller. (Fixedwing, rovers, boats) |
@@ -324,7 +328,7 @@
 | nav_mc_vel_z_i | 50 | I gain of velocity PID controller |
 | nav_mc_vel_z_p | 100 | P gain of velocity PID controller |
 | nav_min_rth_distance | 500 | Minimum distance from homepoint when RTH full procedure will be activated [cm]. Below this distance, the mode will activate at the current location and the final phase is executed (loiter / land). Above this distance, the full procedure is activated, which may include initial climb and flying directly to the homepoint before entering the loiter / land phase. |
-| nav_overrides_motor_stop | ALL_NAV | When set OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF always the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV |
+| nav_overrides_motor_stop | ALL_NAV | When set OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF always the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect on NAV modes which take control of the throttle when combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV |
 | nav_position_timeout | 5 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) |
 | nav_rth_abort_threshold | 50000 | RTH sanity checking feature will notice if distance to home is increasing during RTH and once amount of increase exceeds the threshold defined by this parameter, instead of continuing RTH machine will enter emergency landing, self-level and go down safely. Default is 500m which is safe enough for both multirotor machines and airplanes. [cm] |
 | nav_rth_allow_landing | ALWAYS | If set to ON drone will land as a last phase of RTH. |
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 4b7d453b26..5ecc66907d 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -234,8 +234,8 @@ groups:
       - name: gyro_dyn_lpf_curve_expo
         description: "Expo value for the throttle-to-frequency mapping for Dynamic LPF"
         default_value: "5"
-        field: gyroDynamicLpFCurveExpo
-        min: 0
+        field: gyroDynamicLpfCurveExpo
+        min: 1
         max: 10
       - name: dynamic_gyro_notch_enabled
         description: "Enable/disable dynamic gyro notch also known as Matrix Filter"
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 7adadf2c10..62bc58887c 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -121,7 +121,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .useDynamicLpf = 0,
     .gyroDynamicLpfMinHz = 200,
     .gyroDynamicLpfMaxHz = 500,
-    .gyroDynamicLpFCurveExpo = 5,
+    .gyroDynamicLpfCurveExpo = 5,
     .dynamicGyroNotchRange = DYN_NOTCH_RANGE_MEDIUM,
     .dynamicGyroNotchQ = 120,
     .dynamicGyroNotchMinHz = 150,
@@ -524,23 +524,13 @@ static float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLp
     return (dynLpfMax - dynLpfMin) * curve + dynLpfMin;
 }
 
-static float dynThrottle(float throttle) {
-    return throttle * (1 - (throttle * throttle) / 3.0f) * 1.5f;
-}
-
 void gyroUpdateDynamicLpf(void) {
     if (!gyroConfig()->useDynamicLpf) {
         return;
     }
 
     const float throttle = scaleRangef((float) rcCommand[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle, 0.0f, 1.0f);
-
-    uint16_t cutoffFreq;
-    if (gyroConfig()->gyroDynamicLpFCurveExpo > 0) {
-        cutoffFreq = dynLpfCutoffFreq(throttle, gyroConfig()->gyroDynamicLpfMinHz, gyroConfig()->gyroDynamicLpfMaxHz, gyroConfig()->gyroDynamicLpFCurveExpo);
-    } else {
-        cutoffFreq = fmax(dynThrottle(throttle) * gyroConfig()->gyroDynamicLpfMaxHz, gyroConfig()->gyroDynamicLpfMinHz);
-    }
+    const uint16_t cutoffFreq = dynLpfCutoffFreq(throttle, gyroConfig()->gyroDynamicLpfMinHz, gyroConfig()->gyroDynamicLpfMaxHz, gyroConfig()->gyroDynamicLpfCurveExpo);
 
     if (gyroConfig()->gyro_soft_lpf_type == FILTER_PT1) {
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index b5bf66a4b2..491ff0a106 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -73,7 +73,7 @@ typedef struct gyroConfig_s {
     uint8_t useDynamicLpf;
     uint16_t gyroDynamicLpfMinHz;
     uint16_t gyroDynamicLpfMaxHz;
-    uint8_t gyroDynamicLpFCurveExpo;
+    uint8_t gyroDynamicLpfCurveExpo;
     uint8_t dynamicGyroNotchRange;
     uint16_t dynamicGyroNotchQ;
     uint16_t dynamicGyroNotchMinHz;

From a160f2a4d3c3cbcedc8670736ac561b2809eaaf6 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Mon, 7 Dec 2020 11:16:07 +0100
Subject: [PATCH 13/23] Update tests

---
 src/test/unit/sensor_gyro_unittest.cc | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/test/unit/sensor_gyro_unittest.cc b/src/test/unit/sensor_gyro_unittest.cc
index 7eb0418397..0433b88939 100644
--- a/src/test/unit/sensor_gyro_unittest.cc
+++ b/src/test/unit/sensor_gyro_unittest.cc
@@ -37,6 +37,8 @@ extern "C" {
     #include "sensors/gyro.h"
     #include "sensors/acceleration.h"
     #include "sensors/sensors.h"
+    #include "fc/rc_controls.h"
+    #include "flight/mixer.h"
 
     extern zeroCalibrationVector_t gyroCalibration;
     extern gyroDev_t gyroDev[];

From a15993cf3b6ef975825fda3ca7696f48123c182a Mon Sep 17 00:00:00 2001
From: Jonathan Hudson <jh+github@daria.co.uk>
Date: Mon, 21 Dec 2020 12:41:37 +0000
Subject: [PATCH 14/23] Correct test for Galileo capability (#6434)

* Improve test for Galileo capability
---
 src/main/io/gps_ublox.c | 11 ++++++++++-
 1 file changed, 10 insertions(+), 1 deletion(-)

diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c
index 3a7d3d12c1..a1d1378e5c 100755
--- a/src/main/io/gps_ublox.c
+++ b/src/main/io/gps_ublox.c
@@ -651,7 +651,16 @@ static bool gpsParceFrameUBLOX(void)
             // EXT CORE 3.01 (107900)
             // 01234567890123456789012
             gpsState.hwVersion = fastA2I(_buffer.ver.hwVersion);
-            capGalileo = ((gpsState.hwVersion >= 80000) && (_buffer.ver.swVersion[9] > '2')); // M8N and SW major 3 or later
+            if  ((gpsState.hwVersion >= 80000) && (_buffer.ver.swVersion[9] > '2')) {
+                // check extensions;
+                // after hw + sw vers; each is 30 bytes
+                for(int j = 40; j < _payload_length; j += 30) {
+                    if (strnstr((const char *)(_buffer.bytes+j), "GAL", 30)) {
+                        capGalileo = true;
+                        break;
+                    }
+                }
+            }
         }
         break;
     case MSG_ACK_ACK:

From 121b6ae098451d1c65693658bfb3785be2d890e2 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Thu, 24 Dec 2020 19:57:13 +0100
Subject: [PATCH 15/23] Settings update

---
 docs/Settings.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index 37e053c932..fd8b9fed9b 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -324,7 +324,7 @@
 | nav_mc_vel_z_i | 50 | I gain of velocity PID controller |
 | nav_mc_vel_z_p | 100 | P gain of velocity PID controller |
 | nav_min_rth_distance | 500 | Minimum distance from homepoint when RTH full procedure will be activated [cm]. Below this distance, the mode will activate at the current location and the final phase is executed (loiter / land). Above this distance, the full procedure is activated, which may include initial climb and flying directly to the homepoint before entering the loiter / land phase. |
-| nav_overrides_motor_stop | ALL_NAV | When set OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF always the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect on NAV modes which take control of the throttle when combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV |
+| nav_overrides_motor_stop | ALL_NAV | When set to OFF the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect on NAV modes which take control of the throttle when combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV |
 | nav_position_timeout | 5 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) |
 | nav_rth_abort_threshold | 50000 | RTH sanity checking feature will notice if distance to home is increasing during RTH and once amount of increase exceeds the threshold defined by this parameter, instead of continuing RTH machine will enter emergency landing, self-level and go down safely. Default is 500m which is safe enough for both multirotor machines and airplanes. [cm] |
 | nav_rth_allow_landing | ALWAYS | If set to ON drone will land as a last phase of RTH. |

From ee5bc7baefe3243c410059c2d041242340d1baad Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Thu, 24 Dec 2020 21:06:39 +0100
Subject: [PATCH 16/23] Fix dT component

---
 src/main/programming/pid.c | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/main/programming/pid.c b/src/main/programming/pid.c
index 91134f4096..09916fbb1b 100644
--- a/src/main/programming/pid.c
+++ b/src/main/programming/pid.c
@@ -92,6 +92,8 @@ void programmingPidUpdateTask(timeUs_t currentTimeUs)
 
         }
     }
+
+    previousUpdateTimeUs = currentTimeUs;
 }
 
 void programmingPidInit(void)

From 9c11ceddf135302db21fdbc97d90feecb7496621 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Thu, 24 Dec 2020 21:49:50 +0100
Subject: [PATCH 17/23] MSP layer for Programming PIDs

---
 src/main/fc/fc_msp.c                | 30 +++++++++++++++++++++++++++++
 src/main/msp/msp_protocol_v2_inav.h |  2 ++
 2 files changed, 32 insertions(+)

diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 1573c0b682..d5ad2f3022 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -36,6 +36,7 @@
 #include "common/time.h"
 #include "common/utils.h"
 #include "programming/global_variables.h"
+#include "programming/pid.h"
 
 #include "config/parameter_group_ids.h"
 
@@ -553,6 +554,19 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
             sbufWriteU32(dst, gvGet(i));
         }
         break;
+    case MSP2_INAV_PROGRAMMING_PID:
+        for (int i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
+            sbufWriteU8(dst, programmingPids(i)->enabled);
+            sbufWriteU8(dst, programmingPids(i)->setpoint.type);
+            sbufWriteU32(dst, programmingPids(i)->setpoint.value);
+            sbufWriteU8(dst, programmingPids(i)->measurement.type);
+            sbufWriteU32(dst, programmingPids(i)->measurement.value);
+            sbufWriteU16(dst, programmingPids(i)->gains.P);
+            sbufWriteU16(dst, programmingPids(i)->gains.I);
+            sbufWriteU16(dst, programmingPids(i)->gains.D);
+            sbufWriteU16(dst, programmingPids(i)->gains.FF);
+        }
+        break;
 #endif
     case MSP2_COMMON_MOTOR_MIXER:
         for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
@@ -1969,6 +1983,22 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
         } else
             return MSP_RESULT_ERROR;
         break;
+
+    case MSP2_INAV_SET_PROGRAMMING_PID:
+        sbufReadU8Safe(&tmp_u8, src);
+        if ((dataSize == 20) && (tmp_u8 < MAX_PROGRAMMING_PID_COUNT)) {
+            programmingPidsMutable(tmp_u8)->enabled = sbufReadU8(src);
+            programmingPidsMutable(tmp_u8)->setpoint.type = sbufReadU8(src);
+            programmingPidsMutable(tmp_u8)->setpoint.value = sbufReadU32(src);
+            programmingPidsMutable(tmp_u8)->measurement.type = sbufReadU8(src);
+            programmingPidsMutable(tmp_u8)->measurement.value = sbufReadU32(src);
+            programmingPidsMutable(tmp_u8)->gains.P = sbufReadU16(src);
+            programmingPidsMutable(tmp_u8)->gains.I = sbufReadU16(src);
+            programmingPidsMutable(tmp_u8)->gains.D = sbufReadU16(src);
+            programmingPidsMutable(tmp_u8)->gains.FF = sbufReadU16(src);
+        } else
+            return MSP_RESULT_ERROR;
+        break;
 #endif
     case MSP2_COMMON_SET_MOTOR_MIXER:
         sbufReadU8Safe(&tmp_u8, src);
diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h
index 3da8499507..219e770722 100755
--- a/src/main/msp/msp_protocol_v2_inav.h
+++ b/src/main/msp/msp_protocol_v2_inav.h
@@ -61,6 +61,8 @@
 #define MSP2_INAV_SET_GLOBAL_FUNCTIONS          0x2025
 #define MSP2_INAV_LOGIC_CONDITIONS_STATUS       0x2026
 #define MSP2_INAV_GVAR_STATUS                   0x2027
+#define MSP2_INAV_PROGRAMMING_PID               0x2028
+#define MSP2_INAV_SET_PROGRAMMING_PID           0x2029
 
 #define MSP2_PID                                0x2030
 #define MSP2_SET_PID                            0x2031

From fa9251f8b00ce3f4ca8619b6ca406a36b2740187 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Fri, 25 Dec 2020 14:09:29 +0100
Subject: [PATCH 18/23] Basic doc update

---
 docs/Programming Framework.md | 28 ++++++++++++++++++++++++++--
 1 file changed, 26 insertions(+), 2 deletions(-)

diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md
index fc11010f1f..6e658058d1 100644
--- a/docs/Programming Framework.md	
+++ b/docs/Programming Framework.md	
@@ -9,10 +9,13 @@ INAV Programming Framework coinsists of:
 
 * Logic Conditions - each Logic Condition can be understood as a single command, a single line of code
 * Global Variables - variables that can store values from and for LogiC Conditions and servo mixer
+* Programming PID - general purpose, user configurable PID controllers
 
 IPF can be edited using INAV Configurator user interface, of via CLI
 
-## CLI
+## Logic Conditions
+
+### CLI
 
 `logic <rule> <enabled> <activatorId> <operation> <operand A type> <operand A value> <operand B type> <operand B value> <flags>`
 
@@ -121,7 +124,7 @@ IPF can be edited using INAV Configurator user interface, of via CLI
 | 32            | CROSSFIRE LQ          | Crossfire Link quality as returned by the CRSF protocol   | 
 | 33            | CROSSFIRE SNR          | Crossfire SNR as returned by the CRSF protocol   | 
 
-##### ACTIVE_WAYPOINT_ACTION
+#### ACTIVE_WAYPOINT_ACTION
 
 | Action        |  Value   |
 |----           |----      |
@@ -159,6 +162,27 @@ All flags are reseted on ARM and DISARM event.
 |----   |----       |----                   |
 | 0     | 1         | Latch - after activation LC will stay active until LATCH flag is reseted |
 
+## Global variables
+
+### CLI
+
+`gvar <index> <default value> <min> <max>`
+
+## Programming PID
+
+`pid <index> <enabled> <setpoint type> <setpoint value> <measurement type> <measurement value> <P gain> <I gain> <D gain> <FF gain>`
+
+* `<index>` - ID of PID Controller, starting from `0`
+* `<enabled>` - `0` evaluates as disabled, `1` evaluates as enabled
+* `<setpoint type>` - See `Operands` paragraph
+* `<setpoint value>` - See `Operands` paragraph
+* `<measurement type>` - See `Operands` paragraph
+* `<measurement value>` - See `Operands` paragraph
+* `<P gain>` - P-gain, scaled to `1/1000`
+* `<I gain>` - I-gain, scaled to `1/1000`
+* `<D gain>` - D-gain, scaled to `1/1000`
+* `<FF gain>` - FF-gain, scaled to `1/1000`
+
 ## Examples
 
 ### Dynamic THROTTLE scale

From 1405fd3fc1587a515666c3f3e94bff1c4be7283e Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Fri, 25 Dec 2020 16:15:56 +0100
Subject: [PATCH 19/23] Refactor

---
 src/main/CMakeLists.txt       |  2 ++
 src/main/build/debug.h        |  1 +
 src/main/fc/fc_tasks.c        |  2 +-
 src/main/fc/settings.yaml     |  2 +-
 src/main/flight/dynamic_lpf.c | 50 +++++++++++++++++++++++++++++++++++
 src/main/flight/dynamic_lpf.h | 29 ++++++++++++++++++++
 src/main/sensors/gyro.c       | 17 +-----------
 src/main/sensors/gyro.h       |  2 +-
 8 files changed, 86 insertions(+), 19 deletions(-)
 create mode 100644 src/main/flight/dynamic_lpf.c
 create mode 100644 src/main/flight/dynamic_lpf.h

diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt
index 793c64c3bd..927070dcc4 100644
--- a/src/main/CMakeLists.txt
+++ b/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
diff --git a/src/main/build/debug.h b/src/main/build/debug.h
index 860289c076..b484fd4d35 100644
--- a/src/main/build/debug.h
+++ b/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;
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index 3a4517cf56..e4ff976cfc 100755
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -294,7 +294,7 @@ void taskUpdateAux(timeUs_t currentTimeUs)
 {
     UNUSED(currentTimeUs);
     updatePIDCoefficients();
-    gyroUpdateDynamicLpf();
+    dynamicLpfGyroTask();
 }
 
 void fcTasksInit(void)
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 5ecc66907d..59331e7c7a 100644
--- a/src/main/fc/settings.yaml
+++ b/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
diff --git a/src/main/flight/dynamic_lpf.c b/src/main/flight/dynamic_lpf.c
new file mode 100644
index 0000000000..c93cd92146
--- /dev/null
+++ b/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);
+}
\ No newline at end of file
diff --git a/src/main/flight/dynamic_lpf.h b/src/main/flight/dynamic_lpf.h
new file mode 100644
index 0000000000..f8824fc5b1
--- /dev/null
+++ b/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);
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 62bc58887c..ced897d8ad 100644
--- a/src/main/sensors/gyro.c
+++ b/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);
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index 491ff0a106..737055af1f 100644
--- a/src/main/sensors/gyro.h
+++ b/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);

From 9cc0d79460ebc36a20dfd45ee0bfba11565e8eff Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Fri, 25 Dec 2020 16:26:55 +0100
Subject: [PATCH 20/23] Fix includ

---
 src/main/fc/fc_tasks.c | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index e4ff976cfc..b000de7017 100755
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -49,6 +49,7 @@
 #include "flight/wind_estimator.h"
 #include "flight/rpm_filter.h"
 #include "flight/servos.h"
+#include "flight/dynamic_lpf.h"
 
 #include "navigation/navigation.h"
 

From 4d3fab588130b5929f149cdb8b84bebbc27c43a6 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Fri, 25 Dec 2020 16:31:27 +0100
Subject: [PATCH 21/23] Docs update

---
 docs/Settings.md          | 2 +-
 src/main/fc/settings.yaml | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index 986235b09b..b345407376 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -149,7 +149,7 @@
 | gyro_stage2_lowpass_type | `BIQUAD` | Defines the type of stage 2 gyro LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. |
 | gyro_sync | OFF | This option enables gyro_sync feature. In this case the loop will be synced to gyro refresh rate. Loop will always wait for the newest gyro measurement. Maximum gyro refresh rate is determined by gyro_hardware_lpf |
 | gyro_to_use |  |  |
-| gyro_use_dyn_lpf | OFF | Use Dynamic LPF instead of static gyro stage1 LPF. |
+| gyro_use_dyn_lpf | OFF | Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro LPF based on the throttle position. |
 | has_flaps | OFF | Defines is UAV is capable of having flaps. If ON and AIRPLANE `platform_type` is used, **FLAPERON** flight mode will be available for the pilot |
 | heading_hold_rate_limit | 90 | This setting limits yaw rotation rate that HEADING_HOLD controller can request from PID inner loop controller. It is independent from manual yaw rate and used only when HEADING_HOLD flight mode is enabled by pilot, RTH or WAYPOINT modes. |
 | hott_alarm_sound_interval | 5 | Battery alarm delay in seconds for Hott telemetry |
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 59331e7c7a..85e21cc3c7 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -215,7 +215,7 @@ groups:
         field: gyro_stage2_lowpass_type
         table: filter_type
       - name: gyro_use_dyn_lpf
-        description: "Use Dynamic LPF instead of static gyro stage1 LPF."
+        description: "Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro LPF based on the throttle position."
         default_value: "OFF"
         field: useDynamicLpf
         type: bool

From be73b015e59a6b58540f4886d7a0751148936bbb Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Fri, 25 Dec 2020 16:52:26 +0100
Subject: [PATCH 22/23] One more time regenerate settings.md

---
 docs/Settings.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index b345407376..18cf166e67 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -328,7 +328,7 @@
 | nav_mc_vel_z_i | 50 | I gain of velocity PID controller |
 | nav_mc_vel_z_p | 100 | P gain of velocity PID controller |
 | nav_min_rth_distance | 500 | Minimum distance from homepoint when RTH full procedure will be activated [cm]. Below this distance, the mode will activate at the current location and the final phase is executed (loiter / land). Above this distance, the full procedure is activated, which may include initial climb and flying directly to the homepoint before entering the loiter / land phase. |
-| nav_overrides_motor_stop | ALL_NAV | When set OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF always the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect on NAV modes which take control of the throttle when combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV |
+| nav_overrides_motor_stop | ALL_NAV | When set to OFF the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect on NAV modes which take control of the throttle when combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV |
 | nav_position_timeout | 5 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) |
 | nav_rth_abort_threshold | 50000 | RTH sanity checking feature will notice if distance to home is increasing during RTH and once amount of increase exceeds the threshold defined by this parameter, instead of continuing RTH machine will enter emergency landing, self-level and go down safely. Default is 500m which is safe enough for both multirotor machines and airplanes. [cm] |
 | nav_rth_allow_landing | ALWAYS | If set to ON drone will land as a last phase of RTH. |

From f8ce88f20631ef501a782e2c62481a0a643804f0 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Wed, 30 Dec 2020 22:53:14 +0100
Subject: [PATCH 23/23] dzikuvx-fix-ghst-telemetry

---
 src/main/rx/ghst.c        | 2 +-
 src/main/telemetry/ghst.c | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/rx/ghst.c b/src/main/rx/ghst.c
index 6f3173804f..54d3b6f0f5 100644
--- a/src/main/rx/ghst.c
+++ b/src/main/rx/ghst.c
@@ -241,7 +241,7 @@ uint8_t ghstFrameStatus(rxRuntimeConfig_t *rxRuntimeState)
 {
     UNUSED(rxRuntimeState);
 
-    if(serialIsIdle(serialPort)) {
+    if (serialIsIdle(serialPort)) {
         ghstIdle();
     }
 
diff --git a/src/main/telemetry/ghst.c b/src/main/telemetry/ghst.c
index 121f239446..dc9c02579e 100644
--- a/src/main/telemetry/ghst.c
+++ b/src/main/telemetry/ghst.c
@@ -163,7 +163,7 @@ bool checkGhstTelemetryState(void)
 // Called periodically by the scheduler
  void handleGhstTelemetry(timeUs_t currentTimeUs)
 {
-    static uint32_t ghstLastCycleTime;
+    static timeUs_t ghstLastCycleTime;
 
     if (!ghstTelemetryEnabled) {
         return;