From f88541a0fdd1864c8b8e6285e0628812a34b678a Mon Sep 17 00:00:00 2001
From: luca <l.giovannesi@gmail.com>
Date: Tue, 20 Apr 2021 20:32:40 +0200
Subject: [PATCH 01/23] Fixed turtle mode

---
 src/main/fc/fc_core.c | 39 ++++++++++++++++++++-------------------
 1 file changed, 20 insertions(+), 19 deletions(-)

diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index f4b1479143..7b3ac5d9ce 100755
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -497,15 +497,31 @@ void tryArm(void)
 {
     updateArmingStatus();
 
+#ifdef USE_DSHOT
+    if (
+            STATE(MULTIROTOR) &&
+            IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH) &&
+            emergencyArmingCanOverrideArmingDisabled() &&
+            isMotorProtocolDshot() &&
+            !ARMING_FLAG(ARMED) &&
+            !FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)
+            ) {
+        sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_REVERSED);
+        ENABLE_ARMING_FLAG(ARMED);
+        enableFlightMode(FLIP_OVER_AFTER_CRASH);
+        return;
+    }
+#endif
+
 #ifdef USE_PROGRAMMING_FRAMEWORK
     if (
-        !isArmingDisabled() || 
-        emergencyArmingIsEnabled() || 
+        !isArmingDisabled() ||
+        emergencyArmingIsEnabled() ||
         LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY)
     ) {
-#else 
+#else
     if (
-        !isArmingDisabled() || 
+        !isArmingDisabled() ||
         emergencyArmingIsEnabled()
     ) {
 #endif
@@ -513,21 +529,6 @@ void tryArm(void)
             return;
         }
 
-#ifdef USE_DSHOT
-        if (
-                STATE(MULTIROTOR) &&
-                IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH) &&
-                emergencyArmingCanOverrideArmingDisabled() &&
-                isMotorProtocolDshot() &&
-                !FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)
-                ) {
-            sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_REVERSED);
-            ENABLE_ARMING_FLAG(ARMED);
-            enableFlightMode(FLIP_OVER_AFTER_CRASH);
-            return;
-        }
-#endif
-
 #if defined(USE_NAV)
         // If nav_extra_arming_safety was bypassed we always
         // allow bypassing it even without the sticks set

From 0b8c9839914024e124a0ebb9c69db078db414c6f Mon Sep 17 00:00:00 2001
From: luca <l.giovannesi@gmail.com>
Date: Tue, 20 Apr 2021 20:32:58 +0200
Subject: [PATCH 02/23] Stick sensibility adjusted

---
 src/main/flight/mixer.c | 6 +-----
 1 file changed, 1 insertion(+), 5 deletions(-)

diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index cc9f6b2bcc..55ef2732a1 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -394,11 +394,7 @@ static void applyFlipOverAfterCrashModeToMotors(void) {
 
             motorOutputNormalised = MIN(1.0f, flipPower * motorOutputNormalised);
 
-            float motorOutput = (float)motorConfig()->mincommand + motorOutputNormalised * (float)motorConfig()->maxthrottle;
-
-            // Add a little bit to the motorOutputMin so props aren't spinning when sticks are centered
-            motorOutput = (motorOutput < (float)motorConfig()->mincommand + CRASH_OVER_AFTER_CRASH_FLIP_DEADBAND) ? DSHOT_DISARM_COMMAND : (
-                    motorOutput - CRASH_OVER_AFTER_CRASH_FLIP_DEADBAND);
+            float motorOutput = (float)motorConfig()->mincommand + (motorOutputNormalised * (float)(motorConfig()->maxthrottle - motorConfig()->mincommand));
 
             motor[i] = motorOutput;
         }

From fbf435f6a949d245949af030010d853253771848 Mon Sep 17 00:00:00 2001
From: luca <l.giovannesi@gmail.com>
Date: Wed, 21 Apr 2021 14:12:37 +0200
Subject: [PATCH 03/23] Motor responce adjusted and renamed all to turtleMode

---
 docs/Settings.md          |  2 +-
 src/main/fc/fc_core.c     |  2 +-
 src/main/fc/fc_msp_box.c  | 26 +++++++++++++-------------
 src/main/fc/rc_modes.h    |  2 +-
 src/main/fc/settings.yaml |  8 ++++----
 src/main/flight/mixer.c   | 13 +++++--------
 src/main/flight/mixer.h   |  2 +-
 7 files changed, 26 insertions(+), 29 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index 747bf17248..653a2e18b2 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -103,7 +103,6 @@
 | failsafe_throttle_low_delay | 0 | If failsafe activated when throttle is low for this much time - bypass failsafe and disarm, in 10th of seconds. 0 = No timeout |
 | fixed_wing_auto_arm | OFF | Auto-arm fixed wing aircraft on throttle above min_check, and disarming with stick commands are disabled, so power cycle is required to disarm. Requires enabled motorstop and no arm switch configured. |
 | flaperon_throw_offset | 200 | Defines throw range in us for both ailerons that will be passed to servo mixer via input source 14 (`FEATURE FLAPS`) when FLAPERON mode is activated. |
-| flip_over_after_crash_power_factor | 65 | flip over after crash power factor |
 | fpv_mix_degrees | 0 |  |
 | frsky_coordinates_format | 0 | D-Series telemetry only: FRSKY_FORMAT_DMS (default), FRSKY_FORMAT_NMEA |
 | frsky_default_latitude | 0 | D-Series telemetry only: OpenTX needs a valid set of coordinates to show compass value. A fake value defined in this setting is sent while no fix is acquired. |
@@ -515,6 +514,7 @@
 | tpa_breakpoint | 1500 | See tpa_rate. |
 | tpa_rate | 0 | Throttle PID attenuation reduces influence of P on ROLL and PITCH as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate. |
 | tri_unarmed_servo | ON | On tricopter mix only, if this is set to ON, servo will always be correcting regardless of armed state. to disable this, set it to OFF. |
+| turtle_mode_power_factor | 55 | Turtle mode power factor |
 | tz_automatic_dst | OFF | Automatically add Daylight Saving Time to the GPS time when needed or simply ignore it. Includes presets for EU and the USA - if you live outside these areas it is suggested to manage DST manually via `tz_offset`. |
 | tz_offset | 0 | Time zone offset from UTC, in minutes. This is applied to the GPS time for logging and time-stamping of Blackbox logs |
 | vbat_adc_channel | _target default_ | ADC channel to use for battery voltage sensor. Defaults to board VBAT input (if available). 0 = disabled |
diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index 7b3ac5d9ce..373d2a2e31 100755
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -500,7 +500,7 @@ void tryArm(void)
 #ifdef USE_DSHOT
     if (
             STATE(MULTIROTOR) &&
-            IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH) &&
+            IS_RC_MODE_ACTIVE(BOXTURTLE) &&
             emergencyArmingCanOverrideArmingDisabled() &&
             isMotorProtocolDshot() &&
             !ARMING_FLAG(ARMED) &&
diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c
index 65bf1de1cc..2556645259 100644
--- a/src/main/fc/fc_msp_box.c
+++ b/src/main/fc/fc_msp_box.c
@@ -79,18 +79,18 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { BOXCAMERA1, "CAMERA CONTROL 1", 39 },
     { BOXCAMERA2, "CAMERA CONTROL 2", 40 },
     { BOXCAMERA3, "CAMERA CONTROL 3", 41 },
-    { BOXOSDALT1, "OSD ALT 1", 42 },
-    { BOXOSDALT2, "OSD ALT 2", 43 },
-    { BOXOSDALT3, "OSD ALT 3", 44 },
-    { BOXNAVCOURSEHOLD, "NAV COURSE HOLD", 45 },
-    { BOXBRAKING, "MC BRAKING", 46 },
-    { BOXUSER1, "USER1", BOX_PERMANENT_ID_USER1 },
-    { BOXUSER2, "USER2", BOX_PERMANENT_ID_USER2 },
-    { BOXLOITERDIRCHN, "LOITER CHANGE", 49 },
-    { BOXMSPRCOVERRIDE, "MSP RC OVERRIDE", 50 },
-    { BOXPREARM, "PREARM", 51 },
-    { BOXFLIPOVERAFTERCRASH, "TURTLE", 52 },
-    { CHECKBOX_ITEM_COUNT, NULL, 0xFF }
+    {BOXOSDALT1,       "OSD ALT 1",       42 },
+    {BOXOSDALT2,       "OSD ALT 2",       43 },
+    {BOXOSDALT3,       "OSD ALT 3",       44 },
+    {BOXNAVCOURSEHOLD, "NAV COURSE HOLD", 45 },
+    {BOXBRAKING,       "MC BRAKING",      46 },
+    {BOXUSER1,         "USER1", BOX_PERMANENT_ID_USER1 },
+    {BOXUSER2,         "USER2", BOX_PERMANENT_ID_USER2 },
+    {BOXLOITERDIRCHN,  "LOITER CHANGE",   49 },
+    {BOXMSPRCOVERRIDE, "MSP RC OVERRIDE", 50 },
+    {BOXPREARM,        "PREARM",          51 },
+    {BOXTURTLE,        "TURTLE",          52 },
+    {CHECKBOX_ITEM_COUNT, NULL,           0xFF }
 };
 
 // this is calculated at startup based on enabled features.
