From 8d26904048b158b02913e9f12708738034b5824f Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Tue, 3 Jan 2023 23:15:28 +0000
Subject: [PATCH 01/94] first cut

---
 src/main/CMakeLists.txt        |  2 +
 src/main/drivers/osd_symbols.h |  2 +
 src/main/fc/fc_core.c          | 10 ++--
 src/main/fc/fc_core.h          |  2 +-
 src/main/fc/fc_msp_box.c       |  3 ++
 src/main/fc/multifunction.c    | 87 +++++++++++++++++++++++++++++++
 src/main/fc/multifunction.h    | 34 +++++++++++++
 src/main/fc/rc_controls.c      |  2 +-
 src/main/fc/rc_modes.h         |  1 +
 src/main/io/osd.c              | 93 ++++++++++++++++++++++++++++++++--
 src/main/io/osd.h              |  9 ++++
 11 files changed, 236 insertions(+), 9 deletions(-)
 create mode 100644 src/main/fc/multifunction.c
 create mode 100644 src/main/fc/multifunction.h

diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt
index 317da760b6e..39f46ca9547 100644
--- a/src/main/CMakeLists.txt
+++ b/src/main/CMakeLists.txt
@@ -281,6 +281,8 @@ main_sources(COMMON_SRC
     fc/firmware_update.h
     fc/firmware_update_common.c
     fc/firmware_update_common.h
+    fc/multifunction.c
+    fc/multifunction.h
     fc/rc_smoothing.c
     fc/rc_smoothing.h
     fc/rc_adjustments.c
diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h
index adcb595ce2b..47340d82a06 100644
--- a/src/main/drivers/osd_symbols.h
+++ b/src/main/drivers/osd_symbols.h
@@ -174,6 +174,8 @@
 #define SYM_FLIGHT_MINS_REMAINING   0xDA  // 218 Flight time (mins) remaining
 #define SYM_FLIGHT_HOURS_REMAINING  0xDB  // 219 Flight time (hours) remaining
 #define SYM_GROUND_COURSE           0xDC  // 220 Ground course
+#define SYM_ALERT                   0xDD  // 221 General alert symbol
+
 #define SYM_CROSS_TRACK_ERROR       0xFC  // 252 Cross track error
 
 #define SYM_LOGO_START              0x101 // 257 to 297, INAV logo
diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index 729439aa5ad..898c624bf04 100755
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -365,7 +365,7 @@ static bool emergencyArmingCanOverrideArmingDisabled(void)
 
 static bool emergencyArmingIsEnabled(void)
 {
-    return emergencyArmingUpdate(IS_RC_MODE_ACTIVE(BOXARM)) && emergencyArmingCanOverrideArmingDisabled();
+    return emergencyArmingUpdate(IS_RC_MODE_ACTIVE(BOXARM), false) && emergencyArmingCanOverrideArmingDisabled();
 }
 
 static void processPilotAndFailSafeActions(float dT)
@@ -457,7 +457,7 @@ disarmReason_t getDisarmReason(void)
     return lastDisarmReason;
 }
 
-bool emergencyArmingUpdate(bool armingSwitchIsOn)
+bool emergencyArmingUpdate(bool armingSwitchIsOn, bool forceArm)
 {
     if (ARMING_FLAG(ARMED)) {
         return false;
@@ -486,6 +486,10 @@ bool emergencyArmingUpdate(bool armingSwitchIsOn)
         toggle = true;
     }
 
+    if (forceArm) {
+        counter = EMERGENCY_ARMING_MIN_ARM_COUNT + 1;
+    }
+
     return counter >= EMERGENCY_ARMING_MIN_ARM_COUNT;
 }
 
@@ -838,7 +842,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
 
     if (!ARMING_FLAG(ARMED)) {
         armTime = 0;
-        
+
         processDelayedSave();
     }
 
diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h
index 37d0bbda79a..c66a0050ba2 100644
--- a/src/main/fc/fc_core.h
+++ b/src/main/fc/fc_core.h
@@ -42,7 +42,7 @@ timeUs_t getLastDisarmTimeUs(void);
 void tryArm(void);
 disarmReason_t getDisarmReason(void);
 
-bool emergencyArmingUpdate(bool armingSwitchIsOn);
+bool emergencyArmingUpdate(bool armingSwitchIsOn, bool forceArm);
 
 bool areSensorsCalibrating(void);
 float getFlightTime(void);
diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c
index 80458c3ac37..b8b220fdba9 100644
--- a/src/main/fc/fc_msp_box.c
+++ b/src/main/fc/fc_msp_box.c
@@ -97,6 +97,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { .boxId = BOXPLANWPMISSION,    .boxName = "WP PLANNER",        .permanentId = 55 },
     { .boxId = BOXSOARING,          .boxName = "SOARING",           .permanentId = 56 },
     { .boxId = BOXCHANGEMISSION,    .boxName = "MISSION CHANGE",    .permanentId = 59 },
+    { .boxId = BOXMULTIFUNCTION,    .boxName = "MULTI FUNCTION",    .permanentId = 60 },
     { .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL,                .permanentId = 0xFF }
 };
 
@@ -178,6 +179,7 @@ void initActiveBoxIds(void)
     RESET_BOX_ID_COUNT;
     ADD_ACTIVE_BOX(BOXARM);
     ADD_ACTIVE_BOX(BOXPREARM);
+    ADD_ACTIVE_BOX(BOXMULTIFUNCTION);
 
     if (sensors(SENSOR_ACC) && STATE(ALTITUDE_CONTROL)) {
         ADD_ACTIVE_BOX(BOXANGLE);
@@ -410,6 +412,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
 #ifdef USE_MULTI_MISSION
     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCHANGEMISSION)),   BOXCHANGEMISSION);
 #endif
+    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMULTIFUNCTION)),   BOXMULTIFUNCTION);
 
     memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t));
     for (uint32_t i = 0; i < activeBoxIdCount; i++) {
diff --git a/src/main/fc/multifunction.c b/src/main/fc/multifunction.c
new file mode 100644
index 00000000000..5772d849232
--- /dev/null
+++ b/src/main/fc/multifunction.c
@@ -0,0 +1,87 @@
+/*
+ * 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"
+#include "build/debug.h"
+#include "drivers/time.h"
+
+#include "fc/fc_core.h"
+#include "fc/multifunction.h"
+#include "fc/rc_modes.h"
+
+#include "io/osd.h"
+#include "navigation/navigation.h"
+
+static void multiFunctionApply(multi_function_e selectedItem)
+{
+    switch (selectedItem) {
+    case MULTI_FUNC_NONE:
+        return;
+    case MULTI_FUNC_1:
+        resetOsdWarningMask();
+        break;
+    case MULTI_FUNC_2:
+        emergencyArmingUpdate(true, true);
+        break;
+    case MULTI_FUNC_COUNT:
+        break;
+    }
+}
+
+bool multiFunctionSelection(multi_function_e * returnItem)
+{
+    static timeMs_t startTimer;
+    static timeMs_t selectTimer;
+    static int8_t selectedItem = 0;
+    static bool toggle = true;
+    const timeMs_t currentTime = millis();
+
+    if (IS_RC_MODE_ACTIVE(BOXMULTIFUNCTION)) {
+        if (selectTimer) {
+            if (currentTime - selectTimer > 3000) {
+                *returnItem = selectedItem;
+                multiFunctionApply(selectedItem);
+                selectTimer = 0;
+                selectedItem = 0;
+                return true;
+            }
+        } else if (toggle) {
+            selectedItem++;
+            selectedItem = selectedItem == MULTI_FUNC_COUNT ? 1 : selectedItem;
+            selectTimer = currentTime;
+        }
+        startTimer = currentTime;
+        toggle = false;
+    } else if (startTimer) {
+        selectTimer = 0;
+        if (currentTime - startTimer > 2000) {
+            startTimer = 0;
+            selectedItem = 0;
+        }
+        toggle = true;
+    }
+
+    *returnItem = selectedItem;
+    return false;
+}
diff --git a/src/main/fc/multifunction.h b/src/main/fc/multifunction.h
new file mode 100644
index 00000000000..b302dcf9869
--- /dev/null
+++ b/src/main/fc/multifunction.h
@@ -0,0 +1,34 @@
+/*
+ * 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
+
+typedef enum {
+    MULTI_FUNC_NONE,
+    MULTI_FUNC_1,
+    MULTI_FUNC_2,
+    MULTI_FUNC_COUNT,
+} multi_function_e;
+
+bool multiFunctionSelection(multi_function_e * returnItem);
diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c
index 8391f6bebfa..101e6098ae2 100644
--- a/src/main/fc/rc_controls.c
+++ b/src/main/fc/rc_controls.c
@@ -223,7 +223,7 @@ void processRcStickPositions(bool isThrottleLow)
             rcDisarmTimeMs = currentTimeMs;
             tryArm();
         } else {
-            emergencyArmingUpdate(armingSwitchIsActive);
+            emergencyArmingUpdate(armingSwitchIsActive, false);
             // Disarming via ARM BOX
             // Don't disarm via switch if failsafe is active or receiver doesn't receive data - we can't trust receiver
             // and can't afford to risk disarming in the air
diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h
index a72be3ddb80..be8ca599e81 100644
--- a/src/main/fc/rc_modes.h
+++ b/src/main/fc/rc_modes.h
@@ -77,6 +77,7 @@ typedef enum {
     BOXUSER3         = 48,
     BOXUSER4         = 49,
     BOXCHANGEMISSION = 50,
+    BOXMULTIFUNCTION = 51,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index a671b562104..0415858dac8 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -77,6 +77,7 @@ FILE_COMPILE_FOR_SPEED
 #include "fc/controlrate_profile.h"
 #include "fc/fc_core.h"
 #include "fc/fc_tasks.h"
+#include "fc/multifunction.h"
 #include "fc/rc_adjustments.h"
 #include "fc/rc_controls.h"
 #include "fc/rc_modes.h"
@@ -183,6 +184,9 @@ static bool fullRedraw = false;
 static uint8_t armState;
 static uint8_t statsPagesCheck = 0;
 
+textAttributes_t osdGetMultiFunctionMessage(char *buff);
+static osd_warnings_status_flags_e osdWarningsMask = 0;
+
 typedef struct osdMapData_s {
     uint32_t scale;
     char referenceSymbol;
@@ -1684,8 +1688,10 @@ static bool osdDrawSingleElement(uint8_t item)
         buff[1] = SYM_SAT_R;
         tfp_sprintf(buff + 2, "%2d", gpsSol.numSat);
         if (!STATE(GPS_FIX)) {
-            if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) {
-                strcpy(buff + 2, "X!");
+            hardwareSensorStatus_e sensorStatus = getHwGPSStatus();
+            if (sensorStatus == HW_SENSOR_UNAVAILABLE || sensorStatus == HW_SENSOR_UNHEALTHY) {
+                buff[2] = SYM_ALERT;
+                buff[3] = '\0';
             }
             TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
         }
@@ -3267,6 +3273,12 @@ static bool osdDrawSingleElement(uint8_t item)
         }
 #endif // USE_ADC
 #endif // USE_POWER_LIMITS
+    case OSD_MULTI_FUNCTION:
+        {
+            displayWrite(osdDisplayPort, elemPosX, elemPosY, "          ");
+            elemAttr = osdGetMultiFunctionMessage(buff);
+            break;
+        }
 
     default:
         return false;
@@ -3969,7 +3981,7 @@ static void osdShowStatsPage1(void)
 
     displayWrite(osdDisplayPort, statNameX, top, "DISARMED BY      :");
     displayWrite(osdDisplayPort, statValuesX, top++, disarmReasonStr[getDisarmReason()]);
-    
+
     if (savingSettings == true) {
         displayWrite(osdDisplayPort, statNameX, top++, OSD_MESSAGE_STR(OSD_MSG_SAVING_SETTNGS));
     } else if (notify_settings_saved > 0) {
@@ -3979,7 +3991,7 @@ static void osdShowStatsPage1(void)
             displayWrite(osdDisplayPort, statNameX, top++, OSD_MESSAGE_STR(OSD_MSG_SETTINGS_SAVED));
         }
     }
-    
+
     displayCommitTransaction(osdDisplayPort);
 }
 
@@ -4656,4 +4668,77 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
     return elemAttr;
 }
 
+void resetOsdWarningMask(void)
+{
+    osdWarningsMask = 0;
+}
+
+bool checkOsdWarning(bool condition, osd_warnings_status_flags_e warningType)
+{
+    static timeMs_t newWarningStartTime = 0;
+    const timeMs_t currentTimeMs = millis();
+
+    if (condition) {
+        if (!(osdWarningsMask & warningType)) {
+            newWarningStartTime = currentTimeMs;
+            osdWarningsMask |= warningType;
+        }
+        if (currentTimeMs - newWarningStartTime < 10000) {  // Display new warnings for 10s
+            return true;
+        }
+    } else if (osdWarningsMask & warningType) {
+        osdWarningsMask ^= warningType;
+    }
+
+    return false;
+}
+
+textAttributes_t osdGetMultiFunctionMessage(char *buff)
+{
+    textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE;
+    uint8_t warningCount = BITCOUNT(osdWarningsMask);
+
+    multi_function_e multiFuncItem;
+    multiFunctionSelection(&multiFuncItem);
+    if (multiFuncItem) {
+        switch (multiFuncItem) {
+        case MULTI_FUNC_NONE:
+        case MULTI_FUNC_1:
+            strcpy(buff, warningCount ? "WARNINGS  " : "0 WARNINGS");
+            break;
+        case MULTI_FUNC_2:
+            strcpy(buff, "EMERG ARM ");
+            break;
+        case MULTI_FUNC_COUNT:
+            break;
+        }
+
+        return elemAttr;
+    }
+
+/* WARNINGS --------------------------------------------- */
+    const char *messages[2];
+    const char *message = NULL;
+    uint8_t messageCount = 0;
+
+    if (checkOsdWarning(!STATE(GPS_FIX), OSD_WARN_1)) {
+        hardwareSensorStatus_e sensorStatus = getHwGPSStatus();
+        bool gpsFailed = sensorStatus == HW_SENSOR_UNAVAILABLE || sensorStatus == HW_SENSOR_UNHEALTHY;
+        messages[messageCount++] = gpsFailed ? "GPS FAILED" : "NO GPS FIX";
+    }
+
+    if (messageCount) {
+        message = messages[OSD_ALTERNATING_CHOICES(2000, messageCount)];    // display each warning for 2s
+        strcpy(buff, message);
+        TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+        return elemAttr;
+    } else if (warningCount) {
+        buff[0] = SYM_ALERT;
+        tfp_sprintf(buff + 1, "%u", warningCount);
+        return elemAttr;
+    }
+/* WARNINGS --------------------------------------------- */
+
+    return elemAttr;
+}
 #endif // OSD
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index ba5b26321ab..19071ac1e2f 100644
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -270,6 +270,7 @@ typedef enum {
     OSD_NAV_WP_MULTI_MISSION_INDEX,
     OSD_GROUND_COURSE,      // 140
     OSD_CROSS_TRACK_ERROR,
+    OSD_MULTI_FUNCTION,
     OSD_ITEM_COUNT // MUST BE LAST
 } osd_items_e;
 
@@ -328,6 +329,12 @@ typedef enum {
     OSD_CRSF_LQ_TYPE3
 } osd_crsf_lq_format_e;
 
+typedef enum {
+    OSD_WARN_NONE   = 0,
+    OSD_WARN_1      = 1 << 0,
+    OSD_WARN_2      = 1 << 1,
+} osd_warnings_status_flags_e;
+
 typedef struct osdLayoutsConfig_s {
     // Layouts
     uint16_t item_pos[OSD_LAYOUT_COUNT][OSD_ITEM_COUNT];
@@ -473,6 +480,8 @@ int32_t osdGetAltitude(void);
 void osdStartedSaveProcess(void);
 void osdShowEEPROMSavedNotification(void);
 
+void resetOsdWarningMask(void);
+
 void osdCrosshairPosition(uint8_t *x, uint8_t *y);
 bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length);
 void osdFormatAltitudeSymbol(char *buff, int32_t alt);

From 069e876dd02a054024dd4d4f0f2dcbba4f4de166 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Wed, 4 Jan 2023 13:45:10 +0000
Subject: [PATCH 02/94] update

---
 src/main/fc/multifunction.c |  8 +++----
 src/main/io/osd.c           | 42 +++++++++++++++++++------------------
 2 files changed, 26 insertions(+), 24 deletions(-)

diff --git a/src/main/fc/multifunction.c b/src/main/fc/multifunction.c
index 5772d849232..c77807cd827 100644
--- a/src/main/fc/multifunction.c
+++ b/src/main/fc/multifunction.c
@@ -38,10 +38,10 @@ static void multiFunctionApply(multi_function_e selectedItem)
     switch (selectedItem) {
     case MULTI_FUNC_NONE:
         return;
-    case MULTI_FUNC_1:
+    case MULTI_FUNC_1:  // redisplay current warnings
         resetOsdWarningMask();
         break;
-    case MULTI_FUNC_2:
+    case MULTI_FUNC_2:  // emergency ARM
         emergencyArmingUpdate(true, true);
         break;
     case MULTI_FUNC_COUNT:
@@ -59,7 +59,7 @@ bool multiFunctionSelection(multi_function_e * returnItem)
 
     if (IS_RC_MODE_ACTIVE(BOXMULTIFUNCTION)) {
         if (selectTimer) {
-            if (currentTime - selectTimer > 3000) {
+            if (currentTime - selectTimer > 3000) {     // 3s selection duration to activate selected function
                 *returnItem = selectedItem;
                 multiFunctionApply(selectedItem);
                 selectTimer = 0;
@@ -75,7 +75,7 @@ bool multiFunctionSelection(multi_function_e * returnItem)
         toggle = false;
     } else if (startTimer) {
         selectTimer = 0;
-        if (currentTime - startTimer > 2000) {
+        if (currentTime - startTimer > 2000) {  // 2s reset delay after mode deselected
             startTimer = 0;
             selectedItem = 0;
         }
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 0415858dac8..5d648424276 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -184,7 +184,7 @@ static bool fullRedraw = false;
 static uint8_t armState;
 static uint8_t statsPagesCheck = 0;
 
-textAttributes_t osdGetMultiFunctionMessage(char *buff);
+static textAttributes_t osdGetMultiFunctionMessage(char *buff);
 static osd_warnings_status_flags_e osdWarningsMask = 0;
 
 typedef struct osdMapData_s {
@@ -3275,7 +3275,7 @@ static bool osdDrawSingleElement(uint8_t item)
 #endif // USE_POWER_LIMITS
     case OSD_MULTI_FUNCTION:
         {
-            displayWrite(osdDisplayPort, elemPosX, elemPosY, "          ");
+            displayWrite(osdDisplayPort, elemPosX, elemPosY, "          ");     // Max 10 characters
             elemAttr = osdGetMultiFunctionMessage(buff);
             break;
         }
@@ -4673,41 +4673,42 @@ void resetOsdWarningMask(void)
     osdWarningsMask = 0;
 }
 
-bool checkOsdWarning(bool condition, osd_warnings_status_flags_e warningType)
+static bool checkOsdWarning(bool condition, osd_warnings_status_flags_e warningIndex)
 {
     static timeMs_t newWarningStartTime = 0;
     const timeMs_t currentTimeMs = millis();
 
-    if (condition) {
-        if (!(osdWarningsMask & warningType)) {
+    if (condition) {    // condition required to trigger warning
+        if (!(osdWarningsMask & warningIndex)) {
             newWarningStartTime = currentTimeMs;
-            osdWarningsMask |= warningType;
+            osdWarningsMask |= warningIndex;
         }
         if (currentTimeMs - newWarningStartTime < 10000) {  // Display new warnings for 10s
             return true;
         }
-    } else if (osdWarningsMask & warningType) {
-        osdWarningsMask ^= warningType;
+    } else if (osdWarningsMask & warningIndex) {
+        osdWarningsMask ^= warningIndex;
     }
 
     return false;
 }
 
-textAttributes_t osdGetMultiFunctionMessage(char *buff)
+static textAttributes_t osdGetMultiFunctionMessage(char *buff)
 {
     textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE;
     uint8_t warningCount = BITCOUNT(osdWarningsMask);
 
+/* --- FUNCTIONS --- */
     multi_function_e multiFuncItem;
     multiFunctionSelection(&multiFuncItem);
     if (multiFuncItem) {
         switch (multiFuncItem) {
         case MULTI_FUNC_NONE:
         case MULTI_FUNC_1:
-            strcpy(buff, warningCount ? "WARNINGS  " : "0 WARNINGS");
+            strcpy(buff, warningCount ? "WARNINGS" : "0 WARNINGS");
             break;
         case MULTI_FUNC_2:
-            strcpy(buff, "EMERG ARM ");
+            strcpy(buff, "EMERG ARM");
             break;
         case MULTI_FUNC_COUNT:
             break;
@@ -4716,19 +4717,21 @@ textAttributes_t osdGetMultiFunctionMessage(char *buff)
         return elemAttr;
     }
 
-/* WARNINGS --------------------------------------------- */
+/* --- WARNINGS --- */
     const char *messages[2];
     const char *message = NULL;
     uint8_t messageCount = 0;
-
-    if (checkOsdWarning(!STATE(GPS_FIX), OSD_WARN_1)) {
-        hardwareSensorStatus_e sensorStatus = getHwGPSStatus();
-        bool gpsFailed = sensorStatus == HW_SENSOR_UNAVAILABLE || sensorStatus == HW_SENSOR_UNHEALTHY;
-        messages[messageCount++] = gpsFailed ? "GPS FAILED" : "NO GPS FIX";
+#if defined(USE_GPS)
+    if (feature(FEATURE_GPS)) {
+        if (checkOsdWarning(!STATE(GPS_FIX), OSD_WARN_1)) {
+            hardwareSensorStatus_e sensorStatus = getHwGPSStatus();
+            bool gpsFailed = sensorStatus == HW_SENSOR_UNAVAILABLE || sensorStatus == HW_SENSOR_UNHEALTHY;
+            messages[messageCount++] = gpsFailed ? "GPS FAILED" : "NO GPS FIX";
+        }
     }
-
+#endif
     if (messageCount) {
-        message = messages[OSD_ALTERNATING_CHOICES(2000, messageCount)];    // display each warning for 2s
+        message = messages[OSD_ALTERNATING_CHOICES(2000, messageCount)];    // display each warning on 2s cycle
         strcpy(buff, message);
         TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
         return elemAttr;
@@ -4737,7 +4740,6 @@ textAttributes_t osdGetMultiFunctionMessage(char *buff)
         tfp_sprintf(buff + 1, "%u", warningCount);
         return elemAttr;
     }
-/* WARNINGS --------------------------------------------- */
 
     return elemAttr;
 }

From d27badcfebc31b83613e20e5ad2d11be210faa93 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Mon, 9 Jan 2023 22:47:13 +0000
Subject: [PATCH 03/94] update various

---
 src/main/fc/fc_core.c                         |  2 +-
 src/main/fc/multifunction.c                   |  2 +-
 src/main/io/osd.c                             | 75 ++++++++++++-------
 src/main/io/osd.h                             |  5 +-
 src/main/navigation/navigation.c              |  5 +-
 .../navigation/navigation_pos_estimator.c     |  5 +-
 src/main/navigation/navigation_private.h      |  3 +-
 7 files changed, 62 insertions(+), 35 deletions(-)

diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index 898c624bf04..cc56ed56d8b 100755
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -487,7 +487,7 @@ bool emergencyArmingUpdate(bool armingSwitchIsOn, bool forceArm)
     }
 
     if (forceArm) {
-        counter = EMERGENCY_ARMING_MIN_ARM_COUNT + 1;
+        counter = EMERGENCY_ARMING_MIN_ARM_COUNT;
     }
 
     return counter >= EMERGENCY_ARMING_MIN_ARM_COUNT;
diff --git a/src/main/fc/multifunction.c b/src/main/fc/multifunction.c
index c77807cd827..612a8550b92 100644
--- a/src/main/fc/multifunction.c
+++ b/src/main/fc/multifunction.c
@@ -39,7 +39,7 @@ static void multiFunctionApply(multi_function_e selectedItem)
     case MULTI_FUNC_NONE:
         return;
     case MULTI_FUNC_1:  // redisplay current warnings
-        resetOsdWarningMask();
+        resetOsdWarningFlags();
         break;
     case MULTI_FUNC_2:  // emergency ARM
         emergencyArmingUpdate(true, true);
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 5d648424276..a1c6ccb4bff 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -185,7 +185,7 @@ static uint8_t armState;
 static uint8_t statsPagesCheck = 0;
 
 static textAttributes_t osdGetMultiFunctionMessage(char *buff);
-static osd_warnings_status_flags_e osdWarningsMask = 0;
+static osd_warnings_status_flags_e osdWarningsFlags = 0;
 
 typedef struct osdMapData_s {
     uint32_t scale;
@@ -3373,10 +3373,10 @@ uint8_t osdIncElementIndex(uint8_t elementIndex)
             elementIndex = OSD_AIR_MAX_SPEED;
         }
         if (elementIndex == OSD_GLIDE_RANGE) {
-            elementIndex = feature(FEATURE_CURRENT_METER) ? OSD_CLIMB_EFFICIENCY : OSD_ITEM_COUNT;
+            elementIndex = feature(FEATURE_CURRENT_METER) ? OSD_CLIMB_EFFICIENCY : OSD_MULTI_FUNCTION;
         }
         if (elementIndex == OSD_NAV_WP_MULTI_MISSION_INDEX) {
-            elementIndex = OSD_ITEM_COUNT;
+            elementIndex = OSD_MULTI_FUNCTION;
         }
     }
 
@@ -3652,6 +3652,8 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
     osdLayoutsConfig->item_pos[0][OSD_GVAR_2] = OSD_POS(1, 3);
     osdLayoutsConfig->item_pos[0][OSD_GVAR_3] = OSD_POS(1, 4);
 
+    osdLayoutsConfig->item_pos[0][OSD_MULTI_FUNCTION] = OSD_POS(1, 4);
+
     osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_0] = OSD_POS(2, 7);
     osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_1] = OSD_POS(2, 8);
     osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_2] = OSD_POS(2, 9);
@@ -4640,12 +4642,6 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
             }
         }
 
-#ifdef USE_DEV_TOOLS
-        if (systemConfig()->groundTestMode) {
-            messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_GRD_TEST_MODE);
-        }
-#endif
-
         if (messageCount > 0) {
             message = messages[OSD_ALTERNATING_CHOICES(systemMessageCycleTime(messageCount, messages), messageCount)];
             if (message == failsafeInfoMessage) {
@@ -4668,26 +4664,32 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
     return elemAttr;
 }
 
-void resetOsdWarningMask(void)
+void resetOsdWarningFlags(void)
 {
-    osdWarningsMask = 0;
+    osdWarningsFlags = 0;
 }
 
-static bool checkOsdWarning(bool condition, osd_warnings_status_flags_e warningIndex)
+static bool checkOsdWarning(bool condition, osd_warnings_status_flags_e warningFlag, uint8_t *warningsCount)
 {
     static timeMs_t newWarningStartTime = 0;
     const timeMs_t currentTimeMs = millis();
 
     if (condition) {    // condition required to trigger warning
-        if (!(osdWarningsMask & warningIndex)) {
+        if (!(osdWarningsFlags & warningFlag)) {
             newWarningStartTime = currentTimeMs;
-            osdWarningsMask |= warningIndex;
+            osdWarningsFlags |= warningFlag;
+        }
+#ifdef USE_DEV_TOOLS
+        if (systemConfig()->groundTestMode) {
+            return true;
         }
-        if (currentTimeMs - newWarningStartTime < 10000) {  // Display new warnings for 10s
+#endif
+        if (currentTimeMs - newWarningStartTime < 10000) {      // Display new warnings for 10s
             return true;
         }
-    } else if (osdWarningsMask & warningIndex) {
-        osdWarningsMask ^= warningIndex;
+        *warningsCount += 1;
+    } else if (osdWarningsFlags & warningFlag) {
+        osdWarningsFlags &= ~warningFlag;
     }
 
     return false;
@@ -4695,17 +4697,19 @@ static bool checkOsdWarning(bool condition, osd_warnings_status_flags_e warningI
 
 static textAttributes_t osdGetMultiFunctionMessage(char *buff)
 {
+    /* Message length limit 10 char max */
+
     textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE;
-    uint8_t warningCount = BITCOUNT(osdWarningsMask);
+    static uint8_t osdWarningsCount;
 
-/* --- FUNCTIONS --- */
+    /* --- FUNCTIONS --- */
     multi_function_e multiFuncItem;
     multiFunctionSelection(&multiFuncItem);
     if (multiFuncItem) {
         switch (multiFuncItem) {
         case MULTI_FUNC_NONE:
         case MULTI_FUNC_1:
-            strcpy(buff, warningCount ? "WARNINGS" : "0 WARNINGS");
+            strcpy(buff, osdWarningsCount ? "WARNINGS" : "0 WARNINGS");
             break;
         case MULTI_FUNC_2:
             strcpy(buff, "EMERG ARM");
@@ -4717,27 +4721,44 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
         return elemAttr;
     }
 
-/* --- WARNINGS --- */
-    const char *messages[2];
+    /* --- WARNINGS --- */
+    const char *messages[4];
     const char *message = NULL;
     uint8_t messageCount = 0;
+    bool warningCondition = false;
+    osdWarningsCount = 0;
 #if defined(USE_GPS)
+    // GPS Fix and Failure
     if (feature(FEATURE_GPS)) {
-        if (checkOsdWarning(!STATE(GPS_FIX), OSD_WARN_1)) {
-            hardwareSensorStatus_e sensorStatus = getHwGPSStatus();
-            bool gpsFailed = sensorStatus == HW_SENSOR_UNAVAILABLE || sensorStatus == HW_SENSOR_UNHEALTHY;
+        if (checkOsdWarning(!STATE(GPS_FIX), OSD_WARN_1, &osdWarningsCount)) {
+            bool gpsFailed = getHwGPSStatus() == HW_SENSOR_UNAVAILABLE;
             messages[messageCount++] = gpsFailed ? "GPS FAILED" : "NO GPS FIX";
         }
     }
+    // RTH Sanity (warning if RTH heads 200m further away from home than closest point)
+    warningCondition = NAV_Status.state == MW_NAV_STATE_RTH_ENROUTE && !posControl.flags.rthTrackbackActive &&
+                       (posControl.homeDistance - posControl.rthSanityChecker.minimalDistanceToHome) > 20000;
+    if (checkOsdWarning(warningCondition, OSD_WARN_2, &osdWarningsCount)) {
+        messages[messageCount++] = "RTH SANITY";
+    }
+    // Altitude sanity (warning if significant mismatch between estimated and GPS altitude)
+    if (checkOsdWarning(posControl.flags.gpsCfEstimatedAltitudeMismatch, OSD_WARN_3, &osdWarningsCount)) {
+        messages[messageCount++] = "ALT SANITY";
+    }
+#endif
+#ifdef USE_DEV_TOOLS
+    if (checkOsdWarning(systemConfig()->groundTestMode, OSD_WARN_4, &osdWarningsCount)) {
+        messages[messageCount++] = "GRD TEST";
+    }
 #endif
     if (messageCount) {
         message = messages[OSD_ALTERNATING_CHOICES(2000, messageCount)];    // display each warning on 2s cycle
         strcpy(buff, message);
         TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
         return elemAttr;
-    } else if (warningCount) {
+    } else if (osdWarningsCount) {
         buff[0] = SYM_ALERT;
-        tfp_sprintf(buff + 1, "%u", warningCount);
+        tfp_sprintf(buff + 1, "%u", osdWarningsCount);
         return elemAttr;
     }
 
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index 19071ac1e2f..2c6480f9c0e 100644
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -330,9 +330,10 @@ typedef enum {
 } osd_crsf_lq_format_e;
 
 typedef enum {
-    OSD_WARN_NONE   = 0,
     OSD_WARN_1      = 1 << 0,
     OSD_WARN_2      = 1 << 1,
+    OSD_WARN_3      = 1 << 2,
+    OSD_WARN_4      = 1 << 3,
 } osd_warnings_status_flags_e;
 
 typedef struct osdLayoutsConfig_s {
@@ -480,7 +481,7 @@ int32_t osdGetAltitude(void);
 void osdStartedSaveProcess(void);
 void osdShowEEPROMSavedNotification(void);
 
-void resetOsdWarningMask(void);
+void resetOsdWarningFlags(void);
 
 void osdCrosshairPosition(uint8_t *x, uint8_t *y);
 bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length);
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index a282458fa0d..42d7cd725b6 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -2067,7 +2067,7 @@ void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelVali
 /*-----------------------------------------------------------
  * Processes an update to Z-position and velocity
  *-----------------------------------------------------------*/
-void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus)
+void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus, float gpsCfEstimatedAltitudeError)
 {
     posControl.actualState.abs.pos.z = newAltitude;
     posControl.actualState.abs.vel.z = newVelocity;
@@ -2090,11 +2090,14 @@ void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, flo
         posControl.flags.estAltStatus = EST_TRUSTED;
         posControl.flags.verticalPositionDataNew = true;
         posControl.lastValidAltitudeTimeMs = millis();
+        /* flag set if mismatch between relative GPS and estimated altitude exceeds 20m */
+        posControl.flags.gpsCfEstimatedAltitudeMismatch = fabsf(gpsCfEstimatedAltitudeError) > 2000;
     }
     else {
         posControl.flags.estAltStatus = EST_NONE;
         posControl.flags.estAglStatus = EST_NONE;
         posControl.flags.verticalPositionDataNew = false;
+        posControl.flags.gpsCfEstimatedAltitudeMismatch = false;
     }
 
     if (ARMING_FLAG(ARMED)) {
diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
index b27e72d359d..0a1cf490116 100755
--- a/src/main/navigation/navigation_pos_estimator.c
+++ b/src/main/navigation/navigation_pos_estimator.c
@@ -810,11 +810,12 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)
 
         /* Publish altitude update and set altitude validity */
         if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) {
+            const float gpsCfEstimatedAltitudeError = STATE(GPS_FIX) ? posEstimator.gps.pos.z - posEstimator.est.pos.z : 0;
             navigationEstimateStatus_e aglStatus = (posEstimator.est.aglQual == SURFACE_QUAL_LOW) ? EST_USABLE : EST_TRUSTED;
-            updateActualAltitudeAndClimbRate(true, posEstimator.est.pos.z, posEstimator.est.vel.z, posEstimator.est.aglAlt, posEstimator.est.aglVel, aglStatus);
+            updateActualAltitudeAndClimbRate(true, posEstimator.est.pos.z, posEstimator.est.vel.z, posEstimator.est.aglAlt, posEstimator.est.aglVel, aglStatus, gpsCfEstimatedAltitudeError);
         }
         else {
-            updateActualAltitudeAndClimbRate(false, posEstimator.est.pos.z, 0, posEstimator.est.aglAlt, 0, EST_NONE);
+            updateActualAltitudeAndClimbRate(false, posEstimator.est.pos.z, 0, posEstimator.est.aglAlt, 0, EST_NONE, 0);
         }
 
         //Update Blackbox states
diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
index dd99ce12b57..ab3829f8246 100644
--- a/src/main/navigation/navigation_private.h
+++ b/src/main/navigation/navigation_private.h
@@ -90,6 +90,7 @@ typedef struct navigationFlags_s {
     navigationEstimateStatus_e estVelStatus;        // Indicates that GPS is working (or not)
     navigationEstimateStatus_e estAglStatus;
     navigationEstimateStatus_e estHeadingStatus;    // Indicate valid heading - wither mag or GPS at certain speed on airplane
+    bool gpsCfEstimatedAltitudeMismatch;            // Indicates a mismatch between GPS altitude and estimated altitude
 
     bool isAdjustingPosition;
     bool isAdjustingAltitude;
@@ -458,7 +459,7 @@ bool isWaypointNavTrackingActive(void);
 
 void updateActualHeading(bool headingValid, int32_t newHeading, int32_t newGroundCourse);
 void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY);
-void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus);
+void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus, float gpsCfEstimatedAltitudeError);
 
 bool checkForPositionSensorTimeout(void);
 

From db4d9d24469429254d1b4421223cb35151a9f4c4 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Wed, 11 Jan 2023 20:42:48 +0000
Subject: [PATCH 04/94] change function control

---
 src/main/fc/multifunction.c | 28 ++++++++++----------
 src/main/fc/multifunction.h |  4 +--
 src/main/io/osd.c           | 51 +++++++++++++++++--------------------
 src/main/io/osd.h           | 11 ++------
 4 files changed, 43 insertions(+), 51 deletions(-)

diff --git a/src/main/fc/multifunction.c b/src/main/fc/multifunction.c
index 612a8550b92..4f682225bd8 100644
--- a/src/main/fc/multifunction.c
+++ b/src/main/fc/multifunction.c
@@ -44,44 +44,46 @@ static void multiFunctionApply(multi_function_e selectedItem)
     case MULTI_FUNC_2:  // emergency ARM
         emergencyArmingUpdate(true, true);
         break;
-    case MULTI_FUNC_COUNT:
+    case MULTI_FUNC_END:
         break;
     }
 }
 
-bool multiFunctionSelection(multi_function_e * returnItem)
+multi_function_e multiFunctionSelection(void)
 {
     static timeMs_t startTimer;
     static timeMs_t selectTimer;
-    static int8_t selectedItem = 0;
+    static multi_function_e selectedItem = MULTI_FUNC_NONE;
     static bool toggle = true;
     const timeMs_t currentTime = millis();
 
     if (IS_RC_MODE_ACTIVE(BOXMULTIFUNCTION)) {
         if (selectTimer) {
             if (currentTime - selectTimer > 3000) {     // 3s selection duration to activate selected function
-                *returnItem = selectedItem;
                 multiFunctionApply(selectedItem);
                 selectTimer = 0;
-                selectedItem = 0;
-                return true;
+                selectedItem = MULTI_FUNC_NONE;
             }
         } else if (toggle) {
-            selectedItem++;
-            selectedItem = selectedItem == MULTI_FUNC_COUNT ? 1 : selectedItem;
-            selectTimer = currentTime;
+            if (selectedItem == MULTI_FUNC_NONE) {
+                selectedItem++;
+            } else {
+                selectTimer = currentTime;
+            }
         }
         startTimer = currentTime;
         toggle = false;
     } else if (startTimer) {
+        if (!toggle && selectTimer) {
+            selectedItem = selectedItem == MULTI_FUNC_END - 1 ? MULTI_FUNC_1 : selectedItem + 1;
+        }
         selectTimer = 0;
-        if (currentTime - startTimer > 2000) {  // 2s reset delay after mode deselected
+        if (currentTime - startTimer > 2000) {      // 2s reset delay after mode deselected
             startTimer = 0;
-            selectedItem = 0;
+            selectedItem = MULTI_FUNC_NONE;
         }
         toggle = true;
     }
 
-    *returnItem = selectedItem;
-    return false;
+    return selectedItem;
 }
diff --git a/src/main/fc/multifunction.h b/src/main/fc/multifunction.h
index b302dcf9869..9ba760dc5cf 100644
--- a/src/main/fc/multifunction.h
+++ b/src/main/fc/multifunction.h
@@ -28,7 +28,7 @@ typedef enum {
     MULTI_FUNC_NONE,
     MULTI_FUNC_1,
     MULTI_FUNC_2,
-    MULTI_FUNC_COUNT,
+    MULTI_FUNC_END,
 } multi_function_e;
 
-bool multiFunctionSelection(multi_function_e * returnItem);
+multi_function_e multiFunctionSelection(void);
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index a1c6ccb4bff..70bae4a9224 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -185,7 +185,7 @@ static uint8_t armState;
 static uint8_t statsPagesCheck = 0;
 
 static textAttributes_t osdGetMultiFunctionMessage(char *buff);
-static osd_warnings_status_flags_e osdWarningsFlags = 0;
+static uint8_t osdWarningsFlags = 0;
 
 typedef struct osdMapData_s {
     uint32_t scale;
@@ -4669,7 +4669,7 @@ void resetOsdWarningFlags(void)
     osdWarningsFlags = 0;
 }
 
-static bool checkOsdWarning(bool condition, osd_warnings_status_flags_e warningFlag, uint8_t *warningsCount)
+static bool checkOsdWarning(bool condition, uint8_t warningFlag, uint8_t *warningsCount)
 {
     static timeMs_t newWarningStartTime = 0;
     const timeMs_t currentTimeMs = millis();
@@ -4684,7 +4684,7 @@ static bool checkOsdWarning(bool condition, osd_warnings_status_flags_e warningF
             return true;
         }
 #endif
-        if (currentTimeMs - newWarningStartTime < 10000) {      // Display new warnings for 10s
+        if (currentTimeMs - newWarningStartTime < 10000) {  // Display new warnings for 10s
             return true;
         }
         *warningsCount += 1;
@@ -4700,25 +4700,20 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
     /* Message length limit 10 char max */
 
     textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE;
-    static uint8_t osdWarningsCount;
+    static uint8_t warningsCount;
 
     /* --- FUNCTIONS --- */
-    multi_function_e multiFuncItem;
-    multiFunctionSelection(&multiFuncItem);
-    if (multiFuncItem) {
-        switch (multiFuncItem) {
-        case MULTI_FUNC_NONE:
-        case MULTI_FUNC_1:
-            strcpy(buff, osdWarningsCount ? "WARNINGS" : "0 WARNINGS");
-            break;
-        case MULTI_FUNC_2:
-            strcpy(buff, "EMERG ARM");
-            break;
-        case MULTI_FUNC_COUNT:
-            break;
-        }
-
+    switch (multiFunctionSelection()) {
+    case MULTI_FUNC_NONE:
+        break;
+    case MULTI_FUNC_1:
+        strcpy(buff, warningsCount ? "WARNINGS" : "0 WARNINGS");
+        return elemAttr;
+    case MULTI_FUNC_2:
+        strcpy(buff, "EMERG ARM");
         return elemAttr;
+    case MULTI_FUNC_END:
+        break;
     }
 
     /* --- WARNINGS --- */
@@ -4726,28 +4721,30 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
     const char *message = NULL;
     uint8_t messageCount = 0;
     bool warningCondition = false;
-    osdWarningsCount = 0;
+    warningsCount = 0;
+    uint8_t warningFlagID = 1;
+
 #if defined(USE_GPS)
     // GPS Fix and Failure
     if (feature(FEATURE_GPS)) {
-        if (checkOsdWarning(!STATE(GPS_FIX), OSD_WARN_1, &osdWarningsCount)) {
+        if (checkOsdWarning(!STATE(GPS_FIX), warningFlagID, &warningsCount)) {
             bool gpsFailed = getHwGPSStatus() == HW_SENSOR_UNAVAILABLE;
             messages[messageCount++] = gpsFailed ? "GPS FAILED" : "NO GPS FIX";
         }
     }
-    // RTH Sanity (warning if RTH heads 200m further away from home than closest point)
+    // RTH sanity (warning if RTH heads 200m further away from home than closest point)
     warningCondition = NAV_Status.state == MW_NAV_STATE_RTH_ENROUTE && !posControl.flags.rthTrackbackActive &&
                        (posControl.homeDistance - posControl.rthSanityChecker.minimalDistanceToHome) > 20000;
-    if (checkOsdWarning(warningCondition, OSD_WARN_2, &osdWarningsCount)) {
+    if (checkOsdWarning(warningCondition, warningFlagID << 1, &warningsCount)) {
         messages[messageCount++] = "RTH SANITY";
     }
     // Altitude sanity (warning if significant mismatch between estimated and GPS altitude)
-    if (checkOsdWarning(posControl.flags.gpsCfEstimatedAltitudeMismatch, OSD_WARN_3, &osdWarningsCount)) {
+    if (checkOsdWarning(posControl.flags.gpsCfEstimatedAltitudeMismatch, warningFlagID << 1, &warningsCount)) {
         messages[messageCount++] = "ALT SANITY";
     }
 #endif
 #ifdef USE_DEV_TOOLS
-    if (checkOsdWarning(systemConfig()->groundTestMode, OSD_WARN_4, &osdWarningsCount)) {
+    if (checkOsdWarning(systemConfig()->groundTestMode, warningFlagID << 1, &warningsCount)) {
         messages[messageCount++] = "GRD TEST";
     }
 #endif
@@ -4756,9 +4753,9 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
         strcpy(buff, message);
         TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
         return elemAttr;
-    } else if (osdWarningsCount) {
+    } else if (warningsCount) {
         buff[0] = SYM_ALERT;
-        tfp_sprintf(buff + 1, "%u", osdWarningsCount);
+        tfp_sprintf(buff + 1, "%u ", warningsCount);
         return elemAttr;
     }
 
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index 2c6480f9c0e..05a593ab94e 100644
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -329,13 +329,6 @@ typedef enum {
     OSD_CRSF_LQ_TYPE3
 } osd_crsf_lq_format_e;
 
-typedef enum {
-    OSD_WARN_1      = 1 << 0,
-    OSD_WARN_2      = 1 << 1,
-    OSD_WARN_3      = 1 << 2,
-    OSD_WARN_4      = 1 << 3,
-} osd_warnings_status_flags_e;
-
 typedef struct osdLayoutsConfig_s {
     // Layouts
     uint16_t item_pos[OSD_LAYOUT_COUNT][OSD_ITEM_COUNT];
@@ -481,8 +474,6 @@ int32_t osdGetAltitude(void);
 void osdStartedSaveProcess(void);
 void osdShowEEPROMSavedNotification(void);
 
-void resetOsdWarningFlags(void);
-
 void osdCrosshairPosition(uint8_t *x, uint8_t *y);
 bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length);
 void osdFormatAltitudeSymbol(char *buff, int32_t alt);
@@ -490,6 +481,8 @@ void osdFormatVelocityStr(char* buff, int32_t vel, bool _3D, bool _max);
 // Returns a heading angle in degrees normalized to [0, 360).
 int osdGetHeadingAngle(int angle);
 
+void resetOsdWarningFlags(void);
+
 /**
  * @brief Get the OSD system message
  * @param buff pointer to the message buffer

From bb5dd6c533075dc6b79731fa22a56cc8f9b8a8b6 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Thu, 12 Jan 2023 17:13:50 +0000
Subject: [PATCH 05/94] Update osd.c

---
 src/main/io/osd.c | 44 ++++++++++++++++++++++++++++----------------
 1 file changed, 28 insertions(+), 16 deletions(-)

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 70bae4a9224..946936b1b19 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -3275,8 +3275,17 @@ static bool osdDrawSingleElement(uint8_t item)
 #endif // USE_POWER_LIMITS
     case OSD_MULTI_FUNCTION:
         {
-            displayWrite(osdDisplayPort, elemPosX, elemPosY, "          ");     // Max 10 characters
+            // message shown infrequently so only write when needed
+            static bool clearMultiFunction = true;
             elemAttr = osdGetMultiFunctionMessage(buff);
+            if (buff[0] == 0) {
+                if (clearMultiFunction) {
+                    displayWrite(osdDisplayPort, elemPosX, elemPosY, "          ");
+                    clearMultiFunction = false;
+                }
+                return true;
+            }
+            clearMultiFunction = true;
             break;
         }
 
@@ -4701,24 +4710,29 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
 
     textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE;
     static uint8_t warningsCount;
+    const char *message = NULL;
 
     /* --- FUNCTIONS --- */
-    switch (multiFunctionSelection()) {
-    case MULTI_FUNC_NONE:
-        break;
-    case MULTI_FUNC_1:
-        strcpy(buff, warningsCount ? "WARNINGS" : "0 WARNINGS");
-        return elemAttr;
-    case MULTI_FUNC_2:
-        strcpy(buff, "EMERG ARM");
+    multi_function_e selectedFunction = multiFunctionSelection();
+    if (selectedFunction) {
+        switch (selectedFunction) {
+        case MULTI_FUNC_NONE:
+        case MULTI_FUNC_1:
+            message = warningsCount ? "WARNINGS !" : "0 WARNINGS";
+            break;
+        case MULTI_FUNC_2:
+            message = "EMERG ARM ";
+            break;
+        case MULTI_FUNC_END:
+            break;
+        }
+
+        strcpy(buff, message);
         return elemAttr;
-    case MULTI_FUNC_END:
-        break;
     }
 
     /* --- WARNINGS --- */
     const char *messages[4];
-    const char *message = NULL;
     uint8_t messageCount = 0;
     bool warningCondition = false;
     warningsCount = 0;
@@ -4745,18 +4759,16 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
 #endif
 #ifdef USE_DEV_TOOLS
     if (checkOsdWarning(systemConfig()->groundTestMode, warningFlagID << 1, &warningsCount)) {
-        messages[messageCount++] = "GRD TEST";
+        messages[messageCount++] = "GRD TEST !";
     }
 #endif
     if (messageCount) {
         message = messages[OSD_ALTERNATING_CHOICES(2000, messageCount)];    // display each warning on 2s cycle
         strcpy(buff, message);
         TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
-        return elemAttr;
     } else if (warningsCount) {
         buff[0] = SYM_ALERT;
-        tfp_sprintf(buff + 1, "%u ", warningsCount);
-        return elemAttr;
+        tfp_sprintf(buff + 1, "%u        ", warningsCount);
     }
 
     return elemAttr;

From b9688e5142e080107fa0314fa5b50204a37147c6 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Sun, 29 Jan 2023 00:11:44 +0000
Subject: [PATCH 06/94] add emergency landing

---
 src/main/fc/multifunction.c      | 6 +++++-
 src/main/fc/multifunction.h      | 1 +
 src/main/io/osd.c                | 3 +++
 src/main/navigation/navigation.c | 6 +++---
 src/main/navigation/navigation.h | 2 ++
 5 files changed, 14 insertions(+), 4 deletions(-)

diff --git a/src/main/fc/multifunction.c b/src/main/fc/multifunction.c
index 4f682225bd8..26d6a9b9df2 100644
--- a/src/main/fc/multifunction.c
+++ b/src/main/fc/multifunction.c
@@ -29,6 +29,7 @@
 #include "fc/fc_core.h"
 #include "fc/multifunction.h"
 #include "fc/rc_modes.h"
+#include "fc/runtime_config.h"
 
 #include "io/osd.h"
 #include "navigation/navigation.h"
@@ -41,7 +42,10 @@ static void multiFunctionApply(multi_function_e selectedItem)
     case MULTI_FUNC_1:  // redisplay current warnings
         resetOsdWarningFlags();
         break;
-    case MULTI_FUNC_2:  // emergency ARM
+    case MULTI_FUNC_2:  // control manual emergency landing
+        checkManualEmergencyLandingControl(ARMING_FLAG(ARMED));
+        break;
+    case MULTI_FUNC_3:  // emergency ARM
         emergencyArmingUpdate(true, true);
         break;
     case MULTI_FUNC_END:
diff --git a/src/main/fc/multifunction.h b/src/main/fc/multifunction.h
index 9ba760dc5cf..900d4aad07b 100644
--- a/src/main/fc/multifunction.h
+++ b/src/main/fc/multifunction.h
@@ -28,6 +28,7 @@ typedef enum {
     MULTI_FUNC_NONE,
     MULTI_FUNC_1,
     MULTI_FUNC_2,
+    MULTI_FUNC_3,
     MULTI_FUNC_END,
 } multi_function_e;
 
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 769bd6fd313..b1b87eabff5 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -4740,6 +4740,9 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
             message = warningsCount ? "WARNINGS !" : "0 WARNINGS";
             break;
         case MULTI_FUNC_2:
+            message = posControl.flags.manualEmergLandActive ? "ABORT LAND" : "EMERG LAND";
+            break;
+        case MULTI_FUNC_3:
             message = "EMERG ARM ";
             break;
         case MULTI_FUNC_END:
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index df97ae5e079..0f2a4d0424d 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -3615,7 +3615,7 @@ static bool isWaypointMissionValid(void)
     return posControl.waypointListValid && (posControl.waypointCount > 0);
 }
 
-static void checkManualEmergencyLandingControl(void)
+void checkManualEmergencyLandingControl(bool forcedActivation)
 {
     static timeMs_t timeout = 0;
     static int8_t counter = 0;
@@ -3640,7 +3640,7 @@ static void checkManualEmergencyLandingControl(void)
     }
 
     // Emergency landing toggled ON or OFF after 5 cycles of Poshold mode @ 1Hz minimum rate
-    if (counter >= 5) {
+    if (counter >= 5 || forcedActivation) {
         counter = 0;
         posControl.flags.manualEmergLandActive = !posControl.flags.manualEmergLandActive;
 
@@ -3677,7 +3677,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
 
         /* Emergency landing controlled manually by rapid switching of Poshold mode.
          * Landing toggled ON or OFF for each Poshold activation sequence */
-        checkManualEmergencyLandingControl();
+        checkManualEmergencyLandingControl(false);
 
         /* Emergency landing triggered by failsafe Landing or manually initiated */
         if (posControl.flags.forcedEmergLandingActivated || posControl.flags.manualEmergLandActive) {
diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
index 84255421bef..4125c7eee08 100644
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@@ -619,6 +619,8 @@ bool isAdjustingHeading(void);
 float getEstimatedAglPosition(void);
 bool isEstimatedAglTrusted(void);
 
+void checkManualEmergencyLandingControl(bool forcedActivation);
+
 /* Returns the heading recorded when home position was acquired.
  * Note that the navigation system uses deg*100 as unit and angles
  * are in the [0, 360 * 100) interval.

From 2a18b889540fb30639b6a2fa2cf87ac1f47cb17b Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Wed, 8 Feb 2023 15:52:11 +0000
Subject: [PATCH 07/94] improve display timer + fixes

---
 src/main/fc/multifunction.c |  2 +-
 src/main/io/osd.c           | 52 ++++++++++++++++++++++++++++---------
 src/main/io/osd.h           |  2 +-
 3 files changed, 42 insertions(+), 14 deletions(-)

diff --git a/src/main/fc/multifunction.c b/src/main/fc/multifunction.c
index 26d6a9b9df2..1841b2b6c06 100644
--- a/src/main/fc/multifunction.c
+++ b/src/main/fc/multifunction.c
@@ -40,7 +40,7 @@ static void multiFunctionApply(multi_function_e selectedItem)
     case MULTI_FUNC_NONE:
         return;
     case MULTI_FUNC_1:  // redisplay current warnings
-        resetOsdWarningFlags();
+        osdResetWarningFlags();
         break;
     case MULTI_FUNC_2:  // control manual emergency landing
         checkManualEmergencyLandingControl(ARMING_FLAG(ARMED));
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index b1b87eabff5..a3a4f989c70 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -4692,28 +4692,45 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
     return elemAttr;
 }
 
-void resetOsdWarningFlags(void)
+void osdResetWarningFlags(void)
 {
     osdWarningsFlags = 0;
 }
 
-static bool checkOsdWarning(bool condition, uint8_t warningFlag, uint8_t *warningsCount)
+static bool osdCheckWarning(bool condition, uint8_t warningFlag, uint8_t *warningsCount)
 {
-    static timeMs_t newWarningStartTime = 0;
+#define WARNING_REDISPLAY_DURATION 5000;    // milliseconds
+
     const timeMs_t currentTimeMs = millis();
+    static timeMs_t warningDisplayStartTime = 0;
+    static timeMs_t redisplayStartTimeMs = 0;
+    static uint16_t osdWarningTimerDuration;
+    static uint8_t newWarningFlags;
 
     if (condition) {    // condition required to trigger warning
         if (!(osdWarningsFlags & warningFlag)) {
-            newWarningStartTime = currentTimeMs;
             osdWarningsFlags |= warningFlag;
+            newWarningFlags |= warningFlag;
+            redisplayStartTimeMs = 0;
         }
 #ifdef USE_DEV_TOOLS
         if (systemConfig()->groundTestMode) {
             return true;
         }
 #endif
-        if (currentTimeMs - newWarningStartTime < 10000) {  // Display new warnings for 10s
-            return true;
+        /* Warnings displayed in full for set time before shrinking down to alert symbol with warning count only.
+         * All current warnings then redisplayed for 5s on 30s rolling cycle.
+         * New warnings dislayed individually for 10s */
+        if (currentTimeMs > redisplayStartTimeMs) {
+            warningDisplayStartTime = currentTimeMs;
+            osdWarningTimerDuration = newWarningFlags ? 10000 : WARNING_REDISPLAY_DURATION;
+            redisplayStartTimeMs = currentTimeMs + osdWarningTimerDuration + 30000;
+        }
+
+        if (currentTimeMs - warningDisplayStartTime < osdWarningTimerDuration) {
+            return (newWarningFlags & warningFlag) || osdWarningTimerDuration == WARNING_REDISPLAY_DURATION;
+        } else {
+            newWarningFlags = 0;
         }
         *warningsCount += 1;
     } else if (osdWarningsFlags & warningFlag) {
@@ -4754,38 +4771,49 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
     }
 
     /* --- WARNINGS --- */
-    const char *messages[4];
+    const char *messages[5];
     uint8_t messageCount = 0;
     bool warningCondition = false;
     warningsCount = 0;
     uint8_t warningFlagID = 1;
 
+    // Low Battery
+    const batteryState_e batteryState = getBatteryState();
+    warningCondition = batteryState == BATTERY_CRITICAL || batteryState == BATTERY_WARNING;
+    if (osdCheckWarning(warningCondition, warningFlagID, &warningsCount)) {
+        messages[messageCount++] = batteryState == BATTERY_CRITICAL ? "BATT EMPTY" : "BATT LOW !";
+    }
+
 #if defined(USE_GPS)
     // GPS Fix and Failure
     if (feature(FEATURE_GPS)) {
-        if (checkOsdWarning(!STATE(GPS_FIX), warningFlagID, &warningsCount)) {
+        if (osdCheckWarning(!STATE(GPS_FIX), warningFlagID <<= 1, &warningsCount)) {
             bool gpsFailed = getHwGPSStatus() == HW_SENSOR_UNAVAILABLE;
             messages[messageCount++] = gpsFailed ? "GPS FAILED" : "NO GPS FIX";
         }
     }
+
     // RTH sanity (warning if RTH heads 200m further away from home than closest point)
     warningCondition = NAV_Status.state == MW_NAV_STATE_RTH_ENROUTE && !posControl.flags.rthTrackbackActive &&
                        (posControl.homeDistance - posControl.rthSanityChecker.minimalDistanceToHome) > 20000;
-    if (checkOsdWarning(warningCondition, warningFlagID << 1, &warningsCount)) {
+    if (osdCheckWarning(warningCondition, warningFlagID <<= 1, &warningsCount)) {
         messages[messageCount++] = "RTH SANITY";
     }
+
     // Altitude sanity (warning if significant mismatch between estimated and GPS altitude)
-    if (checkOsdWarning(posControl.flags.gpsCfEstimatedAltitudeMismatch, warningFlagID << 1, &warningsCount)) {
+    if (osdCheckWarning(posControl.flags.gpsCfEstimatedAltitudeMismatch, warningFlagID <<= 1, &warningsCount)) {
         messages[messageCount++] = "ALT SANITY";
     }
 #endif
+
 #ifdef USE_DEV_TOOLS
-    if (checkOsdWarning(systemConfig()->groundTestMode, warningFlagID << 1, &warningsCount)) {
+    if (osdCheckWarning(systemConfig()->groundTestMode, warningFlagID <<= 1, &warningsCount)) {
         messages[messageCount++] = "GRD TEST !";
     }
 #endif
+
     if (messageCount) {
-        message = messages[OSD_ALTERNATING_CHOICES(2000, messageCount)];    // display each warning on 2s cycle
+        message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)];    // display each warning on 1s cycle
         strcpy(buff, message);
         TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
     } else if (warningsCount) {
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index 2a05a4c02fd..7ba1b1fec77 100644
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -483,7 +483,7 @@ void osdFormatVelocityStr(char* buff, int32_t vel, bool _3D, bool _max);
 // Returns a heading angle in degrees normalized to [0, 360).
 int osdGetHeadingAngle(int angle);
 
-void resetOsdWarningFlags(void);
+void osdResetWarningFlags(void);
 
 /**
  * @brief Get the OSD system message

From a5e2550193fa4db9a266e2ed3bc015a6329f3a5f Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Fri, 10 Feb 2023 22:49:48 +0000
Subject: [PATCH 08/94] add Safehome suspend function

---
 src/main/fc/multifunction.c              | 13 ++++++-
 src/main/fc/multifunction.h              |  1 +
 src/main/flight/failsafe.c               |  6 +--
 src/main/io/osd.c                        | 22 +++++++----
 src/main/navigation/navigation.c         | 48 +++++++++++-------------
 src/main/navigation/navigation.h         | 11 ++----
 src/main/navigation/navigation_private.h | 16 ++++++--
 7 files changed, 67 insertions(+), 50 deletions(-)

diff --git a/src/main/fc/multifunction.c b/src/main/fc/multifunction.c
index 1841b2b6c06..83ece659e79 100644
--- a/src/main/fc/multifunction.c
+++ b/src/main/fc/multifunction.c
@@ -45,8 +45,17 @@ static void multiFunctionApply(multi_function_e selectedItem)
     case MULTI_FUNC_2:  // control manual emergency landing
         checkManualEmergencyLandingControl(ARMING_FLAG(ARMED));
         break;
-    case MULTI_FUNC_3:  // emergency ARM
-        emergencyArmingUpdate(true, true);
+    case MULTI_FUNC_3:  // toggle Safehome suspend
+#if defined(USE_SAFE_HOME)
+        if (navConfig()->general.flags.safehome_usage_mode != SAFEHOME_USAGE_OFF) {
+            suspendSafehome();
+        }
+#endif
+        break;
+    case MULTI_FUNC_4:  // emergency ARM
+        if (!ARMING_FLAG(ARMED)) {
+            emergencyArmingUpdate(true, true);
+        }
         break;
     case MULTI_FUNC_END:
         break;
diff --git a/src/main/fc/multifunction.h b/src/main/fc/multifunction.h
index 900d4aad07b..30038b8617e 100644
--- a/src/main/fc/multifunction.h
+++ b/src/main/fc/multifunction.h
@@ -29,6 +29,7 @@ typedef enum {
     MULTI_FUNC_1,
     MULTI_FUNC_2,
     MULTI_FUNC_3,
+    MULTI_FUNC_4,
     MULTI_FUNC_END,
 } multi_function_e;
 
diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c
index ab48fba2c6e..2e179f67bb1 100644
--- a/src/main/flight/failsafe.c
+++ b/src/main/flight/failsafe.c
@@ -353,11 +353,9 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void)
 
     // Craft is closer than minimum failsafe procedure distance (if set to non-zero)
     // GPS must also be working, and home position set
-    if (failsafeConfig()->failsafe_min_distance > 0 &&
-            sensors(SENSOR_GPS) && STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
-
+    if (failsafeConfig()->failsafe_min_distance > 0 && sensors(SENSOR_GPS) && STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) {
         // get the distance to the original arming point
-        uint32_t distance = calculateDistanceToDestination(&original_rth_home);
+        uint32_t distance = calculateDistanceToDestination(&posControl.rthState.originalHomePosition);
         if (distance < failsafeConfig()->failsafe_min_distance) {
             // Use the alternate, minimum distance failsafe procedure instead
             return failsafeConfig()->failsafe_min_distance_procedure;
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index a3a4f989c70..a9f5a5d6f1b 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -959,7 +959,7 @@ static const char * osdFailsafeInfoMessage(void)
 #if defined(USE_SAFE_HOME)
 static const char * divertingToSafehomeMessage(void)
 {
-	if (NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && safehome_applied) {
+	if (NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && posControl.safehomeState.isApplied) {
 	    return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME);
 	}
 	return NULL;
@@ -1005,7 +1005,7 @@ static const char * navigationStateMessage(void)
         case MW_NAV_STATE_HOVER_ABOVE_HOME:
             if (STATE(FIXED_WING_LEGACY)) {
 #if defined(USE_SAFE_HOME)
-                if (safehome_applied) {
+                if (posControl.safehomeState.isApplied) {
                     return OSD_MESSAGE_STR(OSD_MSG_LOITERING_SAFEHOME);
                 }
 #endif
@@ -4224,13 +4224,13 @@ static void osdShowArmed(void)
             }
             y += 4;
 #if defined (USE_SAFE_HOME)
-            if (safehome_distance) { // safehome found during arming
+            if (posControl.safehomeState.distance) { // safehome found during arming
                 if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) {
                     strcpy(buf, "SAFEHOME FOUND; MODE OFF");
 				} else {
 					char buf2[12]; // format the distance first
-					osdFormatDistanceStr(buf2, safehome_distance);
-					tfp_sprintf(buf, "%c - %s -> SAFEHOME %u", SYM_HOME, buf2, safehome_index);
+					osdFormatDistanceStr(buf2, posControl.safehomeState.distance);
+					tfp_sprintf(buf, "%c - %s -> SAFEHOME %u", SYM_HOME, buf2, posControl.safehomeState.index);
 				}
 				textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT;
 				// write this message above the ARMED message to make it obvious
@@ -4305,7 +4305,7 @@ static void osdRefresh(timeUs_t currentTimeUs)
             uint32_t delay = ARMED_SCREEN_DISPLAY_TIME;
             statsPagesCheck = 0;
 #if defined(USE_SAFE_HOME)
-            if (safehome_distance)
+            if (posControl.safehomeState.distance)
                 delay *= 3;
 #endif
             osdSetNextRefreshIn(delay);
@@ -4760,7 +4760,15 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
             message = posControl.flags.manualEmergLandActive ? "ABORT LAND" : "EMERG LAND";
             break;
         case MULTI_FUNC_3:
-            message = "EMERG ARM ";
+            message = "NO SFHOME ";
+#if defined(USE_SAFE_HOME)
+            if (navConfig()->general.flags.safehome_usage_mode != SAFEHOME_USAGE_OFF) {
+                message = posControl.safehomeState.isSuspended ? "USE SFHOME" : "CAN SFHOME";
+            }
+#endif
+            break;
+        case MULTI_FUNC_4:
+            message = ARMING_FLAG(ARMED) ? "NOW ARMED " : "EMERG ARM ";
             break;
         case MULTI_FUNC_END:
             break;
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 0f2a4d0424d..b0bc1f54a0a 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -79,17 +79,10 @@ gpsLocation_t GPS_home;
 uint32_t      GPS_distanceToHome;        // distance to home point in meters
 int16_t       GPS_directionToHome;       // direction to home point in degrees
 
-fpVector3_t   original_rth_home;         // the original rth home - save it, since it could be replaced by safehome or HOME_RESET
-
 radar_pois_t radar_pois[RADAR_MAX_POIS];
-#if defined(USE_SAFE_HOME)
-int8_t safehome_index = -1;               // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise
-uint32_t safehome_distance = 0;           // distance to the nearest safehome
-fpVector3_t nearestSafeHome;              // The nearestSafeHome found during arming
-bool safehome_applied = false;            // whether the safehome has been applied to home.
 
+#if defined(USE_SAFE_HOME)
 PG_REGISTER_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig, PG_SAFE_HOME_CONFIG , 0);
-
 #endif
 
 // waypoint 254, 255 are special waypoints
@@ -2463,12 +2456,18 @@ static navigationHomeFlags_t navigationActualStateHomeValidity(void)
 }
 
 #if defined(USE_SAFE_HOME)
+void suspendSafehome(void)
+{
+    // toggle Safehome suspend each call
+    posControl.safehomeState.isSuspended = !posControl.safehomeState.isSuspended;
+}
 
 void checkSafeHomeState(bool shouldBeEnabled)
 {
     const bool safehomeNotApplicable = navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF ||
+                                       posControl.safehomeState.isSuspended ||
                                        posControl.flags.rthTrackbackActive ||
-                                       (!safehome_applied && posControl.homeDistance < navConfig()->general.min_rth_distance);
+                                       (!posControl.safehomeState.isApplied && posControl.homeDistance < navConfig()->general.min_rth_distance);
 
 	if (safehomeNotApplicable) {
 		shouldBeEnabled = false;
@@ -2478,17 +2477,17 @@ void checkSafeHomeState(bool shouldBeEnabled)
 		shouldBeEnabled = posControl.flags.forcedRTHActivated;
 	}
     // no safe homes found when arming or safehome feature in the correct state, then we don't need to do anything
-	if (safehome_distance == 0 || (safehome_applied == shouldBeEnabled)) {
+	if (posControl.safehomeState.distance == 0 || posControl.safehomeState.isApplied == shouldBeEnabled) {
 		return;
 	}
     if (shouldBeEnabled) {
 		// set home to safehome
-        setHomePosition(&nearestSafeHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
-		safehome_applied = true;
+        setHomePosition(&posControl.safehomeState.nearestSafeHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
+		posControl.safehomeState.isApplied = true;
 	} else {
 		// set home to original arming point
-        setHomePosition(&original_rth_home, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
-		safehome_applied = false;
+        setHomePosition(&posControl.rthState.originalHomePosition, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
+		posControl.safehomeState.isApplied = false;
 	}
 	// if we've changed the home position, update the distance and direction
     updateHomePosition();
@@ -2500,7 +2499,7 @@ void checkSafeHomeState(bool shouldBeEnabled)
  **********************************************************/
 bool findNearestSafeHome(void)
 {
-    safehome_index = -1;
+    posControl.safehomeState.index = -1;
     uint32_t nearest_safehome_distance = navConfig()->general.safehome_max_distance + 1;
     uint32_t distance_to_current;
     fpVector3_t currentSafeHome;
@@ -2516,19 +2515,17 @@ bool findNearestSafeHome(void)
         distance_to_current = calculateDistanceToDestination(&currentSafeHome);
         if (distance_to_current < nearest_safehome_distance) {
              // this safehome is the nearest so far - keep track of it.
-             safehome_index = i;
+             posControl.safehomeState.index = i;
              nearest_safehome_distance = distance_to_current;
-             nearestSafeHome.x = currentSafeHome.x;
-             nearestSafeHome.y = currentSafeHome.y;
-             nearestSafeHome.z = currentSafeHome.z;
+             posControl.safehomeState.nearestSafeHome = currentSafeHome;
         }
     }
-    if (safehome_index >= 0) {
-		safehome_distance = nearest_safehome_distance;
+    if (posControl.safehomeState.index >= 0) {
+		posControl.safehomeState.distance = nearest_safehome_distance;
     } else {
-        safehome_distance = 0;
+        posControl.safehomeState.distance = 0;
     }
-    return safehome_distance > 0;
+    return posControl.safehomeState.distance > 0;
 }
 #endif
 
@@ -2558,9 +2555,7 @@ void updateHomePosition(void)
 #endif
                 setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
                 // save the current location in case it is replaced by a safehome or HOME_RESET
-                original_rth_home.x = posControl.rthState.homePosition.pos.x;
-                original_rth_home.y = posControl.rthState.homePosition.pos.y;
-                original_rth_home.z = posControl.rthState.homePosition.pos.z;
+                posControl.rthState.originalHomePosition = posControl.rthState.homePosition.pos;
             }
         }
     }
@@ -4200,6 +4195,7 @@ void navigationInit(void)
     posControl.wpPlannerActiveWPIndex = 0;
     posControl.flags.wpMissionPlannerActive = false;
     posControl.startWpIndex = 0;
+    posControl.safehomeState.isApplied = false;
 #ifdef USE_MULTI_MISSION
     posControl.multiMissionCount = 0;
 #endif
diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
index 4125c7eee08..a1bca7aa036 100644
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@@ -35,7 +35,6 @@
 extern gpsLocation_t        GPS_home;
 extern uint32_t             GPS_distanceToHome;        // distance to home point in meters
 extern int16_t              GPS_directionToHome;       // direction to home point in degrees
-extern fpVector3_t          original_rth_home;         // the original rth home - save it, since it could be replaced by safehome or HOME_RESET
 
 extern bool autoThrottleManuallyIncreased;
 
@@ -59,13 +58,9 @@ typedef enum {
 
 PG_DECLARE_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig);
 
-extern int8_t safehome_index;                    // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise
-extern uint32_t safehome_distance;               // distance to the nearest safehome
-extern bool safehome_applied;                    // whether the safehome has been applied to home.
-
-void resetSafeHomes(void);                       // remove all safehomes
-bool findNearestSafeHome(void);                  // Find nearest safehome
-
+void resetSafeHomes(void);           // remove all safehomes
+bool findNearestSafeHome(void);      // Find nearest safehome
+void suspendSafehome(void);          // Suspend safehome on demand
 #endif // defined(USE_SAFE_HOME)
 
 #ifndef NAV_MAX_WAYPOINTS
diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
index 2a42d81904b..38a39659de4 100644
--- a/src/main/navigation/navigation_private.h
+++ b/src/main/navigation/navigation_private.h
@@ -336,6 +336,7 @@ typedef struct {
     float                   rthFinalAltitude;       // Altitude at end of RTH approach
     float                   rthInitialDistance;     // Distance when starting flight home
     fpVector3_t             homeTmpWaypoint;        // Temporary storage for home target
+    fpVector3_t             originalHomePosition;   // the original rth home - save it, since it could be replaced by safehome or HOME_RESET
 } rthState_t;
 
 typedef enum {
@@ -346,6 +347,14 @@ typedef enum {
     RTH_HOME_FINAL_LAND,            // Home position and altitude
 } rthTargetMode_e;
 
+typedef struct {
+    fpVector3_t nearestSafeHome;    // The nearestSafeHome found during arming
+    uint32_t    distance;           // distance to the nearest safehome
+    int8_t      index;              // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise
+    bool        isApplied;          // whether the safehome has been applied to home
+    bool        isSuspended;        // used to suspend Safehome on demand
+} safehomeState_t;
+
 typedef struct {
     /* Flags and navigation system state */
     navigationFSMState_t        navState;
@@ -368,14 +377,15 @@ typedef struct {
     /* INAV GPS origin (position where GPS fix was first acquired) */
     gpsOrigin_t                 gpsOrigin;
 
-    /* Home parameters (NEU coordinated), geodetic position of home (LLH) is stores in GPS_home variable */
+    /* Home/RTH parameters - NEU coordinates (geodetic position of home (LLH) is stored in GPS_home variable) */
     rthSanityChecker_t          rthSanityChecker;
     rthState_t                  rthState;
-
-    /* Home parameters */
     uint32_t                    homeDistance;   // cm
     int32_t                     homeDirection;  // deg*100
 
+    /* Safehome parameters */
+    safehomeState_t             safehomeState;
+
     /* Cruise */
     navCruise_t                 cruise;
 

From 36caed01ab125ecdbc048179b753a7ce59fcc498 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Sat, 18 Feb 2023 13:14:05 +0000
Subject: [PATCH 09/94] Add trackback suspend + vibration warning

---
 src/main/fc/multifunction.c              | 14 ++++++++++----
 src/main/fc/multifunction.h              | 12 ++++++++++++
 src/main/io/osd.c                        | 18 +++++++++++++++---
 src/main/navigation/navigation.c         | 11 +++--------
 src/main/navigation/navigation.h         |  1 -
 src/main/navigation/navigation_private.h |  1 -
 6 files changed, 40 insertions(+), 17 deletions(-)

diff --git a/src/main/fc/multifunction.c b/src/main/fc/multifunction.c
index 83ece659e79..a0b341d4c36 100644
--- a/src/main/fc/multifunction.c
+++ b/src/main/fc/multifunction.c
@@ -34,11 +34,13 @@
 #include "io/osd.h"
 #include "navigation/navigation.h"
 
+uint8_t multiFunctionFlags;
+
 static void multiFunctionApply(multi_function_e selectedItem)
 {
     switch (selectedItem) {
     case MULTI_FUNC_NONE:
-        return;
+        break;
     case MULTI_FUNC_1:  // redisplay current warnings
         osdResetWarningFlags();
         break;
@@ -48,15 +50,19 @@ static void multiFunctionApply(multi_function_e selectedItem)
     case MULTI_FUNC_3:  // toggle Safehome suspend
 #if defined(USE_SAFE_HOME)
         if (navConfig()->general.flags.safehome_usage_mode != SAFEHOME_USAGE_OFF) {
-            suspendSafehome();
+            MULTI_FUNC_FLAG(SUSPEND_SAFEHOMES) ? MULTI_FUNC_FLAG_DISABLE(SUSPEND_SAFEHOMES) : MULTI_FUNC_FLAG_ENABLE(SUSPEND_SAFEHOMES);
         }
 #endif
         break;
-    case MULTI_FUNC_4:  // emergency ARM
+    case MULTI_FUNC_4:
+        if (navConfig()->general.flags.rth_trackback_mode != RTH_TRACKBACK_OFF) {
+            MULTI_FUNC_FLAG(SUSPEND_TRACKBACK) ? MULTI_FUNC_FLAG_DISABLE(SUSPEND_TRACKBACK) : MULTI_FUNC_FLAG_ENABLE(SUSPEND_TRACKBACK);
+        }
+        break;
+    case MULTI_FUNC_5:  // emergency ARM
         if (!ARMING_FLAG(ARMED)) {
             emergencyArmingUpdate(true, true);
         }
-        break;
     case MULTI_FUNC_END:
         break;
     }
diff --git a/src/main/fc/multifunction.h b/src/main/fc/multifunction.h
index 30038b8617e..e0187edace1 100644
--- a/src/main/fc/multifunction.h
+++ b/src/main/fc/multifunction.h
@@ -24,12 +24,24 @@
 
 #pragma once
 
+extern uint8_t multiFunctionFlags;
+
+#define MULTI_FUNC_FLAG_DISABLE(mask) (multiFunctionFlags &= ~(mask))
+#define MULTI_FUNC_FLAG_ENABLE(mask) (multiFunctionFlags |= (mask))
+#define MULTI_FUNC_FLAG(mask) (multiFunctionFlags & (mask))
+
+typedef enum {
+    SUSPEND_SAFEHOMES = (1 << 0),
+    SUSPEND_TRACKBACK = (1 << 1),
+} multiFunctionFlags_e;
+
 typedef enum {
     MULTI_FUNC_NONE,
     MULTI_FUNC_1,
     MULTI_FUNC_2,
     MULTI_FUNC_3,
     MULTI_FUNC_4,
+    MULTI_FUNC_5,
     MULTI_FUNC_END,
 } multi_function_e;
 
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index a9f5a5d6f1b..ee6ef05bcb9 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -4751,6 +4751,7 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
     /* --- FUNCTIONS --- */
     multi_function_e selectedFunction = multiFunctionSelection();
     if (selectedFunction) {
+        message = "N/A NEXT >";     // Default message if function unavailable
         switch (selectedFunction) {
         case MULTI_FUNC_NONE:
         case MULTI_FUNC_1:
@@ -4760,14 +4761,18 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
             message = posControl.flags.manualEmergLandActive ? "ABORT LAND" : "EMERG LAND";
             break;
         case MULTI_FUNC_3:
-            message = "NO SFHOME ";
 #if defined(USE_SAFE_HOME)
             if (navConfig()->general.flags.safehome_usage_mode != SAFEHOME_USAGE_OFF) {
-                message = posControl.safehomeState.isSuspended ? "USE SFHOME" : "CAN SFHOME";
+                message = MULTI_FUNC_FLAG(SUSPEND_SAFEHOMES) ? "USE SFHOME" : "SUS SFHOME";
             }
 #endif
             break;
         case MULTI_FUNC_4:
+            if (navConfig()->general.flags.rth_trackback_mode != RTH_TRACKBACK_OFF) {
+                message = MULTI_FUNC_FLAG(SUSPEND_TRACKBACK) ? "USE TKBACK" : "SUS TKBACK";
+            }
+            break;
+        case MULTI_FUNC_5:
             message = ARMING_FLAG(ARMED) ? "NOW ARMED " : "EMERG ARM ";
             break;
         case MULTI_FUNC_END:
@@ -4779,7 +4784,7 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
     }
 
     /* --- WARNINGS --- */
-    const char *messages[5];
+    const char *messages[6];
     uint8_t messageCount = 0;
     bool warningCondition = false;
     warningsCount = 0;
@@ -4792,6 +4797,13 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
         messages[messageCount++] = batteryState == BATTERY_CRITICAL ? "BATT EMPTY" : "BATT LOW !";
     }
 
+    // Vibration levels
+    const float vibrationLevel = accGetVibrationLevel();
+    warningCondition = vibrationLevel > 1.5f;
+    if (osdCheckWarning(warningCondition, warningFlagID <<= 1, &warningsCount)) {
+        messages[messageCount++] = vibrationLevel > 2.5f ? "BAD VIBRTN" : "VIBRATION!";
+    }
+
 #if defined(USE_GPS)
     // GPS Fix and Failure
     if (feature(FEATURE_GPS)) {
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index b0bc1f54a0a..cfdee5e7c4e 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -36,6 +36,7 @@
 
 #include "fc/fc_core.h"
 #include "fc/config.h"
+#include "fc/multifunction.h"
 #include "fc/rc_controls.h"
 #include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
@@ -1312,7 +1313,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_TRACKBACK(navigatio
     if (posControl.flags.estPosStatus >= EST_USABLE) {
         const int32_t distFromStartTrackback = calculateDistanceToDestination(&posControl.rthTBPointsList[posControl.rthTBLastSavedIndex]) / 100;
         const bool cancelTrackback = distFromStartTrackback > navConfig()->general.rth_trackback_distance ||
-                                     (rthAltControlStickOverrideCheck(ROLL) && !posControl.flags.forcedRTHActivated);
+                                     ((rthAltControlStickOverrideCheck(ROLL) || MULTI_FUNC_FLAG(SUSPEND_TRACKBACK)) && !posControl.flags.forcedRTHActivated);
 
         if (posControl.activeRthTBPointIndex < 0 || cancelTrackback) {
             posControl.rthTBWrapAroundCounter = posControl.activeRthTBPointIndex = -1;
@@ -2456,16 +2457,10 @@ static navigationHomeFlags_t navigationActualStateHomeValidity(void)
 }
 
 #if defined(USE_SAFE_HOME)
-void suspendSafehome(void)
-{
-    // toggle Safehome suspend each call
-    posControl.safehomeState.isSuspended = !posControl.safehomeState.isSuspended;
-}
-
 void checkSafeHomeState(bool shouldBeEnabled)
 {
     const bool safehomeNotApplicable = navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF ||
-                                       posControl.safehomeState.isSuspended ||
+                                       (MULTI_FUNC_FLAG(SUSPEND_SAFEHOMES) && !posControl.flags.forcedRTHActivated) ||
                                        posControl.flags.rthTrackbackActive ||
                                        (!posControl.safehomeState.isApplied && posControl.homeDistance < navConfig()->general.min_rth_distance);
 
diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
index a1bca7aa036..34df185b972 100644
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@@ -60,7 +60,6 @@ PG_DECLARE_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig);
 
 void resetSafeHomes(void);           // remove all safehomes
 bool findNearestSafeHome(void);      // Find nearest safehome
-void suspendSafehome(void);          // Suspend safehome on demand
 #endif // defined(USE_SAFE_HOME)
 
 #ifndef NAV_MAX_WAYPOINTS
diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
index 38a39659de4..01b8012de45 100644
--- a/src/main/navigation/navigation_private.h
+++ b/src/main/navigation/navigation_private.h
@@ -352,7 +352,6 @@ typedef struct {
     uint32_t    distance;           // distance to the nearest safehome
     int8_t      index;              // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise
     bool        isApplied;          // whether the safehome has been applied to home
-    bool        isSuspended;        // used to suspend Safehome on demand
 } safehomeState_t;
 
 typedef struct {

From 9871c6edee41c424ef62e44585a008327d263e18 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Wed, 22 Feb 2023 23:14:39 +0000
Subject: [PATCH 10/94] Add turtle mode function

---
 src/main/fc/fc_core.c            |  3 ++-
 src/main/fc/multifunction.c      | 17 ++++++++++++-----
 src/main/fc/multifunction.h      |  6 ++++--
 src/main/io/osd.c                | 11 +++++++++--
 src/main/navigation/navigation.c |  4 ++--
 5 files changed, 29 insertions(+), 12 deletions(-)

diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index 91f2f7a8d6e..f4e8712c0c2 100755
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -56,6 +56,7 @@ FILE_COMPILE_FOR_SPEED
 #include "fc/cli.h"
 #include "fc/config.h"
 #include "fc/controlrate_profile.h"
+#include "fc/multifunction.h"
 #include "fc/rc_adjustments.h"
 #include "fc/rc_smoothing.h"
 #include "fc/rc_controls.h"
@@ -512,7 +513,7 @@ void tryArm(void)
     }
 
 #ifdef USE_DSHOT
-    if (STATE(MULTIROTOR) && IS_RC_MODE_ACTIVE(BOXTURTLE) && !FLIGHT_MODE(TURTLE_MODE) &&
+    if (STATE(MULTIROTOR) && (IS_RC_MODE_ACTIVE(BOXTURTLE) || MULTI_FUNC_FLAG(MF_TURTLE_MODE)) && !FLIGHT_MODE(TURTLE_MODE) &&
         emergencyArmingCanOverrideArmingDisabled() && isMotorProtocolDshot()
         ) {
         sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_REVERSED);
diff --git a/src/main/fc/multifunction.c b/src/main/fc/multifunction.c
index a0b341d4c36..19438599b8a 100644
--- a/src/main/fc/multifunction.c
+++ b/src/main/fc/multifunction.c
@@ -50,16 +50,23 @@ static void multiFunctionApply(multi_function_e selectedItem)
     case MULTI_FUNC_3:  // toggle Safehome suspend
 #if defined(USE_SAFE_HOME)
         if (navConfig()->general.flags.safehome_usage_mode != SAFEHOME_USAGE_OFF) {
-            MULTI_FUNC_FLAG(SUSPEND_SAFEHOMES) ? MULTI_FUNC_FLAG_DISABLE(SUSPEND_SAFEHOMES) : MULTI_FUNC_FLAG_ENABLE(SUSPEND_SAFEHOMES);
+            MULTI_FUNC_FLAG(MF_SUSPEND_SAFEHOMES) ? MULTI_FUNC_FLAG_DISABLE(MF_SUSPEND_SAFEHOMES) : MULTI_FUNC_FLAG_ENABLE(MF_SUSPEND_SAFEHOMES);
         }
 #endif
         break;
-    case MULTI_FUNC_4:
+    case MULTI_FUNC_4:  // toggle RTH Trackback suspend
         if (navConfig()->general.flags.rth_trackback_mode != RTH_TRACKBACK_OFF) {
-            MULTI_FUNC_FLAG(SUSPEND_TRACKBACK) ? MULTI_FUNC_FLAG_DISABLE(SUSPEND_TRACKBACK) : MULTI_FUNC_FLAG_ENABLE(SUSPEND_TRACKBACK);
+            MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK) ? MULTI_FUNC_FLAG_DISABLE(MF_SUSPEND_TRACKBACK) : MULTI_FUNC_FLAG_ENABLE(MF_SUSPEND_TRACKBACK);
         }
         break;
-    case MULTI_FUNC_5:  // emergency ARM
+    case MULTI_FUNC_5:
+#ifdef USE_DSHOT
+        if (STATE(MULTIROTOR)) {    // toggle Turtle mode
+            MULTI_FUNC_FLAG(MF_TURTLE_MODE) ? MULTI_FUNC_FLAG_DISABLE(MF_TURTLE_MODE) : MULTI_FUNC_FLAG_ENABLE(MF_TURTLE_MODE);
+        }
+#endif
+        break;
+    case MULTI_FUNC_6:  // emergency ARM
         if (!ARMING_FLAG(ARMED)) {
             emergencyArmingUpdate(true, true);
         }
@@ -97,7 +104,7 @@ multi_function_e multiFunctionSelection(void)
             selectedItem = selectedItem == MULTI_FUNC_END - 1 ? MULTI_FUNC_1 : selectedItem + 1;
         }
         selectTimer = 0;
-        if (currentTime - startTimer > 2000) {      // 2s reset delay after mode deselected
+        if (currentTime - startTimer > 3000) {      // 3s reset delay after mode deselected
             startTimer = 0;
             selectedItem = MULTI_FUNC_NONE;
         }
diff --git a/src/main/fc/multifunction.h b/src/main/fc/multifunction.h
index e0187edace1..dd1163b3123 100644
--- a/src/main/fc/multifunction.h
+++ b/src/main/fc/multifunction.h
@@ -31,8 +31,9 @@ extern uint8_t multiFunctionFlags;
 #define MULTI_FUNC_FLAG(mask) (multiFunctionFlags & (mask))
 
 typedef enum {
-    SUSPEND_SAFEHOMES = (1 << 0),
-    SUSPEND_TRACKBACK = (1 << 1),
+    MF_SUSPEND_SAFEHOMES    = (1 << 0),
+    MF_SUSPEND_TRACKBACK    = (1 << 1),
+    MF_TURTLE_MODE          = (1 << 2),
 } multiFunctionFlags_e;
 
 typedef enum {
@@ -42,6 +43,7 @@ typedef enum {
     MULTI_FUNC_3,
     MULTI_FUNC_4,
     MULTI_FUNC_5,
+    MULTI_FUNC_6,
     MULTI_FUNC_END,
 } multi_function_e;
 
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index ee6ef05bcb9..9c48f40fcd8 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -4763,16 +4763,23 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
         case MULTI_FUNC_3:
 #if defined(USE_SAFE_HOME)
             if (navConfig()->general.flags.safehome_usage_mode != SAFEHOME_USAGE_OFF) {
-                message = MULTI_FUNC_FLAG(SUSPEND_SAFEHOMES) ? "USE SFHOME" : "SUS SFHOME";
+                message = MULTI_FUNC_FLAG(MF_SUSPEND_SAFEHOMES) ? "USE SFHOME" : "SUS SFHOME";
             }
 #endif
             break;
         case MULTI_FUNC_4:
             if (navConfig()->general.flags.rth_trackback_mode != RTH_TRACKBACK_OFF) {
-                message = MULTI_FUNC_FLAG(SUSPEND_TRACKBACK) ? "USE TKBACK" : "SUS TKBACK";
+                message = MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK) ? "USE TKBACK" : "SUS TKBACK";
             }
             break;
         case MULTI_FUNC_5:
+#ifdef USE_DSHOT
+            if (STATE(MULTIROTOR)) {
+                message = MULTI_FUNC_FLAG(MF_TURTLE_MODE) ? "USE TURTLE" : "END TURTLE";
+            }
+#endif
+            break;
+        case MULTI_FUNC_6:
             message = ARMING_FLAG(ARMED) ? "NOW ARMED " : "EMERG ARM ";
             break;
         case MULTI_FUNC_END:
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index cfdee5e7c4e..c79107e603b 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -1313,7 +1313,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_TRACKBACK(navigatio
     if (posControl.flags.estPosStatus >= EST_USABLE) {
         const int32_t distFromStartTrackback = calculateDistanceToDestination(&posControl.rthTBPointsList[posControl.rthTBLastSavedIndex]) / 100;
         const bool cancelTrackback = distFromStartTrackback > navConfig()->general.rth_trackback_distance ||
-                                     ((rthAltControlStickOverrideCheck(ROLL) || MULTI_FUNC_FLAG(SUSPEND_TRACKBACK)) && !posControl.flags.forcedRTHActivated);
+                                     ((rthAltControlStickOverrideCheck(ROLL) || MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK)) && !posControl.flags.forcedRTHActivated);
 
         if (posControl.activeRthTBPointIndex < 0 || cancelTrackback) {
             posControl.rthTBWrapAroundCounter = posControl.activeRthTBPointIndex = -1;
@@ -2460,7 +2460,7 @@ static navigationHomeFlags_t navigationActualStateHomeValidity(void)
 void checkSafeHomeState(bool shouldBeEnabled)
 {
     const bool safehomeNotApplicable = navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF ||
-                                       (MULTI_FUNC_FLAG(SUSPEND_SAFEHOMES) && !posControl.flags.forcedRTHActivated) ||
+                                       (MULTI_FUNC_FLAG(MF_SUSPEND_SAFEHOMES) && !posControl.flags.forcedRTHActivated) ||
                                        posControl.flags.rthTrackbackActive ||
                                        (!posControl.safehomeState.isApplied && posControl.homeDistance < navConfig()->general.min_rth_distance);
 

From 56c4235b4a42eb5b57954a0646a6e1852a00f748 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E7=8E=8B=E5=BB=BA=E6=AD=A6?= <jianwwpro@sina.com>
Date: Fri, 24 Feb 2023 09:55:55 +0800
Subject: [PATCH 11/94] MAMBAF405US ADD mpu6500 support

---
 src/main/target/MAMBAF405US/target.h | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/src/main/target/MAMBAF405US/target.h b/src/main/target/MAMBAF405US/target.h
index d7ae8462fe3..b6b95cb12b6 100644
--- a/src/main/target/MAMBAF405US/target.h
+++ b/src/main/target/MAMBAF405US/target.h
@@ -40,6 +40,10 @@
 #define MPU6000_SPI_BUS                 BUS_SPI1
 #define MPU6000_CS_PIN                  PA4
 
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN               CW180_DEG
+#define MPU6500_SPI_BUS                 BUS_SPI1
+#define MPU6500_CS_PIN                  PA4
 
 // *************** Baro **************************
 #define USE_I2C

From f10988cac1bf6144e73df8901ac173b80a2dcfda Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Wed, 8 Mar 2023 23:02:09 +0000
Subject: [PATCH 12/94] Add compass failure

---
 src/main/io/osd.c | 13 ++++++++++++-
 1 file changed, 12 insertions(+), 1 deletion(-)

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 9c48f40fcd8..85f8f060e31 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -101,6 +101,7 @@ FILE_COMPILE_FOR_SPEED
 #include "sensors/acceleration.h"
 #include "sensors/battery.h"
 #include "sensors/boardalignment.h"
+#include "sensors/compass.h"
 #include "sensors/diagnostics.h"
 #include "sensors/sensors.h"
 #include "sensors/pitotmeter.h"
@@ -4791,7 +4792,7 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
     }
 
     /* --- WARNINGS --- */
-    const char *messages[6];
+    const char *messages[7];
     uint8_t messageCount = 0;
     bool warningCondition = false;
     warningsCount = 0;
@@ -4833,6 +4834,16 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
     }
 #endif
 
+#if defined(USE_MAG)
+    // Magnetometer failure
+    if (requestedSensors[SENSOR_INDEX_MAG] != MAG_NONE) {
+        hardwareSensorStatus_e magStatus = getHwCompassStatus();
+        if (osdCheckWarning(magStatus == HW_SENSOR_UNAVAILABLE || magStatus == HW_SENSOR_UNHEALTHY, warningFlagID <<= 1, &warningsCount)) {
+            messages[messageCount++] = "MAG FAILED";
+        }
+    }
+#endif
+
 #ifdef USE_DEV_TOOLS
     if (osdCheckWarning(systemConfig()->groundTestMode, warningFlagID <<= 1, &warningsCount)) {
         messages[messageCount++] = "GRD TEST !";

From f8da237e28fce011aa6033146a5c08cebc664681 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Fri, 24 Mar 2023 13:40:49 +0000
Subject: [PATCH 13/94] first build

---
 src/main/navigation/navigation.c             | 46 ++++++++++++--------
 src/main/navigation/navigation_fixedwing.c   |  6 +--
 src/main/navigation/navigation_multicopter.c | 10 ++---
 src/main/navigation/navigation_private.h     |  5 ++-
 4 files changed, 38 insertions(+), 29 deletions(-)

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index e164b33ab52..51d415a960c 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -1286,7 +1286,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
         if (STATE(FIXED_WING_LEGACY)) {
             if (navConfig()->general.flags.rth_climb_first == RTH_CLIMB_ON_FW_SPIRAL) {
                 float altitudeChangeDirection = (tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM) > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1;
-                updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL);
+                updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, 0, ROC_TO_ALT_CONSTANT);
             } else {
                 tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM;
                 setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
@@ -1396,7 +1396,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
     // Wait until target heading is reached for MR (with 15 deg margin for error), or continue for Fixed Wing
     if ((ABS(wrap_18000(posControl.rthState.homePosition.heading - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY)) {
         resetLandingDetector();     // force reset landing detector just in case
-        updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
+        updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
         return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; // success = land
     } else {
         fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL);
@@ -1424,7 +1424,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(na
             setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
         } else {
             float altitudeChangeDirection = tmpHomePos->z > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1;
-            updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL);
+            updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, tmpHomePos->z, ROC_TO_ALT_TARGET);
         }
     }
     else {
@@ -1471,7 +1471,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
         descentVelLimited = constrainf(descentVelScaled, navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd);
     }
 
-    updateClimbRateToAltitudeController(-descentVelLimited, ROC_TO_ALT_NORMAL);
+    updateClimbRateToAltitudeController(-descentVelLimited, 0, ROC_TO_ALT_CONSTANT);
 
     return NAV_FSM_EVENT_NONE;
 }
@@ -1494,7 +1494,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigation
     UNUSED(previousState);
 
     if (STATE(ALTITUDE_CONTROL)) {
-        updateClimbRateToAltitudeController(-1.1f * navConfig()->general.land_minalt_vspd, ROC_TO_ALT_NORMAL);  // FIXME
+        updateClimbRateToAltitudeController(-1.1f * navConfig()->general.land_minalt_vspd, 0, ROC_TO_ALT_CONSTANT);  // FIXME
     }
 
     // Prevent I-terms growing when already landed
@@ -1704,7 +1704,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi
         // Adjust altitude to waypoint setting
         if (STATE(AIRPLANE)) {
             int8_t altitudeChangeDirection = posControl.activeWaypoint.pos.z > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1;
-            updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL);
+            updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, posControl.activeWaypoint.pos.z, ROC_TO_ALT_TARGET);
         } else {
             setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_Z);
         }
@@ -2786,7 +2786,7 @@ void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlag
 
     // Z-position
     if ((useMask & NAV_POS_UPDATE_Z) != 0) {
-        updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);   // Reset RoC/RoD -> altitude controller
+        updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);   // Reset RoC/RoD -> altitude controller
         posControl.desiredState.pos.z = pos->z;
     }
 
@@ -2877,28 +2877,34 @@ bool isFlightDetected(void)
 /*-----------------------------------------------------------
  * Z-position controller
  *-----------------------------------------------------------*/
-void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAltitudeControllerMode_e mode)
+void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode)
 {
+#define MIN_TARGET_CLIMB_RATE   100.0f
+
     static timeUs_t lastUpdateTimeUs;
     timeUs_t currentTimeUs = micros();
 
     // Terrain following uses different altitude measurement
     const float altitudeToUse = navGetCurrentActualPositionAndVelocity()->pos.z;
 
-    if (mode == ROC_TO_ALT_RESET) {
-        lastUpdateTimeUs = currentTimeUs;
-        posControl.desiredState.pos.z = altitudeToUse;
-    }
-    else {      // ROC_TO_ALT_NORMAL
+    if (mode != ROC_TO_ALT_RESET && desiredClimbRate) {  // ROC_TO_ALT_CONSTANT & ROC_TO_ALT_TARGET
+
+        /*  */
+        if (mode == ROC_TO_ALT_TARGET && fabsf(desiredClimbRate) > MIN_TARGET_CLIMB_RATE) {
+            int8_t direction = desiredClimbRate > 0 ? 1 : -1;
+            float absClimbRate = fabsf(desiredClimbRate);
+            uint16_t maxRateCutoffAlt = STATE(AIRPLANE) ? absClimbRate * 5 : absClimbRate;
+            float verticalVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z - targetAltitude,
+                                      direction * -500.0f, direction * -maxRateCutoffAlt, MIN_TARGET_CLIMB_RATE, absClimbRate);
+
+            desiredClimbRate = direction * constrainf(verticalVelScaled, MIN_TARGET_CLIMB_RATE, absClimbRate);
+        }
 
         /*
          * If max altitude is set, reset climb rate if altitude is reached and climb rate is > 0
          * In other words, when altitude is reached, allow it only to shrink
          */
-        if (navConfig()->general.max_altitude > 0 &&
-            altitudeToUse >= navConfig()->general.max_altitude &&
-            desiredClimbRate > 0
-        ) {
+        if (navConfig()->general.max_altitude > 0 && altitudeToUse >= navConfig()->general.max_altitude && desiredClimbRate > 0) {
             desiredClimbRate = 0;
         }
 
@@ -2924,9 +2930,11 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
             // Multicopter climb-rate control is closed-loop, it's possible to directly calculate desired altitude setpoint to yield the required RoC/RoD
             posControl.desiredState.pos.z = altitudeToUse + (desiredClimbRate / posControl.pids.pos[Z].param.kP);
         }
-
-        lastUpdateTimeUs = currentTimeUs;
+    } else {    // ROC_TO_ALT_RESET or zero desired climbrate
+        posControl.desiredState.pos.z = altitudeToUse;
     }
+
+    lastUpdateTimeUs = currentTimeUs;
 }
 
 static void resetAltitudeController(bool useTerrainFollowing)
diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index 4ca3d2fc3d3..075176c79eb 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -117,13 +117,13 @@ bool adjustFixedWingAltitudeFromRCInput(void)
     if (rcAdjustment) {
         // set velocity proportional to stick movement
         float rcClimbRate = -rcAdjustment * navConfig()->general.max_manual_climb_rate / (500.0f - rcControlsConfig()->alt_hold_deadband);
-        updateClimbRateToAltitudeController(rcClimbRate, ROC_TO_ALT_NORMAL);
+        updateClimbRateToAltitudeController(rcClimbRate, 0, ROC_TO_ALT_CONSTANT);
         return true;
     }
     else {
         // Adjusting finished - reset desired position to stay exactly where pilot released the stick
         if (posControl.flags.isAdjustingAltitude) {
-            updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
+            updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
         }
         return false;
     }
@@ -760,7 +760,7 @@ void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
     rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
 
     if (posControl.flags.estAltStatus >= EST_USABLE) {
-        updateClimbRateToAltitudeController(-1.0f * navConfig()->general.emerg_descent_rate, ROC_TO_ALT_NORMAL);
+        updateClimbRateToAltitudeController(-1.0f * navConfig()->general.emerg_descent_rate, 0, ROC_TO_ALT_TARGET);
         applyFixedWingAltitudeAndThrottleController(currentTimeUs);
 
         int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle));
diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c
index 2c5eaf4a34e..23a71c5c68f 100644
--- a/src/main/navigation/navigation_multicopter.c
+++ b/src/main/navigation/navigation_multicopter.c
@@ -133,11 +133,11 @@ bool adjustMulticopterAltitudeFromRCInput(void)
         // In terrain follow mode we apply different logic for terrain control
         if (posControl.flags.estAglStatus == EST_TRUSTED && altTarget > 10.0f) {
             // We have solid terrain sensor signal - directly map throttle to altitude
-            updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
+            updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
             posControl.desiredState.pos.z = altTarget;
         }
         else {
-            updateClimbRateToAltitudeController(-50.0f, ROC_TO_ALT_NORMAL);
+            updateClimbRateToAltitudeController(-50.0f, 0, ROC_TO_ALT_CONSTANT);
         }
 
         // In surface tracking we always indicate that we're adjusting altitude
@@ -159,14 +159,14 @@ bool adjustMulticopterAltitudeFromRCInput(void)
                 rcClimbRate = rcThrottleAdjustment * navConfig()->general.max_manual_climb_rate / (float)(altHoldThrottleRCZero - getThrottleIdleValue() - rcControlsConfig()->alt_hold_deadband);
             }
 
-            updateClimbRateToAltitudeController(rcClimbRate, ROC_TO_ALT_NORMAL);
+            updateClimbRateToAltitudeController(rcClimbRate, 0, ROC_TO_ALT_CONSTANT);
 
             return true;
         }
         else {
             // Adjusting finished - reset desired position to stay exactly where pilot released the stick
             if (posControl.flags.isAdjustingAltitude) {
-                updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
+                updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
             }
 
             return false;
@@ -842,7 +842,7 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
 
         // Check if last correction was not too long ago
         if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
-            updateClimbRateToAltitudeController(-1.0f * navConfig()->general.emerg_descent_rate, ROC_TO_ALT_NORMAL);
+            updateClimbRateToAltitudeController(-1.0f * navConfig()->general.emerg_descent_rate, 0, ROC_TO_ALT_TARGET);
             updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate);
             updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate);
         }
diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
index 0598467d28a..fdfa14e9b40 100644
--- a/src/main/navigation/navigation_private.h
+++ b/src/main/navigation/navigation_private.h
@@ -61,7 +61,8 @@ typedef enum {
 
 typedef enum {
     ROC_TO_ALT_RESET,
-    ROC_TO_ALT_NORMAL
+    ROC_TO_ALT_CONSTANT,
+    ROC_TO_ALT_TARGET
 } climbRateToAltitudeControllerMode_e;
 
 typedef enum {
@@ -447,7 +448,7 @@ void setHomePosition(const fpVector3_t * pos, int32_t heading, navSetWaypointFla
 void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask);
 void setDesiredSurfaceOffset(float surfaceOffset);
 void setDesiredPositionToFarAwayTarget(int32_t yaw, int32_t distance, navSetWaypointFlags_t useMask);   // NOT USED
-void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAltitudeControllerMode_e mode);
+void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode);
 
 bool isNavHoldPositionActive(void);
 bool isLastMissionWaypoint(void);

From 5a454a340031e22f63f3fad81bd75f63141059fc Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Sun, 26 Mar 2023 23:24:26 +0100
Subject: [PATCH 14/94] update

---
 src/main/navigation/navigation.c             | 42 +++++---------------
 src/main/navigation/navigation_fixedwing.c   |  9 ++++-
 src/main/navigation/navigation_multicopter.c |  2 +-
 3 files changed, 18 insertions(+), 35 deletions(-)

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 51d415a960c..76805788dfc 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -1283,17 +1283,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
         }
 
         // Climb to safe altitude and turn to correct direction
+        // Until the initial climb phase is complete target slightly *above* the cruise altitude to ensure we actually reach
+        // it in a reasonable time. Immediately after we finish this phase - target the original altitude.
         if (STATE(FIXED_WING_LEGACY)) {
-            if (navConfig()->general.flags.rth_climb_first == RTH_CLIMB_ON_FW_SPIRAL) {
-                float altitudeChangeDirection = (tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM) > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1;
-                updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, 0, ROC_TO_ALT_CONSTANT);
-            } else {
-                tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM;
-                setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
-            }
+            tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM;
+            setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
         } else {
-            // Until the initial climb phase is complete target slightly *above* the cruise altitude to ensure we actually reach
-            // it in a reasonable time. Immediately after we finish this phase - target the original altitude.
             tmpHomePos->z += MR_RTH_CLIMB_OVERSHOOT_CM;
 
             if (navConfig()->general.flags.rth_tail_first) {
@@ -1415,21 +1410,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(na
     }
 
     fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_HOVER);
-
-    if (navConfig()->general.rth_home_altitude) {
-        float timeToReachHomeAltitude = fabsf(tmpHomePos->z - navGetCurrentActualPositionAndVelocity()->pos.z) / navConfig()->general.max_auto_climb_rate;
-
-        if (timeToReachHomeAltitude < 1) {
-            // we almost reached the target home altitude so set the desired altitude to the desired home altitude
-            setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
-        } else {
-            float altitudeChangeDirection = tmpHomePos->z > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1;
-            updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, tmpHomePos->z, ROC_TO_ALT_TARGET);
-        }
-    }
-    else {
-        setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
-    }
+    setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
 
     return NAV_FSM_EVENT_NONE;
 }
@@ -1702,12 +1683,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi
 
     if (navConfig()->general.waypoint_enforce_altitude && !posControl.wpAltitudeReached) {
         // Adjust altitude to waypoint setting
-        if (STATE(AIRPLANE)) {
-            int8_t altitudeChangeDirection = posControl.activeWaypoint.pos.z > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1;
-            updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, posControl.activeWaypoint.pos.z, ROC_TO_ALT_TARGET);
-        } else {
-            setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_Z);
-        }
+        setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_Z);
 
         posControl.wpAltitudeReached = isWaypointAltitudeReached();
 
@@ -2887,9 +2863,11 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt
     // Terrain following uses different altitude measurement
     const float altitudeToUse = navGetCurrentActualPositionAndVelocity()->pos.z;
 
-    if (mode != ROC_TO_ALT_RESET && desiredClimbRate) {  // ROC_TO_ALT_CONSTANT & ROC_TO_ALT_TARGET
+    if (mode != ROC_TO_ALT_RESET && desiredClimbRate) {
+        /* ROC_TO_ALT_CONSTANT - constant climb rate always
+         * ROC_TO_ALT_TARGET - constant climb rate until close to target altitude reducing to min value when altitude reached
+         * Rate reduction starts at distance from target altitude of 5 x climb rate for FW, 1 x climb rate for MC */
 
-        /*  */
         if (mode == ROC_TO_ALT_TARGET && fabsf(desiredClimbRate) > MIN_TARGET_CLIMB_RATE) {
             int8_t direction = desiredClimbRate > 0 ? 1 : -1;
             float absClimbRate = fabsf(desiredClimbRate);
diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index 075176c79eb..75a93d68e2f 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -157,9 +157,14 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
     const float pitchGainInv = 1.0f / 1.0f;
 
     // Here we use negative values for dive for better clarity
-    const float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle);
+    float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle);
     const float minDiveDeciDeg = -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle);
 
+    // Reduce max allowed climb pitch if performing loiter climb
+    if (isNavHoldPositionActive()) {
+        maxClimbDeciDeg = maxClimbDeciDeg * 0.67f;
+    }
+
     // PID controller to translate energy balance error [J] into pitch angle [decideg]
     float targetPitchAngle = navPidApply3(&posControl.pids.fw_alt, demSEB, estSEB, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, 0, pitchGainInv, 1.0f);
 
@@ -760,7 +765,7 @@ void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
     rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
 
     if (posControl.flags.estAltStatus >= EST_USABLE) {
-        updateClimbRateToAltitudeController(-1.0f * navConfig()->general.emerg_descent_rate, 0, ROC_TO_ALT_TARGET);
+        updateClimbRateToAltitudeController(-navConfig()->general.emerg_descent_rate, 1000, ROC_TO_ALT_TARGET);
         applyFixedWingAltitudeAndThrottleController(currentTimeUs);
 
         int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle));
diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c
index 23a71c5c68f..76f49bb0d28 100644
--- a/src/main/navigation/navigation_multicopter.c
+++ b/src/main/navigation/navigation_multicopter.c
@@ -842,7 +842,7 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
 
         // Check if last correction was not too long ago
         if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
-            updateClimbRateToAltitudeController(-1.0f * navConfig()->general.emerg_descent_rate, 0, ROC_TO_ALT_TARGET);
+            updateClimbRateToAltitudeController(-navConfig()->general.emerg_descent_rate, 500, ROC_TO_ALT_TARGET);
             updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate);
             updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate);
         }

From a0456565aca5ad8c044dbfb2195da265caf42ef2 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Mon, 27 Mar 2023 21:42:01 +0100
Subject: [PATCH 15/94] fix loiter detection FW + tweaks

---
 src/main/navigation/navigation.c             | 8 ++++----
 src/main/navigation/navigation_fixedwing.c   | 5 +++--
 src/main/navigation/navigation_multicopter.c | 1 +
 3 files changed, 8 insertions(+), 6 deletions(-)

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 76805788dfc..99b29f48b4d 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -2855,7 +2855,7 @@ bool isFlightDetected(void)
  *-----------------------------------------------------------*/
 void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode)
 {
-#define MIN_TARGET_CLIMB_RATE   100.0f
+#define MIN_TARGET_CLIMB_RATE   100.0f  // cm/s
 
     static timeUs_t lastUpdateTimeUs;
     timeUs_t currentTimeUs = micros();
@@ -2864,8 +2864,8 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt
     const float altitudeToUse = navGetCurrentActualPositionAndVelocity()->pos.z;
 
     if (mode != ROC_TO_ALT_RESET && desiredClimbRate) {
-        /* ROC_TO_ALT_CONSTANT - constant climb rate always
-         * ROC_TO_ALT_TARGET - constant climb rate until close to target altitude reducing to min value when altitude reached
+        /* ROC_TO_ALT_CONSTANT - constant climb rate
+         * ROC_TO_ALT_TARGET - constant climb rate until close to target altitude reducing to min rate when altitude reached
          * Rate reduction starts at distance from target altitude of 5 x climb rate for FW, 1 x climb rate for MC */
 
         if (mode == ROC_TO_ALT_TARGET && fabsf(desiredClimbRate) > MIN_TARGET_CLIMB_RATE) {
@@ -2873,7 +2873,7 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt
             float absClimbRate = fabsf(desiredClimbRate);
             uint16_t maxRateCutoffAlt = STATE(AIRPLANE) ? absClimbRate * 5 : absClimbRate;
             float verticalVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z - targetAltitude,
-                                      direction * -500.0f, direction * -maxRateCutoffAlt, MIN_TARGET_CLIMB_RATE, absClimbRate);
+                                      0.0f, -maxRateCutoffAlt * direction, MIN_TARGET_CLIMB_RATE, absClimbRate);
 
             desiredClimbRate = direction * constrainf(verticalVelScaled, MIN_TARGET_CLIMB_RATE, absClimbRate);
         }
diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index 75a93d68e2f..dc02205613f 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -160,8 +160,8 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
     float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle);
     const float minDiveDeciDeg = -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle);
 
-    // Reduce max allowed climb pitch if performing loiter climb
-    if (isNavHoldPositionActive()) {
+    // Reduce max allowed climb pitch if performing loiter (stall prevention)
+    if (needToCalculateCircularLoiter) {
         maxClimbDeciDeg = maxClimbDeciDeg * 0.67f;
     }
 
@@ -765,6 +765,7 @@ void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
     rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
 
     if (posControl.flags.estAltStatus >= EST_USABLE) {
+        // target min descent rate 10m above takeoff altitude
         updateClimbRateToAltitudeController(-navConfig()->general.emerg_descent_rate, 1000, ROC_TO_ALT_TARGET);
         applyFixedWingAltitudeAndThrottleController(currentTimeUs);
 
diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c
index 76f49bb0d28..23c008c805c 100644
--- a/src/main/navigation/navigation_multicopter.c
+++ b/src/main/navigation/navigation_multicopter.c
@@ -842,6 +842,7 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
 
         // Check if last correction was not too long ago
         if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
+            // target min descent rate 5m above takeoff altitude
             updateClimbRateToAltitudeController(-navConfig()->general.emerg_descent_rate, 500, ROC_TO_ALT_TARGET);
             updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate);
             updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate);

From 0b7632d4c4f161370eb5d67bb96b319aae9d0957 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Tue, 28 Mar 2023 11:29:50 +0100
Subject: [PATCH 16/94] fixes

---
 src/main/navigation/navigation.c             | 10 +++++-----
 src/main/navigation/navigation_fixedwing.c   |  2 +-
 src/main/navigation/navigation_multicopter.c |  2 +-
 3 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 99b29f48b4d..35c24c909df 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -2869,11 +2869,11 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt
          * Rate reduction starts at distance from target altitude of 5 x climb rate for FW, 1 x climb rate for MC */
 
         if (mode == ROC_TO_ALT_TARGET && fabsf(desiredClimbRate) > MIN_TARGET_CLIMB_RATE) {
-            int8_t direction = desiredClimbRate > 0 ? 1 : -1;
-            float absClimbRate = fabsf(desiredClimbRate);
-            uint16_t maxRateCutoffAlt = STATE(AIRPLANE) ? absClimbRate * 5 : absClimbRate;
-            float verticalVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z - targetAltitude,
-                                      0.0f, -maxRateCutoffAlt * direction, MIN_TARGET_CLIMB_RATE, absClimbRate);
+            const int8_t direction = desiredClimbRate > 0 ? 1 : -1;
+            const float absClimbRate = fabsf(desiredClimbRate);
+            const uint16_t maxRateCutoffAlt = STATE(AIRPLANE) ? absClimbRate * 5 : absClimbRate;
+            const float verticalVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z - targetAltitude,
+                                            0.0f, -maxRateCutoffAlt * direction, MIN_TARGET_CLIMB_RATE, absClimbRate);
 
             desiredClimbRate = direction * constrainf(verticalVelScaled, MIN_TARGET_CLIMB_RATE, absClimbRate);
         }
diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index dc02205613f..e0cd91a0c09 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -766,7 +766,7 @@ void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
 
     if (posControl.flags.estAltStatus >= EST_USABLE) {
         // target min descent rate 10m above takeoff altitude
-        updateClimbRateToAltitudeController(-navConfig()->general.emerg_descent_rate, 1000, ROC_TO_ALT_TARGET);
+        updateClimbRateToAltitudeController(-navConfig()->general.emerg_descent_rate, 1000.0f, ROC_TO_ALT_TARGET);
         applyFixedWingAltitudeAndThrottleController(currentTimeUs);
 
         int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle));
diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c
index 23c008c805c..b429e0ef51c 100644
--- a/src/main/navigation/navigation_multicopter.c
+++ b/src/main/navigation/navigation_multicopter.c
@@ -843,7 +843,7 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
         // Check if last correction was not too long ago
         if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
             // target min descent rate 5m above takeoff altitude
-            updateClimbRateToAltitudeController(-navConfig()->general.emerg_descent_rate, 500, ROC_TO_ALT_TARGET);
+            updateClimbRateToAltitudeController(-navConfig()->general.emerg_descent_rate, 500.0f, ROC_TO_ALT_TARGET);
             updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate);
             updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate);
         }

From 5fde0a041d3b2ba545c56eea97156a773facfad5 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Fri, 28 Apr 2023 10:24:21 +0100
Subject: [PATCH 17/94] Update osd.c

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

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 98b9c9e2c67..5422c497131 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -1969,7 +1969,7 @@ static bool osdDrawSingleElement(uint8_t item)
             } else {
                 osdFormatAltitudeSymbol(buff, alt);
             }
-#else       
+#else
             // BFCOMPAT mode not supported, directly call original altitude formatting function
             osdFormatAltitudeSymbol(buff, alt);
 #endif
@@ -2948,7 +2948,7 @@ static bool osdDrawSingleElement(uint8_t item)
                         tfp_sprintf(buff, "%s%c", buff, SYM_AH_MI);
                     }
                     if (!efficiencyValid) {
-                        buff[0] = buff[1] = buff[2] = buff[3] = '-';    
+                        buff[0] = buff[1] = buff[2] = buff[3] = '-';
                         buff[digits] = SYM_MAH_MI_0;        // This will overwrite the "-" at buff[3] if not in BFCOMPAT mode
                         buff[digits + 1] = SYM_MAH_MI_1;
                         buff[digits + 2] = '\0';
@@ -2962,7 +2962,7 @@ static bool osdDrawSingleElement(uint8_t item)
                         tfp_sprintf(buff, "%s%c", buff, SYM_AH_NM);
                     }
                     if (!efficiencyValid) {
-                        buff[0] = buff[1] = buff[2] = buff[3] = '-';    
+                        buff[0] = buff[1] = buff[2] = buff[3] = '-';
                         buff[digits] = SYM_MAH_NM_0;
                         buff[digits + 1] = SYM_MAH_NM_1;
                         buff[digits + 2] = '\0';
@@ -2978,7 +2978,7 @@ static bool osdDrawSingleElement(uint8_t item)
                         tfp_sprintf(buff, "%s%c", buff, SYM_AH_KM);
                     }
                     if (!efficiencyValid) {
-                        buff[0] = buff[1] = buff[2] = buff[3] = '-';    
+                        buff[0] = buff[1] = buff[2] = buff[3] = '-';
                         buff[digits] = SYM_MAH_KM_0;
                         buff[digits + 1] = SYM_MAH_KM_1;
                         buff[digits + 2] = '\0';
@@ -4946,12 +4946,12 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
         messages[messageCount++] = batteryState == BATTERY_CRITICAL ? "BATT EMPTY" : "BATT LOW !";
     }
 
-    // Vibration levels
-    const float vibrationLevel = accGetVibrationLevel();
-    warningCondition = vibrationLevel > 1.5f;
-    if (osdCheckWarning(warningCondition, warningFlagID <<= 1, &warningsCount)) {
-        messages[messageCount++] = vibrationLevel > 2.5f ? "BAD VIBRTN" : "VIBRATION!";
-    }
+    // Vibration levels   TODO - needs better vibration measurement to be useful
+    // const float vibrationLevel = accGetVibrationLevel();
+    // warningCondition = vibrationLevel > 1.5f;
+    // if (osdCheckWarning(warningCondition, warningFlagID <<= 1, &warningsCount)) {
+        // messages[messageCount++] = vibrationLevel > 2.5f ? "BAD VIBRTN" : "VIBRATION!";
+    // }
 
 #if defined(USE_GPS)
     // GPS Fix and Failure

From 18e8625cb7d8fc8a247753ece2f69e3d3a1258ac Mon Sep 17 00:00:00 2001
From: YI-BOYANG <46438966+YI-BOYANG@users.noreply.github.com>
Date: Fri, 23 Jun 2023 16:58:33 +0800
Subject: [PATCH 18/94] add GEPRCF405 target

---
 src/main/target/GEPRCF405/CMakeLists.txt |   1 +
 src/main/target/GEPRCF405/target.c       |  46 ++++++
 src/main/target/GEPRCF405/target.h       | 177 +++++++++++++++++++++++
 3 files changed, 224 insertions(+)
 create mode 100644 src/main/target/GEPRCF405/CMakeLists.txt
 create mode 100644 src/main/target/GEPRCF405/target.c
 create mode 100644 src/main/target/GEPRCF405/target.h

diff --git a/src/main/target/GEPRCF405/CMakeLists.txt b/src/main/target/GEPRCF405/CMakeLists.txt
new file mode 100644
index 00000000000..2e3b1dec27f
--- /dev/null
+++ b/src/main/target/GEPRCF405/CMakeLists.txt
@@ -0,0 +1 @@
+target_stm32f405xg(GEPRCF405)
diff --git a/src/main/target/GEPRCF405/target.c b/src/main/target/GEPRCF405/target.c
new file mode 100644
index 00000000000..38eee3688d0
--- /dev/null
+++ b/src/main/target/GEPRCF405/target.c
@@ -0,0 +1,46 @@
+/*
+* This file is part of INAV Project.
+*
+* Cleanflight is free software: you can redistribute it 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.
+*
+* Cleanflight 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include <stdint.h>
+
+#include "platform.h"
+#include "drivers/io.h"
+
+#include "drivers/dma.h"
+#include "drivers/timer.h"
+#include "drivers/timer_def.h"
+
+
+
+
+
+timerHardware_t timerHardware[] = {
+    DEF_TIM(TIM8,  CH2,  PC7, TIM_USE_PPM,                        0, 0),
+
+    DEF_TIM(TIM1, CH1, PA8,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1),
+    DEF_TIM(TIM1, CH2, PA9,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1),
+    DEF_TIM(TIM1, CH3, PA10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1),
+    DEF_TIM(TIM8, CH3, PC8,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1),
+    DEF_TIM(TIM8, CH4, PC9,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
+    DEF_TIM(TIM3, CH3, PB0,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
+
+    DEF_TIM(TIM3, CH4, PB1, TIM_USE_LED,                         0, 0),
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
+
+
diff --git a/src/main/target/GEPRCF405/target.h b/src/main/target/GEPRCF405/target.h
new file mode 100644
index 00000000000..5df9af996eb
--- /dev/null
+++ b/src/main/target/GEPRCF405/target.h
@@ -0,0 +1,177 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * Cleanflight is free software: you can redistribute it 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.
+ *
+ * Cleanflight 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#define TARGET_BOARD_IDENTIFIER "GEPR"
+
+#define USBD_PRODUCT_STRING     "GEPRCF405"
+
+#define LED0                    PC14
+#define LED1                    PC15
+
+
+
+#define BEEPER                  PC13
+#define BEEPER_INVERTED
+
+// *************** Gyro & ACC **********************
+
+
+
+#define USE_SPI
+#define USE_SPI_DEVICE_3
+
+#define SPI3_SCK_PIN            PB3
+#define SPI3_MISO_PIN           PB4
+#define SPI3_MOSI_PIN           PB5
+
+
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW180_DEG
+#define MPU6000_CS_PIN          PA15
+#define MPU6000_SPI_BUS         BUS_SPI3
+
+#define USE_IMU_BMI270
+#define IMU_BMI270_ALIGN        CW180_DEG
+#define BMI270_CS_PIN           PA15
+#define BMI270_SPI_BUS          BUS_SPI3
+
+#define USE_IMU_ICM42605
+#define IMU_ICM42605_ALIGN      CW180_DEG
+#define ICM42605_CS_PIN         PA15
+#define ICM42605_SPI_BUS        BUS_SPI3
+
+// *************** I2C/Baro/Mag *********************
+#define USE_I2C
+#define USE_I2C_DEVICE_1
+#define I2C1_SCL                PB8
+#define I2C1_SDA                PB9
+
+#define USE_BARO
+#define BARO_I2C_BUS            BUS_I2C1
+#define USE_BARO_BMP280
+#define USE_BARO_DPS310
+#define USE_BARO_MS5611
+
+#define USE_MAG
+#define MAG_I2C_BUS             BUS_I2C1
+#define USE_MAG_HMC5883
+#define USE_MAG_QMC5883
+#define USE_MAG_IST8310
+#define USE_MAG_IST8308
+#define USE_MAG_MAG3110
+#define USE_MAG_LIS3MDL
+
+#define TEMPERATURE_I2C_BUS     BUS_I2C1
+
+#define PITOT_I2C_BUS           BUS_I2C1
+
+#define USE_RANGEFINDER
+#define RANGEFINDER_I2C_BUS     BUS_I2C1
+#define BNO055_I2C_BUS          BUS_I2C1
+
+// *************** FLASH **************************
+#define M25P16_CS_PIN           PB12
+#define M25P16_SPI_BUS          BUS_SPI2
+#define USE_FLASHFS
+#define USE_FLASH_M25P16
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+
+#define USE_SPI_DEVICE_2
+#define SPI2_SCK_PIN            PB13
+#define SPI2_MISO_PIN   	    PB14
+#define SPI2_MOSI_PIN   	    PB15
+
+// *************** OSD *****************************
+#define USE_SPI_DEVICE_1
+#define SPI1_SCK_PIN            PA5
+#define SPI1_MISO_PIN           PA6
+#define SPI1_MOSI_PIN           PA7
+
+#define USE_MAX7456
+#define MAX7456_SPI_BUS         BUS_SPI1
+#define MAX7456_CS_PIN          PA4
+
+// *************** UART *****************************
+#define USE_VCP
+
+#define USE_UART1
+#define UART1_RX_PIN            PB7
+#define UART1_TX_PIN            PB6
+
+#define USE_UART2
+#define UART2_RX_PIN            PA3
+#define UART2_TX_PIN            PA2
+
+#define USE_UART3
+#define UART3_RX_PIN            PC11
+#define UART3_TX_PIN            PC10
+
+#define USE_UART4
+#define UART4_RX_PIN            PA1
+#define UART4_TX_PIN            PA0
+
+#define USE_UART5
+#define UART5_RX_PIN            PD2
+#define UART5_TX_PIN            PC12
+
+#define USE_UART6
+#define UART6_RX_PIN            PC7
+#define UART6_TX_PIN            PC6
+
+#define SERIAL_PORT_COUNT       7
+
+#define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER       SERIALRX_SBUS
+#define SERIALRX_UART           SERIAL_PORT_USART2
+
+// *************** ADC *****************************
+#define USE_ADC
+#define ADC_INSTANCE                ADC1
+#define ADC1_DMA_STREAM             DMA2_Stream0
+#define ADC_CHANNEL_1_PIN           PC1
+#define ADC_CHANNEL_2_PIN           PC2
+#define ADC_CHANNEL_3_PIN           PC0
+
+#define VBAT_ADC_CHANNEL            ADC_CHN_1
+#define RSSI_ADC_CHANNEL            ADC_CHN_2
+#define CURRENT_METER_ADC_CHANNEL   ADC_CHN_3
+
+
+#define DEFAULT_FEATURES            (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX)
+
+
+
+// *************** LEDSTRIP ************************
+#define USE_LED_STRIP
+#define WS2811_PIN                  PB1
+
+
+
+#define USE_SERIAL_4WAY_BLHELI_INTERFACE
+
+#define TARGET_IO_PORTA         0xffff
+#define TARGET_IO_PORTB         0xffff
+#define TARGET_IO_PORTC         0xffff
+#define TARGET_IO_PORTD         (BIT(2))
+
+
+#define MAX_PWM_OUTPUT_PORTS        6
+
+#define USE_DSHOT
+#define USE_ESC_SENSOR

From 3e6fa98dcd5b46209a0656a603ed5038375a12be Mon Sep 17 00:00:00 2001
From: YI-BOYANG <46438966+YI-BOYANG@users.noreply.github.com>
Date: Fri, 23 Jun 2023 16:59:11 +0800
Subject: [PATCH 19/94] add GEPRCF722 target

---
 src/main/target/GEPRCF722/CMakeLists.txt |   1 +
 src/main/target/GEPRCF722/target.c       |  44 ++++++
 src/main/target/GEPRCF722/target.h       | 173 +++++++++++++++++++++++
 3 files changed, 218 insertions(+)
 create mode 100644 src/main/target/GEPRCF722/CMakeLists.txt
 create mode 100644 src/main/target/GEPRCF722/target.c
 create mode 100644 src/main/target/GEPRCF722/target.h

diff --git a/src/main/target/GEPRCF722/CMakeLists.txt b/src/main/target/GEPRCF722/CMakeLists.txt
new file mode 100644
index 00000000000..2d7f6c7d95f
--- /dev/null
+++ b/src/main/target/GEPRCF722/CMakeLists.txt
@@ -0,0 +1 @@
+target_stm32f722xe(GEPRCF722)
diff --git a/src/main/target/GEPRCF722/target.c b/src/main/target/GEPRCF722/target.c
new file mode 100644
index 00000000000..3514ca283ff
--- /dev/null
+++ b/src/main/target/GEPRCF722/target.c
@@ -0,0 +1,44 @@
+/*
+* This file is part of INAV Project.
+*
+* Cleanflight is free software: you can redistribute it 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.
+*
+* Cleanflight 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include <stdint.h>
+
+#include "platform.h"
+
+#include "drivers/bus.h"
+#include "drivers/io.h"
+#include "drivers/pwm_mapping.h"
+#include "drivers/timer.h"
+#include "drivers/sensor.h"
+
+
+timerHardware_t timerHardware[] = {
+    DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM,                         0, 0),
+
+    DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
+    DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
+    DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
+    DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
+    DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
+    DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
+
+    DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED,                         0, 0),
+
+    
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
diff --git a/src/main/target/GEPRCF722/target.h b/src/main/target/GEPRCF722/target.h
new file mode 100644
index 00000000000..e4f3b9c72da
--- /dev/null
+++ b/src/main/target/GEPRCF722/target.h
@@ -0,0 +1,173 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * Cleanflight is free software: you can redistribute it 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.
+ *
+ * Cleanflight 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#define TARGET_BOARD_IDENTIFIER "GEPR"
+
+#define USBD_PRODUCT_STRING     "GEPRCF722"
+
+#define LED0                    PA13
+#define LED1                    PA14
+
+#define BEEPER                  PC13
+#define BEEPER_INVERTED
+
+// *************** Gyro & ACC **********************
+
+#define USE_SPI
+#define USE_SPI_DEVICE_3
+
+#define SPI3_SCK_PIN            PC10
+#define SPI3_MISO_PIN           PC11
+#define SPI3_MOSI_PIN           PC12
+
+
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW270_DEG
+
+#define MPU6000_CS_PIN          PA15
+#define MPU6000_SPI_BUS         BUS_SPI3
+
+#define USE_IMU_BMI270
+#define IMU_BMI270_ALIGN        CW180_DEG
+#define BMI270_CS_PIN           PA15
+#define BMI270_SPI_BUS          BUS_SPI3
+
+#define USE_IMU_ICM42605
+#define IMU_ICM42605_ALIGN      CW180_DEG
+#define ICM42605_CS_PIN         PA15
+#define ICM42605_SPI_BUS        BUS_SPI3
+
+// *************** I2C/Baro/Mag *********************
+#define USE_I2C
+#define USE_I2C_DEVICE_1
+#define I2C1_SCL                PB8
+#define I2C1_SDA                PB9
+
+#define USE_BARO
+#define BARO_I2C_BUS            BUS_I2C1
+#define USE_BARO_BMP280
+#define USE_BARO_DPS310
+#define USE_BARO_MS5611
+
+#define USE_MAG
+#define MAG_I2C_BUS             BUS_I2C1
+#define USE_MAG_HMC5883
+#define USE_MAG_QMC5883
+#define USE_MAG_IST8310
+#define USE_MAG_IST8308
+#define USE_MAG_MAG3110
+#define USE_MAG_LIS3MDL
+
+#define TEMPERATURE_I2C_BUS     BUS_I2C1
+
+#define PITOT_I2C_BUS           BUS_I2C1
+
+#define USE_RANGEFINDER
+#define RANGEFINDER_I2C_BUS     BUS_I2C1
+#define BNO055_I2C_BUS          BUS_I2C1
+
+// *************** FLASH **************************
+#define M25P16_CS_PIN           PB12
+#define M25P16_SPI_BUS          BUS_SPI2
+#define USE_FLASHFS
+#define USE_FLASH_M25P16
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+
+#define USE_SPI_DEVICE_2
+#define SPI2_SCK_PIN            PB13
+#define SPI2_MISO_PIN   	    PB14
+#define SPI2_MOSI_PIN   	    PB15
+
+// *************** OSD *****************************
+#define USE_SPI_DEVICE_1
+#define SPI1_SCK_PIN            PA5
+#define SPI1_MISO_PIN           PA6
+#define SPI1_MOSI_PIN           PA7
+
+#define USE_MAX7456
+#define MAX7456_SPI_BUS         BUS_SPI1
+#define MAX7456_CS_PIN          PB2
+// *************** UART *****************************
+#define USE_VCP
+
+#define USE_UART1
+#define UART1_RX_PIN            PA10
+#define UART1_TX_PIN            PA9
+
+#define USE_UART2
+#define UART2_RX_PIN            PA3
+#define UART2_TX_PIN            PA2
+
+#define USE_UART3
+#define UART3_RX_PIN            PB11
+#define UART3_TX_PIN            PB10
+
+#define USE_UART4
+#define UART4_RX_PIN            PA1
+#define UART4_TX_PIN            PA0
+
+#define USE_UART6
+#define UART6_RX_PIN            PC7
+#define UART6_TX_PIN            PC6
+
+#define SERIAL_PORT_COUNT       6
+
+#define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER       SERIALRX_SBUS
+#define SERIALRX_UART           SERIAL_PORT_USART2
+
+// *************** ADC *****************************
+#define USE_ADC
+#define ADC_INSTANCE                ADC1
+#define ADC1_DMA_STREAM             DMA2_Stream0
+#define ADC_CHANNEL_1_PIN           PC0
+#define ADC_CHANNEL_2_PIN           PC2
+#define ADC_CHANNEL_3_PIN           PC1
+
+#define VBAT_ADC_CHANNEL            ADC_CHN_1
+#define RSSI_ADC_CHANNEL            ADC_CHN_2
+#define CURRENT_METER_ADC_CHANNEL   ADC_CHN_3
+
+
+#define DEFAULT_FEATURES            (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX)
+
+
+// ************PINIO to other
+#define PINIO1_PIN                  PC8                //Enable vsw
+#define PINIO2_PIN                  PC9
+
+
+
+// *************** LEDSTRIP ************************
+#define USE_LED_STRIP
+#define WS2811_PIN                  PA8
+
+
+
+#define USE_SERIAL_4WAY_BLHELI_INTERFACE
+
+#define TARGET_IO_PORTA             0xffff
+#define TARGET_IO_PORTB             0xffff
+#define TARGET_IO_PORTC             0xffff
+#define TARGET_IO_PORTD             (BIT(2))
+
+#define MAX_PWM_OUTPUT_PORTS        6
+
+#define USE_DSHOT
+#define USE_ESC_SENSOR

From a24828bd9d5ca3418a7345a3a5f6649b3796b4d7 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Tue, 11 Jul 2023 15:26:20 +0100
Subject: [PATCH 20/94] Update navigation_multicopter.c

---
 src/main/navigation/navigation_multicopter.c | 22 +++++++++-----------
 1 file changed, 10 insertions(+), 12 deletions(-)

diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c
index 864cde8b334..7d42d57c128 100644
--- a/src/main/navigation/navigation_multicopter.c
+++ b/src/main/navigation/navigation_multicopter.c
@@ -869,20 +869,21 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
 
     /* Attempt to stabilise */
     rcCommand[YAW] = 0;
+    rcCommand[ROLL] = 0;
+    rcCommand[PITCH] = 0;
 
-    if ((posControl.flags.estAltStatus < EST_USABLE)) {
-        /* Sensors has gone haywire, attempt to land regardless */
-        if (failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_DROP_IT) {
-            rcCommand[THROTTLE] = getThrottleIdleValue();
-        }
-        else {
-            rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
-        }
+    /* Set default throttle to 90% of hover throttle if failsafe LAND throttle not set from default.
+     * Will be overwritten if better altitude descent control available */
+    rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle == SETTING_FAILSAFE_THROTTLE_DEFAULT ?
+                          1000 + 0.9 * (currentBatteryProfile->nav.mc.hover_throttle - 1000) : currentBatteryProfile->failsafe_throttle;
 
+    /* Altitude sensors gone haywire, attempt to land regardless */
+    if ((posControl.flags.estAltStatus < EST_USABLE) && failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_DROP_IT) {
+        rcCommand[THROTTLE] = getThrottleIdleValue();
         return;
     }
 
-    // Normal sensor data
+    // Normal sensor data available, use controlled landing descent
     if (posControl.flags.verticalPositionDataNew) {
         const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
         previousTimePositionUpdate = currentTimeUs;
@@ -908,9 +909,6 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
     // Hold position if possible
     if ((posControl.flags.estPosStatus >= EST_USABLE)) {
         applyMulticopterPositionController(currentTimeUs);
-    } else {
-        rcCommand[ROLL] = 0;
-        rcCommand[PITCH] = 0;
     }
 }
 

From 74635e02ffe46c15eda624bdf8001393a80fcdd2 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Tue, 11 Jul 2023 15:54:14 +0100
Subject: [PATCH 21/94] Update navigation_multicopter.c

---
 src/main/navigation/navigation_multicopter.c | 12 +++++++-----
 1 file changed, 7 insertions(+), 5 deletions(-)

diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c
index 7d42d57c128..03f293f106a 100644
--- a/src/main/navigation/navigation_multicopter.c
+++ b/src/main/navigation/navigation_multicopter.c
@@ -878,8 +878,10 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
                           1000 + 0.9 * (currentBatteryProfile->nav.mc.hover_throttle - 1000) : currentBatteryProfile->failsafe_throttle;
 
     /* Altitude sensors gone haywire, attempt to land regardless */
-    if ((posControl.flags.estAltStatus < EST_USABLE) && failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_DROP_IT) {
-        rcCommand[THROTTLE] = getThrottleIdleValue();
+    if (posControl.flags.estAltStatus < EST_USABLE) {
+        if (failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_DROP_IT) {
+            rcCommand[THROTTLE] = getThrottleIdleValue();
+        }
         return;
     }
 
@@ -893,6 +895,9 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
             updateClimbRateToAltitudeController(-1.0f * navConfig()->general.emerg_descent_rate, ROC_TO_ALT_NORMAL);
             updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate);
             updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate);
+
+            // Update throttle
+            rcCommand[THROTTLE] = posControl.rcAdjustment[THROTTLE];
         }
         else {
             // due to some glitch position update has not occurred in time, reset altitude controller
@@ -903,9 +908,6 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
         posControl.flags.verticalPositionDataConsumed = true;
     }
 
-    // Update throttle controller
-    rcCommand[THROTTLE] = posControl.rcAdjustment[THROTTLE];
-
     // Hold position if possible
     if ((posControl.flags.estPosStatus >= EST_USABLE)) {
         applyMulticopterPositionController(currentTimeUs);

From 4fcf70aa5ca36a9d7212d7cdc8b9b5b704ea6924 Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Sun, 9 Jul 2023 18:33:06 -0500
Subject: [PATCH 22/94] Extend from 4 ADC channels to 6 (Matek H743 and others)

---
 src/main/drivers/adc.c         | 38 ++++++++++++++++++++++++++++++++--
 src/main/drivers/adc.h         |  4 +++-
 src/main/drivers/pwm_mapping.c | 10 +++++++++
 3 files changed, 49 insertions(+), 3 deletions(-)

diff --git a/src/main/drivers/adc.c b/src/main/drivers/adc.c
index eeb65acfab9..679723e0a83 100644
--- a/src/main/drivers/adc.c
+++ b/src/main/drivers/adc.c
@@ -47,6 +47,13 @@
 #ifndef ADC_CHANNEL_4_INSTANCE
 #define ADC_CHANNEL_4_INSTANCE  ADC_INSTANCE
 #endif
+#ifndef ADC_CHANNEL_5_INSTANCE
+#define ADC_CHANNEL_5_INSTANCE  ADC_INSTANCE
+#endif
+#ifndef ADC_CHANNEL_6_INSTANCE
+#define ADC_CHANNEL_6_INSTANCE  ADC_INSTANCE
+#endif
+
 
 #ifdef USE_ADC
 
@@ -99,7 +106,7 @@ uint16_t adcGetChannel(uint8_t function)
     }
 }
 
-#if defined(ADC_CHANNEL_1_PIN) || defined(ADC_CHANNEL_2_PIN) || defined(ADC_CHANNEL_3_PIN) || defined(ADC_CHANNEL_4_PIN)
+#if defined(ADC_CHANNEL_1_PIN) || defined(ADC_CHANNEL_2_PIN) || defined(ADC_CHANNEL_3_PIN) || defined(ADC_CHANNEL_4_PIN) || defined(ADC_CHANNEL_5_PIN) || defined(ADC_CHANNEL_6_PIN)
 static bool isChannelInUse(int channel)
 {
     for (int i = 0; i < ADC_FUNCTION_COUNT; i++) {
@@ -111,7 +118,7 @@ static bool isChannelInUse(int channel)
 }
 #endif
 
-#if !defined(ADC_CHANNEL_1_PIN) || !defined(ADC_CHANNEL_2_PIN) || !defined(ADC_CHANNEL_3_PIN) || !defined(ADC_CHANNEL_4_PIN)
+#if !defined(ADC_CHANNEL_1_PIN) || !defined(ADC_CHANNEL_2_PIN) || !defined(ADC_CHANNEL_3_PIN) || !defined(ADC_CHANNEL_4_PIN) || !defined(ADC_CHANNEL_5_PIN) || !defined(ADC_CHANNEL_6_PIN)
 static void disableChannelMapping(int channel)
 {
     for (int i = 0; i < ADC_FUNCTION_COUNT; i++) {
@@ -192,6 +199,33 @@ void adcInit(drv_adc_config_t *init)
     disableChannelMapping(ADC_CHN_4);
 #endif
 
+#ifdef ADC_CHANNEL_5_PIN
+    if (isChannelInUse(ADC_CHN_5)) {
+        adcConfig[ADC_CHN_5].adcDevice = adcDeviceByInstance(ADC_CHANNEL_5_INSTANCE);
+        if (adcConfig[ADC_CHN_5].adcDevice != ADCINVALID) {
+            adcConfig[ADC_CHN_5].tag = IO_TAG(ADC_CHANNEL_5_PIN);
+#if defined(USE_ADC_AVERAGING)
+            activeChannelCount[adcConfig[ADC_CHN_5].adcDevice] += 1;
+#endif
+        }
+    }
+#else
+    disableChannelMapping(ADC_CHN_5);
+#endif
+
+#ifdef ADC_CHANNEL_6_PIN
+    if (isChannelInUse(ADC_CHN_6)) {
+        adcConfig[ADC_CHN_6].adcDevice = adcDeviceByInstance(ADC_CHANNEL_6_INSTANCE);
+        if (adcConfig[ADC_CHN_6].adcDevice != ADCINVALID) {
+            adcConfig[ADC_CHN_6].tag = IO_TAG(ADC_CHANNEL_6_PIN);
+#if defined(USE_ADC_AVERAGING)
+            activeChannelCount[adcConfig[ADC_CHN_6].adcDevice] += 1;
+#endif
+        }
+    }
+#else
+    disableChannelMapping(ADC_CHN_6);
+#endif
 
     adcHardwareInit(init);
 }
diff --git a/src/main/drivers/adc.h b/src/main/drivers/adc.h
index 5ba4b999da0..1749c9fa63e 100644
--- a/src/main/drivers/adc.h
+++ b/src/main/drivers/adc.h
@@ -33,7 +33,9 @@ typedef enum {
     ADC_CHN_2,
     ADC_CHN_3,
     ADC_CHN_4,
-    ADC_CHN_MAX = ADC_CHN_4,
+	ADC_CHN_5,
+	ADC_CHN_6,
+    ADC_CHN_MAX = ADC_CHN_6,
     ADC_CHN_COUNT
 } adcChannel_e;
 
diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c
index eb14d6772b6..e3acc6d1a0f 100644
--- a/src/main/drivers/pwm_mapping.c
+++ b/src/main/drivers/pwm_mapping.c
@@ -195,6 +195,16 @@ static bool checkPwmTimerConflicts(const timerHardware_t *timHw)
         return true;
     }
 #endif
+#if defined(ADC_CHANNEL_5_PIN)
+    if (timHw->tag == IO_TAG(ADC_CHANNEL_6_PIN)) {
+        return true;
+    }
+#endif
+#if defined(ADC_CHANNEL_6_PIN)
+    if (timHw->tag == IO_TAG(ADC_CHANNEL_6_PIN)) {
+        return true;
+    }
+#endif
 #endif
 
     return false;

From 571fcb53bff9514ae84582c1d457520d910acc90 Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Mon, 17 Jul 2023 21:05:00 -0500
Subject: [PATCH 23/94] pwm_mapping.c: Fix typo ADC_CHANNEL_5, not
 ADC_CHANNEL_6

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

diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c
index e3acc6d1a0f..4fc5a00b83e 100644
--- a/src/main/drivers/pwm_mapping.c
+++ b/src/main/drivers/pwm_mapping.c
@@ -196,7 +196,7 @@ static bool checkPwmTimerConflicts(const timerHardware_t *timHw)
     }
 #endif
 #if defined(ADC_CHANNEL_5_PIN)
-    if (timHw->tag == IO_TAG(ADC_CHANNEL_6_PIN)) {
+    if (timHw->tag == IO_TAG(ADC_CHANNEL_5_PIN)) {
         return true;
     }
 #endif

From 4b30a1669bbab296e38d95cb805688d7ddd43625 Mon Sep 17 00:00:00 2001
From: jmajew <jmajewsk@meil.pw.edu.pl>
Date: Mon, 24 Jul 2023 01:26:00 +0200
Subject: [PATCH 24/94] dlvr-l10d airspeed sensor driver added

---
 src/main/CMakeLists.txt           |  2 ++
 src/main/drivers/bus.h            |  1 +
 src/main/fc/settings.yaml         |  2 +-
 src/main/sensors/pitotmeter.c     | 14 ++++++++++++++
 src/main/sensors/pitotmeter.h     |  1 +
 src/main/target/common.h          |  1 +
 src/main/target/common_hardware.c | 10 +++++++++-
 7 files changed, 29 insertions(+), 2 deletions(-)
 mode change 100644 => 100755 src/main/CMakeLists.txt
 mode change 100644 => 100755 src/main/fc/settings.yaml
 mode change 100644 => 100755 src/main/sensors/pitotmeter.c
 mode change 100644 => 100755 src/main/sensors/pitotmeter.h
 mode change 100644 => 100755 src/main/target/common.h

diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt
old mode 100644
new mode 100755
index 6ebf22a3613..9b637857554
--- a/src/main/CMakeLists.txt
+++ b/src/main/CMakeLists.txt
@@ -208,6 +208,8 @@ main_sources(COMMON_SRC
     drivers/pitotmeter/pitotmeter_adc.h
     drivers/pitotmeter/pitotmeter_ms4525.c
     drivers/pitotmeter/pitotmeter_ms4525.h
+	drivers/pitotmeter/pitotmeter_dlvr_l10d.c
+    drivers/pitotmeter/pitotmeter_dlvr_l10d.h	
     drivers/pitotmeter/pitotmeter_msp.c
     drivers/pitotmeter/pitotmeter_msp.h
     drivers/pitotmeter/pitotmeter_virtual.c
diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h
index c27f21d6006..eca213b7218 100644
--- a/src/main/drivers/bus.h
+++ b/src/main/drivers/bus.h
@@ -138,6 +138,7 @@ typedef enum {
 
     /* Other hardware */
     DEVHW_MS4525,       // Pitot meter
+    DEVHW_DLVR,         // Pitot meter
     DEVHW_M25P16,       // SPI NOR flash
     DEVHW_W25N01G,      // SPI 128MB flash
     DEVHW_UG2864,       // I2C OLED display
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
old mode 100644
new mode 100755
index c5f6acfd799..fdff4f1cb1b
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -19,7 +19,7 @@ tables:
     values: ["NONE", "AUTO", "BMP085", "MS5611", "BMP280", "MS5607", "LPS25H", "SPL06", "BMP388", "DPS310", "B2SMPB", "MSP", "FAKE"]
     enum: baroSensor_e
   - name: pitot_hardware
-    values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE", "MSP"]
+    values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE", "MSP", "DLVR"]
     enum: pitotSensor_e
   - name: receiver_type
     values: ["NONE", "SERIAL", "MSP", "SIM (SITL)"]
diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c
old mode 100644
new mode 100755
index d41b5e4cd3b..acdf51f188d
--- a/src/main/sensors/pitotmeter.c
+++ b/src/main/sensors/pitotmeter.c
@@ -31,6 +31,7 @@
 
 #include "drivers/pitotmeter/pitotmeter.h"
 #include "drivers/pitotmeter/pitotmeter_ms4525.h"
+#include "drivers/pitotmeter/pitotmeter_dlvr_l10d.h"
 #include "drivers/pitotmeter/pitotmeter_adc.h"
 #include "drivers/pitotmeter/pitotmeter_msp.h"
 #include "drivers/pitotmeter/pitotmeter_virtual.h"
@@ -86,6 +87,19 @@ bool pitotDetect(pitotDev_t *dev, uint8_t pitotHardwareToUse)
             }
             FALLTHROUGH;
 
+        case PITOT_DLVR:
+#ifdef USE_PITOT_DLVR
+            if (dlvrDetect(dev)) {
+                pitotHardware = PITOT_DLVR;
+                break;
+            }
+#endif
+            /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
+            if (pitotHardwareToUse != PITOT_AUTODETECT) {
+                break;
+            }
+            FALLTHROUGH;
+			
         case PITOT_ADC:
 #if defined(USE_ADC) && defined(USE_PITOT_ADC)
             if (adcPitotDetect(dev)) {
diff --git a/src/main/sensors/pitotmeter.h b/src/main/sensors/pitotmeter.h
old mode 100644
new mode 100755
index 4f9bccb046c..96059cb718e
--- a/src/main/sensors/pitotmeter.h
+++ b/src/main/sensors/pitotmeter.h
@@ -31,6 +31,7 @@ typedef enum {
     PITOT_VIRTUAL = 4,
     PITOT_FAKE = 5,
     PITOT_MSP = 6,
+	PITOT_DLVR = 7,
 } pitotSensor_e;
 
 #define PITOT_MAX  PITOT_FAKE
diff --git a/src/main/target/common.h b/src/main/target/common.h
old mode 100644
new mode 100755
index 8b7c8b28970..000e1d4707c
--- a/src/main/target/common.h
+++ b/src/main/target/common.h
@@ -98,6 +98,7 @@
 #define USE_PITOT
 #define USE_PITOT_MS4525
 #define USE_PITOT_MSP
+#define USE_PITOT_DLVR
 
 #define USE_1WIRE
 #define USE_1WIRE_DS2482
diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c
index 3f674d8df2b..ab69ac3fa6c 100755
--- a/src/main/target/common_hardware.c
+++ b/src/main/target/common_hardware.c
@@ -357,10 +357,18 @@
 #endif
 
 #if defined(USE_PITOT_MS4525) && defined(MS4525_I2C_BUS)
-    BUSDEV_REGISTER_I2C(busdev_ms5425,      DEVHW_MS4525,       MS4525_I2C_BUS,     0x28,               NONE,           DEVFLAGS_USE_RAW_REGISTERS,  0);    // Requires 0xFF to passthrough
+    BUSDEV_REGISTER_I2C(busdev_ms4525,      DEVHW_MS4525,       MS4525_I2C_BUS,     0x28,               NONE,           DEVFLAGS_USE_RAW_REGISTERS,  0);    // Requires 0xFF to passthrough
 #endif
 
 
+#if defined(PITOT_I2C_BUS) && !defined(DLVR_I2C_BUS)
+    #define DLVR_I2C_BUS PITOT_I2C_BUS
+#endif
+
+#if defined(USE_PITOT_DLVR) && defined(DLVR_I2C_BUS)
+    BUSDEV_REGISTER_I2C(busdev_dlvr,        DEVHW_DLVR,         DLVR_I2C_BUS,        0x28,              NONE,           DEVFLAGS_USE_RAW_REGISTERS,  0);    // Requires 0xFF to passthrough
+#endif
+
 /** OTHER HARDWARE **/
 
 #if defined(USE_MAX7456)

From 46df1ab221e67776b1b8a5eb659918ed5b75fdb0 Mon Sep 17 00:00:00 2001
From: jmajew <jmajewsk@meil.pw.edu.pl>
Date: Mon, 24 Jul 2023 01:27:23 +0200
Subject: [PATCH 25/94] missing files added

---
 .../drivers/pitotmeter/pitotmeter_dlvr_l10d.c | 190 ++++++++++++++++++
 .../drivers/pitotmeter/pitotmeter_dlvr_l10d.h |  20 ++
 2 files changed, 210 insertions(+)
 create mode 100755 src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
 create mode 100755 src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.h

diff --git a/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c b/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
new file mode 100755
index 00000000000..df39a5af315
--- /dev/null
+++ b/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
@@ -0,0 +1,190 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it 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.
+ *
+ * Cleanflight 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include <platform.h>
+#include <build/debug.h>
+
+#include "common/utils.h"
+#include "common/maths.h"
+#include "drivers/bus_i2c.h"
+#include "drivers/time.h"
+#include "drivers/pitotmeter/pitotmeter.h"
+
+// MS4525, Standard address 0x28
+#define DLVR_L10D_ADDR                 0x28
+
+// //---------------------------------------------------
+// //---------------------------------------------------
+// #define C_TO_KELVIN(temp) (temp + 273.15f)
+// #define KELVIN_TO_C(temp) (temp - 273.15f)
+// #define F_TO_KELVIN(temp) C_TO_KELVIN(((temp - 32) * 5/9))
+
+// #define M_PER_SEC_TO_KNOTS 1.94384449f
+// #define KNOTS_TO_M_PER_SEC (1/M_PER_SEC_TO_KNOTS)
+
+// #define KM_PER_HOUR_TO_M_PER_SEC 0.27777778f
+
+// // Gas Constant is from Aerodynamics for Engineering Students, Third Edition, E.L.Houghton and N.B.Carruthers
+// #define ISA_GAS_CONSTANT 287.26f
+// #define ISA_LAPSE_RATE 0.0065f
+
+// // Standard Sea Level values
+// // Ref: https://en.wikipedia.org/wiki/Standard_sea_level
+// #define SSL_AIR_DENSITY         1.225f // kg/m^3
+// #define SSL_AIR_PRESSURE 101325.01576f // Pascal
+// #define SSL_AIR_TEMPERATURE    288.15f // K
+
+// #define INCH_OF_H2O_TO_PASCAL 248.84f
+// //---------------------------------------------------
+// //---------------------------------------------------
+
+#define INCH_H2O_TO_PA      249.09
+#define RANGE_INCH_H2O      10
+#define DLVR_OFFSET         8192.0f
+#define DLVR_SCALE          16384.0f
+
+typedef struct __attribute__ ((__packed__)) dlvrCtx_s {
+    bool     dataValid;
+    uint32_t dlvr_ut;
+    uint32_t dlvr_up;
+} dlvrCtx_t;
+
+STATIC_ASSERT(sizeof(dlvrCtx_t) < BUS_SCRATCHPAD_MEMORY_SIZE, busDevice_scratchpad_memory_too_small);
+
+static bool dlvr_start(pitotDev_t * pitot)
+{
+    uint8_t rxbuf[1];
+    bool ack = busReadBuf(pitot->busDev, 0xFF, rxbuf, 1);
+    return ack;
+}
+
+static bool dlvr_read(pitotDev_t * pitot)
+{
+    uint8_t rxbuf1[4];
+    uint8_t rxbuf2[4];
+
+    dlvrCtx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev);
+    ctx->dataValid = false;
+
+    if (!busReadBuf(pitot->busDev, 0xFF, rxbuf1, 4)) {
+        return false;
+    }
+
+    if (!busReadBuf(pitot->busDev, 0xFF, rxbuf2, 4)) {
+        return false;
+    }
+
+    const uint8_t status = ((rxbuf1[0] & 0xC0) >> 6);
+    if (status == 2 || status == 3) {
+        return false;
+    }
+
+    int16_t dP_raw1, dT_raw1;
+    int16_t dP_raw2, dT_raw2;
+
+    dP_raw1 = 0x3FFF & ((rxbuf1[0] << 8) + rxbuf1[1]);
+    dT_raw1 = (0xFFE0 & ((rxbuf1[2] << 8) + rxbuf1[3])) >> 5;
+    dP_raw2 = 0x3FFF & ((rxbuf2[0] << 8) + rxbuf2[1]);
+    dT_raw2 = (0xFFE0 & ((rxbuf2[2] << 8) + rxbuf2[3])) >> 5;
+
+    // reject any double reads where the value has shifted in the upper more than 0xFF
+    if (ABS(dP_raw1 - dP_raw2) > 0xFF || ABS(dT_raw1 - dT_raw2) > 0xFF) {
+        return false;
+    }
+
+    // Data valid, update ut/up values
+    ctx->dataValid = true;
+    ctx->dlvr_up = (dP_raw1 + dP_raw2) / 2;
+    ctx->dlvr_ut = (dT_raw1 + dT_raw2) / 2;
+    return true;
+}
+
+static void dlvr_calculate(pitotDev_t * pitot, float *pressure, float *temperature)
+{
+    dlvrCtx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev);
+
+    // const float P_max = 1.0f;
+    // const float P_min = -P_max;
+    // const float PSI_to_Pa = 6894.757f;
+
+    // //float dP = ctx->ms4525_up * 10.0f * 0.1052120688f;
+    // const float dP_psi = -((ctx->dlvr_up - 0.1f * 16383) * (P_max - P_min) / (0.8f * 16383) + P_min);
+    // float dP = dP_psi * PSI_to_Pa;
+    // float T  = C_TO_KELVIN((float)(200.0f * (int32_t)ctx->dlvr_ut) / 2047.0f - 50.0f);
+
+    // if (pressure) {
+    //     *pressure = dP;    // Pa
+    // }
+
+    // if (temperature) {
+    //     *temperature = T; // K
+    // }
+
+    // //-----------------------------------------------------------------------------
+    float press_h2o = 1.25f *  2.0f * RANGE_INCH_H2O  * ((ctx->dlvr_up - DLVR_OFFSET) / DLVR_SCALE);
+    float temp = ctx->dlvr_ut * (200.0f / 2047.0f) - 50.0f;
+
+    if ((press_h2o > RANGE_INCH_H2O) || (press_h2o < -RANGE_INCH_H2O)) {
+        //Debug("DLVR: Out of range pressure %f", press_h2o);
+        return;
+    }
+
+    if (pressure) {
+        *pressure = INCH_H2O_TO_PA * press_h2o;; //dP;    // Pa
+    }
+
+    if (temperature) {
+        *temperature = temp; //T; // K
+    }
+}
+
+bool dlvrDetect(pitotDev_t * pitot)
+{
+    uint8_t rxbuf[4];
+    bool ack = false;
+
+    pitot->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_DLVR, 0, OWNER_AIRSPEED);
+    if (pitot->busDev == NULL) {
+        return false;
+    }
+
+    // Read twice to fix:
+    // Sending a start-stop condition without any transitions on the SCL line (no clock pulses in between) creates a
+    // communication error for the next communication, even if the next start condition is correct and the clock pulse is applied.
+    // An additional start condition must be sent, which results in restoration of proper communication.
+    ack = busReadBuf(pitot->busDev, 0xFF, rxbuf, 4);
+    ack = busReadBuf(pitot->busDev, 0xFF, rxbuf, 4);
+    if (!ack) {
+        return false;
+    }
+
+    // Initialize busDev data
+    dlvrCtx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev);
+    ctx->dataValid = false;
+    ctx->dlvr_ut = 0;
+    ctx->dlvr_up = 0;
+
+    // Initialize pitotDev object
+    pitot->delay = 10000;
+    pitot->start = dlvr_start;
+    pitot->get = dlvr_read;
+    pitot->calculate = dlvr_calculate;
+    return true;
+}
diff --git a/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.h b/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.h
new file mode 100755
index 00000000000..275ac25c3f4
--- /dev/null
+++ b/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.h
@@ -0,0 +1,20 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it 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.
+ *
+ * Cleanflight 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+bool dlvrDetect(pitotDev_t *pitot);

From 27f2180126b923c68f7d41ab8a43ee6666f027a1 Mon Sep 17 00:00:00 2001
From: jmajew <jmajewsk@meil.pw.edu.pl>
Date: Mon, 24 Jul 2023 08:37:57 +0200
Subject: [PATCH 26/94] wip

---
 .../drivers/pitotmeter/pitotmeter_dlvr_l10d.c | 39 ++++++++++++-------
 1 file changed, 26 insertions(+), 13 deletions(-)

diff --git a/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c b/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
index df39a5af315..9065b99f70a 100755
--- a/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
+++ b/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
@@ -27,8 +27,7 @@
 #include "drivers/time.h"
 #include "drivers/pitotmeter/pitotmeter.h"
 
-// MS4525, Standard address 0x28
-#define DLVR_L10D_ADDR                 0x28
+#define DLVR_L10D_ADDR                 0x28     // this var is not used !!!
 
 // //---------------------------------------------------
 // //---------------------------------------------------
@@ -51,15 +50,16 @@
 // #define SSL_AIR_PRESSURE 101325.01576f // Pascal
 // #define SSL_AIR_TEMPERATURE    288.15f // K
 
-// #define INCH_OF_H2O_TO_PASCAL 248.84f
-// //---------------------------------------------------
-// //---------------------------------------------------
+#define INCH_OF_H2O_TO_PASCAL   248.84f
+//#define INCH_OF_H2O_TO_PASCAL 249.09
+
+#define INCH_H2O_TO_PASCAL(press) (INCH_OF_H2O_TO_PASCAL * press)
 
-#define INCH_H2O_TO_PA      249.09
 #define RANGE_INCH_H2O      10
 #define DLVR_OFFSET         8192.0f
 #define DLVR_SCALE          16384.0f
 
+
 typedef struct __attribute__ ((__packed__)) dlvrCtx_s {
     bool     dataValid;
     uint32_t dlvr_ut;
@@ -91,6 +91,9 @@ static bool dlvr_read(pitotDev_t * pitot)
         return false;
     }
 
+    // status = 00 -> ok new data
+    // status = 10 -> ok dtata stale
+    // else error
     const uint8_t status = ((rxbuf1[0] & 0xC0) >> 6);
     if (status == 2 || status == 3) {
         return false;
@@ -120,6 +123,9 @@ static void dlvr_calculate(pitotDev_t * pitot, float *pressure, float *temperatu
 {
     dlvrCtx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev);
 
+    // //-----------------------------------------------------------------------------
+    // // MS4525 sensor
+    // //-----------------------------------------------------------------------------
     // const float P_max = 1.0f;
     // const float P_min = -P_max;
     // const float PSI_to_Pa = 6894.757f;
@@ -137,21 +143,28 @@ static void dlvr_calculate(pitotDev_t * pitot, float *pressure, float *temperatu
     //     *temperature = T; // K
     // }
 
-    // //-----------------------------------------------------------------------------
-    float press_h2o = 1.25f *  2.0f * RANGE_INCH_H2O  * ((ctx->dlvr_up - DLVR_OFFSET) / DLVR_SCALE);
-    float temp = ctx->dlvr_ut * (200.0f / 2047.0f) - 50.0f;
+    //-----------------------------------------------------------------------------
+    // DLVR-L10D sensor
+    //-----------------------------------------------------------------------------
+    
+    // pressure in inchH2O
+    float dP_inchH2O = 1.25f *  2.0f * RANGE_INCH_H2O  * (((float)ctx->dlvr_up - DLVR_OFFSET) / DLVR_SCALE);    
+
+    // temperature in deg C
+    float T_C = (float)ctx->dlvr_ut * (200.0f / 2047.0f) - 50.0f;     
 
-    if ((press_h2o > RANGE_INCH_H2O) || (press_h2o < -RANGE_INCH_H2O)) {
-        //Debug("DLVR: Out of range pressure %f", press_h2o);
+    // result must fit inside the range
+    if ((dP_inchH2O > RANGE_INCH_H2O) || (dP_inchH2O < -RANGE_INCH_H2O)) {
+        // Debug("DLVR: Out of range pressure %f", dP_inchH2O);
         return;
     }
 
     if (pressure) {
-        *pressure = INCH_H2O_TO_PA * press_h2o;; //dP;    // Pa
+        *pressure = INCH_H2O_TO_PASCAL( dP_inchH2O);    // Pa
     }
 
     if (temperature) {
-        *temperature = temp; //T; // K
+        *temperature = C_TO_KELVIN( T_C); // K
     }
 }
 

From 4fc262594785ba3efe40ae1dbca483f00872ef89 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Mon, 24 Jul 2023 23:25:06 +0100
Subject: [PATCH 27/94] Update navigation_multicopter.c

---
 src/main/navigation/navigation_multicopter.c | 12 ++++--------
 1 file changed, 4 insertions(+), 8 deletions(-)

diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c
index 03f293f106a..75c45f91215 100644
--- a/src/main/navigation/navigation_multicopter.c
+++ b/src/main/navigation/navigation_multicopter.c
@@ -871,11 +871,7 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
     rcCommand[YAW] = 0;
     rcCommand[ROLL] = 0;
     rcCommand[PITCH] = 0;
-
-    /* Set default throttle to 90% of hover throttle if failsafe LAND throttle not set from default.
-     * Will be overwritten if better altitude descent control available */
-    rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle == SETTING_FAILSAFE_THROTTLE_DEFAULT ?
-                          1000 + 0.9 * (currentBatteryProfile->nav.mc.hover_throttle - 1000) : currentBatteryProfile->failsafe_throttle;
+    rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
 
     /* Altitude sensors gone haywire, attempt to land regardless */
     if (posControl.flags.estAltStatus < EST_USABLE) {
@@ -895,9 +891,6 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
             updateClimbRateToAltitudeController(-1.0f * navConfig()->general.emerg_descent_rate, ROC_TO_ALT_NORMAL);
             updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate);
             updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate);
-
-            // Update throttle
-            rcCommand[THROTTLE] = posControl.rcAdjustment[THROTTLE];
         }
         else {
             // due to some glitch position update has not occurred in time, reset altitude controller
@@ -908,6 +901,9 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
         posControl.flags.verticalPositionDataConsumed = true;
     }
 
+    // Update throttle
+    rcCommand[THROTTLE] = posControl.rcAdjustment[THROTTLE];
+
     // Hold position if possible
     if ((posControl.flags.estPosStatus >= EST_USABLE)) {
         applyMulticopterPositionController(currentTimeUs);

From c0b12b5038f7ef1f41ebebfc311c636432733902 Mon Sep 17 00:00:00 2001
From: jmajew <jerzy.majewski@pw.edu.pl>
Date: Tue, 25 Jul 2023 11:09:01 +0200
Subject: [PATCH 28/94] wip

---
 src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c | 12 +++++++-----
 src/main/sensors/pitotmeter.c                      |  7 ++++++-
 2 files changed, 13 insertions(+), 6 deletions(-)

diff --git a/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c b/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
index 9065b99f70a..0684f0af69f 100755
--- a/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
+++ b/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
@@ -53,7 +53,7 @@
 #define INCH_OF_H2O_TO_PASCAL   248.84f
 //#define INCH_OF_H2O_TO_PASCAL 249.09
 
-#define INCH_H2O_TO_PASCAL(press) (INCH_OF_H2O_TO_PASCAL * press)
+#define INCH_H2O_TO_PASCAL(press) (INCH_OF_H2O_TO_PASCAL * (press))
 
 #define RANGE_INCH_H2O      10
 #define DLVR_OFFSET         8192.0f
@@ -91,9 +91,11 @@ static bool dlvr_read(pitotDev_t * pitot)
         return false;
     }
 
-    // status = 00 -> ok new data
-    // status = 10 -> ok dtata stale
-    // else error
+    // status = 00 -> ok, new data
+	// status = 01 -> reserved
+    // status = 10 -> ok, data stale
+    // status = 11 -> error
+	// check the status of the first read:
     const uint8_t status = ((rxbuf1[0] & 0xC0) >> 6);
     if (status == 2 || status == 3) {
         return false;
@@ -160,7 +162,7 @@ static void dlvr_calculate(pitotDev_t * pitot, float *pressure, float *temperatu
     }
 
     if (pressure) {
-        *pressure = INCH_H2O_TO_PASCAL( dP_inchH2O);    // Pa
+        *pressure = INCH_H2O_TO_PASCAL( dP_inchH2O);	// Pa
     }
 
     if (temperature) {
diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c
index acdf51f188d..d4ef5378a98 100755
--- a/src/main/sensors/pitotmeter.c
+++ b/src/main/sensors/pitotmeter.c
@@ -249,7 +249,12 @@ STATIC_PROTOTHREAD(pitotThread)
             //
             // Therefore we shouldn't care about CAS/TAS and only calculate IAS since it's more indicative to the pilot and more useful in calculations
             // It also allows us to use pitot_scale to calibrate the dynamic pressure sensor scale
-            pitot.airSpeed = pitotmeterConfig()->pitot_scale * fast_fsqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / SSL_AIR_DENSITY) * 100;
+            
+            // // with calibration
+            // pitot.airSpeed = pitotmeterConfig()->pitot_scale * fast_fsqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / SSL_AIR_DENSITY) * 100;
+
+            // no calibibration 
+            pitot.airSpeed = pitotmeterConfig()->pitot_scale * fast_fsqrtf( 2.0f * fabsf(pitot.pressure) / SSL_AIR_DENSITY) * 100;
         } else {
             performPitotCalibrationCycle();
             pitot.airSpeed = 0.0f;

From e7a9fe12ec92147d3245eaf9e77d016c400fe5bb Mon Sep 17 00:00:00 2001
From: jmajew <jmajewsk@meil.pw.edu.pl>
Date: Sat, 29 Jul 2023 17:24:06 +0200
Subject: [PATCH 29/94] debug/testing ip

---
 .../drivers/pitotmeter/pitotmeter_dlvr_l10d.c | 12 ++++++----
 src/main/sensors/pitotmeter.c                 | 22 ++++++++++++++-----
 2 files changed, 25 insertions(+), 9 deletions(-)

diff --git a/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c b/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
index 0684f0af69f..5cfc72be77b 100755
--- a/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
+++ b/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
@@ -21,6 +21,7 @@
 #include <platform.h>
 #include <build/debug.h>
 
+#include "common/log.h"
 #include "common/utils.h"
 #include "common/maths.h"
 #include "drivers/bus_i2c.h"
@@ -56,7 +57,7 @@
 #define INCH_H2O_TO_PASCAL(press) (INCH_OF_H2O_TO_PASCAL * (press))
 
 #define RANGE_INCH_H2O      10
-#define DLVR_OFFSET         8192.0f
+#define DLVR_OFFSET         (8192.0f)
 #define DLVR_SCALE          16384.0f
 
 
@@ -150,7 +151,10 @@ static void dlvr_calculate(pitotDev_t * pitot, float *pressure, float *temperatu
     //-----------------------------------------------------------------------------
     
     // pressure in inchH2O
-    float dP_inchH2O = 1.25f *  2.0f * RANGE_INCH_H2O  * (((float)ctx->dlvr_up - DLVR_OFFSET) / DLVR_SCALE);    
+    float dP_inchH2O = 1.25f *  2.0f * RANGE_INCH_H2O  * (((float)ctx->dlvr_up - DLVR_OFFSET) / DLVR_SCALE); 
+
+    LOG_DEBUG( PITOT, "adc = %f; dP_inchH2O =  %f", (double)ctx->dlvr_up, (double)dP_inchH2O);
+   
 
     // temperature in deg C
     float T_C = (float)ctx->dlvr_ut * (200.0f / 2047.0f) - 50.0f;     
@@ -162,7 +166,7 @@ static void dlvr_calculate(pitotDev_t * pitot, float *pressure, float *temperatu
     }
 
     if (pressure) {
-        *pressure = INCH_H2O_TO_PASCAL( dP_inchH2O);	// Pa
+        *pressure = INCH_H2O_TO_PASCAL( dP_inchH2O);   // Pa
     }
 
     if (temperature) {
@@ -197,7 +201,7 @@ bool dlvrDetect(pitotDev_t * pitot)
     ctx->dlvr_up = 0;
 
     // Initialize pitotDev object
-    pitot->delay = 10000;
+    pitot->delay = 5000;    //10000;
     pitot->start = dlvr_start;
     pitot->get = dlvr_read;
     pitot->calculate = dlvr_calculate;
diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c
index d4ef5378a98..bd8cba61aa8 100755
--- a/src/main/sensors/pitotmeter.c
+++ b/src/main/sensors/pitotmeter.c
@@ -47,6 +47,10 @@
 #include "sensors/pitotmeter.h"
 #include "sensors/sensors.h"
 
+
+#include "build/debug.h"
+
+
 #ifdef USE_PITOT
 
 pitot_t pitot;
@@ -201,6 +205,7 @@ STATIC_PROTOTHREAD(pitotThread)
     ptBegin(pitotThread);
 
     static float pitotPressureTmp;
+    static float pitotTemperatureTmp;
     timeUs_t currentTimeUs;
 
     // Init filter
@@ -220,7 +225,7 @@ STATIC_PROTOTHREAD(pitotThread)
             pitot.lastSeenHealthyMs = millis();
         }
 
-        pitot.dev.calculate(&pitot.dev, &pitotPressureTmp, NULL);
+        pitot.dev.calculate(&pitot.dev, &pitotPressureTmp, &pitotTemperatureTmp);
 #ifdef USE_SIMULATOR
         if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) {
             pitotPressureTmp = sq(simulatorData.airSpeed) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE;     
@@ -249,12 +254,19 @@ STATIC_PROTOTHREAD(pitotThread)
             //
             // Therefore we shouldn't care about CAS/TAS and only calculate IAS since it's more indicative to the pilot and more useful in calculations
             // It also allows us to use pitot_scale to calibrate the dynamic pressure sensor scale
+
+            // // no calibibration 
+            // pitot.airSpeed = pitotmeterConfig()->pitot_scale * fast_fsqrtf( 2.0f * fabsf(pitot.pressure) / SSL_AIR_DENSITY) * 100;
             
-            // // with calibration
-            // pitot.airSpeed = pitotmeterConfig()->pitot_scale * fast_fsqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / SSL_AIR_DENSITY) * 100;
+            // with calibration
+            pitot.airSpeed = pitotmeterConfig()->pitot_scale * fast_fsqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / SSL_AIR_DENSITY) * 100;
+
+            // LOG_DEBUG( PITOT, "HELLO!");
+            debug[0] = pitot.pressure * 1000;
+            debug[1] = pitot.pressureZero * 1000;
+            debug[2] = pitot.airSpeed;
+            // debug[3] = pitot.lastSeenHealthyMs;
 
-            // no calibibration 
-            pitot.airSpeed = pitotmeterConfig()->pitot_scale * fast_fsqrtf( 2.0f * fabsf(pitot.pressure) / SSL_AIR_DENSITY) * 100;
         } else {
             performPitotCalibrationCycle();
             pitot.airSpeed = 0.0f;

From ea7e3d0368e31fa4aaa037fd185919d08c87e558 Mon Sep 17 00:00:00 2001
From: jmajew <jerzy.majewski@pw.edu.pl>
Date: Sat, 29 Jul 2023 21:24:10 +0200
Subject: [PATCH 30/94] wip

---
 .../drivers/pitotmeter/pitotmeter_dlvr_l10d.c    |  7 +++++--
 src/main/sensors/pitotmeter.c                    | 16 +++++++++++-----
 src/main/sensors/pitotmeter.h                    |  2 ++
 3 files changed, 18 insertions(+), 7 deletions(-)

diff --git a/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c b/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
index 5cfc72be77b..d51576419b9 100755
--- a/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
+++ b/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
@@ -115,6 +115,8 @@ static bool dlvr_read(pitotDev_t * pitot)
         return false;
     }
 
+    LOG_DEBUG( PITOT, "dP_raw1 = %f; dP_raw2 =  %f", (double)dP_raw1, (double)dP_raw2 );
+
     // Data valid, update ut/up values
     ctx->dataValid = true;
     ctx->dlvr_up = (dP_raw1 + dP_raw2) / 2;
@@ -153,12 +155,13 @@ static void dlvr_calculate(pitotDev_t * pitot, float *pressure, float *temperatu
     // pressure in inchH2O
     float dP_inchH2O = 1.25f *  2.0f * RANGE_INCH_H2O  * (((float)ctx->dlvr_up - DLVR_OFFSET) / DLVR_SCALE); 
 
-    LOG_DEBUG( PITOT, "adc = %f; dP_inchH2O =  %f", (double)ctx->dlvr_up, (double)dP_inchH2O);
-   
+    LOG_DEBUG( PITOT, "p_adc = %f; dP_inchH2O =  %f; dP_Pa = %f", (double)ctx->dlvr_up, (double)dP_inchH2O, (double)(INCH_H2O_TO_PASCAL( dP_inchH2O)) );
 
     // temperature in deg C
     float T_C = (float)ctx->dlvr_ut * (200.0f / 2047.0f) - 50.0f;     
 
+    LOG_DEBUG( PITOT, "t_adc = %f; T_C = %f", (double)ctx->dlvr_ut, (double)T_C );
+
     // result must fit inside the range
     if ((dP_inchH2O > RANGE_INCH_H2O) || (dP_inchH2O < -RANGE_INCH_H2O)) {
         // Debug("DLVR: Out of range pressure %f", dP_inchH2O);
diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c
index bd8cba61aa8..3b4c30d7bb7 100755
--- a/src/main/sensors/pitotmeter.c
+++ b/src/main/sensors/pitotmeter.c
@@ -194,6 +194,9 @@ static void performPitotCalibrationCycle(void)
 {
     zeroCalibrationAddValueS(&pitot.zeroCalibration, pitot.pressure);
 
+    // pitot.zeroCalibration.params.sampleCount
+    // pitot.zeroCalibration.val.accumulatedValue
+
     if (zeroCalibrationIsCompleteS(&pitot.zeroCalibration)) {
         zeroCalibrationGetZeroS(&pitot.zeroCalibration, &pitot.pressureZero);
         LOG_DEBUG(PITOT, "Pitot calibration complete (%d)", (int)lrintf(pitot.pressureZero));
@@ -205,7 +208,7 @@ STATIC_PROTOTHREAD(pitotThread)
     ptBegin(pitotThread);
 
     static float pitotPressureTmp;
-    static float pitotTemperatureTmp;
+    static float pitotTemperature;
     timeUs_t currentTimeUs;
 
     // Init filter
@@ -225,7 +228,8 @@ STATIC_PROTOTHREAD(pitotThread)
             pitot.lastSeenHealthyMs = millis();
         }
 
-        pitot.dev.calculate(&pitot.dev, &pitotPressureTmp, &pitotTemperatureTmp);
+        pitot.dev.calculate(&pitot.dev, &pitotPressureTmp, &pitotTemperature);
+
 #ifdef USE_SIMULATOR
         if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) {
             pitotPressureTmp = sq(simulatorData.airSpeed) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE;     
@@ -259,18 +263,20 @@ STATIC_PROTOTHREAD(pitotThread)
             // pitot.airSpeed = pitotmeterConfig()->pitot_scale * fast_fsqrtf( 2.0f * fabsf(pitot.pressure) / SSL_AIR_DENSITY) * 100;
             
             // with calibration
-            pitot.airSpeed = pitotmeterConfig()->pitot_scale * fast_fsqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / SSL_AIR_DENSITY) * 100;
+            pitot.airSpeed = pitotmeterConfig()->pitot_scale * fast_fsqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / SSL_AIR_DENSITY) * 100;  // cm/s
+
+            pitot.temperature = pitotTemperature;   // Kelvin
 
-            // LOG_DEBUG( PITOT, "HELLO!");
             debug[0] = pitot.pressure * 1000;
             debug[1] = pitot.pressureZero * 1000;
             debug[2] = pitot.airSpeed;
-            // debug[3] = pitot.lastSeenHealthyMs;
+            debug[3] = millis() - pitot.lastSeenHealthyMs;
 
         } else {
             performPitotCalibrationCycle();
             pitot.airSpeed = 0.0f;
         }
+
 #ifdef USE_SIMULATOR
         if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) {
             pitot.airSpeed = simulatorData.airSpeed;
diff --git a/src/main/sensors/pitotmeter.h b/src/main/sensors/pitotmeter.h
index 96059cb718e..a24fba665ca 100755
--- a/src/main/sensors/pitotmeter.h
+++ b/src/main/sensors/pitotmeter.h
@@ -56,6 +56,8 @@ typedef struct pito_s {
 
     float pressureZero;
     float pressure;
+    
+    float temperature;
 } pitot_t;
 
 #ifdef USE_PITOT

From 4a39badc84b67de11526a870251e4ec7287689e1 Mon Sep 17 00:00:00 2001
From: jmajew <jmajewsk@meil.pw.edu.pl>
Date: Sun, 30 Jul 2023 23:01:00 +0200
Subject: [PATCH 31/94] testing dlvr

---
 .../drivers/pitotmeter/pitotmeter_dlvr_l10d.c | 78 ++++++++-----------
 src/main/fc/fc_tasks.c                        |  4 +-
 src/main/sensors/pitotmeter.c                 |  3 +-
 src/main/sensors/sensors.h                    |  2 +-
 4 files changed, 36 insertions(+), 51 deletions(-)

diff --git a/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c b/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
index d51576419b9..d82f0ec0086 100755
--- a/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
+++ b/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
@@ -57,7 +57,8 @@
 #define INCH_H2O_TO_PASCAL(press) (INCH_OF_H2O_TO_PASCAL * (press))
 
 #define RANGE_INCH_H2O      10
-#define DLVR_OFFSET         (8192.0f)
+#define DLVR_OFFSET_CORR    -9.0f
+#define DLVR_OFFSET         8192.0f
 #define DLVR_SCALE          16384.0f
 
 
@@ -71,15 +72,18 @@ STATIC_ASSERT(sizeof(dlvrCtx_t) < BUS_SCRATCHPAD_MEMORY_SIZE, busDevice_scratchp
 
 static bool dlvr_start(pitotDev_t * pitot)
 {
-    uint8_t rxbuf[1];
-    bool ack = busReadBuf(pitot->busDev, 0xFF, rxbuf, 1);
-    return ack;
+    (void)pitot;
+    return true;
+
+    // uint8_t rxbuf[1];
+    // bool ack = busReadBuf(pitot->busDev, 0xFF, rxbuf, 1);
+    // return ack;
 }
 
 static bool dlvr_read(pitotDev_t * pitot)
 {
     uint8_t rxbuf1[4];
-    uint8_t rxbuf2[4];
+    // uint8_t rxbuf2[4];
 
     dlvrCtx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev);
     ctx->dataValid = false;
@@ -88,9 +92,9 @@ static bool dlvr_read(pitotDev_t * pitot)
         return false;
     }
 
-    if (!busReadBuf(pitot->busDev, 0xFF, rxbuf2, 4)) {
-        return false;
-    }
+    // if (!busReadBuf(pitot->busDev, 0xFF, rxbuf2, 4)) {
+    //     return false;
+    // }
 
     // status = 00 -> ok, new data
 	// status = 01 -> reserved
@@ -103,24 +107,26 @@ static bool dlvr_read(pitotDev_t * pitot)
     }
 
     int16_t dP_raw1, dT_raw1;
-    int16_t dP_raw2, dT_raw2;
+    // int16_t dP_raw2, dT_raw2;
 
     dP_raw1 = 0x3FFF & ((rxbuf1[0] << 8) + rxbuf1[1]);
     dT_raw1 = (0xFFE0 & ((rxbuf1[2] << 8) + rxbuf1[3])) >> 5;
-    dP_raw2 = 0x3FFF & ((rxbuf2[0] << 8) + rxbuf2[1]);
-    dT_raw2 = (0xFFE0 & ((rxbuf2[2] << 8) + rxbuf2[3])) >> 5;
+    // dP_raw2 = 0x3FFF & ((rxbuf2[0] << 8) + rxbuf2[1]);
+    // dT_raw2 = (0xFFE0 & ((rxbuf2[2] << 8) + rxbuf2[3])) >> 5;
 
-    // reject any double reads where the value has shifted in the upper more than 0xFF
-    if (ABS(dP_raw1 - dP_raw2) > 0xFF || ABS(dT_raw1 - dT_raw2) > 0xFF) {
-        return false;
-    }
+    // // reject any double reads where the value has shifted in the upper more than 0xFF
+    // if (ABS(dP_raw1 - dP_raw2) > 0xFF || ABS(dT_raw1 - dT_raw2) > 0xFF) {
+    //     return false;
+    // }
 
-    LOG_DEBUG( PITOT, "dP_raw1 = %f; dP_raw2 =  %f", (double)dP_raw1, (double)dP_raw2 );
+    // LOG_DEBUG( PITOT, "dP_raw1 = %f; dP_raw2 =  %f", (double)dP_raw1, (double)dP_raw2 );
 
     // Data valid, update ut/up values
     ctx->dataValid = true;
-    ctx->dlvr_up = (dP_raw1 + dP_raw2) / 2;
-    ctx->dlvr_ut = (dT_raw1 + dT_raw2) / 2;
+    // ctx->dlvr_up = (dP_raw1 + dP_raw2) / 2;
+    // ctx->dlvr_ut = (dT_raw1 + dT_raw2) / 2;
+    ctx->dlvr_up = dP_raw1;
+    ctx->dlvr_ut = dT_raw1;
     return true;
 }
 
@@ -128,39 +134,19 @@ static void dlvr_calculate(pitotDev_t * pitot, float *pressure, float *temperatu
 {
     dlvrCtx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev);
 
-    // //-----------------------------------------------------------------------------
-    // // MS4525 sensor
-    // //-----------------------------------------------------------------------------
-    // const float P_max = 1.0f;
-    // const float P_min = -P_max;
-    // const float PSI_to_Pa = 6894.757f;
-
-    // //float dP = ctx->ms4525_up * 10.0f * 0.1052120688f;
-    // const float dP_psi = -((ctx->dlvr_up - 0.1f * 16383) * (P_max - P_min) / (0.8f * 16383) + P_min);
-    // float dP = dP_psi * PSI_to_Pa;
-    // float T  = C_TO_KELVIN((float)(200.0f * (int32_t)ctx->dlvr_ut) / 2047.0f - 50.0f);
-
-    // if (pressure) {
-    //     *pressure = dP;    // Pa
-    // }
-
-    // if (temperature) {
-    //     *temperature = T; // K
-    // }
-
-    //-----------------------------------------------------------------------------
-    // DLVR-L10D sensor
-    //-----------------------------------------------------------------------------
-    
+  
     // pressure in inchH2O
-    float dP_inchH2O = 1.25f *  2.0f * RANGE_INCH_H2O  * (((float)ctx->dlvr_up - DLVR_OFFSET) / DLVR_SCALE); 
+    float dP_inchH2O = 1.25f *  2.0f * RANGE_INCH_H2O  * (((float)ctx->dlvr_up - (DLVR_OFFSET + DLVR_OFFSET_CORR) ) / DLVR_SCALE); 
+
+    LOG_DEBUG( PITOT, "dP_adc = %f; dP_inchH2O =  %f; dP_Pa = %f", (double)ctx->dlvr_up, (double)dP_inchH2O, (double)(INCH_H2O_TO_PASCAL( dP_inchH2O)) );
 
-    LOG_DEBUG( PITOT, "p_adc = %f; dP_inchH2O =  %f; dP_Pa = %f", (double)ctx->dlvr_up, (double)dP_inchH2O, (double)(INCH_H2O_TO_PASCAL( dP_inchH2O)) );
+    debug[3] = (int32_t)(ctx->dlvr_up *100);
+    debug[4] = (int32_t)((ctx->dlvr_up - (DLVR_OFFSET + DLVR_OFFSET_CORR)) *100);
 
     // temperature in deg C
     float T_C = (float)ctx->dlvr_ut * (200.0f / 2047.0f) - 50.0f;     
 
-    LOG_DEBUG( PITOT, "t_adc = %f; T_C = %f", (double)ctx->dlvr_ut, (double)T_C );
+    //LOG_DEBUG( PITOT, "t_adc = %f; T_C = %f", (double)ctx->dlvr_ut, (double)T_C );
 
     // result must fit inside the range
     if ((dP_inchH2O > RANGE_INCH_H2O) || (dP_inchH2O < -RANGE_INCH_H2O)) {
@@ -204,7 +190,7 @@ bool dlvrDetect(pitotDev_t * pitot)
     ctx->dlvr_up = 0;
 
     // Initialize pitotDev object
-    pitot->delay = 5000;    //10000;
+    pitot->delay = 1000;
     pitot->start = dlvr_start;
     pitot->get = dlvr_read;
     pitot->calculate = dlvr_calculate;
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index 7b2ba3b5d55..e16f7bccd0b 100755
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -499,8 +499,8 @@ cfTask_t cfTasks[TASK_COUNT] = {
     [TASK_PITOT] = {
         .taskName = "PITOT",
         .taskFunc = taskUpdatePitot,
-        .desiredPeriod = TASK_PERIOD_HZ(100),
-        .staticPriority = TASK_PRIORITY_MEDIUM,
+        .desiredPeriod = TASK_PERIOD_HZ(100),            // min real period ~30ms
+        .staticPriority = TASK_PRIORITY_MEDIUM_HIGH,    // TASK_PRIORITY_MEDIUM, //  ???
     },
 #endif
 
diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c
index 3b4c30d7bb7..d8fe64dc3e0 100755
--- a/src/main/sensors/pitotmeter.c
+++ b/src/main/sensors/pitotmeter.c
@@ -269,8 +269,7 @@ STATIC_PROTOTHREAD(pitotThread)
 
             debug[0] = pitot.pressure * 1000;
             debug[1] = pitot.pressureZero * 1000;
-            debug[2] = pitot.airSpeed;
-            debug[3] = millis() - pitot.lastSeenHealthyMs;
+            debug[2] = (pitot.pressure - pitot.pressureZero) * 1000;
 
         } else {
             performPitotCalibrationCycle();
diff --git a/src/main/sensors/sensors.h b/src/main/sensors/sensors.h
index 1a6ab0b8b20..8080ddb0855 100644
--- a/src/main/sensors/sensors.h
+++ b/src/main/sensors/sensors.h
@@ -40,7 +40,7 @@ typedef union flightDynamicsTrims_u {
 } flightDynamicsTrims_t;
 
 #define CALIBRATING_BARO_TIME_MS            2000
-#define CALIBRATING_PITOT_TIME_MS           2000
+#define CALIBRATING_PITOT_TIME_MS           5000 //2000
 #define CALIBRATING_GYRO_TIME_MS            2000
 #define CALIBRATING_ACC_TIME_MS             500
 

From 0a6a5e26135d1513a89bc3eb1a6fdf83cf95402f Mon Sep 17 00:00:00 2001
From: jmajew <jmajewsk@meil.pw.edu.pl>
Date: Tue, 1 Aug 2023 14:32:58 +0200
Subject: [PATCH 32/94] testing

---
 src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c | 12 ++++--------
 src/main/drivers/pitotmeter/pitotmeter_ms4525.c    |  6 ++++++
 src/main/drivers/pitotmeter/pitotmeter_msp.c       |  1 +
 src/main/fc/fc_tasks.c                             |  2 +-
 src/main/sensors/pitotmeter.c                      | 13 +++++++------
 5 files changed, 19 insertions(+), 15 deletions(-)

diff --git a/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c b/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
index d82f0ec0086..1f842d5a56d 100755
--- a/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
+++ b/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
@@ -72,12 +72,8 @@ STATIC_ASSERT(sizeof(dlvrCtx_t) < BUS_SCRATCHPAD_MEMORY_SIZE, busDevice_scratchp
 
 static bool dlvr_start(pitotDev_t * pitot)
 {
-    (void)pitot;
+    UNUSED(pitot);
     return true;
-
-    // uint8_t rxbuf[1];
-    // bool ack = busReadBuf(pitot->busDev, 0xFF, rxbuf, 1);
-    // return ack;
 }
 
 static bool dlvr_read(pitotDev_t * pitot)
@@ -140,8 +136,8 @@ static void dlvr_calculate(pitotDev_t * pitot, float *pressure, float *temperatu
 
     LOG_DEBUG( PITOT, "dP_adc = %f; dP_inchH2O =  %f; dP_Pa = %f", (double)ctx->dlvr_up, (double)dP_inchH2O, (double)(INCH_H2O_TO_PASCAL( dP_inchH2O)) );
 
-    debug[3] = (int32_t)(ctx->dlvr_up *100);
-    debug[4] = (int32_t)((ctx->dlvr_up - (DLVR_OFFSET + DLVR_OFFSET_CORR)) *100);
+    debug[4] = (int32_t)(ctx->dlvr_up *100);
+    debug[5] = (int32_t)((ctx->dlvr_up - (DLVR_OFFSET + DLVR_OFFSET_CORR)) *100);
 
     // temperature in deg C
     float T_C = (float)ctx->dlvr_ut * (200.0f / 2047.0f) - 50.0f;     
@@ -190,7 +186,7 @@ bool dlvrDetect(pitotDev_t * pitot)
     ctx->dlvr_up = 0;
 
     // Initialize pitotDev object
-    pitot->delay = 1000;
+    pitot->delay = 10;      // 10000
     pitot->start = dlvr_start;
     pitot->get = dlvr_read;
     pitot->calculate = dlvr_calculate;
diff --git a/src/main/drivers/pitotmeter/pitotmeter_ms4525.c b/src/main/drivers/pitotmeter/pitotmeter_ms4525.c
index 3b731eaa492..d7ef6e87465 100644
--- a/src/main/drivers/pitotmeter/pitotmeter_ms4525.c
+++ b/src/main/drivers/pitotmeter/pitotmeter_ms4525.c
@@ -83,6 +83,12 @@ static bool ms4525_read(pitotDev_t * pitot)
     ctx->dataValid = true;
     ctx->ms4525_up = (dP_raw1 + dP_raw2) / 2;
     ctx->ms4525_ut = (dT_raw1 + dT_raw2) / 2;
+
+    // former start
+    uint8_t rxbuf[1];
+    bool ack = busReadBuf(pitot->busDev, 0xFF, rxbuf, 1);
+    //return ack;
+
     return true;
 }
 
diff --git a/src/main/drivers/pitotmeter/pitotmeter_msp.c b/src/main/drivers/pitotmeter/pitotmeter_msp.c
index be1cf852c7f..1b9fae47327 100644
--- a/src/main/drivers/pitotmeter/pitotmeter_msp.c
+++ b/src/main/drivers/pitotmeter/pitotmeter_msp.c
@@ -95,3 +95,4 @@ bool mspPitotmeterDetect(pitotDev_t *pitot)
 }
 
 #endif
+    
\ No newline at end of file
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index e16f7bccd0b..53c4002959f 100755
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -500,7 +500,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
         .taskName = "PITOT",
         .taskFunc = taskUpdatePitot,
         .desiredPeriod = TASK_PERIOD_HZ(100),            // min real period ~30ms
-        .staticPriority = TASK_PRIORITY_MEDIUM_HIGH,    // TASK_PRIORITY_MEDIUM, //  ???
+        .staticPriority = TASK_PRIORITY_MEDIUM_HIGH,       //TASK_PRIORITY_MEDIUM_HIGH,    // TASK_PRIORITY_MEDIUM, //  ???
     },
 #endif
 
diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c
index d8fe64dc3e0..d5e229176a0 100755
--- a/src/main/sensors/pitotmeter.c
+++ b/src/main/sensors/pitotmeter.c
@@ -216,12 +216,12 @@ STATIC_PROTOTHREAD(pitotThread)
     pt1FilterInit(&pitot.lpfState, pitotmeterConfig()->pitot_lpf_milli_hz / 1000.0f, 0.0f);
 
     while(1) {
-        // Start measurement
-        if (pitot.dev.start(&pitot.dev)) {
-            pitot.lastSeenHealthyMs = millis();
-        }
+        // // Start measurement
+        // if (pitot.dev.start(&pitot.dev)) {
+        //     pitot.lastSeenHealthyMs = millis();
+        // }
 
-        ptDelayUs(pitot.dev.delay);
+        // ptDelayUs(pitot.dev.delay);
 
         // Read and calculate data
         if (pitot.dev.get(&pitot.dev)) {
@@ -246,7 +246,7 @@ STATIC_PROTOTHREAD(pitotThread)
         currentTimeUs = micros();
         pitot.pressure = pt1FilterApply3(&pitot.lpfState, pitotPressureTmp, US2S(currentTimeUs - pitot.lastMeasurementUs));
         pitot.lastMeasurementUs = currentTimeUs;
-        ptDelayUs(pitot.dev.delay);
+//        ptDelayUs(pitot.dev.delay);
 
         // Calculate IAS
         if (pitotIsCalibrationComplete()) {
@@ -270,6 +270,7 @@ STATIC_PROTOTHREAD(pitotThread)
             debug[0] = pitot.pressure * 1000;
             debug[1] = pitot.pressureZero * 1000;
             debug[2] = (pitot.pressure - pitot.pressureZero) * 1000;
+            debug[3] = pitot.dev.delay;
 
         } else {
             performPitotCalibrationCycle();

From d5942b6d065fdfc8aa4065aa24f02f5d29c9b8f9 Mon Sep 17 00:00:00 2001
From: jmajew <jerzy.majewski@pw.edu.pl>
Date: Wed, 2 Aug 2023 10:17:05 +0200
Subject: [PATCH 33/94] minimizing delays - wip

---
 .../drivers/pitotmeter/pitotmeter_dlvr_l10d.c |  2 +-
 .../drivers/pitotmeter/pitotmeter_ms4525.c    |  5 --
 src/main/sensors/pitotmeter.c                 | 57 +++++++++++++------
 3 files changed, 41 insertions(+), 23 deletions(-)

diff --git a/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c b/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
index 1f842d5a56d..18e3b31e4b7 100755
--- a/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
+++ b/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
@@ -57,7 +57,7 @@
 #define INCH_H2O_TO_PASCAL(press) (INCH_OF_H2O_TO_PASCAL * (press))
 
 #define RANGE_INCH_H2O      10
-#define DLVR_OFFSET_CORR    -9.0f
+#define DLVR_OFFSET_CORR    -9.0f   // check for other samples of DLVR-L10D; should be 0
 #define DLVR_OFFSET         8192.0f
 #define DLVR_SCALE          16384.0f
 
diff --git a/src/main/drivers/pitotmeter/pitotmeter_ms4525.c b/src/main/drivers/pitotmeter/pitotmeter_ms4525.c
index d7ef6e87465..e3496aa6244 100644
--- a/src/main/drivers/pitotmeter/pitotmeter_ms4525.c
+++ b/src/main/drivers/pitotmeter/pitotmeter_ms4525.c
@@ -84,11 +84,6 @@ static bool ms4525_read(pitotDev_t * pitot)
     ctx->ms4525_up = (dP_raw1 + dP_raw2) / 2;
     ctx->ms4525_ut = (dT_raw1 + dT_raw2) / 2;
 
-    // former start
-    uint8_t rxbuf[1];
-    bool ack = busReadBuf(pitot->busDev, 0xFF, rxbuf, 1);
-    //return ack;
-
     return true;
 }
 
diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c
index 1b171eec5e0..1b574cf6919 100755
--- a/src/main/sensors/pitotmeter.c
+++ b/src/main/sensors/pitotmeter.c
@@ -53,7 +53,7 @@
 
 #ifdef USE_PITOT
 
-pitot_t pitot;
+pitot_t pitot = {.lastMeasurementUs = 0, .lastSeenHealthyMs = 0};
 
 PG_REGISTER_WITH_RESET_TEMPLATE(pitotmeterConfig_t, pitotmeterConfig, PG_PITOTMETER_CONFIG, 2);
 
@@ -92,18 +92,25 @@ bool pitotDetect(pitotDev_t *dev, uint8_t pitotHardwareToUse)
             FALLTHROUGH;
 
         case PITOT_DLVR:
-#ifdef USE_PITOT_DLVR
-            if (dlvrDetect(dev)) {
+// #ifdef USE_PITOT_DLVR
+//             if (dlvrDetect(dev)) {
+//                 pitotHardware = PITOT_DLVR;
+//                 break;
+//             }
+// #endif
+//             /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
+//             if (pitotHardwareToUse != PITOT_AUTODETECT) {
+//                 break;
+//             }
+//             FALLTHROUGH;
+
+			// Skip autodetection for DLVR (it is indistinguishable from MS4525), only allow manual config
+            if (pitotHardwareToUse != PITOT_AUTODETECT && dlvrDetect(dev)) {
                 pitotHardware = PITOT_DLVR;
                 break;
             }
-#endif
-            /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
-            if (pitotHardwareToUse != PITOT_AUTODETECT) {
-                break;
-            }
             FALLTHROUGH;
-			
+
         case PITOT_ADC:
 #if defined(USE_ADC) && defined(USE_PITOT_ADC)
             if (adcPitotDetect(dev)) {
@@ -223,18 +230,34 @@ STATIC_PROTOTHREAD(pitotThread)
     	}
 #endif
 
-        // Start measurement
-        if (pitot.dev.start(&pitot.dev)) {
-            pitot.lastSeenHealthyMs = millis();
+        if ( pitot.lastSeenHealthyMs == 0 ) {
+            if (pitot.dev.start(&pitot.dev)) {
+                pitot.lastSeenHealthyMs = millis();
+            }        
         }
 
-        ptDelayUs(pitot.dev.delay);
+        // if ( (millis() - pitot.lastSeenHealthyMs) >= 10) {
+        if ( (millis() - pitot.lastSeenHealthyMs) >= US2MS(pitot.dev.delay)) {
+            if (pitot.dev.start(&pitot.dev))
+                pitot.lastSeenHealthyMs = millis();
 
-        // Read and calculate data
-        if (pitot.dev.get(&pitot.dev)) {
-            pitot.lastSeenHealthyMs = millis();
+            if (pitot.dev.start(&pitot.dev))
+                pitot.lastSeenHealthyMs = millis();        
         }
 
+
+        // // Start measurement
+        // if (pitot.dev.start(&pitot.dev)) {
+        //     pitot.lastSeenHealthyMs = millis();
+        // }
+
+        // ptDelayUs(pitot.dev.delay);
+
+        // // Read and calculate data
+        // if (pitot.dev.get(&pitot.dev)) {
+        //     pitot.lastSeenHealthyMs = millis();
+        // }
+
         pitot.dev.calculate(&pitot.dev, &pitotPressureTmp, &pitotTemperature);
 
 #ifdef USE_SIMULATOR
@@ -247,7 +270,7 @@ STATIC_PROTOTHREAD(pitotThread)
             pitotPressureTmp = sq(fakePitotGetAirspeed()) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE;     
     	} 
 #endif
-        ptYield();
+        ptYield();  // CHECKME :: is it necessary ??!
 
         // Filter pressure
         currentTimeUs = micros();

From 5b9327bfe11fc579e75146eb98fe2d847ca608d7 Mon Sep 17 00:00:00 2001
From: jmajew <jmajewsk@meil.pw.edu.pl>
Date: Thu, 3 Aug 2023 00:21:11 +0200
Subject: [PATCH 34/94] wip

---
 .../drivers/pitotmeter/pitotmeter_dlvr_l10d.c |  5 ++---
 .../drivers/pitotmeter/pitotmeter_ms4525.c    |  7 +++++++
 src/main/fc/fc_tasks.c                        |  4 ++--
 src/main/sensors/pitotmeter.c                 | 20 ++++++++++++++++---
 4 files changed, 28 insertions(+), 8 deletions(-)

diff --git a/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c b/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
index 18e3b31e4b7..6b10ad57f2e 100755
--- a/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
+++ b/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
@@ -52,14 +52,13 @@
 // #define SSL_AIR_TEMPERATURE    288.15f // K
 
 #define INCH_OF_H2O_TO_PASCAL   248.84f
-//#define INCH_OF_H2O_TO_PASCAL 249.09
 
 #define INCH_H2O_TO_PASCAL(press) (INCH_OF_H2O_TO_PASCAL * (press))
 
 #define RANGE_INCH_H2O      10
-#define DLVR_OFFSET_CORR    -9.0f   // check for other samples of DLVR-L10D; should be 0
 #define DLVR_OFFSET         8192.0f
 #define DLVR_SCALE          16384.0f
+#define DLVR_OFFSET_CORR    -9.0f   // check for other samples of DLVR-L10D; should be 0
 
 
 typedef struct __attribute__ ((__packed__)) dlvrCtx_s {
@@ -186,7 +185,7 @@ bool dlvrDetect(pitotDev_t * pitot)
     ctx->dlvr_up = 0;
 
     // Initialize pitotDev object
-    pitot->delay = 10;      // 10000
+    pitot->delay = 10000;      // 10000
     pitot->start = dlvr_start;
     pitot->get = dlvr_read;
     pitot->calculate = dlvr_calculate;
diff --git a/src/main/drivers/pitotmeter/pitotmeter_ms4525.c b/src/main/drivers/pitotmeter/pitotmeter_ms4525.c
index e3496aa6244..02a828e383e 100644
--- a/src/main/drivers/pitotmeter/pitotmeter_ms4525.c
+++ b/src/main/drivers/pitotmeter/pitotmeter_ms4525.c
@@ -74,6 +74,13 @@ static bool ms4525_read(pitotDev_t * pitot)
     dP_raw2 = 0x3FFF & ((rxbuf2[0] << 8) + rxbuf2[1]);
     dT_raw2 = (0xFFE0 & ((rxbuf2[2] << 8) + rxbuf2[3])) >> 5;
 
+    // reject any values that are the absolute minimum or maximums these
+    // can happen due to gnd lifts or communication errors on the bus
+    if (dP_raw1 == 0x3FFF || dP_raw1 == 0 || dT_raw1 == 0x7FF || dT_raw1 == 0 ||
+        dP_raw2 == 0x3FFF || dP_raw2 == 0 || dT_raw2 == 0x7FF || dT_raw2 == 0) {
+        return false;
+    }
+
     // reject any double reads where the value has shifted in the upper more than 0xFF
     if (ABS(dP_raw1 - dP_raw2) > 0xFF || ABS(dT_raw1 - dT_raw2) > 0xFF) {
         return false;
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index 4bdc330c832..136702963ee 100755
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -503,8 +503,8 @@ cfTask_t cfTasks[TASK_COUNT] = {
     [TASK_PITOT] = {
         .taskName = "PITOT",
         .taskFunc = taskUpdatePitot,
-        .desiredPeriod = TASK_PERIOD_HZ(100),            // min real period ~30ms
-        .staticPriority = TASK_PRIORITY_MEDIUM_HIGH,       //TASK_PRIORITY_MEDIUM_HIGH,    // TASK_PRIORITY_MEDIUM, //  ???
+        .desiredPeriod = TASK_PERIOD_US(15000),         //TASK_PERIOD_HZ(100),            // min real period ~30ms
+        .staticPriority = TASK_PRIORITY_MEDIUM,    //TASK_PRIORITY_MEDIUM_HIGH,    // TASK_PRIORITY_MEDIUM, //  ???
     },
 #endif
 
diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c
index 1b574cf6919..67fb38c1d39 100755
--- a/src/main/sensors/pitotmeter.c
+++ b/src/main/sensors/pitotmeter.c
@@ -45,6 +45,7 @@
 #include "scheduler/protothreads.h"
 
 #include "sensors/pitotmeter.h"
+#include "sensors/barometer.h"
 #include "sensors/sensors.h"
 
 
@@ -53,6 +54,8 @@
 
 #ifdef USE_PITOT
 
+extern baro_t baro;
+
 pitot_t pitot = {.lastMeasurementUs = 0, .lastSeenHealthyMs = 0};
 
 PG_REGISTER_WITH_RESET_TEMPLATE(pitotmeterConfig_t, pitotmeterConfig, PG_PITOTMETER_CONFIG, 2);
@@ -236,12 +239,14 @@ STATIC_PROTOTHREAD(pitotThread)
             }        
         }
 
+        LOG_DEBUG( PITOT, "cur delay = %d, req delay = %d", (int)(millis() - pitot.lastSeenHealthyMs),  (int)US2MS(pitot.dev.delay) );
+
         // if ( (millis() - pitot.lastSeenHealthyMs) >= 10) {
         if ( (millis() - pitot.lastSeenHealthyMs) >= US2MS(pitot.dev.delay)) {
-            if (pitot.dev.start(&pitot.dev))
+            if (pitot.dev.get(&pitot.dev))          // read current data
                 pitot.lastSeenHealthyMs = millis();
 
-            if (pitot.dev.start(&pitot.dev))
+            if (pitot.dev.start(&pitot.dev))        // init for next read
                 pitot.lastSeenHealthyMs = millis();        
         }
 
@@ -278,6 +283,8 @@ STATIC_PROTOTHREAD(pitotThread)
         pitot.lastMeasurementUs = currentTimeUs;
 //        ptDelayUs(pitot.dev.delay);
 
+        static int calib_count = 0;
+
         // Calculate IAS
         if (pitotIsCalibrationComplete()) {
             // https://en.wikipedia.org/wiki/Indicated_airspeed
@@ -297,13 +304,20 @@ STATIC_PROTOTHREAD(pitotThread)
 
             pitot.temperature = pitotTemperature;   // Kelvin
 
+            int32_t baroPress  = baro.baroPressure;
+
             debug[0] = pitot.pressure * 1000;
             debug[1] = pitot.pressureZero * 1000;
             debug[2] = (pitot.pressure - pitot.pressureZero) * 1000;
-            debug[3] = pitot.dev.delay;
+            debug[3] = baroPress;
+
+            LOG_DEBUG( PITOT, "calib_count = %d", (int)(calib_count) );
+            // calib_count = 0;
 
         } else {
             performPitotCalibrationCycle();
+            ++calib_count;
+
             pitot.airSpeed = 0.0f;
         }
 

From c1da9b912be3c3bd3f8a523f714208022bf79659 Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Thu, 3 Aug 2023 20:49:26 -0500
Subject: [PATCH 35/94] SKYSTARSH743HD remove non-existant S9 & S10. Add servos
 target

---
 src/main/target/SKYSTARSH743HD/CMakeLists.txt |  3 ++-
 src/main/target/SKYSTARSH743HD/target.c       | 14 +++++++++-----
 2 files changed, 11 insertions(+), 6 deletions(-)

diff --git a/src/main/target/SKYSTARSH743HD/CMakeLists.txt b/src/main/target/SKYSTARSH743HD/CMakeLists.txt
index 926391fdb1a..35377d39ac4 100644
--- a/src/main/target/SKYSTARSH743HD/CMakeLists.txt
+++ b/src/main/target/SKYSTARSH743HD/CMakeLists.txt
@@ -1 +1,2 @@
-target_stm32h743xi(SKYSTARSH743HD)
\ No newline at end of file
+target_stm32h743xi(SKYSTARSH743HD)
+target_stm32h743xi(SKYSTARSH743HD_SERVOS)
diff --git a/src/main/target/SKYSTARSH743HD/target.c b/src/main/target/SKYSTARSH743HD/target.c
index dff8d0f967a..2c7d1a5bee6 100644
--- a/src/main/target/SKYSTARSH743HD/target.c
+++ b/src/main/target/SKYSTARSH743HD/target.c
@@ -36,15 +36,19 @@ timerHardware_t timerHardware[] = {
     DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 2),   // S3
     DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 3),   // S4
 
-    DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 4),   // S5  
+#if defined(SKYSTARSH743HD_SERVOS)
+    DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 4),   // S5
+    DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 5),   // S6
+    DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 6),   // S7
+    DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 7),   // S8
+#else
+    DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 4),   // S5
     DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 5),   // S6
     DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 6),   // S7
     DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 7),   // S8
-
-    DEF_TIM(TIM15, CH1, PE5, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0),   // S9
-    DEF_TIM(TIM15, CH2, PE6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0),   // S10 DMA_NONE
+#endif
 
     DEF_TIM(TIM1,  CH1, PA8,  TIM_USE_LED, 0, 9),    // LED_2812
 };
 
-const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
\ No newline at end of file
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

From fc6873ba7a630e18c810f2daeef434f6320b5930 Mon Sep 17 00:00:00 2001
From: jmajew <jmajewsk@meil.pw.edu.pl>
Date: Fri, 4 Aug 2023 15:14:00 +0200
Subject: [PATCH 36/94] broken

---
 .../drivers/pitotmeter/pitotmeter_dlvr_l10d.c | 11 ++--
 .../drivers/pitotmeter/pitotmeter_ms4525.c    |  3 +
 src/main/fc/fc_tasks.c                        |  6 +-
 src/main/fc/settings.yaml                     |  2 +-
 src/main/sensors/pitotmeter.c                 | 57 ++++++++++++-------
 src/main/sensors/sensors.h                    |  2 +-
 6 files changed, 51 insertions(+), 30 deletions(-)

diff --git a/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c b/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
index 6b10ad57f2e..ae36f2bfea2 100755
--- a/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
+++ b/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
@@ -58,7 +58,7 @@
 #define RANGE_INCH_H2O      10
 #define DLVR_OFFSET         8192.0f
 #define DLVR_SCALE          16384.0f
-#define DLVR_OFFSET_CORR    -9.0f   // check for other samples of DLVR-L10D; should be 0
+#define DLVR_OFFSET_CORR    0.0f   //-9.0f   // check for other samples of DLVR-L10D; should be 0
 
 
 typedef struct __attribute__ ((__packed__)) dlvrCtx_s {
@@ -118,10 +118,9 @@ static bool dlvr_read(pitotDev_t * pitot)
 
     // Data valid, update ut/up values
     ctx->dataValid = true;
-    // ctx->dlvr_up = (dP_raw1 + dP_raw2) / 2;
-    // ctx->dlvr_ut = (dT_raw1 + dT_raw2) / 2;
     ctx->dlvr_up = dP_raw1;
     ctx->dlvr_ut = dT_raw1;
+
     return true;
 }
 
@@ -133,10 +132,10 @@ static void dlvr_calculate(pitotDev_t * pitot, float *pressure, float *temperatu
     // pressure in inchH2O
     float dP_inchH2O = 1.25f *  2.0f * RANGE_INCH_H2O  * (((float)ctx->dlvr_up - (DLVR_OFFSET + DLVR_OFFSET_CORR) ) / DLVR_SCALE); 
 
-    LOG_DEBUG( PITOT, "dP_adc = %f; dP_inchH2O =  %f; dP_Pa = %f", (double)ctx->dlvr_up, (double)dP_inchH2O, (double)(INCH_H2O_TO_PASCAL( dP_inchH2O)) );
+    // LOG_DEBUG( PITOT, "dP_adc = %f; dP_inchH2O =  %f; dP_Pa = %f", (double)ctx->dlvr_up, (double)dP_inchH2O, (double)(INCH_H2O_TO_PASCAL( dP_inchH2O)) );
 
-    debug[4] = (int32_t)(ctx->dlvr_up *100);
-    debug[5] = (int32_t)((ctx->dlvr_up - (DLVR_OFFSET + DLVR_OFFSET_CORR)) *100);
+    debug[6] = (int32_t)(ctx->dlvr_up *100);
+    debug[7] = (int32_t)((ctx->dlvr_up - (DLVR_OFFSET + DLVR_OFFSET_CORR)) *100);
 
     // temperature in deg C
     float T_C = (float)ctx->dlvr_ut * (200.0f / 2047.0f) - 50.0f;     
diff --git a/src/main/drivers/pitotmeter/pitotmeter_ms4525.c b/src/main/drivers/pitotmeter/pitotmeter_ms4525.c
index 02a828e383e..b8a4cdfafb2 100644
--- a/src/main/drivers/pitotmeter/pitotmeter_ms4525.c
+++ b/src/main/drivers/pitotmeter/pitotmeter_ms4525.c
@@ -107,6 +107,9 @@ static void ms4525_calculate(pitotDev_t * pitot, float *pressure, float *tempera
     float dP = dP_psi * PSI_to_Pa;
     float T  = C_TO_KELVIN((float)(200.0f * (int32_t)ctx->ms4525_ut) / 2047.0f - 50.0f);
 
+    debug[6] = (int32_t)(ctx->ms4525_up *100);
+
+
     if (pressure) {
         *pressure = dP;    // Pa
     }
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index 136702963ee..c1003c18813 100755
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -494,7 +494,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
     [TASK_BARO] = {
         .taskName = "BARO",
         .taskFunc = taskUpdateBaro,
-        .desiredPeriod = TASK_PERIOD_HZ(20),
+        .desiredPeriod = TASK_PERIOD_MS(50),        //ASK_PERIOD_HZ(20), SPL06 uses oversampilng - result 16ms ?
         .staticPriority = TASK_PRIORITY_MEDIUM,
     },
 #endif
@@ -503,8 +503,8 @@ cfTask_t cfTasks[TASK_COUNT] = {
     [TASK_PITOT] = {
         .taskName = "PITOT",
         .taskFunc = taskUpdatePitot,
-        .desiredPeriod = TASK_PERIOD_US(15000),         //TASK_PERIOD_HZ(100),            // min real period ~30ms
-        .staticPriority = TASK_PRIORITY_MEDIUM,    //TASK_PRIORITY_MEDIUM_HIGH,    // TASK_PRIORITY_MEDIUM, //  ???
+        .desiredPeriod = TASK_PERIOD_MS(20),    //TASK_PERIOD_HZ(100)
+        .staticPriority = TASK_PRIORITY_MEDIUM,    //TASK_PRIORITY_MEDIUM_HIGH, TASK_PRIORITY_MEDIUM,
     },
 #endif
 
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 2261fbc870a..69e2771656c 100755
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -19,7 +19,7 @@ tables:
     values: ["NONE", "AUTO", "BMP085", "MS5611", "BMP280", "MS5607", "LPS25H", "SPL06", "BMP388", "DPS310", "B2SMPB", "MSP", "FAKE"]
     enum: baroSensor_e
   - name: pitot_hardware
-    values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE", "MSP", "DLVR"]
+    values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE", "MSP", "DLVR-L10D"]
     enum: pitotSensor_e
   - name: receiver_type
     values: ["NONE", "SERIAL", "MSP", "SIM (SITL)"]
diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c
index 67fb38c1d39..f8099d6576a 100755
--- a/src/main/sensors/pitotmeter.c
+++ b/src/main/sensors/pitotmeter.c
@@ -204,9 +204,6 @@ static void performPitotCalibrationCycle(void)
 {
     zeroCalibrationAddValueS(&pitot.zeroCalibration, pitot.pressure);
 
-    // pitot.zeroCalibration.params.sampleCount
-    // pitot.zeroCalibration.val.accumulatedValue
-
     if (zeroCalibrationIsCompleteS(&pitot.zeroCalibration)) {
         zeroCalibrationGetZeroS(&pitot.zeroCalibration, &pitot.pressureZero);
         LOG_DEBUG(PITOT, "Pitot calibration complete (%d)", (int)lrintf(pitot.pressureZero));
@@ -275,15 +272,18 @@ STATIC_PROTOTHREAD(pitotThread)
             pitotPressureTmp = sq(fakePitotGetAirspeed()) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE;     
     	} 
 #endif
-        ptYield();  // CHECKME :: is it necessary ??!
+        ptYield();
 
-        // Filter pressure
-        currentTimeUs = micros();
-        pitot.pressure = pt1FilterApply3(&pitot.lpfState, pitotPressureTmp, US2S(currentTimeUs - pitot.lastMeasurementUs));
-        pitot.lastMeasurementUs = currentTimeUs;
-//        ptDelayUs(pitot.dev.delay);
+//         // Filter pressure - NOTE : do not filter during calibration !!!
+//         currentTimeUs = micros();
+//         pitot.pressure = pt1FilterApply3(&pitot.lpfState, pitotPressureTmp, US2S(currentTimeUs - pitot.lastMeasurementUs));
+//         pitot.lastMeasurementUs = currentTimeUs;
+// //        ptDelayUs(pitot.dev.delay);
 
         static int calib_count = 0;
+        static timeMs_t calibPeriod_ms;
+        if ( calib_count == 0 )
+            calibPeriod_ms = millis();
 
         // Calculate IAS
         if (pitotIsCalibrationComplete()) {
@@ -296,25 +296,44 @@ STATIC_PROTOTHREAD(pitotThread)
             // Therefore we shouldn't care about CAS/TAS and only calculate IAS since it's more indicative to the pilot and more useful in calculations
             // It also allows us to use pitot_scale to calibrate the dynamic pressure sensor scale
 
-            // // no calibibration 
-            // pitot.airSpeed = pitotmeterConfig()->pitot_scale * fast_fsqrtf( 2.0f * fabsf(pitot.pressure) / SSL_AIR_DENSITY) * 100;
-            
-            // with calibration
-            pitot.airSpeed = pitotmeterConfig()->pitot_scale * fast_fsqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / SSL_AIR_DENSITY) * 100;  // cm/s
+#if 1
+            {
+                // filter pressure
+                // do filter only when NOT calibrating
+                currentTimeUs = micros();
+                pitot.pressure = pt1FilterApply3(&pitot.lpfState, pitotPressureTmp, US2S(currentTimeUs - pitot.lastMeasurementUs));
+                pitot.lastMeasurementUs = currentTimeUs;
 
-            pitot.temperature = pitotTemperature;   // Kelvin
+                pitot.airSpeed = pitotmeterConfig()->pitot_scale * fast_fsqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / SSL_AIR_DENSITY) * 100;  // cm/s
+            }
+#else            
+            {
+                // filter airspeed
+                pitot.pressure = pitotPressureTmp;
 
-            int32_t baroPress  = baro.baroPressure;
+                float airSpeedTmp = pitotmeterConfig()->pitot_scale * fast_fsqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / SSL_AIR_DENSITY) * 100;  // cm/s
+
+                // do filter only when NOT calibrating
+                currentTimeUs = micros();
+                pitot.airSpeed = pt1FilterApply3(&pitot.lpfState, airSpeedTmp, US2S(currentTimeUs - pitot.lastMeasurementUs));
+                pitot.lastMeasurementUs = currentTimeUs;
+            }
+#endif            
+
+
+            pitot.temperature = pitotTemperature;   // Kelvin
 
             debug[0] = pitot.pressure * 1000;
             debug[1] = pitot.pressureZero * 1000;
             debug[2] = (pitot.pressure - pitot.pressureZero) * 1000;
-            debug[3] = baroPress;
+            debug[3] = pitot.temperature *1000;
+            debug[4] = baro.baroPressure;
 
-            LOG_DEBUG( PITOT, "calib_count = %d", (int)(calib_count) );
-            // calib_count = 0;
+            LOG_DEBUG( PITOT, "calib_count = %d, period_ms = %d", (int)(calib_count), (int)(millis() - calibPeriod_ms) );
 
         } else {
+            pitot.pressure = pitotPressureTmp;
+
             performPitotCalibrationCycle();
             ++calib_count;
 
diff --git a/src/main/sensors/sensors.h b/src/main/sensors/sensors.h
index 23349aaf5b8..07b6285c2d6 100644
--- a/src/main/sensors/sensors.h
+++ b/src/main/sensors/sensors.h
@@ -40,7 +40,7 @@ typedef union flightDynamicsTrims_u {
 } flightDynamicsTrims_t;
 
 #define CALIBRATING_BARO_TIME_MS            2000
-#define CALIBRATING_PITOT_TIME_MS           5000 //2000
+#define CALIBRATING_PITOT_TIME_MS           4000 //2000
 #define CALIBRATING_GYRO_TIME_MS            2000
 #define CALIBRATING_ACC_TIME_MS             500
 #define CALIBRATING_GYRO_MORON_THRESHOLD    32

From 9c36b57153c4b020a5cdf8808333ada7a974724f Mon Sep 17 00:00:00 2001
From: jmajew <jmajewsk@meil.pw.edu.pl>
Date: Fri, 4 Aug 2023 20:47:42 +0200
Subject: [PATCH 37/94] ms4525 problem fix

---
 src/main/common/calibration.c                 |   6 +
 .../drivers/pitotmeter/pitotmeter_ms4525.c    |   3 +-
 src/main/sensors/pitotmeter.c                 | 114 +++++++++---------
 3 files changed, 67 insertions(+), 56 deletions(-)

diff --git a/src/main/common/calibration.c b/src/main/common/calibration.c
index be67a0932f8..7f979ae3305 100644
--- a/src/main/common/calibration.c
+++ b/src/main/common/calibration.c
@@ -30,6 +30,9 @@
 #include "drivers/time.h"
 #include "common/calibration.h"
 
+#include "common/log.h"
+#include "build/debug.h"        // TODO remove me
+
 void zeroCalibrationStartS(zeroCalibrationScalar_t * s, timeMs_t window, float threshold, bool allowFailure)
 {
     // Reset parameters and state
@@ -75,6 +78,9 @@ void zeroCalibrationAddValueS(zeroCalibrationScalar_t * s, const float v)
     // Check if calibration is complete
     if ((millis() - s->params.startTimeMs) > s->params.windowSizeMs) {
         const float stddev = devStandardDeviation(&s->val.stdDev);
+
+        LOG_DEBUG( PITOT, " ----- CALIB ADD - stddev = %f, threshold = %f, ", (double)(stddev), (double)(s->params.stdDevThreshold) );
+
         if (stddev > s->params.stdDevThreshold) {
             if (!s->params.allowFailure) {
                 // If deviation is too big - restart calibration
diff --git a/src/main/drivers/pitotmeter/pitotmeter_ms4525.c b/src/main/drivers/pitotmeter/pitotmeter_ms4525.c
index b8a4cdfafb2..3e2d11bf45c 100644
--- a/src/main/drivers/pitotmeter/pitotmeter_ms4525.c
+++ b/src/main/drivers/pitotmeter/pitotmeter_ms4525.c
@@ -107,7 +107,8 @@ static void ms4525_calculate(pitotDev_t * pitot, float *pressure, float *tempera
     float dP = dP_psi * PSI_to_Pa;
     float T  = C_TO_KELVIN((float)(200.0f * (int32_t)ctx->ms4525_ut) / 2047.0f - 50.0f);
 
-    debug[6] = (int32_t)(ctx->ms4525_up *100);
+    debug[6] = (int32_t)(ctx->ms4525_up);
+    debug[7] = (int32_t)(dP_psi * 1000);
 
 
     if (pressure) {
diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c
index f8099d6576a..9dd08a5a5cb 100755
--- a/src/main/sensors/pitotmeter.c
+++ b/src/main/sensors/pitotmeter.c
@@ -197,7 +197,9 @@ bool pitotIsCalibrationComplete(void)
 
 void pitotStartCalibration(void)
 {
-    zeroCalibrationStartS(&pitot.zeroCalibration, CALIBRATING_PITOT_TIME_MS, SSL_AIR_PRESSURE * 0.00001f, false);
+    // problem for ms4525 with threshold  to low when not smoothedd
+//    zeroCalibrationStartS(&pitot.zeroCalibration, CALIBRATING_PITOT_TIME_MS, SSL_AIR_PRESSURE * 0.00001f, false);
+    zeroCalibrationStartS(&pitot.zeroCalibration, CALIBRATING_PITOT_TIME_MS, SSL_AIR_PRESSURE * 0.00004f, false);
 }
 
 static void performPitotCalibrationCycle(void)
@@ -215,11 +217,12 @@ STATIC_PROTOTHREAD(pitotThread)
     ptBegin(pitotThread);
 
     static float pitotPressureTmp;
-    static float pitotTemperature;
+    static float pitotTemperatureTmp;
     timeUs_t currentTimeUs;
 
     // Init filter
     pitot.lastMeasurementUs = micros();
+
     pt1FilterInit(&pitot.lpfState, pitotmeterConfig()->pitot_lpf_milli_hz / 1000.0f, 0.0f);
 
     while(1) {
@@ -236,7 +239,7 @@ STATIC_PROTOTHREAD(pitotThread)
             }        
         }
 
-        LOG_DEBUG( PITOT, "cur delay = %d, req delay = %d", (int)(millis() - pitot.lastSeenHealthyMs),  (int)US2MS(pitot.dev.delay) );
+        // LOG_DEBUG( PITOT, "cur delay = %d, req delay = %d", (int)(millis() - pitot.lastSeenHealthyMs),  (int)US2MS(pitot.dev.delay) );
 
         // if ( (millis() - pitot.lastSeenHealthyMs) >= 10) {
         if ( (millis() - pitot.lastSeenHealthyMs) >= US2MS(pitot.dev.delay)) {
@@ -248,19 +251,7 @@ STATIC_PROTOTHREAD(pitotThread)
         }
 
 
-        // // Start measurement
-        // if (pitot.dev.start(&pitot.dev)) {
-        //     pitot.lastSeenHealthyMs = millis();
-        // }
-
-        // ptDelayUs(pitot.dev.delay);
-
-        // // Read and calculate data
-        // if (pitot.dev.get(&pitot.dev)) {
-        //     pitot.lastSeenHealthyMs = millis();
-        // }
-
-        pitot.dev.calculate(&pitot.dev, &pitotPressureTmp, &pitotTemperature);
+        pitot.dev.calculate(&pitot.dev, &pitotPressureTmp, &pitotTemperatureTmp);
 
 #ifdef USE_SIMULATOR
     	if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) {
@@ -274,16 +265,16 @@ STATIC_PROTOTHREAD(pitotThread)
 #endif
         ptYield();
 
-//         // Filter pressure - NOTE : do not filter during calibration !!!
-//         currentTimeUs = micros();
-//         pitot.pressure = pt1FilterApply3(&pitot.lpfState, pitotPressureTmp, US2S(currentTimeUs - pitot.lastMeasurementUs));
-//         pitot.lastMeasurementUs = currentTimeUs;
-// //        ptDelayUs(pitot.dev.delay);
+        // // Filter pressure - NOTE : do not filter during calibration !!!
+        // currentTimeUs = micros();
+        // pitot.pressure = pt1FilterApply3(&pitot.lpfState, pitotPressureTmp, US2S(currentTimeUs - pitot.lastMeasurementUs));
+        // pitot.lastMeasurementUs = currentTimeUs;
+
+        // currentTimeUs = micros();
+        // float p = pt1FilterApply3(&pitot.lpfState, pitotPressureTmp, US2S(currentTimeUs - pitot.lastMeasurementUs));
+        // pitot.pressure = p; //pitotPressureTmp;
+        // pitot.lastMeasurementUs = micros();
 
-        static int calib_count = 0;
-        static timeMs_t calibPeriod_ms;
-        if ( calib_count == 0 )
-            calibPeriod_ms = millis();
 
         // Calculate IAS
         if (pitotIsCalibrationComplete()) {
@@ -296,46 +287,59 @@ STATIC_PROTOTHREAD(pitotThread)
             // Therefore we shouldn't care about CAS/TAS and only calculate IAS since it's more indicative to the pilot and more useful in calculations
             // It also allows us to use pitot_scale to calibrate the dynamic pressure sensor scale
 
-#if 1
-            {
-                // filter pressure
-                // do filter only when NOT calibrating
-                currentTimeUs = micros();
-                pitot.pressure = pt1FilterApply3(&pitot.lpfState, pitotPressureTmp, US2S(currentTimeUs - pitot.lastMeasurementUs));
-                pitot.lastMeasurementUs = currentTimeUs;
-
-                pitot.airSpeed = pitotmeterConfig()->pitot_scale * fast_fsqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / SSL_AIR_DENSITY) * 100;  // cm/s
-            }
-#else            
-            {
-                // filter airspeed
-                pitot.pressure = pitotPressureTmp;
-
-                float airSpeedTmp = pitotmeterConfig()->pitot_scale * fast_fsqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / SSL_AIR_DENSITY) * 100;  // cm/s
+            // filter pressure
+            // do NOT filter only when calibrating
+            currentTimeUs = micros();
+            pitot.pressure = pt1FilterApply3(&pitot.lpfState, pitotPressureTmp, US2S(currentTimeUs - pitot.lastMeasurementUs));
+            pitot.lastMeasurementUs = currentTimeUs;
+
+            pitot.airSpeed = pitotmeterConfig()->pitot_scale * fast_fsqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / SSL_AIR_DENSITY) * 100;  // cm/s
+
+// #if 1
+//             {
+//                 // filter pressure
+//                 // do NOT filter only when calibrating
+//                 currentTimeUs = micros();
+//                 pitot.pressure = pt1FilterApply3(&pitot.lpfState, pitotPressureTmp, US2S(currentTimeUs - pitot.lastMeasurementUs));
+//                 pitot.lastMeasurementUs = currentTimeUs;
+//
+//                 pitot.airSpeed = pitotmeterConfig()->pitot_scale * fast_fsqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / SSL_AIR_DENSITY) * 100;  // cm/s
+//             }
+// #else            
+//             {
+//                 // filter airspeed
+//                 pitot.pressure = pitotPressureTmp;
+//
+//                 float airSpeedTmp = pitotmeterConfig()->pitot_scale * fast_fsqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / SSL_AIR_DENSITY) * 100;  // cm/s
+//
+//                 // do filter only when NOT calibrating
+//                 currentTimeUs = micros();
+//                 pitot.airSpeed = pt1FilterApply3(&pitot.lpfState, airSpeedTmp, US2S(currentTimeUs - pitot.lastMeasurementUs));
+//                 pitot.lastMeasurementUs = currentTimeUs;
+//             }
+// #endif            
 
-                // do filter only when NOT calibrating
-                currentTimeUs = micros();
-                pitot.airSpeed = pt1FilterApply3(&pitot.lpfState, airSpeedTmp, US2S(currentTimeUs - pitot.lastMeasurementUs));
-                pitot.lastMeasurementUs = currentTimeUs;
-            }
-#endif            
 
+            pitot.temperature = pitotTemperatureTmp;   // Kelvin
 
-            pitot.temperature = pitotTemperature;   // Kelvin
+            debug[0] = (int32_t)(pitot.pressure * 1000);
+            debug[1] = (int32_t)(pitot.pressureZero * 1000);
+            debug[2] = (int32_t)((pitot.pressure - pitot.pressureZero) * 1000);
+            debug[3] = (int32_t)(pitot.temperature *1000);
+            // debug[4] = (int32_t)(baro.baroPressure);
 
-            debug[0] = pitot.pressure * 1000;
-            debug[1] = pitot.pressureZero * 1000;
-            debug[2] = (pitot.pressure - pitot.pressureZero) * 1000;
-            debug[3] = pitot.temperature *1000;
-            debug[4] = baro.baroPressure;
+            // LOG_DEBUG( PITOT, " ----- OUT pZero = %f, ", (double)(pitot.pressureZero) );
 
-            LOG_DEBUG( PITOT, "calib_count = %d, period_ms = %d", (int)(calib_count), (int)(millis() - calibPeriod_ms) );
+            // LOG_DEBUG( PITOT, "calib_count = %d, period_ms = %d", (int)(calib_count), (int)(millis() - calibPeriod_ms) );
 
         } else {
             pitot.pressure = pitotPressureTmp;
 
+            // LOG_DEBUG( PITOT, " ----- INSIDE CALIB PART pZero = %f, ", (double)(pitot.pressureZero) );
+
+            debug[0] = pitot.pressure * 1000;
+
             performPitotCalibrationCycle();
-            ++calib_count;
 
             pitot.airSpeed = 0.0f;
         }

From cd4f97515a2bc171b8bced78fcf777b7e03f2538 Mon Sep 17 00:00:00 2001
From: jmajew <jmajewsk@meil.pw.edu.pl>
Date: Sat, 5 Aug 2023 10:14:53 +0200
Subject: [PATCH 38/94] cleaning

---
 src/main/common/calibration.c                 |  9 ++-
 src/main/drivers/pitotmeter/pitotmeter.h      |  1 +
 src/main/drivers/pitotmeter/pitotmeter_adc.c  |  1 +
 .../drivers/pitotmeter/pitotmeter_dlvr_l10d.c | 46 ++++--------
 src/main/drivers/pitotmeter/pitotmeter_fake.c |  1 +
 .../drivers/pitotmeter/pitotmeter_ms4525.c    |  2 +
 src/main/drivers/pitotmeter/pitotmeter_msp.c  |  1 +
 .../drivers/pitotmeter/pitotmeter_virtual.c   |  1 +
 src/main/fc/fc_tasks.c                        |  6 +-
 src/main/sensors/pitotmeter.c                 | 72 +++----------------
 src/main/sensors/pitotmeter.h                 |  1 -
 src/main/sensors/sensors.h                    |  2 +-
 12 files changed, 38 insertions(+), 105 deletions(-)

diff --git a/src/main/common/calibration.c b/src/main/common/calibration.c
index 7f979ae3305..bd133145d29 100644
--- a/src/main/common/calibration.c
+++ b/src/main/common/calibration.c
@@ -30,8 +30,9 @@
 #include "drivers/time.h"
 #include "common/calibration.h"
 
+// TODO remove me when debugging is done
 #include "common/log.h"
-#include "build/debug.h"        // TODO remove me
+#include "build/debug.h"        
 
 void zeroCalibrationStartS(zeroCalibrationScalar_t * s, timeMs_t window, float threshold, bool allowFailure)
 {
@@ -79,11 +80,13 @@ void zeroCalibrationAddValueS(zeroCalibrationScalar_t * s, const float v)
     if ((millis() - s->params.startTimeMs) > s->params.windowSizeMs) {
         const float stddev = devStandardDeviation(&s->val.stdDev);
 
-        LOG_DEBUG( PITOT, " ----- CALIB ADD - stddev = %f, threshold = %f, ", (double)(stddev), (double)(s->params.stdDevThreshold) );
+        LOG_DEBUG( PITOT, "zeroCalibrationAddValueS window end reached: stddev = %f, threshold = %f, ", 
+            (double)(stddev), (double)(s->params.stdDevThreshold) );
 
         if (stddev > s->params.stdDevThreshold) {
             if (!s->params.allowFailure) {
-                // If deviation is too big - restart calibration
+                // If deviation is too big && no failure allowed - restart calibration
+                // TODO :: some safeguard should exist which will not allow to keep calibration on for ever
                 s->params.startTimeMs = millis();
                 s->params.sampleCount = 0;
                 s->val.accumulatedValue = 0;
diff --git a/src/main/drivers/pitotmeter/pitotmeter.h b/src/main/drivers/pitotmeter/pitotmeter.h
index 474e488d6cc..562a87d8cd0 100644
--- a/src/main/drivers/pitotmeter/pitotmeter.h
+++ b/src/main/drivers/pitotmeter/pitotmeter.h
@@ -26,6 +26,7 @@ typedef void (*pitotCalculateFuncPtr)(struct pitotDev_s * pitot, float *pressure
 typedef struct pitotDev_s {
     busDevice_t * busDev;
     uint16_t delay;
+    float calibThreshold;
     pitotOpFuncPtr start;
     pitotOpFuncPtr get;
     pitotCalculateFuncPtr calculate;
diff --git a/src/main/drivers/pitotmeter/pitotmeter_adc.c b/src/main/drivers/pitotmeter/pitotmeter_adc.c
index 4d0237b3a95..dcc47caf788 100644
--- a/src/main/drivers/pitotmeter/pitotmeter_adc.c
+++ b/src/main/drivers/pitotmeter/pitotmeter_adc.c
@@ -67,6 +67,7 @@ static void adcPitotCalculate(pitotDev_t *pitot, float *pressure, float *tempera
 bool adcPitotDetect(pitotDev_t *pitot)
 {
     pitot->delay = 10000;
+    pitot->calibThreshold = 0.00001f;   // TODO :: should be tested !!!
     pitot->start = adcPitotStart;
     pitot->get = adcPitotRead;
     pitot->calculate = adcPitotCalculate;
diff --git a/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c b/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
index ae36f2bfea2..62647f86760 100755
--- a/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
+++ b/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
@@ -31,16 +31,6 @@
 #define DLVR_L10D_ADDR                 0x28     // this var is not used !!!
 
 // //---------------------------------------------------
-// //---------------------------------------------------
-// #define C_TO_KELVIN(temp) (temp + 273.15f)
-// #define KELVIN_TO_C(temp) (temp - 273.15f)
-// #define F_TO_KELVIN(temp) C_TO_KELVIN(((temp - 32) * 5/9))
-
-// #define M_PER_SEC_TO_KNOTS 1.94384449f
-// #define KNOTS_TO_M_PER_SEC (1/M_PER_SEC_TO_KNOTS)
-
-// #define KM_PER_HOUR_TO_M_PER_SEC 0.27777778f
-
 // // Gas Constant is from Aerodynamics for Engineering Students, Third Edition, E.L.Houghton and N.B.Carruthers
 // #define ISA_GAS_CONSTANT 287.26f
 // #define ISA_LAPSE_RATE 0.0065f
@@ -50,6 +40,7 @@
 // #define SSL_AIR_DENSITY         1.225f // kg/m^3
 // #define SSL_AIR_PRESSURE 101325.01576f // Pascal
 // #define SSL_AIR_TEMPERATURE    288.15f // K
+// //---------------------------------------------------
 
 #define INCH_OF_H2O_TO_PASCAL   248.84f
 
@@ -58,7 +49,8 @@
 #define RANGE_INCH_H2O      10
 #define DLVR_OFFSET         8192.0f
 #define DLVR_SCALE          16384.0f
-#define DLVR_OFFSET_CORR    0.0f   //-9.0f   // check for other samples of DLVR-L10D; should be 0
+// NOTE :: DLVR_OFFSET_CORR can be used for offset correction. Now firmware relies on zero calibration
+#define DLVR_OFFSET_CORR    0.0f   //-9.0f
 
 
 typedef struct __attribute__ ((__packed__)) dlvrCtx_s {
@@ -78,7 +70,6 @@ static bool dlvr_start(pitotDev_t * pitot)
 static bool dlvr_read(pitotDev_t * pitot)
 {
     uint8_t rxbuf1[4];
-    // uint8_t rxbuf2[4];
 
     dlvrCtx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev);
     ctx->dataValid = false;
@@ -87,34 +78,22 @@ static bool dlvr_read(pitotDev_t * pitot)
         return false;
     }
 
-    // if (!busReadBuf(pitot->busDev, 0xFF, rxbuf2, 4)) {
-    //     return false;
-    // }
-
     // status = 00 -> ok, new data
 	// status = 01 -> reserved
     // status = 10 -> ok, data stale
     // status = 11 -> error
-	// check the status of the first read:
     const uint8_t status = ((rxbuf1[0] & 0xC0) >> 6);
-    if (status == 2 || status == 3) {
+
+    if (status) {
+        // anything other then 00 in the status bits is an error
+        LOG_DEBUG( PITOT, "DLVR: Bad status read. status = %u", (unsigned int)(status) );
         return false;
     }
 
     int16_t dP_raw1, dT_raw1;
-    // int16_t dP_raw2, dT_raw2;
 
     dP_raw1 = 0x3FFF & ((rxbuf1[0] << 8) + rxbuf1[1]);
     dT_raw1 = (0xFFE0 & ((rxbuf1[2] << 8) + rxbuf1[3])) >> 5;
-    // dP_raw2 = 0x3FFF & ((rxbuf2[0] << 8) + rxbuf2[1]);
-    // dT_raw2 = (0xFFE0 & ((rxbuf2[2] << 8) + rxbuf2[3])) >> 5;
-
-    // // reject any double reads where the value has shifted in the upper more than 0xFF
-    // if (ABS(dP_raw1 - dP_raw2) > 0xFF || ABS(dT_raw1 - dT_raw2) > 0xFF) {
-    //     return false;
-    // }
-
-    // LOG_DEBUG( PITOT, "dP_raw1 = %f; dP_raw2 =  %f", (double)dP_raw1, (double)dP_raw2 );
 
     // Data valid, update ut/up values
     ctx->dataValid = true;
@@ -133,18 +112,16 @@ static void dlvr_calculate(pitotDev_t * pitot, float *pressure, float *temperatu
     float dP_inchH2O = 1.25f *  2.0f * RANGE_INCH_H2O  * (((float)ctx->dlvr_up - (DLVR_OFFSET + DLVR_OFFSET_CORR) ) / DLVR_SCALE); 
 
     // LOG_DEBUG( PITOT, "dP_adc = %f; dP_inchH2O =  %f; dP_Pa = %f", (double)ctx->dlvr_up, (double)dP_inchH2O, (double)(INCH_H2O_TO_PASCAL( dP_inchH2O)) );
-
+    // TODO :: remove debug vars
     debug[6] = (int32_t)(ctx->dlvr_up *100);
     debug[7] = (int32_t)((ctx->dlvr_up - (DLVR_OFFSET + DLVR_OFFSET_CORR)) *100);
 
     // temperature in deg C
     float T_C = (float)ctx->dlvr_ut * (200.0f / 2047.0f) - 50.0f;     
 
-    //LOG_DEBUG( PITOT, "t_adc = %f; T_C = %f", (double)ctx->dlvr_ut, (double)T_C );
-
-    // result must fit inside the range
+    // result must fit inside the max pressure range
     if ((dP_inchH2O > RANGE_INCH_H2O) || (dP_inchH2O < -RANGE_INCH_H2O)) {
-        // Debug("DLVR: Out of range pressure %f", dP_inchH2O);
+        LOG_DEBUG( PITOT,"DLVR: Out of range. pressure = %f", (double)(dP_inchH2O) );
         return;
     }
 
@@ -184,7 +161,8 @@ bool dlvrDetect(pitotDev_t * pitot)
     ctx->dlvr_up = 0;
 
     // Initialize pitotDev object
-    pitot->delay = 10000;      // 10000
+    pitot->delay = 10000;
+    pitot->calibThreshold = 0.00001f;   // low noise sensor
     pitot->start = dlvr_start;
     pitot->get = dlvr_read;
     pitot->calculate = dlvr_calculate;
diff --git a/src/main/drivers/pitotmeter/pitotmeter_fake.c b/src/main/drivers/pitotmeter/pitotmeter_fake.c
index f913b76d094..f8302a38d0c 100644
--- a/src/main/drivers/pitotmeter/pitotmeter_fake.c
+++ b/src/main/drivers/pitotmeter/pitotmeter_fake.c
@@ -75,6 +75,7 @@ bool fakePitotDetect(pitotDev_t *pitot)
     fakeTemperature = 273;      // 0C
 
     pitot->delay = 10000;
+    pitot->calibThreshold = 0.00001f;
     pitot->start = fakePitotStart;
     pitot->get = fakePitotRead;
     pitot->calculate = fakePitotCalculate;
diff --git a/src/main/drivers/pitotmeter/pitotmeter_ms4525.c b/src/main/drivers/pitotmeter/pitotmeter_ms4525.c
index 3e2d11bf45c..e6edc796047 100644
--- a/src/main/drivers/pitotmeter/pitotmeter_ms4525.c
+++ b/src/main/drivers/pitotmeter/pitotmeter_ms4525.c
@@ -107,6 +107,7 @@ static void ms4525_calculate(pitotDev_t * pitot, float *pressure, float *tempera
     float dP = dP_psi * PSI_to_Pa;
     float T  = C_TO_KELVIN((float)(200.0f * (int32_t)ctx->ms4525_ut) / 2047.0f - 50.0f);
 
+    // TODO :: remove debug vars
     debug[6] = (int32_t)(ctx->ms4525_up);
     debug[7] = (int32_t)(dP_psi * 1000);
 
@@ -148,6 +149,7 @@ bool ms4525Detect(pitotDev_t * pitot)
 
     // Initialize pitotDev object
     pitot->delay = 10000;
+    pitot->calibThreshold = 0.00004f;   // noisy sensor
     pitot->start = ms4525_start;
     pitot->get = ms4525_read;
     pitot->calculate = ms4525_calculate;
diff --git a/src/main/drivers/pitotmeter/pitotmeter_msp.c b/src/main/drivers/pitotmeter/pitotmeter_msp.c
index 1b9fae47327..f0a8e82768a 100644
--- a/src/main/drivers/pitotmeter/pitotmeter_msp.c
+++ b/src/main/drivers/pitotmeter/pitotmeter_msp.c
@@ -87,6 +87,7 @@ bool mspPitotmeterDetect(pitotDev_t *pitot)
     mspPitotTemperature = 27315;    // 0 deg/c
 
     pitot->delay = 10000;
+    pitot->calibThreshold = 0.00001f;
     pitot->start = mspPitotStart;
     pitot->get = mspPitotRead;
     pitot->calculate = mspPitotCalculate;
diff --git a/src/main/drivers/pitotmeter/pitotmeter_virtual.c b/src/main/drivers/pitotmeter/pitotmeter_virtual.c
index ef3eb8d0343..525fae31007 100644
--- a/src/main/drivers/pitotmeter/pitotmeter_virtual.c
+++ b/src/main/drivers/pitotmeter/pitotmeter_virtual.c
@@ -88,6 +88,7 @@ static void virtualPitotCalculate(pitotDev_t *pitot, float *pressure, float *tem
 bool virtualPitotDetect(pitotDev_t *pitot)
 {
     pitot->delay = 10000;
+    pitot->calibThreshold = 0.00001f;
     pitot->start = virtualPitotStart;
     pitot->get = virtualPitotRead;
     pitot->calculate = virtualPitotCalculate;
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index c1003c18813..cdc1908115f 100755
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -494,7 +494,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
     [TASK_BARO] = {
         .taskName = "BARO",
         .taskFunc = taskUpdateBaro,
-        .desiredPeriod = TASK_PERIOD_MS(50),        //ASK_PERIOD_HZ(20), SPL06 uses oversampilng - result 16ms ?
+        .desiredPeriod = TASK_PERIOD_MS(50),        // SPL06 uses oversampilng - result 16ms ?
         .staticPriority = TASK_PRIORITY_MEDIUM,
     },
 #endif
@@ -503,8 +503,8 @@ cfTask_t cfTasks[TASK_COUNT] = {
     [TASK_PITOT] = {
         .taskName = "PITOT",
         .taskFunc = taskUpdatePitot,
-        .desiredPeriod = TASK_PERIOD_MS(20),    //TASK_PERIOD_HZ(100)
-        .staticPriority = TASK_PRIORITY_MEDIUM,    //TASK_PRIORITY_MEDIUM_HIGH, TASK_PRIORITY_MEDIUM,
+        .desiredPeriod = TASK_PERIOD_MS(20),
+        .staticPriority = TASK_PRIORITY_MEDIUM,
     },
 #endif
 
diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c
index 9dd08a5a5cb..034dab475a3 100755
--- a/src/main/sensors/pitotmeter.c
+++ b/src/main/sensors/pitotmeter.c
@@ -95,19 +95,8 @@ bool pitotDetect(pitotDev_t *dev, uint8_t pitotHardwareToUse)
             FALLTHROUGH;
 
         case PITOT_DLVR:
-// #ifdef USE_PITOT_DLVR
-//             if (dlvrDetect(dev)) {
-//                 pitotHardware = PITOT_DLVR;
-//                 break;
-//             }
-// #endif
-//             /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
-//             if (pitotHardwareToUse != PITOT_AUTODETECT) {
-//                 break;
-//             }
-//             FALLTHROUGH;
-
-			// Skip autodetection for DLVR (it is indistinguishable from MS4525), only allow manual config
+
+			// Skip autodetection for DLVR (it is indistinguishable from MS4525) and allow only manual config
             if (pitotHardwareToUse != PITOT_AUTODETECT && dlvrDetect(dev)) {
                 pitotHardware = PITOT_DLVR;
                 break;
@@ -197,9 +186,7 @@ bool pitotIsCalibrationComplete(void)
 
 void pitotStartCalibration(void)
 {
-    // problem for ms4525 with threshold  to low when not smoothedd
-//    zeroCalibrationStartS(&pitot.zeroCalibration, CALIBRATING_PITOT_TIME_MS, SSL_AIR_PRESSURE * 0.00001f, false);
-    zeroCalibrationStartS(&pitot.zeroCalibration, CALIBRATING_PITOT_TIME_MS, SSL_AIR_PRESSURE * 0.00004f, false);
+    zeroCalibrationStartS(&pitot.zeroCalibration, CALIBRATING_PITOT_TIME_MS, SSL_AIR_PRESSURE * pitot.dev.calibThreshold, false);
 }
 
 static void performPitotCalibrationCycle(void)
@@ -239,9 +226,6 @@ STATIC_PROTOTHREAD(pitotThread)
             }        
         }
 
-        // LOG_DEBUG( PITOT, "cur delay = %d, req delay = %d", (int)(millis() - pitot.lastSeenHealthyMs),  (int)US2MS(pitot.dev.delay) );
-
-        // if ( (millis() - pitot.lastSeenHealthyMs) >= 10) {
         if ( (millis() - pitot.lastSeenHealthyMs) >= US2MS(pitot.dev.delay)) {
             if (pitot.dev.get(&pitot.dev))          // read current data
                 pitot.lastSeenHealthyMs = millis();
@@ -265,19 +249,9 @@ STATIC_PROTOTHREAD(pitotThread)
 #endif
         ptYield();
 
-        // // Filter pressure - NOTE : do not filter during calibration !!!
-        // currentTimeUs = micros();
-        // pitot.pressure = pt1FilterApply3(&pitot.lpfState, pitotPressureTmp, US2S(currentTimeUs - pitot.lastMeasurementUs));
-        // pitot.lastMeasurementUs = currentTimeUs;
-
-        // currentTimeUs = micros();
-        // float p = pt1FilterApply3(&pitot.lpfState, pitotPressureTmp, US2S(currentTimeUs - pitot.lastMeasurementUs));
-        // pitot.pressure = p; //pitotPressureTmp;
-        // pitot.lastMeasurementUs = micros();
-
-
         // Calculate IAS
         if (pitotIsCalibrationComplete()) {
+            // NOTE ::
             // https://en.wikipedia.org/wiki/Indicated_airspeed
             // Indicated airspeed (IAS) is the airspeed read directly from the airspeed indicator on an aircraft, driven by the pitot-static system.
             // The IAS is an important value for the pilot because it is the indicated speeds which are specified in the aircraft flight manual for
@@ -287,56 +261,28 @@ STATIC_PROTOTHREAD(pitotThread)
             // Therefore we shouldn't care about CAS/TAS and only calculate IAS since it's more indicative to the pilot and more useful in calculations
             // It also allows us to use pitot_scale to calibrate the dynamic pressure sensor scale
 
-            // filter pressure
-            // do NOT filter only when calibrating
+            // NOTE ::filter pressure - apply filter when NOT calibrating for zero !!!
             currentTimeUs = micros();
             pitot.pressure = pt1FilterApply3(&pitot.lpfState, pitotPressureTmp, US2S(currentTimeUs - pitot.lastMeasurementUs));
             pitot.lastMeasurementUs = currentTimeUs;
 
             pitot.airSpeed = pitotmeterConfig()->pitot_scale * fast_fsqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / SSL_AIR_DENSITY) * 100;  // cm/s
 
-// #if 1
-//             {
-//                 // filter pressure
-//                 // do NOT filter only when calibrating
-//                 currentTimeUs = micros();
-//                 pitot.pressure = pt1FilterApply3(&pitot.lpfState, pitotPressureTmp, US2S(currentTimeUs - pitot.lastMeasurementUs));
-//                 pitot.lastMeasurementUs = currentTimeUs;
-//
-//                 pitot.airSpeed = pitotmeterConfig()->pitot_scale * fast_fsqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / SSL_AIR_DENSITY) * 100;  // cm/s
-//             }
-// #else            
-//             {
-//                 // filter airspeed
-//                 pitot.pressure = pitotPressureTmp;
-//
-//                 float airSpeedTmp = pitotmeterConfig()->pitot_scale * fast_fsqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / SSL_AIR_DENSITY) * 100;  // cm/s
-//
-//                 // do filter only when NOT calibrating
-//                 currentTimeUs = micros();
-//                 pitot.airSpeed = pt1FilterApply3(&pitot.lpfState, airSpeedTmp, US2S(currentTimeUs - pitot.lastMeasurementUs));
-//                 pitot.lastMeasurementUs = currentTimeUs;
-//             }
-// #endif            
-
-
             pitot.temperature = pitotTemperatureTmp;   // Kelvin
 
+            // NOTE :: actual density can be calculated using baro pressure and temperature
+
+            // TODO :: remove debug vars
             debug[0] = (int32_t)(pitot.pressure * 1000);
             debug[1] = (int32_t)(pitot.pressureZero * 1000);
             debug[2] = (int32_t)((pitot.pressure - pitot.pressureZero) * 1000);
             debug[3] = (int32_t)(pitot.temperature *1000);
             // debug[4] = (int32_t)(baro.baroPressure);
 
-            // LOG_DEBUG( PITOT, " ----- OUT pZero = %f, ", (double)(pitot.pressureZero) );
-
-            // LOG_DEBUG( PITOT, "calib_count = %d, period_ms = %d", (int)(calib_count), (int)(millis() - calibPeriod_ms) );
-
         } else {
             pitot.pressure = pitotPressureTmp;
 
-            // LOG_DEBUG( PITOT, " ----- INSIDE CALIB PART pZero = %f, ", (double)(pitot.pressureZero) );
-
+            // TODO :: remove debug vars
             debug[0] = pitot.pressure * 1000;
 
             performPitotCalibrationCycle();
diff --git a/src/main/sensors/pitotmeter.h b/src/main/sensors/pitotmeter.h
index a24fba665ca..3db1b39582f 100755
--- a/src/main/sensors/pitotmeter.h
+++ b/src/main/sensors/pitotmeter.h
@@ -56,7 +56,6 @@ typedef struct pito_s {
 
     float pressureZero;
     float pressure;
-    
     float temperature;
 } pitot_t;
 
diff --git a/src/main/sensors/sensors.h b/src/main/sensors/sensors.h
index 07b6285c2d6..2b706f7dfc6 100644
--- a/src/main/sensors/sensors.h
+++ b/src/main/sensors/sensors.h
@@ -40,7 +40,7 @@ typedef union flightDynamicsTrims_u {
 } flightDynamicsTrims_t;
 
 #define CALIBRATING_BARO_TIME_MS            2000
-#define CALIBRATING_PITOT_TIME_MS           4000 //2000
+#define CALIBRATING_PITOT_TIME_MS           4000
 #define CALIBRATING_GYRO_TIME_MS            2000
 #define CALIBRATING_ACC_TIME_MS             500
 #define CALIBRATING_GYRO_MORON_THRESHOLD    32

From 81f50f601810440ed8d53396cbc49ebe9a92e2b3 Mon Sep 17 00:00:00 2001
From: jmajew <jmajewsk@meil.pw.edu.pl>
Date: Sat, 5 Aug 2023 11:23:35 +0200
Subject: [PATCH 39/94] ms4525 calib_threshold correction

---
 src/main/drivers/pitotmeter/pitotmeter_ms4525.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/drivers/pitotmeter/pitotmeter_ms4525.c b/src/main/drivers/pitotmeter/pitotmeter_ms4525.c
index e6edc796047..d3a5b2b2b1e 100644
--- a/src/main/drivers/pitotmeter/pitotmeter_ms4525.c
+++ b/src/main/drivers/pitotmeter/pitotmeter_ms4525.c
@@ -149,7 +149,7 @@ bool ms4525Detect(pitotDev_t * pitot)
 
     // Initialize pitotDev object
     pitot->delay = 10000;
-    pitot->calibThreshold = 0.00004f;   // noisy sensor
+    pitot->calibThreshold = 0.00005f;   // noisy sensor
     pitot->start = ms4525_start;
     pitot->get = ms4525_read;
     pitot->calculate = ms4525_calculate;

From 3859d30125c4cbe57436587a9423dfffc96a4ece Mon Sep 17 00:00:00 2001
From: jmajew <jmajewsk@meil.pw.edu.pl>
Date: Sat, 5 Aug 2023 16:26:47 +0200
Subject: [PATCH 40/94] cleaning cont.

---
 src/main/CMakeLists.txt                         |  2 +-
 src/main/common/calibration.c                   |  8 +-------
 .../drivers/pitotmeter/pitotmeter_dlvr_l10d.c   |  5 -----
 src/main/drivers/pitotmeter/pitotmeter_ms4525.c |  5 -----
 src/main/sensors/pitotmeter.c                   | 17 +----------------
 src/main/sensors/pitotmeter.h                   |  2 +-
 6 files changed, 4 insertions(+), 35 deletions(-)

diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt
index 7478945ebce..486457c6a5f 100755
--- a/src/main/CMakeLists.txt
+++ b/src/main/CMakeLists.txt
@@ -208,7 +208,7 @@ main_sources(COMMON_SRC
     drivers/pitotmeter/pitotmeter_adc.h
     drivers/pitotmeter/pitotmeter_ms4525.c
     drivers/pitotmeter/pitotmeter_ms4525.h
-	drivers/pitotmeter/pitotmeter_dlvr_l10d.c
+    drivers/pitotmeter/pitotmeter_dlvr_l10d.c
     drivers/pitotmeter/pitotmeter_dlvr_l10d.h	
     drivers/pitotmeter/pitotmeter_msp.c
     drivers/pitotmeter/pitotmeter_msp.h
diff --git a/src/main/common/calibration.c b/src/main/common/calibration.c
index bd133145d29..69420c59f77 100644
--- a/src/main/common/calibration.c
+++ b/src/main/common/calibration.c
@@ -30,9 +30,6 @@
 #include "drivers/time.h"
 #include "common/calibration.h"
 
-// TODO remove me when debugging is done
-#include "common/log.h"
-#include "build/debug.h"        
 
 void zeroCalibrationStartS(zeroCalibrationScalar_t * s, timeMs_t window, float threshold, bool allowFailure)
 {
@@ -80,13 +77,10 @@ void zeroCalibrationAddValueS(zeroCalibrationScalar_t * s, const float v)
     if ((millis() - s->params.startTimeMs) > s->params.windowSizeMs) {
         const float stddev = devStandardDeviation(&s->val.stdDev);
 
-        LOG_DEBUG( PITOT, "zeroCalibrationAddValueS window end reached: stddev = %f, threshold = %f, ", 
-            (double)(stddev), (double)(s->params.stdDevThreshold) );
-
         if (stddev > s->params.stdDevThreshold) {
             if (!s->params.allowFailure) {
                 // If deviation is too big && no failure allowed - restart calibration
-                // TODO :: some safeguard should exist which will not allow to keep calibration on for ever
+                // TODO :: some safeguard should exist which will not allow to keep on calibrating for ever
                 s->params.startTimeMs = millis();
                 s->params.sampleCount = 0;
                 s->val.accumulatedValue = 0;
diff --git a/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c b/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
index 62647f86760..d899ac05a9e 100755
--- a/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
+++ b/src/main/drivers/pitotmeter/pitotmeter_dlvr_l10d.c
@@ -111,11 +111,6 @@ static void dlvr_calculate(pitotDev_t * pitot, float *pressure, float *temperatu
     // pressure in inchH2O
     float dP_inchH2O = 1.25f *  2.0f * RANGE_INCH_H2O  * (((float)ctx->dlvr_up - (DLVR_OFFSET + DLVR_OFFSET_CORR) ) / DLVR_SCALE); 
 
-    // LOG_DEBUG( PITOT, "dP_adc = %f; dP_inchH2O =  %f; dP_Pa = %f", (double)ctx->dlvr_up, (double)dP_inchH2O, (double)(INCH_H2O_TO_PASCAL( dP_inchH2O)) );
-    // TODO :: remove debug vars
-    debug[6] = (int32_t)(ctx->dlvr_up *100);
-    debug[7] = (int32_t)((ctx->dlvr_up - (DLVR_OFFSET + DLVR_OFFSET_CORR)) *100);
-
     // temperature in deg C
     float T_C = (float)ctx->dlvr_ut * (200.0f / 2047.0f) - 50.0f;     
 
diff --git a/src/main/drivers/pitotmeter/pitotmeter_ms4525.c b/src/main/drivers/pitotmeter/pitotmeter_ms4525.c
index d3a5b2b2b1e..c5272ccb217 100644
--- a/src/main/drivers/pitotmeter/pitotmeter_ms4525.c
+++ b/src/main/drivers/pitotmeter/pitotmeter_ms4525.c
@@ -107,11 +107,6 @@ static void ms4525_calculate(pitotDev_t * pitot, float *pressure, float *tempera
     float dP = dP_psi * PSI_to_Pa;
     float T  = C_TO_KELVIN((float)(200.0f * (int32_t)ctx->ms4525_ut) / 2047.0f - 50.0f);
 
-    // TODO :: remove debug vars
-    debug[6] = (int32_t)(ctx->ms4525_up);
-    debug[7] = (int32_t)(dP_psi * 1000);
-
-
     if (pressure) {
         *pressure = dP;    // Pa
     }
diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c
index 034dab475a3..8d2b29c075c 100755
--- a/src/main/sensors/pitotmeter.c
+++ b/src/main/sensors/pitotmeter.c
@@ -49,7 +49,7 @@
 #include "sensors/sensors.h"
 
 
-#include "build/debug.h"
+//#include "build/debug.h"
 
 
 #ifdef USE_PITOT
@@ -267,26 +267,11 @@ STATIC_PROTOTHREAD(pitotThread)
             pitot.lastMeasurementUs = currentTimeUs;
 
             pitot.airSpeed = pitotmeterConfig()->pitot_scale * fast_fsqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / SSL_AIR_DENSITY) * 100;  // cm/s
-
             pitot.temperature = pitotTemperatureTmp;   // Kelvin
 
-            // NOTE :: actual density can be calculated using baro pressure and temperature
-
-            // TODO :: remove debug vars
-            debug[0] = (int32_t)(pitot.pressure * 1000);
-            debug[1] = (int32_t)(pitot.pressureZero * 1000);
-            debug[2] = (int32_t)((pitot.pressure - pitot.pressureZero) * 1000);
-            debug[3] = (int32_t)(pitot.temperature *1000);
-            // debug[4] = (int32_t)(baro.baroPressure);
-
         } else {
             pitot.pressure = pitotPressureTmp;
-
-            // TODO :: remove debug vars
-            debug[0] = pitot.pressure * 1000;
-
             performPitotCalibrationCycle();
-
             pitot.airSpeed = 0.0f;
         }
 
diff --git a/src/main/sensors/pitotmeter.h b/src/main/sensors/pitotmeter.h
index 3db1b39582f..aed924f8e4c 100755
--- a/src/main/sensors/pitotmeter.h
+++ b/src/main/sensors/pitotmeter.h
@@ -31,7 +31,7 @@ typedef enum {
     PITOT_VIRTUAL = 4,
     PITOT_FAKE = 5,
     PITOT_MSP = 6,
-	PITOT_DLVR = 7,
+    PITOT_DLVR = 7,
 } pitotSensor_e;
 
 #define PITOT_MAX  PITOT_FAKE

From 357e9b0e3ca63f35baf055a95045408e656b38f7 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sat, 5 Aug 2023 16:52:27 +0200
Subject: [PATCH 41/94] SpeedyBee F7 Mini 2

---
 src/main/target/SPEEDYBEEF7MINI/CMakeLists.txt | 1 +
 src/main/target/SPEEDYBEEF7MINI/target.h       | 9 +++++++++
 2 files changed, 10 insertions(+)

diff --git a/src/main/target/SPEEDYBEEF7MINI/CMakeLists.txt b/src/main/target/SPEEDYBEEF7MINI/CMakeLists.txt
index c7a92612742..d1b5f7e91f8 100644
--- a/src/main/target/SPEEDYBEEF7MINI/CMakeLists.txt
+++ b/src/main/target/SPEEDYBEEF7MINI/CMakeLists.txt
@@ -1 +1,2 @@
 target_stm32f722xe(SPEEDYBEEF7MINI)
+target_stm32f722xe(SPEEDYBEEF7MINI2)
diff --git a/src/main/target/SPEEDYBEEF7MINI/target.h b/src/main/target/SPEEDYBEEF7MINI/target.h
index f6db8e86fa4..e95101469cf 100644
--- a/src/main/target/SPEEDYBEEF7MINI/target.h
+++ b/src/main/target/SPEEDYBEEF7MINI/target.h
@@ -40,6 +40,15 @@
 #define MPU6000_CS_PIN          PB2
 #define MPU6000_SPI_BUS         BUS_SPI1
 
+#ifdef SPEEDYBEEF7MINI2
+
+#define USE_IMU_BMI270
+#define IMU_BMI270_ALIGN        CW0_DEG
+#define BMI270_CS_PIN           PB2
+#define BMI270_SPI_BUS          BUS_SPI1
+
+#endif
+
 // *************** I2C /Baro/Mag *********************
 #define USE_I2C
 #define USE_I2C_DEVICE_1

From 6eb19f2135232f64caebfe399322dc769d511fd4 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Sat, 5 Aug 2023 22:28:12 +0100
Subject: [PATCH 42/94] first build

---
 docs/Settings.md                             | 20 ++---
 src/main/cms/cms_menu_navigation.c           |  2 +-
 src/main/fc/fc_msp.c                         | 20 ++---
 src/main/fc/settings.yaml                    | 13 ++--
 src/main/io/osd.c                            | 78 ++++++++++----------
 src/main/navigation/navigation.c             |  6 +-
 src/main/navigation/navigation.h             | 10 ++-
 src/main/navigation/navigation_multicopter.c | 17 ++---
 8 files changed, 88 insertions(+), 78 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index dda075cee64..73efc23c83e 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -3272,6 +3272,16 @@ Max allowed above the ground altitude for terrain following mode
 
 ---
 
+### nav_mc_althold_throttle
+
+If set to STICK the FC remembers the throttle stick position when enabling ALTHOLD and treats it as the neutral midpoint for holding altitude. If set to MID_STICK or HOVER the neutral midpoint is set to the mid stick position or the hover throttle position respectively.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| STICK |  |  |
+
+---
+
 ### nav_mc_bank_angle
 
 Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude
@@ -3742,16 +3752,6 @@ Enables or Disables the use of the heading PID controller on fixed wing. Heading
 
 ---
 
-### nav_use_midthr_for_althold
-
-If set to OFF, the FC remembers your throttle stick position when enabling ALTHOLD and treats it as a neutral midpoint for holding altitude
-
-| Default | Min | Max |
-| --- | --- | --- |
-| OFF | OFF | ON |
-
----
-
 ### nav_user_control_mode
 
 Defines how Pitch/Roll input from RC receiver affects flight in POSHOLD mode: ATTI - pitch/roll controls attitude like in ANGLE mode; CRUISE - pitch/roll controls velocity in forward and right direction.
diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c
index d89cb2c249b..a54843afc40 100644
--- a/src/main/cms/cms_menu_navigation.c
+++ b/src/main/cms/cms_menu_navigation.c
@@ -48,7 +48,7 @@ static const OSD_Entry cmsx_menuNavSettingsEntries[] =
     OSD_SETTING_ENTRY("MAX NAV CLIMB RATE", SETTING_NAV_AUTO_CLIMB_RATE),
     OSD_SETTING_ENTRY("MAX AH CLIMB RATE", SETTING_NAV_MANUAL_CLIMB_RATE),
     OSD_SETTING_ENTRY("MC MAX BANK ANGLE", SETTING_NAV_MC_BANK_ANGLE),
-    OSD_SETTING_ENTRY("MID THR FOR AH", SETTING_NAV_USE_MIDTHR_FOR_ALTHOLD),
+    OSD_SETTING_ENTRY("MC ALTHOLD THROT", SETTING_NAV_MC_ALTHOLD_THROTTLE),
     OSD_SETTING_ENTRY("MC HOVER THR", SETTING_NAV_MC_HOVER_THR),
     OSD_SETTING_ENTRY("LANDING DISARM", SETTING_NAV_DISARM_ON_LANDING),
 
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index dd28ae61663..821e4526337 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -1058,7 +1058,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
             legacyLedConfig |= ledConfig->led_function << shiftCount;
             shiftCount += 4;
             legacyLedConfig |= (ledConfig->led_overlay & 0x3F) << (shiftCount);
-            shiftCount += 6; 
+            shiftCount += 6;
             legacyLedConfig |= (ledConfig->led_color) << (shiftCount);
             shiftCount += 4;
             legacyLedConfig |= (ledConfig->led_direction) << (shiftCount);
@@ -1289,7 +1289,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         sbufWriteU16(dst, navConfig()->general.max_manual_speed);
         sbufWriteU16(dst, navConfig()->general.max_manual_climb_rate);
         sbufWriteU8(dst, navConfig()->mc.max_bank_angle);
-        sbufWriteU8(dst, navConfig()->general.flags.use_thr_mid_for_althold);
+        sbufWriteU8(dst, navConfig()->mc.althold_throttle_type);
         sbufWriteU16(dst, currentBatteryProfile->nav.mc.hover_throttle);
         break;
 
@@ -2255,7 +2255,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             navConfigMutable()->general.max_manual_speed = sbufReadU16(src);
             navConfigMutable()->general.max_manual_climb_rate = sbufReadU16(src);
             navConfigMutable()->mc.max_bank_angle = sbufReadU8(src);
-            navConfigMutable()->general.flags.use_thr_mid_for_althold = sbufReadU8(src);
+            navConfigMutable()->mc.althold_throttle_type = sbufReadU8(src);
             currentBatteryProfileMutable->nav.mc.hover_throttle = sbufReadU16(src);
         } else
             return MSP_RESULT_ERROR;
@@ -2757,7 +2757,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             ledConfig->led_position = legacyConfig & 0xFF;
             ledConfig->led_function = (legacyConfig >> 8) & 0xF;
             ledConfig->led_overlay = (legacyConfig >> 12) & 0x3F;
-            ledConfig->led_color = (legacyConfig >> 18) & 0xF; 
+            ledConfig->led_color = (legacyConfig >> 18) & 0xF;
             ledConfig->led_direction = (legacyConfig >> 22) & 0x3F;
             ledConfig->led_params = (legacyConfig >> 28) & 0xF;
 
@@ -3482,7 +3482,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
 #ifdef USE_SIMULATOR
     case MSP_SIMULATOR:
 		tmp_u8 = sbufReadU8(src); // Get the Simulator MSP version
-        
+
         // Check the MSP version of simulator
 		if (tmp_u8 != SIMULATOR_MSP_VERSION) {
             break;
@@ -3508,7 +3508,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
                 // Review: Many states were affected. Reboot?
 
 				disarm(DISARM_SWITCH);  // Disarm to prevent motor output!!!
-			}   
+			}
         } else {
 			if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once
 #ifdef USE_BARO
@@ -3518,7 +3518,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
                     DISABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE);
 				    baroStartCalibration();
                 }
-#endif			
+#endif
 
 #ifdef USE_MAG
 				if (compassConfig()->mag_hardware != MAG_NONE) {
@@ -3578,7 +3578,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
 				} else {
 					sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT);
 				}
-                
+
                 // Get the acceleration in 1G units
 				acc.accADCf[X] = ((int16_t)sbufReadU16(src)) / 1000.0f;
 				acc.accADCf[Y] = ((int16_t)sbufReadU16(src)) / 1000.0f;
@@ -3586,7 +3586,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
 				acc.accVibeSq[X] = 0.0f;
 				acc.accVibeSq[Y] = 0.0f;
 				acc.accVibeSq[Z] = 0.0f;
-                
+
                 // Get the angular velocity in DPS
 				gyro.gyroADCf[X] = ((int16_t)sbufReadU16(src)) / 16.0f;
 				gyro.gyroADCf[Y] = ((int16_t)sbufReadU16(src)) / 16.0f;
@@ -3621,7 +3621,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
                     simulatorData.airSpeed = sbufReadU16(src);
 			    } else {
                     if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) {
-                        sbufReadU16(src); 
+                        sbufReadU16(src);
                     }
                 }
 
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index a0904af6f32..b9feeddc065 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -190,6 +190,9 @@ tables:
   - name: nav_fw_wp_turn_smoothing
     values: ["OFF", "ON", "ON-CUT"]
     enum: wpFwTurnSmoothing_e
+  - name: nav_mc_althold_throttle
+    values: ["STICK", "MID_STICK", "HOVER"]
+    enum: navMcAltHoldThrottle_e
 
 constants:
   RPYL_PID_MIN: 0
@@ -2310,11 +2313,11 @@ groups:
         default_value: OFF
         field: general.flags.landing_bump_detection
         type: bool
-      - name: nav_use_midthr_for_althold
-        description: "If set to OFF, the FC remembers your throttle stick position when enabling ALTHOLD and treats it as a neutral midpoint for holding altitude"
-        default_value: OFF
-        field: general.flags.use_thr_mid_for_althold
-        type: bool
+      - name: nav_mc_althold_throttle
+        description: "If set to STICK the FC remembers the throttle stick position when enabling ALTHOLD and treats it as the neutral midpoint for holding altitude. If set to MID_STICK or HOVER the neutral midpoint is set to the mid stick position or the hover throttle position respectively."
+        default_value: "STICK"
+        field: mc.althold_throttle_type
+        table: nav_mc_althold_throttle
       - name: nav_extra_arming_safety
         description: "If set to ON drone won't arm if no GPS fix and any navigation mode like RTH or POSHOLD is configured. ALLOW_BYPASS allows the user to momentarily disable this check by holding yaw high (left stick held at the bottom right in mode 2) when switch arming is used"
         default_value: "ALLOW_BYPASS"
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 63312af46c2..dacc897eb71 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -542,7 +542,7 @@ void osdFormatAltitudeSymbol(char *buff, int32_t alt)
         buff[0] = ' ';
     }
 
-#ifndef DISABLE_MSP_BF_COMPAT   // IF BFCOMPAT is not supported, there's no need to check for it and change the values  
+#ifndef DISABLE_MSP_BF_COMPAT   // IF BFCOMPAT is not supported, there's no need to check for it and change the values
     if (isBfCompatibleVideoSystem(osdConfig())) {
         totalDigits++;
         digits++;
@@ -636,8 +636,8 @@ static inline void osdFormatFlyTime(char *buff, textAttributes_t *attr)
     }
 }
 
-/** 
- * Trim whitespace from string. 
+/**
+ * Trim whitespace from string.
  * Used in Stats screen on lines with multiple values.
 */
 char *osdFormatTrimWhiteSpace(char *buff)
@@ -648,7 +648,7 @@ char *osdFormatTrimWhiteSpace(char *buff)
     while(isspace((unsigned char)*buff)) buff++;
 
     // All spaces?
-    if(*buff == 0)  
+    if(*buff == 0)
     return buff;
 
     // Trim trailing spaces
@@ -1094,7 +1094,7 @@ void osdCrosshairPosition(uint8_t *x, uint8_t *y)
  * Check if this OSD layout is using scaled or unscaled throttle.
  * If both are used, it will default to scaled.
  */
-bool osdUsingScaledThrottle(void) 
+bool osdUsingScaledThrottle(void)
 {
     bool usingScaledThrottle = OSD_VISIBLE(osdLayoutsConfig()->item_pos[currentLayout][OSD_SCALED_THROTTLE_POS]);
     bool usingRCThrottle = OSD_VISIBLE(osdLayoutsConfig()->item_pos[currentLayout][OSD_THROTTLE_POS]);
@@ -2022,6 +2022,10 @@ static bool osdDrawSingleElement(uint8_t item)
 
                 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
             }
+            if (STATE(MULTIROTOR) && posControl.flags.isAdjustingAltitude) {
+                TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr);
+                TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+            }
             break;
         }
 
@@ -2979,7 +2983,7 @@ static bool osdDrawSingleElement(uint8_t item)
                         tfp_sprintf(buff, "%s%c", buff, SYM_AH_MI);
                     }
                     if (!efficiencyValid) {
-                        buff[0] = buff[1] = buff[2] = buff[3] = '-';    
+                        buff[0] = buff[1] = buff[2] = buff[3] = '-';
                         buff[digits] = SYM_MAH_MI_0;        // This will overwrite the "-" at buff[3] if not in BFCOMPAT mode
                         buff[digits + 1] = SYM_MAH_MI_1;
                         buff[digits + 2] = '\0';
@@ -2993,7 +2997,7 @@ static bool osdDrawSingleElement(uint8_t item)
                         tfp_sprintf(buff, "%s%c", buff, SYM_AH_NM);
                     }
                     if (!efficiencyValid) {
-                        buff[0] = buff[1] = buff[2] = buff[3] = '-';    
+                        buff[0] = buff[1] = buff[2] = buff[3] = '-';
                         buff[digits] = SYM_MAH_NM_0;
                         buff[digits + 1] = SYM_MAH_NM_1;
                         buff[digits + 2] = '\0';
@@ -3009,7 +3013,7 @@ static bool osdDrawSingleElement(uint8_t item)
                         tfp_sprintf(buff, "%s%c", buff, SYM_AH_KM);
                     }
                     if (!efficiencyValid) {
-                        buff[0] = buff[1] = buff[2] = buff[3] = '-';    
+                        buff[0] = buff[1] = buff[2] = buff[3] = '-';
                         buff[digits] = SYM_MAH_KM_0;
                         buff[digits + 1] = SYM_MAH_KM_1;
                         buff[digits + 2] = '\0';
@@ -4094,7 +4098,7 @@ static void osdUpdateStats(void)
 static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
 {
     const char * disarmReasonStr[DISARM_REASON_COUNT] = { "UNKNOWN", "TIMEOUT", "STICKS", "SWITCH", "SWITCH", "KILLSW", "FAILSAFE", "NAV SYS", "LANDING"};
-    uint8_t top = 1;  // Start one line down leaving space at the top of the screen. 
+    uint8_t top = 1;  // Start one line down leaving space at the top of the screen.
     size_t multiValueLengthOffset = 0;
 
     const uint8_t statNameX = osdDisplayIsHD() ? 11 : 1;
@@ -4116,14 +4120,14 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
     }
 
     if (isSinglePageStatsCompatible || page == 0) {
-        if (feature(FEATURE_GPS)) {            
+        if (feature(FEATURE_GPS)) {
             if (isSinglePageStatsCompatible) {
                 displayWrite(osdDisplayPort, statNameX, top, "MAX/AVG SPEED    :");
                 osdFormatVelocityStr(buff, stats.max_3D_speed, true, false);
                 osdLeftAlignString(buff);
                 strcat(osdFormatTrimWhiteSpace(buff),"/");
                 multiValueLengthOffset = strlen(buff);
-                displayWrite(osdDisplayPort, statValuesX, top, buff);            
+                displayWrite(osdDisplayPort, statValuesX, top, buff);
                 osdGenerateAverageVelocityStr(buff);
                 osdLeftAlignString(buff);
                 displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff);
@@ -4160,7 +4164,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
                     osdLeftAlignString(buff);
                     strcat(osdFormatTrimWhiteSpace(buff), "%/");
                     multiValueLengthOffset = strlen(buff);
-                    displayWrite(osdDisplayPort, statValuesX, top, buff);                
+                    displayWrite(osdDisplayPort, statValuesX, top, buff);
                     itoa(stats.min_rssi_dbm, buff, 10);
                     tfp_sprintf(buff, "%s%c", buff, SYM_DBM);
                     osdLeftAlignString(buff);
@@ -4175,7 +4179,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
                     itoa(stats.min_rssi_dbm, buff, 10);
                     tfp_sprintf(buff, "%s%c", buff, SYM_DBM);
                     displayWrite(osdDisplayPort, statValuesX, top++, buff);
-                } 
+                }
 
                 displayWrite(osdDisplayPort, statNameX, top, "MIN LQ           :");
                 itoa(stats.min_lq, buff, 10);
@@ -4201,7 +4205,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
         displayWrite(osdDisplayPort, statNameX, top, "DISARMED BY      :");
         displayWrite(osdDisplayPort, statValuesX, top++, disarmReasonStr[getDisarmReason()]);
     }
-    
+
     if (isSinglePageStatsCompatible || page == 1) {
         if (osdConfig()->stats_min_voltage_unit == OSD_STATS_MIN_VOLTAGE_UNIT_BATTERY) {
             displayWrite(osdDisplayPort, statNameX, top, "MIN BATTERY VOLT :");
@@ -4323,20 +4327,20 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
             }
         }
 
-        const float max_gforce = accGetMeasuredMaxG();    
+        const float max_gforce = accGetMeasuredMaxG();
         displayWrite(osdDisplayPort, statNameX, top, "MAX G-FORCE      :");
         osdFormatCentiNumber(buff, max_gforce * 100, 0, 2, 0, 3);
         displayWrite(osdDisplayPort, statValuesX, top++, buff);
 
-        const acc_extremes_t *acc_extremes = accGetMeasuredExtremes();        
+        const acc_extremes_t *acc_extremes = accGetMeasuredExtremes();
         const float acc_extremes_min = acc_extremes[Z].min;
         const float acc_extremes_max = acc_extremes[Z].max;
         displayWrite(osdDisplayPort, statNameX, top, "MIN/MAX Z G-FORCE:");
         osdFormatCentiNumber(buff, acc_extremes_min * 100, 0, 2, 0, 4);
         osdLeftAlignString(buff);
-        strcat(osdFormatTrimWhiteSpace(buff),"/"); 
-        multiValueLengthOffset = strlen(buff);        
-        displayWrite(osdDisplayPort, statValuesX, top, buff);        
+        strcat(osdFormatTrimWhiteSpace(buff),"/");
+        multiValueLengthOffset = strlen(buff);
+        displayWrite(osdDisplayPort, statValuesX, top, buff);
         osdFormatCentiNumber(buff, acc_extremes_max * 100, 0, 2, 0, 3);
         osdLeftAlignString(buff);
         displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff);
@@ -4546,41 +4550,41 @@ static void osdRefresh(timeUs_t currentTimeUs)
             statsCurrentPage = 0;
             statsAutoPagingEnabled = osdConfig()->stats_page_auto_swap_time > 0 ? true : false;
             osdShowStats(statsSinglePageCompatible, statsCurrentPage);
-            osdSetNextRefreshIn(STATS_SCREEN_DISPLAY_TIME);            
+            osdSetNextRefreshIn(STATS_SCREEN_DISPLAY_TIME);
         }
 
         armState = ARMING_FLAG(ARMED);
     }
 
     // This block is entered when we're showing the "Splash", "Armed" or "Stats" screens
-    if (resumeRefreshAt) {   
-                                
+    if (resumeRefreshAt) {
+
         // Handle events only when the "Stats" screen is being displayed.
         if (statsDisplayed) {
 
              // Manual paging stick commands are only applicable to multi-page stats.
              // ******************************
-             // For single-page stats, this effectively disables the ability to cancel the 
+             // For single-page stats, this effectively disables the ability to cancel the
              // automatic paging/updates with the stick commands. So unless stats_page_auto_swap_time
-             // is set to 0 or greater than 4 (saved settings display interval is 5 seconds), then 
-             // "Saved Settings" should display if it is active within the refresh interval. 
+             // is set to 0 or greater than 4 (saved settings display interval is 5 seconds), then
+             // "Saved Settings" should display if it is active within the refresh interval.
              // ******************************
              // With multi-page stats, "Saved Settings" could also be missed if the user
-             // has canceled automatic paging using the stick commands, because that is only 
-             // updated when osdShowStats() is called. So, in that case, they would only see 
-             // the "Saved Settings" message if they happen to manually change pages using the 
-             // stick commands within the interval the message is displayed. 
+             // has canceled automatic paging using the stick commands, because that is only
+             // updated when osdShowStats() is called. So, in that case, they would only see
+             // the "Saved Settings" message if they happen to manually change pages using the
+             // stick commands within the interval the message is displayed.
             bool manualPageUpRequested = false;
-            bool manualPageDownRequested = false;       
+            bool manualPageDownRequested = false;
             if (!statsSinglePageCompatible) {
                 // These methods ensure the paging stick commands are held for a brief period
-                // Otherwise it can result in a race condition where the stats are 
-                // updated too quickly and can result in partial blanks, etc. 
-                if (osdIsPageUpStickCommandHeld()) {               
+                // Otherwise it can result in a race condition where the stats are
+                // updated too quickly and can result in partial blanks, etc.
+                if (osdIsPageUpStickCommandHeld()) {
                     manualPageUpRequested = true;
                     statsAutoPagingEnabled = false;
                 } else if (osdIsPageDownStickCommandHeld()) {
-                    manualPageDownRequested = true;                
+                    manualPageDownRequested = true;
                     statsAutoPagingEnabled = false;
                 }
             }
@@ -4603,7 +4607,7 @@ static void osdRefresh(timeUs_t currentTimeUs)
                 // Process manual page change events for multi-page stats.
                 if (manualPageUpRequested) {
                     osdShowStats(statsSinglePageCompatible, 1);
-                    statsCurrentPage = 1;                   
+                    statsCurrentPage = 1;
                 } else if (manualPageDownRequested) {
                     osdShowStats(statsSinglePageCompatible, 0);
                     statsCurrentPage = 0;
@@ -4612,7 +4616,7 @@ static void osdRefresh(timeUs_t currentTimeUs)
         }
 
         // Handle events when either "Splash", "Armed" or "Stats" screens are displayed.
-        if ((currentTimeUs > resumeRefreshAt) || OSD_RESUME_UPDATES_STICK_COMMAND) { 
+        if ((currentTimeUs > resumeRefreshAt) || OSD_RESUME_UPDATES_STICK_COMMAND) {
             // Time elapsed or canceled by stick commands.
             // Exit to normal OSD operation.
             displayClearScreen(osdDisplayPort);
@@ -4622,7 +4626,7 @@ static void osdRefresh(timeUs_t currentTimeUs)
             // Continue "Splash", "Armed" or "Stats" screens.
             displayHeartbeat(osdDisplayPort);
         }
-        
+
         return;
     }
 
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 3ebcf53730c..313162869b0 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -107,7 +107,6 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
     .general = {
 
         .flags = {
-            .use_thr_mid_for_althold = SETTING_NAV_USE_MIDTHR_FOR_ALTHOLD_DEFAULT,
             .extra_arming_safety = SETTING_NAV_EXTRA_ARMING_SAFETY_DEFAULT,
             .user_control_mode = SETTING_NAV_USER_CONTROL_MODE_DEFAULT,
             .rth_alt_control_mode = SETTING_NAV_RTH_ALT_MODE_DEFAULT,
@@ -176,9 +175,10 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
         .braking_bank_angle = SETTING_NAV_MC_BRAKING_BANK_ANGLE_DEFAULT,                        // Max braking angle
 #endif
 
-        .posDecelerationTime = SETTING_NAV_MC_POS_DECELERATION_TIME_DEFAULT,          // posDecelerationTime * 100
-        .posResponseExpo = SETTING_NAV_MC_POS_EXPO_DEFAULT,                           // posResponseExpo * 100
+        .posDecelerationTime = SETTING_NAV_MC_POS_DECELERATION_TIME_DEFAULT,        // posDecelerationTime * 100
+        .posResponseExpo = SETTING_NAV_MC_POS_EXPO_DEFAULT,                         // posResponseExpo * 100
         .slowDownForTurning = SETTING_NAV_MC_WP_SLOWDOWN_DEFAULT,
+        .althold_throttle_type = SETTING_NAV_MC_ALTHOLD_THROTTLE_DEFAULT,           // STICK
     },
 
     // Fixed wing
diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
index cf4289e14a1..86f5d43fd2b 100644
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@@ -175,6 +175,12 @@ typedef enum {
     WP_TURN_SMOOTHING_CUT,
 } wpFwTurnSmoothing_e;
 
+typedef enum {
+    MC_ALT_HOLD_STICK,
+    MC_ALT_HOLD_MID,
+    MC_ALT_HOLD_HOVER,
+} navMcAltHoldThrottle_e;
+
 typedef struct positionEstimationConfig_s {
     uint8_t automatic_mag_declination;
     uint8_t reset_altitude_type; // from nav_reset_type_e
@@ -217,7 +223,6 @@ typedef struct navConfig_s {
 
     struct {
         struct {
-            uint8_t use_thr_mid_for_althold;        // Don't remember throttle when althold was initiated, assume that throttle is at Thr Mid = zero climb rate
             uint8_t extra_arming_safety;            // from navExtraArmingSafety_e
             uint8_t user_control_mode;              // NAV_GPS_ATTI or NAV_GPS_CRUISE
             uint8_t rth_alt_control_mode;           // Controls the logic for choosing the RTH altitude
@@ -286,7 +291,8 @@ typedef struct navConfig_s {
 
         uint8_t posDecelerationTime;            // Brake time parameter
         uint8_t posResponseExpo;                // Position controller expo (taret vel expo for MC)
-        bool slowDownForTurning;             // Slow down during WP missions when changing heading on next waypoint
+        bool slowDownForTurning;                // Slow down during WP missions when changing heading on next waypoint
+        uint8_t althold_throttle_type;          // throttle zero datum type for alt hold
     } mc;
 
     struct {
diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c
index 864cde8b334..5cdbf1ab6a0 100644
--- a/src/main/navigation/navigation_multicopter.c
+++ b/src/main/navigation/navigation_multicopter.c
@@ -177,19 +177,16 @@ bool adjustMulticopterAltitudeFromRCInput(void)
 void setupMulticopterAltitudeController(void)
 {
     const bool throttleIsLow = throttleStickIsLow();
+    const uint8_t throttleType = navConfig()->mc.althold_throttle_type;
 
-    if (navConfig()->general.flags.use_thr_mid_for_althold) {
+    if (throttleType == MC_ALT_HOLD_STICK && !throttleIsLow) {
+        // Only use current throttle if not LOW - use Thr Mid otherwise
+        altHoldThrottleRCZero = rcCommand[THROTTLE];
+    } else if (throttleType == MC_ALT_HOLD_HOVER) {
+        altHoldThrottleRCZero = currentBatteryProfile->nav.mc.hover_throttle;
+    } else {
         altHoldThrottleRCZero = rcLookupThrottleMid();
     }
-    else {
-        // If throttle is LOW - use Thr Mid anyway
-        if (throttleIsLow) {
-            altHoldThrottleRCZero = rcLookupThrottleMid();
-        }
-        else {
-            altHoldThrottleRCZero = rcCommand[THROTTLE];
-        }
-    }
 
     // Make sure we are able to satisfy the deadband
     altHoldThrottleRCZero = constrain(altHoldThrottleRCZero,

From ed1e750830def3036bda117676972bf48765ad95 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sun, 6 Aug 2023 11:59:54 +0200
Subject: [PATCH 43/94] Initial fix for m8 gps.

Will setup some hardware on my bench to diagnose further
---
 src/main/io/gps_ublox.c | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c
index ea202797539..b7abae42d76 100755
--- a/src/main/io/gps_ublox.c
+++ b/src/main/io/gps_ublox.c
@@ -997,6 +997,10 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread)
         // Try sending baud rate switch command at all common baud rates
         gpsSetProtocolTimeout((GPS_BAUD_CHANGE_DELAY + 50) * (GPS_BAUDRATE_COUNT));
         for (gpsState.autoBaudrateIndex = 0; gpsState.autoBaudrateIndex < GPS_BAUDRATE_COUNT; gpsState.autoBaudrateIndex++) {
+            if((gpsState.autoBaudrateIndex >= GPS_BAUDRATE_460800) && (gpsState.baudrateIndex < GPS_BAUDRATE_460800)) {
+                // trying higher baud rates fails on m8 gps
+                break;
+            }
             // 2. Set serial port to baud rate and send an $UBX command to switch the baud rate specified by portConfig [baudrateIndex]
             serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.autoBaudrateIndex]]);
             serialPrint(gpsState.gpsPort, baudInitDataNMEA[gpsState.baudrateIndex]);

From 8d20a44b954f7fffebb66349bf5c635677188560 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sun, 6 Aug 2023 14:25:15 +0200
Subject: [PATCH 44/94] Add variable for max autobaud baud rate.

Some old devices seem to reset if set to too high of a baud rate.

A particular old m8 devices tops up at 115200.

INAV 6.0 used to go all the way up to 230400
---
 docs/Settings.md          | 10 ++++++++++
 src/main/fc/settings.yaml | 10 ++++++++++
 src/main/io/gps.c         | 10 +++++++---
 src/main/io/gps.h         |  2 ++
 src/main/io/gps_ublox.c   |  5 +++--
 5 files changed, 32 insertions(+), 5 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index dda075cee64..f65f2e40e21 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -1372,6 +1372,16 @@ Automatic configuration of GPS baudrate(The specified baudrate in configured in
 
 ---
 
+### gps_auto_baud_max
+
+Max baudrate supported by GPS unit. This is used during autobaud. M8 supports up to 460400, M10 supports up to 921600 and 230400 is the value used before INAV 7.0
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 230400 |  |  |
+
+---
+
 ### gps_auto_config
 
 Enable automatic configuration of UBlox GPS receivers.
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index a0904af6f32..b11169b6acc 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -190,6 +190,9 @@ tables:
   - name: nav_fw_wp_turn_smoothing
     values: ["OFF", "ON", "ON-CUT"]
     enum: wpFwTurnSmoothing_e
+  - name: gps_auto_baud_max
+    values: [ '115200', '57600', '38400', '19200', '9600', '230400', '460800', '921600']
+    enum: gpsBaudRate_e
 
 constants:
   RPYL_PID_MIN: 0
@@ -1501,6 +1504,7 @@ groups:
         min: 1
         max: 3000
   - name: PG_GPS_CONFIG
+    headers: [ "io/gps.h" ]
     type: gpsConfig_t
     condition: USE_GPS
     members:
@@ -1532,6 +1536,12 @@ groups:
         default_value: ON
         field: autoBaud
         type: bool
+      - name: gps_auto_baud_max_supported
+        description: "Max baudrate supported by GPS unit. This is used during autobaud. M8 supports up to 460400, M10 supports up to 921600 and 230400 is the value used before INAV 7.0"
+        default_value: "230400"
+        table: gps_auto_baud_max
+        field: autoBaudMax
+        type: uint8_t
       - name: gps_ublox_use_galileo
         description: "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]."
         default_value: OFF
diff --git a/src/main/io/gps.c b/src/main/io/gps.c
index a66409d5651..03f8ca829d3 100755
--- a/src/main/io/gps.c
+++ b/src/main/io/gps.c
@@ -128,10 +128,9 @@ PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig,
     .ubloxNavHz = SETTING_GPS_UBLOX_NAV_HZ_DEFAULT
 );
 
-
-int getGpsBaudrate(void)
+int gpsBaudRateToInt(gpsBaudRate_e baudrate)
 {
-    switch(gpsState.baudrateIndex)
+    switch(baudrate)
     {
         case GPS_BAUDRATE_115200:
             return 115200;
@@ -154,6 +153,11 @@ int getGpsBaudrate(void)
     }
 }
 
+int getGpsBaudrate(void)
+{
+    return gpsBaudRateToInt(gpsState.baudrateIndex);
+}
+
 const char *getGpsHwVersion(void)
 {
     switch(gpsState.hwVersion)
diff --git a/src/main/io/gps.h b/src/main/io/gps.h
index 9f06e21f722..99b6aafbdf0 100755
--- a/src/main/io/gps.h
+++ b/src/main/io/gps.h
@@ -99,6 +99,7 @@ typedef struct gpsConfig_s {
     bool ubloxUseGlonass;
     uint8_t gpsMinSats;
     uint8_t ubloxNavHz;
+    gpsBaudRate_e autoBaudMax;
 } gpsConfig_t;
 
 PG_DECLARE(gpsConfig_t, gpsConfig);
@@ -175,6 +176,7 @@ uint8_t getGpsProtoMajorVersion(void);
 uint8_t getGpsProtoMinorVersion(void);
 
 int getGpsBaudrate(void);
+int gpsBaudRateToInt(gpsBaudRate_e baudrate);
 
 #if defined(USE_GPS_FAKE)
 void gpsFakeSet(
diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c
index b7abae42d76..f2ad3da5720 100755
--- a/src/main/io/gps_ublox.c
+++ b/src/main/io/gps_ublox.c
@@ -997,9 +997,10 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread)
         // Try sending baud rate switch command at all common baud rates
         gpsSetProtocolTimeout((GPS_BAUD_CHANGE_DELAY + 50) * (GPS_BAUDRATE_COUNT));
         for (gpsState.autoBaudrateIndex = 0; gpsState.autoBaudrateIndex < GPS_BAUDRATE_COUNT; gpsState.autoBaudrateIndex++) {
-            if((gpsState.autoBaudrateIndex >= GPS_BAUDRATE_460800) && (gpsState.baudrateIndex < GPS_BAUDRATE_460800)) {
+            if (gpsBaudRateToInt(gpsState.autoBaudrateIndex) > gpsBaudRateToInt(gpsState.gpsConfig->autoBaudMax)) {
                 // trying higher baud rates fails on m8 gps
-                break;
+                // autoBaudRateIndex is not sorted by baud rate
+                continue;
             }
             // 2. Set serial port to baud rate and send an $UBX command to switch the baud rate specified by portConfig [baudrateIndex]
             serialSetBaudRate(gpsState.gpsPort, baudRates[gpsToSerialBaudRate[gpsState.autoBaudrateIndex]]);

From 78a25eff70e52b83ad281db07ea82052b38371e4 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sun, 6 Aug 2023 14:27:28 +0200
Subject: [PATCH 45/94] update docs

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

diff --git a/docs/Settings.md b/docs/Settings.md
index f65f2e40e21..048e8de9d25 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -1372,7 +1372,7 @@ Automatic configuration of GPS baudrate(The specified baudrate in configured in
 
 ---
 
-### gps_auto_baud_max
+### gps_auto_baud_max_supported
 
 Max baudrate supported by GPS unit. This is used during autobaud. M8 supports up to 460400, M10 supports up to 921600 and 230400 is the value used before INAV 7.0
 

From 0f4a35d25e4028cc919a17828bd5fb7f46291c5b Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sun, 6 Aug 2023 15:41:23 +0200
Subject: [PATCH 46/94] bump pg_register version

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

diff --git a/src/main/io/gps.c b/src/main/io/gps.c
index 03f8ca829d3..2c66b98c0dd 100755
--- a/src/main/io/gps.c
+++ b/src/main/io/gps.c
@@ -113,7 +113,7 @@ static gpsProviderDescriptor_t gpsProviders[GPS_PROVIDER_COUNT] = {
 
 };
 
-PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 3);
+PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 4);
 
 PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig,
     .provider = SETTING_GPS_PROVIDER_DEFAULT,

From dfad61b4a8680ae606a47b6defd778adeea2a5f3 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sun, 6 Aug 2023 15:54:17 +0200
Subject: [PATCH 47/94] Make sure autoBaudMax default is set correctly

---
 src/main/io/gps.c | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/main/io/gps.c b/src/main/io/gps.c
index 2c66b98c0dd..4d7380e84df 100755
--- a/src/main/io/gps.c
+++ b/src/main/io/gps.c
@@ -125,7 +125,8 @@ PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig,
     .ubloxUseGalileo = SETTING_GPS_UBLOX_USE_GALILEO_DEFAULT,
     .ubloxUseBeidou = SETTING_GPS_UBLOX_USE_BEIDOU_DEFAULT,
     .ubloxUseGlonass = SETTING_GPS_UBLOX_USE_GLONASS_DEFAULT,
-    .ubloxNavHz = SETTING_GPS_UBLOX_NAV_HZ_DEFAULT
+    .ubloxNavHz = SETTING_GPS_UBLOX_NAV_HZ_DEFAULT,
+    .autoBaudMax = SETTING_GPS_AUTO_BAUD_MAX_SUPPORTED_DEFAULT
 );
 
 int gpsBaudRateToInt(gpsBaudRate_e baudrate)

From 8afc50562f650817a460fa933c678f81424032e2 Mon Sep 17 00:00:00 2001
From: druckgott <forum@coolpixx.de>
Date: Sun, 6 Aug 2023 17:21:37 +0200
Subject: [PATCH 48/94] Update target.h, Fix Sensor VL53L1X, Speedybee Target
 V3

Add I2C1 Adress to the RANGEFINDER, VL53L1X
---
 src/main/target/SPEEDYBEEF7V3/target.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/target/SPEEDYBEEF7V3/target.h b/src/main/target/SPEEDYBEEF7V3/target.h
index f8705f97da7..7c1d565c377 100644
--- a/src/main/target/SPEEDYBEEF7V3/target.h
+++ b/src/main/target/SPEEDYBEEF7V3/target.h
@@ -149,6 +149,7 @@
 // ********** Optiical Flow adn Lidar **************
 #define USE_RANGEFINDER
 #define USE_RANGEFINDER_MSP
+#define RANGEFINDER_I2C_BUS     BUS_I2C1
 #define USE_OPFLOW
 #define USE_OPFLOW_MSP
 

From bdad86c94a7e366544420fca7fdb4f3bcdfce59b Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sun, 6 Aug 2023 21:40:31 +0200
Subject: [PATCH 49/94] Tab settings for vim

Tab and curly bracket settings for vscode

I am not an emacs user, so I will let @stronnag add his .emacs file

Should help address #9173
---
 .gitignore            |  3 ++-
 .vimrc                |  9 +++++++++
 .vscode/settings.json | 13 +++++++++++++
 3 files changed, 24 insertions(+), 1 deletion(-)
 create mode 100644 .vimrc
 create mode 100644 .vscode/settings.json

diff --git a/.gitignore b/.gitignore
index ea13b9f1789..3cbe312ed07 100644
--- a/.gitignore
+++ b/.gitignore
@@ -12,7 +12,7 @@
 __pycache__
 startup_stm32f10x_md_gcc.s
 .vagrant/
-.vscode/
+#.vscode/
 cov-int*
 /build/
 /obj/
@@ -31,3 +31,4 @@ README.pdf
 
 # local changes only
 make/local.mk
+launch.json
diff --git a/.vimrc b/.vimrc
new file mode 100644
index 00000000000..547d37812cb
--- /dev/null
+++ b/.vimrc
@@ -0,0 +1,9 @@
+filetype on
+filetype indent on
+
+set expandtab
+set bs=2
+set sw=4
+set ts=4
+
+
diff --git a/.vscode/settings.json b/.vscode/settings.json
new file mode 100644
index 00000000000..dc340b18efc
--- /dev/null
+++ b/.vscode/settings.json
@@ -0,0 +1,13 @@
+{
+    "files.associations": {
+        "chrono": "c",
+        "cmath": "c",
+        "ranges": "c"
+    },
+    "editor.tabSize": 4,
+    "editor.insertSpaces": true,
+    "editor.detectIndentation": false,
+    "editor.expandTabs": true,
+    "C_Cpp.vcFormat.newLine.beforeOpenBrace.block": "sameLine",
+    "C_Cpp.vcFormat.newLine.beforeOpenBrace.function": "newLine"
+}

From 5501ce3f96137d1a28e1724523cc5c66cef3e82a Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sun, 6 Aug 2023 21:52:13 +0200
Subject: [PATCH 50/94] Vscode settings

---
 .vscode/settings.json | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/.vscode/settings.json b/.vscode/settings.json
index dc340b18efc..62c7206efb6 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -9,5 +9,6 @@
     "editor.detectIndentation": false,
     "editor.expandTabs": true,
     "C_Cpp.vcFormat.newLine.beforeOpenBrace.block": "sameLine",
-    "C_Cpp.vcFormat.newLine.beforeOpenBrace.function": "newLine"
+    "C_Cpp.vcFormat.newLine.beforeOpenBrace.function": "newLine",
+    "C_Cpp.vcFormat.newLine.beforeElse": false
 }

From 8cac3446e1dc138fe6eb3e2abafac99139fc5ecb Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sun, 6 Aug 2023 22:06:15 +0200
Subject: [PATCH 51/94] Fix brances for if and else

Based on https://clang.llvm.org/docs/ClangFormatStyleOptions.html#breakbeforebraces
---
 .vscode/settings.json | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/.vscode/settings.json b/.vscode/settings.json
index 62c7206efb6..2cece3ee127 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -8,7 +8,5 @@
     "editor.insertSpaces": true,
     "editor.detectIndentation": false,
     "editor.expandTabs": true,
-    "C_Cpp.vcFormat.newLine.beforeOpenBrace.block": "sameLine",
-    "C_Cpp.vcFormat.newLine.beforeOpenBrace.function": "newLine",
-    "C_Cpp.vcFormat.newLine.beforeElse": false
+    "C_Cpp.clang_format_fallbackStyle": "{ BasedOnStyle: Google, IndentWidth: 4,  BreakBeforeBraces: Mozilla }"
 }

From bcf8bbb2e3851b26a7c72fda226de405f19c307a Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sun, 6 Aug 2023 23:58:44 +0200
Subject: [PATCH 52/94] Add @stronnag's emacs style settings

---
 .dir-locals.el | 5 +++++
 1 file changed, 5 insertions(+)
 create mode 100644 .dir-locals.el

diff --git a/.dir-locals.el b/.dir-locals.el
new file mode 100644
index 00000000000..9c3727aef8e
--- /dev/null
+++ b/.dir-locals.el
@@ -0,0 +1,5 @@
+;;; Directory Local Variables            -*- no-byte-compile: t -*-
+;;; For more information see (info "(emacs) Directory Variables")
+
+((nil . ((c-basic-offset . 4)
+	 (c-default-style . "k&r"))))

From 522cc92766969ad6a571852aab3485058e0395ce Mon Sep 17 00:00:00 2001
From: EMSR <shanggl@wo.cn>
Date: Mon, 7 Aug 2023 21:38:17 +0800
Subject: [PATCH 53/94] fix motor2 output bug

the neutronrcf435mini aio fc board ,changed the pin io defines before batch production , this commit fix the motor 2 output and slove the issue #9204
---
 src/main/target/NEUTRONRCF435MINI/target.c | 2 +-
 src/main/target/NEUTRONRCF435MINI/target.h | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/target/NEUTRONRCF435MINI/target.c b/src/main/target/NEUTRONRCF435MINI/target.c
index e964c19156e..995bf8f3d1a 100644
--- a/src/main/target/NEUTRONRCF435MINI/target.c
+++ b/src/main/target/NEUTRONRCF435MINI/target.c
@@ -29,7 +29,7 @@ timerHardware_t timerHardware[] = {
     DEF_TIM(TMR1, CH1, PA8,  TIM_USE_ANY |TIM_USE_LED, 0,7),            // PWM1 - LED MCO1 DMA1 CH2
 
     DEF_TIM(TMR4, CH1, PB6,  TIM_USE_MC_MOTOR|TIM_USE_FW_MOTOR, 0,0),   // motor1 DMA2 CH7
-    DEF_TIM(TMR1, CH3, PA10, TIM_USE_MC_MOTOR|TIM_USE_FW_MOTOR, 0,2),   // motor2 DMA2 CH6
+    DEF_TIM(TMR4, CH2, PB7,  TIM_USE_MC_MOTOR|TIM_USE_FW_MOTOR, 0,2),   // motor2 DMA2 CH6
     DEF_TIM(TMR2, CH4, PA3,  TIM_USE_MC_MOTOR|TIM_USE_FW_SERVO, 0,1),   // motor3 DMA2 CH5
     DEF_TIM(TMR3, CH4, PB1,  TIM_USE_MC_MOTOR|TIM_USE_FW_SERVO, 0,3),   // motor4 DMA2 CH4
 
diff --git a/src/main/target/NEUTRONRCF435MINI/target.h b/src/main/target/NEUTRONRCF435MINI/target.h
index 92c9f351a15..00a54eafddd 100644
--- a/src/main/target/NEUTRONRCF435MINI/target.h
+++ b/src/main/target/NEUTRONRCF435MINI/target.h
@@ -142,7 +142,7 @@
 #define USE_USB_DETECT
 
 #define USE_UART1
-#define UART1_RX_PIN            PB7
+#define UART1_RX_PIN            PA10
 #define UART1_TX_PIN            PA9
 
 #define USE_UART2

From 63d6c2e7aa53b82ffb32f255e5f1ded863e81604 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Mon, 7 Aug 2023 16:26:29 +0200
Subject: [PATCH 54/94] Basic target

---
 .../target/SPEEDYBEEF405MINI/CMakeLists.txt   |   2 +
 src/main/target/SPEEDYBEEF405MINI/config.c    |  37 ++++
 src/main/target/SPEEDYBEEF405MINI/target.c    |  47 +++++
 src/main/target/SPEEDYBEEF405MINI/target.h    | 186 ++++++++++++++++++
 4 files changed, 272 insertions(+)
 create mode 100644 src/main/target/SPEEDYBEEF405MINI/CMakeLists.txt
 create mode 100644 src/main/target/SPEEDYBEEF405MINI/config.c
 create mode 100644 src/main/target/SPEEDYBEEF405MINI/target.c
 create mode 100644 src/main/target/SPEEDYBEEF405MINI/target.h

diff --git a/src/main/target/SPEEDYBEEF405MINI/CMakeLists.txt b/src/main/target/SPEEDYBEEF405MINI/CMakeLists.txt
new file mode 100644
index 00000000000..e18c0cd5ada
--- /dev/null
+++ b/src/main/target/SPEEDYBEEF405MINI/CMakeLists.txt
@@ -0,0 +1,2 @@
+target_stm32f405xg(SPEEDYBEEF405MINI)
+target_stm32f405xg(SPEEDYBEEF405MINI_6OUTPUTS)
diff --git a/src/main/target/SPEEDYBEEF405MINI/config.c b/src/main/target/SPEEDYBEEF405MINI/config.c
new file mode 100644
index 00000000000..46aa7f00dcb
--- /dev/null
+++ b/src/main/target/SPEEDYBEEF405MINI/config.c
@@ -0,0 +1,37 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it 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.
+ *
+ * Cleanflight 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdint.h>
+
+#include "platform.h"
+
+#include "fc/fc_msp_box.h"
+
+#include "io/piniobox.h"
+#include "io/serial.h"
+
+void targetConfiguration(void)
+{
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART2)].functionMask = FUNCTION_RX_SERIAL;
+
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART5)].functionMask = FUNCTION_ESCSERIAL;
+
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_MSP;
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].msp_baudrateIndex = BAUD_115200;
+
+    pinioBoxConfigMutable()->permanentId[0] = BOXARM;
+}
diff --git a/src/main/target/SPEEDYBEEF405MINI/target.c b/src/main/target/SPEEDYBEEF405MINI/target.c
new file mode 100644
index 00000000000..9174f248778
--- /dev/null
+++ b/src/main/target/SPEEDYBEEF405MINI/target.c
@@ -0,0 +1,47 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it 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.
+ *
+ * INAV 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 INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdint.h>
+
+#include "platform.h"
+
+#include "drivers/bus.h"
+#include "drivers/io.h"
+#include "drivers/pwm_mapping.h"
+#include "drivers/timer.h"
+#include "drivers/pinio.h"
+#include "drivers/sensor.h"
+
+timerHardware_t timerHardware[] = {
+    DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR  | TIM_USE_FW_MOTOR,   1, 0), // S1
+    DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR  | TIM_USE_FW_MOTOR,   1, 0), // S2
+    DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR  | TIM_USE_FW_SERVO,   1, 0), // S3
+    DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR  | TIM_USE_FW_SERVO,   1, 0), // S4
+
+    DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY,                            0, 0), //TX2  softserial1_Tx
+
+#ifdef SPEEDYBEEF405MINI_6OUTPUTS
+    DEF_TIM(TIM12,  CH1,    PB14,   TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO,     1, 0), // CAM_CTRL
+    DEF_TIM(TIM1,   CH1,    PA8,    TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO,     0, 0), // LED
+#else
+    DEF_TIM(TIM12,  CH1,    PB14,   TIM_USE_ANY,                                                1, 0), // CAM_CTRL
+    DEF_TIM(TIM1,   CH1,    PA8,    TIM_USE_LED,                                                0, 0), // LED
+#endif
+
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
diff --git a/src/main/target/SPEEDYBEEF405MINI/target.h b/src/main/target/SPEEDYBEEF405MINI/target.h
new file mode 100644
index 00000000000..a70e5211c83
--- /dev/null
+++ b/src/main/target/SPEEDYBEEF405MINI/target.h
@@ -0,0 +1,186 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it 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.
+ *
+ * INAV 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 INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+
+#pragma once
+
+#define TARGET_BOARD_IDENTIFIER "SF4M"
+#define USBD_PRODUCT_STRING     "SPEEDYBEEF405MINI"
+
+#define LED0                    PC13
+
+#define BEEPER                  PC15
+#define BEEPER_INVERTED
+
+/*
+ * SPI Buses
+ */
+#define USE_SPI
+#define USE_SPI_DEVICE_1
+#define SPI1_SCK_PIN            PA5
+#define SPI1_MISO_PIN           PA6
+#define SPI1_MOSI_PIN           PA7
+
+#define USE_SPI_DEVICE_2
+#define SPI2_SCK_PIN            PB13
+#define SPI2_MISO_PIN   	    PC2
+#define SPI2_MOSI_PIN   	    PC3
+
+#define USE_SPI_DEVICE_3
+#define SPI3_SCK_PIN            PB3
+#define SPI3_MISO_PIN   	    PB4
+#define SPI3_MOSI_PIN   	    PB5
+
+/*
+ * I2C
+ */
+#define USE_I2C
+#define USE_I2C_DEVICE_1
+#define I2C1_SCL                PB8
+#define I2C1_SDA                PB9
+
+/*
+ * Serial
+ */
+#define USE_VCP
+
+#define USE_UART1
+#define UART1_TX_PIN            PA9
+#define UART1_RX_PIN            PA10
+
+#define USE_UART2
+#define UART2_TX_PIN            PA2
+#define UART2_RX_PIN            PA3
+
+#define USE_UART3
+#define UART3_TX_PIN            PC10
+#define UART3_RX_PIN            PC11
+
+#define USE_UART4 //Internally routed to BLE
+#define UART4_TX_PIN            PA0
+#define UART4_RX_PIN            PA1 
+
+#define USE_UART5
+#define UART5_TX_PIN            PC12
+#define UART5_RX_PIN            PD2
+
+#define USE_UART6
+#define UART6_TX_PIN            PC6
+#define UART6_RX_PIN            PC7
+
+#define USE_SOFTSERIAL1
+#define SOFTSERIAL_1_TX_PIN      PA2
+#define SOFTSERIAL_1_RX_PIN      PA2
+
+#define SERIAL_PORT_COUNT       8
+
+/*
+ * Gyro
+ */
+#define USE_IMU_ICM42605
+#define IMU_ICM42605_ALIGN      CW270_DEG
+#define ICM42605_CS_PIN         PA4
+#define ICM42605_SPI_BUS        BUS_SPI1
+
+/*
+ * Other
+ */
+#define USE_BARO
+#define BARO_I2C_BUS            BUS_I2C1
+#define USE_BARO_BMP280
+#define USE_BARO_MS5611
+#define USE_BARO_DPS310
+
+#define USE_MAG
+#define MAG_I2C_BUS             BUS_I2C1
+#define USE_MAG_AK8975
+#define USE_MAG_HMC5883
+#define USE_MAG_QMC5883
+#define USE_MAG_IST8310
+#define USE_MAG_IST8308
+#define USE_MAG_MAG3110
+#define USE_MAG_LIS3MDL
+
+#define TEMPERATURE_I2C_BUS     BUS_I2C1
+#define PITOT_I2C_BUS           BUS_I2C1
+#define RANGEFINDER_I2C_BUS     BUS_I2C1
+
+/*
+ * OSD
+ */
+#define USE_MAX7456
+#define MAX7456_CS_PIN          PB12
+#define MAX7456_SPI_BUS         BUS_SPI2
+
+/*
+ * Blackbox
+ */
+#define USE_FLASHFS
+#define USE_FLASH_M25P16
+#define M25P16_SPI_BUS          BUS_SPI3
+#define M25P16_CS_PIN           PC14
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+
+#define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER       SERIALRX_SBUS
+#define SERIALRX_UART           SERIAL_PORT_USART2
+
+// *************** ADC *****************************
+#define USE_ADC
+#define ADC_INSTANCE                ADC1
+#define ADC1_DMA_STREAM             DMA2_Stream0
+
+#define ADC_CHANNEL_1_PIN           PC0
+#define ADC_CHANNEL_2_PIN           PC1
+
+#define VBAT_ADC_CHANNEL            ADC_CHN_1
+#define CURRENT_METER_ADC_CHANNEL   ADC_CHN_2
+
+// *************** PINIO ***************************
+#define USE_PINIO
+#define USE_PINIOBOX
+#define PINIO1_PIN                  PB11 // RF Switch
+#define PINIO1_FLAGS                PINIO_FLAGS_INVERTED
+
+// *************** LEDSTRIP ************************
+#define USE_LED_STRIP
+#define WS2811_PIN                  PA8
+
+#define DEFAULT_FEATURES        (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX)
+#define CURRENT_METER_SCALE     250
+#define CURRENT_METER_OFFSET    -500
+
+#define USE_SERIAL_4WAY_BLHELI_INTERFACE
+
+#define TARGET_IO_PORTA 0xffff
+#define TARGET_IO_PORTB 0xffff
+#define TARGET_IO_PORTC 0xffff
+#define TARGET_IO_PORTD 0xffff
+
+#ifdef SPEEDYBEEF405MINI_6OUTPUTS
+
+#define MAX_PWM_OUTPUT_PORTS        6
+
+#else
+
+#define MAX_PWM_OUTPUT_PORTS        4
+
+#endif
+
+#define USE_DSHOT
+#define USE_SERIALSHOT
+#define USE_ESC_SENSOR
\ No newline at end of file

From ce6c5cfbf530d02ce1d191cf0d1be5c467dd405a Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Mon, 7 Aug 2023 17:03:19 +0200
Subject: [PATCH 55/94] Target updates

---
 src/main/target/SPEEDYBEEF405MINI/target.c | 3 +--
 src/main/target/SPEEDYBEEF405MINI/target.h | 2 +-
 2 files changed, 2 insertions(+), 3 deletions(-)

diff --git a/src/main/target/SPEEDYBEEF405MINI/target.c b/src/main/target/SPEEDYBEEF405MINI/target.c
index 9174f248778..1118940e6f6 100644
--- a/src/main/target/SPEEDYBEEF405MINI/target.c
+++ b/src/main/target/SPEEDYBEEF405MINI/target.c
@@ -32,8 +32,6 @@ timerHardware_t timerHardware[] = {
     DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR  | TIM_USE_FW_SERVO,   1, 0), // S3
     DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR  | TIM_USE_FW_SERVO,   1, 0), // S4
 
-    DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY,                            0, 0), //TX2  softserial1_Tx
-
 #ifdef SPEEDYBEEF405MINI_6OUTPUTS
     DEF_TIM(TIM12,  CH1,    PB14,   TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO,     1, 0), // CAM_CTRL
     DEF_TIM(TIM1,   CH1,    PA8,    TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO,     0, 0), // LED
@@ -42,6 +40,7 @@ timerHardware_t timerHardware[] = {
     DEF_TIM(TIM1,   CH1,    PA8,    TIM_USE_LED,                                                0, 0), // LED
 #endif
 
+    DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY,                            0, 0), //TX2  softserial1_Tx
 };
 
 const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
diff --git a/src/main/target/SPEEDYBEEF405MINI/target.h b/src/main/target/SPEEDYBEEF405MINI/target.h
index a70e5211c83..dd5b9f4cfb3 100644
--- a/src/main/target/SPEEDYBEEF405MINI/target.h
+++ b/src/main/target/SPEEDYBEEF405MINI/target.h
@@ -162,7 +162,7 @@
 
 #define DEFAULT_FEATURES        (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX)
 #define CURRENT_METER_SCALE     250
-#define CURRENT_METER_OFFSET    -500
+#define CURRENT_METER_OFFSET    0
 
 #define USE_SERIAL_4WAY_BLHELI_INTERFACE
 

From aad29777c81d43c9febce220b66cf0a5e0338ab0 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Mon, 7 Aug 2023 17:57:44 +0200
Subject: [PATCH 56/94] Block by Runtime calibration of ACC only when ACC is
 required

---
 src/main/fc/fc_core.c | 35 ++++++++++++++++++-----------------
 1 file changed, 18 insertions(+), 17 deletions(-)

diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index ac8cf9a335e..af8d1202b17 100755
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -122,6 +122,21 @@ timeUs_t lastDisarmTimeUs = 0;
 static bool prearmWasReset = false; // Prearm must be reset (RC Mode not active) before arming is possible
 static timeMs_t prearmActivationTime = 0;
 
+static bool isAccRequired(void) {
+    return isModeActivationConditionPresent(BOXNAVPOSHOLD) ||
+        isModeActivationConditionPresent(BOXNAVRTH) ||
+        isModeActivationConditionPresent(BOXNAVWP) ||
+        isModeActivationConditionPresent(BOXANGLE) ||
+        isModeActivationConditionPresent(BOXHORIZON) ||
+        isModeActivationConditionPresent(BOXNAVALTHOLD) ||
+        isModeActivationConditionPresent(BOXHEADINGHOLD) ||
+        isModeActivationConditionPresent(BOXNAVLAUNCH) ||
+        isModeActivationConditionPresent(BOXTURNASSIST) ||
+        isModeActivationConditionPresent(BOXNAVCOURSEHOLD) ||
+        isModeActivationConditionPresent(BOXSOARING) ||
+        failsafeConfig()->failsafe_procedure != FAILSAFE_PROCEDURE_DROP_IT;
+}
+
 bool areSensorsCalibrating(void)
 {
 #ifdef USE_BARO
@@ -142,11 +157,11 @@ bool areSensorsCalibrating(void)
     }
 #endif
 
-    if (!navIsCalibrationComplete()) {
+    if (!navIsCalibrationComplete() && isAccRequired()) {
         return true;
     }
 
-    if (!accIsCalibrationComplete() && sensors(SENSOR_ACC)) {
+    if (!accIsCalibrationComplete() && sensors(SENSOR_ACC) && isAccRequired()) {
         return true;
     }
 
@@ -264,21 +279,7 @@ static void updateArmingStatus(void)
             sensors(SENSOR_ACC) &&
             !STATE(ACCELEROMETER_CALIBRATED) &&
             // Require ACC calibration only if any of the setting might require it
-            (
-                isModeActivationConditionPresent(BOXNAVPOSHOLD) ||
-                isModeActivationConditionPresent(BOXNAVRTH) ||
-                isModeActivationConditionPresent(BOXNAVWP) ||
-                isModeActivationConditionPresent(BOXANGLE) ||
-                isModeActivationConditionPresent(BOXHORIZON) ||
-                isModeActivationConditionPresent(BOXNAVALTHOLD) ||
-                isModeActivationConditionPresent(BOXHEADINGHOLD) ||
-                isModeActivationConditionPresent(BOXNAVLAUNCH) ||
-                isModeActivationConditionPresent(BOXTURNASSIST) ||
-                isModeActivationConditionPresent(BOXNAVCOURSEHOLD) ||
-                isModeActivationConditionPresent(BOXSOARING) ||
-                failsafeConfig()->failsafe_procedure != FAILSAFE_PROCEDURE_DROP_IT
-
-            )
+            isAccRequired()
         ) {
             ENABLE_ARMING_FLAG(ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED);
         }

From 37f56f7b339816bf7a15fb2e85fddeab4bc61e27 Mon Sep 17 00:00:00 2001
From: jmajew <jmajewsk@meil.pw.edu.pl>
Date: Mon, 7 Aug 2023 18:34:54 +0200
Subject: [PATCH 57/94] fc_tasks.c changed

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

diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index cdc1908115f..af5f27b5272 100755
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -494,7 +494,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
     [TASK_BARO] = {
         .taskName = "BARO",
         .taskFunc = taskUpdateBaro,
-        .desiredPeriod = TASK_PERIOD_MS(50),        // SPL06 uses oversampilng - result 16ms ?
+        .desiredPeriod = TASK_PERIOD_HZ(20),
         .staticPriority = TASK_PRIORITY_MEDIUM,
     },
 #endif

From 3f6bfb3b029cac5c788566a846b0bdf43ca03f3b Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Mon, 7 Aug 2023 20:08:39 +0200
Subject: [PATCH 58/94] Fix for #9225

Skipping to OSD_ITEM_COUNT at that point ignores new OSD elements
---
 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 63312af46c2..947c03845d9 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -3552,10 +3552,10 @@ uint8_t osdIncElementIndex(uint8_t elementIndex)
             elementIndex = OSD_AIR_MAX_SPEED;
         }
         if (elementIndex == OSD_GLIDE_RANGE) {
-            elementIndex = feature(FEATURE_CURRENT_METER) ? OSD_CLIMB_EFFICIENCY : OSD_ITEM_COUNT;
+            elementIndex = feature(FEATURE_CURRENT_METER) ? OSD_CLIMB_EFFICIENCY : OSD_PILOT_NAME;
         }
         if (elementIndex == OSD_NAV_WP_MULTI_MISSION_INDEX) {
-            elementIndex = OSD_ITEM_COUNT;
+            elementIndex = OSD_PILOT_NAME;
         }
     }
 

From d5acb34e3e9117a20e70d65d75c5356f1db7028a Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Mon, 7 Aug 2023 19:19:34 -0500
Subject: [PATCH 59/94] Skystars H743HD: Set S5-S8 as servos (unless all-motors
 mode is selected)

---
 src/main/target/SKYSTARSH743HD/target.c | 7 -------
 1 file changed, 7 deletions(-)

diff --git a/src/main/target/SKYSTARSH743HD/target.c b/src/main/target/SKYSTARSH743HD/target.c
index 2c7d1a5bee6..07a2b0ef701 100644
--- a/src/main/target/SKYSTARSH743HD/target.c
+++ b/src/main/target/SKYSTARSH743HD/target.c
@@ -36,17 +36,10 @@ timerHardware_t timerHardware[] = {
     DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 2),   // S3
     DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 3),   // S4
 
-#if defined(SKYSTARSH743HD_SERVOS)
     DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 4),   // S5
     DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 5),   // S6
     DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 6),   // S7
     DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 7),   // S8
-#else
-    DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 4),   // S5
-    DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 5),   // S6
-    DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 6),   // S7
-    DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 7),   // S8
-#endif
 
     DEF_TIM(TIM1,  CH1, PA8,  TIM_USE_LED, 0, 9),    // LED_2812
 };

From 665689d9ed4bd33f43cd75b8a56c75f94e2484d1 Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Mon, 7 Aug 2023 19:26:25 -0500
Subject: [PATCH 60/94] SKYSTARSH743HD update CMakeLists.txt to remove new
 target

---
 src/main/target/SKYSTARSH743HD/CMakeLists.txt | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/main/target/SKYSTARSH743HD/CMakeLists.txt b/src/main/target/SKYSTARSH743HD/CMakeLists.txt
index 35377d39ac4..39b6db376f1 100644
--- a/src/main/target/SKYSTARSH743HD/CMakeLists.txt
+++ b/src/main/target/SKYSTARSH743HD/CMakeLists.txt
@@ -1,2 +1 @@
 target_stm32h743xi(SKYSTARSH743HD)
-target_stm32h743xi(SKYSTARSH743HD_SERVOS)

From 58c2c41508623372d330411de310596d6bc1e489 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 8 Aug 2023 19:53:13 +0200
Subject: [PATCH 61/94] Fix ADC on FOXEERH743

---
 src/main/target/FOXEERH743/target.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/target/FOXEERH743/target.h b/src/main/target/FOXEERH743/target.h
index 72fef026b15..ffb582ea9ad 100644
--- a/src/main/target/FOXEERH743/target.h
+++ b/src/main/target/FOXEERH743/target.h
@@ -135,7 +135,7 @@
 
 // *************** ADC *****************************
 #define USE_ADC
-#define ADC_INSTANCE ADC1
+#define ADC_INSTANCE ADC3
 
 #define ADC_CHANNEL_1_PIN PC3
 #define ADC_CHANNEL_2_PIN PC5

From 3071973c2c83b8d20ce293289d7f7d95ab33757b Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Tue, 8 Aug 2023 20:50:44 +0200
Subject: [PATCH 62/94] Update speedybeef405v3 docs, with some of the common
 issues seen on discord

---
 docs/boards/SPEEDYBEEF405V3.md | 17 ++++++++++++++++-
 1 file changed, 16 insertions(+), 1 deletion(-)

diff --git a/docs/boards/SPEEDYBEEF405V3.md b/docs/boards/SPEEDYBEEF405V3.md
index 8dd310c3851..7ec4b1ce944 100644
--- a/docs/boards/SPEEDYBEEF405V3.md
+++ b/docs/boards/SPEEDYBEEF405V3.md
@@ -1,3 +1,18 @@
 # SpeedyBee F405 V3
 
-> Notice: At the moment, DSHOT is not supported on SpeedyBe F405 V3. Use Multishot instead
\ No newline at end of file
+> Notice: At the moment, DSHOT is not supported on SpeedyBe F405 V3. Use Multishot instead
+
+> Notice: The analog OSD and the SD card (blackbox) share the same SPI bus, which can cause problems when trying to use analog OSD and blackbox at the same time.
+
+## Flashing firmware
+
+We often see reports of users having problems flashing new firmware with this baord. The following sugestions usually seem to fix the issues.
+
+* Remove SD Card
+* Disconnect devices from UART1 and UART3
+
+Try removing the SD Card first, and if that still does not fix the issue, disconnect the devices connected to UART1 and UART3.
+
+
+
+

From ad9d9127949ceb9dd2270dbe2410e74f7046f669 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Thu, 10 Aug 2023 12:57:35 +0100
Subject: [PATCH 63/94] Update navigation_multicopter.c

---
 src/main/navigation/navigation_multicopter.c | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c
index 75c45f91215..a70265380e7 100644
--- a/src/main/navigation/navigation_multicopter.c
+++ b/src/main/navigation/navigation_multicopter.c
@@ -113,8 +113,8 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
 static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros)
 {
     // Calculate min and max throttle boundaries (to compensate for integral windup)
-    const int16_t thrAdjustmentMin = (int16_t)getThrottleIdleValue() - (int16_t)currentBatteryProfile->nav.mc.hover_throttle;
-    const int16_t thrAdjustmentMax = (int16_t)motorConfig()->maxthrottle - (int16_t)currentBatteryProfile->nav.mc.hover_throttle;
+    const int16_t thrAdjustmentMin = getThrottleIdleValue() - currentBatteryProfile->nav.mc.hover_throttle;
+    const int16_t thrAdjustmentMax = motorConfig()->maxthrottle - currentBatteryProfile->nav.mc.hover_throttle;
 
     float velocity_controller = navPidApply2(&posControl.pids.vel[Z], posControl.desiredState.vel.z, navGetCurrentActualPositionAndVelocity()->vel.z, US2S(deltaMicros), thrAdjustmentMin, thrAdjustmentMax, 0);
 
@@ -122,7 +122,7 @@ static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros)
 
     posControl.rcAdjustment[THROTTLE] = constrain(posControl.rcAdjustment[THROTTLE], thrAdjustmentMin, thrAdjustmentMax);
 
-    posControl.rcAdjustment[THROTTLE] = constrain((int16_t)currentBatteryProfile->nav.mc.hover_throttle + posControl.rcAdjustment[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle);
+    posControl.rcAdjustment[THROTTLE] = constrain(currentBatteryProfile->nav.mc.hover_throttle + posControl.rcAdjustment[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle);
 }
 
 bool adjustMulticopterAltitudeFromRCInput(void)
@@ -213,7 +213,7 @@ void resetMulticopterAltitudeController(void)
     navPidReset(&posControl.pids.vel[Z]);
     navPidReset(&posControl.pids.surface);
 
-    posControl.rcAdjustment[THROTTLE] = 0;
+    posControl.rcAdjustment[THROTTLE] = currentBatteryProfile->nav.mc.hover_throttle;
 
     posControl.desiredState.vel.z = posToUse->vel.z;   // Gradually transition from current climb
 

From ec63d263de5a79d547caa5eac160e68133b048fc Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Thu, 10 Aug 2023 19:53:16 +0200
Subject: [PATCH 64/94] cleanup

---
 src/main/io/gps_ublox.c | 19 +++++++++++++++++--
 1 file changed, 17 insertions(+), 2 deletions(-)

diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c
index f2ad3da5720..497b9355afc 100755
--- a/src/main/io/gps_ublox.c
+++ b/src/main/io/gps_ublox.c
@@ -269,6 +269,20 @@ static void ubloxSendSetCfgBytes(ubx_config_data8_payload_t *kvPairs, uint8_t co
 // Info on protocol used by M8-M9, check UBX-CFG-GNSS for gnss configuration
 // https://content.u-blox.com/sites/default/files/products/documents/u-blox8-M8_ReceiverDescrProtSpec_UBX-13003221.pdf
 // https://content.u-blox.com/sites/default/files/documents/u-blox-F9-HPG-1.32_InterfaceDescription_UBX-22008968.pdf
+/*
+CFG-SIGNAL-SBAS_ENA Enabled/disabled status of the SBAS subsystem
+CFG-SBAS-USE_TESTMODE Allow/disallow SBAS usage from satellites in test mode
+CFG-SBAS-USE_RANGING Use the SBAS satellites for navigation (ranging)
+CFG-SBAS-USE_DIFFCORR Combined enable/disable switch for fast, long-term, and ionosphere corrections
+CFG-SBAS-USE_INTEGRITY Apply integrity information data
+CFG-SBAS-PRNSCANMASK Allows selectively enabling/disabling SBAS satellites
+
+- 0 WAAS
+- 1 EGNOS
+- 2 MSAS
+- 3 GAGAN
+- 16 GPS
+*/
 static int configureGNSS_SBAS(ubx_gnss_element_t * gnss_block)
 {
     gnss_block->gnssId = GNSSID_SBAS;
@@ -379,8 +393,9 @@ static void configureGNSS10(void)
 {
         ubx_config_data8_payload_t gnssConfigValues[] = {
             // SBAS
-            {UBLOX_CFG_SIGNAL_SBAS_ENA, 1},
-            {UBLOX_CFG_SIGNAL_SBAS_L1CA_ENA, 1},
+            {UBLOX_CFG_SIGNAL_SBAS_ENA, gpsState.gpsConfig->sbasMode == SBAS_NONE ? 0 : 1},
+            {UBLOX_CFG_SIGNAL_SBAS_L1CA_ENA, gpsState.gpsConfig->sbasMode == SBAS_NONE ? 0 : 1},
+
     
             // Galileo
             {UBLOX_CFG_SIGNAL_GAL_ENA, gpsState.gpsConfig->ubloxUseGalileo},

From 33585de71500a82904ea14c33b167b504489448a Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Thu, 10 Aug 2023 19:55:26 +0200
Subject: [PATCH 65/94] remove comments

---
 src/main/io/gps_ublox.c | 15 ---------------
 1 file changed, 15 deletions(-)

diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c
index 497b9355afc..c2758bc98a7 100755
--- a/src/main/io/gps_ublox.c
+++ b/src/main/io/gps_ublox.c
@@ -269,20 +269,6 @@ static void ubloxSendSetCfgBytes(ubx_config_data8_payload_t *kvPairs, uint8_t co
 // Info on protocol used by M8-M9, check UBX-CFG-GNSS for gnss configuration
 // https://content.u-blox.com/sites/default/files/products/documents/u-blox8-M8_ReceiverDescrProtSpec_UBX-13003221.pdf
 // https://content.u-blox.com/sites/default/files/documents/u-blox-F9-HPG-1.32_InterfaceDescription_UBX-22008968.pdf
-/*
-CFG-SIGNAL-SBAS_ENA Enabled/disabled status of the SBAS subsystem
-CFG-SBAS-USE_TESTMODE Allow/disallow SBAS usage from satellites in test mode
-CFG-SBAS-USE_RANGING Use the SBAS satellites for navigation (ranging)
-CFG-SBAS-USE_DIFFCORR Combined enable/disable switch for fast, long-term, and ionosphere corrections
-CFG-SBAS-USE_INTEGRITY Apply integrity information data
-CFG-SBAS-PRNSCANMASK Allows selectively enabling/disabling SBAS satellites
-
-- 0 WAAS
-- 1 EGNOS
-- 2 MSAS
-- 3 GAGAN
-- 16 GPS
-*/
 static int configureGNSS_SBAS(ubx_gnss_element_t * gnss_block)
 {
     gnss_block->gnssId = GNSSID_SBAS;
@@ -396,7 +382,6 @@ static void configureGNSS10(void)
             {UBLOX_CFG_SIGNAL_SBAS_ENA, gpsState.gpsConfig->sbasMode == SBAS_NONE ? 0 : 1},
             {UBLOX_CFG_SIGNAL_SBAS_L1CA_ENA, gpsState.gpsConfig->sbasMode == SBAS_NONE ? 0 : 1},
 
-    
             // Galileo
             {UBLOX_CFG_SIGNAL_GAL_ENA, gpsState.gpsConfig->ubloxUseGalileo},
             {UBLOX_CFG_SIGNAL_GAL_E1_ENA, gpsState.gpsConfig->ubloxUseGalileo},

From 8d223c78ca839972e53040e8a24aa4191a602125 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Thu, 10 Aug 2023 23:18:16 +0200
Subject: [PATCH 66/94] Update SBAS PRN lists

---
 src/main/io/gps_ublox.c | 13 ++++++++-----
 1 file changed, 8 insertions(+), 5 deletions(-)

diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c
index c2758bc98a7..249060325fd 100755
--- a/src/main/io/gps_ublox.c
+++ b/src/main/io/gps_ublox.c
@@ -55,17 +55,20 @@
 
 
 // SBAS_AUTO, SBAS_EGNOS, SBAS_WAAS, SBAS_MSAS, SBAS_GAGAN, SBAS_NONE
-// note PRNs last upadted 2020-12-18
+// note PRNs last upadted 2023-08-10
+// https://www.gps.gov/technical/prn-codes/L1-CA-PRN-code-assignments-2023-Apr.pdf
 
 #define SBASMASK1_BASE 120
 #define SBASMASK1_BITS(prn) (1 << (prn-SBASMASK1_BASE))
 
 static const uint32_t ubloxScanMode1[] = {
     0x00000000, // AUTO
-    (SBASMASK1_BITS(123) | SBASMASK1_BITS(126) | SBASMASK1_BITS(136)), // SBAS
-    (SBASMASK1_BITS(131) | SBASMASK1_BITS(133) | SBASMASK1_BITS(138)), // WAAS
-    (SBASMASK1_BITS(129) | SBASMASK1_BITS(137)), // MAAS
-    (SBASMASK1_BITS(127) | SBASMASK1_BITS(128)), // GAGAN
+    (SBASMASK1_BITS(121) | SBASMASK1_BITS(123) | SBASMASK1_BITS(126) | SBASMASK1_BITS(136) | SBASMASK1_BITS(150)), // SBAS_EGNOS
+    (SBASMASK1_BITS(131) | SBASMASK1_BITS(133) | SBASMASK1_BITS(135) | SBASMASK1_BITS(138)), // WAAS
+
+    (SBASMASK1_BITS(129) | SBASMASK1_BITS(137) | SBASMASK1_BITS(139)), // MSAS
+
+    (SBASMASK1_BITS(127) | SBASMASK1_BITS(128) | SBASMASK1_BITS(132)), // GAGAN
     0x00000000, // NONE
 };
 

From 44117e50e3b276a90d05ccb7181002659c187c65 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Thu, 10 Aug 2023 23:26:40 +0200
Subject: [PATCH 67/94] Update checksum

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

diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c
index 249060325fd..2baa97ce9b7 100755
--- a/src/main/io/gps_ublox.c
+++ b/src/main/io/gps_ublox.c
@@ -79,8 +79,8 @@ static const char * baudInitDataNMEA[GPS_BAUDRATE_COUNT] = {
     "$PUBX,41,1,0003,0001,19200,0*23\r\n",      // GPS_BAUDRATE_19200
     "$PUBX,41,1,0003,0001,9600,0*16\r\n",       // GPS_BAUDRATE_9600
     "$PUBX,41,1,0003,0001,230400,0*1C\r\n",     // GPS_BAUDRATE_230400
-    "$PUBX,41,1,0003,0001,460800,0*1C\r\n",     // GPS_BAUDRATE_460800
-    "$PUBX,41,1,0003,0001,921600,0*1C\r\n"      // GPS_BAUDRATE_921600
+    "$PUBX,41,1,0003,0001,460800,0*13\r\n",     // GPS_BAUDRATE_460800
+    "$PUBX,41,1,0003,0001,921600,0*15\r\n"      // GPS_BAUDRATE_921600
 };
 
 // Packet checksum accumulators

From fcc4ad08c263775f5c6dee2fc27d7b46b33ee2ae Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Thu, 10 Aug 2023 22:49:23 +0100
Subject: [PATCH 68/94] clarify naming logic

---
 src/main/navigation/navigation_multicopter.c | 13 ++++++-------
 1 file changed, 6 insertions(+), 7 deletions(-)

diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c
index a70265380e7..59fc1177639 100644
--- a/src/main/navigation/navigation_multicopter.c
+++ b/src/main/navigation/navigation_multicopter.c
@@ -113,16 +113,15 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
 static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros)
 {
     // Calculate min and max throttle boundaries (to compensate for integral windup)
-    const int16_t thrAdjustmentMin = getThrottleIdleValue() - currentBatteryProfile->nav.mc.hover_throttle;
-    const int16_t thrAdjustmentMax = motorConfig()->maxthrottle - currentBatteryProfile->nav.mc.hover_throttle;
+    const int16_t thrCorrectionMin = getThrottleIdleValue() - currentBatteryProfile->nav.mc.hover_throttle;
+    const int16_t thrCorrectionMax = motorConfig()->maxthrottle - currentBatteryProfile->nav.mc.hover_throttle;
 
-    float velocity_controller = navPidApply2(&posControl.pids.vel[Z], posControl.desiredState.vel.z, navGetCurrentActualPositionAndVelocity()->vel.z, US2S(deltaMicros), thrAdjustmentMin, thrAdjustmentMax, 0);
+    float velocity_controller = navPidApply2(&posControl.pids.vel[Z], posControl.desiredState.vel.z, navGetCurrentActualPositionAndVelocity()->vel.z, US2S(deltaMicros), thrCorrectionMin, thrCorrectionMax, 0);
 
-    posControl.rcAdjustment[THROTTLE] = pt1FilterApply4(&altholdThrottleFilterState, velocity_controller, NAV_THROTTLE_CUTOFF_FREQENCY_HZ, US2S(deltaMicros));
+    int16_t rcThrottleCorrection = pt1FilterApply4(&altholdThrottleFilterState, velocity_controller, NAV_THROTTLE_CUTOFF_FREQENCY_HZ, US2S(deltaMicros));
+    rcThrottleCorrection = constrain(rcThrottleCorrection, thrCorrectionMin, thrCorrectionMax);
 
-    posControl.rcAdjustment[THROTTLE] = constrain(posControl.rcAdjustment[THROTTLE], thrAdjustmentMin, thrAdjustmentMax);
-
-    posControl.rcAdjustment[THROTTLE] = constrain(currentBatteryProfile->nav.mc.hover_throttle + posControl.rcAdjustment[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle);
+    posControl.rcAdjustment[THROTTLE] = constrain(currentBatteryProfile->nav.mc.hover_throttle + rcThrottleCorrection, getThrottleIdleValue(), motorConfig()->maxthrottle);
 }
 
 bool adjustMulticopterAltitudeFromRCInput(void)

From 7d84c641f4b3066d5d4d079fa8d5a2b2e1b4c8d6 Mon Sep 17 00:00:00 2001
From: psmitty7373 <psmitty7373@gmail.com>
Date: Sun, 13 Aug 2023 07:21:39 -0700
Subject: [PATCH 69/94] Fix SITL memory leaks (#9235)

---
 src/main/target/SITL/sim/realFlight.c         | 7 +++++--
 src/main/target/SITL/sim/simple_soap_client.c | 3 +++
 2 files changed, 8 insertions(+), 2 deletions(-)

diff --git a/src/main/target/SITL/sim/realFlight.c b/src/main/target/SITL/sim/realFlight.c
index eb164c42939..23e77bcde04 100644
--- a/src/main/target/SITL/sim/realFlight.c
+++ b/src/main/target/SITL/sim/realFlight.c
@@ -403,6 +403,9 @@ static void exchangeData(void)
         constrainToInt16(north.y * 16000.0f),
         constrainToInt16(north.z * 16000.0f)
     );
+
+    free(rfValues.m_currentAircraftStatus);
+    free(response);
 }
 
 static void* soapWorker(void* arg)
@@ -412,9 +415,9 @@ static void* soapWorker(void* arg)
     {     
         if (!isInitalised) {
             startRequest("RestoreOriginalControllerDevice", "<RestoreOriginalControllerDevice><a>1</a><b>2</b></RestoreOriginalControllerDevice>");
-            endRequest();
+            free(endRequest());
             startRequest("InjectUAVControllerInterface", "<InjectUAVControllerInterface><a>1</a><b>2</b></InjectUAVControllerInterface>");
-            endRequest();        
+            free(endRequest());  
             exchangeData();
             ENABLE_ARMING_FLAG(SIMULATOR_MODE_SITL);
             
diff --git a/src/main/target/SITL/sim/simple_soap_client.c b/src/main/target/SITL/sim/simple_soap_client.c
index f715e10ace0..e6400ae3b14 100644
--- a/src/main/target/SITL/sim/simple_soap_client.c
+++ b/src/main/target/SITL/sim/simple_soap_client.c
@@ -93,6 +93,9 @@ void soapClientSendRequestVa(soap_client_t *client, const char* action, const ch
          }
 
     send(client->sockedFd, request, strlen(request), 0);
+
+    free(requestBody);
+    free(request);
 }
 
 void soapClientSendRequest(soap_client_t *client, const char* action, const char *fmt, ...)

From be6ae879056f7663d33322dd71ebacbfd3b4f4a4 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Mon, 14 Aug 2023 22:23:53 +0100
Subject: [PATCH 70/94] change OSD indication

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

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index dacc897eb71..e88c8d8b33d 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -2022,10 +2022,6 @@ static bool osdDrawSingleElement(uint8_t item)
 
                 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
             }
-            if (STATE(MULTIROTOR) && posControl.flags.isAdjustingAltitude) {
-                TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr);
-                TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
-            }
             break;
         }
 
@@ -4893,6 +4889,9 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                     if (STATE(LANDING_DETECTED)) {
                         messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_LANDED);
                     }
+                    if (STATE(MULTIROTOR) && posControl.flags.isAdjustingAltitude) {
+                        messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALT_ADJUST);
+                    }
                 }
             }
         } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) {
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index 2dad5ceb819..1efd969c210 100644
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -118,6 +118,7 @@
 #define OSD_MSG_UNABLE_ARM          "UNABLE TO ARM"
 #define OSD_MSG_SAVING_SETTNGS      "** SAVING SETTINGS **"
 #define OSD_MSG_SETTINGS_SAVED      "** SETTINGS SAVED **"
+#define OSD_MSG_ALT_ADJUST          "(ALT ADJUST)"
 
 #ifdef USE_DEV_TOOLS
 #define OSD_MSG_GRD_TEST_MODE       "GRD TEST > MOTORS DISABLED"

From 3660fcb5976bf09b94e6da4c03f77862e7dcf798 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 15 Aug 2023 16:11:30 +0200
Subject: [PATCH 71/94] Add constrain for DynLPF computation

---
 src/main/flight/dynamic_lpf.c | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/main/flight/dynamic_lpf.c b/src/main/flight/dynamic_lpf.c
index c93cd921462..5751c84e85e 100644
--- a/src/main/flight/dynamic_lpf.c
+++ b/src/main/flight/dynamic_lpf.c
@@ -41,7 +41,8 @@ void dynamicLpfGyroTask(void) {
         return;
     }
 
-    const float throttle = scaleRangef((float) rcCommand[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle, 0.0f, 1.0f);
+    const float throttleConstrained = (float) constrain(rcCommand[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle);
+    const float throttle = scaleRangef(throttleConstrained, 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);

From 675111c9c3e6e71d6dbfed5689dd1b8ac232fd1c Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Thu, 17 Aug 2023 14:49:06 +0100
Subject: [PATCH 72/94] Update osd.c

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

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 7f92aaa0079..81ab495e3e5 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -2022,6 +2022,15 @@ static bool osdDrawSingleElement(uint8_t item)
 
                 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
             }
+            if (STATE(MULTIROTOR) && posControl.flags.isAdjustingAltitude) {
+                /* Indicate MR altitude adjustment active with constant "A" if altitude above -99m.
+                 * Alternate "A" on/off with 600ms cycle below -99m to maintain visibility of -ve sign */
+                if (alt > -9900) {
+                    buff[0] = 'A';
+                    break;
+                }
+                buff[0] = OSD_ALTERNATING_CHOICES(600, 2) == 0 ? 'A' : buff[0];
+            }
             break;
         }
 

From fb7f2423121f64319c5b17c89cca516fe40a4ee6 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Thu, 17 Aug 2023 20:53:54 +0100
Subject: [PATCH 73/94] Update osd.c

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

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 81ab495e3e5..e0ecad51c24 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -2023,9 +2023,9 @@ static bool osdDrawSingleElement(uint8_t item)
                 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
             }
             if (STATE(MULTIROTOR) && posControl.flags.isAdjustingAltitude) {
-                /* Indicate MR altitude adjustment active with constant "A" if altitude above -99m.
-                 * Alternate "A" on/off with 600ms cycle below -99m to maintain visibility of -ve sign */
-                if (alt > -9900) {
+                /* Indicate MR altitude adjustment active with constant "A" if first field position blank.
+                 * Alternate "A" on/off with 600ms cycle if not blank to maintain visibility of -ve sign */
+                if (buff[0] == SYM_BLANK) {
                     buff[0] = 'A';
                     break;
                 }

From bd454fb8544ed636b3f3a5bcda989746d1b2abc0 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Fri, 18 Aug 2023 13:02:39 +0200
Subject: [PATCH 74/94] BMI270 update

---
 src/main/target/SPEEDYBEEF7MINI/target.c |  4 ++++
 src/main/target/SPEEDYBEEF7MINI/target.h | 17 ++++++++++++-----
 2 files changed, 16 insertions(+), 5 deletions(-)

diff --git a/src/main/target/SPEEDYBEEF7MINI/target.c b/src/main/target/SPEEDYBEEF7MINI/target.c
index 468ac3c2f62..a7da42c07e2 100644
--- a/src/main/target/SPEEDYBEEF7MINI/target.c
+++ b/src/main/target/SPEEDYBEEF7MINI/target.c
@@ -26,7 +26,11 @@
 #include "drivers/pinio.h"
 #include "drivers/sensor.h"
 
+#ifdef SPEEDYBEEF7MINI2
+BUSDEV_REGISTER_SPI_TAG(busdev_bmi270,      DEVHW_BMI270,       BMI270_SPI_BUS,     BMI270_CS_PIN,      NONE,       0,  DEVFLAGS_NONE,  IMU_BMI270_ALIGN);
+#else
 BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000,     DEVHW_MPU6000,      MPU6000_SPI_BUS,    MPU6000_CS_PIN,     NONE,       0,  DEVFLAGS_NONE,  IMU_MPU6000_ALIGN);
+#endif
 
 timerHardware_t timerHardware[] = {
     DEF_TIM(TIM3, CH1, PB4,   TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S1   UP1-2   D(1, 4, 5)
diff --git a/src/main/target/SPEEDYBEEF7MINI/target.h b/src/main/target/SPEEDYBEEF7MINI/target.h
index e95101469cf..8b2e469ceb5 100644
--- a/src/main/target/SPEEDYBEEF7MINI/target.h
+++ b/src/main/target/SPEEDYBEEF7MINI/target.h
@@ -19,7 +19,12 @@
 #pragma once
 
 #define TARGET_BOARD_IDENTIFIER "SBMN"
+
+#ifdef SPEEDYBEEF7MINI2
+#define USBD_PRODUCT_STRING  "SPEEDYBEEF7MINI2"
+#else
 #define USBD_PRODUCT_STRING  "SPEEDYBEEF7MINI"
+#endif
 
 #define LED0                    PA14  //Blue   SWCLK
 
@@ -35,11 +40,6 @@
 #define SPI1_MISO_PIN           PA6
 #define SPI1_MOSI_PIN           PA7
 
-#define USE_IMU_MPU6000
-#define IMU_MPU6000_ALIGN       CW0_DEG
-#define MPU6000_CS_PIN          PB2
-#define MPU6000_SPI_BUS         BUS_SPI1
-
 #ifdef SPEEDYBEEF7MINI2
 
 #define USE_IMU_BMI270
@@ -47,6 +47,13 @@
 #define BMI270_CS_PIN           PB2
 #define BMI270_SPI_BUS          BUS_SPI1
 
+#else 
+
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW0_DEG
+#define MPU6000_CS_PIN          PB2
+#define MPU6000_SPI_BUS         BUS_SPI1
+
 #endif
 
 // *************** I2C /Baro/Mag *********************

From 90d8a0ef3778e5e9f33ea9b63ced1d868a9d7833 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Sat, 19 Aug 2023 21:26:12 +0100
Subject: [PATCH 75/94] add target heading

---
 src/main/blackbox/blackbox.c     | 22 +++++++++++++---------
 src/main/navigation/navigation.c |  6 ++++--
 src/main/navigation/navigation.h |  1 +
 3 files changed, 18 insertions(+), 11 deletions(-)

diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index 0169a30995f..dc7c0c3239f 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -289,23 +289,23 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
     {"gyroADC",     0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
     {"gyroADC",     1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
     {"gyroADC",     2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
-    
+
     {"gyroRaw",     0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_RAW},
     {"gyroRaw",     1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_RAW},
     {"gyroRaw",     2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_RAW},
-    
+
     {"gyroPeakRoll",    0, UNSIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(UNSIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_ROLL},
     {"gyroPeakRoll",    1, UNSIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(UNSIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_ROLL},
     {"gyroPeakRoll",    2, UNSIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(UNSIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_ROLL},
-    
+
     {"gyroPeakPitch",    0, UNSIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(UNSIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_PITCH},
     {"gyroPeakPitch",    1, UNSIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(UNSIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_PITCH},
     {"gyroPeakPitch",    2, UNSIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(UNSIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_PITCH},
-    
+
     {"gyroPeakYaw",    0, UNSIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(UNSIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_YAW},
     {"gyroPeakYaw",    1, UNSIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(UNSIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_YAW},
     {"gyroPeakYaw",    2, UNSIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(UNSIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_YAW},
-    
+
 
     {"accSmooth",   0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC},
     {"accSmooth",   1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC},
@@ -366,6 +366,7 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
     {"navTgtPos",  0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
     {"navTgtPos",  1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
     {"navTgtPos",  2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
+    {"navTgtHdg", -1, UNSIGNED, .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
     {"navSurf",   -1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
     {"navAcc",     0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_ACC},
     {"navAcc",     1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_ACC},
@@ -517,7 +518,7 @@ typedef struct blackboxMainState_s {
     int16_t navTargetVel[XYZ_AXIS_COUNT];
     int32_t navTargetPos[XYZ_AXIS_COUNT];
     int16_t navHeading;
-    int16_t navTargetHeading;
+    uint16_t navTargetHeading;
     int16_t navSurface;
 } blackboxMainState_t;
 
@@ -740,7 +741,7 @@ static void blackboxBuildConditionCache(void)
 {
     blackboxConditionCache = 0;
     for (uint8_t cond = FLIGHT_LOG_FIELD_CONDITION_FIRST; cond <= FLIGHT_LOG_FIELD_CONDITION_LAST; cond++) {
-        
+
         const uint64_t position = ((uint64_t)1) << cond;
 
         if (testBlackboxConditionUncached(cond)) {
@@ -891,7 +892,7 @@ static void writeIntraframe(void)
     }
 
     blackboxWriteSigned16VBArray(blackboxCurrent->gyroADC, XYZ_AXIS_COUNT);
-                     
+
     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_GYRO_RAW)) {
         blackboxWriteSigned16VBArray(blackboxCurrent->gyroRaw, XYZ_AXIS_COUNT);
     }
@@ -970,6 +971,7 @@ static void writeIntraframe(void)
             blackboxWriteSignedVB(blackboxCurrent->navTargetPos[x]);
         }
 
+        blackboxWriteSignedVB(blackboxCurrent->navTargetHeading);
         blackboxWriteSignedVB(blackboxCurrent->navSurface);
     }
 
@@ -1226,6 +1228,7 @@ static void writeInterframe(void)
             blackboxWriteSignedVB(blackboxHistory[0]->navTargetPos[x] - blackboxLast->navTargetPos[x]);
         }
 
+        blackboxWriteSignedVB(blackboxCurrent->navTargetHeading - blackboxLast->navTargetHeading);
         blackboxWriteSignedVB(blackboxCurrent->navSurface - blackboxLast->navSurface);
     }
 
@@ -1328,7 +1331,7 @@ static void loadSlowState(blackboxSlowState_t *slow)
     bool valid_temp;
     int16_t newTemp = 0;
     valid_temp = getIMUTemperature(&newTemp);
-    if (valid_temp) 
+    if (valid_temp)
         slow->imuTemperature = newTemp;
     else
         slow->imuTemperature = TEMPERATURE_INVALID_VALUE;
@@ -1661,6 +1664,7 @@ static void loadMainState(timeUs_t currentTimeUs)
         blackboxCurrent->navTargetVel[i] = navDesiredVelocity[i];
         blackboxCurrent->navTargetPos[i] = navTargetPosition[i];
     }
+    blackboxCurrent->navTargetHeading = navDesiredHeading;
     blackboxCurrent->navSurface = navActualSurface;
 }
 
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index d8bc20554cc..099aa57f5f5 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -235,10 +235,10 @@ EXTENDED_FASTRAM multicopterPosXyCoefficients_t multicopterPosXyCoefficients;
 int16_t navCurrentState;
 int16_t navActualVelocity[3];
 int16_t navDesiredVelocity[3];
-int16_t navActualHeading;
-int16_t navDesiredHeading;
 int32_t navTargetPosition[3];
 int32_t navLatestActualPosition[3];
+int16_t navActualHeading;
+uint16_t navDesiredHeading;
 int16_t navActualSurface;
 uint16_t navFlags;
 uint16_t navEPH;
@@ -3564,6 +3564,8 @@ void applyWaypointNavigationAndAltitudeHold(void)
     navTargetPosition[X] = lrintf(posControl.desiredState.pos.x);
     navTargetPosition[Y] = lrintf(posControl.desiredState.pos.y);
     navTargetPosition[Z] = lrintf(posControl.desiredState.pos.z);
+
+    navDesiredHeading = wrap_36000(posControl.desiredState.yaw);
 }
 
 /*-----------------------------------------------------------
diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
index cf4289e14a1..8962de39988 100644
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@@ -638,6 +638,7 @@ extern int16_t navActualVelocity[3];
 extern int16_t navDesiredVelocity[3];
 extern int32_t navTargetPosition[3];
 extern int32_t navLatestActualPosition[3];
+extern uint16_t navDesiredHeading;
 extern int16_t navActualSurface;
 extern uint16_t navFlags;
 extern uint16_t navEPH;

From d0a4bd2fcda79f1825cab1472cf64c2b6da8bc68 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Mon, 21 Aug 2023 15:29:14 +0200
Subject: [PATCH 76/94] Target updates

---
 src/main/target/SPEEDYBEEF7MINI/CMakeLists.txt | 2 +-
 src/main/target/SPEEDYBEEF7MINI/target.c       | 2 +-
 src/main/target/SPEEDYBEEF7MINI/target.h       | 8 ++++----
 3 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/main/target/SPEEDYBEEF7MINI/CMakeLists.txt b/src/main/target/SPEEDYBEEF7MINI/CMakeLists.txt
index d1b5f7e91f8..1ae9febd177 100644
--- a/src/main/target/SPEEDYBEEF7MINI/CMakeLists.txt
+++ b/src/main/target/SPEEDYBEEF7MINI/CMakeLists.txt
@@ -1,2 +1,2 @@
 target_stm32f722xe(SPEEDYBEEF7MINI)
-target_stm32f722xe(SPEEDYBEEF7MINI2)
+target_stm32f722xe(SPEEDYBEEF7MINIV2)
diff --git a/src/main/target/SPEEDYBEEF7MINI/target.c b/src/main/target/SPEEDYBEEF7MINI/target.c
index a7da42c07e2..7cc2506b66c 100644
--- a/src/main/target/SPEEDYBEEF7MINI/target.c
+++ b/src/main/target/SPEEDYBEEF7MINI/target.c
@@ -26,7 +26,7 @@
 #include "drivers/pinio.h"
 #include "drivers/sensor.h"
 
-#ifdef SPEEDYBEEF7MINI2
+#ifdef SPEEDYBEEF7MINIV2
 BUSDEV_REGISTER_SPI_TAG(busdev_bmi270,      DEVHW_BMI270,       BMI270_SPI_BUS,     BMI270_CS_PIN,      NONE,       0,  DEVFLAGS_NONE,  IMU_BMI270_ALIGN);
 #else
 BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000,     DEVHW_MPU6000,      MPU6000_SPI_BUS,    MPU6000_CS_PIN,     NONE,       0,  DEVFLAGS_NONE,  IMU_MPU6000_ALIGN);
diff --git a/src/main/target/SPEEDYBEEF7MINI/target.h b/src/main/target/SPEEDYBEEF7MINI/target.h
index 8b2e469ceb5..0dd3a6e7a83 100644
--- a/src/main/target/SPEEDYBEEF7MINI/target.h
+++ b/src/main/target/SPEEDYBEEF7MINI/target.h
@@ -20,8 +20,8 @@
 
 #define TARGET_BOARD_IDENTIFIER "SBMN"
 
-#ifdef SPEEDYBEEF7MINI2
-#define USBD_PRODUCT_STRING  "SPEEDYBEEF7MINI2"
+#ifdef SPEEDYBEEF7MINIV2
+#define USBD_PRODUCT_STRING  "SPEEDYBEEF7MINIV2"
 #else
 #define USBD_PRODUCT_STRING  "SPEEDYBEEF7MINI"
 #endif
@@ -40,10 +40,10 @@
 #define SPI1_MISO_PIN           PA6
 #define SPI1_MOSI_PIN           PA7
 
-#ifdef SPEEDYBEEF7MINI2
+#ifdef SPEEDYBEEF7MINIV2
 
 #define USE_IMU_BMI270
-#define IMU_BMI270_ALIGN        CW0_DEG
+#define IMU_BMI270_ALIGN        CW180_DEG
 #define BMI270_CS_PIN           PB2
 #define BMI270_SPI_BUS          BUS_SPI1
 

From e7b765f5b797424cfd4fe3f8ac55352d10db37cc Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Tue, 22 Aug 2023 09:49:21 +0100
Subject: [PATCH 77/94] Update osd.c

---
 src/main/io/osd.c | 12 +++++++++---
 1 file changed, 9 insertions(+), 3 deletions(-)

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index e0ecad51c24..1c52ff9c92b 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -2023,9 +2023,15 @@ static bool osdDrawSingleElement(uint8_t item)
                 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
             }
             if (STATE(MULTIROTOR) && posControl.flags.isAdjustingAltitude) {
-                /* Indicate MR altitude adjustment active with constant "A" if first field position blank.
-                 * Alternate "A" on/off with 600ms cycle if not blank to maintain visibility of -ve sign */
-                if (buff[0] == SYM_BLANK) {
+                /* Indicate MR altitude adjustment active with constant "A" at first blank position.
+                 * Alternate "A" on/off with 600ms cycle if first position not blank (to maintain visibility of -ve sign) */
+                if (buff[2] == SYM_BLANK) {
+                    buff[2] = 'A';
+                    break;
+                } else if (buff[1] == SYM_BLANK) {
+                    buff[1] = 'A';
+                    break;
+                } else if (buff[0] == SYM_BLANK) {
                     buff[0] = 'A';
                     break;
                 }

From 1524cc5c21f5608ce548203dc6b39995a7e6eaca Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Tue, 22 Aug 2023 10:15:30 +0100
Subject: [PATCH 78/94] Add missing flight modes

---
 src/main/blackbox/blackbox.c     | 10 ++++++++++
 src/main/flight/pid.c            |  6 ++----
 src/main/navigation/navigation.c |  5 ++---
 3 files changed, 14 insertions(+), 7 deletions(-)

diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index dc7c0c3239f..8db66f600e9 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -1301,6 +1301,16 @@ static void writeSlowFrame(void)
 static void loadSlowState(blackboxSlowState_t *slow)
 {
     memcpy(&slow->flightModeFlags, &rcModeActivationMask, sizeof(slow->flightModeFlags)); //was flightModeFlags;
+    // Also log Nav auto selected flight modes rather than just those selected by boxmode
+    if (!IS_RC_MODE_ACTIVE(BOXANGLE) && FLIGHT_MODE(ANGLE_MODE)) {
+        slow->flightModeFlags |= (1 << BOXANGLE);
+    }
+    if (navigationGetHeadingControlState() == HEADING_HOLD_ENABLED) {
+        slow->flightModeFlags |= (1 << BOXHEADINGHOLD);
+    }
+    if (navigationRequiresTurnAssistance()) {
+        slow->flightModeFlags |= (1 << BOXTURNASSIST);
+    }
     slow->stateFlags = stateFlags;
     slow->failsafePhase = failsafePhase();
     slow->rxSignalReceived = rxIsReceivingSignal();
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 20333071b0b..16d0ee2d9eb 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -886,8 +886,6 @@ static uint8_t getHeadingHoldState(void)
     }
     else if (ABS(rcCommand[YAW]) == 0 && FLIGHT_MODE(HEADING_MODE)) {
         return HEADING_HOLD_ENABLED;
-    } else {
-        return HEADING_HOLD_UPDATE_HEADING;
     }
 
     return HEADING_HOLD_UPDATE_HEADING;
@@ -1124,10 +1122,10 @@ void FAST_CODE pidController(float dT)
             pidLevel(angleTarget, &pidState[axis], axis, horizonRateMagnitude, dT);
             canUseFpvCameraMix = false;     // FPVANGLEMIX is incompatible with ANGLE/HORIZON
             levelingEnabled = true;
-        }       
+        }
     }
 
-    if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || navigationRequiresTurnAssistance())) {
+    if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
         float bankAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL]));
         float pitchAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_PITCH], pidProfile()->max_angle_inclination[FD_PITCH]));
         pidTurnAssistant(pidState, bankAngleTarget, pitchAngleTarget);
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 099aa57f5f5..e07951e9a6d 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -3836,9 +3836,8 @@ bool navigationRequiresTurnAssistance(void)
         // For airplanes turn assistant is always required when controlling position
         return (currentState & (NAV_CTL_POS | NAV_CTL_ALT));
     }
-    else {
-        return false;
-    }
+
+    return false;
 }
 
 /**

From a0ef0f93496a6e09bcbd6b7cd578c18d7255772a Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Tue, 22 Aug 2023 21:59:31 +0100
Subject: [PATCH 79/94] Fix alt adjust indication

---
 src/main/drivers/osd_symbols.h |  2 ++
 src/main/io/osd.c              | 25 +++++++++++--------------
 src/main/io/osd.h              |  1 -
 3 files changed, 13 insertions(+), 15 deletions(-)

diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h
index a127234579d..34f66dc84d7 100644
--- a/src/main/drivers/osd_symbols.h
+++ b/src/main/drivers/osd_symbols.h
@@ -174,6 +174,8 @@
 #define SYM_FLIGHT_MINS_REMAINING   0xDA  // 218 Flight time (mins) remaining
 #define SYM_FLIGHT_HOURS_REMAINING  0xDB  // 219 Flight time (hours) remaining
 #define SYM_GROUND_COURSE           0xDC  // 220 Ground course
+
+#define SYM_TERRAIN_FOLLOWING       0xFB  // 251 General alert symbol
 #define SYM_CROSS_TRACK_ERROR       0xFC  // 252 Cross track error
 
 #define SYM_LOGO_START              0x101 // 257 to 297, INAV logo
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 1c52ff9c92b..1c7d0739120 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -2022,22 +2022,22 @@ static bool osdDrawSingleElement(uint8_t item)
 
                 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
             }
+
             if (STATE(MULTIROTOR) && posControl.flags.isAdjustingAltitude) {
                 /* Indicate MR altitude adjustment active with constant "A" at first blank position.
                  * Alternate "A" on/off with 600ms cycle if first position not blank (to maintain visibility of -ve sign) */
-                if (buff[2] == SYM_BLANK) {
-                    buff[2] = 'A';
-                    break;
-                } else if (buff[1] == SYM_BLANK) {
-                    buff[1] = 'A';
-                    break;
-                } else if (buff[0] == SYM_BLANK) {
-                    buff[0] = 'A';
-                    break;
+                int8_t blankPos;
+                for (blankPos = 2; blankPos >= 0; blankPos--) {
+                    if (buff[blankPos] == SYM_BLANK) {
+                        break;
+                    }
+                }
+                if (blankPos >= 0 || OSD_ALTERNATING_CHOICES(600, 2) == 0) {
+                    blankPos = blankPos < 0 ? 0 : blankPos;
+                    displayWriteChar(osdDisplayPort, elemPosX + blankPos, elemPosY, SYM_TERRAIN_FOLLOWING);
                 }
-                buff[0] = OSD_ALTERNATING_CHOICES(600, 2) == 0 ? 'A' : buff[0];
             }
-            break;
+            return true;
         }
 
     case OSD_ALTITUDE_MSL:
@@ -4904,9 +4904,6 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                     if (STATE(LANDING_DETECTED)) {
                         messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_LANDED);
                     }
-                    if (STATE(MULTIROTOR) && posControl.flags.isAdjustingAltitude) {
-                        messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALT_ADJUST);
-                    }
                 }
             }
         } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) {
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index 1efd969c210..2dad5ceb819 100644
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -118,7 +118,6 @@
 #define OSD_MSG_UNABLE_ARM          "UNABLE TO ARM"
 #define OSD_MSG_SAVING_SETTNGS      "** SAVING SETTINGS **"
 #define OSD_MSG_SETTINGS_SAVED      "** SETTINGS SAVED **"
-#define OSD_MSG_ALT_ADJUST          "(ALT ADJUST)"
 
 #ifdef USE_DEV_TOOLS
 #define OSD_MSG_GRD_TEST_MODE       "GRD TEST > MOTORS DISABLED"

From 8e6a4b2db4d711beec2cb114603632edfca221a8 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Tue, 22 Aug 2023 22:07:10 +0100
Subject: [PATCH 80/94] Update osd_symbols.h

---
 src/main/drivers/osd_symbols.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h
index 34f66dc84d7..06ac1fda949 100644
--- a/src/main/drivers/osd_symbols.h
+++ b/src/main/drivers/osd_symbols.h
@@ -175,7 +175,7 @@
 #define SYM_FLIGHT_HOURS_REMAINING  0xDB  // 219 Flight time (hours) remaining
 #define SYM_GROUND_COURSE           0xDC  // 220 Ground course
 
-#define SYM_TERRAIN_FOLLOWING       0xFB  // 251 General alert symbol
+#define SYM_TERRAIN_FOLLOWING       0xFB  // 251 Terrain following (also Alt adjust)
 #define SYM_CROSS_TRACK_ERROR       0xFC  // 252 Cross track error
 
 #define SYM_LOGO_START              0x101 // 257 to 297, INAV logo

From e1f046fb5dfe43df5b1dacfffa65e7937706e40d Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Tue, 22 Aug 2023 22:16:18 +0100
Subject: [PATCH 81/94] Update osd.c

---
 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 1c7d0739120..b9bb0e62254 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -2024,8 +2024,8 @@ static bool osdDrawSingleElement(uint8_t item)
             }
 
             if (STATE(MULTIROTOR) && posControl.flags.isAdjustingAltitude) {
-                /* Indicate MR altitude adjustment active with constant "A" at first blank position.
-                 * Alternate "A" on/off with 600ms cycle if first position not blank (to maintain visibility of -ve sign) */
+                /* Indicate MR altitude adjustment active with constant symbol at first blank position.
+                 * Alternate symbol on/off with 600ms cycle if first position not blank (to maintain visibility of -ve sign) */
                 int8_t blankPos;
                 for (blankPos = 2; blankPos >= 0; blankPos--) {
                     if (buff[blankPos] == SYM_BLANK) {

From 41b493a230587187419614f13b6cb0cf6743d772 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Tue, 22 Aug 2023 22:32:08 +0100
Subject: [PATCH 82/94] Update blackbox.c

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

diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index 8db66f600e9..cb12a7a8968 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -1305,7 +1305,7 @@ static void loadSlowState(blackboxSlowState_t *slow)
     if (!IS_RC_MODE_ACTIVE(BOXANGLE) && FLIGHT_MODE(ANGLE_MODE)) {
         slow->flightModeFlags |= (1 << BOXANGLE);
     }
-    if (navigationGetHeadingControlState() == HEADING_HOLD_ENABLED) {
+    if (navigationGetHeadingControlState() == NAV_HEADING_CONTROL_AUTO) {
         slow->flightModeFlags |= (1 << BOXHEADINGHOLD);
     }
     if (navigationRequiresTurnAssistance()) {

From 72912aa15057d48101be630b409b9d0ce2796d30 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Fri, 25 Aug 2023 07:47:10 +0100
Subject: [PATCH 83/94] Update osd.c

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

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index b9bb0e62254..a17ad486581 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -2022,6 +2022,7 @@ static bool osdDrawSingleElement(uint8_t item)
 
                 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
             }
+            displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr);
 
             if (STATE(MULTIROTOR) && posControl.flags.isAdjustingAltitude) {
                 /* Indicate MR altitude adjustment active with constant symbol at first blank position.
@@ -2040,6 +2041,7 @@ static bool osdDrawSingleElement(uint8_t item)
             return true;
         }
 
+
     case OSD_ALTITUDE_MSL:
         {
             int32_t alt = osdGetAltitudeMsl();

From 7b982c65eb2c814adb5b310cf653fa8bebe5c7b6 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Fri, 25 Aug 2023 22:31:51 +0200
Subject: [PATCH 84/94] Change motor/servo assignements for RUSH_BLADE_F7
 targets

S5 and S6 share TIM8 with S1 and S2, so they need to have consistent function assignments.
---
 src/main/target/RUSH_BLADE_F7/target.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/target/RUSH_BLADE_F7/target.c b/src/main/target/RUSH_BLADE_F7/target.c
index 6052a198232..565c8a23465 100644
--- a/src/main/target/RUSH_BLADE_F7/target.c
+++ b/src/main/target/RUSH_BLADE_F7/target.c
@@ -27,8 +27,8 @@ timerHardware_t timerHardware[] = {
     DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2
     DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3
     DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4
-    DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // S5
-    DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // S6
+    DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S5
+    DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S6
     DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // S7
     DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // S8
 

From dd1afc850c3051adc72e4325a9d251ee0e4b1778 Mon Sep 17 00:00:00 2001
From: Andrey Akimov <shirase55@gmail.com>
Date: Mon, 28 Aug 2023 13:55:09 +0300
Subject: [PATCH 85/94] timer dma burst

---
 src/main/drivers/pwm_output.c           | 71 ++++++++++++++++++
 src/main/drivers/timer.c                | 12 +++
 src/main/drivers/timer.h                | 18 +++++
 src/main/drivers/timer_impl.h           |  5 ++
 src/main/drivers/timer_impl_stdperiph.c | 98 +++++++++++++++++++++++++
 5 files changed, 204 insertions(+)

diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c
index 300853513f4..5351c9dfc34 100644
--- a/src/main/drivers/pwm_output.c
+++ b/src/main/drivers/pwm_output.c
@@ -42,6 +42,9 @@
 #include "fc/config.h"
 #include "fc/runtime_config.h"
 
+#include "drivers/timer_impl.h"
+#include "drivers/timer.h"
+
 #define MULTISHOT_5US_PW    (MULTISHOT_TIMER_HZ * 5 / 1000000.0f)
 #define MULTISHOT_20US_MULT (MULTISHOT_TIMER_HZ * 20 / 1000000.0f / 1000.0f)
 
@@ -56,6 +59,7 @@
 #define DSHOT_MOTOR_BITLENGTH   20
 
 #define DSHOT_DMA_BUFFER_SIZE   18 /* resolution + frame reset (2us) */
+#define MAX_DMA_TIMERS          8
 
 #define DSHOT_COMMAND_INTERVAL_US 10000
 #define DSHOT_COMMAND_QUEUE_LENGTH 8
@@ -64,6 +68,10 @@
 
 typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value);  // function pointer used to write motors
 
+#ifdef USE_DSHOT_DMAR
+    timerDMASafeType_t dmaBurstBuffer[MAX_DMA_TIMERS][DSHOT_DMA_BUFFER_SIZE * 4];
+#endif
+
 typedef struct {
     TCH_t * tch;
     bool configured;
@@ -77,6 +85,9 @@ typedef struct {
 #ifdef USE_DSHOT
     // DSHOT parameters
     timerDMASafeType_t dmaBuffer[DSHOT_DMA_BUFFER_SIZE];
+#ifdef USE_DSHOT_DMAR
+    timerDMASafeType_t *dmaBurstBuffer;
+#endif
 #endif
 } pwmOutputPort_t;
 
@@ -249,6 +260,22 @@ uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType)
     }
 }
 
+#ifdef USE_DSHOT_DMAR
+burstDmaTimer_t burstDmaTimers[MAX_DMA_TIMERS];
+uint8_t burstDmaTimersCount = 0;
+
+static uint8_t getBurstDmaTimerIndex(TIM_TypeDef *timer)
+{
+    for (int i = 0; i < burstDmaTimersCount; i++) {
+        if (burstDmaTimers[i].timer == timer) {
+            return i;
+        }
+    }
+    burstDmaTimers[burstDmaTimersCount++].timer = timer;
+    return burstDmaTimersCount - 1;
+}
+#endif
+
 static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware, uint32_t dshotHz, bool enableOutput)
 {
     // Try allocating new port
@@ -259,15 +286,43 @@ static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware,
     }
 
     // Configure timer DMA
+#ifdef USE_DSHOT_DMAR
+    //uint8_t timerIndex = lookupTimerIndex(timerHardware->tim);
+    uint8_t burstDmaTimerIndex = getBurstDmaTimerIndex(timerHardware->tim);
+    if (burstDmaTimerIndex >= MAX_DMA_TIMERS) {
+        return NULL;
+    }
+
+    port->dmaBurstBuffer = &dmaBurstBuffer[burstDmaTimerIndex][0];
+    burstDmaTimer_t *burstDmaTimer = &burstDmaTimers[burstDmaTimerIndex];
+    burstDmaTimer->dmaBurstBuffer = port->dmaBurstBuffer;
+
+    if (timerPWMConfigDMABurst(burstDmaTimer, port->tch, port->dmaBurstBuffer, sizeof(port->dmaBurstBuffer[0]), DSHOT_DMA_BUFFER_SIZE)) {
+        port->configured = true;
+    }
+#else
     if (timerPWMConfigChannelDMA(port->tch, port->dmaBuffer, sizeof(port->dmaBuffer[0]), DSHOT_DMA_BUFFER_SIZE)) {
         // Only mark as DSHOT channel if DMA was set successfully
         ZERO_FARRAY(port->dmaBuffer);
         port->configured = true;
     }
+#endif
 
     return port;
 }
 
+#ifdef USE_DSHOT_DMAR
+static void loadDmaBufferDshotStride(timerDMASafeType_t *dmaBuffer, int stride, uint16_t packet)
+{
+    int i;
+    for (i = 0; i < 16; i++) {
+        dmaBuffer[i * stride] = (packet & 0x8000) ? DSHOT_MOTOR_BIT_1 : DSHOT_MOTOR_BIT_0;  // MSB first
+        packet <<= 1;
+    }
+    dmaBuffer[i++ * stride] = 0;
+    dmaBuffer[i++ * stride] = 0;
+}
+#else
 static void loadDmaBufferDshot(timerDMASafeType_t *dmaBuffer, uint16_t packet)
 {
     for (int i = 0; i < 16; i++) {
@@ -275,6 +330,7 @@ static void loadDmaBufferDshot(timerDMASafeType_t *dmaBuffer, uint16_t packet)
         packet <<= 1;
     }
 }
+#endif
 
 static uint16_t prepareDshotPacket(const uint16_t value, bool requestTelemetry)
 {
@@ -408,6 +464,20 @@ void pwmCompleteMotorUpdate(void) {
 
         executeDShotCommands();
 
+#ifdef USE_DSHOT_DMAR
+        for (int index = 0; index < motorCount; index++) {
+            if (motors[index].pwmPort && motors[index].pwmPort->configured) {
+                uint16_t packet = prepareDshotPacket(motors[index].value, motors[index].requestTelemetry);
+                loadDmaBufferDshotStride(&motors[index].pwmPort->dmaBurstBuffer[motors[index].pwmPort->tch->timHw->channelIndex], 4, packet);
+                motors[index].requestTelemetry = false;
+            }
+        }
+
+        for (int burstDmaTimerIndex = 0; burstDmaTimerIndex < burstDmaTimersCount; burstDmaTimerIndex++) {
+            burstDmaTimer_t *burstDmaTimer = &burstDmaTimers[burstDmaTimerIndex];
+            pwmBurstDMAStart(burstDmaTimer, TIM_DMABase_CCR1, TIM_DMABurstLength_4Transfers, DSHOT_DMA_BUFFER_SIZE * 4);
+        }
+#else
         // Generate DMA buffers
         for (int index = 0; index < motorCount; index++) {
             if (motors[index].pwmPort && motors[index].pwmPort->configured) {
@@ -424,6 +494,7 @@ void pwmCompleteMotorUpdate(void) {
                 timerPWMStartDMA(motors[index].pwmPort->tch);
             }
         }
+#endif
     }
 #endif
 }
diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c
index ecdf1ba48d0..79b60e975b9 100644
--- a/src/main/drivers/timer.c
+++ b/src/main/drivers/timer.c
@@ -287,3 +287,15 @@ bool timerPWMDMAInProgress(TCH_t * tch)
 {
     return tch->dmaState != TCH_DMA_IDLE;
 }
+
+#ifdef USE_DSHOT_DMAR
+bool timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount)
+{
+    return impl_timerPWMConfigDMABurst(burstDmaTimer, tch, dmaBuffer, dmaBufferElementSize, dmaBufferElementCount);
+}
+
+void pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstBaseAddress, uint32_t BurstUnit, uint32_t BurstLength)
+{
+    impl_pwmBurstDMAStart(burstDmaTimer, BurstBaseAddress, BurstUnit, BurstLength);
+}
+#endif
diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h
index 5272c804b8f..7c2c3262e07 100644
--- a/src/main/drivers/timer.h
+++ b/src/main/drivers/timer.h
@@ -158,6 +158,10 @@ typedef struct timHardwareContext_s {
     TIM_HandleTypeDef * timHandle;
 #endif
     TCH_t               ch[CC_CHANNELS_PER_TIMER];
+#ifdef USE_DSHOT_DMAR
+    DMA_t dmaBurstRef;
+    uint16_t DMASource;
+#endif
 } timHardwareContext_t;
 
 // Per MCU timer definitions
@@ -168,6 +172,15 @@ extern const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT];
 extern timerHardware_t timerHardware[];
 extern const int timerHardwareCount;
 
+#ifdef USE_DSHOT_DMAR
+typedef struct {
+    TIM_TypeDef *timer;
+    DMA_Stream_TypeDef *dmaBurstStream;
+    timerDMASafeType_t *dmaBurstBuffer;
+    uint16_t burstRequestSource;
+} burstDmaTimer_t;
+#endif
+
 typedef enum {
     TYPE_FREE,
     TYPE_PWMINPUT,
@@ -229,3 +242,8 @@ void timerPWMStopDMA(TCH_t * tch);
 bool timerPWMDMAInProgress(TCH_t * tch);
 
 volatile timCCR_t *timerCCR(TCH_t * tch);
+
+#ifdef USE_DSHOT_DMAR
+bool timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount);
+void pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstBaseAddress, uint32_t BurstUnit, uint32_t BurstLength);
+#endif
\ No newline at end of file
diff --git a/src/main/drivers/timer_impl.h b/src/main/drivers/timer_impl.h
index 82c09837219..e869b986202 100644
--- a/src/main/drivers/timer_impl.h
+++ b/src/main/drivers/timer_impl.h
@@ -84,3 +84,8 @@ bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBuf
 void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferElementCount);
 void impl_timerPWMStartDMA(TCH_t * tch);
 void impl_timerPWMStopDMA(TCH_t * tch);
+
+#ifdef USE_DSHOT_DMAR
+bool impl_timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount);
+void impl_pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstBaseAddress, uint32_t BurstUnit, uint32_t BurstLength);
+#endif
\ No newline at end of file
diff --git a/src/main/drivers/timer_impl_stdperiph.c b/src/main/drivers/timer_impl_stdperiph.c
index 599b8b5cb3f..17039c6822a 100644
--- a/src/main/drivers/timer_impl_stdperiph.c
+++ b/src/main/drivers/timer_impl_stdperiph.c
@@ -357,6 +357,104 @@ bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBuf
     return true;
 }
 
+#ifdef USE_DSHOT_DMAR
+bool impl_timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount)
+{
+    DMA_InitTypeDef DMA_InitStructure;
+    TIM_TypeDef * timer = tch->timHw->tim;
+    
+    if (!tch->timCtx->dmaBurstRef) {
+        tch->dma = dmaGetByTag(tch->timHw->dmaTag);
+        if (tch->dma == NULL) {
+            return false;
+        }
+
+        // If DMA is already in use - abort
+        if (tch->dma->owner != OWNER_FREE) {
+            return false;
+        }
+    }
+
+    // We assume that timer channels are already initialized by calls to:
+    //  timerConfigBase
+    //  timerPWMConfigChannel
+
+    TIM_CtrlPWMOutputs(timer, ENABLE);
+    TIM_ARRPreloadConfig(timer, ENABLE);
+
+    TIM_CCxCmd(timer, lookupTIMChannelTable[tch->timHw->channelIndex], TIM_CCx_Enable);
+    TIM_Cmd(timer, ENABLE);
+
+    if (!tch->timCtx->dmaBurstRef) {
+        dmaInit(tch->dma, OWNER_TIMER, 0);
+        dmaSetHandler(tch->dma, impl_timerDMA_IRQHandler, NVIC_PRIO_TIMER_DMA, (uint32_t)tch);
+
+        DMA_DeInit(tch->dma->ref);
+        DMA_Cmd(tch->dma->ref, DISABLE);
+
+        DMA_StructInit(&DMA_InitStructure);
+
+        DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&tch->timHw->tim->DMAR;
+        DMA_InitStructure.DMA_BufferSize = dmaBufferElementCount;
+        DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
+        DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
+        DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
+
+        switch (dmaBufferElementSize) {
+            case 1:
+                DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
+                DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
+                break;
+            case 2:
+                DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord;
+                DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord;
+                break;
+            case 4:
+                DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Word;
+                DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word;
+                break;
+            default:
+                // Programmer error
+                while(1) {
+
+                }
+        }
+
+#ifdef STM32F4
+        DMA_InitStructure.DMA_Channel = dmaGetChannelByTag(tch->timHw->dmaTag);
+        DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)dmaBuffer;
+        DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral;
+        DMA_InitStructure.DMA_Priority = DMA_Priority_High;
+#else // F3
+        DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)dmaBuffer;
+        DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST;
+        DMA_InitStructure.DMA_Priority = DMA_Priority_High;
+        DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
+#endif
+
+        DMA_Init(tch->dma->ref, &DMA_InitStructure);
+        DMA_ITConfig(tch->dma->ref, DMA_IT_TC, ENABLE);
+
+        tch->timCtx->dmaBurstRef = tch->dma;
+        tch->timCtx->DMASource = lookupDMASourceTable[tch->timHw->channelIndex];
+        burstDmaTimer->dmaBurstStream = tch->timCtx->dmaBurstRef->ref;
+        burstDmaTimer->burstRequestSource = tch->timCtx->DMASource;
+        
+        tch->dmaState = TCH_DMA_READY;
+    }
+
+    return true;
+}
+
+void impl_pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstBaseAddress, uint32_t BurstUnit, uint32_t BurstLength)
+{
+    DMA_SetCurrDataCounter(burstDmaTimer->dmaBurstStream, BurstLength);
+    DMA_Cmd(burstDmaTimer->dmaBurstStream, ENABLE);
+    TIM_DMAConfig(burstDmaTimer->timer, BurstBaseAddress, BurstUnit);
+    TIM_DMACmd(burstDmaTimer->timer, burstDmaTimer->burstRequestSource, ENABLE);
+}
+#endif
+
 void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferElementCount)
 {
     // Make sure we terminate any DMA transaction currently in progress

From db3dad8a24f3e821e681cbb474b6bee6f72d2796 Mon Sep 17 00:00:00 2001
From: Andrey Akimov <shirase55@gmail.com>
Date: Mon, 28 Aug 2023 13:55:45 +0300
Subject: [PATCH 86/94] enable timer dma burst on speedybeef405v3

---
 src/main/target/SPEEDYBEEF405V3/target.h | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/main/target/SPEEDYBEEF405V3/target.h b/src/main/target/SPEEDYBEEF405V3/target.h
index d7bb949b2e6..99711f82deb 100644
--- a/src/main/target/SPEEDYBEEF405V3/target.h
+++ b/src/main/target/SPEEDYBEEF405V3/target.h
@@ -152,7 +152,8 @@
 #define RANGEFINDER_I2C_BUS     BUS_I2C2
 
 #define DEFAULT_FEATURES            (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX)
-// #define USE_DSHOT
+#define USE_DSHOT
+#define USE_DSHOT_DMAR
 // #define USE_ESC_SENSOR
 #define USE_SERIAL_4WAY_BLHELI_INTERFACE
 

From 6b1355e6e8b40e937dd15accf40777c48b9fedb6 Mon Sep 17 00:00:00 2001
From: Andrey Akimov <shirase55@gmail.com>
Date: Mon, 28 Aug 2023 18:30:22 +0300
Subject: [PATCH 87/94] DShot command delay

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

diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c
index 5351c9dfc34..134ac223dd1 100644
--- a/src/main/drivers/pwm_output.c
+++ b/src/main/drivers/pwm_output.c
@@ -61,6 +61,7 @@
 #define DSHOT_DMA_BUFFER_SIZE   18 /* resolution + frame reset (2us) */
 #define MAX_DMA_TIMERS          8
 
+#define DSHOT_COMMAND_DELAY_US 1000
 #define DSHOT_COMMAND_INTERVAL_US 10000
 #define DSHOT_COMMAND_QUEUE_LENGTH 8
 #define DHSOT_COMMAND_QUEUE_SIZE   DSHOT_COMMAND_QUEUE_LENGTH * sizeof(dshotCommands_e)
@@ -434,6 +435,7 @@ static void executeDShotCommands(void){
         return;
         }  
     }
+    delayMicroseconds(DSHOT_COMMAND_DELAY_US);
     for (uint8_t i = 0; i < getMotorCount(); i++) {
          motors[i].requestTelemetry = true;
          motors[i].value = currentExecutingCommand.cmd;

From dc3e9555d221a9b3fe3c6c5b6e9787f490ee0190 Mon Sep 17 00:00:00 2001
From: Jonathan Hudson <jh+github@daria.co.uk>
Date: Mon, 28 Aug 2023 17:12:40 +0100
Subject: [PATCH 88/94] update document iaw current implementation (#9266)

---
 docs/development/serial_printf_debugging.md | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/docs/development/serial_printf_debugging.md b/docs/development/serial_printf_debugging.md
index c9977e98bd3..6cbeec6cfdd 100644
--- a/docs/development/serial_printf_debugging.md
+++ b/docs/development/serial_printf_debugging.md
@@ -76,9 +76,11 @@ If the CLI `log_topics` is non-zero, then all topics matching the mask will be d
 
 A set of macros `LOG_ERROR()` (log error) through `LOG_DEBUG()` (log debug) may be used, subject to compile time log level constraints. These provide `printf` style logging for a given topic.
 
+Note that the `topic` is specified without the `LOG_TOPIC_` prefix:
+
 ```
 //  LOG_DEBUG(topic, fmt, ...)
-LOG_DEBUG(LOG_TOPIC_SYSTEM, "This is %s topic debug message, value %d", "system", 42);
+LOG_DEBUG(SYSTEM, "This is %s topic debug message, value %d", "system", 42);
 ```
 
 It is also possible to dump a hex representation of arbitrary  data, using functions named variously `LOG_BUFFER_` (`ERROR`) and `LOG_BUF_` (anything else, alas) e.g.:
@@ -89,7 +91,7 @@ It is also possible to dump a hex representation of arbitrary  data, using funct
 
 struct {...} tstruct;
 ...
-LOG_BUF_DEBUG(LOG_TOPIC_TEMPERATURE, &tstruct, sizeof(tstruct));
+LOG_BUF_DEBUG(TEMPERATURE, &tstruct, sizeof(tstruct));
 ```
 
 ## Output Support
@@ -104,7 +106,7 @@ Log messages are transmitted through the `FUNCTION_LOG` serial port as MSP messa
 For example, with the final lines of `src/main/fc/fc_init.c` set to:
 
 ```
- LOG_ERROR(LOG_TOPIC_SYSTEM, "Init is complete");
+ LOG_ERROR(SYSTEM, "Init is complete");
  systemState |= SYSTEM_STATE_READY;
 ```
 

From 13b1f94c5ebcf3b2e1aff90b3172f01968209c83 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Tue, 29 Aug 2023 20:05:35 +0100
Subject: [PATCH 89/94] Fix message selection logic

---
 src/main/fc/multifunction.c | 19 ++++++++++++++++---
 src/main/fc/multifunction.h |  2 ++
 src/main/io/osd.c           | 25 ++++++++++++++++---------
 3 files changed, 34 insertions(+), 12 deletions(-)

diff --git a/src/main/fc/multifunction.c b/src/main/fc/multifunction.c
index 19438599b8a..85afdcf4cda 100644
--- a/src/main/fc/multifunction.c
+++ b/src/main/fc/multifunction.c
@@ -34,7 +34,9 @@
 #include "io/osd.h"
 #include "navigation/navigation.h"
 
+multi_function_e selectedItem = MULTI_FUNC_NONE;
 uint8_t multiFunctionFlags;
+bool nextItemIsAvailable = false;
 
 static void multiFunctionApply(multi_function_e selectedItem)
 {
@@ -75,11 +77,20 @@ static void multiFunctionApply(multi_function_e selectedItem)
     }
 }
 
+bool isNextMultifunctionItemAvailable(void)
+{
+    return nextItemIsAvailable;
+}
+
+void incrementMultifunctionSelection(void)
+{
+    selectedItem = selectedItem == MULTI_FUNC_END - 1 ? MULTI_FUNC_1 : selectedItem + 1;
+}
+
 multi_function_e multiFunctionSelection(void)
 {
     static timeMs_t startTimer;
     static timeMs_t selectTimer;
-    static multi_function_e selectedItem = MULTI_FUNC_NONE;
     static bool toggle = true;
     const timeMs_t currentTime = millis();
 
@@ -95,19 +106,21 @@ multi_function_e multiFunctionSelection(void)
                 selectedItem++;
             } else {
                 selectTimer = currentTime;
+                nextItemIsAvailable = true;
             }
         }
         startTimer = currentTime;
         toggle = false;
     } else if (startTimer) {
         if (!toggle && selectTimer) {
-            selectedItem = selectedItem == MULTI_FUNC_END - 1 ? MULTI_FUNC_1 : selectedItem + 1;
+            incrementMultifunctionSelection();
+            nextItemIsAvailable = false;
         }
-        selectTimer = 0;
         if (currentTime - startTimer > 3000) {      // 3s reset delay after mode deselected
             startTimer = 0;
             selectedItem = MULTI_FUNC_NONE;
         }
+        selectTimer = 0;
         toggle = true;
     }
 
diff --git a/src/main/fc/multifunction.h b/src/main/fc/multifunction.h
index dd1163b3123..56c46ebcb4e 100644
--- a/src/main/fc/multifunction.h
+++ b/src/main/fc/multifunction.h
@@ -48,3 +48,5 @@ typedef enum {
 } multi_function_e;
 
 multi_function_e multiFunctionSelection(void);
+bool isNextMultifunctionItemAvailable(void);
+void incrementMultifunctionSelection(void);
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 0a2fe5d9c2a..dcdd3c20e67 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -5056,7 +5056,6 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
     /* --- FUNCTIONS --- */
     multi_function_e selectedFunction = multiFunctionSelection();
     if (selectedFunction) {
-        message = "N/A NEXT >";     // Default message if function unavailable
         switch (selectedFunction) {
         case MULTI_FUNC_NONE:
         case MULTI_FUNC_1:
@@ -5091,7 +5090,16 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
             break;
         }
 
-        strcpy(buff, message);
+        if (message == NULL) {
+            incrementMultifunctionSelection();   // skip to next selection if function unavailable
+        } else {
+            strcpy(buff, message);
+
+            if (isNextMultifunctionItemAvailable()) {
+                // provides feedback indicating when a new selection command has been received by flight controller
+                buff[9] = '>';
+            }
+        }
         return elemAttr;
     }
 
@@ -5109,13 +5117,6 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
         messages[messageCount++] = batteryState == BATTERY_CRITICAL ? "BATT EMPTY" : "BATT LOW !";
     }
 
-    // Vibration levels   TODO - needs better vibration measurement to be useful
-    // const float vibrationLevel = accGetVibrationLevel();
-    // warningCondition = vibrationLevel > 1.5f;
-    // if (osdCheckWarning(warningCondition, warningFlagID <<= 1, &warningsCount)) {
-        // messages[messageCount++] = vibrationLevel > 2.5f ? "BAD VIBRTN" : "VIBRATION!";
-    // }
-
 #if defined(USE_GPS)
     // GPS Fix and Failure
     if (feature(FEATURE_GPS)) {
@@ -5147,6 +5148,12 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
         }
     }
 #endif
+    // Vibration levels   TODO - needs better vibration measurement to be useful
+    // const float vibrationLevel = accGetVibrationLevel();
+    // warningCondition = vibrationLevel > 1.5f;
+    // if (osdCheckWarning(warningCondition, warningFlagID <<= 1, &warningsCount)) {
+        // messages[messageCount++] = vibrationLevel > 2.5f ? "BAD VIBRTN" : "VIBRATION!";
+    // }
 
 #ifdef USE_DEV_TOOLS
     if (osdCheckWarning(systemConfig()->groundTestMode, warningFlagID <<= 1, &warningsCount)) {

From 993c007a04e6aa9001008dc7e9f5e257fd443c62 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Wed, 30 Aug 2023 15:52:58 +0100
Subject: [PATCH 90/94] Improve function selection

---
 src/main/fc/multifunction.c | 11 ++++++-----
 src/main/fc/multifunction.h |  2 +-
 src/main/io/osd.c           | 32 +++++++++++++++++++++-----------
 3 files changed, 28 insertions(+), 17 deletions(-)

diff --git a/src/main/fc/multifunction.c b/src/main/fc/multifunction.c
index 85afdcf4cda..31b0c669358 100644
--- a/src/main/fc/multifunction.c
+++ b/src/main/fc/multifunction.c
@@ -82,9 +82,10 @@ bool isNextMultifunctionItemAvailable(void)
     return nextItemIsAvailable;
 }
 
-void incrementMultifunctionSelection(void)
+void setMultifunctionSelection(multi_function_e item)
 {
-    selectedItem = selectedItem == MULTI_FUNC_END - 1 ? MULTI_FUNC_1 : selectedItem + 1;
+    selectedItem = item == MULTI_FUNC_END ? MULTI_FUNC_1 : item;
+    nextItemIsAvailable = false;
 }
 
 multi_function_e multiFunctionSelection(void)
@@ -100,6 +101,7 @@ multi_function_e multiFunctionSelection(void)
                 multiFunctionApply(selectedItem);
                 selectTimer = 0;
                 selectedItem = MULTI_FUNC_NONE;
+                nextItemIsAvailable = false;
             }
         } else if (toggle) {
             if (selectedItem == MULTI_FUNC_NONE) {
@@ -113,10 +115,9 @@ multi_function_e multiFunctionSelection(void)
         toggle = false;
     } else if (startTimer) {
         if (!toggle && selectTimer) {
-            incrementMultifunctionSelection();
-            nextItemIsAvailable = false;
+            setMultifunctionSelection(++selectedItem);
         }
-        if (currentTime - startTimer > 3000) {      // 3s reset delay after mode deselected
+        if (currentTime - startTimer > 4000) {      // 4s reset delay after mode deselected
             startTimer = 0;
             selectedItem = MULTI_FUNC_NONE;
         }
diff --git a/src/main/fc/multifunction.h b/src/main/fc/multifunction.h
index 56c46ebcb4e..e868e235221 100644
--- a/src/main/fc/multifunction.h
+++ b/src/main/fc/multifunction.h
@@ -49,4 +49,4 @@ typedef enum {
 
 multi_function_e multiFunctionSelection(void);
 bool isNextMultifunctionItemAvailable(void);
-void incrementMultifunctionSelection(void);
+void setMultifunctionSelection(multi_function_e item);
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index dcdd3c20e67..ea2885fd4c7 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -5055,7 +5055,10 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
 
     /* --- FUNCTIONS --- */
     multi_function_e selectedFunction = multiFunctionSelection();
+
     if (selectedFunction) {
+        multi_function_e activeFunction = selectedFunction;
+
         switch (selectedFunction) {
         case MULTI_FUNC_NONE:
         case MULTI_FUNC_1:
@@ -5068,21 +5071,27 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
 #if defined(USE_SAFE_HOME)
             if (navConfig()->general.flags.safehome_usage_mode != SAFEHOME_USAGE_OFF) {
                 message = MULTI_FUNC_FLAG(MF_SUSPEND_SAFEHOMES) ? "USE SFHOME" : "SUS SFHOME";
+                break;
             }
 #endif
-            break;
+            activeFunction++;
+            FALLTHROUGH;
         case MULTI_FUNC_4:
             if (navConfig()->general.flags.rth_trackback_mode != RTH_TRACKBACK_OFF) {
                 message = MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK) ? "USE TKBACK" : "SUS TKBACK";
+                break;
             }
-            break;
+            activeFunction++;
+            FALLTHROUGH;
         case MULTI_FUNC_5:
 #ifdef USE_DSHOT
             if (STATE(MULTIROTOR)) {
                 message = MULTI_FUNC_FLAG(MF_TURTLE_MODE) ? "USE TURTLE" : "END TURTLE";
+                break;
             }
 #endif
-            break;
+            activeFunction++;
+            FALLTHROUGH;
         case MULTI_FUNC_6:
             message = ARMING_FLAG(ARMED) ? "NOW ARMED " : "EMERG ARM ";
             break;
@@ -5090,16 +5099,17 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
             break;
         }
 
-        if (message == NULL) {
-            incrementMultifunctionSelection();   // skip to next selection if function unavailable
-        } else {
-            strcpy(buff, message);
+        if (activeFunction != selectedFunction) {
+            setMultifunctionSelection(activeFunction);
+        }
 
-            if (isNextMultifunctionItemAvailable()) {
-                // provides feedback indicating when a new selection command has been received by flight controller
-                buff[9] = '>';
-            }
+        strcpy(buff, message);
+
+        if (isNextMultifunctionItemAvailable()) {
+            // provides feedback indicating when a new selection command has been received by flight controller
+            buff[9] = '>';
         }
+
         return elemAttr;
     }
 

From 1b79cbd232c04fbaa3f85e51adb16da7d2a7050d Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Wed, 30 Aug 2023 23:29:46 +0100
Subject: [PATCH 91/94] Add define for functions only

---
 src/main/fc/fc_core.c            | 17 ++++++++++-------
 src/main/fc/fc_msp_box.c         |  5 ++++-
 src/main/fc/multifunction.c      |  3 +++
 src/main/fc/multifunction.h      |  3 +++
 src/main/io/osd.c                |  4 +++-
 src/main/navigation/navigation.c | 16 +++++++++++-----
 src/main/target/common.h         |  1 +
 7 files changed, 35 insertions(+), 14 deletions(-)

diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index 8058c418ac3..fc9b57dea67 100755
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -177,7 +177,7 @@ int16_t getAxisRcCommand(int16_t rawData, int16_t rate, int16_t deadband)
 {
     int16_t stickDeflection = 0;
 
-#if defined(SITL_BUILD) // Workaround due to strange bug in GCC > 10.2 https://gcc.gnu.org/bugzilla/show_bug.cgi?id=108914   
+#if defined(SITL_BUILD) // Workaround due to strange bug in GCC > 10.2 https://gcc.gnu.org/bugzilla/show_bug.cgi?id=108914
     const int16_t value = rawData - PWM_RANGE_MIDDLE;
     if (value < -500) {
         stickDeflection = -500;
@@ -187,9 +187,9 @@ int16_t getAxisRcCommand(int16_t rawData, int16_t rate, int16_t deadband)
         stickDeflection = value;
     }
 #else
-    stickDeflection = constrain(rawData - PWM_RANGE_MIDDLE, -500, 500);   
+    stickDeflection = constrain(rawData - PWM_RANGE_MIDDLE, -500, 500);
 #endif
-    
+
     stickDeflection = applyDeadbandRescaled(stickDeflection, deadband, -500, 500);
     return rcLookup(stickDeflection, rate);
 }
@@ -523,9 +523,12 @@ void tryArm(void)
     }
 
 #ifdef USE_DSHOT
-    if (STATE(MULTIROTOR) && (IS_RC_MODE_ACTIVE(BOXTURTLE) || MULTI_FUNC_FLAG(MF_TURTLE_MODE)) && !FLIGHT_MODE(TURTLE_MODE) &&
-        emergencyArmingCanOverrideArmingDisabled() && isMotorProtocolDshot()
-        ) {
+#ifdef USE_MULTI_FUNCTIONS
+    const bool turtleIsActive = IS_RC_MODE_ACTIVE(BOXTURTLE) || MULTI_FUNC_FLAG(MF_TURTLE_MODE);
+#else
+    const bool turtleIsActive = IS_RC_MODE_ACTIVE(BOXTURTLE);
+#endif
+    if (STATE(MULTIROTOR) && turtleIsActive && !FLIGHT_MODE(TURTLE_MODE) && emergencyArmingCanOverrideArmingDisabled() && isMotorProtocolDshot()) {
         sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_REVERSED);
         ENABLE_ARMING_FLAG(ARMED);
         enableFlightMode(TURTLE_MODE);
@@ -842,7 +845,7 @@ static float calculateThrottleTiltCompensationFactor(uint8_t throttleTiltCompens
 
 void taskMainPidLoop(timeUs_t currentTimeUs)
 {
-  
+
     cycleTime = getTaskDeltaTime(TASK_SELF);
     dT = (float)cycleTime * 0.000001f;
 
diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c
index 7cf603b163e..0890b1d6056 100644
--- a/src/main/fc/fc_msp_box.c
+++ b/src/main/fc/fc_msp_box.c
@@ -180,7 +180,9 @@ void initActiveBoxIds(void)
     RESET_BOX_ID_COUNT;
     ADD_ACTIVE_BOX(BOXARM);
     ADD_ACTIVE_BOX(BOXPREARM);
+#ifdef USE_MULTI_FUNCTIONS
     ADD_ACTIVE_BOX(BOXMULTIFUNCTION);
+#endif
 
     if (sensors(SENSOR_ACC) && STATE(ALTITUDE_CONTROL)) {
         ADD_ACTIVE_BOX(BOXANGLE);
@@ -414,8 +416,9 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
 #ifdef USE_MULTI_MISSION
     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCHANGEMISSION)),   BOXCHANGEMISSION);
 #endif
+#ifdef USE_MULTI_FUNCTIONS
     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMULTIFUNCTION)),   BOXMULTIFUNCTION);
-
+#endif
     memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t));
     for (uint32_t i = 0; i < activeBoxIdCount; i++) {
         if (activeBoxes[activeBoxIds[i]]) {
diff --git a/src/main/fc/multifunction.c b/src/main/fc/multifunction.c
index 31b0c669358..c217110842d 100644
--- a/src/main/fc/multifunction.c
+++ b/src/main/fc/multifunction.c
@@ -26,6 +26,8 @@
 #include "build/debug.h"
 #include "drivers/time.h"
 
+#ifdef USE_MULTI_FUNCTIONS
+
 #include "fc/fc_core.h"
 #include "fc/multifunction.h"
 #include "fc/rc_modes.h"
@@ -127,3 +129,4 @@ multi_function_e multiFunctionSelection(void)
 
     return selectedItem;
 }
+#endif
diff --git a/src/main/fc/multifunction.h b/src/main/fc/multifunction.h
index e868e235221..b5c1e1d9d69 100644
--- a/src/main/fc/multifunction.h
+++ b/src/main/fc/multifunction.h
@@ -24,6 +24,8 @@
 
 #pragma once
 
+#ifdef USE_MULTI_FUNCTIONS
+
 extern uint8_t multiFunctionFlags;
 
 #define MULTI_FUNC_FLAG_DISABLE(mask) (multiFunctionFlags &= ~(mask))
@@ -50,3 +52,4 @@ typedef enum {
 multi_function_e multiFunctionSelection(void);
 bool isNextMultifunctionItemAvailable(void);
 void setMultifunctionSelection(multi_function_e item);
+#endif
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index ea2885fd4c7..8983fc5792d 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -5053,6 +5053,7 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
     static uint8_t warningsCount;
     const char *message = NULL;
 
+#ifdef USE_MULTI_FUNCTIONS
     /* --- FUNCTIONS --- */
     multi_function_e selectedFunction = multiFunctionSelection();
 
@@ -5086,7 +5087,7 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
         case MULTI_FUNC_5:
 #ifdef USE_DSHOT
             if (STATE(MULTIROTOR)) {
-                message = MULTI_FUNC_FLAG(MF_TURTLE_MODE) ? "USE TURTLE" : "END TURTLE";
+                message = MULTI_FUNC_FLAG(MF_TURTLE_MODE) ? "END TURTLE" : "USE TURTLE";
                 break;
             }
 #endif
@@ -5112,6 +5113,7 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
 
         return elemAttr;
     }
+#endif  // MULTIFUNCTION - functions only, warnings always defined
 
     /* --- WARNINGS --- */
     const char *messages[7];
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index e1e5217e4b4..17cfe3a5609 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -1305,8 +1305,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_TRACKBACK(navigatio
 
     if (posControl.flags.estPosStatus >= EST_USABLE) {
         const int32_t distFromStartTrackback = calculateDistanceToDestination(&posControl.rthTBPointsList[posControl.rthTBLastSavedIndex]) / 100;
+#ifdef USE_MULTI_FUNCTIONS
+        const bool overrideTrackback = rthAltControlStickOverrideCheck(ROLL) || MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK);
+#else
+        const bool overrideTrackback = rthAltControlStickOverrideCheck(ROLL);
+#endif
         const bool cancelTrackback = distFromStartTrackback > navConfig()->general.rth_trackback_distance ||
-                                     ((rthAltControlStickOverrideCheck(ROLL) || MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK)) && !posControl.flags.forcedRTHActivated);
+                                     (overrideTrackback && !posControl.flags.forcedRTHActivated);
 
         if (posControl.activeRthTBPointIndex < 0 || cancelTrackback) {
             posControl.rthTBWrapAroundCounter = posControl.activeRthTBPointIndex = -1;
@@ -2437,10 +2442,11 @@ static navigationHomeFlags_t navigationActualStateHomeValidity(void)
 #if defined(USE_SAFE_HOME)
 void checkSafeHomeState(bool shouldBeEnabled)
 {
-    const bool safehomeNotApplicable = navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF ||
-                                       (MULTI_FUNC_FLAG(MF_SUSPEND_SAFEHOMES) && !posControl.flags.forcedRTHActivated) ||
-                                       posControl.flags.rthTrackbackActive ||
-                                       (!posControl.safehomeState.isApplied && posControl.homeDistance < navConfig()->general.min_rth_distance);
+    bool safehomeNotApplicable = navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF || posControl.flags.rthTrackbackActive ||
+                                 (!posControl.safehomeState.isApplied && posControl.homeDistance < navConfig()->general.min_rth_distance);
+#ifdef USE_MULTI_FUNCTIONS
+    safehomeNotApplicable = safehomeNotApplicable || (MULTI_FUNC_FLAG(MF_SUSPEND_SAFEHOMES) && !posControl.flags.forcedRTHActivated);
+#endif
 
 	if (safehomeNotApplicable) {
 		shouldBeEnabled = false;
diff --git a/src/main/target/common.h b/src/main/target/common.h
index d18bdfb1a2e..457e9d47807 100644
--- a/src/main/target/common.h
+++ b/src/main/target/common.h
@@ -161,6 +161,7 @@
 #define NAV_MAX_WAYPOINTS       120
 #define USE_RCDEVICE
 #define USE_MULTI_MISSION
+#define USE_MULTI_FUNCTIONS  // defines functions only, warnings always defined
 
 //Enable VTX control
 #define USE_VTX_CONTROL

From b1b43e3827cb1dfd8417d3eabac5762fc2c0ef12 Mon Sep 17 00:00:00 2001
From: Andrey Akimov <shirase55@gmail.com>
Date: Thu, 31 Aug 2023 14:12:21 +0300
Subject: [PATCH 92/94] Timer DMA burst, add HAL implementation

---
 src/main/drivers/pwm_output.c           |   2 +-
 src/main/drivers/timer.c                |   4 +-
 src/main/drivers/timer.h                |   7 +-
 src/main/drivers/timer_impl.h           |   2 +-
 src/main/drivers/timer_impl_hal.c       | 103 ++++++++++++++++++++++++
 src/main/drivers/timer_impl_stdperiph.c |   4 +-
 6 files changed, 115 insertions(+), 7 deletions(-)

diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c
index 134ac223dd1..bf38ec93404 100644
--- a/src/main/drivers/pwm_output.c
+++ b/src/main/drivers/pwm_output.c
@@ -477,7 +477,7 @@ void pwmCompleteMotorUpdate(void) {
 
         for (int burstDmaTimerIndex = 0; burstDmaTimerIndex < burstDmaTimersCount; burstDmaTimerIndex++) {
             burstDmaTimer_t *burstDmaTimer = &burstDmaTimers[burstDmaTimerIndex];
-            pwmBurstDMAStart(burstDmaTimer, TIM_DMABase_CCR1, TIM_DMABurstLength_4Transfers, DSHOT_DMA_BUFFER_SIZE * 4);
+            pwmBurstDMAStart(burstDmaTimer, DSHOT_DMA_BUFFER_SIZE * 4);
         }
 #else
         // Generate DMA buffers
diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c
index 79b60e975b9..664bae3ca59 100644
--- a/src/main/drivers/timer.c
+++ b/src/main/drivers/timer.c
@@ -294,8 +294,8 @@ bool timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, void *
     return impl_timerPWMConfigDMABurst(burstDmaTimer, tch, dmaBuffer, dmaBufferElementSize, dmaBufferElementCount);
 }
 
-void pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstBaseAddress, uint32_t BurstUnit, uint32_t BurstLength)
+void pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstLength)
 {
-    impl_pwmBurstDMAStart(burstDmaTimer, BurstBaseAddress, BurstUnit, BurstLength);
+    impl_pwmBurstDMAStart(burstDmaTimer, BurstLength);
 }
 #endif
diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h
index 7c2c3262e07..b4b3ed8da03 100644
--- a/src/main/drivers/timer.h
+++ b/src/main/drivers/timer.h
@@ -175,7 +175,12 @@ extern const int timerHardwareCount;
 #ifdef USE_DSHOT_DMAR
 typedef struct {
     TIM_TypeDef *timer;
+#ifdef USE_HAL_DRIVER
+    DMA_TypeDef *dma;
+    uint32_t streamLL;
+#else
     DMA_Stream_TypeDef *dmaBurstStream;
+#endif
     timerDMASafeType_t *dmaBurstBuffer;
     uint16_t burstRequestSource;
 } burstDmaTimer_t;
@@ -245,5 +250,5 @@ volatile timCCR_t *timerCCR(TCH_t * tch);
 
 #ifdef USE_DSHOT_DMAR
 bool timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount);
-void pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstBaseAddress, uint32_t BurstUnit, uint32_t BurstLength);
+void pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstLength);
 #endif
\ No newline at end of file
diff --git a/src/main/drivers/timer_impl.h b/src/main/drivers/timer_impl.h
index e869b986202..4d0a87f9aa5 100644
--- a/src/main/drivers/timer_impl.h
+++ b/src/main/drivers/timer_impl.h
@@ -87,5 +87,5 @@ void impl_timerPWMStopDMA(TCH_t * tch);
 
 #ifdef USE_DSHOT_DMAR
 bool impl_timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount);
-void impl_pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstBaseAddress, uint32_t BurstUnit, uint32_t BurstLength);
+void impl_pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstLength);
 #endif
\ No newline at end of file
diff --git a/src/main/drivers/timer_impl_hal.c b/src/main/drivers/timer_impl_hal.c
index 0649513f210..e800a942353 100644
--- a/src/main/drivers/timer_impl_hal.c
+++ b/src/main/drivers/timer_impl_hal.c
@@ -408,6 +408,109 @@ bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBuf
     return true;
 }
 
+#ifdef USE_DSHOT_DMAR
+bool impl_timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount)
+{
+    tch->dma = dmaGetByTag(tch->timHw->dmaTag);
+    tch->dmaBuffer = dmaBuffer;
+    if (tch->dma == NULL) {
+        return false;
+    }
+
+    // If DMA is already in use - abort
+    if (dmaGetOwner(tch->dma) != OWNER_FREE) {
+        return false;
+    }
+
+    if (!tch->timCtx->dmaBurstRef) {
+        // We assume that timer channels are already initialized by calls to:
+        //  timerConfigBase
+        //  timerPWMConfigChannel
+        const uint32_t streamLL = lookupDMALLStreamTable[DMATAG_GET_STREAM(tch->timHw->dmaTag)];
+
+        LL_DMA_DeInit(tch->dma->dma, streamLL);
+
+        LL_DMA_InitTypeDef init;
+        LL_DMA_StructInit(&init);
+
+#if defined(STM32H7) || defined(STM32G4)
+        // For H7 the DMA periphRequest is encoded in the DMA tag
+        init.PeriphRequest = DMATAG_GET_CHANNEL(tch->timHw->dmaTag);
+#else
+        init.Channel = lookupDMALLChannelTable[DMATAG_GET_CHANNEL(tch->timHw->dmaTag)];
+#endif
+
+        init.PeriphOrM2MSrcAddress = (uint32_t)&tch->timHw->tim->DMAR;
+        init.PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT;
+
+        switch (dmaBufferElementSize) {
+            case 1:
+                init.MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_BYTE;
+                init.PeriphOrM2MSrcDataSize = LL_DMA_MDATAALIGN_BYTE;
+                break;
+            case 2:
+                init.MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_HALFWORD;
+                init.PeriphOrM2MSrcDataSize = LL_DMA_MDATAALIGN_HALFWORD;
+                break;
+            case 4:
+                init.MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_WORD;
+                init.PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_WORD;
+                break;
+            default:
+                // Programmer error
+                while(1) {
+
+                }
+        }
+
+        init.MemoryOrM2MDstAddress = (uint32_t)dmaBuffer;
+        init.MemoryOrM2MDstIncMode = LL_DMA_MEMORY_INCREMENT;
+        init.NbData = dmaBufferElementCount;
+        init.Direction = LL_DMA_DIRECTION_MEMORY_TO_PERIPH;
+        init.Mode = LL_DMA_MODE_NORMAL;
+        init.Priority = LL_DMA_PRIORITY_HIGH;
+        init.FIFOMode = LL_DMA_FIFOMODE_ENABLE;
+        init.FIFOThreshold = LL_DMA_FIFOTHRESHOLD_FULL;
+        init.MemBurst = LL_DMA_MBURST_SINGLE;
+        init.PeriphBurst = LL_DMA_PBURST_SINGLE;
+
+        dmaInit(tch->dma, OWNER_TIMER, 0);
+        dmaSetHandler(tch->dma, impl_timerDMA_IRQHandler, NVIC_PRIO_TIMER_DMA, (uint32_t)tch);
+
+        LL_DMA_Init(tch->dma->dma, streamLL, &init);
+
+        tch->timCtx->dmaBurstRef = tch->dma;
+        burstDmaTimer->burstRequestSource = lookupDMASourceTable[tch->timHw->channelIndex];
+        burstDmaTimer->streamLL = lookupDMALLStreamTable[DMATAG_GET_STREAM(tch->timHw->dmaTag)];
+        burstDmaTimer->dma = tch->dma->dma;
+
+        tch->dmaState = TCH_DMA_READY;
+    }
+
+    // Start PWM generation
+    if (tch->timHw->output & TIMER_OUTPUT_N_CHANNEL) {
+        HAL_TIMEx_PWMN_Start(tch->timCtx->timHandle, lookupTIMChannelTable[tch->timHw->channelIndex]);
+    }
+    else {
+        HAL_TIM_PWM_Start(tch->timCtx->timHandle, lookupTIMChannelTable[tch->timHw->channelIndex]);
+    }
+
+    return true;
+}
+
+void impl_pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstLength)
+{
+    LL_DMA_SetDataLength(burstDmaTimer->dma, burstDmaTimer->streamLL, BurstLength);
+    LL_DMA_EnableIT_TC(burstDmaTimer->dma, burstDmaTimer->streamLL);
+    LL_DMA_EnableStream(burstDmaTimer->dma, burstDmaTimer->streamLL);
+    /* configure the DMA Burst Mode */
+    LL_TIM_ConfigDMABurst(burstDmaTimer->timer, LL_TIM_DMABURST_BASEADDR_CCR1, LL_TIM_DMABURST_LENGTH_4TRANSFERS);
+    /* Enable the TIM DMA Request */
+    //LL_TIM_EnableDMAReq_UPDATE(burstDmaTimer->timer);
+    LL_TIM_EnableDMAReq_CCx(burstDmaTimer->timer, burstDmaTimer->burstRequestSource);
+}
+#endif
+
 void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferElementCount)
 {
     const uint32_t streamLL = lookupDMALLStreamTable[DMATAG_GET_STREAM(tch->timHw->dmaTag)];
diff --git a/src/main/drivers/timer_impl_stdperiph.c b/src/main/drivers/timer_impl_stdperiph.c
index 17039c6822a..5cb3457c77b 100644
--- a/src/main/drivers/timer_impl_stdperiph.c
+++ b/src/main/drivers/timer_impl_stdperiph.c
@@ -446,11 +446,11 @@ bool impl_timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, vo
     return true;
 }
 
-void impl_pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstBaseAddress, uint32_t BurstUnit, uint32_t BurstLength)
+void impl_pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstLength)
 {
     DMA_SetCurrDataCounter(burstDmaTimer->dmaBurstStream, BurstLength);
     DMA_Cmd(burstDmaTimer->dmaBurstStream, ENABLE);
-    TIM_DMAConfig(burstDmaTimer->timer, BurstBaseAddress, BurstUnit);
+    TIM_DMAConfig(burstDmaTimer->timer, TIM_DMABase_CCR1, TIM_DMABurstLength_4Transfers);
     TIM_DMACmd(burstDmaTimer->timer, burstDmaTimer->burstRequestSource, ENABLE);
 }
 #endif

From ad678dcd30ced6576850d18597c5a7a631b1fa81 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Fri, 1 Sep 2023 19:27:12 +0200
Subject: [PATCH 93/94] some strange stuff

---
 src/main/target/GEPRCF405/CMakeLists.txt |   2 +-
 src/main/target/GEPRCF405/target.c       |  92 +++---
 src/main/target/GEPRCF405/target.h       | 354 +++++++++++------------
 src/main/target/GEPRCF722/CMakeLists.txt |   2 +-
 src/main/target/GEPRCF722/target.c       |  88 +++---
 src/main/target/GEPRCF722/target.h       | 346 +++++++++++-----------
 6 files changed, 442 insertions(+), 442 deletions(-)

diff --git a/src/main/target/GEPRCF405/CMakeLists.txt b/src/main/target/GEPRCF405/CMakeLists.txt
index 2e3b1dec27f..d500b48baa1 100644
--- a/src/main/target/GEPRCF405/CMakeLists.txt
+++ b/src/main/target/GEPRCF405/CMakeLists.txt
@@ -1 +1 @@
-target_stm32f405xg(GEPRCF405)
+target_stm32f405xg(GEPRCF405)
diff --git a/src/main/target/GEPRCF405/target.c b/src/main/target/GEPRCF405/target.c
index 38eee3688d0..ace9b307366 100644
--- a/src/main/target/GEPRCF405/target.c
+++ b/src/main/target/GEPRCF405/target.c
@@ -1,46 +1,46 @@
-/*
-* This file is part of INAV Project.
-*
-* Cleanflight is free software: you can redistribute it 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.
-*
-* Cleanflight 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
-*/
-
-#include <stdint.h>
-
-#include "platform.h"
-#include "drivers/io.h"
-
-#include "drivers/dma.h"
-#include "drivers/timer.h"
-#include "drivers/timer_def.h"
-
-
-
-
-
-timerHardware_t timerHardware[] = {
-    DEF_TIM(TIM8,  CH2,  PC7, TIM_USE_PPM,                        0, 0),
-
-    DEF_TIM(TIM1, CH1, PA8,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1),
-    DEF_TIM(TIM1, CH2, PA9,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1),
-    DEF_TIM(TIM1, CH3, PA10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1),
-    DEF_TIM(TIM8, CH3, PC8,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1),
-    DEF_TIM(TIM8, CH4, PC9,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
-    DEF_TIM(TIM3, CH3, PB0,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
-
-    DEF_TIM(TIM3, CH4, PB1, TIM_USE_LED,                         0, 0),
-};
-
-const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
-
-
+/*
+* This file is part of INAV Project.
+*
+* Cleanflight is free software: you can redistribute it 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.
+*
+* Cleanflight 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include <stdint.h>
+
+#include "platform.h"
+#include "drivers/io.h"
+
+#include "drivers/dma.h"
+#include "drivers/timer.h"
+#include "drivers/timer_def.h"
+
+
+
+
+
+timerHardware_t timerHardware[] = {
+    DEF_TIM(TIM8,  CH2,  PC7, TIM_USE_PPM,                        0, 0),
+
+    DEF_TIM(TIM1, CH1, PA8,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1),
+    DEF_TIM(TIM1, CH2, PA9,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1),
+    DEF_TIM(TIM1, CH3, PA10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1),
+    DEF_TIM(TIM8, CH3, PC8,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1),
+    DEF_TIM(TIM8, CH4, PC9,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
+    DEF_TIM(TIM3, CH3, PB0,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
+
+    DEF_TIM(TIM3, CH4, PB1, TIM_USE_LED,                         0, 0),
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
+
+
diff --git a/src/main/target/GEPRCF405/target.h b/src/main/target/GEPRCF405/target.h
index 5df9af996eb..3a628ffe596 100644
--- a/src/main/target/GEPRCF405/target.h
+++ b/src/main/target/GEPRCF405/target.h
@@ -1,177 +1,177 @@
-/*
- * This file is part of INAV Project.
- *
- * Cleanflight is free software: you can redistribute it 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.
- *
- * Cleanflight 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#pragma once
-
-#define TARGET_BOARD_IDENTIFIER "GEPR"
-
-#define USBD_PRODUCT_STRING     "GEPRCF405"
-
-#define LED0                    PC14
-#define LED1                    PC15
-
-
-
-#define BEEPER                  PC13
-#define BEEPER_INVERTED
-
-// *************** Gyro & ACC **********************
-
-
-
-#define USE_SPI
-#define USE_SPI_DEVICE_3
-
-#define SPI3_SCK_PIN            PB3
-#define SPI3_MISO_PIN           PB4
-#define SPI3_MOSI_PIN           PB5
-
-
-#define USE_IMU_MPU6000
-#define IMU_MPU6000_ALIGN       CW180_DEG
-#define MPU6000_CS_PIN          PA15
-#define MPU6000_SPI_BUS         BUS_SPI3
-
-#define USE_IMU_BMI270
-#define IMU_BMI270_ALIGN        CW180_DEG
-#define BMI270_CS_PIN           PA15
-#define BMI270_SPI_BUS          BUS_SPI3
-
-#define USE_IMU_ICM42605
-#define IMU_ICM42605_ALIGN      CW180_DEG
-#define ICM42605_CS_PIN         PA15
-#define ICM42605_SPI_BUS        BUS_SPI3
-
-// *************** I2C/Baro/Mag *********************
-#define USE_I2C
-#define USE_I2C_DEVICE_1
-#define I2C1_SCL                PB8
-#define I2C1_SDA                PB9
-
-#define USE_BARO
-#define BARO_I2C_BUS            BUS_I2C1
-#define USE_BARO_BMP280
-#define USE_BARO_DPS310
-#define USE_BARO_MS5611
-
-#define USE_MAG
-#define MAG_I2C_BUS             BUS_I2C1
-#define USE_MAG_HMC5883
-#define USE_MAG_QMC5883
-#define USE_MAG_IST8310
-#define USE_MAG_IST8308
-#define USE_MAG_MAG3110
-#define USE_MAG_LIS3MDL
-
-#define TEMPERATURE_I2C_BUS     BUS_I2C1
-
-#define PITOT_I2C_BUS           BUS_I2C1
-
-#define USE_RANGEFINDER
-#define RANGEFINDER_I2C_BUS     BUS_I2C1
-#define BNO055_I2C_BUS          BUS_I2C1
-
-// *************** FLASH **************************
-#define M25P16_CS_PIN           PB12
-#define M25P16_SPI_BUS          BUS_SPI2
-#define USE_FLASHFS
-#define USE_FLASH_M25P16
-#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
-
-#define USE_SPI_DEVICE_2
-#define SPI2_SCK_PIN            PB13
-#define SPI2_MISO_PIN   	    PB14
-#define SPI2_MOSI_PIN   	    PB15
-
-// *************** OSD *****************************
-#define USE_SPI_DEVICE_1
-#define SPI1_SCK_PIN            PA5
-#define SPI1_MISO_PIN           PA6
-#define SPI1_MOSI_PIN           PA7
-
-#define USE_MAX7456
-#define MAX7456_SPI_BUS         BUS_SPI1
-#define MAX7456_CS_PIN          PA4
-
-// *************** UART *****************************
-#define USE_VCP
-
-#define USE_UART1
-#define UART1_RX_PIN            PB7
-#define UART1_TX_PIN            PB6
-
-#define USE_UART2
-#define UART2_RX_PIN            PA3
-#define UART2_TX_PIN            PA2
-
-#define USE_UART3
-#define UART3_RX_PIN            PC11
-#define UART3_TX_PIN            PC10
-
-#define USE_UART4
-#define UART4_RX_PIN            PA1
-#define UART4_TX_PIN            PA0
-
-#define USE_UART5
-#define UART5_RX_PIN            PD2
-#define UART5_TX_PIN            PC12
-
-#define USE_UART6
-#define UART6_RX_PIN            PC7
-#define UART6_TX_PIN            PC6
-
-#define SERIAL_PORT_COUNT       7
-
-#define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
-#define SERIALRX_PROVIDER       SERIALRX_SBUS
-#define SERIALRX_UART           SERIAL_PORT_USART2
-
-// *************** ADC *****************************
-#define USE_ADC
-#define ADC_INSTANCE                ADC1
-#define ADC1_DMA_STREAM             DMA2_Stream0
-#define ADC_CHANNEL_1_PIN           PC1
-#define ADC_CHANNEL_2_PIN           PC2
-#define ADC_CHANNEL_3_PIN           PC0
-
-#define VBAT_ADC_CHANNEL            ADC_CHN_1
-#define RSSI_ADC_CHANNEL            ADC_CHN_2
-#define CURRENT_METER_ADC_CHANNEL   ADC_CHN_3
-
-
-#define DEFAULT_FEATURES            (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX)
-
-
-
-// *************** LEDSTRIP ************************
-#define USE_LED_STRIP
-#define WS2811_PIN                  PB1
-
-
-
-#define USE_SERIAL_4WAY_BLHELI_INTERFACE
-
-#define TARGET_IO_PORTA         0xffff
-#define TARGET_IO_PORTB         0xffff
-#define TARGET_IO_PORTC         0xffff
-#define TARGET_IO_PORTD         (BIT(2))
-
-
-#define MAX_PWM_OUTPUT_PORTS        6
-
-#define USE_DSHOT
-#define USE_ESC_SENSOR
+/*
+ * This file is part of INAV Project.
+ *
+ * Cleanflight is free software: you can redistribute it 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.
+ *
+ * Cleanflight 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#define TARGET_BOARD_IDENTIFIER "GEPR"
+
+#define USBD_PRODUCT_STRING     "GEPRCF405"
+
+#define LED0                    PC14
+#define LED1                    PC15
+
+
+
+#define BEEPER                  PC13
+#define BEEPER_INVERTED
+
+// *************** Gyro & ACC **********************
+
+
+
+#define USE_SPI
+#define USE_SPI_DEVICE_3
+
+#define SPI3_SCK_PIN            PB3
+#define SPI3_MISO_PIN           PB4
+#define SPI3_MOSI_PIN           PB5
+
+
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW180_DEG
+#define MPU6000_CS_PIN          PA15
+#define MPU6000_SPI_BUS         BUS_SPI3
+
+#define USE_IMU_BMI270
+#define IMU_BMI270_ALIGN        CW180_DEG
+#define BMI270_CS_PIN           PA15
+#define BMI270_SPI_BUS          BUS_SPI3
+
+#define USE_IMU_ICM42605
+#define IMU_ICM42605_ALIGN      CW180_DEG
+#define ICM42605_CS_PIN         PA15
+#define ICM42605_SPI_BUS        BUS_SPI3
+
+// *************** I2C/Baro/Mag *********************
+#define USE_I2C
+#define USE_I2C_DEVICE_1
+#define I2C1_SCL                PB8
+#define I2C1_SDA                PB9
+
+#define USE_BARO
+#define BARO_I2C_BUS            BUS_I2C1
+#define USE_BARO_BMP280
+#define USE_BARO_DPS310
+#define USE_BARO_MS5611
+
+#define USE_MAG
+#define MAG_I2C_BUS             BUS_I2C1
+#define USE_MAG_HMC5883
+#define USE_MAG_QMC5883
+#define USE_MAG_IST8310
+#define USE_MAG_IST8308
+#define USE_MAG_MAG3110
+#define USE_MAG_LIS3MDL
+
+#define TEMPERATURE_I2C_BUS     BUS_I2C1
+
+#define PITOT_I2C_BUS           BUS_I2C1
+
+#define USE_RANGEFINDER
+#define RANGEFINDER_I2C_BUS     BUS_I2C1
+#define BNO055_I2C_BUS          BUS_I2C1
+
+// *************** FLASH **************************
+#define M25P16_CS_PIN           PB12
+#define M25P16_SPI_BUS          BUS_SPI2
+#define USE_FLASHFS
+#define USE_FLASH_M25P16
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+
+#define USE_SPI_DEVICE_2
+#define SPI2_SCK_PIN            PB13
+#define SPI2_MISO_PIN   	    PB14
+#define SPI2_MOSI_PIN   	    PB15
+
+// *************** OSD *****************************
+#define USE_SPI_DEVICE_1
+#define SPI1_SCK_PIN            PA5
+#define SPI1_MISO_PIN           PA6
+#define SPI1_MOSI_PIN           PA7
+
+#define USE_MAX7456
+#define MAX7456_SPI_BUS         BUS_SPI1
+#define MAX7456_CS_PIN          PA4
+
+// *************** UART *****************************
+#define USE_VCP
+
+#define USE_UART1
+#define UART1_RX_PIN            PB7
+#define UART1_TX_PIN            PB6
+
+#define USE_UART2
+#define UART2_RX_PIN            PA3
+#define UART2_TX_PIN            PA2
+
+#define USE_UART3
+#define UART3_RX_PIN            PC11
+#define UART3_TX_PIN            PC10
+
+#define USE_UART4
+#define UART4_RX_PIN            PA1
+#define UART4_TX_PIN            PA0
+
+#define USE_UART5
+#define UART5_RX_PIN            PD2
+#define UART5_TX_PIN            PC12
+
+#define USE_UART6
+#define UART6_RX_PIN            PC7
+#define UART6_TX_PIN            PC6
+
+#define SERIAL_PORT_COUNT       7
+
+#define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER       SERIALRX_SBUS
+#define SERIALRX_UART           SERIAL_PORT_USART2
+
+// *************** ADC *****************************
+#define USE_ADC
+#define ADC_INSTANCE                ADC1
+#define ADC1_DMA_STREAM             DMA2_Stream0
+#define ADC_CHANNEL_1_PIN           PC1
+#define ADC_CHANNEL_2_PIN           PC2
+#define ADC_CHANNEL_3_PIN           PC0
+
+#define VBAT_ADC_CHANNEL            ADC_CHN_1
+#define RSSI_ADC_CHANNEL            ADC_CHN_2
+#define CURRENT_METER_ADC_CHANNEL   ADC_CHN_3
+
+
+#define DEFAULT_FEATURES            (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX)
+
+
+
+// *************** LEDSTRIP ************************
+#define USE_LED_STRIP
+#define WS2811_PIN                  PB1
+
+
+
+#define USE_SERIAL_4WAY_BLHELI_INTERFACE
+
+#define TARGET_IO_PORTA         0xffff
+#define TARGET_IO_PORTB         0xffff
+#define TARGET_IO_PORTC         0xffff
+#define TARGET_IO_PORTD         (BIT(2))
+
+
+#define MAX_PWM_OUTPUT_PORTS        6
+
+#define USE_DSHOT
+#define USE_ESC_SENSOR
diff --git a/src/main/target/GEPRCF722/CMakeLists.txt b/src/main/target/GEPRCF722/CMakeLists.txt
index 2d7f6c7d95f..8164cb86230 100644
--- a/src/main/target/GEPRCF722/CMakeLists.txt
+++ b/src/main/target/GEPRCF722/CMakeLists.txt
@@ -1 +1 @@
-target_stm32f722xe(GEPRCF722)
+target_stm32f722xe(GEPRCF722)
diff --git a/src/main/target/GEPRCF722/target.c b/src/main/target/GEPRCF722/target.c
index 3514ca283ff..8e206e7b199 100644
--- a/src/main/target/GEPRCF722/target.c
+++ b/src/main/target/GEPRCF722/target.c
@@ -1,44 +1,44 @@
-/*
-* This file is part of INAV Project.
-*
-* Cleanflight is free software: you can redistribute it 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.
-*
-* Cleanflight 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
-*/
-
-#include <stdint.h>
-
-#include "platform.h"
-
-#include "drivers/bus.h"
-#include "drivers/io.h"
-#include "drivers/pwm_mapping.h"
-#include "drivers/timer.h"
-#include "drivers/sensor.h"
-
-
-timerHardware_t timerHardware[] = {
-    DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM,                         0, 0),
-
-    DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
-    DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
-    DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
-    DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
-    DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
-    DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
-
-    DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED,                         0, 0),
-
-    
-};
-
-const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
+/*
+* This file is part of INAV Project.
+*
+* Cleanflight is free software: you can redistribute it 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.
+*
+* Cleanflight 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include <stdint.h>
+
+#include "platform.h"
+
+#include "drivers/bus.h"
+#include "drivers/io.h"
+#include "drivers/pwm_mapping.h"
+#include "drivers/timer.h"
+#include "drivers/sensor.h"
+
+
+timerHardware_t timerHardware[] = {
+    DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM,                         0, 0),
+
+    DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
+    DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
+    DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
+    DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
+    DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
+    DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),
+
+    DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED,                         0, 0),
+
+    
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
diff --git a/src/main/target/GEPRCF722/target.h b/src/main/target/GEPRCF722/target.h
index e4f3b9c72da..bc08477c451 100644
--- a/src/main/target/GEPRCF722/target.h
+++ b/src/main/target/GEPRCF722/target.h
@@ -1,173 +1,173 @@
-/*
- * This file is part of INAV Project.
- *
- * Cleanflight is free software: you can redistribute it 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.
- *
- * Cleanflight 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#pragma once
-
-#define TARGET_BOARD_IDENTIFIER "GEPR"
-
-#define USBD_PRODUCT_STRING     "GEPRCF722"
-
-#define LED0                    PA13
-#define LED1                    PA14
-
-#define BEEPER                  PC13
-#define BEEPER_INVERTED
-
-// *************** Gyro & ACC **********************
-
-#define USE_SPI
-#define USE_SPI_DEVICE_3
-
-#define SPI3_SCK_PIN            PC10
-#define SPI3_MISO_PIN           PC11
-#define SPI3_MOSI_PIN           PC12
-
-
-#define USE_IMU_MPU6000
-#define IMU_MPU6000_ALIGN       CW270_DEG
-
-#define MPU6000_CS_PIN          PA15
-#define MPU6000_SPI_BUS         BUS_SPI3
-
-#define USE_IMU_BMI270
-#define IMU_BMI270_ALIGN        CW180_DEG
-#define BMI270_CS_PIN           PA15
-#define BMI270_SPI_BUS          BUS_SPI3
-
-#define USE_IMU_ICM42605
-#define IMU_ICM42605_ALIGN      CW180_DEG
-#define ICM42605_CS_PIN         PA15
-#define ICM42605_SPI_BUS        BUS_SPI3
-
-// *************** I2C/Baro/Mag *********************
-#define USE_I2C
-#define USE_I2C_DEVICE_1
-#define I2C1_SCL                PB8
-#define I2C1_SDA                PB9
-
-#define USE_BARO
-#define BARO_I2C_BUS            BUS_I2C1
-#define USE_BARO_BMP280
-#define USE_BARO_DPS310
-#define USE_BARO_MS5611
-
-#define USE_MAG
-#define MAG_I2C_BUS             BUS_I2C1
-#define USE_MAG_HMC5883
-#define USE_MAG_QMC5883
-#define USE_MAG_IST8310
-#define USE_MAG_IST8308
-#define USE_MAG_MAG3110
-#define USE_MAG_LIS3MDL
-
-#define TEMPERATURE_I2C_BUS     BUS_I2C1
-
-#define PITOT_I2C_BUS           BUS_I2C1
-
-#define USE_RANGEFINDER
-#define RANGEFINDER_I2C_BUS     BUS_I2C1
-#define BNO055_I2C_BUS          BUS_I2C1
-
-// *************** FLASH **************************
-#define M25P16_CS_PIN           PB12
-#define M25P16_SPI_BUS          BUS_SPI2
-#define USE_FLASHFS
-#define USE_FLASH_M25P16
-#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
-
-#define USE_SPI_DEVICE_2
-#define SPI2_SCK_PIN            PB13
-#define SPI2_MISO_PIN   	    PB14
-#define SPI2_MOSI_PIN   	    PB15
-
-// *************** OSD *****************************
-#define USE_SPI_DEVICE_1
-#define SPI1_SCK_PIN            PA5
-#define SPI1_MISO_PIN           PA6
-#define SPI1_MOSI_PIN           PA7
-
-#define USE_MAX7456
-#define MAX7456_SPI_BUS         BUS_SPI1
-#define MAX7456_CS_PIN          PB2
-// *************** UART *****************************
-#define USE_VCP
-
-#define USE_UART1
-#define UART1_RX_PIN            PA10
-#define UART1_TX_PIN            PA9
-
-#define USE_UART2
-#define UART2_RX_PIN            PA3
-#define UART2_TX_PIN            PA2
-
-#define USE_UART3
-#define UART3_RX_PIN            PB11
-#define UART3_TX_PIN            PB10
-
-#define USE_UART4
-#define UART4_RX_PIN            PA1
-#define UART4_TX_PIN            PA0
-
-#define USE_UART6
-#define UART6_RX_PIN            PC7
-#define UART6_TX_PIN            PC6
-
-#define SERIAL_PORT_COUNT       6
-
-#define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
-#define SERIALRX_PROVIDER       SERIALRX_SBUS
-#define SERIALRX_UART           SERIAL_PORT_USART2
-
-// *************** ADC *****************************
-#define USE_ADC
-#define ADC_INSTANCE                ADC1
-#define ADC1_DMA_STREAM             DMA2_Stream0
-#define ADC_CHANNEL_1_PIN           PC0
-#define ADC_CHANNEL_2_PIN           PC2
-#define ADC_CHANNEL_3_PIN           PC1
-
-#define VBAT_ADC_CHANNEL            ADC_CHN_1
-#define RSSI_ADC_CHANNEL            ADC_CHN_2
-#define CURRENT_METER_ADC_CHANNEL   ADC_CHN_3
-
-
-#define DEFAULT_FEATURES            (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX)
-
-
-// ************PINIO to other
-#define PINIO1_PIN                  PC8                //Enable vsw
-#define PINIO2_PIN                  PC9
-
-
-
-// *************** LEDSTRIP ************************
-#define USE_LED_STRIP
-#define WS2811_PIN                  PA8
-
-
-
-#define USE_SERIAL_4WAY_BLHELI_INTERFACE
-
-#define TARGET_IO_PORTA             0xffff
-#define TARGET_IO_PORTB             0xffff
-#define TARGET_IO_PORTC             0xffff
-#define TARGET_IO_PORTD             (BIT(2))
-
-#define MAX_PWM_OUTPUT_PORTS        6
-
-#define USE_DSHOT
-#define USE_ESC_SENSOR
+/*
+ * This file is part of INAV Project.
+ *
+ * Cleanflight is free software: you can redistribute it 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.
+ *
+ * Cleanflight 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#define TARGET_BOARD_IDENTIFIER "GEPR"
+
+#define USBD_PRODUCT_STRING     "GEPRCF722"
+
+#define LED0                    PA13
+#define LED1                    PA14
+
+#define BEEPER                  PC13
+#define BEEPER_INVERTED
+
+// *************** Gyro & ACC **********************
+
+#define USE_SPI
+#define USE_SPI_DEVICE_3
+
+#define SPI3_SCK_PIN            PC10
+#define SPI3_MISO_PIN           PC11
+#define SPI3_MOSI_PIN           PC12
+
+
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW270_DEG
+
+#define MPU6000_CS_PIN          PA15
+#define MPU6000_SPI_BUS         BUS_SPI3
+
+#define USE_IMU_BMI270
+#define IMU_BMI270_ALIGN        CW180_DEG
+#define BMI270_CS_PIN           PA15
+#define BMI270_SPI_BUS          BUS_SPI3
+
+#define USE_IMU_ICM42605
+#define IMU_ICM42605_ALIGN      CW180_DEG
+#define ICM42605_CS_PIN         PA15
+#define ICM42605_SPI_BUS        BUS_SPI3
+
+// *************** I2C/Baro/Mag *********************
+#define USE_I2C
+#define USE_I2C_DEVICE_1
+#define I2C1_SCL                PB8
+#define I2C1_SDA                PB9
+
+#define USE_BARO
+#define BARO_I2C_BUS            BUS_I2C1
+#define USE_BARO_BMP280
+#define USE_BARO_DPS310
+#define USE_BARO_MS5611
+
+#define USE_MAG
+#define MAG_I2C_BUS             BUS_I2C1
+#define USE_MAG_HMC5883
+#define USE_MAG_QMC5883
+#define USE_MAG_IST8310
+#define USE_MAG_IST8308
+#define USE_MAG_MAG3110
+#define USE_MAG_LIS3MDL
+
+#define TEMPERATURE_I2C_BUS     BUS_I2C1
+
+#define PITOT_I2C_BUS           BUS_I2C1
+
+#define USE_RANGEFINDER
+#define RANGEFINDER_I2C_BUS     BUS_I2C1
+#define BNO055_I2C_BUS          BUS_I2C1
+
+// *************** FLASH **************************
+#define M25P16_CS_PIN           PB12
+#define M25P16_SPI_BUS          BUS_SPI2
+#define USE_FLASHFS
+#define USE_FLASH_M25P16
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+
+#define USE_SPI_DEVICE_2
+#define SPI2_SCK_PIN            PB13
+#define SPI2_MISO_PIN   	    PB14
+#define SPI2_MOSI_PIN   	    PB15
+
+// *************** OSD *****************************
+#define USE_SPI_DEVICE_1
+#define SPI1_SCK_PIN            PA5
+#define SPI1_MISO_PIN           PA6
+#define SPI1_MOSI_PIN           PA7
+
+#define USE_MAX7456
+#define MAX7456_SPI_BUS         BUS_SPI1
+#define MAX7456_CS_PIN          PB2
+// *************** UART *****************************
+#define USE_VCP
+
+#define USE_UART1
+#define UART1_RX_PIN            PA10
+#define UART1_TX_PIN            PA9
+
+#define USE_UART2
+#define UART2_RX_PIN            PA3
+#define UART2_TX_PIN            PA2
+
+#define USE_UART3
+#define UART3_RX_PIN            PB11
+#define UART3_TX_PIN            PB10
+
+#define USE_UART4
+#define UART4_RX_PIN            PA1
+#define UART4_TX_PIN            PA0
+
+#define USE_UART6
+#define UART6_RX_PIN            PC7
+#define UART6_TX_PIN            PC6
+
+#define SERIAL_PORT_COUNT       6
+
+#define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER       SERIALRX_SBUS
+#define SERIALRX_UART           SERIAL_PORT_USART2
+
+// *************** ADC *****************************
+#define USE_ADC
+#define ADC_INSTANCE                ADC1
+#define ADC1_DMA_STREAM             DMA2_Stream0
+#define ADC_CHANNEL_1_PIN           PC0
+#define ADC_CHANNEL_2_PIN           PC2
+#define ADC_CHANNEL_3_PIN           PC1
+
+#define VBAT_ADC_CHANNEL            ADC_CHN_1
+#define RSSI_ADC_CHANNEL            ADC_CHN_2
+#define CURRENT_METER_ADC_CHANNEL   ADC_CHN_3
+
+
+#define DEFAULT_FEATURES            (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX)
+
+
+// ************PINIO to other
+#define PINIO1_PIN                  PC8                //Enable vsw
+#define PINIO2_PIN                  PC9
+
+
+
+// *************** LEDSTRIP ************************
+#define USE_LED_STRIP
+#define WS2811_PIN                  PA8
+
+
+
+#define USE_SERIAL_4WAY_BLHELI_INTERFACE
+
+#define TARGET_IO_PORTA             0xffff
+#define TARGET_IO_PORTB             0xffff
+#define TARGET_IO_PORTC             0xffff
+#define TARGET_IO_PORTD             (BIT(2))
+
+#define MAX_PWM_OUTPUT_PORTS        6
+
+#define USE_DSHOT
+#define USE_ESC_SENSOR

From 3c9c0cbdb3c374aa3c3c7281403c5d74b6ea6a20 Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Sun, 3 Sep 2023 11:40:03 -0500
Subject: [PATCH 94/94] Programming Frameword.md: Update to match Configurator
 renaming

---
 docs/Programming Framework.md | 30 +++++++++++++++---------------
 1 file changed, 15 insertions(+), 15 deletions(-)

diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md
index c2ef089afca..ed9647ae92a 100644
--- a/docs/Programming Framework.md	
+++ b/docs/Programming Framework.md	
@@ -46,16 +46,16 @@ IPF can be edited using INAV Configurator user interface, or via CLI
 | 10            | NAND                          | `false` if `Operand A` and `Operand B` are both `true`|
 | 11            | NOR                           | `true` if `Operand A` and `Operand B` are both `false` |
 | 12            | NOT                           | The boolean opposite to `Operand A` |         
-| 13            | STICKY                        | `Operand A` is the activation operator, `Operand B` is the deactivation operator. After the activation is `true`, the operator will return `true` until Operand B is evaluated as `true`|         
-| 14            | ADD                           | Add `Operand A` to `Operand B` and returns the result |
-| 15            | SUB                           | Substract `Operand B` from `Operand A` and returns the result |
-| 16            | MUL                           | Multiply `Operand A` by `Operand B` and returns the result |
-| 17            | DIV                           | Divide `Operand A` by `Operand B` and returns the result |
-| 18            | GVAR SET                      | Store value from `Operand B` into the Global Variable addressed by
+| 13            | Sticky                        | `Operand A` is the activation operator, `Operand B` is the deactivation operator. After the activation is `true`, the operator will return `true` until Operand B is evaluated as `true`|         
+| 14            | Basic: Add                           | Add `Operand A` to `Operand B` and returns the result |
+| 15            | Basic: Subtract                           | Substract `Operand B` from `Operand A` and returns the result |
+| 16            | Basic: Multiply                           | Multiply `Operand A` by `Operand B` and returns the result |
+| 17            | Basic: Divide                           | Divide `Operand A` by `Operand B` and returns the result |
+| 18            | Set GVAR                      | Store value from `Operand B` into the Global Variable addressed by
 `Operand A`. Bear in mind, that operand `Global Variable` means: Value stored in Global Variable of an index! To store in GVAR 1 use `Value 1` not `Global Variable 1` |
-| 19            | GVAR INC                      | Increase the GVAR indexed by `Operand A` (use `Value 1` for Global Variable 1) with value from `Operand B`  |
-| 20            | GVAR DEC                      | Decrease the GVAR indexed by `Operand A` (use `Value 1` for Global Variable 1) with value from `Operand B`  |
-| 21            | IO PORT SET                   | Set I2C IO Expander pin `Operand A` to value of `Operand B`. `Operand A` accepts values `0-7` and `Operand B` accepts `0` and `1` |
+| 19            | Increase GVAR                      | Increase the GVAR indexed by `Operand A` (use `Value 1` for Global Variable 1) with value from `Operand B`  |
+| 20            | Decrease GVAR                      | Decrease the GVAR indexed by `Operand A` (use `Value 1` for Global Variable 1) with value from `Operand B`  |
+| 21            | Set IO Port                   | Set I2C IO Expander pin `Operand A` to value of `Operand B`. `Operand A` accepts values `0-7` and `Operand B` accepts `0` and `1` |
 | 22            | OVERRIDE_ARMING_SAFETY        | Allows the craft to arm on any angle even without GPS fix. WARNING: This bypasses all safety checks, even that the throttle is low, so use with caution. If you only want to check for certain conditions, such as arm without GPS fix. You will need to add logic conditions to check the throttle is low. |
 | 23            | OVERRIDE_THROTTLE_SCALE       | Override throttle scale to the value defined by operand. Operand type `0` and value `50` means throttle will be scaled by 50%. |
 | 24            | SWAP_ROLL_YAW                 | basically, when activated, yaw stick will control roll and roll stick will control yaw. Required for tail-sitters VTOL during vertical-horizonral transition when body frame changes |
@@ -67,18 +67,18 @@ IPF can be edited using INAV Configurator user interface, or via CLI
 | 30            | SET_VTX_BAND                  | Sets VTX band. Accepted values are `1-5` |
 | 31            | SET_VTX_CHANNEL               | Sets VTX channel. Accepted values are `1-8` |
 | 32            | SET_OSD_LAYOUT                | Sets OSD layout. Accepted values are `0-3` |
-| 33            | SIN                           | Computes SIN of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` |
-| 34            | COS                           | Computes COS of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` |
-| 35            | TAN                           | Computes TAN of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` |
+| 33            | Trigonometry: Sine                           | Computes SIN of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` |
+| 34            | Trigonometry: Cosine                           | Computes COS of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` |
+| 35            | Trigonometry: Tangent                           | Computes TAN of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` |
 | 36            | MAP_INPUT                     | Scales `Operand A` from [`0` : `Operand B`] to [`0` : `1000`]. Note: input will be constrained and then scaled |
 | 37            | MAP_OUTPUT                    | Scales `Operand A` from [`0` : `1000`] to [`0` : `Operand B`]. Note: input will be constrained and then scaled |
 | 38            | RC_CHANNEL_OVERRIDE           | Overrides channel set by `Operand A` to value of `Operand B` |
 | 39            | SET_HEADING_TARGET            | Sets heading-hold target to `Operand A`, in degrees. Value wraps-around. |
-| 40            | MOD                           | Modulo. Divide `Operand A` by `Operand B` and returns the remainder |
+| 40            | Modulo                           | Modulo. Divide `Operand A` by `Operand B` and returns the remainder |
 | 41            | LOITER_RADIUS_OVERRIDE        | Sets the loiter radius to `Operand A` [`0` : `100000`] in cm. If the value is lower than the loiter radius set in the **Advanced Tuning**, that will be used. |
 | 42            | SET_PROFILE                   | Sets the active config profile (PIDFF/Rates/Filters/etc) to `Operand A`. `Operand A` must be a valid profile number, currently from 1 to 3. If not, the profile will not change |
-| 43            | MIN                           | Finds the lowest value of `Operand A` and `Operand B` |
-| 44            | MAX                           | Finds the highest value of `Operand A` and `Operand B` |
+| 43            | Use Lowest Value                           | Finds the lowest value of `Operand A` and `Operand B` |
+| 44            | Use Highest Value                           | Finds the highest value of `Operand A` and `Operand B` |
 | 45			| FLIGHT_AXIS_ANGLE_OVERRIDE	| Sets the target attitude angle for axis. In other words, when active, it enforces Angle mode (Heading Hold for Yaw) on this axis (Angle mode does not have to be active). `Operand A` defines the axis: `0` - Roll, `1` - Pitch, `2` - Yaw. `Operand B` defines the angle in degrees |
 | 46			| FLIGHT_AXIS_RATE_OVERRIDE	    | Sets the target rate (rotation speed) for axis. `Operand A` defines the axis: `0` - Roll, `1` - Pitch, `2` - Yaw. `Operand B` defines the rate in degrees per second |
 | 47            | EDGE                          | Momentarily true when triggered by `Operand A`. `Operand A` is the activation operator [`boolean`], `Operand B` _(Optional)_ is the time for the edge to stay active [ms]. After activation, operator will return `true` until the time in Operand B is reached. If a pure momentary edge is wanted. Just leave `Operand B` as the default `Value: 0` setting. |