@@ -312,7 +312,7 @@ void initActiveBoxIds(void)
 
 #ifdef USE_DSHOT
     if(STATE(MULTIROTOR) && isMotorProtocolDshot())
-        activeBoxIds[activeBoxIdCount++] = BOXFLIPOVERAFTERCRASH;
+        activeBoxIds[activeBoxIdCount++] = BOXTURTLE;
 #endif
 }
 
diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h
index 6b3f877197..4a840a4d3a 100755
--- a/src/main/fc/rc_modes.h
+++ b/src/main/fc/rc_modes.h
@@ -69,7 +69,7 @@ typedef enum {
     BOXLOITERDIRCHN  = 40,
     BOXMSPRCOVERRIDE = 41,
     BOXPREARM        = 42,
-    BOXFLIPOVERAFTERCRASH = 43,
+    BOXTURTLE        = 43,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 5459b45c3c..453d2429c8 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -836,10 +836,10 @@ groups:
         min: 4
         max: 255
         default_value: 14
-      - name: flip_over_after_crash_power_factor
-        field: flipOverAfterPowerFactor
-        default_value: 65
-        description: "flip over after crash power factor"
+      - name: turtle_mode_power_factor
+        field: turtleModePowerFactor
+        default_value: 55
+        description: "Turtle mode power factor"
         condition: "USE_DSHOT"
         min: 0
         max: 100
diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index 55ef2732a1..360586cecf 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -116,13 +116,12 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
     .throttleScale = SETTING_THROTTLE_SCALE_DEFAULT,
     .motorPoleCount = SETTING_MOTOR_POLES_DEFAULT,            // Most brushless motors that we use are 14 poles
 #ifdef USE_DSHOT
-    .flipOverAfterPowerFactor = SETTING_FLIP_OVER_AFTER_CRASH_POWER_FACTOR_DEFAULT,
+    .turtleModePowerFactor = SETTING_TURTLE_MODE_POWER_FACTOR_DEFAULT,
 #endif
 );
 
 PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0);
 
-#define CRASH_OVER_AFTER_CRASH_FLIP_DEADBAND 20.0f
 #define CRASH_OVER_AFTER_CRASH_FLIP_STICK_MIN 0.15f
 
 typedef void (*motorRateLimitingApplyFnPtr)(const float dT);
@@ -328,10 +327,10 @@ static uint16_t handleOutputScaling(
     }
     return value;
 }
-static void applyFlipOverAfterCrashModeToMotors(void) {
+static void applyTurtleModeToMotors(void) {
 
     if (ARMING_FLAG(ARMED)) {
-        const float flipPowerFactor = ((float)motorConfig()->flipOverAfterPowerFactor)/100.0f;
+        const float flipPowerFactor = ((float)motorConfig()->turtleModePowerFactor)/100.0f;
         const float stickDeflectionPitchAbs = ABS(((float) rcCommand[PITCH]) / 500.0f);
         const float stickDeflectionRollAbs = ABS(((float) rcCommand[ROLL]) / 500.0f);
         const float stickDeflectionYawAbs = ABS(((float) rcCommand[YAW]) / 500.0f);
@@ -394,9 +393,7 @@ static void applyFlipOverAfterCrashModeToMotors(void) {
 
             motorOutputNormalised = MIN(1.0f, flipPower * motorOutputNormalised);
 
-            float motorOutput = (float)motorConfig()->mincommand + (motorOutputNormalised * (float)(motorConfig()->maxthrottle - motorConfig()->mincommand));
-
-            motor[i] = motorOutput;
+            motor[i] = (int16_t)scaleRangef(motorOutputNormalised, 0, 1, motorConfig()->mincommand, motorConfig()->maxthrottle);
         }
     } else {
         // Disarmed mode
@@ -529,7 +526,7 @@ void FAST_CODE mixTable(const float dT)
 {
 #ifdef USE_DSHOT
     if (FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)) {
-        applyFlipOverAfterCrashModeToMotors();
+        applyTurtleModeToMotors();
         return;
     }
 #endif
diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h
index 2fcb25647b..fcc2625de7 100644
--- a/src/main/flight/mixer.h
+++ b/src/main/flight/mixer.h
@@ -92,7 +92,7 @@ typedef struct motorConfig_s {
     float throttleIdle;                     // Throttle IDLE value based on min_command, max_throttle, in percent
     float throttleScale;                    // Scaling factor for throttle.
     uint8_t motorPoleCount;                 // Magnetic poles in the motors for calculating actual RPM from eRPM provided by ESC telemetry
-    uint8_t flipOverAfterPowerFactor;       // Power factor from 0 to 100% of flip over after crash
+    uint8_t turtleModePowerFactor;          // Power factor from 0 to 100% of flip over after crash
 } motorConfig_t;
 
 PG_DECLARE(motorConfig_t, motorConfig);

From bf320c69fca748f5942c36dbb70f241b00b3793f Mon Sep 17 00:00:00 2001
From: luca <l.giovannesi@gmail.com>
Date: Wed, 21 Apr 2021 14:26:13 +0200
Subject: [PATCH 04/23] Formatted code

---
 src/main/fc/fc_msp_box.c | 26 +++++++++++++-------------
 1 file changed, 13 insertions(+), 13 deletions(-)

diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c
index 2556645259..afd472573b 100644
--- a/src/main/fc/fc_msp_box.c
+++ b/src/main/fc/fc_msp_box.c
@@ -79,18 +79,18 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { BOXCAMERA1, "CAMERA CONTROL 1", 39 },
     { BOXCAMERA2, "CAMERA CONTROL 2", 40 },
     { BOXCAMERA3, "CAMERA CONTROL 3", 41 },
-    {BOXOSDALT1,       "OSD ALT 1",       42 },
-    {BOXOSDALT2,       "OSD ALT 2",       43 },
-    {BOXOSDALT3,       "OSD ALT 3",       44 },
-    {BOXNAVCOURSEHOLD, "NAV COURSE HOLD", 45 },
-    {BOXBRAKING,       "MC BRAKING",      46 },
-    {BOXUSER1,         "USER1", BOX_PERMANENT_ID_USER1 },
-    {BOXUSER2,         "USER2", BOX_PERMANENT_ID_USER2 },
-    {BOXLOITERDIRCHN,  "LOITER CHANGE",   49 },
-    {BOXMSPRCOVERRIDE, "MSP RC OVERRIDE", 50 },
-    {BOXPREARM,        "PREARM",          51 },
-    {BOXTURTLE,        "TURTLE",          52 },
-    {CHECKBOX_ITEM_COUNT, NULL,           0xFF }
+    { BOXOSDALT1, "OSD ALT 1", 42 },
+    { BOXOSDALT2, "OSD ALT 2", 43 },
+    { BOXOSDALT3, "OSD ALT 3", 44 },
+    { BOXNAVCOURSEHOLD, "NAV COURSE HOLD", 45 },
+    { BOXBRAKING, "MC BRAKING", 46 },
+    { BOXUSER1, "USER1", BOX_PERMANENT_ID_USER1 },
+    { BOXUSER2, "USER2", BOX_PERMANENT_ID_USER2 },
+    { BOXLOITERDIRCHN, "LOITER CHANGE", 49 },
+    { BOXMSPRCOVERRIDE, "MSP RC OVERRIDE", 50 },
+    { BOXPREARM, "PREARM", 51 },
+    { BOXTURTLE, "TURTLE", 52 },
+    { CHECKBOX_ITEM_COUNT, NULL, 0xFF }
 };
 
 // this is calculated at startup based on enabled features.
@@ -312,7 +312,7 @@ void initActiveBoxIds(void)
 
 #ifdef USE_DSHOT
     if(STATE(MULTIROTOR) && isMotorProtocolDshot())
-        activeBoxIds[activeBoxIdCount++] = BOXTURTLE;
+        activeBoxIds[activeBoxIdCount++] = BOXFLIPOVERAFTERCRASH;
 #endif
 }
 

From 3956e0a8bbece0517ffdfcac7ff76cc9c62eead5 Mon Sep 17 00:00:00 2001
From: luca <l.giovannesi@gmail.com>
Date: Wed, 21 Apr 2021 14:28:55 +0200
Subject: [PATCH 05/23] Formatted code error

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

diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c
index afd472573b..9ba7e4a0f4 100644
--- a/src/main/fc/fc_msp_box.c
+++ b/src/main/fc/fc_msp_box.c
@@ -312,7 +312,7 @@ void initActiveBoxIds(void)
 
 #ifdef USE_DSHOT
     if(STATE(MULTIROTOR) && isMotorProtocolDshot())
-        activeBoxIds[activeBoxIdCount++] = BOXFLIPOVERAFTERCRASH;
+        activeBoxIds[activeBoxIdCount++] = BOXTURTLE;
 #endif
 }
 

From 073fd3d167dbf186c1456c95e48521dd369e7d61 Mon Sep 17 00:00:00 2001
From: kernel-machine <l.giovannesi@gmail.com>
Date: Mon, 26 Apr 2021 10:39:04 +0200
Subject: [PATCH 06/23] Flight mode renamed with the new notation

---
 src/main/fc/fc_core.c        | 8 ++++----
 src/main/fc/runtime_config.h | 2 +-
 src/main/flight/mixer.c      | 2 +-
 src/main/io/osd.c            | 2 +-
 4 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index 373d2a2e31..c90cbff0a0 100755
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -423,9 +423,9 @@ void disarm(disarmReason_t disarmReason)
         }
 #endif
 #ifdef USE_DSHOT
-        if (FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)) {
+        if (FLIGHT_MODE(TURTLE_MODE)) {
             sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_NORMAL);
-            DISABLE_FLIGHT_MODE(FLIP_OVER_AFTER_CRASH);
+            DISABLE_FLIGHT_MODE(TURTLE_MODE);
         }
 #endif
         statsOnDisarm();
@@ -504,11 +504,11 @@ void tryArm(void)
             emergencyArmingCanOverrideArmingDisabled() &&
             isMotorProtocolDshot() &&
             !ARMING_FLAG(ARMED) &&
-            !FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)
+            !FLIGHT_MODE(TURTLE_MODE)
             ) {
         sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_REVERSED);
         ENABLE_ARMING_FLAG(ARMED);
-        enableFlightMode(FLIP_OVER_AFTER_CRASH);
+        enableFlightMode(TURTLE_MODE);
         return;
     }
 #endif
diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h
index 84fe113c59..55f3c3df56 100644
--- a/src/main/fc/runtime_config.h
+++ b/src/main/fc/runtime_config.h
@@ -98,7 +98,7 @@ typedef enum {
     NAV_COURSE_HOLD_MODE  = (1 << 12),
     FLAPERON              = (1 << 13),
     TURN_ASSISTANT        = (1 << 14),
-    FLIP_OVER_AFTER_CRASH = (1 << 15),
+    TURTLE_MODE           = (1 << 15),
 } flightModeFlags_e;
 
 extern uint32_t flightModeFlags;
diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index 360586cecf..2ee79621f0 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -525,7 +525,7 @@ static int getReversibleMotorsThrottleDeadband(void)
 void FAST_CODE mixTable(const float dT)
 {
 #ifdef USE_DSHOT
-    if (FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)) {
+    if (FLIGHT_MODE(TURTLE_MODE)) {
         applyTurtleModeToMotors();
         return;
     }
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 3cc6898cc0..f5f9ce3e10 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -1715,7 +1715,7 @@ static bool osdDrawSingleElement(uint8_t item)
                 p = "ANGL";
             else if (FLIGHT_MODE(HORIZON_MODE))
                 p = "HOR ";
-            else if (FLIGHT_MODE(FLIP_OVER_AFTER_CRASH))
+            else if (FLIGHT_MODE(TURTLE_MODE))
                 p = "TURT";
 
             displayWrite(osdDisplayPort, elemPosX, elemPosY, p);

From d0ee62867f4d1c208c94cc42df69f518cf43d47e Mon Sep 17 00:00:00 2001
From: kernel-machine <l.giovannesi@gmail.com>
Date: Tue, 27 Apr 2021 12:49:25 +0200
Subject: [PATCH 07/23] Turtle mode OSD priority

---
 src/main/io/osd.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index c0f66b40e0..b6aaf1e0b1 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -1709,6 +1709,8 @@ static bool osdDrawSingleElement(uint8_t item)
                 p = "!FS!";
             else if (FLIGHT_MODE(MANUAL_MODE))
                 p = "MANU";
+            else if (FLIGHT_MODE(TURTLE_MODE))
+                p = "TURT";
             else if (FLIGHT_MODE(NAV_RTH_MODE))
                 p = "RTH ";
             else if (FLIGHT_MODE(NAV_POSHOLD_MODE))
@@ -1729,8 +1731,6 @@ static bool osdDrawSingleElement(uint8_t item)
                 p = "ANGL";
             else if (FLIGHT_MODE(HORIZON_MODE))
                 p = "HOR ";
-            else if (FLIGHT_MODE(TURTLE_MODE))
-                p = "TURT";
 
             displayWrite(osdDisplayPort, elemPosX, elemPosY, p);
             return true;

From 7a51536c31ee1879708846cd2de12e6f2f5ce5d2 Mon Sep 17 00:00:00 2001
From: kernel-machine <l.giovannesi@gmail.com>
Date: Tue, 27 Apr 2021 12:49:36 +0200
Subject: [PATCH 08/23] Docs updated

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

diff --git a/docs/Settings.md b/docs/Settings.md
index ec328afa93..2c350cac64 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -103,7 +103,6 @@
 | failsafe_throttle_low_delay | 0 | 0 | 300 | If failsafe activated when throttle is low for this much time - bypass failsafe and disarm, in 10th of seconds. 0 = No timeout |
 | fixed_wing_auto_arm | OFF |  |  | Auto-arm fixed wing aircraft on throttle above min_check, and disarming with stick commands are disabled, so power cycle is required to disarm. Requires enabled motorstop and no arm switch configured. |
 | flaperon_throw_offset | 200 | FLAPERON_THROW_MIN | FLAPERON_THROW_MAX | Defines throw range in us for both ailerons that will be passed to servo mixer via input source 14 (`FEATURE FLAPS`) when FLAPERON mode is activated. |
-| flip_over_after_crash_power_factor | 65 | 0 | 100 | flip over after crash power factor |
 | fpv_mix_degrees | 0 | 0 | 50 |  |
 | frsky_coordinates_format | 0 | 0 | FRSKY_FORMAT_NMEA | D-Series telemetry only: FRSKY_FORMAT_DMS (default), FRSKY_FORMAT_NMEA |
 | frsky_default_latitude | 0 | -90 | 90 | D-Series telemetry only: OpenTX needs a valid set of coordinates to show compass value. A fake value defined in this setting is sent while no fix is acquired. |
@@ -517,6 +516,7 @@
 | tpa_breakpoint | 1500 | PWM_RANGE_MIN | PWM_RANGE_MAX | See tpa_rate. |
 | tpa_rate | 0 | 0 | 100 | Throttle PID attenuation reduces influence of P on ROLL and PITCH as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate. |
 | tri_unarmed_servo | ON |  |  | On tricopter mix only, if this is set to ON, servo will always be correcting regardless of armed state. to disable this, set it to OFF. |
+| turtle_mode_power_factor | 55 | 0 | 100 | Turtle mode power factor |
 | tz_automatic_dst | OFF |  |  | Automatically add Daylight Saving Time to the GPS time when needed or simply ignore it. Includes presets for EU and the USA - if you live outside these areas it is suggested to manage DST manually via `tz_offset`. |
 | tz_offset | 0 | -1440 | 1440 | Time zone offset from UTC, in minutes. This is applied to the GPS time for logging and time-stamping of Blackbox logs |
 | vbat_adc_channel | _target default_ | ADC_CHN_NONE | ADC_CHN_MAX | ADC channel to use for battery voltage sensor. Defaults to board VBAT input (if available). 0 = disabled |

From c4ca990b0bb169644917cbdb0a77c962d5238a11 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Tue, 27 Apr 2021 13:36:14 +0100
Subject: [PATCH 09/23] Initial build

---
 docs/Settings.md          |  1 +
 src/main/fc/settings.yaml |  9 ++++++++-
 src/main/io/osd.c         |  1 +
 src/main/io/osd.h         |  1 +
 src/main/io/osd_grid.c    | 11 ++++++-----
 5 files changed, 17 insertions(+), 6 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index ec328afa93..ebd3365171 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -424,6 +424,7 @@
 | osd_right_sidebar_scroll_step | 0 |  | 255 | Same as left_sidebar_scroll_step, but for the right sidebar |
 | osd_row_shiftdown | 0 | 0 | 1 | Number of rows to shift the OSD display (increase if top rows are cut off) |
 | osd_rssi_alarm | 20 | 0 | 100 | Value below which to make the OSD RSSI indicator blink |
+| osd_sidebar_height | 3 | 0 | 5 | Height of sidebars in rows. 0 leaves only the level indicator arrows (Not for pixel OSD) |
 | osd_sidebar_horizontal_offset | 0 | -128 | 127 | Sidebar horizontal offset from default position. Positive values move the sidebars closer to the edges. |
 | osd_sidebar_scroll_arrows | OFF |  |  |  |
 | osd_snr_alarm | 4 | -20 | 10 | Value below which Crossfire SNR Alarm pops-up. (dB) |
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index c40ca0d1cb..39ea9ff6b8 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -107,7 +107,7 @@ tables:
     enum: navRTHAllowLanding_e
   - name: bat_capacity_unit
     values: ["MAH", "MWH"]
-    enum: batCapacityUnit_e    
+    enum: batCapacityUnit_e
   - name: bat_voltage_source
     values: ["RAW", "SAG_COMP"]
     enum: batVoltageSource_e
@@ -3252,6 +3252,13 @@ groups:
         default_value: 0
         description: Same as left_sidebar_scroll_step, but for the right sidebar
 
+      - name: osd_sidebar_height
+        field: sidebar_height
+        min: 0
+        max: 5
+        default_value: 3
+        description: Height of sidebars in rows. 0 leaves only the level indicator arrows (Not for pixel OSD)
+
       - name: osd_home_position_arm_screen
         type: bool
         default_value: ON
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 7302ee568f..052ed1d2c6 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -2747,6 +2747,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
     .sidebar_horizontal_offset = SETTING_OSD_SIDEBAR_HORIZONTAL_OFFSET_DEFAULT,
     .left_sidebar_scroll_step = SETTING_OSD_LEFT_SIDEBAR_SCROLL_STEP_DEFAULT,
     .right_sidebar_scroll_step = SETTING_OSD_RIGHT_SIDEBAR_SCROLL_STEP_DEFAULT,
+    .sidebar_height = SETTING_OSD_SIDEBAR_HEIGHT_DEFAULT,
     .osd_home_position_arm_screen = SETTING_OSD_HOME_POSITION_ARM_SCREEN_DEFAULT,
     .pan_servo_index = SETTING_OSD_PAN_SERVO_INDEX_DEFAULT,
     .pan_servo_pwm2centideg = SETTING_OSD_PAN_SERVO_PWM2CENTIDEG_DEFAULT,
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index b32722ce6a..73edc99208 100755
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -368,6 +368,7 @@ typedef struct osdConfig_s {
     uint8_t pan_servo_index;            // Index of the pan servo used for home direction offset
     int8_t pan_servo_pwm2centideg;      // Centidegrees of servo rotation per us pwm
     uint8_t crsf_lq_format;
+    uint8_t sidebar_height;             // sidebar height in rows, 0 turns off sidebars leaving only level indicator arrows
 
 } osdConfig_t;
 
diff --git a/src/main/io/osd_grid.c b/src/main/io/osd_grid.c
index 971fd1f106..da984665aa 100644
--- a/src/main/io/osd_grid.c
+++ b/src/main/io/osd_grid.c
@@ -295,7 +295,7 @@ void osdGridDrawSidebars(displayPort_t *display)
     uint8_t rightDecoration = osdUpdateSidebar(osdConfig()->right_sidebar_scroll, &right, currentTimeMs);
 
     const int hudwidth = OSD_AH_SIDEBAR_WIDTH_POS;
-    const int hudheight = OSD_AH_SIDEBAR_HEIGHT_POS;
+    const int hudheight = osdConfig()->sidebar_height;
 
     // Arrows
     if (osdConfig()->sidebar_scroll_arrows) {
@@ -315,11 +315,12 @@ void osdGridDrawSidebars(displayPort_t *display)
     // Draw AH sides
     int leftX = MAX(elemPosX - hudwidth - osdConfig()->sidebar_horizontal_offset, 0);
     int rightX = MIN(elemPosX + hudwidth + osdConfig()->sidebar_horizontal_offset, display->cols - 1);
-    for (int y = -hudheight; y <= hudheight; y++) {
-        displayWriteChar(display, leftX, elemPosY + y, leftDecoration);
-        displayWriteChar(display, rightX, elemPosY + y, rightDecoration);
+    if (osdConfig()->sidebar_height) {
+        for (int y = -hudheight; y <= hudheight; y++) {
+            displayWriteChar(display, leftX, elemPosY + y, leftDecoration);
+            displayWriteChar(display, rightX, elemPosY + y, rightDecoration);
+        }
     }
-
     // AH level indicators
     displayWriteChar(display, leftX + 1, elemPosY, SYM_AH_RIGHT);
     displayWriteChar(display, rightX - 1, elemPosY, SYM_AH_LEFT);

From 22a15dc0b61427f2b2e45bff91eb4f1cc6472454 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Tue, 27 Apr 2021 19:15:02 +0100
Subject: [PATCH 10/23] Increment PG index

---
 src/main/io/osd.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 052ed1d2c6..7a9179c5ae 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -193,7 +193,7 @@ static bool osdDisplayHasCanvas;
 
 #define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees)
 
-PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 0);
+PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 1);
 PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 0);
 
 static int digitCount(int32_t value)

From f91279cb8a6fe718ab4bfd96132729780528c160 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Wed, 28 Apr 2021 12:40:28 +0200
Subject: [PATCH 11/23] First cut on Alpha-Beta-Gamma filter for gyro

---
 src/main/build/debug.h    |  1 +
 src/main/common/filter.c  | 66 +++++++++++++++++++++++++++++++++++++++
 src/main/common/filter.h  | 15 +++++++++
 src/main/fc/settings.yaml | 24 +++++++++++++-
 src/main/sensors/gyro.c   | 38 +++++++++++++++++++++-
 src/main/sensors/gyro.h   |  5 +++
 src/main/target/common.h  |  1 +
 7 files changed, 148 insertions(+), 2 deletions(-)

diff --git a/src/main/build/debug.h b/src/main/build/debug.h
index e8f47543eb..dfdb4c9533 100644
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@@ -83,5 +83,6 @@ typedef enum {
     DEBUG_FW_D,
     DEBUG_IMU2,
     DEBUG_ALTITUDE,
+    DEBUG_GYRO_ALPHA_BETA_GAMMA,
     DEBUG_COUNT
 } debugType_e;
diff --git a/src/main/common/filter.c b/src/main/common/filter.c
index 3a4c9797ef..0656a5dba7 100644
--- a/src/main/common/filter.c
+++ b/src/main/common/filter.c
@@ -250,3 +250,69 @@ FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint
     filter->y1 = y1;
     filter->y2 = y2;
 }
+
+#ifdef USE_ALPHA_BETA_GAMMA_FILTER
+void alphaBetaGammaFilterInit(alphaBetaGammaFilter_t *filter, float alpha, float boostGain, float halfLife, float dT) {
+    // beta, gamma, and eta gains all derived from
+    // http://yadda.icm.edu.pl/yadda/element/bwmeta1.element.baztech-922ff6cb-e991-417f-93f0-77448f1ef4ec/c/A_Study_Jeong_1_2017.pdf
+
+    const float xi = powf(-alpha + 1.0f, 0.25); // fourth rool of -a + 1
+    filter->xk = 0.0f;
+    filter->vk = 0.0f;
+    filter->ak = 0.0f;
+    filter->jk = 0.0f;
+    filter->a = alpha;
+    filter->b = (1.0f / 6.0f) * powf(1.0f - xi, 2) * (11.0f + 14.0f * xi + 11 * xi * xi);
+    filter->g = 2 * powf(1.0f - xi, 3) * (1 + xi);
+    filter->e = (1.0f / 6.0f) * powf(1 - xi, 4);
+	filter->dT = dT;
+	filter->dT2 = dT * dT;
+    filter->dT3 = dT * dT * dT;
+    pt1FilterInit(&filter->boostFilter, 100, dT);
+
+    const float boost = boostGain * 100;
+
+    filter->boost = (boost * boost / 10000) * 0.003;
+    filter->halfLife = halfLife != 0 ? powf(0.5f, dT / halfLife): 1.0f;
+}
+
+FAST_CODE float alphaBetaGammaFilterApply(alphaBetaGammaFilter_t *filter, float input) {
+    //xk - current system state (ie: position)
+	//vk - derivative of system state (ie: velocity)
+    //ak - derivative of system velociy (ie: acceleration)
+    //jk - derivative of system acceleration (ie: jerk)
+    float rk;   // residual error
+
+    // give the filter limited history
+    filter->xk *= filter->halfLife;
+    filter->vk *= filter->halfLife;
+    filter->ak *= filter->halfLife;
+    filter->jk *= filter->halfLife;
+
+    // update our (estimated) state 'x' from the system (ie pos = pos + vel (last).dT)
+    filter->xk += filter->dT * filter->vk + (1.0f / 2.0f) * filter->dT2 * filter->ak + (1.0f / 6.0f) * filter->dT3 * filter->jk;
+    
+    // update (estimated) velocity (also estimated dterm from measurement)
+    filter->vk += filter->dT * filter->ak + 0.5f * filter->dT2 * filter->jk;
+    filter->ak += filter->dT * filter->jk;
+    
+    // what is our residual error (measured - estimated)
+    rk = input - filter->xk;
+
+    // artificially boost the error to increase the response of the filter
+    rk += pt1FilterApply(&filter->boostFilter, fabsf(rk) * rk * filter->boost);
+    if ((fabsf(rk * filter->a) > fabsf(input - filter->xk))) {
+        rk = (input - filter->xk) / filter->a;
+    }
+    filter->rk = rk; // for logging
+
+    // update our estimates given the residual error.
+    filter->xk += filter->a * rk;
+    filter->vk += filter->b / filter->dT * rk;
+    filter->ak += filter->g / (2.0f * filter->dT2) * rk;
+    filter->jk += filter->e / (6.0f * filter->dT3) * rk;
+
+	return filter->xk;
+}
+
+#endif
\ No newline at end of file
diff --git a/src/main/common/filter.h b/src/main/common/filter.h
index 81c76cd0dc..a6406ee91c 100644
--- a/src/main/common/filter.h
+++ b/src/main/common/filter.h
@@ -56,6 +56,18 @@ typedef struct firFilter_s {
     uint8_t coeffsLength;
 } firFilter_t;
 
+typedef struct alphaBetaGammaFilter_s {
+    float a, b, g, e;
+    float ak; // derivative of system velociy (ie: acceleration)
+    float vk; // derivative of system state (ie: velocity)
+    float xk; // current system state (ie: position)
+    float jk; // derivative of system acceleration (ie: jerk)
+    float rk; // residual error
+    float dT, dT2, dT3;
+    float halfLife, boost;
+    pt1Filter_t boostFilter;
+} alphaBetaGammaFilter_t;
+
 typedef float (*filterApplyFnPtr)(void *filter, float input);
 typedef float (*filterApply4FnPtr)(void *filter, float input, float f_cut, float dt);
 
@@ -86,3 +98,6 @@ float biquadFilterReset(biquadFilter_t *filter, float value);
 float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
 float filterGetNotchQ(float centerFrequencyHz, float cutoffFrequencyHz);
 void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
+
+void alphaBetaGammaFilterInit(alphaBetaGammaFilter_t *filter, float alpha, float boostGain, float halfLife, float dT);
+float alphaBetaGammaFilterApply(alphaBetaGammaFilter_t *filter, float input);
\ No newline at end of file
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index c40ca0d1cb..34a9f9c9e3 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -90,7 +90,8 @@ 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", "DYN_GYRO_LPF", "FW_D", "IMU2", "ALTITUDE"]
+      "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "FW_D", "IMU2", "ALTITUDE",
+      "DEBUG_GYRO_ALPHA_BETA_GAMMA"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
   - name: aux_operator
@@ -292,6 +293,27 @@ groups:
         min: 0
         max: 1
         default_value: 0
+      - name: gyro_abg_alpha
+        description: "Alpha factor for Gyro Alpha-Beta-Gamma filter"
+        default_value: 0
+        field: alphaBetaGammaAlpha
+        condition: USE_ALPHA_BETA_GAMMA_FILTER
+        min: 0
+        max: 1
+      - name: gyro_abg_boost
+        description: "Boost factor for Gyro Alpha-Beta-Gamma filter"
+        default_value: 0.35
+        field: alphaBetaGammaBoost
+        condition: USE_ALPHA_BETA_GAMMA_FILTER
+        min: 0
+        max: 2
+      - name: gyro_abg_half_life
+        description: "Sample half-life for Gyro Alpha-Beta-Gamma filter"
+        default_value: 0.5
+        field: alphaBetaGammaHalfLife
+        condition: USE_ALPHA_BETA_GAMMA_FILTER
+        min: 0
+        max: 10
 
   - name: PG_ADC_CHANNEL_CONFIG
     type: adcChannelConfig_t
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 0d32b49bfd..bfc05c2ba4 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -104,7 +104,14 @@ EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState;
 
 #endif
 
-PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 12);
+#ifdef USE_ALPHA_BETA_GAMMA_FILTER
+
+STATIC_FASTRAM filterApplyFnPtr abgFilterApplyFn;
+STATIC_FASTRAM alphaBetaGammaFilter_t abgFilter[XYZ_AXIS_COUNT];
+
+#endif
+
+PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 13);
 
 PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT,
@@ -130,6 +137,11 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .dynamicGyroNotchMinHz = SETTING_DYNAMIC_GYRO_NOTCH_MIN_HZ_DEFAULT,
     .dynamicGyroNotchEnabled = SETTING_DYNAMIC_GYRO_NOTCH_ENABLED_DEFAULT,
 #endif
+#ifdef USE_ALPHA_BETA_GAMMA_FILTER
+    .alphaBetaGammaAlpha = SETTING_GYRO_ABG_ALPHA_DEFAULT,
+    .alphaBetaGammaBoost = SETTING_GYRO_ABG_BOOST_DEFAULT,
+    .alphaBetaGammaHalfLife = SETTING_GYRO_ABG_HALF_LIFE_DEFAULT,
+#endif
 );
 
 STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware)
@@ -284,6 +296,24 @@ static void gyroInitFilters(void)
             biquadFilterInitNotch(notchFilter1[axis], getLooptime(), gyroConfig()->gyro_notch_hz, gyroConfig()->gyro_notch_cutoff);
         }
     }
+
+#ifdef USE_ALPHA_BETA_GAMMA_FILTER
+    
+    abgFilterApplyFn = (filterApplyFnPtr)nullFilterApply;
+
+    if (gyroConfig()->alphaBetaGammaAlpha > 0) {
+        abgFilterApplyFn = (filterApplyFnPtr)alphaBetaGammaFilterApply;
+        for (int axis = 0; axis < 3; axis++) {
+            alphaBetaGammaFilterInit(
+                &abgFilter[axis], 
+                gyroConfig()->alphaBetaGammaAlpha, 
+                gyroConfig()->alphaBetaGammaBoost, 
+                gyroConfig()->alphaBetaGammaHalfLife, 
+                getLooptime() * 1e-6f
+            );
+        }
+    } 
+#endif
 }
 
 bool gyroInit(void)
@@ -458,6 +488,12 @@ void FAST_CODE NOINLINE gyroUpdate()
         gyroADCf = gyroLpfApplyFn((filter_t *) &gyroLpfState[axis], gyroADCf);
         gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf);
 
+#ifdef USE_ALPHA_BETA_GAMMA_FILTER
+        DEBUG_SET(DEBUG_GYRO_ALPHA_BETA_GAMMA, axis, gyroADCf);
+        gyroADCf = abgFilterApplyFn(&abgFilter[axis], gyroADCf);
+        DEBUG_SET(DEBUG_GYRO_ALPHA_BETA_GAMMA, axis + 3, gyroADCf);
+#endif
+
 #ifdef USE_DYNAMIC_FILTERS
         if (dynamicGyroNotchState.enabled) {
             gyroDataAnalysePush(&gyroAnalyseState, axis, gyroADCf);
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index b84477d686..e2d224d477 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -82,6 +82,11 @@ typedef struct gyroConfig_s {
     uint16_t dynamicGyroNotchMinHz;
     uint8_t dynamicGyroNotchEnabled;
 #endif
+#ifdef USE_ALPHA_BETA_GAMMA_FILTER
+    float alphaBetaGammaAlpha;
+    float alphaBetaGammaBoost;
+    float alphaBetaGammaHalfLife;
+#endif
 } gyroConfig_t;
 
 PG_DECLARE(gyroConfig_t, gyroConfig);
diff --git a/src/main/target/common.h b/src/main/target/common.h
index c0310e7da8..7692dbfa15 100755
--- a/src/main/target/common.h
+++ b/src/main/target/common.h
@@ -84,6 +84,7 @@
 #define USE_PITOT_ADC
 #define USE_PITOT_VIRTUAL
 
+#define USE_ALPHA_BETA_GAMMA_FILTER
 #define USE_DYNAMIC_FILTERS
 #define USE_GYRO_KALMAN
 #define USE_EXTENDED_CMS_MENUS

From 344de36d4e1f1e9397b26e96d68b827c030b2eb5 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Wed, 28 Apr 2021 12:41:25 +0200
Subject: [PATCH 12/23] Docs update

---
 docs/Settings.md | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/docs/Settings.md b/docs/Settings.md
index ec328afa93..0d8eeb7f7e 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -148,6 +148,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_abg_alpha | 0 | 0 | 1 | Alpha factor for Gyro Alpha-Beta-Gamma filter |
+| gyro_abg_boost | 0.35 | 0 | 2 | Boost factor for Gyro Alpha-Beta-Gamma filter |
+| gyro_abg_half_life | 0.5 | 0 | 10 | Sample half-life for Gyro Alpha-Beta-Gamma filter |
 | gyro_dyn_lpf_curve_expo | 5 | 1 | 10 | Expo value for the throttle-to-frequency mapping for Dynamic LPF |
 | gyro_dyn_lpf_max_hz | 500 | 40 | 1000 | Maximum frequency of the gyro Dynamic LPF |
 | gyro_dyn_lpf_min_hz | 200 | 40 | 400 | Minimum frequency of the gyro Dynamic LPF |

From 481558adbbec3fb2acdf07d530cdbfed5e8e69ce Mon Sep 17 00:00:00 2001
From: bkleiner <b.kleiner@str8labs.com>
Date: Sun, 2 May 2021 23:54:19 +0200
Subject: [PATCH 13/23] h7: enable usb voltage detection

---
 src/main/drivers/serial_usb_vcp.c | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c
index f4713af6d7..a90633d6cf 100644
--- a/src/main/drivers/serial_usb_vcp.c
+++ b/src/main/drivers/serial_usb_vcp.c
@@ -216,6 +216,12 @@ void usbVcpInitHardware(void)
 
     /* Start Device Process */
     USBD_Start(&USBD_Device);
+    
+#ifdef STM32H7
+    HAL_PWREx_EnableUSBVoltageDetector();
+    delay(100); // Cold boot failures observed without this, even when USB cable is not connected
+#endif
+
 #else
     Set_System();
     Set_USBClock();

From b8485f6d4defaee746f9fb786857303253e06a54 Mon Sep 17 00:00:00 2001
From: bkleiner <b.kleiner@str8labs.com>
Date: Mon, 3 May 2021 00:19:56 +0200
Subject: [PATCH 14/23] h7: fix bootloader vector table address was returning
 the stack addr instead of the table itself

---
 src/main/drivers/system_stm32h7xx.c | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/src/main/drivers/system_stm32h7xx.c b/src/main/drivers/system_stm32h7xx.c
index 72826b1fae..a1de36f2c2 100644
--- a/src/main/drivers/system_stm32h7xx.c
+++ b/src/main/drivers/system_stm32h7xx.c
@@ -51,10 +51,13 @@ bool isMPUSoftReset(void)
         return false;
 }
 
-#define SYSMEMBOOT_VECTOR_TABLE ((uint32_t *)0x1ff09800)
 uint32_t systemBootloaderAddress(void)
 {
-    return SYSMEMBOOT_VECTOR_TABLE[1];
+#if defined(STM32H743xx) || defined(STM32H750xx) || defined(STM32H723xx) || defined(STM32H725xx)
+    return 0x1ff09800;
+#else
+#error Unknown MCU
+#endif
 }
 
 void systemClockSetup(uint8_t cpuUnderclock)

From ac8c69ab8b5cadda55592f4f6799d9762bd86f7e Mon Sep 17 00:00:00 2001
From: Darren Lines <darren@darrenlines.uk>
Date: Mon, 3 May 2021 07:23:43 +0100
Subject: [PATCH 15/23] Clarified "climb angle" in the launch sequence.

---
 src/main/fc/settings.yaml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index c050d5e8b8..e47f62cf6b 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -2686,7 +2686,7 @@ groups:
         min: 0
         max: 60000
       - name: nav_fw_launch_climb_angle
-        description: "Climb angle for launch sequence (degrees), is also restrained by global max_angle_inclination_pit"
+        description: "Climb angle (attitude of model, not climb slope) for launch sequence (degrees), is also restrained by global max_angle_inclination_pit"
         default_value: 18
         field: fw.launch_climb_angle
         min: 5

From a9baeb85b35ed1060a03a3419e83328c3e0b1d15 Mon Sep 17 00:00:00 2001
From: Darren Lines <darren@darrenlines.uk>
Date: Mon, 3 May 2021 07:25:13 +0100
Subject: [PATCH 16/23] Clarified climb angle in launch settings

---
 docs/Settings.md | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index 1d1014a093..1ad98463b9 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -292,7 +292,7 @@
 | nav_fw_heading_p | 60 | 0 | 255 | P gain of Heading Hold controller (Fixedwing) |
 | nav_fw_land_dive_angle | 2 | -20 | 20 | Dive angle that airplane will use during final landing phase. During dive phase, motor is stopped or IDLE and roll control is locked to 0 degrees |
 | nav_fw_launch_accel | 1863 | 1000 | 20000 | Forward acceleration threshold for bungee launch of throw launch [cm/s/s], 1G = 981 cm/s/s |
-| nav_fw_launch_climb_angle | 18 | 5 | 45 | Climb angle for launch sequence (degrees), is also restrained by global max_angle_inclination_pit |
+| nav_fw_launch_climb_angle | 18 | 5 | 45 | Climb angle (attitude of model, not climb slope) for launch sequence (degrees), is also restrained by global max_angle_inclination_pit |
 | nav_fw_launch_detect_time | 40 | 10 | 1000 | Time for which thresholds have to breached to consider launch happened [ms] |
 | nav_fw_launch_end_time | 3000 | 0 | 5000 | Time for the transition of throttle and pitch angle, between the launch state and the subsequent flight mode [ms] |
 | nav_fw_launch_idle_thr | 1000 | 1000 | 2000 | Launch idle throttle - throttle to be set before launch sequence is initiated. If set below minimum throttle it will force motor stop or at idle throttle (depending if the MOTOR_STOP is enabled). If set above minimum throttle it will force throttle to this value (if MOTOR_STOP is enabled it will be handled according to throttle stick position) |
@@ -545,4 +545,4 @@
 | yaw_lpf_hz | 0 | 0 | 200 | Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below) |
 | yaw_rate | 20 | 2 | 180 | Defines rotation rate on YAW axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. |
 
-> Note: this table is autogenerated. Do not edit it manually.
\ No newline at end of file
+> Note: this table is autogenerated. Do not edit it manually.

From e31c9ee3acf92ab393f109fa9d12e10812c8ad83 Mon Sep 17 00:00:00 2001
From: Darren Lines <darren@darrenlines.uk>
Date: Mon, 3 May 2021 08:55:07 +0100
Subject: [PATCH 17/23] updated docs with the py script

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

diff --git a/docs/Settings.md b/docs/Settings.md
index 1ad98463b9..6e99ed84e5 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -545,4 +545,4 @@
 | yaw_lpf_hz | 0 | 0 | 200 | Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below) |
 | yaw_rate | 20 | 2 | 180 | Defines rotation rate on YAW axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. |
 
-> Note: this table is autogenerated. Do not edit it manually.
+> Note: this table is autogenerated. Do not edit it manually.
\ No newline at end of file

From c8e78aa63f22f6a9e192b009aeb7e6e98c6edc32 Mon Sep 17 00:00:00 2001
From: bkleiner <b.kleiner@str8labs.com>
Date: Wed, 28 Apr 2021 22:25:30 +0200
Subject: [PATCH 18/23] h7: import timer clock calculation from betaflight

---
 src/main/drivers/timer_stm32h7xx.c | 44 +++++++++++++++++++++++++++---
 1 file changed, 40 insertions(+), 4 deletions(-)

diff --git a/src/main/drivers/timer_stm32h7xx.c b/src/main/drivers/timer_stm32h7xx.c
index 20b7f62902..f52aab2049 100644
--- a/src/main/drivers/timer_stm32h7xx.c
+++ b/src/main/drivers/timer_stm32h7xx.c
@@ -56,8 +56,46 @@ uint8_t timerClockDivisor(TIM_TypeDef *tim)
 
 uint32_t timerClock(TIM_TypeDef *tim)
 {
-    UNUSED(tim);
-    return SystemCoreClock;
+    int timpre;
+    uint32_t pclk;
+    uint32_t ppre;
+
+    // Implement the table:
+    // RM0433 (Rev 6) Table 52.
+    // RM0455 (Rev 3) Table 55.
+    // "Ratio between clock timer and pclk"
+    // (Tables are the same, just D2 or CD difference)
+
+#if defined(STM32H743xx) || defined(STM32H750xx) || defined(STM32H723xx) || defined(STM32H725xx)
+#define PERIPH_PRESCALER(bus) ((RCC->D2CFGR & RCC_D2CFGR_D2PPRE ## bus) >> RCC_D2CFGR_D2PPRE ## bus ## _Pos)
+#elif defined(STM32H7A3xx) || defined(STM32H7A3xxQ)
+#define PERIPH_PRESCALER(bus) ((RCC->CDCFGR2 & RCC_CDCFGR2_CDPPRE ## bus) >> RCC_CDCFGR2_CDPPRE ## bus ## _Pos)
+#else
+#error Unknown MCU type
+#endif
+
+    if (tim == TIM1 || tim == TIM8 || tim == TIM15 || tim == TIM16 || tim == TIM17) {
+        // Timers on APB2
+        pclk = HAL_RCC_GetPCLK2Freq();
+        ppre = PERIPH_PRESCALER(2);
+    } else {
+        // Timers on APB1
+        pclk = HAL_RCC_GetPCLK1Freq();
+        ppre = PERIPH_PRESCALER(1);
+    }
+
+    timpre = (RCC->CFGR & RCC_CFGR_TIMPRE) ? 1 : 0;
+
+    int index = (timpre << 3) | ppre;
+
+    static uint8_t periphToKernel[16] = { // The mutiplier table
+        1, 1, 1, 1, 2, 2, 2, 2, // TIMPRE = 0
+        1, 1, 1, 1, 2, 4, 4, 4  // TIMPRE = 1
+    };
+
+    return pclk * periphToKernel[index];
+
+#undef PERIPH_PRESCALER
 }
 
 _TIM_IRQ_HANDLER(TIM1_CC_IRQHandler, 1);
@@ -66,8 +104,6 @@ _TIM_IRQ_HANDLER(TIM3_IRQHandler, 3);
 _TIM_IRQ_HANDLER(TIM4_IRQHandler, 4);
 _TIM_IRQ_HANDLER(TIM5_IRQHandler, 5);
 _TIM_IRQ_HANDLER(TIM8_CC_IRQHandler, 8);
-//_TIM_IRQ_HANDLER(TIM1_BRK_TIM9_IRQHandler, 9);
-//_TIM_IRQ_HANDLER(TIM1_TRG_COM_TIM11_IRQHandler, 11);
 _TIM_IRQ_HANDLER(TIM8_BRK_TIM12_IRQHandler, 12);
 _TIM_IRQ_HANDLER(TIM15_IRQHandler, 15);
 _TIM_IRQ_HANDLER(TIM16_IRQHandler, 16);

From 586b9916b02ee3eca61d11c2d4e21f5bd50f65ed Mon Sep 17 00:00:00 2001
From: bkleiner <b.kleiner@str8labs.com>
Date: Wed, 28 Apr 2021 22:30:21 +0200
Subject: [PATCH 19/23] replace timerClockDivisor with timerClock for more
 flexible timer clocks

---
 src/main/drivers/serial_softserial.c | 2 +-
 src/main/drivers/timer.c             | 2 +-
 src/main/drivers/timer.h             | 2 +-
 src/main/drivers/timer_stm32f30x.c   | 4 ++--
 src/main/drivers/timer_stm32f4xx.c   | 8 ++++----
 src/main/drivers/timer_stm32f7xx.c   | 6 ------
 src/main/drivers/timer_stm32h7xx.c   | 6 ------
 7 files changed, 9 insertions(+), 21 deletions(-)

diff --git a/src/main/drivers/serial_softserial.c b/src/main/drivers/serial_softserial.c
index 4a382536df..aaed828375 100644
--- a/src/main/drivers/serial_softserial.c
+++ b/src/main/drivers/serial_softserial.c
@@ -169,7 +169,7 @@ static bool isTimerPeriodTooLarge(uint32_t timerPeriod)
 
 static void serialTimerConfigureTimebase(TCH_t * tch, uint32_t baud)
 {
-    uint32_t baseClock = SystemCoreClock / timerClockDivisor(tch->timHw->tim);
+    uint32_t baseClock = timerClock(tch->timHw->tim);
     uint32_t clock = baseClock;
     uint32_t timerPeriod;
 
diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c
index 4b7e15b551..4418e9ad2c 100755
--- a/src/main/drivers/timer.c
+++ b/src/main/drivers/timer.c
@@ -240,7 +240,7 @@ uint32_t timerGetBaseClock(TCH_t * tch)
 
 uint32_t timerGetBaseClockHW(const timerHardware_t * timHw)
 {
-    return SystemCoreClock / timerClockDivisor(timHw->tim);
+    return timerClock(timHw->tim);
 }
 
 bool timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount)
diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h
index 661f705584..699d38e953 100644
--- a/src/main/drivers/timer.h
+++ b/src/main/drivers/timer.h
@@ -157,7 +157,7 @@ typedef enum {
     TYPE_TIMER
 } channelType_t;
 
-uint8_t timerClockDivisor(TIM_TypeDef *tim);
+uint32_t timerClock(TIM_TypeDef *tim);
 uint32_t timerGetBaseClockHW(const timerHardware_t * timHw);
 
 const timerHardware_t * timerGetByUsageFlag(timerUsageFlag_e flag);
diff --git a/src/main/drivers/timer_stm32f30x.c b/src/main/drivers/timer_stm32f30x.c
index 5aaa4df205..545dab0c8f 100644
--- a/src/main/drivers/timer_stm32f30x.c
+++ b/src/main/drivers/timer_stm32f30x.c
@@ -48,10 +48,10 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
     [16] = { .tim = TIM17, .rcc = RCC_APB2(TIM17), .irq = TIM1_TRG_COM_TIM17_IRQn },
 };
 
-uint8_t timerClockDivisor(TIM_TypeDef *tim)
+uint32_t timerClock(TIM_TypeDef *tim)
 {
     UNUSED(tim);
-    return 1;
+    return SystemCoreClock;
 }
 
 _TIM_IRQ_HANDLER(TIM1_CC_IRQHandler, 1);
diff --git a/src/main/drivers/timer_stm32f4xx.c b/src/main/drivers/timer_stm32f4xx.c
index 75b46f1cf8..bc5ec353ad 100644
--- a/src/main/drivers/timer_stm32f4xx.c
+++ b/src/main/drivers/timer_stm32f4xx.c
@@ -95,16 +95,16 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
 #endif
 };
 
-uint8_t timerClockDivisor(TIM_TypeDef *tim)
+uint32_t timerClock(TIM_TypeDef *tim)
 {
 #if defined (STM32F411xE)
     UNUSED(tim);
-    return 1;
+    return SystemCoreClock;
 #elif defined (STM32F40_41xxx) || defined (STM32F427_437xx) || defined (STM32F446xx)
     if (tim == TIM1 || tim == TIM8 || tim == TIM9 || tim == TIM10 || tim == TIM11) {
-        return 1;
+        return SystemCoreClock;
     } else {
-        return 2;
+        return SystemCoreClock / 2;
     }
 #else
     #error "No timer clock defined correctly for the MCU"
diff --git a/src/main/drivers/timer_stm32f7xx.c b/src/main/drivers/timer_stm32f7xx.c
index 4f8057baf1..ca4f4cb8b3 100644
--- a/src/main/drivers/timer_stm32f7xx.c
+++ b/src/main/drivers/timer_stm32f7xx.c
@@ -48,12 +48,6 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
     [13] = { .tim = TIM14, .rcc = RCC_APB1(TIM14), .irq = TIM8_TRG_COM_TIM14_IRQn},
 };
 
-uint8_t timerClockDivisor(TIM_TypeDef *tim)
-{
-    UNUSED(tim);
-    return 1;
-}
-
 uint32_t timerClock(TIM_TypeDef *tim)
 {
     UNUSED(tim);
diff --git a/src/main/drivers/timer_stm32h7xx.c b/src/main/drivers/timer_stm32h7xx.c
index f52aab2049..b01c8647e4 100644
--- a/src/main/drivers/timer_stm32h7xx.c
+++ b/src/main/drivers/timer_stm32h7xx.c
@@ -48,12 +48,6 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
     [13] = { .tim = TIM17, .rcc = RCC_APB2(TIM17),  .irq = TIM17_IRQn},
 };
 
-uint8_t timerClockDivisor(TIM_TypeDef *tim)
-{
-    UNUSED(tim);
-    return 1;
-}
-
 uint32_t timerClock(TIM_TypeDef *tim)
 {
     int timpre;

From ede5df23cba120a1bcb6def319c9ae3e00d136a6 Mon Sep 17 00:00:00 2001
From: bkleiner <b.kleiner@str8labs.com>
Date: Thu, 29 Apr 2021 22:42:07 +0200
Subject: [PATCH 20/23] import timer defintions from betaflight

---
 src/main/target/MATEKH743/target.c | 26 ++++++++++++++++++++++++--
 1 file changed, 24 insertions(+), 2 deletions(-)

diff --git a/src/main/target/MATEKH743/target.c b/src/main/target/MATEKH743/target.c
index 209789cb8a..1b4dab4973 100644
--- a/src/main/target/MATEKH743/target.c
+++ b/src/main/target/MATEKH743/target.c
@@ -33,8 +33,30 @@ BUSDEV_REGISTER_SPI_TAG(busdev_imu2_6500,   DEVHW_MPU6500,  IMU2_SPI_BUS,   IMU2
 //BUSDEV_REGISTER_SPI_TAG(busdev_imu2_6000,   DEVHW_MPU6000,  IMU2_SPI_BUS,   IMU2_CS_PIN,    IMU2_EXTI_PIN,  1,  DEVFLAGS_NONE,  IMU2_ALIGN);
 
 const timerHardware_t timerHardware[] = {
-    DEF_TIM(TIM2,  CH1, PA0,  TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0),   // S1
-    DEF_TIM(TIM2,  CH2, PA1,  TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0),   // S2
+    DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 2),   // S1
+    DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 2),   // S2
+
+    DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S3  
+    DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S4
+    DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S5
+    DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S6
+
+    DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1),   // S7
+    DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1),   // S8
+    DEF_TIM(TIM4, CH3, PD14, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1),   // S9
+
+    DEF_TIM(TIM4, CH4, PD15, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0),   // S10 DMA_NONE
+
+    DEF_TIM(TIM15, CH1, PE5, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0),   // S11
+    DEF_TIM(TIM15, CH2, PE6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0),   // S12
+
+    DEF_TIM(TIM1,  CH1, PA8,  TIM_USE_LED, 0, 0),    // LED_2812
+    DEF_TIM(TIM2,  CH1, PA15, TIM_USE_BEEPER, 0, 0),  // BEEPER PWM
+
+    DEF_TIM(TIM8,  CH2, PC7,  TIM_USE_PPM, 0, 0),   // RX6 PPM
+    DEF_TIM(TIM8,  CH1, PC6,  TIM_USE_ANY, 0, 0),   // TX6    
+    DEF_TIM(TIM16, CH1, PB8,  TIM_USE_ANY, 0, 0),   // RX4
+    DEF_TIM(TIM17, CH1, PB9,  TIM_USE_ANY, 0, 0),   // TX4
 };
 
 const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

From f4b92bc0f766c88c8b2f3ff5faad85181b924b04 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Mon, 3 May 2021 14:44:56 +0100
Subject: [PATCH 21/23] WP mission sealevel altitude datum (#6662)

Changed to using WP P3 for altitude datum flag now set individually for each geospatial WP in mission.
---
 src/main/io/osd.c                |  5 +++--
 src/main/navigation/navigation.c | 17 +++++++++++------
 src/main/navigation/navigation.h |  7 +++++++
 3 files changed, 21 insertions(+), 8 deletions(-)

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 7f9d038753..81ab58b480 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -1963,9 +1963,10 @@ static bool osdDrawSingleElement(uint8_t item)
                         wp2.lon = posControl.waypointList[j].lon;
                         wp2.alt = posControl.waypointList[j].alt;
                         fpVector3_t poi;
-                        geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp2, GEO_ALT_RELATIVE);
+                        geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp2, waypointMissionAltConvMode(posControl.waypointList[j].p3));
+                        int32_t altConvModeAltitude = waypointMissionAltConvMode(posControl.waypointList[j].p3) == GEO_ALT_ABSOLUTE ? osdGetAltitudeMsl() : osdGetAltitude();
                         while (j > 9) j -= 10; // Only the last digit displayed if WP>=10, no room for more
-                        osdHudDrawPoi(calculateDistanceToDestination(&poi) / 100, osdGetHeadingAngle(calculateBearingToDestination(&poi) / 100), (posControl.waypointList[j].alt - osdGetAltitude())/ 100, 2, SYM_WAYPOINT, 49 + j, i);
+                        osdHudDrawPoi(calculateDistanceToDestination(&poi) / 100, osdGetHeadingAngle(calculateBearingToDestination(&poi) / 100), (posControl.waypointList[j].alt - altConvModeAltitude)/ 100, 2, SYM_WAYPOINT, 49 + j, i);
                     }
                 }
             }
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 94db2be867..94e3c85e0f 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -236,7 +236,7 @@ void calculateInitialHoldPosition(fpVector3_t * pos);
 void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t distance);
 void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distance);
 static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome);
-static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint);
+static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv);
 static navigationFSMEvent_t nextForNonGeoStates(void);
 static bool isWaypointMissionValid(void);
 
@@ -1502,7 +1502,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
             if (STATE(MULTIROTOR)) {
                 wpHeadingControl.mode = NAV_WP_HEAD_MODE_POI;
                 mapWaypointToLocalPosition(&wpHeadingControl.poi_pos,
-                                           &posControl.waypointList[posControl.activeWaypointIndex]);
+                                           &posControl.waypointList[posControl.activeWaypointIndex], GEO_ALT_RELATIVE);
             }
             return nextForNonGeoStates();
 
@@ -2890,7 +2890,7 @@ void resetSafeHomes(void)
 }
 #endif
 
-static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint)
+static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv)
 {
     gpsLocation_t wpLLH;
 
@@ -2898,7 +2898,7 @@ static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint
     wpLLH.lon = waypoint->lon;
     wpLLH.alt = waypoint->alt;
 
-    geoConvertGeodeticToLocal(localPos, &posControl.gpsOrigin, &wpLLH, GEO_ALT_RELATIVE);
+    geoConvertGeodeticToLocal(localPos, &posControl.gpsOrigin, &wpLLH, altConv);
 }
 
 static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos)
@@ -2912,10 +2912,15 @@ static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos
     setDesiredPosition(&posControl.activeWaypoint.pos, posControl.activeWaypoint.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
 }
 
+geoAltitudeConversionMode_e waypointMissionAltConvMode(geoAltitudeDatumFlag_e datumFlag)
+{
+    return datumFlag == NAV_WP_MSL_DATUM ? GEO_ALT_ABSOLUTE : GEO_ALT_RELATIVE;
+}
+
 static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint)
 {
     fpVector3_t localPos;
-    mapWaypointToLocalPosition(&localPos, waypoint);
+    mapWaypointToLocalPosition(&localPos, waypoint, waypointMissionAltConvMode(waypoint->p3));
     calculateAndSetActiveWaypointToLocalPosition(&localPos);
 }
 
@@ -3321,7 +3326,7 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
     // Don't allow arming if first waypoint is farther than configured safe distance
     if ((posControl.waypointCount > 0) && (navConfig()->general.waypoint_safe_distance != 0)) {
         fpVector3_t startingWaypointPos;
-        mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[0]);
+        mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[0], GEO_ALT_RELATIVE);
 
         const bool navWpMissionStartTooFar = calculateDistanceToDestination(&startingWaypointPos) > navConfig()->general.waypoint_safe_distance;
 
diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
index 0638e6e844..86cec421bf 100755
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@@ -481,6 +481,11 @@ typedef enum {
     GEO_ORIGIN_RESET_ALTITUDE
 } geoOriginResetMode_e;
 
+typedef enum {
+    NAV_WP_TAKEOFF_DATUM,
+    NAV_WP_MSL_DATUM
+} geoAltitudeDatumFlag_e;
+
 // geoSetOrigin stores the location provided in llh as a GPS origin in the
 // provided origin parameter. resetMode indicates wether all origin coordinates
 // should be overwritten by llh (GEO_ORIGIN_SET) or just the altitude, leaving
@@ -501,6 +506,8 @@ bool geoConvertGeodeticToLocalOrigin(fpVector3_t * pos, const gpsLocation_t *llh
 // the provided origin is valid and the conversion could be performed.
 bool geoConvertLocalToGeodetic(gpsLocation_t *llh, const gpsOrigin_t *origin, const fpVector3_t *pos);
 float geoCalculateMagDeclination(const gpsLocation_t * llh); // degrees units
+// Select absolute or relative altitude based on WP mission flag setting
+geoAltitudeConversionMode_e waypointMissionAltConvMode(geoAltitudeDatumFlag_e datumFlag);
 
 /* Distance/bearing calculation */
 bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos);

From 40f23445f73b0461c644d960c44f1fd1d0ad1050 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Mon, 3 May 2021 16:14:02 +0100
Subject: [PATCH 22/23] WP Mission Landing Elevation Setting (#6822)

* Initial build

* Fix p1 error + change cm to meters

* Update navigation.c

Add correction if absolute SL landing elevation used (related PR #6662)
---
 src/main/navigation/navigation.c | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 94e3c85e0f..eba7382530 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -1875,6 +1875,13 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode)
             break;
 
         case RTH_HOME_FINAL_LAND:
+            // if WP mission p2 > 0 use p2 value as landing elevation (in meters !) (otherwise default to takeoff home elevation)
+            if (FLIGHT_MODE(NAV_WP_MODE) && posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND && posControl.waypointList[posControl.activeWaypointIndex].p2 != 0) {
+                posControl.rthState.homeTmpWaypoint.z = posControl.waypointList[posControl.activeWaypointIndex].p2 * 100;   // 100 -> m to cm
+                if (waypointMissionAltConvMode(posControl.waypointList[posControl.activeWaypointIndex].p3) == GEO_ALT_ABSOLUTE) {
+                    posControl.rthState.homeTmpWaypoint.z -= posControl.gpsOrigin.alt;  // correct to relative if absolute SL altitude datum used
+                }
+            }
             break;
     }
 

From fe795ca05d17dce4f5a8c7bf998964c662e1caf6 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Mon, 3 May 2021 17:41:22 +0200
Subject: [PATCH 23/23] Update H743 pin mapping in MC mode

---
 src/main/target/MATEKH743/target.c | 11 +++++------
 1 file changed, 5 insertions(+), 6 deletions(-)

diff --git a/src/main/target/MATEKH743/target.c b/src/main/target/MATEKH743/target.c
index 1b4dab4973..5a7f27b98d 100644
--- a/src/main/target/MATEKH743/target.c
+++ b/src/main/target/MATEKH743/target.c
@@ -41,14 +41,13 @@ const timerHardware_t timerHardware[] = {
     DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S5
     DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S6
 
-    DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1),   // S7
-    DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1),   // S8
-    DEF_TIM(TIM4, CH3, PD14, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1),   // S9
-
+    DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 1),   // S7
+    DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 1),   // S8
+    DEF_TIM(TIM4, CH3, PD14, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 1),   // S9
     DEF_TIM(TIM4, CH4, PD15, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0),   // S10 DMA_NONE
 
-    DEF_TIM(TIM15, CH1, PE5, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0),   // S11
-    DEF_TIM(TIM15, CH2, PE6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0),   // S12
+    DEF_TIM(TIM15, CH1, PE5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S11
+    DEF_TIM(TIM15, CH2, PE6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S12
 
     DEF_TIM(TIM1,  CH1, PA8,  TIM_USE_LED, 0, 0),    // LED_2812
     DEF_TIM(TIM2,  CH1, PA15, TIM_USE_BEEPER, 0, 0),  // BEEPER PWM