From 15c0b58c7de25a0aac56dd0fbdf9da003eb54a17 Mon Sep 17 00:00:00 2001
From: Roman Lut <11955117+RomanLut@users.noreply.github.com>
Date: Tue, 2 Aug 2022 10:19:51 +0300
Subject: [PATCH 001/323] mavlink 2 output 18 channels

---
 src/main/telemetry/mavlink.c | 96 +++++++++++++++++++++++++++---------
 1 file changed, 72 insertions(+), 24 deletions(-)

diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index b13a9f04971..716ec3258c3 100644
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -489,30 +489,78 @@ void mavlinkSendSystemStatus(void)
 void mavlinkSendRCChannelsAndRSSI(void)
 {
 #define GET_CHANNEL_VALUE(x) ((rxRuntimeConfig.channelCount >= (x + 1)) ? rxGetChannelValue(x) : 0)
-    mavlink_msg_rc_channels_raw_pack(mavSystemId, mavComponentId, &mavSendMsg,
-        // time_boot_ms Timestamp (milliseconds since system boot)
-        millis(),
-        // port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos.
-        0,
-        // chan1_raw RC channel 1 value, in microseconds
-        GET_CHANNEL_VALUE(0),
-        // chan2_raw RC channel 2 value, in microseconds
-        GET_CHANNEL_VALUE(1),
-        // chan3_raw RC channel 3 value, in microseconds
-        GET_CHANNEL_VALUE(2),
-        // chan4_raw RC channel 4 value, in microseconds
-        GET_CHANNEL_VALUE(3),
-        // chan5_raw RC channel 5 value, in microseconds
-        GET_CHANNEL_VALUE(4),
-        // chan6_raw RC channel 6 value, in microseconds
-        GET_CHANNEL_VALUE(5),
-        // chan7_raw RC channel 7 value, in microseconds
-        GET_CHANNEL_VALUE(6),
-        // chan8_raw RC channel 8 value, in microseconds
-        GET_CHANNEL_VALUE(7),
-        // rssi Receive signal strength indicator, 0: 0%, 254: 100%
-		//https://github.com/mavlink/mavlink/issues/1027
-        scaleRange(getRSSI(), 0, 1023, 0, 254));
+    if (telemetryConfig()->mavlink.version == 1) {
+        mavlink_msg_rc_channels_raw_pack(mavSystemId, mavComponentId, &mavSendMsg,
+            // time_boot_ms Timestamp (milliseconds since system boot)
+            millis(),
+            // port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos.
+            0,
+            // chan1_raw RC channel 1 value, in microseconds
+            GET_CHANNEL_VALUE(0),
+            // chan2_raw RC channel 2 value, in microseconds
+            GET_CHANNEL_VALUE(1),
+            // chan3_raw RC channel 3 value, in microseconds
+            GET_CHANNEL_VALUE(2),
+            // chan4_raw RC channel 4 value, in microseconds
+            GET_CHANNEL_VALUE(3),
+            // chan5_raw RC channel 5 value, in microseconds
+            GET_CHANNEL_VALUE(4),
+            // chan6_raw RC channel 6 value, in microseconds
+            GET_CHANNEL_VALUE(5),
+            // chan7_raw RC channel 7 value, in microseconds
+            GET_CHANNEL_VALUE(6),
+            // chan8_raw RC channel 8 value, in microseconds
+            GET_CHANNEL_VALUE(7),
+            // rssi Receive signal strength indicator, 0: 0%, 254: 100%
+    		//https://github.com/mavlink/mavlink/issues/1027
+            scaleRange(getRSSI(), 0, 1023, 0, 254));
+	} 
+    else {
+        mavlink_msg_rc_channels_pack(mavSystemId, mavComponentId, &mavSendMsg,
+            // time_boot_ms Timestamp (milliseconds since system boot)
+            millis(),
+            // Total number of RC channels being received. 
+            rxRuntimeConfig.channelCount,
+            // chan1_raw RC channel 1 value, in microseconds
+            GET_CHANNEL_VALUE(0),
+            // chan2_raw RC channel 2 value, in microseconds
+            GET_CHANNEL_VALUE(1),
+            // chan3_raw RC channel 3 value, in microseconds
+            GET_CHANNEL_VALUE(2),
+            // chan4_raw RC channel 4 value, in microseconds
+            GET_CHANNEL_VALUE(3),
+            // chan5_raw RC channel 5 value, in microseconds
+            GET_CHANNEL_VALUE(4),
+            // chan6_raw RC channel 6 value, in microseconds
+            GET_CHANNEL_VALUE(5),
+            // chan7_raw RC channel 7 value, in microseconds
+            GET_CHANNEL_VALUE(6),
+            // chan8_raw RC channel 8 value, in microseconds
+            GET_CHANNEL_VALUE(7),
+            // chan9_raw RC channel 9 value, in microseconds
+            GET_CHANNEL_VALUE(8),
+            // chan10_raw RC channel 10 value, in microseconds
+            GET_CHANNEL_VALUE(9),
+            // chan11_raw RC channel 11 value, in microseconds
+            GET_CHANNEL_VALUE(10),
+            // chan12_raw RC channel 12 value, in microseconds
+            GET_CHANNEL_VALUE(11),
+            // chan13_raw RC channel 13 value, in microseconds
+            GET_CHANNEL_VALUE(12),
+            // chan14_raw RC channel 14 value, in microseconds
+            GET_CHANNEL_VALUE(13),
+            // chan15_raw RC channel 15 value, in microseconds
+            GET_CHANNEL_VALUE(14),
+            // chan16_raw RC channel 16 value, in microseconds
+            GET_CHANNEL_VALUE(15),
+            // chan17_raw RC channel 17 value, in microseconds
+            GET_CHANNEL_VALUE(16),
+            // chan18_raw RC channel 18 value, in microseconds
+            GET_CHANNEL_VALUE(17),
+            // rssi Receive signal strength indicator, 0: 0%, 254: 100%
+    		//https://github.com/mavlink/mavlink/issues/1027
+            scaleRange(getRSSI(), 0, 1023, 0, 254));
+    }
 #undef GET_CHANNEL_VALUE
 
     mavlinkSendMessage();

From 0246892708759f16d6910e6e570f7b5ce5acc582 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Tue, 7 Nov 2023 23:20:49 +0000
Subject: [PATCH 002/323] initial build

---
 src/main/navigation/navigation.c             | 109 ++++++++++---------
 src/main/navigation/navigation_fixedwing.c   |  46 +++-----
 src/main/navigation/navigation_multicopter.c |  35 +++---
 src/main/navigation/navigation_private.h     |   6 +-
 4 files changed, 92 insertions(+), 104 deletions(-)

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index c566c94bfad..57a6ba5c066 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -2927,8 +2927,7 @@ void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlag
 
     // Z-position
     if ((useMask & NAV_POS_UPDATE_Z) != 0) {
-        updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);   // Reset RoC/RoD -> altitude controller
-        posControl.desiredState.pos.z = pos->z;
+        updateClimbRateToAltitudeController(navConfig()->general.max_auto_climb_rate, pos->z, ROC_TO_ALT_TARGET);
     }
 
     // Heading
@@ -3036,66 +3035,68 @@ bool isProbablyStillFlying(void)
 /*-----------------------------------------------------------
  * Z-position controller
  *-----------------------------------------------------------*/
-void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode)
+static bool isMaxAltitudeLimitExceeded(void)
 {
-#define MIN_TARGET_CLIMB_RATE   100.0f  // cm/s
+    return navGetCurrentActualPositionAndVelocity()->pos.z >= navConfig()->general.max_altitude;
+}
 
-    static timeUs_t lastUpdateTimeUs;
-    timeUs_t currentTimeUs = micros();
+int32_t getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros)
+{
+    if (navConfig()->general.max_altitude && isMaxAltitudeLimitExceeded()) {
+        targetAltitude = MIN(targetAltitude, navConfig()->general.max_altitude);
+    }
 
-    // Terrain following uses different altitude measurement
-    const float altitudeToUse = navGetCurrentActualPositionAndVelocity()->pos.z;
+    const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding();
 
-    if (mode != ROC_TO_ALT_RESET && desiredClimbRate) {
-        /* 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 */
+    float maxClimbRate = navConfig()->general.max_auto_climb_rate;
+    if (emergLandingIsActive) {
+        maxClimbRate = navConfig()->general.emerg_descent_rate;
+    } else if (posControl.flags.isAdjustingAltitude) {
+        maxClimbRate = navConfig()->general.max_manual_climb_rate;
+    }
+    const float targetAltitudeError = targetAltitude - navGetCurrentActualPositionAndVelocity()->pos.z;
+    float targetVel = 0.0f;
 
-        if (mode == ROC_TO_ALT_TARGET && fabsf(desiredClimbRate) > MIN_TARGET_CLIMB_RATE) {
-            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);
+    if (STATE(MULTIROTOR)) {
+        targetVel = getSqrtControllerVelocity(targetAltitude, deltaMicros);
+    } else {
+        targetVel = scaleRangef(targetAltitudeError, 0.0f, maxClimbRate * 5, 0.0f, maxClimbRate);
+    }
 
-            desiredClimbRate = direction * constrainf(verticalVelScaled, MIN_TARGET_CLIMB_RATE, absClimbRate);
-        }
+    if (emergLandingIsActive && targetAltitudeError > -50) {
+        return -100;
+    } else {
+        return constrainf(targetVel, -maxClimbRate, maxClimbRate);
+    }
+}
 
-        /*
-         * 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) {
-            desiredClimbRate = 0;
-        }
+void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode)
+{
+    /* ROC_TO_ALT_CONSTANT - constant climb rate. desiredClimbRate direction (+ or -) is required
+     * 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.
+     * desiredClimbRate direction isn't required, set by target altitude direction instead
+     * NAV_IMPOSSIBLE_ALTITUDE_TARGET simply uses posControl.desiredState.pos.z to set a constant climb rate */
 
-        if (STATE(FIXED_WING_LEGACY)) {
-            // Fixed wing climb rate controller is open-loop. We simply move the known altitude target
-            float timeDelta = US2S(currentTimeUs - lastUpdateTimeUs);
-            static bool targetHoldActive = false;
-
-            if (timeDelta <= HZ2S(MIN_POSITION_UPDATE_RATE_HZ) && desiredClimbRate) {
-                // Update target altitude only if actual altitude moving in same direction and lagging by < 5 m, otherwise hold target
-                if (navGetCurrentActualPositionAndVelocity()->vel.z * desiredClimbRate >= 0 && fabsf(posControl.desiredState.pos.z - altitudeToUse) < 500) {
-                    posControl.desiredState.pos.z += desiredClimbRate * timeDelta;
-                    targetHoldActive = false;
-                } else if (!targetHoldActive) {     // Reset and hold target to actual + climb rate boost until actual catches up
-                    posControl.desiredState.pos.z = altitudeToUse + desiredClimbRate;
-                    targetHoldActive = true;
-                }
-            } else {
-                targetHoldActive = false;
-            }
-        }
-        else {
-            // 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);
-        }
-    } else {    // ROC_TO_ALT_RESET or zero desired climbrate
+    // Terrain following uses different altitude measurement
+    const float altitudeToUse = navGetCurrentActualPositionAndVelocity()->pos.z;
+
+    if (mode == ROC_TO_ALT_RESET) {
         posControl.desiredState.pos.z = altitudeToUse;
+    } else if (mode == ROC_TO_ALT_TARGET) {
+        posControl.desiredState.pos.z = targetAltitude;
+    } else {    // ROC_TO_ALT_CONSTANT
+        posControl.desiredState.pos.z = NAV_IMPOSSIBLE_ALTITUDE_TARGET;
+    }
+
+    /*
+     * If max altitude is set and altitude limit reached reset altitude target to max altitude unless desired climb rate is -ve
+     */
+    if (navConfig()->general.max_altitude && isMaxAltitudeLimitExceeded() && desiredClimbRate >= 0.0f) {
+        posControl.desiredState.pos.z = MIN(posControl.desiredState.pos.z, navConfig()->general.max_altitude);
     }
 
-    lastUpdateTimeUs = currentTimeUs;
+    posControl.desiredState.vel.z = desiredClimbRate;
 }
 
 static void resetAltitudeController(bool useTerrainFollowing)
@@ -4317,9 +4318,9 @@ void navigationUsePIDs(void)
                                         0.0f
     );
 
-    navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 10.0f,
-                                        (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 10.0f,
-                                        (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 10.0f,
+    navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 100.0f,
+                                        (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 100.0f,
+                                        (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 100.0f,
                                         0.0f,
                                         NAV_DTERM_CUT_HZ,
                                         0.0f
diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index e6084e0972e..c5dde6dba48 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -123,7 +123,7 @@ bool adjustFixedWingAltitudeFromRCInput(void)
     else {
         // Adjusting finished - reset desired position to stay exactly where pilot released the stick
         if (posControl.flags.isAdjustingAltitude) {
-            updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
+            updateClimbRateToAltitudeController(navConfig()->general.max_auto_climb_rate, 0, ROC_TO_ALT_RESET);
         }
         return false;
     }
@@ -134,46 +134,34 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
 {
     static pt1Filter_t velzFilterState;
 
-    // On a fixed wing we might not have a reliable climb rate source (if no BARO available), so we can't apply PID controller to
-    // velocity error. We use PID controller on altitude error and calculate desired pitch angle
+    float desiredClimbRate = posControl.desiredState.vel.z;
 
-    // Update energies
-    const float demSPE = (posControl.desiredState.pos.z * 0.01f) * GRAVITY_MSS;
-    const float demSKE = 0.0f;
-
-    const float estSPE = (navGetCurrentActualPositionAndVelocity()->pos.z * 0.01f) * GRAVITY_MSS;
-    const float estSKE = 0.0f;
-
-    // speedWeight controls balance between potential and kinetic energy used for pitch controller
-    //  speedWeight = 1.0 : pitch will only control airspeed and won't control altitude
-    //  speedWeight = 0.5 : pitch will be used to control both airspeed and altitude
-    //  speedWeight = 0.0 : pitch will only control altitude
-    const float speedWeight = 0.0f; // no speed sensing for now
-
-    const float demSEB = demSPE * (1.0f - speedWeight) - demSKE * speedWeight;
-    const float estSEB = estSPE * (1.0f - speedWeight) - estSKE * speedWeight;
+    if (posControl.desiredState.pos.z != NAV_IMPOSSIBLE_ALTITUDE_TARGET) {
+        desiredClimbRate = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros);
+    }
 
-    // SEB to pitch angle gain to account for airspeed (with respect to specified reference (tuning) speed
-    const float pitchGainInv = 1.0f / 1.0f;
+    // Reduce max allowed climb pitch if performing loiter (stall prevention)
+    if (needToCalculateCircularLoiter && desiredClimbRate > 0.0f) {
+        desiredClimbRate *= 0.67f;
+    }
 
     // Here we use negative values for dive for better clarity
-    float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle);
+    const 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 (stall prevention)
-    if (needToCalculateCircularLoiter) {
-        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);
+    // PID controller to translate desired climb rate error into pitch angle [decideg]
+    float currentClimbRate = navGetCurrentActualPositionAndVelocity()->vel.z;
+    float targetPitchAngle = navPidApply2(&posControl.pids.fw_alt, desiredClimbRate, currentClimbRate, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, PID_DTERM_FROM_ERROR);
 
     // Apply low-pass filter to prevent rapid correction
     targetPitchAngle = pt1FilterApply4(&velzFilterState, targetPitchAngle, getSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicros));
 
-    // Reconstrain pitch angle ( >0 - climb, <0 - dive)
+    // Reconstrain pitch angle (> 0 - climb, < 0 - dive)
     targetPitchAngle = constrainf(targetPitchAngle, minDiveDeciDeg, maxClimbDeciDeg);
     posControl.rcAdjustment[PITCH] = targetPitchAngle;
+
+    posControl.desiredState.vel.z = desiredClimbRate;
+    navDesiredVelocity[Z] = constrain(lrintf(posControl.desiredState.vel.z), -32678, 32767);
 }
 
 void applyFixedWingAltitudeAndThrottleController(timeUs_t currentTimeUs)
diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c
index 2264d842014..935665ee60e 100644
--- a/src/main/navigation/navigation_multicopter.c
+++ b/src/main/navigation/navigation_multicopter.c
@@ -63,28 +63,26 @@ static pt1Filter_t altholdThrottleFilterState;
 static bool prepareForTakeoffOnReset = false;
 static sqrt_controller_t alt_hold_sqrt_controller;
 
-// Position to velocity controller for Z axis
-static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
+float getSqrtControllerVelocity(float targetAltitude, timeDelta_t deltaMicros)
 {
-    float targetVel = sqrtControllerApply(
-        &alt_hold_sqrt_controller,
-        posControl.desiredState.pos.z,
-        navGetCurrentActualPositionAndVelocity()->pos.z,
-        US2S(deltaMicros)
+    return sqrtControllerApply(
+            &alt_hold_sqrt_controller,
+            targetAltitude,
+            navGetCurrentActualPositionAndVelocity()->pos.z,
+            US2S(deltaMicros)
     );
+}
 
-    // hard limit desired target velocity to max_climb_rate
-    float vel_max_z = 0.0f;
+// Position to velocity controller for Z axis
+static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
+{
+    float targetVel = posControl.desiredState.vel.z;
 
-    if (posControl.flags.isAdjustingAltitude) {
-        vel_max_z = navConfig()->general.max_manual_climb_rate;
-    } else {
-        vel_max_z = navConfig()->general.max_auto_climb_rate;
+    if (posControl.desiredState.pos.z != NAV_IMPOSSIBLE_ALTITUDE_TARGET) {
+        targetVel = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros);
     }
 
-    targetVel = constrainf(targetVel, -vel_max_z, vel_max_z);
-
-    posControl.pids.pos[Z].output_constrained = targetVel;
+    posControl.pids.pos[Z].output_constrained = targetVel;      // only used for Blackbox and OSD info
 
     // Limit max up/down acceleration target
     const float smallVelChange = US2S(deltaMicros) * (GRAVITY_CMSS * 0.1f);
@@ -132,8 +130,7 @@ 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, 0, ROC_TO_ALT_RESET);
-            posControl.desiredState.pos.z = altTarget;
+            updateClimbRateToAltitudeController(navConfig()->general.max_manual_climb_rate, altTarget, ROC_TO_ALT_TARGET);
         }
         else {
             updateClimbRateToAltitudeController(-50.0f, 0, ROC_TO_ALT_CONSTANT);
@@ -165,7 +162,7 @@ bool adjustMulticopterAltitudeFromRCInput(void)
         else {
             // Adjusting finished - reset desired position to stay exactly where pilot released the stick
             if (posControl.flags.isAdjustingAltitude) {
-                updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
+                updateClimbRateToAltitudeController(navConfig()->general.max_auto_climb_rate, 0, ROC_TO_ALT_RESET);
             }
 
             return false;
diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
index c408f109c9b..35e5492c6f1 100644
--- a/src/main/navigation/navigation_private.h
+++ b/src/main/navigation/navigation_private.h
@@ -47,6 +47,8 @@
 
 #define NAV_RTH_TRACKBACK_POINTS            50      // max number RTH trackback points
 
+#define NAV_IMPOSSIBLE_ALTITUDE_TARGET      10000000    // 100km in cm. Serves as a flag for vertical velocity control
+
 #define MAX_POSITION_UPDATE_INTERVAL_US     HZ2US(MIN_POSITION_UPDATE_RATE_HZ)        // convenience macro
 _Static_assert(MAX_POSITION_UPDATE_INTERVAL_US <= TIMEDELTA_MAX, "deltaMicros can overflow!");
 
@@ -481,6 +483,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, float gpsCfEstimatedAltitudeError);
+int32_t getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros);
 
 bool checkForPositionSensorTimeout(void);
 
@@ -499,10 +502,9 @@ bool adjustMulticopterHeadingFromRCInput(void);
 bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcRollAdjustment);
 
 void applyMulticopterNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs);
-
 bool isMulticopterLandingDetected(void);
-
 void calculateMulticopterInitialHoldPosition(fpVector3_t * pos);
+float getSqrtControllerVelocity(float targetAltitude, timeDelta_t deltaMicros);
 
 /* Fixed-wing specific functions */
 void setupFixedWingAltitudeController(void);

From 7ae41ad928348b0d253b6df49eace47138e0358c Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Wed, 8 Nov 2023 22:48:33 +0000
Subject: [PATCH 003/323] cleanup + add roc to alt flag

---
 src/main/navigation/navigation.c             | 26 +++++++++++---------
 src/main/navigation/navigation_fixedwing.c   |  6 ++---
 src/main/navigation/navigation_multicopter.c |  9 +++----
 src/main/navigation/navigation_private.h     |  4 +--
 4 files changed, 24 insertions(+), 21 deletions(-)

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 57a6ba5c066..f18c1cdc567 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -2927,7 +2927,7 @@ void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlag
 
     // Z-position
     if ((useMask & NAV_POS_UPDATE_Z) != 0) {
-        updateClimbRateToAltitudeController(navConfig()->general.max_auto_climb_rate, pos->z, ROC_TO_ALT_TARGET);
+        updateClimbRateToAltitudeController(0, pos->z, ROC_TO_ALT_TARGET);
     }
 
     // Heading
@@ -3060,11 +3060,12 @@ int32_t getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros)
     if (STATE(MULTIROTOR)) {
         targetVel = getSqrtControllerVelocity(targetAltitude, deltaMicros);
     } else {
-        targetVel = scaleRangef(targetAltitudeError, 0.0f, maxClimbRate * 5, 0.0f, maxClimbRate);
+        // 0.2f relates to climb rate starting to reduce from maxClimbRate at a distance of 5 x maxClimbRate from targetAltitude
+        targetVel = 0.2f * targetAltitudeError;
     }
 
     if (emergLandingIsActive && targetAltitudeError > -50) {
-        return -100;
+        return -100;    // maintain 1 m/s descent during emerg landing when approaching T/O altitude
     } else {
         return constrainf(targetVel, -maxClimbRate, maxClimbRate);
     }
@@ -3072,11 +3073,14 @@ int32_t getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros)
 
 void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode)
 {
-    /* ROC_TO_ALT_CONSTANT - constant climb rate. desiredClimbRate direction (+ or -) is required
-     * 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.
-     * desiredClimbRate direction isn't required, set by target altitude direction instead
-     * NAV_IMPOSSIBLE_ALTITUDE_TARGET simply uses posControl.desiredState.pos.z to set a constant climb rate */
+    /* ROC_TO_ALT_TARGET - constant climb rate until close to target altitude reducing to 0 when reached.
+     * Rate reduction starts at distance from target altitude of 5 x climb rate for Fixed wing.
+     * No climb rate required, set by target altitude direction instead.
+     *
+     * ROC_TO_ALT_RESET - similar to ROC_TO_ALT_TARGET except target altitude set to current altitude.
+     * No climb rate or altitude target required.
+     *
+     * ROC_TO_ALT_CONSTANT - constant climb rate. Climb rate and direction required. Target alt not required. */
 
     // Terrain following uses different altitude measurement
     const float altitudeToUse = navGetCurrentActualPositionAndVelocity()->pos.z;
@@ -3085,10 +3089,10 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt
         posControl.desiredState.pos.z = altitudeToUse;
     } else if (mode == ROC_TO_ALT_TARGET) {
         posControl.desiredState.pos.z = targetAltitude;
-    } else {    // ROC_TO_ALT_CONSTANT
-        posControl.desiredState.pos.z = NAV_IMPOSSIBLE_ALTITUDE_TARGET;
     }
 
+    posControl.flags.rocToAltMode = mode;
+
     /*
      * If max altitude is set and altitude limit reached reset altitude target to max altitude unless desired climb rate is -ve
      */
@@ -3096,7 +3100,7 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt
         posControl.desiredState.pos.z = MIN(posControl.desiredState.pos.z, navConfig()->general.max_altitude);
     }
 
-    posControl.desiredState.vel.z = desiredClimbRate;
+    posControl.desiredState.vel.z = desiredClimbRate;   // only used for ROC_TO_ALT_CONSTANT
 }
 
 static void resetAltitudeController(bool useTerrainFollowing)
diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index c5dde6dba48..0272df114c7 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -123,7 +123,7 @@ bool adjustFixedWingAltitudeFromRCInput(void)
     else {
         // Adjusting finished - reset desired position to stay exactly where pilot released the stick
         if (posControl.flags.isAdjustingAltitude) {
-            updateClimbRateToAltitudeController(navConfig()->general.max_auto_climb_rate, 0, ROC_TO_ALT_RESET);
+            updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
         }
         return false;
     }
@@ -136,7 +136,7 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
 
     float desiredClimbRate = posControl.desiredState.vel.z;
 
-    if (posControl.desiredState.pos.z != NAV_IMPOSSIBLE_ALTITUDE_TARGET) {
+    if (posControl.flags.rocToAltMode != ROC_TO_ALT_CONSTANT) {
         desiredClimbRate = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros);
     }
 
@@ -757,7 +757,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.0f, ROC_TO_ALT_TARGET);
+        updateClimbRateToAltitudeController(0, 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 935665ee60e..559898eadfd 100644
--- a/src/main/navigation/navigation_multicopter.c
+++ b/src/main/navigation/navigation_multicopter.c
@@ -78,7 +78,7 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
 {
     float targetVel = posControl.desiredState.vel.z;
 
-    if (posControl.desiredState.pos.z != NAV_IMPOSSIBLE_ALTITUDE_TARGET) {
+    if (posControl.flags.rocToAltMode != ROC_TO_ALT_CONSTANT) {
         targetVel = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros);
     }
 
@@ -130,7 +130,7 @@ 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(navConfig()->general.max_manual_climb_rate, altTarget, ROC_TO_ALT_TARGET);
+            updateClimbRateToAltitudeController(0, altTarget, ROC_TO_ALT_TARGET);
         }
         else {
             updateClimbRateToAltitudeController(-50.0f, 0, ROC_TO_ALT_CONSTANT);
@@ -162,7 +162,7 @@ bool adjustMulticopterAltitudeFromRCInput(void)
         else {
             // Adjusting finished - reset desired position to stay exactly where pilot released the stick
             if (posControl.flags.isAdjustingAltitude) {
-                updateClimbRateToAltitudeController(navConfig()->general.max_auto_climb_rate, 0, ROC_TO_ALT_RESET);
+                updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
             }
 
             return false;
@@ -249,7 +249,6 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs)
             if (prepareForTakeoffOnReset) {
                 const navEstimatedPosVel_t *posToUse = navGetCurrentActualPositionAndVelocity();
 
-                posControl.desiredState.vel.z = -navConfig()->general.max_manual_climb_rate;
                 posControl.desiredState.pos.z = posToUse->pos.z - (navConfig()->general.max_manual_climb_rate / posControl.pids.pos[Z].param.kP);
                 posControl.pids.vel[Z].integrator = -500.0f;
                 pt1FilterReset(&altholdThrottleFilterState, -500.0f);
@@ -930,7 +929,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.0f, ROC_TO_ALT_TARGET);
+            updateClimbRateToAltitudeController(0, 500.0f, 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 35e5492c6f1..8452650ffaa 100644
--- a/src/main/navigation/navigation_private.h
+++ b/src/main/navigation/navigation_private.h
@@ -47,8 +47,6 @@
 
 #define NAV_RTH_TRACKBACK_POINTS            50      // max number RTH trackback points
 
-#define NAV_IMPOSSIBLE_ALTITUDE_TARGET      10000000    // 100km in cm. Serves as a flag for vertical velocity control
-
 #define MAX_POSITION_UPDATE_INTERVAL_US     HZ2US(MIN_POSITION_UPDATE_RATE_HZ)        // convenience macro
 _Static_assert(MAX_POSITION_UPDATE_INTERVAL_US <= TIMEDELTA_MAX, "deltaMicros can overflow!");
 
@@ -95,6 +93,8 @@ typedef struct navigationFlags_s {
     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
 
+    climbRateToAltitudeControllerMode_e rocToAltMode;
+
     bool isAdjustingPosition;
     bool isAdjustingAltitude;
     bool isAdjustingHeading;

From a8079c28423d74ae592cbd9c51a3911ad7f0b0e7 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Thu, 9 Nov 2023 11:24:37 +0000
Subject: [PATCH 004/323] fixes

---
 src/main/navigation/navigation.c         | 8 +++-----
 src/main/navigation/navigation_private.h | 2 +-
 2 files changed, 4 insertions(+), 6 deletions(-)

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index f18c1cdc567..7505260c889 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -3040,7 +3040,7 @@ static bool isMaxAltitudeLimitExceeded(void)
     return navGetCurrentActualPositionAndVelocity()->pos.z >= navConfig()->general.max_altitude;
 }
 
-int32_t getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros)
+float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros)
 {
     if (navConfig()->general.max_altitude && isMaxAltitudeLimitExceeded()) {
         targetAltitude = MIN(targetAltitude, navConfig()->general.max_altitude);
@@ -3082,11 +3082,8 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt
      *
      * ROC_TO_ALT_CONSTANT - constant climb rate. Climb rate and direction required. Target alt not required. */
 
-    // Terrain following uses different altitude measurement
-    const float altitudeToUse = navGetCurrentActualPositionAndVelocity()->pos.z;
-
     if (mode == ROC_TO_ALT_RESET) {
-        posControl.desiredState.pos.z = altitudeToUse;
+        posControl.desiredState.pos.z = navGetCurrentActualPositionAndVelocity()->pos.z;
     } else if (mode == ROC_TO_ALT_TARGET) {
         posControl.desiredState.pos.z = targetAltitude;
     }
@@ -3098,6 +3095,7 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt
      */
     if (navConfig()->general.max_altitude && isMaxAltitudeLimitExceeded() && desiredClimbRate >= 0.0f) {
         posControl.desiredState.pos.z = MIN(posControl.desiredState.pos.z, navConfig()->general.max_altitude);
+        posControl.flags.rocToAltMode = ROC_TO_ALT_TARGET;
     }
 
     posControl.desiredState.vel.z = desiredClimbRate;   // only used for ROC_TO_ALT_CONSTANT
diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
index 8452650ffaa..428b54cd4f4 100644
--- a/src/main/navigation/navigation_private.h
+++ b/src/main/navigation/navigation_private.h
@@ -483,7 +483,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, float gpsCfEstimatedAltitudeError);
-int32_t getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros);
+float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros);
 
 bool checkForPositionSensorTimeout(void);
 

From 1fcac6134a0fa3ad7a27ad1cc16d5096e5082ffd Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Fri, 10 Nov 2023 22:29:19 +0000
Subject: [PATCH 005/323] fix multirotor velocity z

---
 src/main/navigation/navigation.c             | 4 ++--
 src/main/navigation/navigation_fixedwing.c   | 2 +-
 src/main/navigation/navigation_multicopter.c | 3 ++-
 src/main/navigation/navigation_private.h     | 1 +
 4 files changed, 6 insertions(+), 4 deletions(-)

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 7505260c889..ded835919dd 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -3086,6 +3086,8 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt
         posControl.desiredState.pos.z = navGetCurrentActualPositionAndVelocity()->pos.z;
     } else if (mode == ROC_TO_ALT_TARGET) {
         posControl.desiredState.pos.z = targetAltitude;
+    } else {    // ROC_TO_ALT_CONSTANT
+        posControl.desiredState.climbRateDemand = desiredClimbRate;
     }
 
     posControl.flags.rocToAltMode = mode;
@@ -3097,8 +3099,6 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt
         posControl.desiredState.pos.z = MIN(posControl.desiredState.pos.z, navConfig()->general.max_altitude);
         posControl.flags.rocToAltMode = ROC_TO_ALT_TARGET;
     }
-
-    posControl.desiredState.vel.z = desiredClimbRate;   // only used for ROC_TO_ALT_CONSTANT
 }
 
 static void resetAltitudeController(bool useTerrainFollowing)
diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index 0272df114c7..f5c3fb159ae 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -134,7 +134,7 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
 {
     static pt1Filter_t velzFilterState;
 
-    float desiredClimbRate = posControl.desiredState.vel.z;
+    float desiredClimbRate = posControl.desiredState.climbRateDemand;
 
     if (posControl.flags.rocToAltMode != ROC_TO_ALT_CONSTANT) {
         desiredClimbRate = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros);
diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c
index 559898eadfd..b35d88c6093 100644
--- a/src/main/navigation/navigation_multicopter.c
+++ b/src/main/navigation/navigation_multicopter.c
@@ -76,7 +76,7 @@ float getSqrtControllerVelocity(float targetAltitude, timeDelta_t deltaMicros)
 // Position to velocity controller for Z axis
 static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
 {
-    float targetVel = posControl.desiredState.vel.z;
+    float targetVel = posControl.desiredState.climbRateDemand;
 
     if (posControl.flags.rocToAltMode != ROC_TO_ALT_CONSTANT) {
         targetVel = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros);
@@ -249,6 +249,7 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs)
             if (prepareForTakeoffOnReset) {
                 const navEstimatedPosVel_t *posToUse = navGetCurrentActualPositionAndVelocity();
 
+                posControl.desiredState.vel.z = -navConfig()->general.max_manual_climb_rate;
                 posControl.desiredState.pos.z = posToUse->pos.z - (navConfig()->general.max_manual_climb_rate / posControl.pids.pos[Z].param.kP);
                 posControl.pids.vel[Z].integrator = -500.0f;
                 pt1FilterReset(&altholdThrottleFilterState, -500.0f);
diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
index 428b54cd4f4..cf4698a24c8 100644
--- a/src/main/navigation/navigation_private.h
+++ b/src/main/navigation/navigation_private.h
@@ -140,6 +140,7 @@ typedef struct {
     fpVector3_t pos;
     fpVector3_t vel;
     int32_t     yaw;
+    int16_t     climbRateDemand;
 } navigationDesiredState_t;
 
 typedef enum {

From 87faeb11ee6c25f024cf38832ec22746b9f6ce74 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Thu, 16 Nov 2023 11:13:21 +0000
Subject: [PATCH 006/323] WP Linear climb + Update max alt limter

---
 src/main/navigation/navigation.c | 38 ++++++++++++++++++--------------
 1 file changed, 22 insertions(+), 16 deletions(-)

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index ded835919dd..2fd1cfd3256 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -1691,10 +1691,20 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
                     fpVector3_t tmpWaypoint;
                     tmpWaypoint.x = posControl.activeWaypoint.pos.x;
                     tmpWaypoint.y = posControl.activeWaypoint.pos.y;
-                    tmpWaypoint.z = scaleRangef(constrainf(posControl.wpDistance, posControl.wpInitialDistance / 10.0f, posControl.wpInitialDistance),
-                        posControl.wpInitialDistance, posControl.wpInitialDistance / 10.0f,
-                        posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z);
-                    setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
+                    setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING);
+
+                    // Use linear climb bewtween WPs until within 10% of total distance to current active WP
+                    float linearClimbDistRemaining = posControl.wpDistance - 0.1f * posControl.wpInitialDistance;
+                    if (linearClimbDistRemaining > 0) {
+                        int16_t climbRate = constrainf(posControl.actualState.velXY *
+                                            (posControl.activeWaypoint.pos.z - posControl.actualState.abs.pos.z) / linearClimbDistRemaining,
+                                            -navConfig()->general.max_auto_climb_rate,
+                                            navConfig()->general.max_auto_climb_rate);
+                        updateClimbRateToAltitudeController(climbRate, 0, ROC_TO_ALT_CONSTANT);
+                    } else {
+                        updateClimbRateToAltitudeController(0, posControl.activeWaypoint.pos.z, ROC_TO_ALT_TARGET);
+                    }
+
                     if(STATE(MULTIROTOR)) {
                         switch (wpHeadingControl.mode) {
                             case NAV_WP_HEAD_MODE_NONE:
@@ -3035,17 +3045,8 @@ bool isProbablyStillFlying(void)
 /*-----------------------------------------------------------
  * Z-position controller
  *-----------------------------------------------------------*/
-static bool isMaxAltitudeLimitExceeded(void)
-{
-    return navGetCurrentActualPositionAndVelocity()->pos.z >= navConfig()->general.max_altitude;
-}
-
 float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros)
 {
-    if (navConfig()->general.max_altitude && isMaxAltitudeLimitExceeded()) {
-        targetAltitude = MIN(targetAltitude, navConfig()->general.max_altitude);
-    }
-
     const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding();
 
     float maxClimbRate = navConfig()->general.max_auto_climb_rate;
@@ -3093,11 +3094,16 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt
     posControl.flags.rocToAltMode = mode;
 
     /*
-     * If max altitude is set and altitude limit reached reset altitude target to max altitude unless desired climb rate is -ve
+     * If max altitude is set and altitude limit reached reset altitude target to max altitude unless desired climb rate is -ve.
+     * Inhibit during RTH mode and also WP mode with altitude enforce active to avoid climbs getting stuck at max alt limit.
      */
-    if (navConfig()->general.max_altitude && isMaxAltitudeLimitExceeded() && desiredClimbRate >= 0.0f) {
+    if (navConfig()->general.max_altitude && !FLIGHT_MODE(NAV_RTH_MODE) && !(FLIGHT_MODE(NAV_WP_MODE) && navConfig()->general.waypoint_enforce_altitude)) {
         posControl.desiredState.pos.z = MIN(posControl.desiredState.pos.z, navConfig()->general.max_altitude);
-        posControl.flags.rocToAltMode = ROC_TO_ALT_TARGET;
+
+        if (navGetCurrentActualPositionAndVelocity()->pos.z >= navConfig()->general.max_altitude && desiredClimbRate >= 0.0f) {
+            posControl.desiredState.pos.z = navConfig()->general.max_altitude;
+            posControl.flags.rocToAltMode = ROC_TO_ALT_TARGET;
+        }
     }
 }
 

From f521f9895f7781724f37a17ef5a90464a18e5b45 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Sat, 18 Nov 2023 20:50:04 +0000
Subject: [PATCH 007/323] WP linear climb improvements

---
 src/main/navigation/navigation.c | 35 +++++++++++++++++---------------
 1 file changed, 19 insertions(+), 16 deletions(-)

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 2fd1cfd3256..eae0be28756 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -1694,16 +1694,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
                     setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING);
 
                     // Use linear climb bewtween WPs until within 10% of total distance to current active WP
-                    float linearClimbDistRemaining = posControl.wpDistance - 0.1f * posControl.wpInitialDistance;
-                    if (linearClimbDistRemaining > 0) {
-                        int16_t climbRate = constrainf(posControl.actualState.velXY *
-                                            (posControl.activeWaypoint.pos.z - posControl.actualState.abs.pos.z) / linearClimbDistRemaining,
-                                            -navConfig()->general.max_auto_climb_rate,
-                                            navConfig()->general.max_auto_climb_rate);
-                        updateClimbRateToAltitudeController(climbRate, 0, ROC_TO_ALT_CONSTANT);
-                    } else {
-                        updateClimbRateToAltitudeController(0, posControl.activeWaypoint.pos.z, ROC_TO_ALT_TARGET);
+                    float climbRate = posControl.actualState.velXY * (posControl.activeWaypoint.pos.z - posControl.wpInitialAltitude) /
+                                      (0.9f * posControl.wpInitialDistance);
+
+                    if (fabsf(climbRate) >= navConfig()->general.max_auto_climb_rate) {
+                        climbRate = 0;
                     }
+                    updateClimbRateToAltitudeController(climbRate, posControl.activeWaypoint.pos.z, ROC_TO_ALT_TARGET);
 
                     if(STATE(MULTIROTOR)) {
                         switch (wpHeadingControl.mode) {
@@ -3050,11 +3047,14 @@ float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros)
     const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding();
 
     float maxClimbRate = navConfig()->general.max_auto_climb_rate;
-    if (emergLandingIsActive) {
+    if (posControl.desiredState.climbRateDemand) {
+        maxClimbRate = ABS(posControl.desiredState.climbRateDemand);
+    } else if (emergLandingIsActive) {
         maxClimbRate = navConfig()->general.emerg_descent_rate;
     } else if (posControl.flags.isAdjustingAltitude) {
         maxClimbRate = navConfig()->general.max_manual_climb_rate;
     }
+
     const float targetAltitudeError = targetAltitude - navGetCurrentActualPositionAndVelocity()->pos.z;
     float targetVel = 0.0f;
 
@@ -3076,7 +3076,7 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt
 {
     /* ROC_TO_ALT_TARGET - constant climb rate until close to target altitude reducing to 0 when reached.
      * Rate reduction starts at distance from target altitude of 5 x climb rate for Fixed wing.
-     * No climb rate required, set by target altitude direction instead.
+     * Any non zero climb rate sets the max allowed climb rate. Default max climb rate limits are used when set to 0.
      *
      * ROC_TO_ALT_RESET - similar to ROC_TO_ALT_TARGET except target altitude set to current altitude.
      * No climb rate or altitude target required.
@@ -3087,21 +3087,24 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt
         posControl.desiredState.pos.z = navGetCurrentActualPositionAndVelocity()->pos.z;
     } else if (mode == ROC_TO_ALT_TARGET) {
         posControl.desiredState.pos.z = targetAltitude;
-    } else {    // ROC_TO_ALT_CONSTANT
-        posControl.desiredState.climbRateDemand = desiredClimbRate;
     }
 
+    posControl.desiredState.climbRateDemand = desiredClimbRate;
     posControl.flags.rocToAltMode = mode;
 
     /*
-     * If max altitude is set and altitude limit reached reset altitude target to max altitude unless desired climb rate is -ve.
+     * If max altitude is set limit desired altitude and impose altitude limit for constant climbs unless climb rate is -ve.
      * Inhibit during RTH mode and also WP mode with altitude enforce active to avoid climbs getting stuck at max alt limit.
      */
     if (navConfig()->general.max_altitude && !FLIGHT_MODE(NAV_RTH_MODE) && !(FLIGHT_MODE(NAV_WP_MODE) && navConfig()->general.waypoint_enforce_altitude)) {
         posControl.desiredState.pos.z = MIN(posControl.desiredState.pos.z, navConfig()->general.max_altitude);
 
-        if (navGetCurrentActualPositionAndVelocity()->pos.z >= navConfig()->general.max_altitude && desiredClimbRate >= 0.0f) {
-            posControl.desiredState.pos.z = navConfig()->general.max_altitude;
+        if (mode == ROC_TO_ALT_TARGET || (mode == ROC_TO_ALT_CONSTANT && desiredClimbRate < 0.0f)) {
+            return;
+        }
+
+        if (navGetCurrentActualPositionAndVelocity()->pos.z > navConfig()->general.max_altitude) {
+            posControl.desiredState.climbRateDemand = 0;
             posControl.flags.rocToAltMode = ROC_TO_ALT_TARGET;
         }
     }

From 49715f8c22ac154d7cfc0486fcf540cfbb3c8be8 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Sun, 19 Nov 2023 21:52:58 +0000
Subject: [PATCH 008/323] Improvements

---
 src/main/navigation/navigation.c         | 17 ++++++++++-------
 src/main/navigation/navigation_private.h |  1 -
 2 files changed, 10 insertions(+), 8 deletions(-)

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index eae0be28756..b10a7dc6fe4 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -1627,7 +1627,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
         case NAV_WP_ACTION_LAND:
             calculateAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]);
             posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos);
-            posControl.wpInitialAltitude = posControl.actualState.abs.pos.z;
             posControl.wpAltitudeReached = false;
             return NAV_FSM_EVENT_SUCCESS;       // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
 
@@ -1693,9 +1692,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
                     tmpWaypoint.y = posControl.activeWaypoint.pos.y;
                     setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING);
 
-                    // Use linear climb bewtween WPs until within 10% of total distance to current active WP
-                    float climbRate = posControl.actualState.velXY * (posControl.activeWaypoint.pos.z - posControl.wpInitialAltitude) /
-                                      (0.9f * posControl.wpInitialDistance);
+                    // Use linear climb between WPs arriving at WP altitude when within 10% of total distance to WP
+                    // Update climbrate until within 25% of total distance to WP, then hold constant
+                    static float climbRate = 0;
+                    if (posControl.wpDistance > 0.25f * posControl.wpInitialDistance) {
+                        climbRate = posControl.actualState.velXY * (posControl.activeWaypoint.pos.z - posControl.actualState.abs.pos.z) /
+                                    (posControl.wpDistance - 0.1f * posControl.wpInitialDistance);
+                    }
 
                     if (fabsf(climbRate) >= navConfig()->general.max_auto_climb_rate) {
                         climbRate = 0;
@@ -3099,12 +3102,12 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt
     if (navConfig()->general.max_altitude && !FLIGHT_MODE(NAV_RTH_MODE) && !(FLIGHT_MODE(NAV_WP_MODE) && navConfig()->general.waypoint_enforce_altitude)) {
         posControl.desiredState.pos.z = MIN(posControl.desiredState.pos.z, navConfig()->general.max_altitude);
 
-        if (mode == ROC_TO_ALT_TARGET || (mode == ROC_TO_ALT_CONSTANT && desiredClimbRate < 0.0f)) {
+        if (mode != ROC_TO_ALT_CONSTANT || (mode == ROC_TO_ALT_CONSTANT && desiredClimbRate < 0.0f)) {
             return;
         }
 
-        if (navGetCurrentActualPositionAndVelocity()->pos.z > navConfig()->general.max_altitude) {
-            posControl.desiredState.climbRateDemand = 0;
+        if (posControl.flags.isAdjustingAltitude) {
+            posControl.desiredState.pos.z = navConfig()->general.max_altitude;
             posControl.flags.rocToAltMode = ROC_TO_ALT_TARGET;
         }
     }
diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
index cf4698a24c8..3a669443835 100644
--- a/src/main/navigation/navigation_private.h
+++ b/src/main/navigation/navigation_private.h
@@ -423,7 +423,6 @@ typedef struct {
 #endif
     navWaypointPosition_t       activeWaypoint;             // Local position, current bearing and turn angle to next WP, filled on waypoint activation
     int8_t                      activeWaypointIndex;
-    float                       wpInitialAltitude;          // Altitude at start of WP
     float                       wpInitialDistance;          // Distance when starting flight to WP
     float                       wpDistance;                 // Distance to active WP
     timeMs_t                    wpReachedTime;              // Time the waypoint was reached

From d6de35bf8b334032c9eb44264ed6baa8e98967ae Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Wed, 6 Dec 2023 22:46:06 +0000
Subject: [PATCH 009/323] add control response setting

---
 docs/Settings.md                 | 10 ++++++++++
 src/main/fc/settings.yaml        |  6 ++++++
 src/main/flight/pid.c            |  9 +++++----
 src/main/flight/pid.h            |  2 ++
 src/main/navigation/navigation.c |  6 ++----
 5 files changed, 25 insertions(+), 8 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index 17384f92732..7b58d96cdbe 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -2842,6 +2842,16 @@ Enable the possibility to manually increase the throttle in auto throttle contro
 
 ---
 
+### nav_fw_alt_control_response
+
+Adjusts the deceleration response of fixed wing altitude control as the target altitude is approached. Decrease value to help avoid overshooting the target altitude.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 20 | 5 | 100 |
+
+---
+
 ### nav_fw_bank_angle
 
 Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 4f870fa8f7a..8ce69c23123 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -2087,6 +2087,12 @@ groups:
         field: bank_fw.pid[PID_POS_Z].D
         min: 0
         max: 255
+      - name: nav_fw_alt_control_response
+        description: "Adjusts the deceleration response of fixed wing altitude control as the target altitude is approached. Decrease value to help avoid overshooting the target altitude."
+        default_value: 20
+        field: fwAltControlResponseFactor
+        min: 5
+        max: 100
       - name: nav_fw_pos_xy_p
         description: "P gain of 2D trajectory PID controller. Play with this to get a straight line between waypoints or a straight RTH"
         default_value: 75
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index bc794735eb2..9bae1799e45 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -170,7 +170,7 @@ static EXTENDED_FASTRAM bool angleHoldIsLevel = false;
 static EXTENDED_FASTRAM float fixedWingLevelTrim;
 static EXTENDED_FASTRAM pidController_t fixedWingLevelTrimController;
 
-PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 6);
+PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 7);
 
 PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .bank_mc = {
@@ -231,9 +231,9 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
                 },
                 [PID_HEADING] = { SETTING_NAV_FW_HEADING_P_DEFAULT, 0, 0, 0 },
                 [PID_POS_Z] = {
-                    .P = SETTING_NAV_FW_POS_Z_P_DEFAULT,      // FW_POS_Z_P * 10
-                    .I = SETTING_NAV_FW_POS_Z_I_DEFAULT,      // FW_POS_Z_I * 10
-                    .D = SETTING_NAV_FW_POS_Z_D_DEFAULT,      // FW_POS_Z_D * 10
+                    .P = SETTING_NAV_FW_POS_Z_P_DEFAULT,      // FW_POS_Z_P * 100
+                    .I = SETTING_NAV_FW_POS_Z_I_DEFAULT,      // FW_POS_Z_I * 100
+                    .D = SETTING_NAV_FW_POS_Z_D_DEFAULT,      // FW_POS_Z_D * 100
                     .FF = 0,
                 },
                 [PID_POS_XY] = {
@@ -301,6 +301,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .fixedWingLevelTrim = SETTING_FW_LEVEL_PITCH_TRIM_DEFAULT,
         .fixedWingLevelTrimGain = SETTING_FW_LEVEL_PITCH_GAIN_DEFAULT,
 
+        .fwAltControlResponseFactor = SETTING_NAV_FW_ALT_CONTROL_RESPONSE_DEFAULT,
 #ifdef USE_SMITH_PREDICTOR
         .smithPredictorStrength = SETTING_SMITH_PREDICTOR_STRENGTH_DEFAULT,
         .smithPredictorDelay = SETTING_SMITH_PREDICTOR_DELAY_DEFAULT,
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index e0a8b9d310b..3f3658ae162 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -151,6 +151,8 @@ typedef struct pidProfile_s {
 
     float fixedWingLevelTrim;
     float fixedWingLevelTrimGain;
+
+    uint8_t fwAltControlResponseFactor;
 #ifdef USE_SMITH_PREDICTOR
     float smithPredictorStrength;
     float smithPredictorDelay;
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index b10a7dc6fe4..8fbe4c07854 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -3064,8 +3064,7 @@ float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros)
     if (STATE(MULTIROTOR)) {
         targetVel = getSqrtControllerVelocity(targetAltitude, deltaMicros);
     } else {
-        // 0.2f relates to climb rate starting to reduce from maxClimbRate at a distance of 5 x maxClimbRate from targetAltitude
-        targetVel = 0.2f * targetAltitudeError;
+        targetVel = pidProfile()->fwAltControlResponseFactor * targetAltitudeError / 100.0f;
     }
 
     if (emergLandingIsActive && targetAltitudeError > -50) {
@@ -3077,8 +3076,7 @@ float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros)
 
 void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode)
 {
-    /* ROC_TO_ALT_TARGET - constant climb rate until close to target altitude reducing to 0 when reached.
-     * Rate reduction starts at distance from target altitude of 5 x climb rate for Fixed wing.
+    /* ROC_TO_ALT_TARGET - constant climb rate until close to target altitude then reducing down as altitude is reached.
      * Any non zero climb rate sets the max allowed climb rate. Default max climb rate limits are used when set to 0.
      *
      * ROC_TO_ALT_RESET - similar to ROC_TO_ALT_TARGET except target altitude set to current altitude.

From 3378bcd8179296790969cf35ba0e6083280270f4 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Mon, 11 Dec 2023 10:48:52 +0000
Subject: [PATCH 010/323] change alt reset to alt current

---
 src/main/navigation/navigation.c             | 6 +++---
 src/main/navigation/navigation_fixedwing.c   | 2 +-
 src/main/navigation/navigation_multicopter.c | 2 +-
 src/main/navigation/navigation_private.h     | 2 +-
 4 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 8fbe4c07854..3ca2d126e99 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -1474,7 +1474,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, 0, ROC_TO_ALT_RESET);
+        updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT);
         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);
@@ -3079,12 +3079,12 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt
     /* ROC_TO_ALT_TARGET - constant climb rate until close to target altitude then reducing down as altitude is reached.
      * Any non zero climb rate sets the max allowed climb rate. Default max climb rate limits are used when set to 0.
      *
-     * ROC_TO_ALT_RESET - similar to ROC_TO_ALT_TARGET except target altitude set to current altitude.
+     * ROC_TO_ALT_CURRENT - similar to ROC_TO_ALT_TARGET except target altitude set to current altitude.
      * No climb rate or altitude target required.
      *
      * ROC_TO_ALT_CONSTANT - constant climb rate. Climb rate and direction required. Target alt not required. */
 
-    if (mode == ROC_TO_ALT_RESET) {
+    if (mode == ROC_TO_ALT_CURRENT) {
         posControl.desiredState.pos.z = navGetCurrentActualPositionAndVelocity()->pos.z;
     } else if (mode == ROC_TO_ALT_TARGET) {
         posControl.desiredState.pos.z = targetAltitude;
diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index f5c3fb159ae..3d66f7d7d32 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -123,7 +123,7 @@ bool adjustFixedWingAltitudeFromRCInput(void)
     else {
         // Adjusting finished - reset desired position to stay exactly where pilot released the stick
         if (posControl.flags.isAdjustingAltitude) {
-            updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
+            updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT);
         }
         return false;
     }
diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c
index b35d88c6093..d2bc457f340 100644
--- a/src/main/navigation/navigation_multicopter.c
+++ b/src/main/navigation/navigation_multicopter.c
@@ -162,7 +162,7 @@ bool adjustMulticopterAltitudeFromRCInput(void)
         else {
             // Adjusting finished - reset desired position to stay exactly where pilot released the stick
             if (posControl.flags.isAdjustingAltitude) {
-                updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
+                updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT);
             }
 
             return false;
diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
index 3a669443835..664787decff 100644
--- a/src/main/navigation/navigation_private.h
+++ b/src/main/navigation/navigation_private.h
@@ -60,7 +60,7 @@ typedef enum {
 } navSetWaypointFlags_t;
 
 typedef enum {
-    ROC_TO_ALT_RESET,
+    ROC_TO_ALT_CURRENT,
     ROC_TO_ALT_CONSTANT,
     ROC_TO_ALT_TARGET
 } climbRateToAltitudeControllerMode_e;

From 86afe137baa0577c91b5dcf0a2a88bc7b159900e Mon Sep 17 00:00:00 2001
From: Roman Lut <11955117+RomanLut@users.noreply.github.com>
Date: Thu, 14 Dec 2023 12:03:40 +0100
Subject: [PATCH 011/323] fixed: SITL accelerometer/mag data is not processed
 when using SITL with HITL plugin

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

diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index a61204dee90..3248e4a9bf7 100644
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -905,7 +905,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
     }
 
 #if defined(SITL_BUILD)
-    if (lockMainPID()) {
+    if (ARMING_FLAG(SIMULATOR_MODE_HITL) || lockMainPID()) {
 #endif
 
     gyroFilter();

From 93f98a089f3855752a287f40b5acc7f27e7289dc Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Sun, 10 Mar 2024 23:55:10 +0000
Subject: [PATCH 012/323] update

---
 docs/Settings.md                         | 10 ++++++
 src/main/cms/cms_menu_imu.c              |  2 ++
 src/main/fc/fc_msp.c                     | 44 +++++++++++++-----------
 src/main/fc/settings.yaml                | 10 ++++--
 src/main/navigation/navigation.c         |  9 ++---
 src/main/navigation/navigation.h         |  5 +--
 src/main/navigation/navigation_private.h |  3 +-
 7 files changed, 54 insertions(+), 29 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index 740a7d0914f..0efeaf8de15 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -2862,6 +2862,16 @@ Adjusts the deceleration response of fixed wing altitude control as the target a
 
 ---
 
+### nav_fw_auto_climb_rate
+
+Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s]
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 500 | 10 | 2000 |
+
+---
+
 ### nav_fw_bank_angle
 
 Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll
diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c
index 7612ec44181..45e9b34b94b 100644
--- a/src/main/cms/cms_menu_imu.c
+++ b/src/main/cms/cms_menu_imu.c
@@ -198,6 +198,8 @@ static const OSD_Entry cmsx_menuPidAltMagEntries[] =
 {
     OSD_LABEL_DATA_ENTRY("-- ALT&MAG --", profileIndexString),
 
+    OSD_SETTING_ENTRY("FW ALT RESPONSE", SETTING_NAV_FW_ALT_CONTROL_RESPONSE),
+
     OTHER_PIDFF_ENTRY("ALT P", &cmsx_pidPosZ.P),
     OTHER_PIDFF_ENTRY("ALT I", &cmsx_pidPosZ.I),
     OTHER_PIDFF_ENTRY("ALT D", &cmsx_pidPosZ.D),
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 2c3f40dfe45..5e9b498874c 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -1113,7 +1113,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);
@@ -1340,9 +1340,9 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
     case MSP_NAV_POSHOLD:
         sbufWriteU8(dst, navConfig()->general.flags.user_control_mode);
         sbufWriteU16(dst, navConfig()->general.max_auto_speed);
-        sbufWriteU16(dst, navConfig()->mc.max_auto_climb_rate);
+        sbufWriteU16(dst, mixerConfig()->platformType == PLATFORM_AIRPLANE ? navConfig()->fw.max_auto_climb_rate : navConfig()->mc.max_auto_climb_rate);
         sbufWriteU16(dst, navConfig()->general.max_manual_speed);
-        sbufWriteU16(dst, mixerConfig()->platformType != PLATFORM_AIRPLANE ? navConfig()->mc.max_manual_climb_rate:navConfig()->fw.max_manual_climb_rate);
+        sbufWriteU16(dst, mixerConfig()->platformType == PLATFORM_AIRPLANE ? navConfig()->fw.max_manual_climb_rate : navConfig()->mc.max_manual_climb_rate);
         sbufWriteU8(dst, navConfig()->mc.max_bank_angle);
         sbufWriteU8(dst, navConfig()->mc.althold_throttle_type);
         sbufWriteU16(dst, currentBatteryProfile->nav.mc.hover_throttle);
@@ -1590,7 +1590,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
                 sbufWriteU8(dst, timerHardware[i].usageFlags);
             }
         break;
-    
+
     case MSP2_INAV_MC_BRAKING:
 #ifdef USE_MR_BRAKING_MODE
         sbufWriteU16(dst, navConfig()->mc.braking_speed_threshold);
@@ -2394,12 +2394,16 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
         if (dataSize == 13) {
             navConfigMutable()->general.flags.user_control_mode = sbufReadU8(src);
             navConfigMutable()->general.max_auto_speed = sbufReadU16(src);
-            navConfigMutable()->mc.max_auto_climb_rate = sbufReadU16(src);
+            if (mixerConfig()->platformType == PLATFORM_AIRPLANE) {
+                navConfigMutable()->fw.max_auto_climb_rate = sbufReadU16(src);
+            } else {
+                navConfigMutable()->mc.max_auto_climb_rate = sbufReadU16(src);
+            }
             navConfigMutable()->general.max_manual_speed = sbufReadU16(src);
-            if (mixerConfig()->platformType != PLATFORM_AIRPLANE) {
-                navConfigMutable()->mc.max_manual_climb_rate = sbufReadU16(src);
-            }else{
+            if (mixerConfig()->platformType == PLATFORM_AIRPLANE) {
                 navConfigMutable()->fw.max_manual_climb_rate = sbufReadU16(src);
+            } else {
+                navConfigMutable()->mc.max_manual_climb_rate = sbufReadU16(src);
             }
             navConfigMutable()->mc.max_bank_angle = sbufReadU8(src);
             navConfigMutable()->mc.althold_throttle_type = sbufReadU8(src);
@@ -2729,7 +2733,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
 
     case MSP_SET_WP:
         if (dataSize == 21) {
-            
+
             const uint8_t msp_wp_no = sbufReadU8(src);     // get the waypoint number
             navWaypoint_t msp_wp;
             msp_wp.action = sbufReadU8(src);    // action
@@ -2943,7 +2947,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;
 
@@ -3233,7 +3237,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             fwAutolandApproachConfigMutable(i)->approachAlt = sbufReadU32(src);
             fwAutolandApproachConfigMutable(i)->landAlt = sbufReadU32(src);
             fwAutolandApproachConfigMutable(i)->approachDirection = sbufReadU8(src);
-             
+
             int16_t head1 = 0, head2 = 0;
             if (sbufReadI16Safe(&head1, src)) {
                 fwAutolandApproachConfigMutable(i)->landApproachHeading1 = head1;
@@ -3283,12 +3287,12 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.correctionEnd = sbufReadU8(src);
             ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.weightCenter = sbufReadU8(src);
             ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.weightEnd = sbufReadU8(src);
-            
+
         } else {
             return MSP_RESULT_ERROR;
         }
 
-        break;    
+        break;
 
 #endif
 #ifdef USE_PROGRAMMING_FRAMEWORK
@@ -3582,7 +3586,7 @@ void mspWriteSimulatorOSD(sbuf_t *dst)
 	static uint8_t osdPos_x = 0;
 
     //indicate new format hitl 1.4.0
-	sbufWriteU8(dst, 255);  
+	sbufWriteU8(dst, 255);
 
 	if (isOSDTypeSupportedBySimulator())
 	{
@@ -3788,7 +3792,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;
@@ -3883,7 +3887,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;
@@ -3891,7 +3895,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;
@@ -3922,7 +3926,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);
                     }
                 }
 
@@ -3997,8 +4001,8 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
             *ret = MSP_RESULT_ERROR;
         }
         break;
-#endif 
-    
+#endif
+
     default:
         // Not handled
         return false;
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 61b0c23bbe4..c215f402a7d 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -2334,7 +2334,7 @@ groups:
         field: w_z_surface_p
         min: 0
         max: 100
-        default_value: 3.5 
+        default_value: 3.5
       - name: inav_w_z_surface_v
         field: w_z_surface_v
         min: 0
@@ -2801,6 +2801,12 @@ groups:
         field: fw.max_bank_angle
         min: 5
         max: 80
+      - name: nav_fw_auto_climb_rate
+        description: "Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s]"
+        default_value: 500
+        field: fw.max_auto_climb_rate
+        min: 10
+        max: 2000
       - name: nav_fw_manual_climb_rate
         description: "Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s]"
         default_value: 300
@@ -4143,7 +4149,7 @@ groups:
     type: navFwAutolandConfig_t
     headers: ["navigation/navigation.h"]
     condition: USE_FW_AUTOLAND
-    members: 
+    members:
       - name: nav_fw_land_approach_length
         description: "Length of the final approach"
         default_value: 35000
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index b721ee4817d..51047974ca8 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -204,6 +204,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
     // Fixed wing
     .fw = {
         .max_bank_angle = SETTING_NAV_FW_BANK_ANGLE_DEFAULT,                    // degrees
+        .max_auto_climb_rate = SETTING_NAV_FW_AUTO_CLIMB_RATE_DEFAULT,          // 5 m/s
         .max_manual_climb_rate = SETTING_NAV_FW_MANUAL_CLIMB_RATE_DEFAULT,      // 3 m/s
         .max_climb_angle = SETTING_NAV_FW_CLIMB_ANGLE_DEFAULT,                  // degrees
         .max_dive_angle = SETTING_NAV_FW_DIVE_ANGLE_DEFAULT,                    // degrees
@@ -1891,7 +1892,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
                                     (posControl.wpDistance - 0.1f * posControl.wpInitialDistance);
                     }
 
-                    if (fabsf(climbRate) >= navConfig()->general.max_auto_climb_rate) {
+                    if (fabsf(climbRate) >= (STATE(AIRPLANE) ? navConfig()->fw.max_auto_climb_rate : navConfig()->mc.max_auto_climb_rate)) {
                         climbRate = 0;
                     }
                     updateClimbRateToAltitudeController(climbRate, posControl.activeWaypoint.pos.z, ROC_TO_ALT_TARGET);
@@ -2212,7 +2213,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_CLIMB_TO_LOI
     }
 
     if (ABS(getEstimatedActualPosition(Z) - posControl.fwLandState.landAproachAltAgl) < (navConfig()->general.waypoint_enforce_altitude > 0 ? navConfig()->general.waypoint_enforce_altitude : FW_LAND_LOITER_ALT_TOLERANCE)) {
-        updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
+        updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT);
         posControl.fwLandState.landState = FW_AUTOLAND_STATE_LOITER;
         return NAV_FSM_EVENT_SUCCESS;
     }
@@ -3379,13 +3380,13 @@ float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros)
 {
     const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding();
 
-    float maxClimbRate = navConfig()->general.max_auto_climb_rate;
+    float maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_auto_climb_rate : navConfig()->fw.max_auto_climb_rate;
     if (posControl.desiredState.climbRateDemand) {
         maxClimbRate = ABS(posControl.desiredState.climbRateDemand);
     } else if (emergLandingIsActive) {
         maxClimbRate = navConfig()->general.emerg_descent_rate;
     } else if (posControl.flags.isAdjustingAltitude) {
-        maxClimbRate = navConfig()->general.max_manual_climb_rate;
+        maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_manual_climb_rate : navConfig()->fw.max_manual_climb_rate;
     }
 
     const float targetAltitudeError = targetAltitude - navGetCurrentActualPositionAndVelocity()->pos.z;
diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
index 84e5390d43e..d02a69d060c 100644
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@@ -265,7 +265,7 @@ typedef struct positionEstimationConfig_s {
 
 #ifdef USE_GPS_FIX_ESTIMATION
     uint8_t allow_gps_fix_estimation;
-#endif    
+#endif
 } positionEstimationConfig_t;
 
 PG_DECLARE(positionEstimationConfig_t, positionEstimationConfig);
@@ -329,7 +329,7 @@ typedef struct navConfig_s {
 
     struct {
         uint8_t  max_bank_angle;                // multicopter max banking angle (deg)
-        uint16_t max_auto_climb_rate;           // max vertical speed limitation cm/sec
+        uint16_t max_auto_climb_rate;           // max vertical speed limitation nav modes cm/sec
         uint16_t max_manual_climb_rate;         // manual velocity control max vertical speed
 
 #ifdef USE_MR_BRAKING_MODE
@@ -351,6 +351,7 @@ typedef struct navConfig_s {
 
     struct {
         uint8_t  max_bank_angle;             // Fixed wing max banking angle (deg)
+        uint16_t max_auto_climb_rate;        // max vertical speed limitation nav modes cm/sec
         uint16_t max_manual_climb_rate;      // manual velocity control max vertical speed
         uint8_t  max_climb_angle;            // Fixed wing max banking angle (deg)
         uint8_t  max_dive_angle;             // Fixed wing max banking angle (deg)
diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
index f0d6a5c7068..95953c948cf 100644
--- a/src/main/navigation/navigation_private.h
+++ b/src/main/navigation/navigation_private.h
@@ -288,7 +288,7 @@ typedef enum {
     NAV_STATE_CRUISE_INITIALIZE,
     NAV_STATE_CRUISE_IN_PROGRESS,
     NAV_STATE_CRUISE_ADJUSTING,
-    
+
     NAV_STATE_FW_LANDING_CLIMB_TO_LOITER,
     NAV_STATE_FW_LANDING_LOITER,
     NAV_STATE_FW_LANDING_APPROACH,
@@ -462,6 +462,7 @@ typedef struct {
 #endif
     navWaypointPosition_t       activeWaypoint;             // Local position, current bearing and turn angle to next WP, filled on waypoint activation
     int8_t                      activeWaypointIndex;
+    float                       wpInitialAltitude;          // Altitude at start of WP
     float                       wpInitialDistance;          // Distance when starting flight to WP
     float                       wpDistance;                 // Distance to active WP
     timeMs_t                    wpReachedTime;              // Time the waypoint was reached

From a29d0a464b8d7ec571f0543701fd6eb8130cad13 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Mon, 11 Mar 2024 12:03:57 +0000
Subject: [PATCH 013/323] Update sitl.cmake

---
 cmake/sitl.cmake | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/cmake/sitl.cmake b/cmake/sitl.cmake
index 78697f98a9d..ee43aa9a93a 100644
--- a/cmake/sitl.cmake
+++ b/cmake/sitl.cmake
@@ -34,7 +34,7 @@ set(SITL_LINK_OPTIONS
     -Wl,-L${STM32_LINKER_DIR}
 )
 
-if(${WIN32} OR ${CYGWIN})
+if(${CYGWIN})
     set(SITL_LINK_OPTIONS ${SITL_LINK_OPTIONS} "-static-libgcc")
 endif()
 
@@ -131,7 +131,7 @@ function (target_sitl name)
         target_link_options(${exe_target} PRIVATE -T${script_path})
     endif()
 
-    if(${WIN32} OR ${CYGWIN})
+    if(${CYGWIN})
         set(exe_filename ${CMAKE_BINARY_DIR}/${binary_name}.exe)
     else()
         set(exe_filename ${CMAKE_BINARY_DIR}/${binary_name})

From a625e83534d42245875c4bee93981e2859d995fb Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sat, 30 Mar 2024 22:01:40 +0100
Subject: [PATCH 014/323] Increase allowed speeds in NAV modes on multirotors

---
 src/main/fc/settings.yaml | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 106a830decc..52c727b846f 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -2510,7 +2510,7 @@ groups:
         table: nav_fw_wp_turn_smoothing
       - name: nav_auto_speed
         description: "Speed in fully autonomous modes (RTH, WP) [cm/s]. Used for WP mode when no specific WP speed set. [Multirotor only]"
-        default_value: 300
+        default_value: 600
         field: general.auto_speed
         min: 10
         max: 2000
@@ -2522,13 +2522,13 @@ groups:
         max: 50
       - name: nav_max_auto_speed
         description: "Maximum speed allowed in fully autonomous modes (RTH, WP) [cm/s] [Multirotor only]"
-        default_value: 1000
+        default_value: 1200
         field: general.max_auto_speed
         min: 10
         max: 2000
       - name: nav_manual_speed
         description: "Maximum speed allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only]"
-        default_value: 500
+        default_value: 1000
         field: general.max_manual_speed
         min: 10
         max: 2000

From 890147770019da9f6dd2e2a2c6d49b7fbb548026 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sat, 30 Mar 2024 22:05:58 +0100
Subject: [PATCH 015/323] Increase bank angles

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

diff --git a/docs/Settings.md b/docs/Settings.md
index fefcc927f20..40490808c83 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -2388,7 +2388,7 @@ Maximum inclination in level (angle) mode (PITCH axis). 100=10°
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 300 | 100 | 900 |
+| 450 | 100 | 900 |
 
 ---
 
@@ -2398,7 +2398,7 @@ Maximum inclination in level (angle) mode (ROLL axis). 100=10°
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 300 | 100 | 900 |
+| 450 | 100 | 900 |
 
 ---
 
@@ -2758,7 +2758,7 @@ Speed in fully autonomous modes (RTH, WP) [cm/s]. Used for WP mode when no speci
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 300 | 10 | 2000 |
+| 600 | 10 | 2000 |
 
 ---
 
@@ -3418,7 +3418,7 @@ Maximum speed allowed when processing pilot input for POSHOLD/CRUISE control mod
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 500 | 10 | 2000 |
+| 1000 | 10 | 2000 |
 
 ---
 
@@ -3438,7 +3438,7 @@ Maximum speed allowed in fully autonomous modes (RTH, WP) [cm/s] [Multirotor onl
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 1000 | 10 | 2000 |
+| 1200 | 10 | 2000 |
 
 ---
 
@@ -3478,7 +3478,7 @@ Maximum banking angle (deg) that multicopter navigation is allowed to set. Machi
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 30 | 15 | 45 |
+| 40 | 15 | 45 |
 
 ---
 
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 52c727b846f..71964891ce9 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -1913,13 +1913,13 @@ groups:
         max: RPYL_PID_MAX
       - name: max_angle_inclination_rll
         description: "Maximum inclination in level (angle) mode (ROLL axis). 100=10°"
-        default_value: 300
+        default_value: 450
         field: max_angle_inclination[FD_ROLL]
         min: 100
         max: 900
       - name: max_angle_inclination_pit
         description: "Maximum inclination in level (angle) mode (PITCH axis). 100=10°"
-        default_value: 300
+        default_value: 450
         field: max_angle_inclination[FD_PITCH]
         min: 100
         max: 900
@@ -2704,7 +2704,7 @@ groups:
         max: 120
       - name: nav_mc_bank_angle
         description: "Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude"
-        default_value: 30
+        default_value: 40
         field: mc.max_bank_angle
         min: 15
         max: 45

From 93a636d3f1d8fec4af5c6effdc6aee3de06171dd Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 31 Mar 2024 18:21:08 +0200
Subject: [PATCH 016/323] Compute RMS of filtered gyro data

---
 .vscode/c_cpp_properties.json     |  3 +-
 cmake/at32.cmake                  |  3 ++
 cmake/stm32.cmake                 |  3 ++
 src/main/CMakeLists.txt           |  2 +
 src/main/build/debug.h            |  1 +
 src/main/fc/fc_tasks.c            | 13 ++++++
 src/main/fc/settings.yaml         |  2 +-
 src/main/flight/adaptive_filter.c | 77 +++++++++++++++++++++++++++++++
 src/main/flight/adaptive_filter.h | 32 +++++++++++++
 src/main/scheduler/scheduler.h    |  3 ++
 src/main/sensors/gyro.c           |  3 ++
 src/main/target/common.h          |  6 +++
 12 files changed, 146 insertions(+), 2 deletions(-)
 create mode 100644 src/main/flight/adaptive_filter.c
 create mode 100644 src/main/flight/adaptive_filter.h

diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
index 6e7d914b25a..a2c5f582e1c 100755
--- a/.vscode/c_cpp_properties.json
+++ b/.vscode/c_cpp_properties.json
@@ -57,7 +57,8 @@
                 "USE_SDCARD_SDIO",
                 "USE_SDCARD",
                 "USE_Q_TUNE",
-                "USE_GYRO_FFT_FILTER"
+                "USE_GYRO_FFT_FILTER",
+                "USE_ADAPTIVE_FILTER",
             ],
             "configurationProvider": "ms-vscode.cmake-tools"
         }
diff --git a/cmake/at32.cmake b/cmake/at32.cmake
index 54e178deb7b..79efcfc0f17 100644
--- a/cmake/at32.cmake
+++ b/cmake/at32.cmake
@@ -18,6 +18,7 @@ set(CMSIS_DSP_SRC
     BasicMathFunctions/arm_scale_f32.c
     BasicMathFunctions/arm_sub_f32.c
     BasicMathFunctions/arm_mult_f32.c
+    BasicMathFunctions/arm_offset_f32.c
     TransformFunctions/arm_rfft_fast_f32.c
     TransformFunctions/arm_cfft_f32.c
     TransformFunctions/arm_rfft_fast_init_f32.c
@@ -26,6 +27,8 @@ set(CMSIS_DSP_SRC
     CommonTables/arm_common_tables.c
     ComplexMathFunctions/arm_cmplx_mag_f32.c
     StatisticsFunctions/arm_max_f32.c
+    StatisticsFunctions/arm_rms_f32.c
+    StatisticsFunctions/arm_mean_f32.c
 )
 list(TRANSFORM CMSIS_DSP_SRC PREPEND "${CMSIS_DSP_DIR}/Source/")
 
diff --git a/cmake/stm32.cmake b/cmake/stm32.cmake
index f02185e9aff..256f8d09bef 100644
--- a/cmake/stm32.cmake
+++ b/cmake/stm32.cmake
@@ -19,6 +19,7 @@ set(CMSIS_DSP_SRC
     BasicMathFunctions/arm_scale_f32.c
     BasicMathFunctions/arm_sub_f32.c
     BasicMathFunctions/arm_mult_f32.c
+    BasicMathFunctions/arm_offset_f32.c
     TransformFunctions/arm_rfft_fast_f32.c
     TransformFunctions/arm_cfft_f32.c
     TransformFunctions/arm_rfft_fast_init_f32.c
@@ -27,6 +28,8 @@ set(CMSIS_DSP_SRC
     CommonTables/arm_common_tables.c
     ComplexMathFunctions/arm_cmplx_mag_f32.c
     StatisticsFunctions/arm_max_f32.c
+    StatisticsFunctions/arm_rms_f32.c
+    StatisticsFunctions/arm_mean_f32.c
 )
 list(TRANSFORM CMSIS_DSP_SRC PREPEND "${CMSIS_DSP_DIR}/Source/")
 
diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt
index 98a43854cf2..7b3a22006f7 100755
--- a/src/main/CMakeLists.txt
+++ b/src/main/CMakeLists.txt
@@ -343,6 +343,8 @@ main_sources(COMMON_SRC
     flight/dynamic_lpf.h
     flight/ez_tune.c
     flight/ez_tune.h
+    flight/adaptive_filter.c
+    flight/adaptive_filter.h
 
     io/adsb.c
     io/beeper.c
diff --git a/src/main/build/debug.h b/src/main/build/debug.h
index 3a09da50daf..60b22df27bc 100644
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@@ -72,5 +72,6 @@ typedef enum {
     DEBUG_RATE_DYNAMICS,
     DEBUG_LANDING,
     DEBUG_POS_EST,
+    DEBUG_ADAPTIVE_FILTER,
     DEBUG_COUNT
 } debugType_e;
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index 246d65c0a7e..74be2f42258 100755
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -51,6 +51,7 @@
 #include "flight/rpm_filter.h"
 #include "flight/servos.h"
 #include "flight/wind_estimator.h"
+#include "flight/adaptive_filter.h"
 
 #include "navigation/navigation.h"
 
@@ -421,6 +422,10 @@ void fcTasksInit(void)
 #if defined(USE_SMARTPORT_MASTER)
     setTaskEnabled(TASK_SMARTPORT_MASTER, true);
 #endif
+
+#ifdef USE_ADAPTIVE_FILTER
+    setTaskEnabled(TASK_ADAPTIVE_FILTER, true);
+#endif
 }
 
 cfTask_t cfTasks[TASK_COUNT] = {
@@ -672,4 +677,12 @@ cfTask_t cfTasks[TASK_COUNT] = {
         .desiredPeriod = TASK_PERIOD_HZ(TASK_AUX_RATE_HZ),          // 100Hz @10ms
         .staticPriority = TASK_PRIORITY_HIGH,
     },
+#ifdef USE_ADAPTIVE_FILTER
+    [TASK_ADAPTIVE_FILTER] = {
+        .taskName = "ADAPTIVE_FILTER",
+        .taskFunc = adaptiveFilterTask,
+        .desiredPeriod = TASK_PERIOD_HZ(ADAPTIVE_FILTER_RATE_HZ),          // 100Hz @10ms
+        .staticPriority = TASK_PRIORITY_LOW,
+    },
+#endif
 };
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 106a830decc..8c2ed270d35 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -88,7 +88,7 @@ tables:
     values: ["NONE", "AGL", "FLOW_RAW", "FLOW", "ALWAYS", "SAG_COMP_VOLTAGE",
       "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
       "NAV_YAW", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "ALTITUDE",
-      "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST"]
+      "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST", "ADAPTIVE_FILTER"]
   - name: aux_operator
     values: ["OR", "AND"]
     enum: modeActivationOperator_e
diff --git a/src/main/flight/adaptive_filter.c b/src/main/flight/adaptive_filter.c
new file mode 100644
index 00000000000..cb057ad5cfb
--- /dev/null
+++ b/src/main/flight/adaptive_filter.c
@@ -0,0 +1,77 @@
+/*
+ * 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"
+
+#ifdef USE_ADAPTIVE_FILTER
+
+#include <stdlib.h>
+#include "flight/adaptive_filter.h"
+#include "arm_math.h"
+#include <math.h>
+#include "common/maths.h"
+#include "common/axis.h"
+#include "build/debug.h"
+
+static float32_t adaptiveFilterSamples[XYZ_AXIS_COUNT][ADAPTIVE_FILTER_BUFFER_SIZE];
+static uint8_t adaptiveFilterSampleIndex = 0;
+
+void adaptiveFilterPush(const flight_dynamics_index_t index, const float value) {
+    //Push new sample to the buffer so later we can compute RMS and other measures
+    adaptiveFilterSamples[index][adaptiveFilterSampleIndex] = value;
+    adaptiveFilterSampleIndex = (adaptiveFilterSampleIndex + 1) % ADAPTIVE_FILTER_BUFFER_SIZE;
+}
+
+void adaptiveFilterTask(timeUs_t currentTimeUs) {
+    UNUSED(currentTimeUs);
+
+    //Compute RMS for each axis
+    for (flight_dynamics_index_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+
+        //Copy axis samples to a temporary buffer
+        float32_t tempBuffer[ADAPTIVE_FILTER_BUFFER_SIZE];
+        //Use memcpy to copy the samples to the temporary buffer
+        memcpy(tempBuffer, adaptiveFilterSamples[axis], sizeof(adaptiveFilterSamples[axis]));
+
+        //Use arm_mean_f32 to compute the mean of the samples
+        float32_t mean;
+        arm_mean_f32(tempBuffer, ADAPTIVE_FILTER_BUFFER_SIZE, &mean);
+
+        float32_t normalizedBuffer[ADAPTIVE_FILTER_BUFFER_SIZE];
+
+        //Use arm_offset_f32 to subtract the mean from the samples
+        arm_offset_f32(tempBuffer, -mean, normalizedBuffer, ADAPTIVE_FILTER_BUFFER_SIZE);
+
+        //Compute RMS from normalizedBuffer using arm_rms_f32
+        float32_t rms;
+        arm_rms_f32(normalizedBuffer, ADAPTIVE_FILTER_BUFFER_SIZE, &rms);
+
+        DEBUG_SET(DEBUG_ADAPTIVE_FILTER, axis, rms * 1000.0f);
+    }
+
+
+}
+
+
+#endif /* USE_ADAPTIVE_FILTER */
\ No newline at end of file
diff --git a/src/main/flight/adaptive_filter.h b/src/main/flight/adaptive_filter.h
new file mode 100644
index 00000000000..9999f17105d
--- /dev/null
+++ b/src/main/flight/adaptive_filter.h
@@ -0,0 +1,32 @@
+/*
+ * 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 "common/axis.h"
+#include "common/time.h"
+
+#define ADAPTIVE_FILTER_BUFFER_SIZE 32
+#define ADAPTIVE_FILTER_RATE_HZ 100
+
+void adaptiveFilterPush(const flight_dynamics_index_t index, const float value);
+void adaptiveFilterTask(timeUs_t currentTimeUs);
diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h
index 0d91876cb72..c1e0ab6bb53 100755
--- a/src/main/scheduler/scheduler.h
+++ b/src/main/scheduler/scheduler.h
@@ -121,6 +121,9 @@ typedef enum {
 #endif
 #ifdef USE_IRLOCK
     TASK_IRLOCK,
+#endif
+#ifdef USE_ADAPTIVE_FILTER
+    TASK_ADAPTIVE_FILTER,
 #endif
     /* Count of real tasks */
     TASK_COUNT,
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index a586ce6df6b..5873c9d7950 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -68,6 +68,7 @@
 #include "flight/gyroanalyse.h"
 #include "flight/rpm_filter.h"
 #include "flight/kalman.h"
+#include "flight/adaptive_filter.h"
 
 #ifdef USE_HARDWARE_REVISION_DETECTION
 #include "hardware_revision.h"
@@ -453,6 +454,8 @@ void FAST_CODE NOINLINE gyroFilter(void)
 
         gyroADCf = gyroLpf2ApplyFn((filter_t *) &gyroLpf2State[axis], gyroADCf);
 
+        adaptiveFilterPush(axis, gyroADCf);
+
 #ifdef USE_DYNAMIC_FILTERS
         if (dynamicGyroNotchState.enabled) {
             gyroDataAnalysePush(&gyroAnalyseState, axis, gyroADCf);
diff --git a/src/main/target/common.h b/src/main/target/common.h
index 8bbaadef718..63a03b59851 100644
--- a/src/main/target/common.h
+++ b/src/main/target/common.h
@@ -203,3 +203,9 @@
 #define MAX_MIXER_PROFILE_COUNT 1
 #endif
 #define USE_EZ_TUNE
+
+#ifdef STM32H7
+
+#define USE_ADAPTIVE_FILTER
+
+#endif

From e99563deaebb0a02f4f226f2fcf8e4ccdb46a378 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 31 Mar 2024 21:34:59 +0200
Subject: [PATCH 017/323] Add a filter

---
 src/main/flight/adaptive_filter.c | 20 ++++++++++++++++++--
 src/main/flight/adaptive_filter.h |  1 +
 2 files changed, 19 insertions(+), 2 deletions(-)

diff --git a/src/main/flight/adaptive_filter.c b/src/main/flight/adaptive_filter.c
index cb057ad5cfb..31078b95ba9 100644
--- a/src/main/flight/adaptive_filter.c
+++ b/src/main/flight/adaptive_filter.c
@@ -32,10 +32,15 @@
 #include <math.h>
 #include "common/maths.h"
 #include "common/axis.h"
+#include "common/filter.h"
 #include "build/debug.h"
 
-static float32_t adaptiveFilterSamples[XYZ_AXIS_COUNT][ADAPTIVE_FILTER_BUFFER_SIZE];
-static uint8_t adaptiveFilterSampleIndex = 0;
+STATIC_FASTRAM float32_t adaptiveFilterSamples[XYZ_AXIS_COUNT][ADAPTIVE_FILTER_BUFFER_SIZE];
+STATIC_FASTRAM uint8_t adaptiveFilterSampleIndex = 0;
+
+STATIC_FASTRAM pt1Filter_t rmsFilter[XYZ_AXIS_COUNT];
+
+STATIC_FASTRAM uint8_t adaptiveFilterInitialized = 0;
 
 void adaptiveFilterPush(const flight_dynamics_index_t index, const float value) {
     //Push new sample to the buffer so later we can compute RMS and other measures
@@ -46,6 +51,14 @@ void adaptiveFilterPush(const flight_dynamics_index_t index, const float value)
 void adaptiveFilterTask(timeUs_t currentTimeUs) {
     UNUSED(currentTimeUs);
 
+    if (!adaptiveFilterInitialized) {
+        //Initialize the filter
+        for (flight_dynamics_index_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+            pt1FilterInit(&rmsFilter[axis], ADAPTIVE_FILTER_LPF_HZ, 1.0f / ADAPTIVE_FILTER_RATE_HZ);
+        }
+        adaptiveFilterInitialized = 1;
+    }
+
     //Compute RMS for each axis
     for (flight_dynamics_index_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 
@@ -67,7 +80,10 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) {
         float32_t rms;
         arm_rms_f32(normalizedBuffer, ADAPTIVE_FILTER_BUFFER_SIZE, &rms);
 
+        float32_t filteredRms = pt1FilterApply(&rmsFilter[axis], rms);
+
         DEBUG_SET(DEBUG_ADAPTIVE_FILTER, axis, rms * 1000.0f);
+        DEBUG_SET(DEBUG_ADAPTIVE_FILTER, axis + XYZ_AXIS_COUNT, filteredRms * 1000.0f);
     }
 
 
diff --git a/src/main/flight/adaptive_filter.h b/src/main/flight/adaptive_filter.h
index 9999f17105d..1b61c769a97 100644
--- a/src/main/flight/adaptive_filter.h
+++ b/src/main/flight/adaptive_filter.h
@@ -27,6 +27,7 @@
 
 #define ADAPTIVE_FILTER_BUFFER_SIZE 32
 #define ADAPTIVE_FILTER_RATE_HZ 100
+#define ADAPTIVE_FILTER_LPF_HZ 1
 
 void adaptiveFilterPush(const flight_dynamics_index_t index, const float value);
 void adaptiveFilterTask(timeUs_t currentTimeUs);

From 4c46e519c518711f507adf6d076631ccd6bd9c70 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 31 Mar 2024 22:47:06 +0200
Subject: [PATCH 018/323] Change formating

---
 src/main/flight/adaptive_filter.c | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/main/flight/adaptive_filter.c b/src/main/flight/adaptive_filter.c
index 31078b95ba9..2ec4e9780bc 100644
--- a/src/main/flight/adaptive_filter.c
+++ b/src/main/flight/adaptive_filter.c
@@ -86,7 +86,6 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) {
         DEBUG_SET(DEBUG_ADAPTIVE_FILTER, axis + XYZ_AXIS_COUNT, filteredRms * 1000.0f);
     }
 
-
 }
 
 

From 64cfdba5befa792d806aa87bd4e57f62cbefc4bf Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 2 Apr 2024 13:02:09 +0200
Subject: [PATCH 019/323] Use HPF on samples instead of buffer mean value

---
 src/main/flight/adaptive_filter.c | 30 +++++++++++++++++++-----------
 src/main/flight/adaptive_filter.h |  1 +
 2 files changed, 20 insertions(+), 11 deletions(-)

diff --git a/src/main/flight/adaptive_filter.c b/src/main/flight/adaptive_filter.c
index 2ec4e9780bc..d402369c910 100644
--- a/src/main/flight/adaptive_filter.c
+++ b/src/main/flight/adaptive_filter.c
@@ -34,17 +34,34 @@
 #include "common/axis.h"
 #include "common/filter.h"
 #include "build/debug.h"
+#include "fc/config.h"
 
 STATIC_FASTRAM float32_t adaptiveFilterSamples[XYZ_AXIS_COUNT][ADAPTIVE_FILTER_BUFFER_SIZE];
 STATIC_FASTRAM uint8_t adaptiveFilterSampleIndex = 0;
 
 STATIC_FASTRAM pt1Filter_t rmsFilter[XYZ_AXIS_COUNT];
+STATIC_FASTRAM pt1Filter_t hpfFilter[XYZ_AXIS_COUNT];
 
 STATIC_FASTRAM uint8_t adaptiveFilterInitialized = 0;
+STATIC_FASTRAM uint8_t hpfFilterInitialized = 0;
 
+/**
+ * This function is called at pid rate, so has to be initialized at PID loop frequency
+*/
 void adaptiveFilterPush(const flight_dynamics_index_t index, const float value) {
+
+    if (!hpfFilterInitialized) {
+        //Initialize the filter
+        for (flight_dynamics_index_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+            pt1FilterInit(&hpfFilter[axis], ADAPTIVE_FILTER_HPF_HZ, US2S(getLooptime()));
+        }
+        hpfFilterInitialized = 1;
+    }
+
+    const float filteredGyro = value - pt1FilterApply(&hpfFilter[index], value);
+
     //Push new sample to the buffer so later we can compute RMS and other measures
-    adaptiveFilterSamples[index][adaptiveFilterSampleIndex] = value;
+    adaptiveFilterSamples[index][adaptiveFilterSampleIndex] = filteredGyro;
     adaptiveFilterSampleIndex = (adaptiveFilterSampleIndex + 1) % ADAPTIVE_FILTER_BUFFER_SIZE;
 }
 
@@ -67,18 +84,9 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) {
         //Use memcpy to copy the samples to the temporary buffer
         memcpy(tempBuffer, adaptiveFilterSamples[axis], sizeof(adaptiveFilterSamples[axis]));
 
-        //Use arm_mean_f32 to compute the mean of the samples
-        float32_t mean;
-        arm_mean_f32(tempBuffer, ADAPTIVE_FILTER_BUFFER_SIZE, &mean);
-
-        float32_t normalizedBuffer[ADAPTIVE_FILTER_BUFFER_SIZE];
-
-        //Use arm_offset_f32 to subtract the mean from the samples
-        arm_offset_f32(tempBuffer, -mean, normalizedBuffer, ADAPTIVE_FILTER_BUFFER_SIZE);
-
         //Compute RMS from normalizedBuffer using arm_rms_f32
         float32_t rms;
-        arm_rms_f32(normalizedBuffer, ADAPTIVE_FILTER_BUFFER_SIZE, &rms);
+        arm_rms_f32(tempBuffer, ADAPTIVE_FILTER_BUFFER_SIZE, &rms);
 
         float32_t filteredRms = pt1FilterApply(&rmsFilter[axis], rms);
 
diff --git a/src/main/flight/adaptive_filter.h b/src/main/flight/adaptive_filter.h
index 1b61c769a97..9a9d4dc88fb 100644
--- a/src/main/flight/adaptive_filter.h
+++ b/src/main/flight/adaptive_filter.h
@@ -28,6 +28,7 @@
 #define ADAPTIVE_FILTER_BUFFER_SIZE 32
 #define ADAPTIVE_FILTER_RATE_HZ 100
 #define ADAPTIVE_FILTER_LPF_HZ 1
+#define ADAPTIVE_FILTER_HPF_HZ 15
 
 void adaptiveFilterPush(const flight_dynamics_index_t index, const float value);
 void adaptiveFilterTask(timeUs_t currentTimeUs);

From 3585f889296ea882139dd43aa2f4284f37c9f2be Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 2 Apr 2024 14:58:04 +0200
Subject: [PATCH 020/323] Compute combined RMS

---
 src/main/flight/adaptive_filter.c | 10 ++++++++--
 1 file changed, 8 insertions(+), 2 deletions(-)

diff --git a/src/main/flight/adaptive_filter.c b/src/main/flight/adaptive_filter.c
index d402369c910..0c4767e293f 100644
--- a/src/main/flight/adaptive_filter.c
+++ b/src/main/flight/adaptive_filter.c
@@ -76,6 +76,8 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) {
         adaptiveFilterInitialized = 1;
     }
 
+    float combinedRms = 0.0f;
+
     //Compute RMS for each axis
     for (flight_dynamics_index_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 
@@ -90,10 +92,14 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) {
 
         float32_t filteredRms = pt1FilterApply(&rmsFilter[axis], rms);
 
-        DEBUG_SET(DEBUG_ADAPTIVE_FILTER, axis, rms * 1000.0f);
-        DEBUG_SET(DEBUG_ADAPTIVE_FILTER, axis + XYZ_AXIS_COUNT, filteredRms * 1000.0f);
+        combinedRms += filteredRms;
     }
 
+    combinedRms /= XYZ_AXIS_COUNT;
+    
+    DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 0, combinedRms);
+
+
 }
 
 

From 49aba32044d2a7de3cb373ed0003b91855e5c1ec Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 2 Apr 2024 15:04:10 +0200
Subject: [PATCH 021/323] Compute task dT

---
 src/main/flight/adaptive_filter.c | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/src/main/flight/adaptive_filter.c b/src/main/flight/adaptive_filter.c
index 0c4767e293f..52f82ec72bb 100644
--- a/src/main/flight/adaptive_filter.c
+++ b/src/main/flight/adaptive_filter.c
@@ -66,7 +66,8 @@ void adaptiveFilterPush(const flight_dynamics_index_t index, const float value)
 }
 
 void adaptiveFilterTask(timeUs_t currentTimeUs) {
-    UNUSED(currentTimeUs);
+    static timeUs_t previousUpdateTimeUs;
+    const float dT = US2S(currentTimeUs - previousUpdateTimeUs);
 
     if (!adaptiveFilterInitialized) {
         //Initialize the filter
@@ -96,8 +97,8 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) {
     }
 
     combinedRms /= XYZ_AXIS_COUNT;
-    
-    DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 0, combinedRms);
+
+    DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 0, combinedRms * 1000.0f);
 
 
 }

From 9f72d0744af7a91802c18859d6d81f9a1cd8dae5 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 2 Apr 2024 17:36:32 +0200
Subject: [PATCH 022/323] Change filtering rules

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

diff --git a/src/main/flight/adaptive_filter.c b/src/main/flight/adaptive_filter.c
index 52f82ec72bb..bb35557f317 100644
--- a/src/main/flight/adaptive_filter.c
+++ b/src/main/flight/adaptive_filter.c
@@ -94,11 +94,13 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) {
         float32_t filteredRms = pt1FilterApply(&rmsFilter[axis], rms);
 
         combinedRms += filteredRms;
+
+        DEBUG_SET(DEBUG_ADAPTIVE_FILTER, axis, filteredRms * 1000.0f);
     }
 
     combinedRms /= XYZ_AXIS_COUNT;
 
-    DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 0, combinedRms * 1000.0f);
+    DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 3, combinedRms * 1000.0f);
 
 
 }

From bc8446fd0ebdfc8d3d436d57a74b71e3906de33b Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 2 Apr 2024 21:17:03 +0200
Subject: [PATCH 023/323] Compute RMS and STD

---
 cmake/at32.cmake                  |  1 +
 cmake/stm32.cmake                 |  1 +
 src/main/flight/adaptive_filter.c | 29 +++++++++++++++++++++++++----
 3 files changed, 27 insertions(+), 4 deletions(-)

diff --git a/cmake/at32.cmake b/cmake/at32.cmake
index 79efcfc0f17..ef5041a8267 100644
--- a/cmake/at32.cmake
+++ b/cmake/at32.cmake
@@ -28,6 +28,7 @@ set(CMSIS_DSP_SRC
     ComplexMathFunctions/arm_cmplx_mag_f32.c
     StatisticsFunctions/arm_max_f32.c
     StatisticsFunctions/arm_rms_f32.c
+    StatisticsFunctions/arm_std_f32.c
     StatisticsFunctions/arm_mean_f32.c
 )
 list(TRANSFORM CMSIS_DSP_SRC PREPEND "${CMSIS_DSP_DIR}/Source/")
diff --git a/cmake/stm32.cmake b/cmake/stm32.cmake
index 256f8d09bef..bbb9c3b8dc9 100644
--- a/cmake/stm32.cmake
+++ b/cmake/stm32.cmake
@@ -29,6 +29,7 @@ set(CMSIS_DSP_SRC
     ComplexMathFunctions/arm_cmplx_mag_f32.c
     StatisticsFunctions/arm_max_f32.c
     StatisticsFunctions/arm_rms_f32.c
+    StatisticsFunctions/arm_std_f32.c
     StatisticsFunctions/arm_mean_f32.c
 )
 list(TRANSFORM CMSIS_DSP_SRC PREPEND "${CMSIS_DSP_DIR}/Source/")
diff --git a/src/main/flight/adaptive_filter.c b/src/main/flight/adaptive_filter.c
index bb35557f317..22a9809fbdc 100644
--- a/src/main/flight/adaptive_filter.c
+++ b/src/main/flight/adaptive_filter.c
@@ -37,8 +37,10 @@
 #include "fc/config.h"
 
 STATIC_FASTRAM float32_t adaptiveFilterSamples[XYZ_AXIS_COUNT][ADAPTIVE_FILTER_BUFFER_SIZE];
+STATIC_FASTRAM float32_t adaptiveFilterRaw[XYZ_AXIS_COUNT][ADAPTIVE_FILTER_BUFFER_SIZE];
 STATIC_FASTRAM uint8_t adaptiveFilterSampleIndex = 0;
 
+STATIC_FASTRAM pt1Filter_t stdFilter[XYZ_AXIS_COUNT];
 STATIC_FASTRAM pt1Filter_t rmsFilter[XYZ_AXIS_COUNT];
 STATIC_FASTRAM pt1Filter_t hpfFilter[XYZ_AXIS_COUNT];
 
@@ -62,6 +64,7 @@ void adaptiveFilterPush(const flight_dynamics_index_t index, const float value)
 
     //Push new sample to the buffer so later we can compute RMS and other measures
     adaptiveFilterSamples[index][adaptiveFilterSampleIndex] = filteredGyro;
+    adaptiveFilterRaw[index][adaptiveFilterSampleIndex] = value;
     adaptiveFilterSampleIndex = (adaptiveFilterSampleIndex + 1) % ADAPTIVE_FILTER_BUFFER_SIZE;
 }
 
@@ -73,11 +76,14 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) {
         //Initialize the filter
         for (flight_dynamics_index_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
             pt1FilterInit(&rmsFilter[axis], ADAPTIVE_FILTER_LPF_HZ, 1.0f / ADAPTIVE_FILTER_RATE_HZ);
+            pt1FilterInit(&stdFilter[axis], ADAPTIVE_FILTER_LPF_HZ, 1.0f / ADAPTIVE_FILTER_RATE_HZ);
         }
         adaptiveFilterInitialized = 1;
     }
 
     float combinedRms = 0.0f;
+    float combinedStd = 0.0f;
+    float combinedStdRaw = 0.0f;
 
     //Compute RMS for each axis
     for (flight_dynamics_index_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
@@ -91,18 +97,33 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) {
         float32_t rms;
         arm_rms_f32(tempBuffer, ADAPTIVE_FILTER_BUFFER_SIZE, &rms);
 
+        float32_t std;
+        memcpy(tempBuffer, adaptiveFilterRaw[axis], sizeof(adaptiveFilterRaw[axis]));
+        arm_std_f32(tempBuffer, ADAPTIVE_FILTER_BUFFER_SIZE, &std);
+
         float32_t filteredRms = pt1FilterApply(&rmsFilter[axis], rms);
+        float32_t filteredStd = pt1FilterApply(&stdFilter[axis], std);
 
         combinedRms += filteredRms;
 
-        DEBUG_SET(DEBUG_ADAPTIVE_FILTER, axis, filteredRms * 1000.0f);
+        combinedStd += filteredStd;
+        combinedStdRaw += std;
+
+        if (axis == 0) {
+            DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 0, rms * 1000.0f);
+            DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 1, filteredRms * 1000.0f);
+            DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 2, std * 1000.0f);
+            DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 3, filteredStd * 1000.0f);
+        }
     }
 
     combinedRms /= XYZ_AXIS_COUNT;
+    combinedStd /= XYZ_AXIS_COUNT;
+    combinedStdRaw /= XYZ_AXIS_COUNT;
 
-    DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 3, combinedRms * 1000.0f);
-
-
+    DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 4, combinedRms * 1000.0f);
+    DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 5, combinedStdRaw * 1000.0f);
+    DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 6, combinedStd * 1000.0f);
 }
 
 

From f86365ba5f43d5dcf8fc8df20925608e6053a106 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Thu, 4 Apr 2024 13:14:11 +0200
Subject: [PATCH 024/323] Settle only on Standard Deviation on high pass filter
 gyro data

---
 .vscode/c_cpp_properties.json     | 33 +++++--------------------------
 src/main/flight/adaptive_filter.c | 31 +++++------------------------
 2 files changed, 10 insertions(+), 54 deletions(-)

diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
index a2c5f582e1c..3a8d8d1c8ca 100755
--- a/.vscode/c_cpp_properties.json
+++ b/.vscode/c_cpp_properties.json
@@ -3,24 +3,19 @@
         {
             "name": "Linux",
             "includePath": [
-                "${workspaceRoot}/src/main/**",
-                "${workspaceRoot}/lib/main/**",
-                "/usr/include/**"
+                "${workspaceRoot}",
+                "${workspaceRoot}/src/main/**"
             ],
             "browse": {
                 "limitSymbolsToIncludedHeaders": false,
                 "path": [
-                    "${workspaceRoot}/src/main/**",
-                    "${workspaceRoot}/lib/main/**"
+                    "${workspaceRoot}/**"
                 ]
             },
-            "intelliSenseMode": "linux-gcc-arm",
+            "intelliSenseMode": "msvc-x64",
             "cStandard": "c11",
             "cppStandard": "c++17",
             "defines": [
-                "MCU_FLASH_SIZE 512",
-                "USE_NAV",
-                "NAV_FIXED_WING_LANDING",
                 "USE_OSD",
                 "USE_GYRO_NOTCH_1",
                 "USE_GYRO_NOTCH_2",
@@ -34,31 +29,13 @@
                 "USE_RPM_FILTER",
                 "USE_GLOBAL_FUNCTIONS",
                 "USE_DYNAMIC_FILTERS",
-                "USE_IMU_BNO055",
-                "USE_SECONDARY_IMU",
                 "USE_DSHOT",
                 "FLASH_SIZE 480",
                 "USE_I2C_IO_EXPANDER",
                 "USE_PCF8574",
                 "USE_ESC_SENSOR",
-                "USE_PROGRAMMING_FRAMEWORK",
-                "USE_SERIALRX_GHST",
-                "USE_TELEMETRY_GHST",
-                "USE_CMS",
-                "USE_DJI_HD_OSD",
-                "USE_GYRO_KALMAN",
-                "USE_RANGEFINDER",
-                "USE_RATE_DYNAMICS",
-                "USE_SMITH_PREDICTOR",
-                "USE_ALPHA_BETA_GAMMA_FILTER",
-                "USE_MAG_VCM5883",
-                "USE_TELEMETRY_JETIEXBUS",
-                "USE_NAV",
-                "USE_SDCARD_SDIO",
-                "USE_SDCARD",
-                "USE_Q_TUNE",
-                "USE_GYRO_FFT_FILTER",
                 "USE_ADAPTIVE_FILTER",
+                "MCU_FLASH_SIZE 1024",
             ],
             "configurationProvider": "ms-vscode.cmake-tools"
         }
diff --git a/src/main/flight/adaptive_filter.c b/src/main/flight/adaptive_filter.c
index 22a9809fbdc..b6a584f92db 100644
--- a/src/main/flight/adaptive_filter.c
+++ b/src/main/flight/adaptive_filter.c
@@ -37,7 +37,6 @@
 #include "fc/config.h"
 
 STATIC_FASTRAM float32_t adaptiveFilterSamples[XYZ_AXIS_COUNT][ADAPTIVE_FILTER_BUFFER_SIZE];
-STATIC_FASTRAM float32_t adaptiveFilterRaw[XYZ_AXIS_COUNT][ADAPTIVE_FILTER_BUFFER_SIZE];
 STATIC_FASTRAM uint8_t adaptiveFilterSampleIndex = 0;
 
 STATIC_FASTRAM pt1Filter_t stdFilter[XYZ_AXIS_COUNT];
@@ -60,11 +59,11 @@ void adaptiveFilterPush(const flight_dynamics_index_t index, const float value)
         hpfFilterInitialized = 1;
     }
 
+    //Apply high pass filter, we are not interested in slowly changing values, only noise
     const float filteredGyro = value - pt1FilterApply(&hpfFilter[index], value);
 
     //Push new sample to the buffer so later we can compute RMS and other measures
     adaptiveFilterSamples[index][adaptiveFilterSampleIndex] = filteredGyro;
-    adaptiveFilterRaw[index][adaptiveFilterSampleIndex] = value;
     adaptiveFilterSampleIndex = (adaptiveFilterSampleIndex + 1) % ADAPTIVE_FILTER_BUFFER_SIZE;
 }
 
@@ -75,54 +74,34 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) {
     if (!adaptiveFilterInitialized) {
         //Initialize the filter
         for (flight_dynamics_index_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-            pt1FilterInit(&rmsFilter[axis], ADAPTIVE_FILTER_LPF_HZ, 1.0f / ADAPTIVE_FILTER_RATE_HZ);
             pt1FilterInit(&stdFilter[axis], ADAPTIVE_FILTER_LPF_HZ, 1.0f / ADAPTIVE_FILTER_RATE_HZ);
         }
         adaptiveFilterInitialized = 1;
     }
 
-    float combinedRms = 0.0f;
     float combinedStd = 0.0f;
-    float combinedStdRaw = 0.0f;
 
     //Compute RMS for each axis
     for (flight_dynamics_index_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 
         //Copy axis samples to a temporary buffer
         float32_t tempBuffer[ADAPTIVE_FILTER_BUFFER_SIZE];
-        //Use memcpy to copy the samples to the temporary buffer
-        memcpy(tempBuffer, adaptiveFilterSamples[axis], sizeof(adaptiveFilterSamples[axis]));
-
-        //Compute RMS from normalizedBuffer using arm_rms_f32
-        float32_t rms;
-        arm_rms_f32(tempBuffer, ADAPTIVE_FILTER_BUFFER_SIZE, &rms);
-
+        
+        //Copute STD from buffer using arm_std_f32
         float32_t std;
-        memcpy(tempBuffer, adaptiveFilterRaw[axis], sizeof(adaptiveFilterRaw[axis]));
+        memcpy(tempBuffer, adaptiveFilterSamples[axis], sizeof(adaptiveFilterSamples[axis]));
         arm_std_f32(tempBuffer, ADAPTIVE_FILTER_BUFFER_SIZE, &std);
 
-        float32_t filteredRms = pt1FilterApply(&rmsFilter[axis], rms);
         float32_t filteredStd = pt1FilterApply(&stdFilter[axis], std);
 
-        combinedRms += filteredRms;
-
-        combinedStd += filteredStd;
-        combinedStdRaw += std;
+        combinedStd += std;
 
         if (axis == 0) {
-            DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 0, rms * 1000.0f);
-            DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 1, filteredRms * 1000.0f);
-            DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 2, std * 1000.0f);
-            DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 3, filteredStd * 1000.0f);
         }
     }
 
-    combinedRms /= XYZ_AXIS_COUNT;
     combinedStd /= XYZ_AXIS_COUNT;
-    combinedStdRaw /= XYZ_AXIS_COUNT;
 
-    DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 4, combinedRms * 1000.0f);
-    DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 5, combinedStdRaw * 1000.0f);
     DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 6, combinedStd * 1000.0f);
 }
 

From 369613643de343e35f8903966577b69dd3e004a9 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Thu, 4 Apr 2024 16:04:08 +0200
Subject: [PATCH 025/323] Compute the integator of adaptive filter

---
 src/main/flight/adaptive_filter.c | 43 +++++++++++++++++++++++++++----
 src/main/flight/adaptive_filter.h |  1 +
 src/main/flight/pid.c             |  3 +++
 3 files changed, 42 insertions(+), 5 deletions(-)

diff --git a/src/main/flight/adaptive_filter.c b/src/main/flight/adaptive_filter.c
index b6a584f92db..448e37fd782 100644
--- a/src/main/flight/adaptive_filter.c
+++ b/src/main/flight/adaptive_filter.c
@@ -40,12 +40,21 @@ STATIC_FASTRAM float32_t adaptiveFilterSamples[XYZ_AXIS_COUNT][ADAPTIVE_FILTER_B
 STATIC_FASTRAM uint8_t adaptiveFilterSampleIndex = 0;
 
 STATIC_FASTRAM pt1Filter_t stdFilter[XYZ_AXIS_COUNT];
-STATIC_FASTRAM pt1Filter_t rmsFilter[XYZ_AXIS_COUNT];
 STATIC_FASTRAM pt1Filter_t hpfFilter[XYZ_AXIS_COUNT];
 
+/*
+    We want to run adaptive filter only when UAV is commanded to stay stationary
+    Any rotation request on axis will add noise that we are not interested in as it will
+    automatically cause LPF frequency to be lowered
+*/
+STATIC_FASTRAM float axisAttenuationFactor[XYZ_AXIS_COUNT];
+
 STATIC_FASTRAM uint8_t adaptiveFilterInitialized = 0;
 STATIC_FASTRAM uint8_t hpfFilterInitialized = 0;
 
+STATIC_FASTRAM float adaptiveFilterIntegrator;
+STATIC_FASTRAM float adaptiveIntegratorTarget;
+
 /**
  * This function is called at pid rate, so has to be initialized at PID loop frequency
 */
@@ -67,11 +76,19 @@ void adaptiveFilterPush(const flight_dynamics_index_t index, const float value)
     adaptiveFilterSampleIndex = (adaptiveFilterSampleIndex + 1) % ADAPTIVE_FILTER_BUFFER_SIZE;
 }
 
+void adaptiveFilterPushRate(const flight_dynamics_index_t index, const float rate, const uint8_t configRate) {
+    const float maxRate = configRate * 10.0f;
+    axisAttenuationFactor[index] = scaleRangef(fabsf(rate), 0.0f, maxRate, 1.0f, 0.0f);
+    axisAttenuationFactor[index] = constrainf(axisAttenuationFactor[index], 0.0f, 1.0f);
+}
+
 void adaptiveFilterTask(timeUs_t currentTimeUs) {
-    static timeUs_t previousUpdateTimeUs;
-    const float dT = US2S(currentTimeUs - previousUpdateTimeUs);
+    static timeUs_t previousUpdateTimeUs = 0;
 
     if (!adaptiveFilterInitialized) {
+        adaptiveIntegratorTarget = 3.5f;
+        previousUpdateTimeUs = currentTimeUs;
+
         //Initialize the filter
         for (flight_dynamics_index_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
             pt1FilterInit(&stdFilter[axis], ADAPTIVE_FILTER_LPF_HZ, 1.0f / ADAPTIVE_FILTER_RATE_HZ);
@@ -79,6 +96,8 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) {
         adaptiveFilterInitialized = 1;
     }
 
+    const float dT = US2S(currentTimeUs - previousUpdateTimeUs);
+
     float combinedStd = 0.0f;
 
     //Compute RMS for each axis
@@ -92,17 +111,31 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) {
         memcpy(tempBuffer, adaptiveFilterSamples[axis], sizeof(adaptiveFilterSamples[axis]));
         arm_std_f32(tempBuffer, ADAPTIVE_FILTER_BUFFER_SIZE, &std);
 
-        float32_t filteredStd = pt1FilterApply(&stdFilter[axis], std);
+        const float filteredStd = pt1FilterApply(&stdFilter[axis], std);
+
+        const float error = filteredStd - adaptiveIntegratorTarget;
+
+        const float adjustedError = error * axisAttenuationFactor[axis];
+
+        const float timeAdjustedError = adjustedError * dT;
+
+        //Put into integrator
+        adaptiveFilterIntegrator += timeAdjustedError;
 
         combinedStd += std;
 
         if (axis == 0) {
+            DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 2, filteredStd * 1000.0f);
+            DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 3, error * 1000.0f);
+            DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 4, adjustedError * 1000.0f);
+            DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 5, timeAdjustedError * 1000.0f);
         }
     }
 
     combinedStd /= XYZ_AXIS_COUNT;
 
-    DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 6, combinedStd * 1000.0f);
+    DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 0, combinedStd * 1000.0f);
+    DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 1, adaptiveFilterIntegrator);
 }
 
 
diff --git a/src/main/flight/adaptive_filter.h b/src/main/flight/adaptive_filter.h
index 9a9d4dc88fb..2885023bbd6 100644
--- a/src/main/flight/adaptive_filter.h
+++ b/src/main/flight/adaptive_filter.h
@@ -31,4 +31,5 @@
 #define ADAPTIVE_FILTER_HPF_HZ 15
 
 void adaptiveFilterPush(const flight_dynamics_index_t index, const float value);
+void adaptiveFilterPushRate(const flight_dynamics_index_t index, const float rate, const uint8_t configRate);
 void adaptiveFilterTask(timeUs_t currentTimeUs);
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 3da150e39dc..d1dbb75ce67 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -47,6 +47,7 @@
 #include "flight/rpm_filter.h"
 #include "flight/kalman.h"
 #include "flight/smith_predictor.h"
+#include "flight/adaptive_filter.h"
 
 #include "io/gps.h"
 
@@ -1168,6 +1169,8 @@ void FAST_CODE pidController(float dT)
         // Limit desired rate to something gyro can measure reliably
         pidState[axis].rateTarget = constrainf(rateTarget, -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT);
 
+        adaptiveFilterPushRate(axis, pidState[axis].rateTarget, currentControlRateProfile->stabilized.rates[axis]);
+
 #ifdef USE_GYRO_KALMAN
         gyroKalmanUpdateSetpoint(axis, pidState[axis].rateTarget);
 #endif

From 561f7b8be254bc049524ada5d6582ea2022e3028 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Thu, 4 Apr 2024 16:39:25 +0200
Subject: [PATCH 026/323] Compute integrator

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

diff --git a/src/main/flight/adaptive_filter.c b/src/main/flight/adaptive_filter.c
index 448e37fd782..3a453679af3 100644
--- a/src/main/flight/adaptive_filter.c
+++ b/src/main/flight/adaptive_filter.c
@@ -85,6 +85,7 @@ void adaptiveFilterPushRate(const flight_dynamics_index_t index, const float rat
 void adaptiveFilterTask(timeUs_t currentTimeUs) {
     static timeUs_t previousUpdateTimeUs = 0;
 
+    //Initialization procedure, filter setup etc.
     if (!adaptiveFilterInitialized) {
         adaptiveIntegratorTarget = 3.5f;
         previousUpdateTimeUs = currentTimeUs;
@@ -96,7 +97,9 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) {
         adaptiveFilterInitialized = 1;
     }
 
+    //Prepare time delta to normalize time factor of the integrator 
     const float dT = US2S(currentTimeUs - previousUpdateTimeUs);
+    previousUpdateTimeUs = currentTimeUs;
 
     float combinedStd = 0.0f;
 

From ac242b566a07e96d61705940248b9514f26e826c Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Thu, 4 Apr 2024 21:01:39 +0200
Subject: [PATCH 027/323] Basic approach to adaptive filter.

---
 src/main/flight/adaptive_filter.c | 55 +++++++++++++++++++++++++++----
 src/main/flight/adaptive_filter.h |  3 ++
 src/main/sensors/gyro.c           |  4 +++
 3 files changed, 56 insertions(+), 6 deletions(-)

diff --git a/src/main/flight/adaptive_filter.c b/src/main/flight/adaptive_filter.c
index 3a453679af3..7501d0b5d3d 100644
--- a/src/main/flight/adaptive_filter.c
+++ b/src/main/flight/adaptive_filter.c
@@ -35,6 +35,7 @@
 #include "common/filter.h"
 #include "build/debug.h"
 #include "fc/config.h"
+#include "sensors/gyro.h"
 
 STATIC_FASTRAM float32_t adaptiveFilterSamples[XYZ_AXIS_COUNT][ADAPTIVE_FILTER_BUFFER_SIZE];
 STATIC_FASTRAM uint8_t adaptiveFilterSampleIndex = 0;
@@ -52,6 +53,12 @@ STATIC_FASTRAM float axisAttenuationFactor[XYZ_AXIS_COUNT];
 STATIC_FASTRAM uint8_t adaptiveFilterInitialized = 0;
 STATIC_FASTRAM uint8_t hpfFilterInitialized = 0;
 
+//Defines if current, min and max  values for the filter were set and filter is ready to work
+STATIC_FASTRAM uint8_t targetsSet = 0;
+STATIC_FASTRAM float currentLpf;
+STATIC_FASTRAM float minLpf;
+STATIC_FASTRAM float maxLpf; 
+
 STATIC_FASTRAM float adaptiveFilterIntegrator;
 STATIC_FASTRAM float adaptiveIntegratorTarget;
 
@@ -82,7 +89,24 @@ void adaptiveFilterPushRate(const flight_dynamics_index_t index, const float rat
     axisAttenuationFactor[index] = constrainf(axisAttenuationFactor[index], 0.0f, 1.0f);
 }
 
+void adaptiveFilterResetIntegrator(void) {
+    adaptiveFilterIntegrator = 0.0f;
+}
+
+void adaptiveFilterSetDefaultFrequency(int lpf, int min, int max) {
+    currentLpf = lpf;
+    minLpf = min;
+    maxLpf = max;
+    targetsSet = 1;
+}
+
 void adaptiveFilterTask(timeUs_t currentTimeUs) {
+
+    //If we don't have current, min and max values for the filter, we can't run it yet
+    if (!targetsSet) {
+        return;
+    }
+
     static timeUs_t previousUpdateTimeUs = 0;
 
     //Initialization procedure, filter setup etc.
@@ -127,18 +151,37 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) {
 
         combinedStd += std;
 
-        if (axis == 0) {
-            DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 2, filteredStd * 1000.0f);
-            DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 3, error * 1000.0f);
-            DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 4, adjustedError * 1000.0f);
-            DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 5, timeAdjustedError * 1000.0f);
-        }
+        // if (axis == 0) {
+        //     DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 2, filteredStd * 1000.0f);
+        //     DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 3, error * 1000.0f);
+        //     DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 4, adjustedError * 1000.0f);
+        //     DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 5, timeAdjustedError * 1000.0f);
+        // }
     }
 
+    //TODO filter gets updated only when ARMED
+
+    if (adaptiveFilterIntegrator > ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD) {
+        //In this case there is too much noise, we need to lower the LPF frequency
+        currentLpf = constrainf(currentLpf - 1.0f, minLpf, maxLpf);
+        gyroUpdateDynamicLpf(currentLpf);
+        adaptiveFilterResetIntegrator();
+    } else if (adaptiveFilterIntegrator < -ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD) {
+        //In this case there is too little noise, we can to increase the LPF frequency
+        currentLpf = constrainf(currentLpf + 1.0f, minLpf, maxLpf);
+        gyroUpdateDynamicLpf(currentLpf);
+        adaptiveFilterResetIntegrator();
+    }
+
+    // if (ARMING_FLAG(ARMED)) {
+    //     // 
+    // }
+
     combinedStd /= XYZ_AXIS_COUNT;
 
     DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 0, combinedStd * 1000.0f);
     DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 1, adaptiveFilterIntegrator);
+    DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 2, currentLpf);
 }
 
 
diff --git a/src/main/flight/adaptive_filter.h b/src/main/flight/adaptive_filter.h
index 2885023bbd6..725f992a665 100644
--- a/src/main/flight/adaptive_filter.h
+++ b/src/main/flight/adaptive_filter.h
@@ -29,7 +29,10 @@
 #define ADAPTIVE_FILTER_RATE_HZ 100
 #define ADAPTIVE_FILTER_LPF_HZ 1
 #define ADAPTIVE_FILTER_HPF_HZ 15
+#define ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD 20.0f
 
 void adaptiveFilterPush(const flight_dynamics_index_t index, const float value);
 void adaptiveFilterPushRate(const flight_dynamics_index_t index, const float rate, const uint8_t configRate);
+void adaptiveFilterResetIntegrator(void);
+void adaptiveFilterSetDefaultFrequency(int lpf, int min, int max);
 void adaptiveFilterTask(timeUs_t currentTimeUs);
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 5873c9d7950..0a9c8be96fd 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -264,6 +264,10 @@ static void gyroInitFilters(void)
     //Second gyro LPF runnig and PID frequency - this filter is dynamic when gyro_use_dyn_lpf = ON
     initGyroFilter(&gyroLpf2ApplyFn, gyroLpf2State, gyroConfig()->gyro_main_lpf_type, gyroConfig()->gyro_main_lpf_hz, getLooptime());
 
+#ifdef USE_ADAPTIVE_FILTER
+    adaptiveFilterSetDefaultFrequency(gyroConfig()->gyro_main_lpf_hz, 50, 150);
+#endif
+
 #ifdef USE_GYRO_KALMAN
     if (gyroConfig()->kalmanEnabled) {
         gyroKalmanInitialize(gyroConfig()->kalman_q);

From 63993ba7ff868ff5f2078c5e296ec7851790b9dd Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Thu, 4 Apr 2024 21:04:38 +0200
Subject: [PATCH 028/323] Compilation fix

---
 src/main/flight/pid.c   | 4 +++-
 src/main/sensors/gyro.c | 2 ++
 2 files changed, 5 insertions(+), 1 deletion(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index d1dbb75ce67..61a5083dfe0 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -1168,8 +1168,10 @@ void FAST_CODE pidController(float dT)
 
         // Limit desired rate to something gyro can measure reliably
         pidState[axis].rateTarget = constrainf(rateTarget, -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT);
-
+    
+#ifdef USE_ADAPTIVE_FILTER
         adaptiveFilterPushRate(axis, pidState[axis].rateTarget, currentControlRateProfile->stabilized.rates[axis]);
+#endif
 
 #ifdef USE_GYRO_KALMAN
         gyroKalmanUpdateSetpoint(axis, pidState[axis].rateTarget);
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 0a9c8be96fd..c439f8659ee 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -458,7 +458,9 @@ void FAST_CODE NOINLINE gyroFilter(void)
 
         gyroADCf = gyroLpf2ApplyFn((filter_t *) &gyroLpf2State[axis], gyroADCf);
 
+#ifdef USE_ADAPTIVE_FILTER
         adaptiveFilterPush(axis, gyroADCf);
+#endif
 
 #ifdef USE_DYNAMIC_FILTERS
         if (dynamicGyroNotchState.enabled) {

From 731f0608c46e2f218585d4226b8516456f40959c Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 7 Apr 2024 14:22:30 +0200
Subject: [PATCH 029/323] Run damping of FW with stick movements

---
 src/main/flight/pid.c | 9 ++++++---
 1 file changed, 6 insertions(+), 3 deletions(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 3da150e39dc..d6c8fe81f39 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -758,16 +758,19 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
 {
     const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget);
 
+    const float maxRate = currentControlRateProfile->stabilized.rates[axis] * 10.0f;
+    const float dampingFactor = bellCurve(MIN(rateTarget, maxRate), maxRate);
+
     const float rateError = rateTarget - pidState->gyroRate;
-    const float newPTerm = pTermProcess(pidState, rateError, dT);
-    const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv);
+    const float newPTerm = pTermProcess(pidState, rateError, dT) * dampingFactor;
+    const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv) * dampingFactor;
     const float newFFTerm = rateTarget * pidState->kFF;
 
     /*
      * Integral should be updated only if axis Iterm is not frozen
      */
     if (!pidState->itermFreezeActive) {
-        pidState->errorGyroIf += rateError * pidState->kI * dT;
+        pidState->errorGyroIf += rateError * pidState->kI * dT * dampingFactor;
     }
 
     applyItermLimiting(pidState);

From 1f723837e96e37ea05e0aba99b983d91c1d87152 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 7 Apr 2024 20:28:45 +0200
Subject: [PATCH 030/323] Improve attenuation

---
 src/main/common/maths.c | 18 +++++++++++++++++-
 src/main/common/maths.h |  2 ++
 src/main/flight/pid.c   |  4 +++-
 3 files changed, 22 insertions(+), 2 deletions(-)

diff --git a/src/main/common/maths.c b/src/main/common/maths.c
index 1bc61ae4c36..eed7b863bae 100644
--- a/src/main/common/maths.c
+++ b/src/main/common/maths.c
@@ -516,9 +516,25 @@ bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float resu
     return sensorCalibrationValidateResult(result);
 }
 
+float gaussian(const float x, const float mu, const float sigma) {
+    return exp(-pow(x - mu, 2) / (2 * pow(sigma, 2)));
+}
+
 float bellCurve(const float x, const float curveWidth)
 {
-    return powf(M_Ef, -sq(x) / (2.0f * sq(curveWidth)));
+    return gaussian(x, 0.0f, curveWidth);
+}
+
+/**
+ * @brief Calculate the attenuation of a value using a Gaussian function.
+ * Retuns 1 for input 0 and ~0 for input width.
+ * @param input The input value.
+ * @param width The width of the Gaussian function.
+ * @return The attenuation of the input value.
+*/
+float attenuation(const float input, const float width) {
+    const float sigma = width / 2.35482f; // Approximately width / sqrt(2 * ln(2))
+    return gaussian(input, 0.0f, sigma);
 }
 
 float fast_fsqrtf(const float value) {
diff --git a/src/main/common/maths.h b/src/main/common/maths.h
index f3f2fd62746..ce96b7064fd 100644
--- a/src/main/common/maths.h
+++ b/src/main/common/maths.h
@@ -195,6 +195,8 @@ float acos_approx(float x);
 void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count);
 
 float bellCurve(const float x, const float curveWidth);
+float attenuation(const float input, const float width);
+float gaussian(const float x, const float mu, const float sigma);
 float fast_fsqrtf(const float value);
 float calc_length_pythagorean_2D(const float firstElement, const float secondElement);
 float calc_length_pythagorean_3D(const float firstElement, const float secondElement, const float thirdElement);
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index d6c8fe81f39..9e50522ae3f 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -759,7 +759,9 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
     const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget);
 
     const float maxRate = currentControlRateProfile->stabilized.rates[axis] * 10.0f;
-    const float dampingFactor = bellCurve(MIN(rateTarget, maxRate), maxRate);
+    const float dampingFactor = attenuation(rateTarget, maxRate / 2.5f);
+
+    DEBUG_SET(DEBUG_ALWAYS, axis, dampingFactor * 1000);
 
     const float rateError = rateTarget - pidState->gyroRate;
     const float newPTerm = pTermProcess(pidState, rateError, dT) * dampingFactor;

From 37bb8580db0f7cadf109bbeba079bd2999aa1556 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 7 Apr 2024 21:46:36 +0200
Subject: [PATCH 031/323] Fix compilation for SITL

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

diff --git a/src/main/common/maths.c b/src/main/common/maths.c
index eed7b863bae..b8ec59c33a2 100644
--- a/src/main/common/maths.c
+++ b/src/main/common/maths.c
@@ -517,7 +517,7 @@ bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float resu
 }
 
 float gaussian(const float x, const float mu, const float sigma) {
-    return exp(-pow(x - mu, 2) / (2 * pow(sigma, 2)));
+    return exp(-pow((double)(x - mu), 2) / (2 * pow((double)sigma, 2)));
 }
 
 float bellCurve(const float x, const float curveWidth)

From 5f4dbb79138eab5c5c1f598c6ca338fb89dffe5c Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 7 Apr 2024 21:54:04 +0200
Subject: [PATCH 032/323] Drop no longer previous generation dampening solution

---
 src/main/fc/settings.yaml |  6 ------
 src/main/flight/pid.c     | 27 ++++-----------------------
 src/main/flight/pid.h     |  1 -
 3 files changed, 4 insertions(+), 30 deletions(-)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index cb586eacf06..a3b117d7a3b 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -1934,12 +1934,6 @@ groups:
         field: fixedWingCoordinatedPitchGain
         min: 0
         max: 2
-      - name: fw_iterm_limit_stick_position
-        description: "Iterm is not allowed to grow when stick position is above threshold. This solves the problem of bounceback or followthrough when full stick deflection is applied on poorely tuned fixed wings. In other words, stabilization is partialy disabled when pilot is actively controlling the aircraft and active when sticks are not touched. `0` mean stick is in center position, `1` means it is fully deflected to either side"
-        default_value: 0.5
-        field: fixedWingItermLimitOnStickPosition
-        min: 0
-        max: 1
       - name: fw_yaw_iterm_freeze_bank_angle
         description: "Yaw Iterm is frozen when bank angle is above this threshold [degrees]. This solves the problem of the rudder counteracting turns by partially disabling yaw stabilization when making banked turns. Setting to 0 (the default) disables this feature. Only applies when autopilot is not active and TURN ASSIST is disabled."
         default_value: 0
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 9e50522ae3f..b92aec705f2 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -158,7 +158,6 @@ static EXTENDED_FASTRAM uint8_t usedPidControllerType;
 typedef void (*pidControllerFnPtr)(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv);
 static EXTENDED_FASTRAM pidControllerFnPtr pidControllerApplyFn;
 static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn;
-static EXTENDED_FASTRAM bool levelingEnabled = false;
 static EXTENDED_FASTRAM bool restartAngleHoldMode = true;
 static EXTENDED_FASTRAM bool angleHoldIsLevel = false;
 
@@ -170,7 +169,7 @@ static EXTENDED_FASTRAM bool angleHoldIsLevel = false;
 static EXTENDED_FASTRAM float fixedWingLevelTrim;
 static EXTENDED_FASTRAM pidController_t fixedWingLevelTrimController;
 
-PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 6);
+PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 7);
 
 PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .bank_mc = {
@@ -271,7 +270,6 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .fixedWingReferenceAirspeed = SETTING_FW_REFERENCE_AIRSPEED_DEFAULT,
         .fixedWingCoordinatedYawGain = SETTING_FW_TURN_ASSIST_YAW_GAIN_DEFAULT,
         .fixedWingCoordinatedPitchGain = SETTING_FW_TURN_ASSIST_PITCH_GAIN_DEFAULT,
-        .fixedWingItermLimitOnStickPosition = SETTING_FW_ITERM_LIMIT_STICK_POSITION_DEFAULT,
         .fixedWingYawItermBankFreeze = SETTING_FW_YAW_ITERM_FREEZE_BANK_ANGLE_DEFAULT,
 
         .navVelXyDTermLpfHz = SETTING_NAV_MC_VEL_XY_DTERM_LPF_HZ_DEFAULT,
@@ -672,19 +670,6 @@ static void pidApplySetpointRateLimiting(pidState_t *pidState, flight_dynamics_i
     }
 }
 
-bool isFixedWingItermLimitActive(float stickPosition)
-{
-    /*
-     * Iterm anti windup whould be active only when pilot controls the rotation
-     * velocity directly, not when ANGLE or HORIZON are used
-     */
-    if (levelingEnabled) {
-        return false;
-    }
-
-    return fabsf(stickPosition) > pidProfile()->fixedWingItermLimitOnStickPosition;
-}
-
 static float pTermProcess(pidState_t *pidState, float rateError, float dT) {
     float newPTerm = rateError * pidState->kP;
 
@@ -1050,11 +1035,9 @@ static void pidApplyFpvCameraAngleMix(pidState_t *pidState, uint8_t fpvCameraAng
 
 void checkItermLimitingActive(pidState_t *pidState)
 {
-    bool shouldActivate;
-    if (usedPidControllerType == PID_TYPE_PIFF) {
-        shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition);
-    } else
-    {
+    bool shouldActivate = false;
+
+    if (usedPidControllerType == PID_TYPE_PID) {
         shouldActivate = mixerIsOutputSaturated(); //just in case, since it is already managed by itermWindupPointPercent
     }
 
@@ -1184,7 +1167,6 @@ void FAST_CODE pidController(float dT)
 
     // Step 3: Run control for ANGLE_MODE, HORIZON_MODE and ANGLEHOLD_MODE
     const float horizonRateMagnitude = FLIGHT_MODE(HORIZON_MODE) ? calcHorizonRateMagnitude() : 0.0f;
-    levelingEnabled = false;
     angleHoldIsLevel = false;
 
     for (uint8_t axis = FD_ROLL; axis <= FD_PITCH; axis++) {
@@ -1204,7 +1186,6 @@ void FAST_CODE pidController(float dT)
             // Apply the Level PID controller
             pidLevel(angleTarget, &pidState[axis], axis, horizonRateMagnitude, dT);
             canUseFpvCameraMix = false;     // FPVANGLEMIX is incompatible with ANGLE/HORIZON
-            levelingEnabled = true;
         } else {
             restartAngleHoldMode = true;
         }
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index e0a8b9d310b..2e28848d3cc 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -123,7 +123,6 @@ typedef struct pidProfile_s {
     float       fixedWingReferenceAirspeed;     // Reference tuning airspeed for the airplane - the speed for which PID gains are tuned
     float       fixedWingCoordinatedYawGain;    // This is the gain of the yaw rate required to keep the yaw rate consistent with the turn rate for a coordinated turn.
     float       fixedWingCoordinatedPitchGain;    // This is the gain of the pitch rate to keep the pitch angle constant during coordinated turns.
-    float       fixedWingItermLimitOnStickPosition;   //Do not allow Iterm to grow when stick position is above this point
     uint16_t    fixedWingYawItermBankFreeze;       // Freeze yaw Iterm when bank angle is more than this many degrees
 
     float       navVelXyDTermLpfHz;

From 292db1f14c9bac72fbe7ac17963f6c2a4b21e36e Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Mon, 8 Apr 2024 15:39:33 +0200
Subject: [PATCH 033/323] Allo only gradual relaxation of the Axis factor

---
 src/main/flight/pid.c | 15 ++++++++++++++-
 1 file changed, 14 insertions(+), 1 deletion(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index b92aec705f2..19708b50f00 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -107,6 +107,8 @@ typedef struct {
     pt3Filter_t rateTargetFilter;
 
     smithPredictor_t smithPredictor;
+
+    float dampingFactor;
 } pidState_t;
 
 STATIC_FASTRAM bool pidFiltersConfigured = false;
@@ -746,7 +748,16 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
     const float maxRate = currentControlRateProfile->stabilized.rates[axis] * 10.0f;
     const float dampingFactor = attenuation(rateTarget, maxRate / 2.5f);
 
-    DEBUG_SET(DEBUG_ALWAYS, axis, dampingFactor * 1000);
+    if (fabsf(dampingFactor) < fabsf(pidState->dampingFactor)) {
+        pidState->dampingFactor = dampingFactor;
+    } else {
+        pidState->dampingFactor = pidState->dampingFactor + (dampingFactor - pidState->dampingFactor) * dT * 10;
+    }
+
+    float newDF = pidState->dampingFactor;
+    
+    DEBUG_SET(DEBUG_ALWAYS, axis * 2, dampingFactor * 1000);
+    DEBUG_SET(DEBUG_ALWAYS, (axis * 2) + 1, newDF * 1000);
 
     const float rateError = rateTarget - pidState->gyroRate;
     const float newPTerm = pTermProcess(pidState, rateError, dT) * dampingFactor;
@@ -1262,6 +1273,8 @@ void pidInit(void)
 
     for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) {
 
+        pidState[axis].dampingFactor = 1.0f;
+
     #ifdef USE_D_BOOST
         // Rate * 10 * 10. First 10 is to convert stick to DPS. Second 10 is to convert target to acceleration.
         // We assume, max acceleration is when pilot deflects the stick fully in 100ms

From cf10c071e072589527d072b5fa381b7e416bb148 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Mon, 8 Apr 2024 15:40:21 +0200
Subject: [PATCH 034/323] Docs update

---
 docs/Settings.md | 10 ----------
 1 file changed, 10 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index b6fd431d950..c33efe6ef9d 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -1292,16 +1292,6 @@ Fixed-wing rate stabilisation I-gain for YAW
 
 ---
 
-### fw_iterm_limit_stick_position
-
-Iterm is not allowed to grow when stick position is above threshold. This solves the problem of bounceback or followthrough when full stick deflection is applied on poorely tuned fixed wings. In other words, stabilization is partialy disabled when pilot is actively controlling the aircraft and active when sticks are not touched. `0` mean stick is in center position, `1` means it is fully deflected to either side
-
-| Default | Min | Max |
-| --- | --- | --- |
-| 0.5 | 0 | 1 |
-
----
-
 ### fw_level_pitch_gain
 
 I-gain for the pitch trim for self-leveling flight modes. Higher values means that AUTOTRIM will be faster but might introduce oscillations

From c41abacd2a37fee7e0ef2af0aabcb4d818a96f5b Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Mon, 8 Apr 2024 19:53:11 +0200
Subject: [PATCH 035/323] Reset adaptive filter on disarm

---
 src/main/flight/adaptive_filter.c | 29 +++++++++++++++--------------
 src/main/flight/adaptive_filter.h |  3 ++-
 2 files changed, 17 insertions(+), 15 deletions(-)

diff --git a/src/main/flight/adaptive_filter.c b/src/main/flight/adaptive_filter.c
index 7501d0b5d3d..138203a6b0e 100644
--- a/src/main/flight/adaptive_filter.c
+++ b/src/main/flight/adaptive_filter.c
@@ -35,6 +35,7 @@
 #include "common/filter.h"
 #include "build/debug.h"
 #include "fc/config.h"
+#include "fc/runtime_config.h"
 #include "sensors/gyro.h"
 
 STATIC_FASTRAM float32_t adaptiveFilterSamples[XYZ_AXIS_COUNT][ADAPTIVE_FILTER_BUFFER_SIZE];
@@ -56,6 +57,7 @@ STATIC_FASTRAM uint8_t hpfFilterInitialized = 0;
 //Defines if current, min and max  values for the filter were set and filter is ready to work
 STATIC_FASTRAM uint8_t targetsSet = 0;
 STATIC_FASTRAM float currentLpf;
+STATIC_FASTRAM float initialLpf;
 STATIC_FASTRAM float minLpf;
 STATIC_FASTRAM float maxLpf; 
 
@@ -97,6 +99,8 @@ void adaptiveFilterSetDefaultFrequency(int lpf, int min, int max) {
     currentLpf = lpf;
     minLpf = min;
     maxLpf = max;
+    initialLpf = currentLpf;
+
     targetsSet = 1;
 }
 
@@ -121,6 +125,14 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) {
         adaptiveFilterInitialized = 1;
     }
 
+    //If not armed, leave this routine but reset integrator and set default LPF
+    if (!ARMING_FLAG(ARMED)) {
+        currentLpf = initialLpf;
+        adaptiveFilterResetIntegrator();
+        gyroUpdateDynamicLpf(currentLpf);
+        return;
+    }
+
     //Prepare time delta to normalize time factor of the integrator 
     const float dT = US2S(currentTimeUs - previousUpdateTimeUs);
     previousUpdateTimeUs = currentTimeUs;
@@ -150,37 +162,26 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) {
         adaptiveFilterIntegrator += timeAdjustedError;
 
         combinedStd += std;
-
-        // if (axis == 0) {
-        //     DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 2, filteredStd * 1000.0f);
-        //     DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 3, error * 1000.0f);
-        //     DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 4, adjustedError * 1000.0f);
-        //     DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 5, timeAdjustedError * 1000.0f);
-        // }
     }
 
     //TODO filter gets updated only when ARMED
 
-    if (adaptiveFilterIntegrator > ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD) {
+    if (adaptiveFilterIntegrator > ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD_HIGH) {
         //In this case there is too much noise, we need to lower the LPF frequency
         currentLpf = constrainf(currentLpf - 1.0f, minLpf, maxLpf);
         gyroUpdateDynamicLpf(currentLpf);
         adaptiveFilterResetIntegrator();
-    } else if (adaptiveFilterIntegrator < -ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD) {
+    } else if (adaptiveFilterIntegrator < ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD_LOW) {
         //In this case there is too little noise, we can to increase the LPF frequency
         currentLpf = constrainf(currentLpf + 1.0f, minLpf, maxLpf);
         gyroUpdateDynamicLpf(currentLpf);
         adaptiveFilterResetIntegrator();
     }
 
-    // if (ARMING_FLAG(ARMED)) {
-    //     // 
-    // }
-
     combinedStd /= XYZ_AXIS_COUNT;
 
     DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 0, combinedStd * 1000.0f);
-    DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 1, adaptiveFilterIntegrator);
+    DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 1, adaptiveFilterIntegrator * 10.0f);
     DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 2, currentLpf);
 }
 
diff --git a/src/main/flight/adaptive_filter.h b/src/main/flight/adaptive_filter.h
index 725f992a665..0a8696d90dd 100644
--- a/src/main/flight/adaptive_filter.h
+++ b/src/main/flight/adaptive_filter.h
@@ -29,7 +29,8 @@
 #define ADAPTIVE_FILTER_RATE_HZ 100
 #define ADAPTIVE_FILTER_LPF_HZ 1
 #define ADAPTIVE_FILTER_HPF_HZ 15
-#define ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD 20.0f
+#define ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD_HIGH 5.0f
+#define ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD_LOW -2.0f
 
 void adaptiveFilterPush(const flight_dynamics_index_t index, const float value);
 void adaptiveFilterPushRate(const flight_dynamics_index_t index, const float rate, const uint8_t configRate);

From 6fd60772510f2351ca667d138b506e470bdb0119 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Mon, 8 Apr 2024 20:03:57 +0200
Subject: [PATCH 036/323] Run the filter only if throttle is raised

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

diff --git a/src/main/flight/adaptive_filter.c b/src/main/flight/adaptive_filter.c
index 138203a6b0e..e94df2d19fe 100644
--- a/src/main/flight/adaptive_filter.c
+++ b/src/main/flight/adaptive_filter.c
@@ -36,6 +36,7 @@
 #include "build/debug.h"
 #include "fc/config.h"
 #include "fc/runtime_config.h"
+#include "fc/rc_controls.h"
 #include "sensors/gyro.h"
 
 STATIC_FASTRAM float32_t adaptiveFilterSamples[XYZ_AXIS_COUNT][ADAPTIVE_FILTER_BUFFER_SIZE];
@@ -133,6 +134,11 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) {
         return;
     }
 
+    //Do not run adaptive filter when throttle is low
+    if (rcCommand[THROTTLE] < 1200) {
+        return;
+    }
+
     //Prepare time delta to normalize time factor of the integrator 
     const float dT = US2S(currentTimeUs - previousUpdateTimeUs);
     previousUpdateTimeUs = currentTimeUs;

From b3438919f5fddbfac221affcc48cadbc87464ac9 Mon Sep 17 00:00:00 2001
From: error414 <error414@error414.com>
Date: Sat, 16 Mar 2024 10:31:06 +0100
Subject: [PATCH 037/323] new NRA protocol fro   radars from nanoradar new
 rangerfinder NRA15/NRA24 from nanoradar

---
 docs/Rangefinder.md                 |  16 +++
 src/main/CMakeLists.txt             |   3 +
 src/main/fc/settings.yaml           |   2 +-
 src/main/io/rangefinder.h           |   2 +
 src/main/io/rangefinder_nanoradar.c | 167 ++++++++++++++++++++++++++++
 src/main/io/rangefinder_usd1_v0.c   | 136 ++++++++++++++++++++++
 src/main/sensors/rangefinder.c      |  16 +++
 src/main/sensors/rangefinder.h      |   2 +
 src/main/target/common.h            |   2 +
 9 files changed, 345 insertions(+), 1 deletion(-)
 create mode 100644 src/main/io/rangefinder_nanoradar.c
 create mode 100644 src/main/io/rangefinder_usd1_v0.c

diff --git a/docs/Rangefinder.md b/docs/Rangefinder.md
index 095a6f0df33..813184f8374 100644
--- a/docs/Rangefinder.md
+++ b/docs/Rangefinder.md
@@ -21,6 +21,22 @@ Following rangefinders are supported:
 * MSP - experimental
 * TOF10120 - small & lightweight laser range sensor, usable up to 200cm
 * TERARANGER EVO - 30cm to 600cm, depends on version https://www.terabee.com/sensors-modules/lidar-tof-range-finders/#individual-distance-measurement-sensors
+* NRA15/NRA24 - experimental, UART version
+
+#### NRA15/NRA24
+NRA15/NRA24 from nanoradar use US-D1_V0 or NRA protocol, it depends which firmware you use. Radar can be set by firmware
+to two different resolutions. See table below.
+
+| Radar | Protocol | Resolution      | Name in configurator  |
+|-------|----------|-----------------|-----------------------|
+| NRA15 | US-D1_V0 | 0-30m (+-4cm)   | USD1_V0               |
+| NRA15 | NRA      | 0-30m (+-4cm)   | NRA15/NRA24           | 
+| NRA15 | NRA      | 0-100m (+-10cm) | NRA15/NRA24           | 
+| NRA24 | US-D1_V0 | 0-50m (+-4cm)   | USD1_V0               |
+| NRA24 | US-D1_V0 | 0-200m (+-10cm) | USD1_V0               |
+| NRA24 | NRA      | 0-50m (+-4cm)   | NRA15/NRA24           | 
+| NRA24 | NRA      | 0-200m (+-10cm) | NRA15/NRA24           | 
+
 
 ## Connections
 
diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt
index cba13db687e..28c068966f8 100755
--- a/src/main/CMakeLists.txt
+++ b/src/main/CMakeLists.txt
@@ -1,3 +1,4 @@
+
 main_sources(COMMON_SRC
     main.c
 
@@ -485,6 +486,8 @@ main_sources(COMMON_SRC
     io/rangefinder.h
     io/rangefinder_msp.c
     io/rangefinder_benewake.c
+    io/rangefinder_usd1_v0.c
+    io/rangefinder_nanoradar.c
     io/rangefinder_fake.c
     io/opflow.h
     io/opflow_cxof.c
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index cb586eacf06..b2be345391d 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -5,7 +5,7 @@ tables:
     values: ["NONE", "AUTO", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "BMI270","LSM6DXX",  "FAKE"]
     enum: accelerationSensor_e
   - name: rangefinder_hardware
-    values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C", "FAKE", "TERARANGER_EVO"]
+    values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C", "FAKE", "TERARANGER_EVO", "USD1_V0", "NRA15/NRA24"]
     enum: rangefinderType_e
   - name: mag_hardware
     values: ["NONE", "AUTO", "HMC5883", "AK8975", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", "VCM5883", "MLX90393", "FAKE"]
diff --git a/src/main/io/rangefinder.h b/src/main/io/rangefinder.h
index 6768c7d75d8..fd51aeb44aa 100644
--- a/src/main/io/rangefinder.h
+++ b/src/main/io/rangefinder.h
@@ -31,6 +31,8 @@
 
 extern virtualRangefinderVTable_t rangefinderMSPVtable;
 extern virtualRangefinderVTable_t rangefinderBenewakeVtable;
+extern virtualRangefinderVTable_t rangefinderUSD1Vtable;
+extern virtualRangefinderVTable_t rangefinderNanoradarVtable; //NRA15/NRA24
 extern virtualRangefinderVTable_t rangefinderFakeVtable;
 
 void mspRangefinderReceiveNewData(uint8_t * bufferPtr);
diff --git a/src/main/io/rangefinder_nanoradar.c b/src/main/io/rangefinder_nanoradar.c
new file mode 100644
index 00000000000..f6978448ea4
--- /dev/null
+++ b/src/main/io/rangefinder_nanoradar.c
@@ -0,0 +1,167 @@
+/*
+ * 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 <stdbool.h>
+#include <ctype.h>
+
+#include "platform.h"
+#include "io/serial.h"
+#include "drivers/time.h"
+
+#if defined(USE_RANGEFINDER_NANORADAR)
+#include "drivers/rangefinder/rangefinder_virtual.h"
+
+#define NANORADAR_HDR 0xAA // Header Byte
+#define NANORADAR_END 0x55
+
+#define NANORADAR_COMMAND_TARGET_INFO 0x70C
+
+typedef struct __attribute__((packed)) {
+    uint8_t     header0;
+    uint8_t     header1;
+    uint8_t     commandH;
+    uint8_t     commandL;
+    uint8_t     index; // Target ID
+    uint8_t     rcs; // The section of radar reflection
+    uint8_t     rangeH; // Target distance high 8 bi
+    uint8_t     rangeL; // Target distance low 8 bit
+    uint8_t     rsvd1;
+    uint8_t     info; // VrelH Rsvd1 RollCount
+    uint8_t     vrelL;
+    uint8_t     SNR; // Signal-Noise Ratio
+    uint8_t     end0;
+    uint8_t     end1;
+} nanoradarPacket_t;
+
+#define NANORADAR_PACKET_SIZE    sizeof(nanoradarPacket_t)
+#define NANORADAR_TIMEOUT_MS     2000 // 2s
+
+static bool hasNewData = false;
+static bool hasEverData = false;
+static serialPort_t * serialPort = NULL;
+static serialPortConfig_t * portConfig;
+static uint8_t buffer[NANORADAR_PACKET_SIZE];
+static unsigned bufferPtr;
+
+static int32_t sensorData = RANGEFINDER_NO_NEW_DATA;
+static timeMs_t lastProtocolActivityMs;
+
+static bool nanoradarRangefinderDetect(void)
+{
+    portConfig = findSerialPortConfig(FUNCTION_RANGEFINDER);
+    if (!portConfig) {
+        return false;
+    }
+
+    return true;
+}
+
+static void nanoradarRangefinderInit(void)
+{
+    if (!portConfig) {
+        return;
+    }
+
+    serialPort = openSerialPort(portConfig->identifier, FUNCTION_RANGEFINDER, NULL, NULL, 115200, MODE_RXTX, SERIAL_NOT_INVERTED);
+    if (!serialPort) {
+        return;
+    }
+
+    lastProtocolActivityMs = 0;
+}
+
+static void nanoradarRangefinderUpdate(void)
+{
+
+    nanoradarPacket_t *nanoradarPacket = (nanoradarPacket_t *)buffer;
+    while (serialRxBytesWaiting(serialPort) > 0) {
+        uint8_t c = serialRead(serialPort);
+
+        if (bufferPtr < NANORADAR_PACKET_SIZE) {
+            buffer[bufferPtr++] = c;
+        }
+
+        if ((bufferPtr == 1) && (nanoradarPacket->header0 != NANORADAR_HDR)) {
+            bufferPtr = 0;
+            continue;
+        }
+
+        if ((bufferPtr == 2) && (nanoradarPacket->header1 != NANORADAR_HDR)) {
+            bufferPtr = 0;
+            continue;
+        }
+
+        //only target info packet we are interested
+        if (bufferPtr == 4) {
+            uint16_t command = nanoradarPacket->commandH + (nanoradarPacket->commandL * 0x100);
+
+            if (command != NANORADAR_COMMAND_TARGET_INFO) {
+                bufferPtr = 0;
+                continue;
+            }
+        }
+
+        // Check for complete packet
+        if (bufferPtr == NANORADAR_PACKET_SIZE) {
+            if (nanoradarPacket->end0 == NANORADAR_END && nanoradarPacket->end1 == NANORADAR_END) {
+                // Valid packet
+                hasNewData = true;
+                hasEverData = true;
+                lastProtocolActivityMs = millis();
+
+                sensorData = ((nanoradarPacket->rangeH * 0x100) + nanoradarPacket->rangeL);
+            }
+
+            // Prepare for new packet
+            bufferPtr = 0;
+        }
+    }
+}
+
+static int32_t nanoradarRangefinderGetDistance(void)
+{
+    int32_t altitude = (sensorData > 0) ? (sensorData) : RANGEFINDER_OUT_OF_RANGE;
+
+    if (hasNewData) {
+        hasNewData = false;
+        return altitude;
+    }
+    else {
+        //keep last value for timeout, because radar sends data only if change
+        if ((millis() - lastProtocolActivityMs) < NANORADAR_TIMEOUT_MS) {
+            return altitude;
+        }
+
+        return hasEverData ? RANGEFINDER_OUT_OF_RANGE : RANGEFINDER_NO_NEW_DATA;
+    }
+}
+
+virtualRangefinderVTable_t rangefinderNanoradarVtable = {
+    .detect = nanoradarRangefinderDetect,
+    .init = nanoradarRangefinderInit,
+    .update = nanoradarRangefinderUpdate,
+    .read = nanoradarRangefinderGetDistance
+};
+
+#endif
diff --git a/src/main/io/rangefinder_usd1_v0.c b/src/main/io/rangefinder_usd1_v0.c
new file mode 100644
index 00000000000..a509d4096c2
--- /dev/null
+++ b/src/main/io/rangefinder_usd1_v0.c
@@ -0,0 +1,136 @@
+/*
+ * 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 <stdbool.h>
+#include <ctype.h>
+
+
+#include "platform.h"
+#include "io/serial.h"
+#include "drivers/time.h"
+
+#if defined(USE_RANGEFINDER_USD1_V0)
+#include "drivers/rangefinder/rangefinder_virtual.h"
+
+#define USD1_HDR_V0 72 // Header Byte for beta V0 of USD1_Serial (0x48)
+
+#define USD1_PACKET_SIZE 3
+#define USD1_KEEP_DATA_TIMEOUT 2000 // 2s
+
+
+static serialPort_t * serialPort = NULL;
+static serialPortConfig_t * portConfig;
+
+static bool hasNewData = false;
+static bool hasEverData = false;
+static uint8_t  lineBuf[USD1_PACKET_SIZE];
+static int32_t sensorData = RANGEFINDER_NO_NEW_DATA;
+static timeMs_t lastProtocolActivityMs;
+
+static bool usd1RangefinderDetect(void)
+{
+    portConfig = findSerialPortConfig(FUNCTION_RANGEFINDER);
+    if (!portConfig) {
+        return false;
+    }
+
+    return true;
+}
+
+static void usd1RangefinderInit(void)
+{
+    if (!portConfig) {
+        return;
+    }
+
+    serialPort = openSerialPort(portConfig->identifier, FUNCTION_RANGEFINDER, NULL, NULL, 115200, MODE_RXTX, SERIAL_NOT_INVERTED);
+    if (!serialPort) {
+        return;
+    }
+
+    lastProtocolActivityMs = 0;
+}
+
+static void usd1RangefinderUpdate(void)
+{
+    float sum = 0;
+    uint16_t count = 0;
+    uint8_t  index = 0;
+
+    while (serialRxBytesWaiting(serialPort) > 0) {
+        uint8_t c = serialRead(serialPort);
+
+        if (c == USD1_HDR_V0 && index == 0) {
+            lineBuf[index] = c;
+            index = 1;
+            continue;
+        }
+
+        if (index > 0) {
+            lineBuf[index] = c;
+            index++;
+            if (index == 3) {
+                index = 0;
+                sum += (float)((lineBuf[2]&0x7F) * 128 + (lineBuf[1]&0x7F));
+                count++;
+            }
+        }
+    }
+
+    if (count == 0) {
+        return;
+    }
+
+    hasNewData = true;
+    hasEverData = true;
+    lastProtocolActivityMs = millis();
+    sensorData = (int32_t)(2.5f * sum / (float)count);
+}
+
+static int32_t usd1RangefinderGetDistance(void)
+{
+    int32_t altitude = (sensorData > 0) ? (sensorData) : RANGEFINDER_OUT_OF_RANGE;
+
+    if (hasNewData) {
+        hasNewData = false;
+        return altitude;
+    }
+    else {
+        //keep last value for timeout, because radar sends data only if change
+        if ((millis() - lastProtocolActivityMs) < USD1_KEEP_DATA_TIMEOUT) {
+            return altitude;
+        }
+
+        return hasEverData ? RANGEFINDER_OUT_OF_RANGE : RANGEFINDER_NO_NEW_DATA;
+    }
+}
+
+virtualRangefinderVTable_t rangefinderUSD1Vtable = {
+    .detect = usd1RangefinderDetect,
+    .init = usd1RangefinderInit,
+    .update = usd1RangefinderUpdate,
+    .read = usd1RangefinderGetDistance
+};
+
+#endif
diff --git a/src/main/sensors/rangefinder.c b/src/main/sensors/rangefinder.c
index 88f027f1545..a721bb98634 100644
--- a/src/main/sensors/rangefinder.c
+++ b/src/main/sensors/rangefinder.c
@@ -130,6 +130,22 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar
                 rangefinderHardware = RANGEFINDER_BENEWAKE;
                 rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_VIRTUAL_TASK_PERIOD_MS));
             }
+#endif
+            break;
+        case RANGEFINDER_USD1_V0:
+#if defined(USE_RANGEFINDER_USD1_V0)
+            if (virtualRangefinderDetect(dev, &rangefinderUSD1Vtable)) {
+                rangefinderHardware = RANGEFINDER_USD1_V0;
+                rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_VIRTUAL_TASK_PERIOD_MS));
+            }
+#endif
+            break;
+        case RANGEFINDER_NANORADAR:
+#if defined(USE_RANGEFINDER_NANORADAR)
+            if (virtualRangefinderDetect(dev, &rangefinderNanoradarVtable)) {
+                rangefinderHardware = RANGEFINDER_NANORADAR;
+                rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_VIRTUAL_TASK_PERIOD_MS));
+            }
 #endif
             break;
 
diff --git a/src/main/sensors/rangefinder.h b/src/main/sensors/rangefinder.h
index 2888e7838af..4c2a4e629aa 100644
--- a/src/main/sensors/rangefinder.h
+++ b/src/main/sensors/rangefinder.h
@@ -32,6 +32,8 @@ typedef enum {
     RANGEFINDER_TOF10102I2C         = 7,
     RANGEFINDER_FAKE                = 8,
     RANGEFINDER_TERARANGER_EVO      = 9,
+    RANGEFINDER_USD1_V0             = 10,
+    RANGEFINDER_NANORADAR           = 11,
 } rangefinderType_e;
 
 typedef struct rangefinderConfig_s {
diff --git a/src/main/target/common.h b/src/main/target/common.h
index d146ebb9790..b550df42d0f 100644
--- a/src/main/target/common.h
+++ b/src/main/target/common.h
@@ -84,6 +84,8 @@
 #define USE_RANGEFINDER_US42
 #define USE_RANGEFINDER_TOF10120_I2C
 #define USE_RANGEFINDER_TERARANGER_EVO_I2C
+#define USE_RANGEFINDER_USD1_V0
+#define USE_RANGEFINDER_NANORADAR
 
 // Allow default optic flow boards
 #define USE_OPFLOW

From 3005c69f8f99616f3822056baebfa9f6cae980a8 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 9 Apr 2024 10:30:36 +0200
Subject: [PATCH 038/323] Some progress

---
 src/main/flight/pid.c | 50 ++++++++++++++++++++++++++++++++-----------
 1 file changed, 38 insertions(+), 12 deletions(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 19708b50f00..50c67c86074 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -108,7 +108,10 @@ typedef struct {
 
     smithPredictor_t smithPredictor;
 
-    float dampingFactor;
+    float dampingFactorPrevious;
+    float dampingFactorLockValue;
+    float dampingFactotLockUntilMs;
+    pt1Filter_t dampingFactorFilter;
 } pidState_t;
 
 STATIC_FASTRAM bool pidFiltersConfigured = false;
@@ -322,6 +325,7 @@ bool pidInitFilters(void)
 
     for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
         pt1FilterInit(&windupLpf[i], pidProfile()->iterm_relax_cutoff, US2S(refreshRate));
+        pt1FilterInit(&pidState[i].dampingFactorFilter, 2.0f, US2S(refreshRate));
     }
 
 #ifdef USE_ANTIGRAVITY
@@ -748,27 +752,49 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
     const float maxRate = currentControlRateProfile->stabilized.rates[axis] * 10.0f;
     const float dampingFactor = attenuation(rateTarget, maxRate / 2.5f);
 
-    if (fabsf(dampingFactor) < fabsf(pidState->dampingFactor)) {
-        pidState->dampingFactor = dampingFactor;
+    float dampingFactorP;
+    float dampingFactorD;
+    float dampingFactorI;
+
+    if (fabsf(dampingFactor) <= fabsf(pidState->dampingFactorPrevious)) {
+        dampingFactorP = dampingFactor;
+        dampingFactorD = dampingFactor;
+        dampingFactorI = dampingFactor;
+
+        pidState->dampingFactotLockUntilMs = millis() + scaleRangef(fabsf(dampingFactor), 1.0f, 0.0f, 0, 300);
+        pidState->dampingFactorLockValue = dampingFactor;
+
+        // pt1FilterReset(&pidState->dampingFactorFilter, pidState->dampingFactorPrevious);
+        pidState->dampingFactorPrevious = dampingFactor;
     } else {
-        pidState->dampingFactor = pidState->dampingFactor + (dampingFactor - pidState->dampingFactor) * dT * 10;
+        dampingFactorP = dampingFactor;
+        dampingFactorD = dampingFactor;
+
+        if (millis() > pidState->dampingFactorLockValue) {
+            dampingFactorI = dampingFactor;
+            pidState->dampingFactorPrevious = dampingFactor;
+        } else {
+            dampingFactorI = pidState->dampingFactorLockValue;
+        }
     }
 
-    float newDF = pidState->dampingFactor;
-    
-    DEBUG_SET(DEBUG_ALWAYS, axis * 2, dampingFactor * 1000);
-    DEBUG_SET(DEBUG_ALWAYS, (axis * 2) + 1, newDF * 1000);
+    if (axis == FD_ROLL) {
+        DEBUG_SET(DEBUG_ALWAYS, 0, pidState->dampingFactorPrevious * 1000);
+        DEBUG_SET(DEBUG_ALWAYS, 1, dampingFactorP * 1000);
+        DEBUG_SET(DEBUG_ALWAYS, 2, dampingFactorI * 1000);
+        DEBUG_SET(DEBUG_ALWAYS, 3, dampingFactorD * 1000);
+    }
 
     const float rateError = rateTarget - pidState->gyroRate;
-    const float newPTerm = pTermProcess(pidState, rateError, dT) * dampingFactor;
-    const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv) * dampingFactor;
+    const float newPTerm = pTermProcess(pidState, rateError, dT) * dampingFactorP;
+    const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv) * dampingFactorD;
     const float newFFTerm = rateTarget * pidState->kFF;
 
     /*
      * Integral should be updated only if axis Iterm is not frozen
      */
     if (!pidState->itermFreezeActive) {
-        pidState->errorGyroIf += rateError * pidState->kI * dT * dampingFactor;
+        pidState->errorGyroIf += rateError * pidState->kI * dT * dampingFactorI;
     }
 
     applyItermLimiting(pidState);
@@ -1273,7 +1299,7 @@ void pidInit(void)
 
     for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) {
 
-        pidState[axis].dampingFactor = 1.0f;
+        pidState[axis].dampingFactorPrevious = 1.0f;
 
     #ifdef USE_D_BOOST
         // Rate * 10 * 10. First 10 is to convert stick to DPS. Second 10 is to convert target to acceleration.

From bff23ee0e5b23110a8225535478177ed9824f88e Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 9 Apr 2024 13:53:25 +0200
Subject: [PATCH 039/323] Some updates

---
 src/main/flight/pid.c | 19 ++++++++++++++-----
 1 file changed, 14 insertions(+), 5 deletions(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 50c67c86074..90a31a4aa16 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -756,9 +756,16 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
     float dampingFactorD;
     float dampingFactorI;
 
+    /*
+    TODO conditions have to be reworked
+    when drowing, damp
+    when releasing, lock
+    locking procedure has to be separated from action
+    
+
+    */
+
     if (fabsf(dampingFactor) <= fabsf(pidState->dampingFactorPrevious)) {
-        dampingFactorP = dampingFactor;
-        dampingFactorD = dampingFactor;
         dampingFactorI = dampingFactor;
 
         pidState->dampingFactotLockUntilMs = millis() + scaleRangef(fabsf(dampingFactor), 1.0f, 0.0f, 0, 300);
@@ -767,9 +774,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
         // pt1FilterReset(&pidState->dampingFactorFilter, pidState->dampingFactorPrevious);
         pidState->dampingFactorPrevious = dampingFactor;
     } else {
-        dampingFactorP = dampingFactor;
-        dampingFactorD = dampingFactor;
-
+        
         if (millis() > pidState->dampingFactorLockValue) {
             dampingFactorI = dampingFactor;
             pidState->dampingFactorPrevious = dampingFactor;
@@ -778,6 +783,10 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
         }
     }
 
+    //P & D damping factors are always the same and based on current damping factor
+    dampingFactorP = dampingFactor;
+    dampingFactorD = dampingFactor;
+
     if (axis == FD_ROLL) {
         DEBUG_SET(DEBUG_ALWAYS, 0, pidState->dampingFactorPrevious * 1000);
         DEBUG_SET(DEBUG_ALWAYS, 1, dampingFactorP * 1000);

From aef3fbe3b966696cdd3fe5555d35dfce99749a71 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 9 Apr 2024 18:42:20 +0200
Subject: [PATCH 040/323] New conditions for Iterm dampener

---
 src/main/flight/pid.c | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 90a31a4aa16..436e4b17471 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -757,12 +757,12 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
     float dampingFactorI;
 
     /*
-    TODO conditions have to be reworked
-    when drowing, damp
-    when releasing, lock
-    locking procedure has to be separated from action
-    
+    New idea.... 
+    Iterm damping is applied (down to 0) when:
+    abs(error) > 10% rate and sticks were moved in the last 500ms (hard stop at this mark)
 
+    itermAttenuation  = MIN(curve(setpoint), (abs(error) > 10%) && (sticks were deflected in 500ms) ? 0 : 1)
+    
     */
 
     if (fabsf(dampingFactor) <= fabsf(pidState->dampingFactorPrevious)) {

From 3eece0a3773e5dc27dfe454f6199cc5ec541d41d Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Wed, 10 Apr 2024 12:39:32 +0200
Subject: [PATCH 041/323] Commit switch to longer buffer

---
 src/main/flight/adaptive_filter.c | 3 ---
 src/main/flight/adaptive_filter.h | 2 +-
 2 files changed, 1 insertion(+), 4 deletions(-)

diff --git a/src/main/flight/adaptive_filter.c b/src/main/flight/adaptive_filter.c
index e94df2d19fe..a8e3dec0293 100644
--- a/src/main/flight/adaptive_filter.c
+++ b/src/main/flight/adaptive_filter.c
@@ -157,11 +157,8 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) {
         arm_std_f32(tempBuffer, ADAPTIVE_FILTER_BUFFER_SIZE, &std);
 
         const float filteredStd = pt1FilterApply(&stdFilter[axis], std);
-
         const float error = filteredStd - adaptiveIntegratorTarget;
-
         const float adjustedError = error * axisAttenuationFactor[axis];
-
         const float timeAdjustedError = adjustedError * dT;
 
         //Put into integrator
diff --git a/src/main/flight/adaptive_filter.h b/src/main/flight/adaptive_filter.h
index 0a8696d90dd..76117850d3e 100644
--- a/src/main/flight/adaptive_filter.h
+++ b/src/main/flight/adaptive_filter.h
@@ -25,7 +25,7 @@
 #include "common/axis.h"
 #include "common/time.h"
 
-#define ADAPTIVE_FILTER_BUFFER_SIZE 32
+#define ADAPTIVE_FILTER_BUFFER_SIZE 64
 #define ADAPTIVE_FILTER_RATE_HZ 100
 #define ADAPTIVE_FILTER_LPF_HZ 1
 #define ADAPTIVE_FILTER_HPF_HZ 15

From 9ac3b540a1eabdb83ef6a4af2103667f9b898614 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Thu, 11 Apr 2024 11:59:06 +0200
Subject: [PATCH 042/323] Make the filter configurable

---
 src/main/fc/config.c              | 10 ++++++
 src/main/fc/settings.yaml         | 55 +++++++++++++++++++++++++++++++
 src/main/flight/adaptive_filter.c | 10 +++---
 src/main/flight/adaptive_filter.h |  4 ---
 src/main/sensors/gyro.c           | 16 +++++++--
 src/main/sensors/gyro.h           | 10 ++++++
 6 files changed, 93 insertions(+), 12 deletions(-)

diff --git a/src/main/fc/config.c b/src/main/fc/config.c
index c9a50c60a25..c403b0cfefd 100755
--- a/src/main/fc/config.c
+++ b/src/main/fc/config.c
@@ -190,6 +190,16 @@ uint32_t getGyroLooptime(void)
 
 void validateAndFixConfig(void)
 {
+
+    // gyroConfig()->adaptiveFilterMinHz has to be at least 5 units lower than gyroConfig()->gyro_main_lpf_hz
+    if (gyroConfig()->adaptiveFilterMinHz + 5 > gyroConfig()->gyro_main_lpf_hz) {
+        gyroConfigMutable()->adaptiveFilterMinHz = gyroConfig()->gyro_main_lpf_hz - 5;
+    }
+    //gyroConfig()->adaptiveFilterMaxHz has to be at least 5 units higher than gyroConfig()->gyro_main_lpf_hz
+    if (gyroConfig()->adaptiveFilterMaxHz - 5 < gyroConfig()->gyro_main_lpf_hz) {
+        gyroConfigMutable()->adaptiveFilterMaxHz = gyroConfig()->gyro_main_lpf_hz + 5;
+    }
+
     if (accelerometerConfig()->acc_notch_cutoff >= accelerometerConfig()->acc_notch_hz) {
         accelerometerConfigMutable()->acc_notch_hz = 0;
     }
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 9fd6c10e860..4196a7cefc3 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -330,6 +330,61 @@ groups:
         field: gravity_cmss_cal
         min: 0
         max: 2000
+      - name: gyro_adaptive_filter
+        description: "Enable adaptive filter for gyro data"
+        default_value: OFF
+        field: adaptiveFilterEnabled
+        type: bool
+        condition: USE_ADAPTIVE_FILTER
+      - name: gyro_adaptive_filter_target
+        description: "Target value for adaptive filter"
+        default_value: 3.5
+        field: adaptiveFilterTarget
+        min: 2
+        max: 6
+        condition: USE_ADAPTIVE_FILTER
+      - name: gyro_adaptive_filter_min_hz
+        description: "Minimum frequency for adaptive filter"
+        default_value: 50
+        field: adaptiveFilterMinHz
+        min: 30
+        max: 250
+        condition: USE_ADAPTIVE_FILTER
+      - name: gyro_adaptive_filter_max_hz
+        description: "Maximum frequency for adaptive filter"
+        default_value: 150
+        field: adaptiveFilterMaxHz
+        min: 100
+        max: 500
+        condition: USE_ADAPTIVE_FILTER
+      - name: gyro_adaptive_filter_std_lpf_hz
+        description: "Standard deviation low pass filter cutoff frequency"
+        default_value: 1
+        field: adaptiveFilterStdLpfHz
+        min: 0
+        max: 5
+        condition: USE_ADAPTIVE_FILTER
+      - name: gyro_adaptive_filter_hpf_hz
+        description: "High pass filter cutoff frequency"
+        default_value: 15
+        field: adaptiveFilterHpfHz
+        min: 1
+        max: 50
+        condition: USE_ADAPTIVE_FILTER
+      - name: gyro_adaptive_filter_integrator_threshold_high
+        description: "High threshold for adaptive filter integrator"
+        default_value: 5
+        field: adaptiveFilterIntegratorThresholdHigh
+        min: 1
+        max: 10
+        condition: USE_ADAPTIVE_FILTER
+      - name: gyro_adaptive_filter_integrator_threshold_low
+        description: "Low threshold for adaptive filter integrator"
+        default_value: -2
+        field: adaptiveFilterIntegratorThresholdLow
+        min: -10
+        max: 0
+        condition: USE_ADAPTIVE_FILTER
 
   - name: PG_ADC_CHANNEL_CONFIG
     type: adcChannelConfig_t
diff --git a/src/main/flight/adaptive_filter.c b/src/main/flight/adaptive_filter.c
index a8e3dec0293..4770e9763ab 100644
--- a/src/main/flight/adaptive_filter.c
+++ b/src/main/flight/adaptive_filter.c
@@ -73,7 +73,7 @@ void adaptiveFilterPush(const flight_dynamics_index_t index, const float value)
     if (!hpfFilterInitialized) {
         //Initialize the filter
         for (flight_dynamics_index_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-            pt1FilterInit(&hpfFilter[axis], ADAPTIVE_FILTER_HPF_HZ, US2S(getLooptime()));
+            pt1FilterInit(&hpfFilter[axis], gyroConfig()->adaptiveFilterHpfHz, US2S(getLooptime()));
         }
         hpfFilterInitialized = 1;
     }
@@ -121,7 +121,7 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) {
 
         //Initialize the filter
         for (flight_dynamics_index_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-            pt1FilterInit(&stdFilter[axis], ADAPTIVE_FILTER_LPF_HZ, 1.0f / ADAPTIVE_FILTER_RATE_HZ);
+            pt1FilterInit(&stdFilter[axis], gyroConfig()->adaptiveFilterStdLpfHz, 1.0f / ADAPTIVE_FILTER_RATE_HZ);
         }
         adaptiveFilterInitialized = 1;
     }
@@ -167,14 +167,12 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) {
         combinedStd += std;
     }
 
-    //TODO filter gets updated only when ARMED
-
-    if (adaptiveFilterIntegrator > ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD_HIGH) {
+    if (adaptiveFilterIntegrator > gyroConfig()->adaptiveFilterIntegratorThresholdHigh) {
         //In this case there is too much noise, we need to lower the LPF frequency
         currentLpf = constrainf(currentLpf - 1.0f, minLpf, maxLpf);
         gyroUpdateDynamicLpf(currentLpf);
         adaptiveFilterResetIntegrator();
-    } else if (adaptiveFilterIntegrator < ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD_LOW) {
+    } else if (adaptiveFilterIntegrator < gyroConfig()->adaptiveFilterIntegratorThresholdLow) {
         //In this case there is too little noise, we can to increase the LPF frequency
         currentLpf = constrainf(currentLpf + 1.0f, minLpf, maxLpf);
         gyroUpdateDynamicLpf(currentLpf);
diff --git a/src/main/flight/adaptive_filter.h b/src/main/flight/adaptive_filter.h
index 76117850d3e..c8696a91f8a 100644
--- a/src/main/flight/adaptive_filter.h
+++ b/src/main/flight/adaptive_filter.h
@@ -27,10 +27,6 @@
 
 #define ADAPTIVE_FILTER_BUFFER_SIZE 64
 #define ADAPTIVE_FILTER_RATE_HZ 100
-#define ADAPTIVE_FILTER_LPF_HZ 1
-#define ADAPTIVE_FILTER_HPF_HZ 15
-#define ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD_HIGH 5.0f
-#define ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD_LOW -2.0f
 
 void adaptiveFilterPush(const flight_dynamics_index_t index, const float value);
 void adaptiveFilterPushRate(const flight_dynamics_index_t index, const float rate, const uint8_t configRate);
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 8585d3a0522..fd2036ef9a2 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -96,7 +96,7 @@ EXTENDED_FASTRAM secondaryDynamicGyroNotchState_t secondaryDynamicGyroNotchState
 
 #endif
 
-PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 8);
+PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 9);
 
 PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .gyro_anti_aliasing_lpf_hz = SETTING_GYRO_ANTI_ALIASING_LPF_HZ_DEFAULT,
@@ -123,6 +123,16 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .init_gyro_cal_enabled = SETTING_INIT_GYRO_CAL_DEFAULT,
     .gyro_zero_cal = {SETTING_GYRO_ZERO_X_DEFAULT, SETTING_GYRO_ZERO_Y_DEFAULT, SETTING_GYRO_ZERO_Z_DEFAULT},
     .gravity_cmss_cal = SETTING_INS_GRAVITY_CMSS_DEFAULT,
+#ifdef USE_ADAPTIVE_FILTER
+    .adaptiveFilterEnabled = SETTING_GYRO_ADAPTIVE_FILTER_DEFAULT,
+    .adaptiveFilterTarget = SETTING_GYRO_ADAPTIVE_FILTER_TARGET_DEFAULT,
+    .adaptiveFilterMinHz = SETTING_GYRO_ADAPTIVE_FILTER_MIN_HZ_DEFAULT,
+    .adaptiveFilterMaxHz = SETTING_GYRO_ADAPTIVE_FILTER_MAX_HZ_DEFAULT,
+    .adaptiveFilterStdLpfHz = SETTING_GYRO_ADAPTIVE_FILTER_STD_LPF_HZ_DEFAULT,
+    .adaptiveFilterHpfHz = SETTING_GYRO_ADAPTIVE_FILTER_HPF_HZ_DEFAULT,
+    .adaptiveFilterIntegratorThresholdHigh = SETTING_GYRO_ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD_HIGH_DEFAULT,
+    .adaptiveFilterIntegratorThresholdLow  = SETTING_GYRO_ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD_LOW_DEFAULT,
+#endif
 );
 
 STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware)
@@ -251,7 +261,9 @@ static void gyroInitFilters(void)
     initGyroFilter(&gyroLpf2ApplyFn, gyroLpf2State, gyroConfig()->gyro_main_lpf_hz, getLooptime());
 
 #ifdef USE_ADAPTIVE_FILTER
-    adaptiveFilterSetDefaultFrequency(gyroConfig()->gyro_main_lpf_hz, 50, 150);
+    if (gyroConfig()->adaptiveFilterEnabled) {
+        adaptiveFilterSetDefaultFrequency(gyroConfig()->gyro_main_lpf_hz, gyroConfig()->adaptiveFilterMinHz, gyroConfig()->adaptiveFilterMaxHz);
+    }
 #endif
 
 #ifdef USE_GYRO_KALMAN
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index 2ea70c589da..75ff7983372 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -87,6 +87,16 @@ typedef struct gyroConfig_s {
     bool init_gyro_cal_enabled;
     int16_t gyro_zero_cal[XYZ_AXIS_COUNT];
     float gravity_cmss_cal;
+#ifdef USE_ADAPTIVE_FILTER
+    uint8_t adaptiveFilterEnabled;
+    float adaptiveFilterTarget;
+    uint16_t adaptiveFilterMinHz;
+    uint16_t adaptiveFilterMaxHz;
+    float adaptiveFilterStdLpfHz;
+    float adaptiveFilterHpfHz;
+    float adaptiveFilterIntegratorThresholdHigh;
+    float adaptiveFilterIntegratorThresholdLow;
+#endif
 } gyroConfig_t;
 
 PG_DECLARE(gyroConfig_t, gyroConfig);

From 8c8840f432f730f9985812ef528bd4ccd4c3d7c3 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Thu, 11 Apr 2024 15:05:09 +0200
Subject: [PATCH 043/323] Update defaults

---
 src/main/fc/settings.yaml | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 4196a7cefc3..a495e691cbc 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -340,7 +340,7 @@ groups:
         description: "Target value for adaptive filter"
         default_value: 3.5
         field: adaptiveFilterTarget
-        min: 2
+        min: 1
         max: 6
         condition: USE_ADAPTIVE_FILTER
       - name: gyro_adaptive_filter_min_hz
@@ -359,21 +359,21 @@ groups:
         condition: USE_ADAPTIVE_FILTER
       - name: gyro_adaptive_filter_std_lpf_hz
         description: "Standard deviation low pass filter cutoff frequency"
-        default_value: 1
+        default_value: 2
         field: adaptiveFilterStdLpfHz
         min: 0
-        max: 5
+        max: 10
         condition: USE_ADAPTIVE_FILTER
       - name: gyro_adaptive_filter_hpf_hz
         description: "High pass filter cutoff frequency"
-        default_value: 15
+        default_value: 10
         field: adaptiveFilterHpfHz
         min: 1
         max: 50
         condition: USE_ADAPTIVE_FILTER
       - name: gyro_adaptive_filter_integrator_threshold_high
         description: "High threshold for adaptive filter integrator"
-        default_value: 5
+        default_value: 4
         field: adaptiveFilterIntegratorThresholdHigh
         min: 1
         max: 10

From 9db5a5446b80331c37d61900d15555722601b592 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Thu, 11 Apr 2024 20:57:12 +0200
Subject: [PATCH 044/323] Version to 7.1.1

---
 CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index d8247e7cb5a..f74527e5f1b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -51,7 +51,7 @@ else()
     endif()
 endif()
 
-project(INAV VERSION 7.1.0)
+project(INAV VERSION 7.1.1)
 
 enable_language(ASM)
 

From f40361034d28f1273bcb3ff7aa56b293b8166ecb Mon Sep 17 00:00:00 2001
From: 0crap <31951195+0crap@users.noreply.github.com>
Date: Mon, 8 Apr 2024 12:54:16 +0200
Subject: [PATCH 045/323] Update Fixed Wing Landing.md

Simple typo fix.
---
 docs/Fixed Wing Landing.md | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/docs/Fixed Wing Landing.md b/docs/Fixed Wing Landing.md
index 4ccb3249536..118221229c5 100644
--- a/docs/Fixed Wing Landing.md	
+++ b/docs/Fixed Wing Landing.md	
@@ -54,7 +54,7 @@ This means that practically 4 landing directions can be saved.
 > [!CAUTION]
 > The Configuator automatically determines the ground altitude based on databases on the Internet, which may be inaccurate. Please always compare with the measured GPS altitude at the landing site to avoid crashes.
 
-### Global paramters
+### Global parameters
 
 All settings are available via “Advanced Tuning” in the Configurator.
 
@@ -87,7 +87,7 @@ If the altitude of the waypoint and the "Approach Altitude" are different, the a
 
 ## Logic Conditions
 
-The current landing state can be retrieved via ID 41 in "Flight" (FW Land State). This allows additional actions to be executed according to the landing phases, e.g. deplyoment of the landing flaps.
+The current landing state can be retrieved via ID 41 in "Flight" (FW Land State). This allows additional actions to be executed according to the landing phases, e.g. deployment of the landing flaps.
 
 | Returned value | State |
 | --- | --- |

From 67b688c6b4b0e365ebf200bac175609fec237515 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Fri, 22 Mar 2024 20:01:09 +0100
Subject: [PATCH 046/323] No longer require MAG to unlock GPS related flight
 modes

---
 src/main/fc/fc_msp_box.c | 3 ---
 1 file changed, 3 deletions(-)

diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c
index c61c2aa19e9..f1c72458bff 100644
--- a/src/main/fc/fc_msp_box.c
+++ b/src/main/fc/fc_msp_box.c
@@ -220,9 +220,6 @@ void initActiveBoxIds(void)
 
     const bool navFlowDeadReckoning = sensors(SENSOR_OPFLOW) && sensors(SENSOR_ACC) && positionEstimationConfig()->allow_dead_reckoning;
     bool navReadyPosControl = sensors(SENSOR_ACC) && feature(FEATURE_GPS);
-    if (STATE(MULTIROTOR)) {
-        navReadyPosControl = navReadyPosControl && getHwCompassStatus() != HW_SENSOR_NONE;
-    }
 
     if (STATE(ALTITUDE_CONTROL) && navReadyAltControl && (navReadyPosControl || navFlowDeadReckoning)) {
         ADD_ACTIVE_BOX(BOXNAVPOSHOLD);

From 8eeea07da815538a861902477e9c14fa4d76ea5c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Pawe=C5=82=20Spychalski?= <pspychalski@gmail.com>
Date: Thu, 11 Apr 2024 21:01:07 +0200
Subject: [PATCH 047/323] Merge pull request #9896 from
 iNavFlight/b14ckyy-Update-FW-landing-Doc

Update Fixed Wing Landing.md
---
 docs/Fixed Wing Landing.md | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/docs/Fixed Wing Landing.md b/docs/Fixed Wing Landing.md
index 118221229c5..10eff364f34 100644
--- a/docs/Fixed Wing Landing.md	
+++ b/docs/Fixed Wing Landing.md	
@@ -5,6 +5,8 @@
 INAV supports advanced automatic landings for fixed wing aircraft from version 7.1.
 The procedure is based on landings for man-carrying aircraft, so that safe landings at a specific location are possible.
 Supported are landings at safehome after "Return to Home" or at a defined LAND waypoint for missions. 
+Every landing locations can be defined with a target point and 2 different approach headings (colinear to the landing strips) with exclusive direction or opposite directions allowed. 
+This enables up to 4 different approach directions, based on the landing site and surrounding area. 
 
 ## General procedure:
 
@@ -34,7 +36,7 @@ The following graphics illustrate the process:
 
 ### The following parameters are set for each landing site (Safefome/LAND waypoint):
 
-All settings can also be conveniently made in the Configurator via Missionplanner.
+All settings can also be conveniently made in the Configurator via Mission Control.
 
 CLI command `fwapproach`:
 `fwapproach <index> <Approach altitude> <Land altitude> <Approach direction> <approach heading 1> <approach heading 2> <sea level>`

From 80adbbc1e52e875070c8fa04a93d241817b9d276 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Pawe=C5=82=20Spychalski?= <pspychalski@gmail.com>
Date: Thu, 11 Apr 2024 21:01:57 +0200
Subject: [PATCH 048/323] Merge pull request #9890 from
 iNavFlight/MrD_Add-extra-description-to-the-min-ground-speed-parameter

Add extra description to nav_min_ground_speed parameter
---
 docs/Settings.md          | 2 +-
 src/main/fc/settings.yaml | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index 1f5ccde178e..8608eb01f51 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -3764,7 +3764,7 @@ When ON, NAV engine will slow down when switching to the next waypoint. This pri
 
 ### nav_min_ground_speed
 
-Minimum ground speed for navigation flight modes [m/s]. Default 7 m/s.
+Minimum ground speed for navigation flight modes [m/s]. Currently, this only affects fixed wing. Default 7 m/s.
 
 | Default | Min | Max |
 | --- | --- | --- |
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 0f63c66ddd5..66abea46399 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -2493,7 +2493,7 @@ groups:
         min: 10
         max: 2000
       - name: nav_min_ground_speed
-        description: "Minimum ground speed for navigation flight modes [m/s]. Default 7 m/s."
+        description: "Minimum ground speed for navigation flight modes [m/s]. Currently, this only affects fixed wing. Default 7 m/s."
         default_value: 7
         field: general.min_ground_speed
         min: 6

From db13d5da541342fa16b6c9d435b1e83e4fa7826f Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Thu, 11 Apr 2024 22:57:28 +0100
Subject: [PATCH 049/323] Update navigation.c

---
 src/main/navigation/navigation.c | 39 +++++---------------------------
 1 file changed, 6 insertions(+), 33 deletions(-)

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 9e81024c3af..69b4a112b65 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -1142,7 +1142,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .mwError = MW_NAV_ERROR_NONE,
         .onEvent = {
             [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_FW_LANDING_FLARE,   // re-process the state
-            [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_IDLE,
             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
         }
     },
@@ -2043,21 +2042,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navig
 
     const navigationFSMEvent_t landEvent = navOnEnteringState_NAV_STATE_RTH_LANDING(previousState);
 
-#ifdef USE_FW_AUTOLAND
-    if (landEvent == NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING) {
-        return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING;
-    } else 
-#endif
-    if (landEvent == NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME) {
-        return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
-    } else if (landEvent == NAV_FSM_EVENT_SUCCESS) {
+    if (landEvent == NAV_FSM_EVENT_SUCCESS) {
         // Landing controller returned success - invoke RTH finishing state and finish the waypoint
         navOnEnteringState_NAV_STATE_RTH_FINISHING(previousState);
-        return NAV_FSM_EVENT_SUCCESS;
-    }
-    else {
-        return NAV_FSM_EVENT_NONE;
     }
+
+    return landEvent;
 }
 
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_NEXT(navigationFSMState_t previousState)
@@ -2376,12 +2366,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(nav
         return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT;
     }
 
-    if (isLandingDetected()) {
-        posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
-        //disarm(DISARM_LANDING);
-        return NAV_FSM_EVENT_SWITCH_TO_IDLE;
-    }
-
     if (getLandAltitude() <= fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landAlt + navFwAutolandConfig()->glideAltitude - (fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->isSeaLevelRef ? GPS_home.alt : 0)) {
         resetPositionController();
         posControl.cruise.course = posControl.fwLandState.landingDirection;
@@ -2427,12 +2411,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE(naviga
         return NAV_FSM_EVENT_SUCCESS;
     }
 
-    if (isLandingDetected()) {
-        posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
-        //disarm(DISARM_LANDING);
-        return NAV_FSM_EVENT_SWITCH_TO_IDLE;
-    }
-
     setDesiredPosition(NULL, posControl.cruise.course, NAV_POS_UPDATE_HEADING);
     return NAV_FSM_EVENT_NONE;
 }
@@ -2441,11 +2419,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FLARE(naviga
 {
     UNUSED(previousState);
 
-    if (isLandingDetected()) {
-        posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
-        //disarm(DISARM_LANDING);
-        return NAV_FSM_EVENT_SUCCESS;
-    }
     setDesiredPosition(NULL, posControl.cruise.course, NAV_POS_UPDATE_HEADING);
 
     return NAV_FSM_EVENT_NONE;
@@ -4083,7 +4056,7 @@ bool isNavHoldPositionActive(void)
     // Also hold position during emergency landing if position valid
     return (FLIGHT_MODE(NAV_RTH_MODE) && !posControl.flags.rthTrackbackActive) ||
             FLIGHT_MODE(NAV_POSHOLD_MODE) ||
-            (posControl.navState == NAV_STATE_FW_LANDING_CLIMB_TO_LOITER || posControl.navState == NAV_STATE_FW_LANDING_LOITER) || 
+            (posControl.navState == NAV_STATE_FW_LANDING_CLIMB_TO_LOITER || posControl.navState == NAV_STATE_FW_LANDING_LOITER) ||
             navigationIsExecutingAnEmergencyLanding();
 }
 
@@ -4123,7 +4096,7 @@ bool isWaypointNavTrackingActive(void)
     // is set from current position not previous WP. Works for WP Restart intermediate WP as well as first mission WP.
     // (NAV_WP_MODE flag isn't set until WP initialisation is finished, i.e. after calculateAndSetActiveWaypoint called)
 
-    return FLIGHT_MODE(NAV_WP_MODE) 
+    return FLIGHT_MODE(NAV_WP_MODE)
     || posControl.navState == NAV_STATE_FW_LANDING_APPROACH
     || (posControl.flags.rthTrackbackActive && posControl.activeRthTBPointIndex != posControl.rthTBLastSavedIndex);
 }
@@ -5011,7 +4984,7 @@ bool navigationRTHAllowsLanding(void)
         return false;
     }
 #endif
-    
+
     // WP mission RTH landing setting
     if (isWaypointMissionRTHActive() && isWaypointMissionValid()) {
         return posControl.waypointList[posControl.startWpIndex + posControl.waypointCount - 1].p1 > 0;

From 354eb2cfedb125bf5e03a3c998d692480052154c Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Fri, 12 Apr 2024 23:18:02 +0100
Subject: [PATCH 050/323] updates

---
 src/main/fc/fc_init.c                    |  1 -
 src/main/io/osd.c                        | 12 ++---
 src/main/navigation/navigation.c         | 68 ++++++++++++++++++------
 src/main/navigation/navigation.h         |  2 +-
 src/main/navigation/navigation_private.h | 22 ++++----
 5 files changed, 71 insertions(+), 34 deletions(-)

diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index 849cdc2b7c0..0d4897dd557 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -60,7 +60,6 @@
 #include "drivers/pwm_esc_detect.h"
 #include "drivers/pwm_mapping.h"
 #include "drivers/pwm_output.h"
-#include "drivers/pwm_output.h"
 #include "drivers/sensor.h"
 #include "drivers/serial.h"
 #include "drivers/serial_softserial.h"
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index f0cc50bbaa1..435b09f9e3f 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -1003,7 +1003,7 @@ static const char * divertingToSafehomeMessage(void)
 	    return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME);
 	}
 #endif
-	return NULL;   
+	return NULL;
 }
 
 
@@ -1028,7 +1028,7 @@ static const char * navigationStateMessage(void)
                         linearDescentMessageMs = millis() + 5000; // Show message for 5 seconds.
 
                     return OSD_MESSAGE_STR(OSD_MSG_RTH_LINEAR_DESCENT);
-                } else 
+                } else
                     return OSD_MESSAGE_STR(OSD_MSG_HEADING_HOME);
             }
         case MW_NAV_STATE_HOLD_INFINIT:
@@ -1069,7 +1069,7 @@ static const char * navigationStateMessage(void)
             // If there is a FS landing delay occurring. That is handled by the calling function.
             if (posControl.landingDelay > 0)
                 break;
-            
+
             return OSD_MESSAGE_STR(OSD_MSG_PREPARING_LAND);
         }
         case MW_NAV_STATE_LAND_START_DESCENT:
@@ -2277,7 +2277,7 @@ static bool osdDrawSingleElement(uint8_t item)
         {
             char *p = "ACRO";
 #ifdef USE_FW_AUTOLAND
-            if (FLIGHT_MODE(NAV_FW_AUTOLAND)) 
+            if (FLIGHT_MODE(NAV_FW_AUTOLAND))
                 p = "LAND";
             else
 #endif
@@ -5177,11 +5177,11 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                     tfp_sprintf(messageBuf, "LANDING DELAY: %3u SECONDS", remainingHoldSec);
 
                     messages[messageCount++] = messageBuf;
-                } 
+                }
 
                 else {
 #ifdef USE_FW_AUTOLAND
-                    if (canFwLandCanceld()) {
+                    if (canFwLandingBeCancelled()) {
                          messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MOVE_STICKS);
                     } else if (!FLIGHT_MODE(NAV_FW_AUTOLAND)) {
 #endif
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 69b4a112b65..284afc74109 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -356,6 +356,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_LOITER(navig
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(navigationFSMState_t previousState);
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE(navigationFSMState_t previousState);
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FLARE(navigationFSMState_t previousState);
+static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FINISHED(navigationFSMState_t previousState);
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(navigationFSMState_t previousState);
 #endif
 
@@ -1107,6 +1108,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
             [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]             = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
             [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD]                   = NAV_STATE_COURSE_HOLD_INITIALIZE,
             [NAV_FSM_EVENT_SWITCH_TO_CRUISE]                        = NAV_STATE_CRUISE_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED] = NAV_STATE_FW_LANDING_FINISHED,
             [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT]    = NAV_STATE_FW_LANDING_ABORT,
         }
     },
@@ -1128,6 +1130,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
             [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]             = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
             [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD]                   = NAV_STATE_COURSE_HOLD_INITIALIZE,
             [NAV_FSM_EVENT_SWITCH_TO_CRUISE]                        = NAV_STATE_CRUISE_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED] = NAV_STATE_FW_LANDING_FINISHED,
             [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT]    = NAV_STATE_FW_LANDING_ABORT,
         }
     },
@@ -1141,10 +1144,26 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
         .mwError = MW_NAV_ERROR_NONE,
         .onEvent = {
-            [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_FW_LANDING_FLARE,   // re-process the state
-            [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
+            [NAV_FSM_EVENT_TIMEOUT]                                 = NAV_STATE_FW_LANDING_FLARE,   // re-process the state
+            [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED] = NAV_STATE_FW_LANDING_FINISHED,
+            [NAV_FSM_EVENT_SWITCH_TO_IDLE]                          = NAV_STATE_IDLE,
         }
     },
+
+    [NAV_STATE_FW_LANDING_FINISHED] = {
+        .persistentId = NAV_PERSISTENT_ID_FW_LANDING_FINISHED,
+        .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_FINISHED,
+        .timeoutMs = 10,
+        .stateFlags = NAV_REQUIRE_ANGLE,
+        .mapToFlightModes = NAV_FW_AUTOLAND,
+        .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
+        .mwError = MW_NAV_ERROR_NONE,
+        .onEvent = {
+            [NAV_FSM_EVENT_TIMEOUT]                                 = NAV_STATE_FW_LANDING_FINISHED,   // re-process the state
+            [NAV_FSM_EVENT_SWITCH_TO_IDLE]                          = NAV_STATE_IDLE,
+        }
+    },
+
     [NAV_STATE_FW_LANDING_ABORT] = {
         .persistentId = NAV_PERSISTENT_ID_FW_LANDING_ABORT,
         .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_ABORT,
@@ -1697,9 +1716,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(na
 
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationFSMState_t previousState)
 {
-#ifndef USE_FW_AUTOLAND
     UNUSED(previousState);
-#endif
 
     //On ROVER and BOAT we immediately switch to the next event
     if (!STATE(ALTITUDE_CONTROL)) {
@@ -1712,7 +1729,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
 
     /* If position sensors unavailable - land immediately (wait for timeout on GPS)
      * Continue to check for RTH sanity during landing */
-    if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout() || (previousState != NAV_STATE_WAYPOINT_REACHED && !validateRTHSanityChecker())) {
+    if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout() || (FLIGHT_MODE(NAV_RTH_MODE) && !validateRTHSanityChecker())) {
         return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
     }
 
@@ -1731,7 +1748,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
         shIdx = posControl.safehomeState.index;
         missionFwLandConfigStartIdx = MAX_SAFE_HOMES;
 #endif
-        if (previousState == NAV_STATE_WAYPOINT_REACHED && missionIdx >= 0) {
+        if (FLIGHT_MODE(NAV_WP_MODE) && missionIdx >= 0) {
             approachSettingIdx = missionFwLandConfigStartIdx + missionIdx;
         } else if (shIdx >= 0) {
             approachSettingIdx = shIdx;
@@ -1739,7 +1756,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
 
         if (!posControl.fwLandState.landAborted && approachSettingIdx >= 0 && (fwAutolandApproachConfig(approachSettingIdx)->landApproachHeading1 != 0 || fwAutolandApproachConfig(approachSettingIdx)->landApproachHeading2 != 0)) {
 
-            if (previousState == NAV_STATE_WAYPOINT_REACHED) {
+            if (FLIGHT_MODE(NAV_WP_MODE)) {
                 posControl.fwLandState.landPos = posControl.activeWaypoint.pos;
                 posControl.fwLandState.landWp = true;
             } else {
@@ -2032,8 +2049,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi
 
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navigationFSMState_t previousState)
 {
-    UNUSED(previousState);
-
 #ifdef USE_FW_AUTOLAND
     if (posControl.fwLandState.landAborted) {
         return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
@@ -2043,8 +2058,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navig
     const navigationFSMEvent_t landEvent = navOnEnteringState_NAV_STATE_RTH_LANDING(previousState);
 
     if (landEvent == NAV_FSM_EVENT_SUCCESS) {
-        // Landing controller returned success - invoke RTH finishing state and finish the waypoint
+        // Landing controller returned success - invoke RTH finish states and finish the waypoint
         navOnEnteringState_NAV_STATE_RTH_FINISHING(previousState);
+        navOnEnteringState_NAV_STATE_RTH_FINISHED(previousState);
     }
 
     return landEvent;
@@ -2358,6 +2374,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(nav
 {
     UNUSED(previousState);
 
+    if (STATE(LANDING_DETECTED)) {
+        return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED;
+    }
+
     if ((posControl.flags.estHeadingStatus == EST_NONE) || checkForPositionSensorTimeout()) {
         return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
     }
@@ -2402,6 +2422,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE(naviga
 {
     UNUSED(previousState);
 
+    if (STATE(LANDING_DETECTED)) {
+        return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED;
+    }
+
     if (isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) {
         return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT;
     }
@@ -2419,11 +2443,24 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FLARE(naviga
 {
     UNUSED(previousState);
 
+    if (STATE(LANDING_DETECTED)) {
+        return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED;
+    }
+
     setDesiredPosition(NULL, posControl.cruise.course, NAV_POS_UPDATE_HEADING);
 
     return NAV_FSM_EVENT_NONE;
 }
 
+static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FINISHED(navigationFSMState_t previousState)
+{
+    UNUSED(previousState);
+
+    posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
+
+    return NAV_FSM_EVENT_NONE;
+}
+
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(navigationFSMState_t previousState)
 {
     UNUSED(previousState);
@@ -4896,9 +4933,9 @@ void abortForcedRTH(void)
 
 rthState_e getStateOfForcedRTH(void)
 {
-    /* If forced RTH activated and in AUTO_RTH or EMERG state */
+    /* If forced RTH activated and in AUTO_RTH, EMERG state or FW Auto Landing */
     if (posControl.flags.forcedRTHActivated && ((navGetStateFlags(posControl.navState) & (NAV_AUTO_RTH | NAV_CTL_EMERG | NAV_MIXERAT)) || FLIGHT_MODE(NAV_FW_AUTOLAND))) {
-        if (posControl.navState == NAV_STATE_RTH_FINISHED || posControl.navState == NAV_STATE_EMERGENCY_LANDING_FINISHED) {
+        if (posControl.navState == NAV_STATE_RTH_FINISHED || posControl.navState == NAV_STATE_EMERGENCY_LANDING_FINISHED || posControl.navState == NAV_STATE_FW_LANDING_FINISHED) {
             return RTH_HAS_LANDED;
         }
         else {
@@ -5142,12 +5179,9 @@ void resetFwAutolandApproach(int8_t idx)
     }
 }
 
-bool canFwLandCanceld(void)
+bool canFwLandingBeCancelled(void)
 {
-    return posControl.navState == NAV_STATE_FW_LANDING_CLIMB_TO_LOITER
-    || posControl.navState == NAV_STATE_FW_LANDING_LOITER
-    || posControl.navState == NAV_STATE_FW_LANDING_APPROACH
-    || posControl.navState == NAV_STATE_FW_LANDING_GLIDE;
+    return FLIGHT_MODE(NAV_FW_AUTOLAND) && posControl.navState != NAV_STATE_FW_LANDING_FLARE;
 }
 
 #endif
diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
index 6ebdaeccead..19cc5022ed3 100644
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@@ -693,7 +693,7 @@ uint8_t getActiveWpNumber(void);
 int32_t navigationGetHomeHeading(void);
 
 #ifdef USE_FW_AUTOLAND
-bool canFwLandCanceld(void);
+bool canFwLandingBeCancelled(void);
 #endif
 
 /* Compatibility data */
diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
index d716c58c496..bc5a1482894 100644
--- a/src/main/navigation/navigation_private.h
+++ b/src/main/navigation/navigation_private.h
@@ -165,16 +165,18 @@ typedef enum {
     NAV_FSM_EVENT_STATE_SPECIFIC_3,             // State-specific event
     NAV_FSM_EVENT_STATE_SPECIFIC_4,             // State-specific event
     NAV_FSM_EVENT_STATE_SPECIFIC_5,             // State-specific event
-    NAV_FSM_EVENT_STATE_SPECIFIC_6,             // State-specific event
+
+    NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT = NAV_FSM_EVENT_STATE_SPECIFIC_1,
+    NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED = NAV_FSM_EVENT_STATE_SPECIFIC_2,
+    NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME = NAV_FSM_EVENT_STATE_SPECIFIC_1,
+    NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND = NAV_FSM_EVENT_STATE_SPECIFIC_2,
+    NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED = NAV_FSM_EVENT_STATE_SPECIFIC_3,
+    NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE = NAV_FSM_EVENT_STATE_SPECIFIC_1,
+    NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK = NAV_FSM_EVENT_STATE_SPECIFIC_2,
     NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME = NAV_FSM_EVENT_STATE_SPECIFIC_3,
-    NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING = NAV_FSM_EVENT_STATE_SPECIFIC_1,
-    NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND = NAV_FSM_EVENT_STATE_SPECIFIC_1,
-    NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED = NAV_FSM_EVENT_STATE_SPECIFIC_2,
-    NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME = NAV_FSM_EVENT_STATE_SPECIFIC_3,
     NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME = NAV_FSM_EVENT_STATE_SPECIFIC_4,
-    NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK = NAV_FSM_EVENT_STATE_SPECIFIC_5,
-    NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE = NAV_FSM_EVENT_STATE_SPECIFIC_6,
-    NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT = NAV_FSM_EVENT_STATE_SPECIFIC_6,
+    NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING = NAV_FSM_EVENT_STATE_SPECIFIC_5,
+
     NAV_FSM_EVENT_COUNT,
 } navigationFSMEvent_t;
 
@@ -241,6 +243,7 @@ typedef enum {
     NAV_PERSISTENT_ID_FW_LANDING_GLIDE                          = 45,
     NAV_PERSISTENT_ID_FW_LANDING_FLARE                          = 46,
     NAV_PERSISTENT_ID_FW_LANDING_ABORT                          = 47,
+    NAV_PERSISTENT_ID_FW_LANDING_FINISHED                       = 48,
 } navigationPersistentId_e;
 
 typedef enum {
@@ -287,12 +290,13 @@ typedef enum {
     NAV_STATE_CRUISE_INITIALIZE,
     NAV_STATE_CRUISE_IN_PROGRESS,
     NAV_STATE_CRUISE_ADJUSTING,
-    
+
     NAV_STATE_FW_LANDING_CLIMB_TO_LOITER,
     NAV_STATE_FW_LANDING_LOITER,
     NAV_STATE_FW_LANDING_APPROACH,
     NAV_STATE_FW_LANDING_GLIDE,
     NAV_STATE_FW_LANDING_FLARE,
+    NAV_STATE_FW_LANDING_FINISHED,
     NAV_STATE_FW_LANDING_ABORT,
 
     NAV_STATE_MIXERAT_INITIALIZE,

From 81b1f03325698313be0f167318b406d144a3ffc6 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sat, 13 Apr 2024 23:48:29 +0200
Subject: [PATCH 051/323] Use 2 bits for msp displayport font page

---
 src/main/io/displayport_msp_bf_compat.c |  2 +-
 src/main/io/displayport_msp_osd.c       | 19 +++++++++----------
 2 files changed, 10 insertions(+), 11 deletions(-)

diff --git a/src/main/io/displayport_msp_bf_compat.c b/src/main/io/displayport_msp_bf_compat.c
index 7067ac140f5..4219d2b2db8 100644
--- a/src/main/io/displayport_msp_bf_compat.c
+++ b/src/main/io/displayport_msp_bf_compat.c
@@ -27,7 +27,7 @@
 
 uint8_t getBfCharacter(uint8_t ch, uint8_t page)
 {
-    uint16_t ech = ch | (page << 8);
+    uint16_t ech = ch | ((page & 0x3)<< 8) ;
 
     if (ech >= 0x20 && ech <= 0x5F) { // ASCII range
         return ch;
diff --git a/src/main/io/displayport_msp_osd.c b/src/main/io/displayport_msp_osd.c
index e1e6cb29676..56dbc723439 100644
--- a/src/main/io/displayport_msp_osd.c
+++ b/src/main/io/displayport_msp_osd.c
@@ -100,7 +100,7 @@ static timeMs_t sendSubFrameMs = 0;
 static uint8_t currentOsdMode; // HDZero screen mode can change across layouts
 
 static uint8_t screen[SCREENSIZE];
-static BITARRAY_DECLARE(fontPage, SCREENSIZE);  // font page for each character on the screen
+static uint8_t fontPage[SCREENSIZE];  // font page for each character on the screen
 static BITARRAY_DECLARE(dirty, SCREENSIZE);     // change status for each character on the screen
 static BITARRAY_DECLARE(blinkChar, SCREENSIZE); // Does the character blink?
 static bool screenCleared;
@@ -171,7 +171,7 @@ static int setDisplayMode(displayPort_t *displayPort)
 static void init(void)
 {
     memset(screen, SYM_BLANK, sizeof(screen));
-    BITARRAY_CLR_ALL(fontPage);
+    memset(fontPage, 0, sizeof(fontPage));
     BITARRAY_CLR_ALL(dirty);
     BITARRAY_CLR_ALL(blinkChar);
 }
@@ -204,9 +204,8 @@ static bool readChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint1
     }
 
     *c = screen[pos];
-    if (bitArrayGet(fontPage, pos)) {
-        *c |= 0x100;
-    }
+    uint8_t page = fontPage[pos];
+    *c |= (page & 0x3) << 8;
 
     if (attr) {
         *attr = TEXT_ATTRIBUTES_NONE;
@@ -219,10 +218,10 @@ static int setChar(const uint16_t pos, const uint16_t c, textAttributes_t attr)
 {
     if (pos < SCREENSIZE) {
         uint8_t ch = c & 0xFF;
-        bool page = (c >> 8);
-        if (screen[pos] != ch || bitArrayGet(fontPage, pos) != page) {
+        uint8_t page = (c >> 8) & 0x3;
+        if (screen[pos] != ch || fontPage[pos] != page) {
             screen[pos] = ch;
-            (page) ? bitArraySet(fontPage, pos) : bitArrayClr(fontPage, pos);
+            fontPage[pos] = page & 0x3;
             (TEXT_ATTRIBUTES_HAVE_BLINK(attr)) ? bitArraySet(blinkChar, pos) : bitArrayClr(blinkChar, pos);
             bitArraySet(dirty, pos);
         }
@@ -287,7 +286,7 @@ static int drawScreen(displayPort_t *displayPort) // 250Hz
         uint8_t col = pos % COLS;
         uint8_t attributes = 0;
         int endOfLine = row * COLS + screenCols;
-        bool page = bitArrayGet(fontPage, pos);
+        uint8_t page = fontPage[pos];
         bool blink = bitArrayGet(blinkChar, pos);
 
         uint8_t len = 4;
@@ -299,7 +298,7 @@ static int drawScreen(displayPort_t *displayPort) // 250Hz
             if (bitArrayGet(dirty, pos)) {
                 next = pos;
             }
-        } while (next == pos && next < endOfLine && bitArrayGet(fontPage, next) == page && bitArrayGet(blinkChar, next) == blink);
+        } while (next == pos && next < endOfLine && fontPage[next] == page && bitArrayGet(blinkChar, next) == blink);
 
         if (!isBfCompatibleVideoSystem(osdConfig())) {
             attributes |= (page << DISPLAYPORT_MSP_ATTR_FONTPAGE);

From 9da168d5dcc6339eb8cc105b93ec2c6e46c3c6ea Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sun, 14 Apr 2024 16:12:38 +0200
Subject: [PATCH 052/323] Merge blink bitarry into attr array

---
 src/main/io/displayport_msp_osd.c | 41 +++++++++++++++++++++----------
 src/main/io/displayport_msp_osd.h | 20 ++++++++++++---
 2 files changed, 44 insertions(+), 17 deletions(-)

diff --git a/src/main/io/displayport_msp_osd.c b/src/main/io/displayport_msp_osd.c
index 56dbc723439..ab88ec661fb 100644
--- a/src/main/io/displayport_msp_osd.c
+++ b/src/main/io/displayport_msp_osd.c
@@ -100,9 +100,8 @@ static timeMs_t sendSubFrameMs = 0;
 static uint8_t currentOsdMode; // HDZero screen mode can change across layouts
 
 static uint8_t screen[SCREENSIZE];
-static uint8_t fontPage[SCREENSIZE];  // font page for each character on the screen
+static uint8_t attrs[SCREENSIZE];  // font page for each character on the screen
 static BITARRAY_DECLARE(dirty, SCREENSIZE);     // change status for each character on the screen
-static BITARRAY_DECLARE(blinkChar, SCREENSIZE); // Does the character blink?
 static bool screenCleared;
 static uint8_t screenRows, screenCols;
 static videoSystem_e osdVideoSystem;
@@ -158,6 +157,22 @@ static uint8_t determineHDZeroOsdMode(void)
     return HD_3016;
 }
 
+
+uint8_t setAttrPage(uint8_t origAttr, uint8_t page)
+{
+        return (origAttr & ~DISPLAYPORT_MSP_ATTR_FONTPAGE_MASK) | (page & DISPLAYPORT_MSP_ATTR_FONTPAGE_MASK);
+}
+
+uint8_t setAttrBlink(uint8_t origAttr, uint8_t blink)
+{
+    return (origAttr & ~DISPLAYPORT_MSP_ATTR_BLINK_MASK) | ((blink << DISPLAYPORT_MSP_ATTR_BLINK) & DISPLAYPORT_MSP_ATTR_BLINK_MASK);
+}
+
+uint8_t setAttrVersion(uint8_t origAttr, uint8_t version)
+{
+    return (origAttr & ~DISPLAYPORT_MSP_ATTR_VERSION_MASK) | ((version << DISPLAYPORT_MSP_ATTR_VERSION) & DISPLAYPORT_MSP_ATTR_VERSION_MASK);
+}
+
 static int setDisplayMode(displayPort_t *displayPort)
 {
     if (osdVideoSystem == VIDEO_SYSTEM_HDZERO) {
@@ -171,9 +186,8 @@ static int setDisplayMode(displayPort_t *displayPort)
 static void init(void)
 {
     memset(screen, SYM_BLANK, sizeof(screen));
-    memset(fontPage, 0, sizeof(fontPage));
+    memset(attrs, 0, sizeof(attrs));
     BITARRAY_CLR_ALL(dirty);
-    BITARRAY_CLR_ALL(blinkChar);
 }
 
 static int clearScreen(displayPort_t *displayPort)
@@ -204,8 +218,8 @@ static bool readChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint1
     }
 
     *c = screen[pos];
-    uint8_t page = fontPage[pos];
-    *c |= (page & 0x3) << 8;
+    uint8_t page = getAttrPage(attrs[pos]);
+    *c |= page << 8;
 
     if (attr) {
         *attr = TEXT_ATTRIBUTES_NONE;
@@ -218,11 +232,12 @@ static int setChar(const uint16_t pos, const uint16_t c, textAttributes_t attr)
 {
     if (pos < SCREENSIZE) {
         uint8_t ch = c & 0xFF;
-        uint8_t page = (c >> 8) & 0x3;
-        if (screen[pos] != ch || fontPage[pos] != page) {
+        uint8_t page = (c >> 8) & DISPLAYPORT_MSP_ATTR_FONTPAGE_MASK;
+        if (screen[pos] != ch || getAttrPage(attrs[pos]) != page) {
             screen[pos] = ch;
-            fontPage[pos] = page & 0x3;
-            (TEXT_ATTRIBUTES_HAVE_BLINK(attr)) ? bitArraySet(blinkChar, pos) : bitArrayClr(blinkChar, pos);
+            attrs[pos] = setAttrPage(attrs[pos], page);
+            uint8_t blink = (TEXT_ATTRIBUTES_HAVE_BLINK(attr)) ? 1 : 0;
+            attrs[pos] = setAttrBlink(attrs[pos], blink);
             bitArraySet(dirty, pos);
         }
     }
@@ -286,8 +301,8 @@ static int drawScreen(displayPort_t *displayPort) // 250Hz
         uint8_t col = pos % COLS;
         uint8_t attributes = 0;
         int endOfLine = row * COLS + screenCols;
-        uint8_t page = fontPage[pos];
-        bool blink = bitArrayGet(blinkChar, pos);
+        uint8_t page = getAttrPage(attrs[pos]);
+        uint8_t blink = getAttrBlink(attrs[pos]);
 
         uint8_t len = 4;
         do {
@@ -298,7 +313,7 @@ static int drawScreen(displayPort_t *displayPort) // 250Hz
             if (bitArrayGet(dirty, pos)) {
                 next = pos;
             }
-        } while (next == pos && next < endOfLine && fontPage[next] == page && bitArrayGet(blinkChar, next) == blink);
+        } while (next == pos && next < endOfLine && getAttrPage(attrs[next]) == page && getAttrBlink(attrs[next]) == blink);
 
         if (!isBfCompatibleVideoSystem(osdConfig())) {
             attributes |= (page << DISPLAYPORT_MSP_ATTR_FONTPAGE);
diff --git a/src/main/io/displayport_msp_osd.h b/src/main/io/displayport_msp_osd.h
index 0a2f64c48a2..affd5b39be5 100644
--- a/src/main/io/displayport_msp_osd.h
+++ b/src/main/io/displayport_msp_osd.h
@@ -27,13 +27,25 @@
 #include "drivers/osd.h"
 #include "msp/msp_serial.h"
 
+// MSP displayport V2 attribute byte bit functions
+#define DISPLAYPORT_MSP_ATTR_FONTPAGE   0 // Select bank of 256 characters as per displayPortSeverity_e
+#define DISPLAYPORT_MSP_ATTR_BLINK      6 // Device local blink
+#define DISPLAYPORT_MSP_ATTR_VERSION    7 // Format indicator; must be zero for V2 (and V1)
+
+#define DISPLAYPORT_MSP_ATTR_FONTPAGE_MASK   0x3
+#define DISPLAYPORT_MSP_ATTR_BLINK_MASK      (1 << DISPLAYPORT_MSP_ATTR_BLINK)
+#define DISPLAYPORT_MSP_ATTR_VERSION_MASK    (1 << DISPLAYPORT_MSP_ATTR_VERSION)
+
 typedef struct displayPort_s displayPort_t;
 
 displayPort_t *mspOsdDisplayPortInit(const videoSystem_e videoSystem);
 void mspOsdSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn);
 mspPort_t *getMspOsdPort(void);
 
-// MSP displayport V2 attribute byte bit functions
-#define DISPLAYPORT_MSP_ATTR_FONTPAGE   0 // Select bank of 256 characters as per displayPortSeverity_e
-#define DISPLAYPORT_MSP_ATTR_BLINK      6 // Device local blink
-#define DISPLAYPORT_MSP_ATTR_VERSION    7 // Format indicator; must be zero for V2 (and V1)
\ No newline at end of file
+#define getAttrPage(attr) (attr & DISPLAYPORT_MSP_ATTR_FONTPAGE_MASK)
+#define getAttrBlink(attr) ((attr & DISPLAYPORT_MSP_ATTR_BLINK_MASK) >> DISPLAYPORT_MSP_ATTR_BLINK)
+#define getAttrVersion(attr) ((attr & DISPLAYPORT_MSP_ATTR_VERSION_MASK) >> DISPLAYPORT_MSP_ATTR_VERSION)
+
+uint8_t setAttrPage(uint8_t origAttr, uint8_t page);
+uint8_t setAttrBlink(uint8_t origAttr, uint8_t page);
+uint8_t setAttrVersion(uint8_t origAttr, uint8_t page);
\ No newline at end of file

From 1ee021b5a8cf3dc83afe52347ea6ef8ec71922a6 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sun, 14 Apr 2024 16:16:58 +0200
Subject: [PATCH 053/323] Formating changes

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

diff --git a/src/main/io/displayport_msp_osd.c b/src/main/io/displayport_msp_osd.c
index ab88ec661fb..c9372d9fbf9 100644
--- a/src/main/io/displayport_msp_osd.c
+++ b/src/main/io/displayport_msp_osd.c
@@ -59,7 +59,7 @@ typedef enum {          // defines are from hdzero code
     SD_3016,
     HD_5018,
     HD_3016,           // Special HDZERO mode that just sends the centre 30x16 of the 50x18 canvas to the VRX
-    HD_6022,            // added to support DJI wtfos 60x22 grid
+    HD_6022,           // added to support DJI wtfos 60x22 grid
     HD_5320            // added to support Avatar and BetaflightHD
 } resolutionType_e;
 
@@ -97,11 +97,11 @@ static timeMs_t sendSubFrameMs = 0;
 // set screen size
 #define SCREENSIZE (ROWS*COLS)
 
-static uint8_t currentOsdMode; // HDZero screen mode can change across layouts
+static uint8_t currentOsdMode;               // HDZero screen mode can change across layouts
 
 static uint8_t screen[SCREENSIZE];
-static uint8_t attrs[SCREENSIZE];  // font page for each character on the screen
-static BITARRAY_DECLARE(dirty, SCREENSIZE);     // change status for each character on the screen
+static uint8_t attrs[SCREENSIZE];            // font page, blink and other attributes
+static BITARRAY_DECLARE(dirty, SCREENSIZE);  // change status for each character on the screen
 static bool screenCleared;
 static uint8_t screenRows, screenCols;
 static videoSystem_e osdVideoSystem;

From 02d4a52d5da9329b005e62f1445716c80e14e755 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Mon, 15 Apr 2024 11:44:25 +0200
Subject: [PATCH 054/323] Enable adaptive filter for all targets

---
 src/main/target/common.h | 5 -----
 1 file changed, 5 deletions(-)

diff --git a/src/main/target/common.h b/src/main/target/common.h
index 09f1b71e7e2..a6da8c5a1d3 100644
--- a/src/main/target/common.h
+++ b/src/main/target/common.h
@@ -211,9 +211,4 @@
 #endif
 
 #define USE_EZ_TUNE
-
-#ifdef STM32H7
-
 #define USE_ADAPTIVE_FILTER
-
-#endif

From 1134c4541477b87b5264476fa056fd9528583a47 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Mon, 15 Apr 2024 15:40:10 +0100
Subject: [PATCH 055/323] Update osd.c

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

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index f0cc50bbaa1..f06845c3662 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -1003,7 +1003,7 @@ static const char * divertingToSafehomeMessage(void)
 	    return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME);
 	}
 #endif
-	return NULL;   
+	return NULL;
 }
 
 
@@ -1028,7 +1028,7 @@ static const char * navigationStateMessage(void)
                         linearDescentMessageMs = millis() + 5000; // Show message for 5 seconds.
 
                     return OSD_MESSAGE_STR(OSD_MSG_RTH_LINEAR_DESCENT);
-                } else 
+                } else
                     return OSD_MESSAGE_STR(OSD_MSG_HEADING_HOME);
             }
         case MW_NAV_STATE_HOLD_INFINIT:
@@ -1069,7 +1069,7 @@ static const char * navigationStateMessage(void)
             // If there is a FS landing delay occurring. That is handled by the calling function.
             if (posControl.landingDelay > 0)
                 break;
-            
+
             return OSD_MESSAGE_STR(OSD_MSG_PREPARING_LAND);
         }
         case MW_NAV_STATE_LAND_START_DESCENT:
@@ -2277,7 +2277,7 @@ static bool osdDrawSingleElement(uint8_t item)
         {
             char *p = "ACRO";
 #ifdef USE_FW_AUTOLAND
-            if (FLIGHT_MODE(NAV_FW_AUTOLAND)) 
+            if (FLIGHT_MODE(NAV_FW_AUTOLAND))
                 p = "LAND";
             else
 #endif
@@ -4864,6 +4864,7 @@ static void osdRefresh(timeUs_t currentTimeUs)
     static uint8_t statsCurrentPage = 0;
     static bool statsDisplayed = false;
     static bool statsAutoPagingEnabled = true;
+    static bool throttleHigh = false;
 
     // Detect arm/disarm
     if (armState != ARMING_FLAG(ARMED)) {
@@ -4889,6 +4890,7 @@ static void osdRefresh(timeUs_t currentTimeUs)
             statsAutoPagingEnabled = osdConfig()->stats_page_auto_swap_time > 0 ? true : false;
             osdShowStats(statsSinglePageCompatible, statsCurrentPage);
             osdSetNextRefreshIn(STATS_SCREEN_DISPLAY_TIME);
+            throttleHigh = checkStickPosition(THR_HI);
         }
 
         armState = ARMING_FLAG(ARMED);
@@ -4954,7 +4956,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 && !throttleHigh)) {
             // Time elapsed or canceled by stick commands.
             // Exit to normal OSD operation.
             displayClearScreen(osdDisplayPort);
@@ -4963,6 +4965,7 @@ static void osdRefresh(timeUs_t currentTimeUs)
         } else {
             // Continue "Splash", "Armed" or "Stats" screens.
             displayHeartbeat(osdDisplayPort);
+            throttleHigh = checkStickPosition(THR_HI);
         }
 
         return;
@@ -5177,7 +5180,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                     tfp_sprintf(messageBuf, "LANDING DELAY: %3u SECONDS", remainingHoldSec);
 
                     messages[messageCount++] = messageBuf;
-                } 
+                }
 
                 else {
 #ifdef USE_FW_AUTOLAND

From d00780a3b898d38eae90960bf69122f91874af3c Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Mon, 15 Apr 2024 16:04:18 +0100
Subject: [PATCH 056/323] Update blackbox.c

---
 src/main/blackbox/blackbox.c | 10 ++++++++--
 1 file changed, 8 insertions(+), 2 deletions(-)

diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index 453b8fda0cc..1bb0165761e 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -1308,10 +1308,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)) {
+    // Also log Nav auto enabled flight modes rather than just those selected by boxmode
+    if (FLIGHT_MODE(ANGLE_MODE)) {
         slow->flightModeFlags |= (1 << BOXANGLE);
     }
+    if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) {
+        slow->flightModeFlags |= (1 << BOXNAVALTHOLD);
+    }
+    if (FLIGHT_MODE(NAV_RTH_MODE)) {
+        slow->flightModeFlags |= (1 << BOXNAVRTH);
+    }
     if (navigationGetHeadingControlState() == NAV_HEADING_CONTROL_AUTO) {
         slow->flightModeFlags |= (1 << BOXHEADINGHOLD);
     }

From eb8843bc9c190650c21b3ec10964c1ed96ea8296 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Mon, 15 Apr 2024 18:09:06 +0100
Subject: [PATCH 057/323] changes

---
 src/main/fc/fc_core.c                    |   4 +
 src/main/navigation/navigation.c         | 159 +++++++++++------------
 src/main/navigation/navigation_private.h |  19 +--
 3 files changed, 87 insertions(+), 95 deletions(-)

diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index 0d0863760de..0c8daea2ca8 100644
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -350,6 +350,10 @@ static void updateArmingStatus(void)
             DISABLE_ARMING_FLAG(ARMING_DISABLED_NO_PREARM);
         }
 
+        if (ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED) && !IS_RC_MODE_ACTIVE(BOXARM)) {
+            DISABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
+        }
+
         /* CHECK: Arming switch */
         // If arming is disabled and the ARM switch is on
         // Note that this should be last check so all other blockers could be cleared correctly
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 9e81024c3af..d2a5732de79 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -328,8 +328,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState);
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_TRACKBACK(navigationFSMState_t previousState);
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigationFSMState_t previousState);
-static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING(navigationFSMState_t previousState);
-static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(navigationFSMState_t previousState);
+static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING(navigationFSMState_t previousState);
+static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LOITER_ABOVE_HOME(navigationFSMState_t previousState);
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationFSMState_t previousState);
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHING(navigationFSMState_t previousState);
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigationFSMState_t previousState);
@@ -423,7 +423,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_POSHOLD_3D_INITIALIZE,
         .onEntry = navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE,
         .timeoutMs = 0,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT,
         .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_POSHOLD_MODE,
         .mwState = MW_NAV_STATE_HOLD_INFINIT,
         .mwError = MW_NAV_ERROR_NONE,
@@ -438,7 +438,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_POSHOLD_3D_IN_PROGRESS,
         .onEntry = navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_RC_ALT | NAV_RC_POS | NAV_RC_YAW,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_RC_ALT | NAV_RC_POS | NAV_RC_YAW,
         .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_POSHOLD_MODE,
         .mwState = MW_NAV_STATE_HOLD_INFINIT,
         .mwError = MW_NAV_ERROR_NONE,
@@ -585,7 +585,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
             [NAV_FSM_EVENT_SUCCESS]                             = NAV_STATE_RTH_CLIMB_TO_SAFE_ALT,
             [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK]   = NAV_STATE_RTH_TRACKBACK,
             [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]         = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
-            [NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING]               = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
+            [NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING]               = NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING,
             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                      = NAV_STATE_IDLE,
         }
     },
@@ -594,7 +594,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_RTH_CLIMB_TO_SAFE_ALT,
         .onEntry = navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,     // allow pos adjustment while climbind to safe alt
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,     // allow pos adjustment while climbind to safe alt
         .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
         .mwState = MW_NAV_STATE_RTH_CLIMB,
         .mwError = MW_NAV_ERROR_WAIT_FOR_RTH_ALT,
@@ -634,13 +634,13 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_RTH_HEAD_HOME,
         .onEntry = navOnEnteringState_NAV_STATE_RTH_HEAD_HOME,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
         .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
         .mwState = MW_NAV_STATE_RTH_ENROUTE,
         .mwError = MW_NAV_ERROR_NONE,
         .onEvent = {
             [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_RTH_HEAD_HOME,           // re-process the state
-            [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
+            [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING,
             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
             [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]              = NAV_STATE_ALTHOLD_INITIALIZE,
             [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]           = NAV_STATE_POSHOLD_3D_INITIALIZE,
@@ -651,37 +651,37 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         }
     },
 
-    [NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING] = {
-        .persistentId = NAV_PERSISTENT_ID_RTH_HOVER_PRIOR_TO_LANDING,
-        .onEntry = navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
+    [NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING] = {
+        .persistentId = NAV_PERSISTENT_ID_RTH_LOITER_PRIOR_TO_LANDING,
+        .onEntry = navOnEnteringState_NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING,
         .timeoutMs = 500,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
         .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
         .mwState = MW_NAV_STATE_LAND_SETTLE,
         .mwError = MW_NAV_ERROR_NONE,
         .onEvent = {
-            [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
-            [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_RTH_LANDING,
-            [NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME] = NAV_STATE_RTH_HOVER_ABOVE_HOME,
-            [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
-            [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]              = NAV_STATE_ALTHOLD_INITIALIZE,
-            [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]           = NAV_STATE_POSHOLD_3D_INITIALIZE,
-            [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
-            [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD]          = NAV_STATE_COURSE_HOLD_INITIALIZE,
-            [NAV_FSM_EVENT_SWITCH_TO_CRUISE]               = NAV_STATE_CRUISE_INITIALIZE,
+            [NAV_FSM_EVENT_TIMEOUT]                         = NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING,
+            [NAV_FSM_EVENT_SUCCESS]                         = NAV_STATE_RTH_LANDING,
+            [NAV_FSM_EVENT_SWITCH_TO_RTH_LOITER_ABOVE_HOME] = NAV_STATE_RTH_LOITER_ABOVE_HOME,
+            [NAV_FSM_EVENT_SWITCH_TO_IDLE]                  = NAV_STATE_IDLE,
+            [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]               = NAV_STATE_ALTHOLD_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]            = NAV_STATE_POSHOLD_3D_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]     = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD]           = NAV_STATE_COURSE_HOLD_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE]                = NAV_STATE_CRUISE_INITIALIZE,
         }
     },
 
-    [NAV_STATE_RTH_HOVER_ABOVE_HOME] = {
-        .persistentId = NAV_PERSISTENT_ID_RTH_HOVER_ABOVE_HOME,
-        .onEntry = navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME,
+    [NAV_STATE_RTH_LOITER_ABOVE_HOME] = {
+        .persistentId = NAV_PERSISTENT_ID_RTH_LOITER_ABOVE_HOME,
+        .onEntry = navOnEnteringState_NAV_STATE_RTH_LOITER_ABOVE_HOME,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT,
         .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
         .mwState = MW_NAV_STATE_HOVER_ABOVE_HOME,
         .mwError = MW_NAV_ERROR_NONE,
         .onEvent = {
-            [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_RTH_HOVER_ABOVE_HOME,
+            [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_RTH_LOITER_ABOVE_HOME,
             [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
             [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]              = NAV_STATE_ALTHOLD_INITIALIZE,
             [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]           = NAV_STATE_POSHOLD_3D_INITIALIZE,
@@ -695,20 +695,20 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_RTH_LANDING,
         .onEntry = navOnEnteringState_NAV_STATE_RTH_LANDING,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
         .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
         .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
         .mwError = MW_NAV_ERROR_LANDING,
         .onEvent = {
-            [NAV_FSM_EVENT_TIMEOUT]                        = NAV_STATE_RTH_LANDING,         // re-process state
-            [NAV_FSM_EVENT_SUCCESS]                        = NAV_STATE_RTH_FINISHING,
-            [NAV_FSM_EVENT_SWITCH_TO_IDLE]                 = NAV_STATE_IDLE,
-            [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]              = NAV_STATE_ALTHOLD_INITIALIZE,
-            [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]           = NAV_STATE_POSHOLD_3D_INITIALIZE,
-            [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]    = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
-            [NAV_FSM_EVENT_SWITCH_TO_MIXERAT]              = NAV_STATE_MIXERAT_INITIALIZE,
-            [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING] = NAV_STATE_FW_LANDING_CLIMB_TO_LOITER,
-            [NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME] = NAV_STATE_RTH_HOVER_ABOVE_HOME,
+            [NAV_FSM_EVENT_TIMEOUT]                         = NAV_STATE_RTH_LANDING,         // re-process state
+            [NAV_FSM_EVENT_SUCCESS]                         = NAV_STATE_RTH_FINISHING,
+            [NAV_FSM_EVENT_SWITCH_TO_IDLE]                  = NAV_STATE_IDLE,
+            [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]               = NAV_STATE_ALTHOLD_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]            = NAV_STATE_POSHOLD_3D_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING]     = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_MIXERAT]               = NAV_STATE_MIXERAT_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING]  = NAV_STATE_FW_LANDING_CLIMB_TO_LOITER,
+            [NAV_FSM_EVENT_SWITCH_TO_RTH_LOITER_ABOVE_HOME] = NAV_STATE_RTH_LOITER_ABOVE_HOME,
         }
     },
 
@@ -716,7 +716,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_RTH_FINISHING,
         .onEntry = navOnEnteringState_NAV_STATE_RTH_FINISHING,
         .timeoutMs = 0,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
         .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
         .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
         .mwError = MW_NAV_ERROR_LANDING,
@@ -827,7 +827,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME,                             // There is no state for timed hold?
         .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
         .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
         .mwState = MW_NAV_STATE_HOLD_TIMED,
         .mwError = MW_NAV_ERROR_NONE,
@@ -848,7 +848,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_WAYPOINT_RTH_LAND,
         .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
         .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
         .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
         .mwError = MW_NAV_ERROR_LANDING,
@@ -886,7 +886,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_WAYPOINT_FINISHED,
         .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP | NAV_AUTO_WP_DONE,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP | NAV_AUTO_WP_DONE,
         .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
         .mwState = MW_NAV_STATE_WP_ENROUTE,
         .mwError = MW_NAV_ERROR_FINISH,
@@ -907,7 +907,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_INITIALIZE,
         .onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
         .timeoutMs = 0,
-        .stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
+        .stateFlags = NAV_CTL_EMERG | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE,
         .mapToFlightModes = 0,
         .mwState = MW_NAV_STATE_EMERGENCY_LANDING,
         .mwError = MW_NAV_ERROR_LANDING,
@@ -925,7 +925,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_IN_PROGRESS,
         .onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
+        .stateFlags = NAV_CTL_EMERG | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE,
         .mapToFlightModes = 0,
         .mwState = MW_NAV_STATE_EMERGENCY_LANDING,
         .mwError = MW_NAV_ERROR_LANDING,
@@ -943,7 +943,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_FINISHED,
         .onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINISHED,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
+        .stateFlags = NAV_CTL_EMERG | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE,
         .mapToFlightModes = 0,
         .mwState = MW_NAV_STATE_LANDED,
         .mwError = MW_NAV_ERROR_LANDING,
@@ -1052,7 +1052,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_FW_LANDING_CLIMB_TO_LOITER,
         .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_CLIMB_TO_LOITER,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH,
         .mapToFlightModes = NAV_FW_AUTOLAND,
         .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
         .mwError = MW_NAV_ERROR_NONE,
@@ -1073,7 +1073,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_FW_LANDING_LOITER,
         .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_LOITER,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH,
         .mapToFlightModes = NAV_FW_AUTOLAND,
         .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
         .mwError = MW_NAV_ERROR_NONE,
@@ -1246,7 +1246,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(n
     }
 
     // Prepare position controller if idle or current Mode NOT active in position hold state
-    if (previousState != NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING && previousState != NAV_STATE_RTH_HOVER_ABOVE_HOME &&
+    if (previousState != NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING && previousState != NAV_STATE_RTH_LOITER_ABOVE_HOME &&
         previousState != NAV_STATE_RTH_LANDING && previousState != NAV_STATE_WAYPOINT_RTH_LAND &&
         previousState != NAV_STATE_WAYPOINT_FINISHED && previousState != NAV_STATE_WAYPOINT_HOLD_TIME)
         {
@@ -1427,7 +1427,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
             setHomePosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING, NAV_HOME_VALID_ALL);
             setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
 
-            return NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING;   // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING
+            return NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING;   // NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING
         }
         else {
             // Switch to RTH trackback
@@ -1501,8 +1501,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
             initializeRTHSanityChecker();
         }
 
-        // Save initial home distance for future use
+        // Save initial home distance and direction for future use
         posControl.rthState.rthInitialDistance = posControl.homeDistance;
+        posControl.activeWaypoint.bearing = posControl.homeDirection;
         fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL);
 
         if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING_LEGACY)) {
@@ -1616,7 +1617,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
     if ((posControl.flags.estPosStatus >= EST_USABLE)) {
         fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL);
 
-        if (isWaypointReached(tmpHomePos, 0)) {
+        if (isWaypointReached(tmpHomePos, &posControl.activeWaypoint.bearing)) {
             // Successfully reached position target - update XYZ-position
             setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
 
@@ -1625,7 +1626,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
             if (navConfig()->general.flags.rth_use_linear_descent && posControl.rthState.rthLinearDescentActive)
                 posControl.rthState.rthLinearDescentActive = false;
 
-            return NAV_FSM_EVENT_SUCCESS;       // NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING
+            return NAV_FSM_EVENT_SUCCESS;       // NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING
         } else {
             setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_XY);
             return NAV_FSM_EVENT_NONE;
@@ -1639,7 +1640,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
     return NAV_FSM_EVENT_NONE;
 }
 
-static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING(navigationFSMState_t previousState)
+static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING(navigationFSMState_t previousState)
 {
     UNUSED(previousState);
 
@@ -1673,7 +1674,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
     if (!pauseLanding && ((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, 0, ROC_TO_ALT_RESET);
-        return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; // success = land
+        return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_LOITER_ABOVE_HOME; // success = land
     } else {
         fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL);
         setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
@@ -1681,7 +1682,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
     }
 }
 
-static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(navigationFSMState_t previousState)
+static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LOITER_ABOVE_HOME(navigationFSMState_t previousState)
 {
     UNUSED(previousState);
 
@@ -1690,7 +1691,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(na
         return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
     }
 
-    fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_HOVER);
+    fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_LOITER);
     setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
 
     return NAV_FSM_EVENT_NONE;
@@ -2046,9 +2047,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navig
 #ifdef USE_FW_AUTOLAND
     if (landEvent == NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING) {
         return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING;
-    } else 
+    } else
 #endif
-    if (landEvent == NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME) {
+    if (landEvent == NAV_FSM_EVENT_SWITCH_TO_RTH_LOITER_ABOVE_HOME) {
         return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
     } else if (landEvent == NAV_FSM_EVENT_SUCCESS) {
         // Landing controller returned success - invoke RTH finishing state and finish the waypoint
@@ -2567,7 +2568,7 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode)
             posControl.rthState.homeTmpWaypoint.z = posControl.rthState.rthFinalAltitude;
             break;
 
-        case RTH_HOME_FINAL_HOVER:
+        case RTH_HOME_FINAL_LOITER:
             if (navConfig()->general.rth_home_altitude) {
                 posControl.rthState.homeTmpWaypoint.z = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_home_altitude;
             }
@@ -2852,30 +2853,28 @@ static bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos)
 
 /*-----------------------------------------------------------
  * Check if waypoint is/was reached.
- * waypointBearing stores initial bearing to waypoint
+ * 'waypointBearing' stores initial bearing to waypoint.
  *-----------------------------------------------------------*/
 static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointBearing)
 {
     posControl.wpDistance = calculateDistanceToDestination(waypointPos);
 
-    // Airplane will do a circular loiter at hold waypoints and might never approach them closer than waypoint_radius
-    // Check within 10% margin of circular loiter radius
-    if (STATE(AIRPLANE) && isNavHoldPositionActive() && posControl.wpDistance <= (navConfig()->fw.loiter_radius * 1.10f)) {
-        return true;
-    }
+    // Check if waypoint was missed based on bearing to WP exceeding given angular limit relative to initial waypoint bearing.
+    // Default angular limit = 100 degs with a reduced limit of 60 degs used if fixed wing waypoint turn smoothing option active
+    uint16_t relativeBearingErrorLimit = 10000;
 
-    if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP || posControl.flags.rthTrackbackActive) {
+    if (STATE(AIRPLANE) && posControl.flags.wpTurnSmoothingActive) {
         // If WP turn smoothing CUT option used WP is reached when start of turn is initiated
-        if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_CUT && posControl.flags.wpTurnSmoothingActive) {
+        if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_CUT) {
             posControl.flags.wpTurnSmoothingActive = false;
             return true;
         }
-        // Check if waypoint was missed based on bearing to WP exceeding 100 degrees relative to waypoint Yaw
-        // Same method for turn smoothing option but relative bearing set at 60 degrees
-        uint16_t relativeBearing = posControl.flags.wpTurnSmoothingActive ? 6000 : 10000;
-        if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointBearing)) > relativeBearing) {
-            return true;
-        }
+        relativeBearingErrorLimit = 6000;
+    }
+
+
+    if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointBearing)) > relativeBearingErrorLimit) {
+        return true;
     }
 
     return posControl.wpDistance <= (navConfig()->general.waypoint_radius);
@@ -3453,9 +3452,6 @@ void updateLandingStatus(timeMs_t currentTimeMs)
         }
         resetLandingDetector();
 
-        if (!IS_RC_MODE_ACTIVE(BOXARM)) {
-            DISABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
-        }
         return;
     }
 
@@ -4075,16 +4071,7 @@ bool isLastMissionWaypoint(void)
 /* Checks if Nav hold position is active */
 bool isNavHoldPositionActive(void)
 {
-    // WP mode last WP hold and Timed hold positions
-    if (FLIGHT_MODE(NAV_WP_MODE)) {
-        return isLastMissionWaypoint() || NAV_Status.state == MW_NAV_STATE_HOLD_TIMED;
-    }
-    // RTH mode (spiral climb and Home positions but excluding RTH Trackback point positions) and POSHOLD mode
-    // Also hold position during emergency landing if position valid
-    return (FLIGHT_MODE(NAV_RTH_MODE) && !posControl.flags.rthTrackbackActive) ||
-            FLIGHT_MODE(NAV_POSHOLD_MODE) ||
-            (posControl.navState == NAV_STATE_FW_LANDING_CLIMB_TO_LOITER || posControl.navState == NAV_STATE_FW_LANDING_LOITER) || 
-            navigationIsExecutingAnEmergencyLanding();
+    return navGetCurrentStateFlags() & NAV_CTL_HOLD;
 }
 
 float getActiveSpeed(void)
@@ -4123,7 +4110,7 @@ bool isWaypointNavTrackingActive(void)
     // is set from current position not previous WP. Works for WP Restart intermediate WP as well as first mission WP.
     // (NAV_WP_MODE flag isn't set until WP initialisation is finished, i.e. after calculateAndSetActiveWaypoint called)
 
-    return FLIGHT_MODE(NAV_WP_MODE) 
+    return FLIGHT_MODE(NAV_WP_MODE)
     || posControl.navState == NAV_STATE_FW_LANDING_APPROACH
     || (posControl.flags.rthTrackbackActive && posControl.activeRthTBPointIndex != posControl.rthTBLastSavedIndex);
 }
@@ -5011,7 +4998,7 @@ bool navigationRTHAllowsLanding(void)
         return false;
     }
 #endif
-    
+
     // WP mission RTH landing setting
     if (isWaypointMissionRTHActive() && isWaypointMissionValid()) {
         return posControl.waypointList[posControl.startWpIndex + posControl.waypointCount - 1].p1 > 0;
diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
index d716c58c496..121597c93bc 100644
--- a/src/main/navigation/navigation_private.h
+++ b/src/main/navigation/navigation_private.h
@@ -171,7 +171,7 @@ typedef enum {
     NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND = NAV_FSM_EVENT_STATE_SPECIFIC_1,
     NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED = NAV_FSM_EVENT_STATE_SPECIFIC_2,
     NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME = NAV_FSM_EVENT_STATE_SPECIFIC_3,
-    NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME = NAV_FSM_EVENT_STATE_SPECIFIC_4,
+    NAV_FSM_EVENT_SWITCH_TO_RTH_LOITER_ABOVE_HOME = NAV_FSM_EVENT_STATE_SPECIFIC_4,
     NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK = NAV_FSM_EVENT_STATE_SPECIFIC_5,
     NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE = NAV_FSM_EVENT_STATE_SPECIFIC_6,
     NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT = NAV_FSM_EVENT_STATE_SPECIFIC_6,
@@ -197,7 +197,7 @@ typedef enum {
     NAV_PERSISTENT_ID_RTH_INITIALIZE                            = 8,
     NAV_PERSISTENT_ID_RTH_CLIMB_TO_SAFE_ALT                     = 9,
     NAV_PERSISTENT_ID_RTH_HEAD_HOME                             = 10,
-    NAV_PERSISTENT_ID_RTH_HOVER_PRIOR_TO_LANDING                = 11,
+    NAV_PERSISTENT_ID_RTH_LOITER_PRIOR_TO_LANDING               = 11,
     NAV_PERSISTENT_ID_RTH_LANDING                               = 12,
     NAV_PERSISTENT_ID_RTH_FINISHING                             = 13,
     NAV_PERSISTENT_ID_RTH_FINISHED                              = 14,
@@ -228,7 +228,7 @@ typedef enum {
     NAV_PERSISTENT_ID_CRUISE_ADJUSTING                          = 34,
 
     NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME                        = 35,
-    NAV_PERSISTENT_ID_RTH_HOVER_ABOVE_HOME                      = 36,
+    NAV_PERSISTENT_ID_RTH_LOITER_ABOVE_HOME                     = 36,
     NAV_PERSISTENT_ID_UNUSED_4                                  = 37, // was NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME
     NAV_PERSISTENT_ID_RTH_TRACKBACK                             = 38,
 
@@ -258,8 +258,8 @@ typedef enum {
     NAV_STATE_RTH_CLIMB_TO_SAFE_ALT,
     NAV_STATE_RTH_TRACKBACK,
     NAV_STATE_RTH_HEAD_HOME,
-    NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING,
-    NAV_STATE_RTH_HOVER_ABOVE_HOME,
+    NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING,
+    NAV_STATE_RTH_LOITER_ABOVE_HOME,
     NAV_STATE_RTH_LANDING,
     NAV_STATE_RTH_FINISHING,
     NAV_STATE_RTH_FINISHED,
@@ -287,7 +287,7 @@ typedef enum {
     NAV_STATE_CRUISE_INITIALIZE,
     NAV_STATE_CRUISE_IN_PROGRESS,
     NAV_STATE_CRUISE_ADJUSTING,
-    
+
     NAV_STATE_FW_LANDING_CLIMB_TO_LOITER,
     NAV_STATE_FW_LANDING_LOITER,
     NAV_STATE_FW_LANDING_APPROACH,
@@ -327,9 +327,10 @@ typedef enum {
 
     /* Additional flags */
     NAV_CTL_LAND            = (1 << 14),
-    NAV_AUTO_WP_DONE        = (1 << 15),    //Waypoint mission reached the last waypoint and is idling
+    NAV_AUTO_WP_DONE        = (1 << 15),    // Waypoint mission reached the last waypoint and is idling
 
-    NAV_MIXERAT             = (1 << 16),    //MIXERAT in progress
+    NAV_MIXERAT             = (1 << 16),    // MIXERAT in progress
+    NAV_CTL_HOLD            = (1 << 17),    // Nav loiter active at position or will be when position reached
 } navigationFSMStateFlags_t;
 
 typedef struct {
@@ -396,7 +397,7 @@ typedef enum {
     RTH_HOME_ENROUTE_INITIAL,       // Initial position for RTH approach
     RTH_HOME_ENROUTE_PROPORTIONAL,  // Prorpotional position for RTH approach
     RTH_HOME_ENROUTE_FINAL,         // Final position for RTH approach
-    RTH_HOME_FINAL_HOVER,           // Final hover altitude (if rth_home_altitude is set)
+    RTH_HOME_FINAL_LOITER,          // Final loiter altitude (if rth_home_altitude is set)
     RTH_HOME_FINAL_LAND,            // Home position and altitude
 } rthTargetMode_e;
 

From 79e974d73555d8c91037e48acd7da35c7f547fe9 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Tue, 16 Apr 2024 09:48:31 +0100
Subject: [PATCH 058/323] Update navigation.c

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

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index d2a5732de79..c85359ee50d 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -2859,21 +2859,21 @@ static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * w
 {
     posControl.wpDistance = calculateDistanceToDestination(waypointPos);
 
-    // Check if waypoint was missed based on bearing to WP exceeding given angular limit relative to initial waypoint bearing.
+    // Check if waypoint was missed based on bearing to waypoint exceeding given angular limit relative to initial waypoint bearing.
     // Default angular limit = 100 degs with a reduced limit of 60 degs used if fixed wing waypoint turn smoothing option active
-    uint16_t relativeBearingErrorLimit = 10000;
+    uint16_t relativeBearingTargetAngle = 10000;
 
     if (STATE(AIRPLANE) && posControl.flags.wpTurnSmoothingActive) {
-        // If WP turn smoothing CUT option used WP is reached when start of turn is initiated
+        // If WP mode turn smoothing CUT option used waypoint is reached when start of turn is initiated
         if (navConfig()->fw.wp_turn_smoothing == WP_TURN_SMOOTHING_CUT) {
             posControl.flags.wpTurnSmoothingActive = false;
             return true;
         }
-        relativeBearingErrorLimit = 6000;
+        relativeBearingTargetAngle = 6000;
     }
 
 
-    if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointBearing)) > relativeBearingErrorLimit) {
+    if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointBearing)) > relativeBearingTargetAngle) {
         return true;
     }
 

From 17e785514493cdedc4dbd3c31434491ea792ca82 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Tue, 16 Apr 2024 09:56:45 +0100
Subject: [PATCH 059/323] Update osd.c

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

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index f06845c3662..c6574db97c6 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -4864,7 +4864,7 @@ static void osdRefresh(timeUs_t currentTimeUs)
     static uint8_t statsCurrentPage = 0;
     static bool statsDisplayed = false;
     static bool statsAutoPagingEnabled = true;
-    static bool throttleHigh = false;
+    static bool isThrottleHigh = false;
 
     // Detect arm/disarm
     if (armState != ARMING_FLAG(ARMED)) {
@@ -4890,7 +4890,7 @@ static void osdRefresh(timeUs_t currentTimeUs)
             statsAutoPagingEnabled = osdConfig()->stats_page_auto_swap_time > 0 ? true : false;
             osdShowStats(statsSinglePageCompatible, statsCurrentPage);
             osdSetNextRefreshIn(STATS_SCREEN_DISPLAY_TIME);
-            throttleHigh = checkStickPosition(THR_HI);
+            isThrottleHigh = checkStickPosition(THR_HI);
         }
 
         armState = ARMING_FLAG(ARMED);
@@ -4956,7 +4956,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 && !throttleHigh)) {
+        if (currentTimeUs > resumeRefreshAt || (OSD_RESUME_UPDATES_STICK_COMMAND && !isThrottleHigh)) {
             // Time elapsed or canceled by stick commands.
             // Exit to normal OSD operation.
             displayClearScreen(osdDisplayPort);
@@ -4965,7 +4965,7 @@ static void osdRefresh(timeUs_t currentTimeUs)
         } else {
             // Continue "Splash", "Armed" or "Stats" screens.
             displayHeartbeat(osdDisplayPort);
-            throttleHigh = checkStickPosition(THR_HI);
+            isThrottleHigh = checkStickPosition(THR_HI);
         }
 
         return;

From c3eba6ad835d80b9a3ba56103e2d24660e22aa36 Mon Sep 17 00:00:00 2001
From: Sensei <deepmagicbeginshere@gmail.com>
Date: Tue, 16 Apr 2024 22:56:44 -0500
Subject: [PATCH 060/323] Merge pull request #9669 from
 Aocoda-RC/aocodarc-f405v3

Add AOCODARCF4V3 target
---
 src/main/target/AOCODARCF4V3/CMakeLists.txt |   3 +
 src/main/target/AOCODARCF4V3/config.c       |  40 ++++
 src/main/target/AOCODARCF4V3/target.c       |  46 +++++
 src/main/target/AOCODARCF4V3/target.h       | 192 ++++++++++++++++++++
 4 files changed, 281 insertions(+)
 create mode 100644 src/main/target/AOCODARCF4V3/CMakeLists.txt
 create mode 100644 src/main/target/AOCODARCF4V3/config.c
 create mode 100644 src/main/target/AOCODARCF4V3/target.c
 create mode 100644 src/main/target/AOCODARCF4V3/target.h

diff --git a/src/main/target/AOCODARCF4V3/CMakeLists.txt b/src/main/target/AOCODARCF4V3/CMakeLists.txt
new file mode 100644
index 00000000000..706b818e2b8
--- /dev/null
+++ b/src/main/target/AOCODARCF4V3/CMakeLists.txt
@@ -0,0 +1,3 @@
+target_stm32f405xg(AOCODARCF4V3_SD)
+target_stm32f405xg(AOCODARCF4V3)
+
diff --git a/src/main/target/AOCODARCF4V3/config.c b/src/main/target/AOCODARCF4V3/config.c
new file mode 100644
index 00000000000..c80a92940a9
--- /dev/null
+++ b/src/main/target/AOCODARCF4V3/config.c
@@ -0,0 +1,40 @@
+/*
+ * 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 "fc/config.h"
+
+#include "io/piniobox.h"
+#include "drivers/serial.h"
+#include "io/serial.h"
+void targetConfiguration(void)
+{
+    pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1;
+    pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2;
+    pinioBoxConfigMutable()->permanentId[2] = BOX_PERMANENT_ID_USER3;
+   // beeperConfigMutable()->pwmMode = true;
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART5)].functionMask = FUNCTION_MSP;
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART5)].msp_baudrateIndex = BAUD_115200;
+    serialConfigMutable()->portConfigs[4].functionMask = FUNCTION_VTX_TRAMP;
+        serialConfigMutable()->portConfigs[4].peripheral_baudrateIndex = BAUD_115200;
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART2)].functionMask = FUNCTION_RX_SERIAL;
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART3)].functionMask = FUNCTION_ESCSERIAL;
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].functionMask = FUNCTION_GPS;
+}
diff --git a/src/main/target/AOCODARCF4V3/target.c b/src/main/target/AOCODARCF4V3/target.c
new file mode 100644
index 00000000000..8581cc5bf11
--- /dev/null
+++ b/src/main/target/AOCODARCF4V3/target.c
@@ -0,0 +1,46 @@
+/*
+ * 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/io.h"
+#include "drivers/timer.h"
+#include "drivers/pwm_mapping.h"
+#include "drivers/bus.h"
+
+timerHardware_t timerHardware[] = {
+    DEF_TIM(TIM5,  CH4,  PA3,     TIM_USE_PPM,       0, 0 ),       // PPM IN
+
+    DEF_TIM(TIM8,   CH1,  PC6,    TIM_USE_OUTPUT_AUTO,  0, 1 ),     // S1 
+    DEF_TIM(TIM8,   CH2,  PC7,    TIM_USE_OUTPUT_AUTO,  0, 1 ),     // S2
+    DEF_TIM(TIM8,   CH3,  PC8,    TIM_USE_OUTPUT_AUTO,  0, 1 ),     // S3
+    DEF_TIM(TIM8,   CH4,  PC9,    TIM_USE_OUTPUT_AUTO,  0, 0 ),     // S4
+
+    DEF_TIM(TIM2,   CH1,  PA15,   TIM_USE_OUTPUT_AUTO,  0, 0 ),     // S5
+    DEF_TIM(TIM1,   CH1,  PA8,    TIM_USE_OUTPUT_AUTO,  0, 1 ),     // S6
+
+    DEF_TIM(TIM2,   CH3,  PB10,   TIM_USE_OUTPUT_AUTO,  0, 0 ),     // S7
+    DEF_TIM(TIM2,   CH4,  PB11,   TIM_USE_OUTPUT_AUTO,  0, 0 ),     // S8
+
+    DEF_TIM(TIM3,   CH4,  PB1,   TIM_USE_LED,  0, 0 ),     // LED_STRIP
+
+    
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
diff --git a/src/main/target/AOCODARCF4V3/target.h b/src/main/target/AOCODARCF4V3/target.h
new file mode 100644
index 00000000000..0c95564162e
--- /dev/null
+++ b/src/main/target/AOCODARCF4V3/target.h
@@ -0,0 +1,192 @@
+/*
+ * 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 USE_TARGET_CONFIG
+
+#ifdef AOCODARCF4V3
+#define TARGET_BOARD_IDENTIFIER "AOF4V3"
+#define USBD_PRODUCT_STRING     "AOCODARCF4V3"
+#else
+#define TARGET_BOARD_IDENTIFIER "AOF4V3SD"
+#define USBD_PRODUCT_STRING     "AOCODARCF4V3_SD"
+#endif
+
+// ******** Board LEDs  **********************
+#define LED0                            PC13
+
+// ******* Beeper ***********
+#define BEEPER                          PB8
+#define BEEPER_INVERTED
+
+
+// ******* GYRO and ACC ********
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN               CW90_DEG
+#define MPU6500_SPI_BUS                 BUS_SPI1
+#define MPU6500_CS_PIN                  PA4
+
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN               CW90_DEG
+#define MPU6000_SPI_BUS                 BUS_SPI1
+#define MPU6000_CS_PIN                  PA4
+
+#define USE_IMU_ICM42605
+#define IMU_ICM42605_ALIGN               CW90_DEG
+#define ICM42605_SPI_BUS                 BUS_SPI1
+#define ICM42605_CS_PIN                  PA4
+
+
+// *************** Baro **************************
+#define USE_I2C
+#define USE_I2C_DEVICE_1
+
+#define I2C1_SCL                        PB6        // SCL
+#define I2C1_SDA                        PB7        // SDA
+#define DEFAULT_I2C_BUS                 BUS_I2C1
+
+#define USE_BARO
+#define BARO_I2C_BUS                    DEFAULT_I2C_BUS
+
+#define USE_BARO_BMP280
+#define USE_BARO_MS5611
+#define USE_BARO_DPS310
+#define USE_BARO_SPL06
+
+//*********** Magnetometer / Compass *************
+#define USE_MAG
+#define MAG_I2C_BUS                     DEFAULT_I2C_BUS
+#define USE_MAG_ALL
+
+// ******* SERIAL ********
+#define USE_VCP
+#define VBUS_SENSING_PIN        PB12
+#define VBUS_SENSING_ENABLED
+
+#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
+#define UART4_TX_PIN                    PA0
+#define UART4_RX_PIN                    PA1
+
+#define USE_UART5
+#define UART5_TX_PIN                    PC12
+#define UART5_RX_PIN                    PD2
+
+
+#define SERIAL_PORT_COUNT              6
+
+
+// ******* SPI ********
+#define USE_SPI
+
+#define USE_SPI_DEVICE_1
+#define SPI1_NSS_PIN                    PA4
+#define SPI1_SCK_PIN                    PA5
+#define SPI1_MISO_PIN                   PA6
+#define SPI1_MOSI_PIN                   PA7
+
+#define USE_SPI_DEVICE_2
+#define SPI2_NSS_PIN                    PA13
+#define SPI2_SCK_PIN                    PB13
+#define SPI2_MISO_PIN                   PB14
+#define SPI2_MOSI_PIN                   PB15
+
+#define USE_SPI_DEVICE_3
+#define SPI3_NSS_PIN                    PC0
+#define SPI3_SCK_PIN                    PB3
+#define SPI3_MISO_PIN                   PB4
+#define SPI3_MOSI_PIN                   PB5
+
+// ******* ADC ********
+#define USE_ADC
+#define ADC_CHANNEL_1_PIN               PC1
+#define ADC_CHANNEL_2_PIN               PC2
+#define ADC_CHANNEL_3_PIN               PC3
+
+#define VBAT_ADC_CHANNEL                ADC_CHN_2
+#define RSSI_ADC_CHANNEL                ADC_CHN_3
+#define CURRENT_METER_ADC_CHANNEL       ADC_CHN_1
+
+#define VBAT_SCALE_DEFAULT              1100
+#define CURRENT_METER_SCALE             206
+
+// ******* OSD ********
+#define USE_OSD
+#define USE_MAX7456
+#define MAX7456_SPI_BUS                 BUS_SPI2
+#define MAX7456_CS_PIN                  PA13
+
+//******* FLASH ********
+#if defined(AOCODARCF4V3_SD)
+#define USE_SDCARD
+#define USE_SDCARD_SPI
+#define SDCARD_SPI_BUS          BUS_SPI3
+#define SDCARD_CS_PIN           PC0
+#define SDCARD_DETECT_INVERTED
+#define SDCARD_DETECT_PIN       PC14
+#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
+#else
+#define USE_FLASHFS
+#define USE_FLASH_M25P16
+#define M25P16_SPI_BUS                  BUS_SPI3
+#define M25P16_CS_PIN                   PC0
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+#endif
+// *************** PINIO ***************************
+#define USE_PINIO
+#define USE_PINIOBOX
+#define PINIO1_PIN                  PC5  // VTX power switcher
+#define PINIO2_PIN                  PA14  //bluetooth
+#define PINIO3_PIN                  PC15 //Camera control
+#define PINIO1_FLAGS				PINIO_FLAGS_INVERTED
+#define PINIO2_FLAGS				PINIO_FLAGS_INVERTED
+
+//************ LEDSTRIP *****************
+#define USE_LED_STRIP
+#define WS2811_PIN                      PB1
+
+// ******* FEATURES ********
+#define DEFAULT_RX_TYPE                 RX_TYPE_SERIAL
+#define SERIALRX_UART                   SERIAL_PORT_USART2
+#define SERIALRX_PROVIDER               SERIALRX_CRSF
+
+#define DEFAULT_FEATURES                (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_GPS | FEATURE_BLACKBOX | FEATURE_LED_STRIP)
+
+#define TARGET_IO_PORTA                 0xffff
+#define TARGET_IO_PORTB                 0xffff
+#define TARGET_IO_PORTC                 0xffff
+#define TARGET_IO_PORTD                 0xffff
+
+#define MAX_PWM_OUTPUT_PORTS            10
+
+// ESC-related features
+#define USE_DSHOT
+#define USE_SERIALSHOT
+#define USE_ESC_SENSOR
+#define USE_SERIAL_4WAY_BLHELI_INTERFACE

From c5a05c0496aef2b136100436febe456b4db592e3 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Pawe=C5=82=20Spychalski?= <pspychalski@gmail.com>
Date: Wed, 10 Apr 2024 14:13:50 +0200
Subject: [PATCH 061/323] Merge pull request #9919 from YI-BOYANG/master

Add GEPRC TAKERF722SE target
---
 src/main/target/TAKERF722SE/CMakeLists.txt |   1 +
 src/main/target/TAKERF722SE/target.c       |  48 ++++++
 src/main/target/TAKERF722SE/target.h       | 186 +++++++++++++++++++++
 3 files changed, 235 insertions(+)
 create mode 100644 src/main/target/TAKERF722SE/CMakeLists.txt
 create mode 100644 src/main/target/TAKERF722SE/target.c
 create mode 100644 src/main/target/TAKERF722SE/target.h

diff --git a/src/main/target/TAKERF722SE/CMakeLists.txt b/src/main/target/TAKERF722SE/CMakeLists.txt
new file mode 100644
index 00000000000..c4716f616ee
--- /dev/null
+++ b/src/main/target/TAKERF722SE/CMakeLists.txt
@@ -0,0 +1 @@
+target_stm32f722xe(TAKERF722SE)
\ No newline at end of file
diff --git a/src/main/target/TAKERF722SE/target.c b/src/main/target/TAKERF722SE/target.c
new file mode 100644
index 00000000000..247efb84e52
--- /dev/null
+++ b/src/main/target/TAKERF722SE/target.c
@@ -0,0 +1,48 @@
+/*
+* This file is part of INAV Project.
+*
+* 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/io.h"
+
+#include "drivers/dma.h"
+#include "drivers/timer.h"
+#include "drivers/timer_def.h"
+
+
+
+
+
+timerHardware_t timerHardware[] = {
+
+    DEF_TIM(TIM3, CH1, PB4,  TIM_USE_OUTPUT_AUTO, 0, 0),
+    DEF_TIM(TIM3, CH2, PB5,  TIM_USE_OUTPUT_AUTO, 0, 0),
+    DEF_TIM(TIM3, CH3, PB0,  TIM_USE_OUTPUT_AUTO, 0, 0),
+    DEF_TIM(TIM3, CH4, PB1,  TIM_USE_OUTPUT_AUTO, 0, 0),
+
+    DEF_TIM(TIM4, CH1, PB6,  TIM_USE_OUTPUT_AUTO, 0, 0),
+    DEF_TIM(TIM4, CH2, PB7,  TIM_USE_OUTPUT_AUTO, 0, 0),
+    DEF_TIM(TIM8, CH3, PC8,  TIM_USE_OUTPUT_AUTO, 0, 0),
+    DEF_TIM(TIM8, CH4, PC9,  TIM_USE_OUTPUT_AUTO, 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/TAKERF722SE/target.h b/src/main/target/TAKERF722SE/target.h
new file mode 100644
index 00000000000..61ab442653d
--- /dev/null
+++ b/src/main/target/TAKERF722SE/target.h
@@ -0,0 +1,186 @@
+/*
+ * 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.
+ *
+ * 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 "GEPR"
+
+#define USBD_PRODUCT_STRING     "TAKERF722SE"
+
+#define LED0                    PC14
+
+
+// *************** BEEPER ************************
+
+#define BEEPER                  PC13
+#define BEEPER_INVERTED
+
+
+// *************** LEDSTRIP ************************
+#define USE_LED_STRIP
+#define WS2811_PIN                  PA8
+
+
+// *************** UART *****************************
+#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            PB10
+#define UART3_RX_PIN            PB11
+
+#define USE_UART4
+#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 SERIAL_PORT_COUNT       7
+
+#define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER       SERIALRX_SBUS
+#define SERIALRX_UART           SERIAL_PORT_USART2
+
+
+// *************** SPI Bus **********************
+
+#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   	    PB14
+#define SPI2_MOSI_PIN   	    PB15
+
+#define USE_SPI_DEVICE_3
+#define SPI3_SCK_PIN            PC10
+#define SPI3_MISO_PIN           PC11
+#define SPI3_MOSI_PIN           PB2
+#define SPI3_SCK_AF             GPIO_AF6_SPI3
+#define SPI3_MISO_AF            GPIO_AF6_SPI3
+#define SPI3_MOSI_AF            GPIO_AF7_SPI3
+
+
+// *************** Gyro & ACC **********************
+
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW0_DEG
+#define MPU6000_CS_PIN          PA4
+#define MPU6000_SPI_BUS         BUS_SPI1
+
+
+#define USE_IMU_ICM42605
+#define IMU_ICM42605_ALIGN      CW0_DEG
+#define ICM42605_CS_PIN         PA4
+#define ICM42605_SPI_BUS        BUS_SPI1
+
+// *************** 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 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 USE_FLASHFS
+
+#define USE_FLASH_M25P16
+#define M25P16_CS_PIN           PA15
+#define M25P16_SPI_BUS          BUS_SPI3
+
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+
+// *************** OSD *****************************
+
+#define USE_MAX7456
+#define MAX7456_SPI_BUS         BUS_SPI2
+#define MAX7456_CS_PIN          PB12
+
+// *************** ADC *****************************
+
+#define USE_ADC
+#define ADC_INSTANCE                ADC1
+#define ADC1_DMA_STREAM             DMA2_Stream0
+#define ADC_CHANNEL_1_PIN           PC3
+#define ADC_CHANNEL_2_PIN           PC0
+#define ADC_CHANNEL_3_PIN           PC2
+
+#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)
+
+
+
+
+//**************************************************
+
+#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        8
+
+#define USE_DSHOT
+#define USE_ESC_SENSOR

From d27f3fdaa0e5b9f676a7f4e94a1c90e659e43703 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Wed, 17 Apr 2024 15:18:09 +0200
Subject: [PATCH 062/323] Updated Iterm locking mechanism

---
 src/main/flight/pid.c | 61 ++++++++++++++++---------------------------
 1 file changed, 23 insertions(+), 38 deletions(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index ae51385cf11..941e08a1a2e 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -107,10 +107,8 @@ typedef struct {
 
     smithPredictor_t smithPredictor;
 
-    float dampingFactorPrevious;
-    float dampingFactorLockValue;
-    float dampingFactotLockUntilMs;
-    pt1Filter_t dampingFactorFilter;
+    timeMs_t targetOverThresholdTimeMs;
+
 } pidState_t;
 
 STATIC_FASTRAM bool pidFiltersConfigured = false;
@@ -322,7 +320,6 @@ bool pidInitFilters(void)
 
     for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
         pt1FilterInit(&windupLpf[i], pidProfile()->iterm_relax_cutoff, US2S(refreshRate));
-        pt1FilterInit(&pidState[i].dampingFactorFilter, 2.0f, US2S(refreshRate));
     }
 
 #ifdef USE_ANTIGRAVITY
@@ -745,53 +742,43 @@ static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axi
 static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv)
 {
     const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget);
+    const float rateError = rateTarget - pidState->gyroRate;
 
     const float maxRate = currentControlRateProfile->stabilized.rates[axis] * 10.0f;
     const float dampingFactor = attenuation(rateTarget, maxRate / 2.5f);
 
-    float dampingFactorP;
-    float dampingFactorD;
-    float dampingFactorI;
-
     /*
-    New idea.... 
-    Iterm damping is applied (down to 0) when:
-    abs(error) > 10% rate and sticks were moved in the last 500ms (hard stop at this mark)
+     * Iterm damping is applied (down to 0) when:
+     * abs(error) > 10% rate and sticks were moved in the last 500ms (hard stop at this mark)
 
-    itermAttenuation  = MIN(curve(setpoint), (abs(error) > 10%) && (sticks were deflected in 500ms) ? 0 : 1)
-    
-    */
+     * itermAttenuation  = MIN(curve(setpoint), (abs(error) > 10%) && (sticks were deflected in 500ms) ? 0 : 1)
+     */
 
-    if (fabsf(dampingFactor) <= fabsf(pidState->dampingFactorPrevious)) {
-        dampingFactorI = dampingFactor;
+    //If error is greater than 10% or max rate
+    const bool errorThresholdReached = fabsf(rateError) > maxRate * 0.1f;
 
-        pidState->dampingFactotLockUntilMs = millis() + scaleRangef(fabsf(dampingFactor), 1.0f, 0.0f, 0, 300);
-        pidState->dampingFactorLockValue = dampingFactor;
+    //If stick (setpoint) was moved above threshold in the last 500ms
+    if (fabsf(rateTarget) > maxRate * 0.2f) {
+        pidState->targetOverThresholdTimeMs = millis();
+    }
 
-        // pt1FilterReset(&pidState->dampingFactorFilter, pidState->dampingFactorPrevious);
-        pidState->dampingFactorPrevious = dampingFactor;
-    } else {
-        
-        if (millis() > pidState->dampingFactorLockValue) {
-            dampingFactorI = dampingFactor;
-            pidState->dampingFactorPrevious = dampingFactor;
-        } else {
-            dampingFactorI = pidState->dampingFactorLockValue;
-        }
+    //If error is below threshold, we no longer track time for lock mechanism
+    if (!errorThresholdReached) {
+        pidState->targetOverThresholdTimeMs = 0;
     }
 
+    const float dampingFactorI = MIN(dampingFactor, (errorThresholdReached && (millis() - pidState->targetOverThresholdTimeMs) < 500) ? 0.0f : 1.0f);
+
     //P & D damping factors are always the same and based on current damping factor
-    dampingFactorP = dampingFactor;
-    dampingFactorD = dampingFactor;
+    const float dampingFactorP = dampingFactor;
+    const float dampingFactorD = dampingFactor;
 
     if (axis == FD_ROLL) {
-        DEBUG_SET(DEBUG_ALWAYS, 0, pidState->dampingFactorPrevious * 1000);
-        DEBUG_SET(DEBUG_ALWAYS, 1, dampingFactorP * 1000);
-        DEBUG_SET(DEBUG_ALWAYS, 2, dampingFactorI * 1000);
-        DEBUG_SET(DEBUG_ALWAYS, 3, dampingFactorD * 1000);
+        DEBUG_SET(DEBUG_ALWAYS, 0, dampingFactorP * 1000);
+        DEBUG_SET(DEBUG_ALWAYS, 1, dampingFactorI * 1000);
+        DEBUG_SET(DEBUG_ALWAYS, 2, dampingFactorD * 1000);
     }
 
-    const float rateError = rateTarget - pidState->gyroRate;
     const float newPTerm = pTermProcess(pidState, rateError, dT) * dampingFactorP;
     const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv) * dampingFactorD;
     const float newFFTerm = rateTarget * pidState->kFF;
@@ -1309,8 +1296,6 @@ void pidInit(void)
 
     for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) {
 
-        pidState[axis].dampingFactorPrevious = 1.0f;
-
     #ifdef USE_D_BOOST
         // Rate * 10 * 10. First 10 is to convert stick to DPS. Second 10 is to convert target to acceleration.
         // We assume, max acceleration is when pilot deflects the stick fully in 100ms

From b810f99b310e6bae1840d1068eb954cab7fac101 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Wed, 17 Apr 2024 16:45:40 +0200
Subject: [PATCH 063/323] Refactor to contain logic in a single function

---
 src/main/flight/pid.c | 51 ++++++++++++++++++++++++++-----------------
 1 file changed, 31 insertions(+), 20 deletions(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 941e08a1a2e..805450ca3e6 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -65,6 +65,14 @@
 
 #include "programming/logic_condition.h"
 
+typedef struct {
+    float aP;
+    float aI;
+    float aD;
+    float aFF;
+    timeMs_t targetOverThresholdTimeMs;
+} fwPidAttenuation_t;
+
 typedef struct {
     uint8_t axis;
     float kP;   // Proportional gain
@@ -107,8 +115,7 @@ typedef struct {
 
     smithPredictor_t smithPredictor;
 
-    timeMs_t targetOverThresholdTimeMs;
-
+    fwPidAttenuation_t attenuation;
 } pidState_t;
 
 STATIC_FASTRAM bool pidFiltersConfigured = false;
@@ -739,12 +746,8 @@ static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axi
     UNUSED(dT_inv);
 }
 
-static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv)
-{
-    const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget);
-    const float rateError = rateTarget - pidState->gyroRate;
-
-    const float maxRate = currentControlRateProfile->stabilized.rates[axis] * 10.0f;
+static void fwRateAttenuation(pidState_t *pidState, const float rateTarget, const float rateError) {
+    const float maxRate = currentControlRateProfile->stabilized.rates[pidState->axis] * 10.0f;
     const float dampingFactor = attenuation(rateTarget, maxRate / 2.5f);
 
     /*
@@ -759,35 +762,43 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
 
     //If stick (setpoint) was moved above threshold in the last 500ms
     if (fabsf(rateTarget) > maxRate * 0.2f) {
-        pidState->targetOverThresholdTimeMs = millis();
+        pidState->attenuation.targetOverThresholdTimeMs = millis();
     }
 
     //If error is below threshold, we no longer track time for lock mechanism
     if (!errorThresholdReached) {
-        pidState->targetOverThresholdTimeMs = 0;
+        pidState->attenuation.targetOverThresholdTimeMs = 0;
     }
 
-    const float dampingFactorI = MIN(dampingFactor, (errorThresholdReached && (millis() - pidState->targetOverThresholdTimeMs) < 500) ? 0.0f : 1.0f);
+    pidState->attenuation.aI = MIN(dampingFactor, (errorThresholdReached && (millis() - pidState->attenuation.targetOverThresholdTimeMs) < 500) ? 0.0f : 1.0f);
 
     //P & D damping factors are always the same and based on current damping factor
-    const float dampingFactorP = dampingFactor;
-    const float dampingFactorD = dampingFactor;
+    pidState->attenuation.aP = dampingFactor;
+    pidState->attenuation.aD = dampingFactor;
 
-    if (axis == FD_ROLL) {
-        DEBUG_SET(DEBUG_ALWAYS, 0, dampingFactorP * 1000);
-        DEBUG_SET(DEBUG_ALWAYS, 1, dampingFactorI * 1000);
-        DEBUG_SET(DEBUG_ALWAYS, 2, dampingFactorD * 1000);
+    if (pidState->axis == FD_ROLL) {
+        DEBUG_SET(DEBUG_ALWAYS, 0, pidState->attenuation.aP * 1000);
+        DEBUG_SET(DEBUG_ALWAYS, 1, pidState->attenuation.aI * 1000);
+        DEBUG_SET(DEBUG_ALWAYS, 2, pidState->attenuation.aD * 1000);
     }
+}
+
+static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv)
+{
+    const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget);
+    const float rateError = rateTarget - pidState->gyroRate;
+
+    fwRateAttenuation(pidState, rateTarget, rateError);
 
-    const float newPTerm = pTermProcess(pidState, rateError, dT) * dampingFactorP;
-    const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv) * dampingFactorD;
+    const float newPTerm = pTermProcess(pidState, rateError, dT) * pidState->attenuation.aP;
+    const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv) * pidState->attenuation.aD;
     const float newFFTerm = rateTarget * pidState->kFF;
 
     /*
      * Integral should be updated only if axis Iterm is not frozen
      */
     if (!pidState->itermFreezeActive) {
-        pidState->errorGyroIf += rateError * pidState->kI * dT * dampingFactorI;
+        pidState->errorGyroIf += rateError * pidState->kI * dT * pidState->attenuation.aI;
     }
 
     applyItermLimiting(pidState);

From db497ffb2b56caffce0ec6b8a5a4ab230483c1b9 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Wed, 17 Apr 2024 18:18:07 +0200
Subject: [PATCH 064/323] Optimize PID axis processing

---
 src/main/flight/pid.c | 49 +++++++++++++++++++++----------------------
 1 file changed, 24 insertions(+), 25 deletions(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index c61fe5353f7..02ff8c2dad8 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -154,7 +154,7 @@ static EXTENDED_FASTRAM float iTermAntigravityGain;
 #endif
 static EXTENDED_FASTRAM uint8_t usedPidControllerType;
 
-typedef void (*pidControllerFnPtr)(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv);
+typedef void (*pidControllerFnPtr)(pidState_t *pidState, float dT, float dT_inv);
 static EXTENDED_FASTRAM pidControllerFnPtr pidControllerApplyFn;
 static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn;
 static EXTENDED_FASTRAM bool levelingEnabled = false;
@@ -744,16 +744,15 @@ static void applyItermLimiting(pidState_t *pidState) {
     }
 }
 
-static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv) {
+static void nullRateController(pidState_t *pidState, float dT, float dT_inv) {
     UNUSED(pidState);
-    UNUSED(axis);
     UNUSED(dT);
     UNUSED(dT_inv);
 }
 
-static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv)
+static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, float dT, float dT_inv)
 {
-    const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget);
+    const float rateTarget = getFlightAxisRateOverride(pidState->axis, pidState->rateTarget);
 
     const float rateError = rateTarget - pidState->gyroRate;
     const float newPTerm = pTermProcess(pidState, rateError, dT);
@@ -769,16 +768,16 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
 
     applyItermLimiting(pidState);
 
-    const uint16_t limit = getPidSumLimit(axis);
+    const uint16_t limit = getPidSumLimit(pidState->axis);
 
     if (pidProfile()->pidItermLimitPercent != 0){
         float itermLimit = limit * pidProfile()->pidItermLimitPercent * 0.01f;
         pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -itermLimit, +itermLimit);
     }
 
-    axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf + newDTerm, -limit, +limit);
+    axisPID[pidState->axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf + newDTerm, -limit, +limit);
 
-    if (FLIGHT_MODE(SOARING_MODE) && axis == FD_PITCH && calculateRollPitchCenterStatus() == CENTERED) {
+    if (FLIGHT_MODE(SOARING_MODE) && pidState->axis == FD_PITCH && calculateRollPitchCenterStatus() == CENTERED) {
         if (!angleFreefloatDeadband(DEGREES_TO_DECIDEGREES(navConfig()->fw.soaring_pitch_deadband), FD_PITCH)) {
             axisPID[FD_PITCH] = 0;  // center pitch servo if pitch attitude within soaring mode deadband
         }
@@ -786,16 +785,16 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
 
 #ifdef USE_AUTOTUNE_FIXED_WING
     if (FLIGHT_MODE(AUTO_TUNE) && !FLIGHT_MODE(MANUAL_MODE)) {
-        autotuneFixedWingUpdate(axis, rateTarget, pidState->gyroRate, constrainf(newPTerm + newFFTerm, -limit, +limit));
+        autotuneFixedWingUpdate(pidState->axis, rateTarget, pidState->gyroRate, constrainf(newPTerm + newFFTerm, -limit, +limit));
     }
 #endif
 
 #ifdef USE_BLACKBOX
-    axisPID_P[axis] = newPTerm;
-    axisPID_I[axis] = pidState->errorGyroIf;
-    axisPID_D[axis] = newDTerm;
-    axisPID_F[axis] = newFFTerm;
-    axisPID_Setpoint[axis] = rateTarget;
+    axisPID_P[pidState->axis] = newPTerm;
+    axisPID_I[pidState->axis] = pidState->errorGyroIf;
+    axisPID_D[pidState->axis] = newDTerm;
+    axisPID_F[pidState->axis] = newFFTerm;
+    axisPID_Setpoint[pidState->axis] = rateTarget;
 #endif
 
     pidState->previousRateGyro = pidState->gyroRate;
@@ -818,10 +817,10 @@ static float FAST_CODE applyItermRelax(const int axis, float currentPidSetpoint,
     return itermErrorRate;
 }
 
-static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv)
+static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pidState, float dT, float dT_inv)
 {
 
-    const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget);
+    const float rateTarget = getFlightAxisRateOverride(pidState->axis, pidState->rateTarget);
 
     const float rateError = rateTarget - pidState->gyroRate;
     const float newPTerm = pTermProcess(pidState, rateError, dT);
@@ -835,13 +834,13 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid
      */
     const float newCDTerm = rateTargetDeltaFiltered * pidState->kCD;
 
-    const uint16_t limit = getPidSumLimit(axis);
+    const uint16_t limit = getPidSumLimit(pidState->axis);
 
     // TODO: Get feedback from mixer on available correction range for each axis
     const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf + newCDTerm;
     const float newOutputLimited = constrainf(newOutput, -limit, +limit);
 
-    float itermErrorRate = applyItermRelax(axis, rateTarget, rateError);
+    float itermErrorRate = applyItermRelax(pidState->axis, rateTarget, rateError);
 
 #ifdef USE_ANTIGRAVITY
     itermErrorRate *= iTermAntigravityGain;
@@ -859,14 +858,14 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid
     // Don't grow I-term if motors are at their limit
     applyItermLimiting(pidState);
 
-    axisPID[axis] = newOutputLimited;
+    axisPID[pidState->axis] = newOutputLimited;
 
 #ifdef USE_BLACKBOX
-    axisPID_P[axis] = newPTerm;
-    axisPID_I[axis] = pidState->errorGyroIf;
-    axisPID_D[axis] = newDTerm;
-    axisPID_F[axis] = newCDTerm;
-    axisPID_Setpoint[axis] = rateTarget;
+    axisPID_P[pidState->axis] = newPTerm;
+    axisPID_I[pidState->axis] = pidState->errorGyroIf;
+    axisPID_D[pidState->axis] = newDTerm;
+    axisPID_F[pidState->axis] = newCDTerm;
+    axisPID_Setpoint[pidState->axis] = rateTarget;
 #endif
 
     pidState->previousRateTarget = rateTarget;
@@ -1230,7 +1229,7 @@ void FAST_CODE pidController(float dT)
         checkItermLimitingActive(&pidState[axis]);
         checkItermFreezingActive(&pidState[axis], axis);
 
-        pidControllerApplyFn(&pidState[axis], axis, dT, dT_inv);
+        pidControllerApplyFn(&pidState[axis], dT, dT_inv);
     }
 }
 

From 09ed11ca35a7cd09741c17723287348005eb0bf5 Mon Sep 17 00:00:00 2001
From: error414 <error414@error414.com>
Date: Wed, 17 Apr 2024 21:34:31 +0200
Subject: [PATCH 065/323] rename NRA15/NRA24 only to NRA

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

diff --git a/docs/Rangefinder.md b/docs/Rangefinder.md
index 813184f8374..b53ae439b7e 100644
--- a/docs/Rangefinder.md
+++ b/docs/Rangefinder.md
@@ -27,15 +27,15 @@ Following rangefinders are supported:
 NRA15/NRA24 from nanoradar use US-D1_V0 or NRA protocol, it depends which firmware you use. Radar can be set by firmware
 to two different resolutions. See table below.
 
-| Radar | Protocol | Resolution      | Name in configurator  |
-|-------|----------|-----------------|-----------------------|
-| NRA15 | US-D1_V0 | 0-30m (+-4cm)   | USD1_V0               |
-| NRA15 | NRA      | 0-30m (+-4cm)   | NRA15/NRA24           | 
-| NRA15 | NRA      | 0-100m (+-10cm) | NRA15/NRA24           | 
-| NRA24 | US-D1_V0 | 0-50m (+-4cm)   | USD1_V0               |
-| NRA24 | US-D1_V0 | 0-200m (+-10cm) | USD1_V0               |
-| NRA24 | NRA      | 0-50m (+-4cm)   | NRA15/NRA24           | 
-| NRA24 | NRA      | 0-200m (+-10cm) | NRA15/NRA24           | 
+| Radar | Protocol | Resolution      | Name in configurator |
+|-------|----------|-----------------|----------------------|
+| NRA15 | US-D1_V0 | 0-30m (+-4cm)   | USD1_V0              |
+| NRA15 | NRA      | 0-30m (+-4cm)   | NRA                  | 
+| NRA15 | NRA      | 0-100m (+-10cm) | NRA                  | 
+| NRA24 | US-D1_V0 | 0-50m (+-4cm)   | USD1_V0              |
+| NRA24 | US-D1_V0 | 0-200m (+-10cm) | USD1_V0              |
+| NRA24 | NRA      | 0-50m (+-4cm)   | NRA                  | 
+| NRA24 | NRA      | 0-200m (+-10cm) | NRA                  | 
 
 
 ## Connections
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index b2be345391d..11658135417 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -5,7 +5,7 @@ tables:
     values: ["NONE", "AUTO", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "BMI270","LSM6DXX",  "FAKE"]
     enum: accelerationSensor_e
   - name: rangefinder_hardware
-    values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C", "FAKE", "TERARANGER_EVO", "USD1_V0", "NRA15/NRA24"]
+    values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C", "FAKE", "TERARANGER_EVO", "USD1_V0", "NRA"]
     enum: rangefinderType_e
   - name: mag_hardware
     values: ["NONE", "AUTO", "HMC5883", "AK8975", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", "VCM5883", "MLX90393", "FAKE"]

From a1d28a5e2c8f2f57112d4cc5f6de954dd3803f6e Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sat, 20 Apr 2024 21:46:05 +0200
Subject: [PATCH 066/323] Merge pull request #9807 from jhemcu/jhemcu-targets

Target: Add JHEMCUF405WING, JHEMCUF745, JHEMCUH743HD board
---
 src/main/target/JHEMCUF405WING/CMakeLists.txt |   1 +
 src/main/target/JHEMCUF405WING/config.c       |  42 ++++
 src/main/target/JHEMCUF405WING/target.c       |  47 ++++
 src/main/target/JHEMCUF405WING/target.h       | 163 ++++++++++++++
 src/main/target/JHEMCUF745/CMakeLists.txt     |   1 +
 src/main/target/JHEMCUF745/config.c           |  29 +++
 src/main/target/JHEMCUF745/target.c           |  44 ++++
 src/main/target/JHEMCUF745/target.h           | 157 ++++++++++++++
 src/main/target/JHEMCUH743HD/CMakeLists.txt   |   1 +
 src/main/target/JHEMCUH743HD/config.c         |  66 ++++++
 src/main/target/JHEMCUH743HD/target.c         |  49 +++++
 src/main/target/JHEMCUH743HD/target.h         | 205 ++++++++++++++++++
 12 files changed, 805 insertions(+)
 create mode 100644 src/main/target/JHEMCUF405WING/CMakeLists.txt
 create mode 100644 src/main/target/JHEMCUF405WING/config.c
 create mode 100644 src/main/target/JHEMCUF405WING/target.c
 create mode 100644 src/main/target/JHEMCUF405WING/target.h
 create mode 100644 src/main/target/JHEMCUF745/CMakeLists.txt
 create mode 100644 src/main/target/JHEMCUF745/config.c
 create mode 100644 src/main/target/JHEMCUF745/target.c
 create mode 100644 src/main/target/JHEMCUF745/target.h
 create mode 100644 src/main/target/JHEMCUH743HD/CMakeLists.txt
 create mode 100644 src/main/target/JHEMCUH743HD/config.c
 create mode 100644 src/main/target/JHEMCUH743HD/target.c
 create mode 100644 src/main/target/JHEMCUH743HD/target.h

diff --git a/src/main/target/JHEMCUF405WING/CMakeLists.txt b/src/main/target/JHEMCUF405WING/CMakeLists.txt
new file mode 100644
index 00000000000..fb99881c9bd
--- /dev/null
+++ b/src/main/target/JHEMCUF405WING/CMakeLists.txt
@@ -0,0 +1 @@
+target_stm32f405xg(JHEMCUF405WING)
diff --git a/src/main/target/JHEMCUF405WING/config.c b/src/main/target/JHEMCUF405WING/config.c
new file mode 100644
index 00000000000..07f44ab29f7
--- /dev/null
+++ b/src/main/target/JHEMCUF405WING/config.c
@@ -0,0 +1,42 @@
+/*
+ * 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 <stdbool.h>
+#include <stdint.h>
+
+#include "platform.h"
+
+#include "fc/fc_msp_box.h"
+#include "fc/config.h"
+#include "io/serial.h"
+#include "io/piniobox.h"
+
+void targetConfiguration(void)
+{
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].functionMask = FUNCTION_RX_SERIAL;
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART3)].functionMask = FUNCTION_GPS;
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART6)].functionMask = FUNCTION_MSP;
+	
+    pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1;
+}
diff --git a/src/main/target/JHEMCUF405WING/target.c b/src/main/target/JHEMCUF405WING/target.c
new file mode 100644
index 00000000000..e8187f936b2
--- /dev/null
+++ b/src/main/target/JHEMCUF405WING/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 <stdbool.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, CH2, PB7, TIM_USE_OUTPUT_AUTO, 1, 0), // S1 D(1,3,2)
+    DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 1, 0), // S2 D(1,0,2)
+
+    DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 1, 0), // S3 D(1,7,5)
+    DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 1, 0), // S4 D(1,2,5)
+    DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 1, 0), // S5 D(2,4,7)
+    DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 1, 0), // S6 D(2,7,7)
+    
+    DEF_TIM(TIM8, CH2N,PB14, TIM_USE_OUTPUT_AUTO, 1, 0), // S7
+    DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 1, 0), // S8
+    DEF_TIM(TIM2, CH3, PB10, TIM_USE_OUTPUT_AUTO, 1, 0), // S9
+    DEF_TIM(TIM2, CH4, PB11, TIM_USE_OUTPUT_AUTO, 1, 0), // S10
+    DEF_TIM(TIM12, CH2,PB15, TIM_USE_OUTPUT_AUTO, 1, 0), // S11 
+
+    DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // 2812LED
+    DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), // TX2
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
diff --git a/src/main/target/JHEMCUF405WING/target.h b/src/main/target/JHEMCUF405WING/target.h
new file mode 100644
index 00000000000..fd64ff153d6
--- /dev/null
+++ b/src/main/target/JHEMCUF405WING/target.h
@@ -0,0 +1,163 @@
+/*
+ * 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 USE_TARGET_CONFIG
+
+#define TARGET_BOARD_IDENTIFIER "JH45"
+#define USBD_PRODUCT_STRING     "JHEMCUF405WING"
+
+// LEDs
+#define LED0                    PA14  //Blue
+#define LED1                    PA13  //Green
+
+// Beeper
+#define BEEPER                  PC15
+#define BEEPER_INVERTED
+
+// SPI1
+#define USE_SPI
+#define USE_SPI_DEVICE_1
+#define SPI1_SCK_PIN            PA5
+#define SPI1_MISO_PIN   	    PA6
+#define SPI1_MOSI_PIN   	    PA7
+
+// SPI2
+#define USE_SPI_DEVICE_2
+#define SPI2_SCK_PIN            PB13
+#define SPI2_MISO_PIN   	    PC2
+#define SPI2_MOSI_PIN   	    PC3
+
+// SPI3
+#define USE_SPI_DEVICE_3
+#define SPI3_SCK_PIN            PB3
+#define SPI3_MISO_PIN   	    PB4
+#define SPI3_MOSI_PIN   	    PB5
+
+// Serial ports
+#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
+#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
+
+// Optional Softserial on UART2 TX Pin PA2
+#define USE_SOFTSERIAL1
+#define SOFTSERIAL_1_TX_PIN      PA2
+#define SOFTSERIAL_1_RX_PIN      PA2
+
+#define SERIAL_PORT_COUNT       8
+
+#define USE_I2C
+#define USE_I2C_DEVICE_1
+#define I2C1_SCL                PB8
+#define I2C1_SDA                PB9
+
+// ICM42605/ICM42688P
+#define USE_IMU_ICM42605
+#define IMU_ICM42605_ALIGN      CW270_DEG
+#define ICM42605_CS_PIN         PA4
+#define ICM42605_SPI_BUS        BUS_SPI1
+
+// Baro
+#define USE_BARO
+#define BARO_I2C_BUS            BUS_I2C1
+#define USE_BARO_ALL
+
+// Mag
+#define USE_MAG
+#define MAG_I2C_BUS             BUS_I2C1
+#define USE_MAG_ALL 
+
+#define RANGEFINDER_I2C_BUS     BUS_I2C1
+#define PITOT_I2C_BUS           BUS_I2C1
+#define TEMPERATURE_I2C_BUS     BUS_I2C1
+
+// OSD
+#define USE_MAX7456
+#define MAX7456_CS_PIN          PB12
+#define MAX7456_SPI_BUS         BUS_SPI2
+
+// SD Card
+#define USE_SDCARD
+#define USE_SDCARD_SPI
+#define SDCARD_SPI_BUS          BUS_SPI3
+#define SDCARD_CS_PIN           PC14
+
+#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
+
+#define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER       SERIALRX_CRSF
+#define SERIALRX_UART           SERIAL_PORT_USART1
+
+// 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 ADC_CHANNEL_3_PIN           PC5
+#define ADC_CHANNEL_4_PIN           PC4
+#define VBAT_ADC_CHANNEL            ADC_CHN_1
+#define CURRENT_METER_ADC_CHANNEL   ADC_CHN_2
+#define RSSI_ADC_CHANNEL            ADC_CHN_4
+#define AIRSPEED_ADC_CHANNEL        ADC_CHN_3
+
+// LED2812
+#define USE_LED_STRIP
+#define WS2811_PIN              PA8
+
+// PINIO
+#define USE_PINIO
+#define USE_PINIOBOX
+#define PINIO1_PIN              PC13  // 2xCamera switcher
+
+// OTHERS
+#define DEFAULT_FEATURES        (FEATURE_TX_PROF_SEL | FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY | FEATURE_BLACKBOX | FEATURE_AIRMODE)
+#define CURRENT_METER_SCALE     175
+
+#define USE_DSHOT
+#define USE_ESC_SENSOR
+#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    11
\ No newline at end of file
diff --git a/src/main/target/JHEMCUF745/CMakeLists.txt b/src/main/target/JHEMCUF745/CMakeLists.txt
new file mode 100644
index 00000000000..5f5a85e7c5d
--- /dev/null
+++ b/src/main/target/JHEMCUF745/CMakeLists.txt
@@ -0,0 +1 @@
+target_stm32f745xg(JHEMCUF745)
diff --git a/src/main/target/JHEMCUF745/config.c b/src/main/target/JHEMCUF745/config.c
new file mode 100644
index 00000000000..fb2fe04f963
--- /dev/null
+++ b/src/main/target/JHEMCUF745/config.c
@@ -0,0 +1,29 @@
+/*
+ * 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 <stdbool.h>
+#include <stdint.h>
+
+#include <platform.h>
+
+#include "drivers/pwm_mapping.h"
+
+void targetConfiguration(void)
+{
+    timerOverridesMutable(timer2id(TIM1))->outputMode = OUTPUT_MODE_MOTORS;
+    timerOverridesMutable(timer2id(TIM3))->outputMode = OUTPUT_MODE_MOTORS;
+}
diff --git a/src/main/target/JHEMCUF745/target.c b/src/main/target/JHEMCUF745/target.c
new file mode 100644
index 00000000000..f603cbe43be
--- /dev/null
+++ b/src/main/target/JHEMCUF745/target.c
@@ -0,0 +1,44 @@
+/*
+ * This file is part of INAV.
+ *
+ * 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 "drivers/io.h"
+#include "drivers/pwm_mapping.h"
+#include "drivers/timer.h"
+#include "drivers/bus.h"
+#include "drivers/pinio.h"
+
+timerHardware_t timerHardware[] = {
+    DEF_TIM(TIM3, CH3, PB0,  TIM_USE_OUTPUT_AUTO,   0, 0), // M1, DMA1_ST7
+    DEF_TIM(TIM3, CH4, PB1,  TIM_USE_OUTPUT_AUTO,   0, 0), // M2, DMA1_ST2
+    DEF_TIM(TIM1, CH1, PE9,  TIM_USE_OUTPUT_AUTO,   0, 0), // M3
+    DEF_TIM(TIM1, CH2, PE11, TIM_USE_OUTPUT_AUTO,   0, 1), // M4, DMA2_ST4
+    DEF_TIM(TIM8, CH4, PC9,  TIM_USE_OUTPUT_AUTO,   0, 0), // M5, DMA2_ST7
+    DEF_TIM(TIM5, CH4, PA3,  TIM_USE_OUTPUT_AUTO,   0, 0), // M6, DMA1_ST1
+    DEF_TIM(TIM3, CH1, PB4,  TIM_USE_OUTPUT_AUTO,   0, 0), // S7
+    DEF_TIM(TIM3, CH2, PB5,  TIM_USE_OUTPUT_AUTO,   0, 0), // S8
+    DEF_TIM(TIM4, CH1, PD12, TIM_USE_LED,           0, 0), // LED_STRIP, DMA1_ST0
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
diff --git a/src/main/target/JHEMCUF745/target.h b/src/main/target/JHEMCUF745/target.h
new file mode 100644
index 00000000000..dfd45749d2d
--- /dev/null
+++ b/src/main/target/JHEMCUF745/target.h
@@ -0,0 +1,157 @@
+/*
+ * This file is part of INAV.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software 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 and Betaflight are distributed in the hope that they
+ * 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 software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#define TARGET_BOARD_IDENTIFIER "JHF7"
+#define USBD_PRODUCT_STRING     "JHEMCUF745"
+
+#define LED0                PA2
+
+#define BEEPER              PD15
+#define BEEPER_INVERTED
+
+#define USE_DSHOT
+#define USE_DSHOT_DMAR
+#define USE_ESC_SENSOR
+
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN    CW270_DEG
+#define GYRO_INT_EXTI        PE1
+#define MPU6000_CS_PIN       SPI4_NSS_PIN
+#define MPU6000_SPI_BUS      BUS_SPI4
+
+#define USE_IMU_ICM42605
+#define IMU_ICM42605_ALIGN   CW270_DEG
+#define ICM42605_CS_PIN      SPI4_NSS_PIN
+#define ICM42605_SPI_BUS     BUS_SPI4
+#define ICM42605_EXTI_PIN    PE1 
+
+#define USB_IO
+#define USE_VCP
+#define VBUS_SENSING_ENABLED
+#define VBUS_SENSING_PIN    PA8
+
+#define USE_UART1
+#define UART1_TX_PIN        PA9
+#define UART1_RX_PIN        PA10
+
+#define USE_UART2
+#define UART2_TX_PIN        PD5
+#define UART2_RX_PIN        PD6
+
+#define USE_UART3
+#define UART3_TX_PIN        PB10
+#define UART3_RX_PIN        PB11
+
+#define USE_UART4
+#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_UART7
+#define UART7_RX_PIN PE7
+#define UART7_TX_PIN PE8
+
+#define SERIAL_PORT_COUNT   8 // VCP,UART1,UART2,UART3,UART4,UART5,UART6
+
+#define USE_SPI
+#define USE_SPI_DEVICE_1    // FLASH
+#define USE_SPI_DEVICE_2    // OSD
+#define USE_SPI_DEVICE_4    // ICM20689
+
+#define SPI1_NSS_PIN        PA4
+#define SPI1_SCK_PIN        PA5
+#define SPI1_MISO_PIN       PA6
+#define SPI1_MOSI_PIN       PA7
+
+#define SPI2_NSS_PIN        PB12
+#define SPI2_SCK_PIN        PB13
+#define SPI2_MISO_PIN       PB14
+#define SPI2_MOSI_PIN       PB15
+
+#define SPI4_NSS_PIN        PE4
+#define SPI4_SCK_PIN        PE2
+#define SPI4_MISO_PIN       PE5
+#define SPI4_MOSI_PIN       PE6
+
+#define USE_MAX7456
+#define MAX7456_SPI_BUS     BUS_SPI2
+#define MAX7456_CS_PIN      SPI2_NSS_PIN
+
+#define M25P16_CS_PIN       SPI1_NSS_PIN
+#define M25P16_SPI_BUS      BUS_SPI1
+#define USE_FLASHFS
+#define USE_FLASH_M25P16
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+
+#define USE_I2C
+#define USE_I2C_DEVICE_1
+#define I2C1_SCL            PB6
+#define I2C1_SDA            PB7
+
+// External I2C bus is the same as interal one
+#define MAG_I2C_BUS         BUS_I2C1
+#define TEMPERATURE_I2C_BUS BUS_I2C1
+#define RANGEFINDER_I2C_BUS BUS_I2C1
+#define DEFAULT_I2C_BUS     BUS_I2C1
+
+#define USE_BARO
+#define USE_BARO_ALL
+#define BARO_I2C_BUS        BUS_I2C1
+
+#define USE_MAG
+#define USE_MAG_ALL
+
+#define USE_ADC
+#define ADC_CHANNEL_1_PIN   PC2
+#define ADC_CHANNEL_2_PIN   PC3
+#define ADC_CHANNEL_3_PIN   PC5
+
+#define CURRENT_METER_ADC_CHANNEL   ADC_CHN_1
+#define VBAT_ADC_CHANNEL            ADC_CHN_2
+#define RSSI_ADC_CHANNEL            ADC_CHN_3
+
+#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
+
+#define DEFAULT_FEATURES    (FEATURE_TX_PROF_SEL | FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_VBAT | FEATURE_BLACKBOX)
+#define DEFAULT_RX_TYPE     RX_TYPE_SERIAL
+#define SERIALRX_UART       SERIAL_PORT_USART1
+#define SERIALRX_PROVIDER   SERIALRX_SBUS
+
+#define USE_LED_STRIP
+#define WS2811_PIN          PD12
+
+#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
+#define TARGET_IO_PORTE 0xffff
+
+#define MAX_PWM_OUTPUT_PORTS    8
diff --git a/src/main/target/JHEMCUH743HD/CMakeLists.txt b/src/main/target/JHEMCUH743HD/CMakeLists.txt
new file mode 100644
index 00000000000..b0192886d6c
--- /dev/null
+++ b/src/main/target/JHEMCUH743HD/CMakeLists.txt
@@ -0,0 +1 @@
+target_stm32h743xi(JHEMCUH743HD)
\ No newline at end of file
diff --git a/src/main/target/JHEMCUH743HD/config.c b/src/main/target/JHEMCUH743HD/config.c
new file mode 100644
index 00000000000..4f0aea919e7
--- /dev/null
+++ b/src/main/target/JHEMCUH743HD/config.c
@@ -0,0 +1,66 @@
+/*
+ * 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 "common/axis.h"
+
+#include "config/config_master.h"
+#include "config/feature.h"
+
+#include "drivers/sensor.h"
+#include "drivers/pwm_esc_detect.h"
+#include "drivers/pwm_output.h"
+#include "drivers/serial.h"
+
+#include "fc/rc_controls.h"
+
+#include "flight/failsafe.h"
+#include "flight/mixer.h"
+#include "flight/pid.h"
+
+#include "rx/rx.h"
+
+#include "io/serial.h"
+
+#include "sensors/battery.h"
+#include "sensors/sensors.h"
+
+#include "telemetry/telemetry.h"
+
+#include "io/piniobox.h"
+
+void targetConfiguration(void)
+{
+    pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1;
+    pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2;
+
+
+    /*
+     * UART1 is SerialRX
+     */
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].functionMask = FUNCTION_RX_SERIAL;
+    
+    /*
+     * Enable MSP at 115200 at UART4
+     */
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_MSP;
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].msp_baudrateIndex = BAUD_115200;
+}
diff --git a/src/main/target/JHEMCUH743HD/target.c b/src/main/target/JHEMCUH743HD/target.c
new file mode 100644
index 00000000000..adb753b9d17
--- /dev/null
+++ b/src/main/target/JHEMCUH743HD/target.c
@@ -0,0 +1,49 @@
+/*
+ * 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"
+
+BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000,   DEVHW_MPU6000,  MPU6000_SPI_BUS,   MPU6000_CS_PIN,   NONE,   0,  DEVFLAGS_NONE,  IMU_MPU6000_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_bmi270,   DEVHW_BMI270,  BMI270_SPI_BUS,   BMI270_CS_PIN,   NONE,   0,  DEVFLAGS_NONE,  IMU_BMI270_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_icm42605,    DEVHW_ICM42605, ICM42605_SPI_BUS,   ICM42605_CS_PIN,    NONE,  0,  DEVFLAGS_NONE,  IMU_ICM42605_ALIGN);
+
+
+timerHardware_t timerHardware[] = {
+
+    DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0),   // S1  
+    DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 1),   // S2
+    DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 2),   // S3
+    DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 3),   // S4
+
+    DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 4),   // S5
+    DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 5),   // S6
+    DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 6),   // S7
+    DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 7),   // S8
+
+    DEF_TIM(TIM1,  CH1, PA8,  TIM_USE_LED, 0, 9),    // LED_2812
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
diff --git a/src/main/target/JHEMCUH743HD/target.h b/src/main/target/JHEMCUH743HD/target.h
new file mode 100644
index 00000000000..92300b52564
--- /dev/null
+++ b/src/main/target/JHEMCUH743HD/target.h
@@ -0,0 +1,205 @@
+/*
+ * 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 "JHEH743HD"
+#define USBD_PRODUCT_STRING     "JHEMCUH743HD"
+
+#define USE_TARGET_CONFIG
+
+#define LED0 PE5
+#define LED1 PE4
+
+#define BEEPER PE3
+#define BEEPER_INVERTED
+
+// *************** IMU generic ***********************
+#define USE_DUAL_GYRO
+#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS
+
+// *************** SPI1 IMU0 MPU6000 ****************
+#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_IMU_MPU6000
+#define IMU_MPU6000_ALIGN CW180_DEG
+#define MPU6000_SPI_BUS BUS_SPI1
+#define MPU6000_CS_PIN PA4
+
+#define USE_IMU_BMI270
+#define IMU_BMI270_ALIGN CW180_DEG
+#define BMI270_SPI_BUS BUS_SPI1
+#define BMI270_CS_PIN PA4
+
+#define USE_IMU_ICM42605
+#define IMU_ICM42605_ALIGN      CW180_DEG
+#define ICM42605_SPI_BUS        BUS_SPI1
+#define ICM42605_CS_PIN         PA4
+
+// *************** SPI2 OSD ***********************
+#define USE_SPI_DEVICE_2
+#define SPI2_SCK_PIN PB13
+#define SPI2_MISO_PIN PB14
+#define SPI2_MOSI_PIN PB15
+
+#define USE_MAX7456
+#define MAX7456_SPI_BUS BUS_SPI2
+#define MAX7456_CS_PIN PB12
+
+// *************** SPI3 FLASH ***********************
+#define USE_SPI_DEVICE_3
+#define SPI3_SCK_PIN PC10
+#define SPI3_MISO_PIN PC11
+#define SPI3_MOSI_PIN PB2
+#define SPI3_NSS_PIN PA15
+#define SPI3_SCK_AF GPIO_AF6_SPI3
+#define SPI3_MISO_AF GPIO_AF6_SPI3
+#define SPI3_MOSI_AF GPIO_AF7_SPI3
+
+#define M25P16_SPI_BUS BUS_SPI3
+#define M25P16_CS_PIN SPI3_NSS_PIN
+
+#define W25N01G_SPI_BUS BUS_SPI3
+#define W25N01G_CS_PIN SPI3_NSS_PIN
+
+#define USE_BLACKBOX
+#define USE_FLASHFS
+#define USE_FLASH_M25P16
+#define USE_FLASH_W25N01G
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+
+// *************** I2C /Baro/Mag *********************
+#define USE_I2C
+#define USE_I2C_DEVICE_1
+#define I2C1_SCL PB6
+#define I2C1_SDA PB7
+
+#define USE_I2C_DEVICE_2
+#define I2C2_SCL PB10
+#define I2C2_SDA PB11
+
+#define USE_BARO
+#define BARO_I2C_BUS BUS_I2C1
+#define USE_BARO_ALL
+
+#define USE_MAG
+#define MAG_I2C_BUS BUS_I2C1
+#define USE_MAG_ALL
+
+#define TEMPERATURE_I2C_BUS     BUS_I2C1
+#define PITOT_I2C_BUS           BUS_I2C1
+
+#define USE_RANGEFINDER
+#define RANGEFINDER_I2C_BUS BUS_I2C1
+
+// *************** UART *****************************
+#define USE_VCP
+
+#define USE_UART1
+#define UART1_TX_PIN PA9
+#define UART1_RX_PIN PA10
+
+#define USE_UART2
+#define UART2_TX_PIN PD5
+#define UART2_RX_PIN PD6
+
+#define USE_UART3
+#define UART3_TX_PIN PD8
+#define UART3_RX_PIN PD9
+
+#define USE_UART4
+#define UART4_TX_PIN PD1
+#define UART4_RX_PIN PD0
+
+#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_UART7
+#define UART7_TX_PIN PE8
+#define UART7_RX_PIN PE7
+
+#define USE_UART8
+#define UART8_TX_PIN PE1
+#define UART8_RX_PIN PE0
+
+#define SERIAL_PORT_COUNT 9
+
+#define DEFAULT_RX_TYPE RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER SERIALRX_SBUS
+
+// *************** ADC *****************************
+#define USE_ADC
+#define ADC_INSTANCE ADC3
+
+#ifdef MAMBAH743_2022B
+
+#define ADC_CHANNEL_1_PIN PC1
+#define ADC_CHANNEL_2_PIN PC3
+#define ADC_CHANNEL_3_PIN PC0
+
+#define VBAT_ADC_CHANNEL ADC_CHN_1
+#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2
+#define AIRSPEED_ADC_CHANNEL ADC_CHN_3
+
+#else
+
+#define ADC_CHANNEL_1_PIN PC1
+#define ADC_CHANNEL_2_PIN PC3
+#define ADC_CHANNEL_3_PIN PC2
+#define ADC_CHANNEL_4_PIN PC0
+
+#define VBAT_ADC_CHANNEL ADC_CHN_1
+#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2
+#define RSSI_ADC_CHANNEL ADC_CHN_3
+#define AIRSPEED_ADC_CHANNEL ADC_CHN_4
+
+#endif
+
+// *************** PINIO ***************************
+#define USE_PINIO
+#define USE_PINIOBOX
+
+#define PINIO1_PIN PC2
+#define PINIO2_PIN PC5
+
+// *************** LEDSTRIP ************************
+#define USE_LED_STRIP
+#define WS2811_PIN PA8
+
+#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL)
+#define CURRENT_METER_SCALE 250
+
+#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
+#define TARGET_IO_PORTE 0xffff
+
+#define MAX_PWM_OUTPUT_PORTS 8
+#define USE_DSHOT
+#define USE_ESC_SENSOR
\ No newline at end of file

From 269f1f915b8b36e2c2f07d4a4ffeed58cae65651 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 21 Apr 2024 11:53:21 +0200
Subject: [PATCH 067/323] Fix ZEEZF7 output mapping

---
 src/main/target/ZEEZF7/target.c | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/main/target/ZEEZF7/target.c b/src/main/target/ZEEZF7/target.c
index f1f5356dcb2..79bfc08d2cb 100755
--- a/src/main/target/ZEEZF7/target.c
+++ b/src/main/target/ZEEZF7/target.c
@@ -32,9 +32,9 @@ timerHardware_t timerHardware[] = {
     DEF_TIM(TIM3, CH1,  PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S3
     DEF_TIM(TIM2, CH2,  PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S4
     DEF_TIM(TIM8, CH1,  PC6, TIM_USE_OUTPUT_AUTO, 0, 1), // S5
-    DEF_TIM(TIM3, CH2,  PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // S6
-    DEF_TIM(TIM3, CH3,  PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S7
-    DEF_TIM(TIM3, CH4,  PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S8
+    DEF_TIM(TIM8, CH2,  PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // S6
+    DEF_TIM(TIM8, CH3,  PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S7
+    DEF_TIM(TIM1, CH3N,  PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S8
 #endif
 
 #ifdef ZEEZF7V2
@@ -42,10 +42,10 @@ timerHardware_t timerHardware[] = {
     DEF_TIM(TIM4, CH1,  PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S2
     DEF_TIM(TIM3, CH1,  PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S3
     DEF_TIM(TIM2, CH2,  PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S4
-    DEF_TIM(TIM3, CH3,  PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S5
-    DEF_TIM(TIM3, CH2,  PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // S6
+    DEF_TIM(TIM8, CH3,  PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S5
+    DEF_TIM(TIM8, CH2,  PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // S6
     DEF_TIM(TIM8, CH1,  PC6, TIM_USE_OUTPUT_AUTO, 0, 1), // S7
-    DEF_TIM(TIM3, CH4,  PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S8
+    DEF_TIM(TIM1, CH3N,  PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S8
 #endif
 
 #ifdef ZEEZF7

From 7dabfb0ddfe4d424f19beedc5a7a1968ead4492f Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 21 Apr 2024 11:53:21 +0200
Subject: [PATCH 068/323] Fix ZEEZF7 output mapping

---
 src/main/target/ZEEZF7/target.c | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/main/target/ZEEZF7/target.c b/src/main/target/ZEEZF7/target.c
index f1f5356dcb2..79bfc08d2cb 100755
--- a/src/main/target/ZEEZF7/target.c
+++ b/src/main/target/ZEEZF7/target.c
@@ -32,9 +32,9 @@ timerHardware_t timerHardware[] = {
     DEF_TIM(TIM3, CH1,  PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S3
     DEF_TIM(TIM2, CH2,  PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S4
     DEF_TIM(TIM8, CH1,  PC6, TIM_USE_OUTPUT_AUTO, 0, 1), // S5
-    DEF_TIM(TIM3, CH2,  PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // S6
-    DEF_TIM(TIM3, CH3,  PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S7
-    DEF_TIM(TIM3, CH4,  PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S8
+    DEF_TIM(TIM8, CH2,  PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // S6
+    DEF_TIM(TIM8, CH3,  PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S7
+    DEF_TIM(TIM1, CH3N,  PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S8
 #endif
 
 #ifdef ZEEZF7V2
@@ -42,10 +42,10 @@ timerHardware_t timerHardware[] = {
     DEF_TIM(TIM4, CH1,  PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S2
     DEF_TIM(TIM3, CH1,  PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S3
     DEF_TIM(TIM2, CH2,  PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S4
-    DEF_TIM(TIM3, CH3,  PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S5
-    DEF_TIM(TIM3, CH2,  PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // S6
+    DEF_TIM(TIM8, CH3,  PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S5
+    DEF_TIM(TIM8, CH2,  PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // S6
     DEF_TIM(TIM8, CH1,  PC6, TIM_USE_OUTPUT_AUTO, 0, 1), // S7
-    DEF_TIM(TIM3, CH4,  PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S8
+    DEF_TIM(TIM1, CH3N,  PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S8
 #endif
 
 #ifdef ZEEZF7

From 3e961f725d3c157ef1f17e746f3f25d8aad342ee Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 21 Apr 2024 11:58:22 +0200
Subject: [PATCH 069/323] Fix line endings

---
 src/main/target/TAKERF722SE/target.c |  96 +++----
 src/main/target/TAKERF722SE/target.h | 372 +++++++++++++--------------
 2 files changed, 234 insertions(+), 234 deletions(-)

diff --git a/src/main/target/TAKERF722SE/target.c b/src/main/target/TAKERF722SE/target.c
index 247efb84e52..d67062a9fbc 100644
--- a/src/main/target/TAKERF722SE/target.c
+++ b/src/main/target/TAKERF722SE/target.c
@@ -1,48 +1,48 @@
-/*
-* This file is part of INAV Project.
-*
-* 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/io.h"
-
-#include "drivers/dma.h"
-#include "drivers/timer.h"
-#include "drivers/timer_def.h"
-
-
-
-
-
-timerHardware_t timerHardware[] = {
-
-    DEF_TIM(TIM3, CH1, PB4,  TIM_USE_OUTPUT_AUTO, 0, 0),
-    DEF_TIM(TIM3, CH2, PB5,  TIM_USE_OUTPUT_AUTO, 0, 0),
-    DEF_TIM(TIM3, CH3, PB0,  TIM_USE_OUTPUT_AUTO, 0, 0),
-    DEF_TIM(TIM3, CH4, PB1,  TIM_USE_OUTPUT_AUTO, 0, 0),
-
-    DEF_TIM(TIM4, CH1, PB6,  TIM_USE_OUTPUT_AUTO, 0, 0),
-    DEF_TIM(TIM4, CH2, PB7,  TIM_USE_OUTPUT_AUTO, 0, 0),
-    DEF_TIM(TIM8, CH3, PC8,  TIM_USE_OUTPUT_AUTO, 0, 0),
-    DEF_TIM(TIM8, CH4, PC9,  TIM_USE_OUTPUT_AUTO, 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.
+*
+* 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/io.h"
+
+#include "drivers/dma.h"
+#include "drivers/timer.h"
+#include "drivers/timer_def.h"
+
+
+
+
+
+timerHardware_t timerHardware[] = {
+
+    DEF_TIM(TIM3, CH1, PB4,  TIM_USE_OUTPUT_AUTO, 0, 0),
+    DEF_TIM(TIM3, CH2, PB5,  TIM_USE_OUTPUT_AUTO, 0, 0),
+    DEF_TIM(TIM3, CH3, PB0,  TIM_USE_OUTPUT_AUTO, 0, 0),
+    DEF_TIM(TIM3, CH4, PB1,  TIM_USE_OUTPUT_AUTO, 0, 0),
+
+    DEF_TIM(TIM4, CH1, PB6,  TIM_USE_OUTPUT_AUTO, 0, 0),
+    DEF_TIM(TIM4, CH2, PB7,  TIM_USE_OUTPUT_AUTO, 0, 0),
+    DEF_TIM(TIM8, CH3, PC8,  TIM_USE_OUTPUT_AUTO, 0, 0),
+    DEF_TIM(TIM8, CH4, PC9,  TIM_USE_OUTPUT_AUTO, 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/TAKERF722SE/target.h b/src/main/target/TAKERF722SE/target.h
index 61ab442653d..817dc535f03 100644
--- a/src/main/target/TAKERF722SE/target.h
+++ b/src/main/target/TAKERF722SE/target.h
@@ -1,186 +1,186 @@
-/*
- * 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.
- *
- * 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 "GEPR"
-
-#define USBD_PRODUCT_STRING     "TAKERF722SE"
-
-#define LED0                    PC14
-
-
-// *************** BEEPER ************************
-
-#define BEEPER                  PC13
-#define BEEPER_INVERTED
-
-
-// *************** LEDSTRIP ************************
-#define USE_LED_STRIP
-#define WS2811_PIN                  PA8
-
-
-// *************** UART *****************************
-#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            PB10
-#define UART3_RX_PIN            PB11
-
-#define USE_UART4
-#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 SERIAL_PORT_COUNT       7
-
-#define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
-#define SERIALRX_PROVIDER       SERIALRX_SBUS
-#define SERIALRX_UART           SERIAL_PORT_USART2
-
-
-// *************** SPI Bus **********************
-
-#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   	    PB14
-#define SPI2_MOSI_PIN   	    PB15
-
-#define USE_SPI_DEVICE_3
-#define SPI3_SCK_PIN            PC10
-#define SPI3_MISO_PIN           PC11
-#define SPI3_MOSI_PIN           PB2
-#define SPI3_SCK_AF             GPIO_AF6_SPI3
-#define SPI3_MISO_AF            GPIO_AF6_SPI3
-#define SPI3_MOSI_AF            GPIO_AF7_SPI3
-
-
-// *************** Gyro & ACC **********************
-
-#define USE_IMU_MPU6000
-#define IMU_MPU6000_ALIGN       CW0_DEG
-#define MPU6000_CS_PIN          PA4
-#define MPU6000_SPI_BUS         BUS_SPI1
-
-
-#define USE_IMU_ICM42605
-#define IMU_ICM42605_ALIGN      CW0_DEG
-#define ICM42605_CS_PIN         PA4
-#define ICM42605_SPI_BUS        BUS_SPI1
-
-// *************** 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 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 USE_FLASHFS
-
-#define USE_FLASH_M25P16
-#define M25P16_CS_PIN           PA15
-#define M25P16_SPI_BUS          BUS_SPI3
-
-#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
-
-// *************** OSD *****************************
-
-#define USE_MAX7456
-#define MAX7456_SPI_BUS         BUS_SPI2
-#define MAX7456_CS_PIN          PB12
-
-// *************** ADC *****************************
-
-#define USE_ADC
-#define ADC_INSTANCE                ADC1
-#define ADC1_DMA_STREAM             DMA2_Stream0
-#define ADC_CHANNEL_1_PIN           PC3
-#define ADC_CHANNEL_2_PIN           PC0
-#define ADC_CHANNEL_3_PIN           PC2
-
-#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)
-
-
-
-
-//**************************************************
-
-#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        8
-
-#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.
+ *
+ * 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 "GEPR"
+
+#define USBD_PRODUCT_STRING     "TAKERF722SE"
+
+#define LED0                    PC14
+
+
+// *************** BEEPER ************************
+
+#define BEEPER                  PC13
+#define BEEPER_INVERTED
+
+
+// *************** LEDSTRIP ************************
+#define USE_LED_STRIP
+#define WS2811_PIN                  PA8
+
+
+// *************** UART *****************************
+#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            PB10
+#define UART3_RX_PIN            PB11
+
+#define USE_UART4
+#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 SERIAL_PORT_COUNT       7
+
+#define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER       SERIALRX_SBUS
+#define SERIALRX_UART           SERIAL_PORT_USART2
+
+
+// *************** SPI Bus **********************
+
+#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   	    PB14
+#define SPI2_MOSI_PIN   	    PB15
+
+#define USE_SPI_DEVICE_3
+#define SPI3_SCK_PIN            PC10
+#define SPI3_MISO_PIN           PC11
+#define SPI3_MOSI_PIN           PB2
+#define SPI3_SCK_AF             GPIO_AF6_SPI3
+#define SPI3_MISO_AF            GPIO_AF6_SPI3
+#define SPI3_MOSI_AF            GPIO_AF7_SPI3
+
+
+// *************** Gyro & ACC **********************
+
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW0_DEG
+#define MPU6000_CS_PIN          PA4
+#define MPU6000_SPI_BUS         BUS_SPI1
+
+
+#define USE_IMU_ICM42605
+#define IMU_ICM42605_ALIGN      CW0_DEG
+#define ICM42605_CS_PIN         PA4
+#define ICM42605_SPI_BUS        BUS_SPI1
+
+// *************** 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 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 USE_FLASHFS
+
+#define USE_FLASH_M25P16
+#define M25P16_CS_PIN           PA15
+#define M25P16_SPI_BUS          BUS_SPI3
+
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+
+// *************** OSD *****************************
+
+#define USE_MAX7456
+#define MAX7456_SPI_BUS         BUS_SPI2
+#define MAX7456_CS_PIN          PB12
+
+// *************** ADC *****************************
+
+#define USE_ADC
+#define ADC_INSTANCE                ADC1
+#define ADC1_DMA_STREAM             DMA2_Stream0
+#define ADC_CHANNEL_1_PIN           PC3
+#define ADC_CHANNEL_2_PIN           PC0
+#define ADC_CHANNEL_3_PIN           PC2
+
+#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)
+
+
+
+
+//**************************************************
+
+#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        8
+
+#define USE_DSHOT
+#define USE_ESC_SENSOR

From 34e8d82b6039cd4208fba7e987bed163fc92f902 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 21 Apr 2024 12:02:36 +0200
Subject: [PATCH 070/323] Enable baro-less navigation by default

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

diff --git a/docs/Settings.md b/docs/Settings.md
index 8608eb01f51..9c31498ef19 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -1844,11 +1844,11 @@ Allows to chose when the home position is reset. Can help prevent resetting home
 
 ### inav_use_gps_no_baro
 
-_// TODO_
+Defines if INAV should use only use GPS data for altitude estimation when barometer is not available. If set to ON, INAV will allow GPS assisted modes and RTH even when there is no barometer installed.
 
 | Default | Min | Max |
 | --- | --- | --- |
-| OFF | OFF | ON |
+| ON | OFF | ON |
 
 ---
 
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 66abea46399..d2d8c5c433e 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -2288,9 +2288,10 @@ groups:
         field: use_gps_velned
         type: bool
       - name: inav_use_gps_no_baro
+        description: "Defines if INAV should use only use GPS data for altitude estimation when barometer is not available. If set to ON, INAV will allow GPS assisted modes and RTH even when there is no barometer installed."
         field: use_gps_no_baro
         type: bool
-        default_value: OFF
+        default_value: ON
       - name: inav_allow_dead_reckoning
         description: "Defines if INAV will dead-reckon over short GPS outages. May also be useful for indoors OPFLOW navigation"
         default_value: OFF

From 1d919de74403f53bf0c1a6ef0ff5735f08d84676 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Sun, 21 Apr 2024 20:58:42 +0100
Subject: [PATCH 071/323] Update navigation.c

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

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 284afc74109..e8d481a6cf8 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -1156,7 +1156,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .timeoutMs = 10,
         .stateFlags = NAV_REQUIRE_ANGLE,
         .mapToFlightModes = NAV_FW_AUTOLAND,
-        .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
+        .mwState = MW_NAV_STATE_LANDED,
         .mwError = MW_NAV_ERROR_NONE,
         .onEvent = {
             [NAV_FSM_EVENT_TIMEOUT]                                 = NAV_STATE_FW_LANDING_FINISHED,   // re-process the state

From feba572aea875a2b0bdb2bb876ae1e7256e67caf Mon Sep 17 00:00:00 2001
From: YI-BOYANG <46438966+YI-BOYANG@users.noreply.github.com>
Date: Sat, 27 Apr 2024 17:53:14 +0800
Subject: [PATCH 072/323] Add GEPRCF745_BT_HD target

---
 .../target/GEPRCF745_BT_HD/CMakeLists.txt     |   1 +
 src/main/target/GEPRCF745_BT_HD/config.c      |  63 ++++++
 src/main/target/GEPRCF745_BT_HD/target.c      |  52 +++++
 src/main/target/GEPRCF745_BT_HD/target.h      | 198 ++++++++++++++++++
 4 files changed, 314 insertions(+)
 create mode 100644 src/main/target/GEPRCF745_BT_HD/CMakeLists.txt
 create mode 100644 src/main/target/GEPRCF745_BT_HD/config.c
 create mode 100644 src/main/target/GEPRCF745_BT_HD/target.c
 create mode 100644 src/main/target/GEPRCF745_BT_HD/target.h

diff --git a/src/main/target/GEPRCF745_BT_HD/CMakeLists.txt b/src/main/target/GEPRCF745_BT_HD/CMakeLists.txt
new file mode 100644
index 00000000000..815e08923a8
--- /dev/null
+++ b/src/main/target/GEPRCF745_BT_HD/CMakeLists.txt
@@ -0,0 +1 @@
+target_stm32f745xg(GEPRCF745_BT_HD)
diff --git a/src/main/target/GEPRCF745_BT_HD/config.c b/src/main/target/GEPRCF745_BT_HD/config.c
new file mode 100644
index 00000000000..f228c3e023f
--- /dev/null
+++ b/src/main/target/GEPRCF745_BT_HD/config.c
@@ -0,0 +1,63 @@
+/*
+ * 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 <stdbool.h>
+#include <stdint.h>
+
+#include <platform.h>
+
+#include "common/axis.h"
+
+#include "config/config_master.h"
+#include "config/feature.h"
+
+#include "drivers/sensor.h"
+#include "drivers/pwm_esc_detect.h"
+#include "drivers/pwm_output.h"
+#include "drivers/serial.h"
+
+#include "fc/rc_controls.h"
+
+#include "flight/failsafe.h"
+#include "flight/mixer.h"
+#include "flight/pid.h"
+
+#include "rx/rx.h"
+
+#include "io/serial.h"
+
+#include "sensors/battery.h"
+#include "sensors/sensors.h"
+
+#include "telemetry/telemetry.h"
+
+#include "fc/fc_msp_box.h"
+
+#include "io/piniobox.h"
+
+
+#define BLUETOOTH_MSP_BAUDRATE      BAUD_115200
+
+void targetConfiguration(void)
+{
+    pinioBoxConfigMutable()->permanentId[0] = BOXARM;
+    pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER1;
+
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART3)].functionMask = FUNCTION_MSP;
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART3)].msp_baudrateIndex = BLUETOOTH_MSP_BAUDRATE;
+}
+
diff --git a/src/main/target/GEPRCF745_BT_HD/target.c b/src/main/target/GEPRCF745_BT_HD/target.c
new file mode 100644
index 00000000000..57a8c2a8914
--- /dev/null
+++ b/src/main/target/GEPRCF745_BT_HD/target.c
@@ -0,0 +1,52 @@
+/*
+* This file is part of INAV Project.
+*
+* 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/sensor.h"
+
+
+BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000,  DEVHW_MPU6000,  IMU1_SPI_BUS,   IMU1_CS_PIN,   NONE,   0,  DEVFLAGS_NONE,  IMU1_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000_2,  DEVHW_MPU6000, IMU2_SPI_BUS,   IMU2_CS_PIN,   NONE,   1,  DEVFLAGS_NONE,  IMU2_ALIGN);
+
+BUSDEV_REGISTER_SPI_TAG(busdev_icm42688,  DEVHW_MPU6000,  IMU1_SPI_BUS,   IMU1_CS_PIN,   NONE,   0,  DEVFLAGS_NONE,  IMU1_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_icm42688_2, DEVHW_ICM42605, IMU2_SPI_BUS,   IMU2_CS_PIN,   NONE,   1,  DEVFLAGS_NONE,  IMU2_ALIGN);
+
+
+
+timerHardware_t timerHardware[] = {
+
+    DEF_TIM(TIM3, CH3, PB0,  TIM_USE_OUTPUT_AUTO, 0, 0),
+    DEF_TIM(TIM3, CH4, PB1,  TIM_USE_OUTPUT_AUTO, 0, 0),
+    DEF_TIM(TIM3, CH2, PB5,  TIM_USE_OUTPUT_AUTO, 0, 0),
+    DEF_TIM(TIM3, CH1, PB4,  TIM_USE_OUTPUT_AUTO, 0, 0),
+
+    DEF_TIM(TIM4, CH1, PD12, TIM_USE_OUTPUT_AUTO, 0, 0),
+    DEF_TIM(TIM4, CH2, PD13, TIM_USE_OUTPUT_AUTO, 0, 0),
+    DEF_TIM(TIM8, CH3, PC8,  TIM_USE_OUTPUT_AUTO, 0, 0),
+    DEF_TIM(TIM8, CH4, PC9,  TIM_USE_OUTPUT_AUTO, 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/GEPRCF745_BT_HD/target.h b/src/main/target/GEPRCF745_BT_HD/target.h
new file mode 100644
index 00000000000..340f7bbe390
--- /dev/null
+++ b/src/main/target/GEPRCF745_BT_HD/target.h
@@ -0,0 +1,198 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * 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 "GEPR"
+#define USBD_PRODUCT_STRING     "GEPRCF745_BT_HD"
+
+#define USE_TARGET_CONFIG
+
+#define LED0                    PC13
+
+#define BEEPER                  PD2
+#define BEEPER_INVERTED
+
+
+//**************** SPI BUS *************************
+#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           PB14
+#define SPI2_MOSI_PIN           PB15
+
+#define USE_SPI_DEVICE_3
+#define SPI3_SCK_PIN            PC10
+#define SPI3_MISO_PIN   	    PC11
+#define SPI3_MOSI_PIN   	    PC12
+
+#define USE_SPI_DEVICE_4
+#define SPI4_SCK_PIN            PE2
+#define SPI4_MISO_PIN           PE5
+#define SPI4_MOSI_PIN           PE6
+
+
+
+// *************** Gyro & ACC **********************
+
+#define USE_DUAL_GYRO
+#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS
+#define USE_IMU_MPU6000
+#define USE_IMU_ICM42605
+
+
+// *****IMU1 MPU6000 & ICM42688 ON  SPI1 **************
+#define IMU1_ALIGN              CW180_DEG
+#define IMU1_SPI_BUS            BUS_SPI1
+#define IMU1_CS_PIN             PA4
+
+
+
+// *****IMU2 MPU6000 & ICM42688 ON  SPI2 **************
+#define IMU2_ALIGN              CW0_DEG
+#define IMU2_SPI_BUS            BUS_SPI2
+#define IMU2_CS_PIN             PB12
+
+
+// *************** 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 USE_SDCARD
+#define USE_SDCARD_SPI
+#define SDCARD_SPI_BUS          BUS_SPI3
+#define SDCARD_CS_PIN           PA15
+
+#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
+
+
+// ************PINIO to disable BT*****************
+#define USE_PINIO
+#define USE_PINIOBOX
+#define PINIO1_PIN                  PE13  
+#define PINIO1_FLAGS                PINIO_FLAGS_INVERTED
+
+
+// ************PINIO to other
+#define PINIO2_PIN                  PC14               //Enable vsw
+
+
+// *************** OSD *****************************
+#define USE_MAX7456
+#define MAX7456_SPI_BUS         BUS_SPI4
+#define MAX7456_CS_PIN          PE4
+
+// *************** 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 USE_UART7
+#define UART7_RX_PIN            PE7
+#define UART7_TX_PIN            PE8
+
+#define USE_UART8
+#define UART8_RX_PIN            PE0
+#define UART8_TX_PIN            PE1
+
+#define SERIAL_PORT_COUNT       8
+
+#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           PC3
+#define ADC_CHANNEL_2_PIN           PC5
+#define ADC_CHANNEL_3_PIN           PC2
+
+#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)
+
+#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 0xffff
+#define TARGET_IO_PORTE 0xffff
+
+#define MAX_PWM_OUTPUT_PORTS        8
+
+#define USE_DSHOT
+#define USE_ESC_SENSOR

From a53296abb7d9aa10f850b34aae11cc142db6aab3 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sat, 27 Apr 2024 13:11:40 +0200
Subject: [PATCH 073/323] Fix function prototypes and macos warnings

---
 src/main/flight/mixer.c           | 2 +-
 src/main/flight/mixer_profile.c   | 2 +-
 src/main/io/displayport_msp_osd.c | 2 +-
 3 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index d2a073f26ee..716b48d7aa4 100644
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -728,7 +728,7 @@ bool areMotorsRunning(void)
     return false;
 }
 
-uint16_t getMaxThrottle() {
+uint16_t getMaxThrottle(void) {
 
     static uint16_t throttle = 0;
 
diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c
index 7b2590ff70b..5769b56fbdb 100644
--- a/src/main/flight/mixer_profile.c
+++ b/src/main/flight/mixer_profile.c
@@ -79,7 +79,7 @@ void pgResetFn_mixerProfiles(mixerProfile_t *instance)
     }
 }
 
-void activateMixerConfig(){
+void activateMixerConfig(void){
     currentMixerProfileIndex = getConfigMixerProfile();
     currentMixerConfig = *mixerConfig();
     nextMixerProfileIndex = (currentMixerProfileIndex + 1) % MAX_MIXER_PROFILE_COUNT;
diff --git a/src/main/io/displayport_msp_osd.c b/src/main/io/displayport_msp_osd.c
index e1e6cb29676..0d797e1f756 100644
--- a/src/main/io/displayport_msp_osd.c
+++ b/src/main/io/displayport_msp_osd.c
@@ -525,7 +525,7 @@ void mspOsdSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn)
     }
 }
 
-mspPort_t *getMspOsdPort()
+mspPort_t *getMspOsdPort(void)
 {
     if (mspPort.port) {
         return &mspPort;

From 30d96107bc6a506e4816375a0e08e68936feee67 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sat, 27 Apr 2024 13:19:12 +0200
Subject: [PATCH 074/323] macos-latest got bumped to macos-14 and introduces
 some new failures on sitl

Move it back to macos-13 until it can be updated back to latest.
---
 .github/workflows/ci.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index 09e27045ca6..6fddc02df85 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -74,7 +74,7 @@ jobs:
           path: ./build_SITL/*_SITL
 
   build-SITL-Mac:
-    runs-on: macos-latest
+    runs-on: macos-13
     steps:
       - uses: actions/checkout@v3
       - name: Install dependencies

From cc276acef9070b2a19a64c75dd7e35098374022b Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sat, 27 Apr 2024 13:56:54 +0200
Subject: [PATCH 075/323] More warnings, last of the alignment errors

---
 src/main/build/build_config.h     | 2 +-
 src/main/cms/cms_types.h          | 8 +++++++-
 src/main/config/parameter_group.h | 2 +-
 src/main/fc/cli.c                 | 2 +-
 src/main/io/osd_hud.c             | 4 ++--
 5 files changed, 12 insertions(+), 6 deletions(-)

diff --git a/src/main/build/build_config.h b/src/main/build/build_config.h
index d97e6d300ce..75bb46668d7 100644
--- a/src/main/build/build_config.h
+++ b/src/main/build/build_config.h
@@ -40,7 +40,7 @@
 #endif
 
 #ifdef __APPLE__
-#define FASTRAM                     __attribute__ ((section("__DATA,__.fastram_bss"), aligned(4)))
+#define FASTRAM                     __attribute__ ((section("__DATA,__.fastram_bss"), aligned(8)))
 #else
 #define FASTRAM                     __attribute__ ((section(".fastram_bss"), aligned(4)))
 #endif
diff --git a/src/main/cms/cms_types.h b/src/main/cms/cms_types.h
index 9b2b06646c5..a07e55eaaa4 100644
--- a/src/main/cms/cms_types.h
+++ b/src/main/cms/cms_types.h
@@ -26,6 +26,12 @@
 
 //type of elements
 
+#ifndef __APPLE__
+#define OSD_ENTRY_ATTR __attribute__((packed))
+#else
+#define OSD_ENTRY_ATTR
+#endif
+
 typedef enum
 {
     OME_Label,
@@ -71,7 +77,7 @@ typedef struct
     const void * const data;
     const uint8_t type; // from OSD_MenuElement
     uint8_t flags;
-} __attribute__((packed)) OSD_Entry;
+} OSD_ENTRY_ATTR OSD_Entry;
 
 // Bits in flags
 #define PRINT_VALUE    (1 << 0)  // Value has been changed, need to redraw
diff --git a/src/main/config/parameter_group.h b/src/main/config/parameter_group.h
index 9594ad96c41..28a647ecb96 100644
--- a/src/main/config/parameter_group.h
+++ b/src/main/config/parameter_group.h
@@ -64,7 +64,7 @@ static inline uint16_t pgIsProfile(const pgRegistry_t* reg) {return (reg->size &
 #ifdef __APPLE__
 extern const pgRegistry_t __pg_registry_start[] __asm("section$start$__DATA$__pg_registry");
 extern const pgRegistry_t __pg_registry_end[] __asm("section$end$__DATA$__pg_registry");
-#define PG_REGISTER_ATTRIBUTES __attribute__ ((section("__DATA,__pg_registry"), used, aligned(4)))
+#define PG_REGISTER_ATTRIBUTES __attribute__ ((section("__DATA,__pg_registry"), used, aligned(8)))
 
 extern const uint8_t __pg_resetdata_start[] __asm("section$start$__DATA$__pg_resetdata");
 extern const uint8_t __pg_resetdata_end[] __asm("section$end$__DATA$__pg_resetdata");
diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index 48345583412..97de1bd6ed1 100644
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -1171,7 +1171,7 @@ static void cliRxRange(char *cmdline)
         ptr = cmdline;
         i = fastA2I(ptr);
         if (i >= 0 && i < NON_AUX_CHANNEL_COUNT) {
-            int rangeMin, rangeMax;
+            int rangeMin = 0, rangeMax = 0;
 
             ptr = nextArg(ptr);
             if (ptr) {
diff --git a/src/main/io/osd_hud.c b/src/main/io/osd_hud.c
index 47cc96f834b..07bc181d1c1 100644
--- a/src/main/io/osd_hud.c
+++ b/src/main/io/osd_hud.c
@@ -122,8 +122,8 @@ int8_t radarGetNearestPOI(void)
  */
 void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitude, uint8_t poiType, uint16_t poiSymbol, int16_t poiP1, int16_t poiP2)
 {
-    int poi_x;
-    int poi_y;
+    int poi_x = -1;
+    int poi_y = -1;
     uint8_t center_x;
     uint8_t center_y;
     bool poi_is_oos = 0;

From 8256875af9e297df0dd0895b73c5d0243bb44383 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sat, 27 Apr 2024 14:01:53 +0200
Subject: [PATCH 076/323] Back to latest macos

---
 .github/workflows/ci.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index 6fddc02df85..09e27045ca6 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -74,7 +74,7 @@ jobs:
           path: ./build_SITL/*_SITL
 
   build-SITL-Mac:
-    runs-on: macos-13
+    runs-on: macos-latest
     steps:
       - uses: actions/checkout@v3
       - name: Install dependencies

From a030a2d06787ef81c601e9f940f3d78ab7633743 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sat, 27 Apr 2024 15:42:07 +0200
Subject: [PATCH 077/323] Add separate x64 build

---
 .github/workflows/ci.yml | 36 ++++++++++++++++++++++++++++++++++++
 1 file changed, 36 insertions(+)

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index 09e27045ca6..4ff42bfa935 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -109,6 +109,42 @@ jobs:
           name: ${{ env.BUILD_NAME }}_SITL-MacOS
           path: ./build_SITL/*_SITL
 
+  build-SITL-Mac-large:
+    runs-on: macos-latest-large
+    steps:
+      - uses: actions/checkout@v3
+      - name: Install dependencies
+        run: |
+          brew install cmake ninja ruby
+
+      - name: Setup environment
+        env:
+          ACTIONS_ALLOW_UNSECURE_COMMANDS: true
+        run: |
+          # This is the hash of the commit for the PR
+          # when the action is triggered by PR, empty otherwise
+          COMMIT_ID=${{ github.event.pull_request.head.sha }}
+          # This is the hash of the commit when triggered by push
+          # but the hash of refs/pull/<n>/merge, which is different
+          # from the hash of the latest commit in the PR, that's
+          # why we try github.event.pull_request.head.sha first
+          COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
+          BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
+          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }')
+          echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
+          echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
+      - name: Build SITL
+        run: |
+          mkdir -p build_SITL && cd build_SITL
+          cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja ..
+          ninja
+
+      - name: Upload artifacts
+        uses: actions/upload-artifact@v3
+        with:
+          name: ${{ env.BUILD_NAME }}_SITL-MacOS-x64
+          path: ./build_SITL/*_SITL
+
   build-SITL-Windows:
     runs-on: windows-latest
     defaults:

From f6a86f80f978c24c7e5bcca5c21c63091d3166ef Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sat, 27 Apr 2024 15:47:06 +0200
Subject: [PATCH 078/323] Build MacOSX sitl as universal binary

---
 .github/workflows/ci.yml | 38 +-------------------------------------
 1 file changed, 1 insertion(+), 37 deletions(-)

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index 4ff42bfa935..111a0cc35c0 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -100,7 +100,7 @@ jobs:
       - name: Build SITL
         run: |
           mkdir -p build_SITL && cd build_SITL
-          cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja ..
+          cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -DCMAKE_OSX_ARCHITECTURES="arm64;x86_64" -G Ninja ..
           ninja
 
       - name: Upload artifacts
@@ -109,42 +109,6 @@ jobs:
           name: ${{ env.BUILD_NAME }}_SITL-MacOS
           path: ./build_SITL/*_SITL
 
-  build-SITL-Mac-large:
-    runs-on: macos-latest-large
-    steps:
-      - uses: actions/checkout@v3
-      - name: Install dependencies
-        run: |
-          brew install cmake ninja ruby
-
-      - name: Setup environment
-        env:
-          ACTIONS_ALLOW_UNSECURE_COMMANDS: true
-        run: |
-          # This is the hash of the commit for the PR
-          # when the action is triggered by PR, empty otherwise
-          COMMIT_ID=${{ github.event.pull_request.head.sha }}
-          # This is the hash of the commit when triggered by push
-          # but the hash of refs/pull/<n>/merge, which is different
-          # from the hash of the latest commit in the PR, that's
-          # why we try github.event.pull_request.head.sha first
-          COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
-          BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
-          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }')
-          echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
-          echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
-      - name: Build SITL
-        run: |
-          mkdir -p build_SITL && cd build_SITL
-          cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja ..
-          ninja
-
-      - name: Upload artifacts
-        uses: actions/upload-artifact@v3
-        with:
-          name: ${{ env.BUILD_NAME }}_SITL-MacOS-x64
-          path: ./build_SITL/*_SITL
-
   build-SITL-Windows:
     runs-on: windows-latest
     defaults:

From e92337a5fce818c08a68ed632b46ef32f7740680 Mon Sep 17 00:00:00 2001
From: Roman Lut <11955117+RomanLut@users.noreply.github.com>
Date: Sat, 27 Apr 2024 17:32:00 +0200
Subject: [PATCH 079/323] SITL: implemented built-in serial receivers support
 in SITL, implemented FC proxy mode, updated SITL docs (#9365)

* implemented built-in serial receivers support in SITL
implemented FC Proxy for SITL
update SITL.md

* Update SITL.md

* Update SITL.md

* Update SITL.md

* Update SITL.md

* [SITL] update POSIX serial functions to support non-standard baud rates

* disable xon/hof on serial port (SITL serial_proxy)

---------

Co-authored-by: Jonathan Hudson <jh+github@daria.co.uk>
---
 docs/SITL/SITL.md                          | 142 ++--
 docs/SITL/assets/serial_receiver_crsf.png  | Bin 0 -> 6993 bytes
 docs/SITL/assets/serial_receiver_proxy.png | Bin 0 -> 7107 bytes
 docs/SITL/assets/serial_receiver_sbus.png  | Bin 0 -> 6803 bytes
 docs/development/Building SITL.md          |  39 +
 src/main/drivers/serial_tcp.c              |  49 +-
 src/main/drivers/serial_tcp.h              |   8 +-
 src/main/fc/fc_init.c                      |   8 +
 src/main/fc/fc_tasks.c                     |   8 +
 src/main/main.c                            |   8 +
 src/main/rx/sim.c                          |  19 +-
 src/main/rx/sim.h                          |   1 +
 src/main/target/SITL/serial_proxy.c        | 784 +++++++++++++++++++++
 src/main/target/SITL/serial_proxy.h        |  64 ++
 src/main/target/SITL/target.c              | 109 ++-
 15 files changed, 1143 insertions(+), 96 deletions(-)
 create mode 100644 docs/SITL/assets/serial_receiver_crsf.png
 create mode 100644 docs/SITL/assets/serial_receiver_proxy.png
 create mode 100644 docs/SITL/assets/serial_receiver_sbus.png
 create mode 100644 docs/development/Building SITL.md
 create mode 100644 src/main/target/SITL/serial_proxy.c
 create mode 100644 src/main/target/SITL/serial_proxy.h

diff --git a/docs/SITL/SITL.md b/docs/SITL/SITL.md
index 0c127135781..df3279f6d31 100644
--- a/docs/SITL/SITL.md
+++ b/docs/SITL/SITL.md
@@ -16,6 +16,10 @@ Currently supported are
 
 INAV SITL communicates for sensor data and control directly with the corresponding simulator, see the documentation of the individual simulators and the Configurator or the command line options.
 
+AS SITL is still an inav software, but running on PC, it is possible to use HITL interface for communication.
+
+INAV-X-Plane-HITL plugin https://github.com/RomanLut/INAV-X-Plane-HITL can be used with SITL.
+
 ## Sensors
 The following sensors are emulated:
 - IMU (Gyro, Accelerometer)
@@ -30,13 +34,18 @@ The following sensors are emulated:
 
 Select "FAKE" as type for all mentioned, so that they receive the data from the simulator.
 
-## Serial ports+
-UARTs are replaced by TCP starting with port 5760 ascending. UART 1 port 5760, UART2 5761, ...
-By default, UART1 and UART2 are available as MSP connections. Other UARTs will have TCP listeners if they have an INAV function assigned.
-To connect the Configurator to SITL: Select TCP and connect to ```localhost:5760``` (or ```127.0.0.1:5760``` if your OS doesn't understand `localhost`) (if SITL is running on the same machine).
+## Serial ports
+UARTs are replaced by TCP starting with port 5760 ascending. UART1 is mapped to port 5760, UART2 to 5761, etc.
+
+By default, UART1 and UART2 are configured for MSP connections. Other UARTs will have TCP listeners if they have an INAV function assigned.
+
+To connect the Configurator to SITL, select "SITL".
+
+Alternativelly, select "TCP" and connect to ```localhost:5760``` (or ```127.0.0.1:5760``` if your OS doesn't understand `localhost`) (if SITL is running on the same machine).
+
 IPv4 and IPv6 are supported, either raw addresses or host-name lookup.
 
-The assignment and status of user UART/TCP connections is displayed on the console.
+The assignment and status of used UART/TCP connections is displayed on the console.
 
 ```
 INAV 6.1.0 SITL
@@ -51,39 +60,73 @@ INAV 6.1.0 SITL
 All other interfaces (I2C, SPI, etc.) are not emulated.
 
 ## Remote control
-MSP_RX (TCP/IP) or joystick (via simulator) or serial receiver via USB/Serial interface are supported.
+Multiple methods for connecting RC Controllers are available:
+- MSP_RX (TCP/IP)
+- joystick (via simulator)
+- serial receiver via USB to serial converter
+- any receiver with proxy flight controller
+
 
 ### MSP_RX
 
-MSP_RX is the default, 18 channels are supported over TCP/IP serial emulation.
+MSP_RX is the default, 18 channels are supported over TCP/IP connection.
 
 ### Joystick interface
 Only 8 channels are supported.
-Select "SIM (SITL)" as the receiver and set up a joystick in the simulator, details of which can be found in the documentation for the individual simulators.
+
+Select "SIM (SITL)" as the receiver and set up a joystick in the simulator.
+
+*Not available with INAV-X-Plane-HITL plugin.*
 
 ### Serial Receiver via USB
 
-Connect a serial receiver (e.g. SBUS) to the PC via a UART/USB adapter. Configure the receiver in the Configurator as usual.
+- Connect a serial receiver to the PC via a USB-to-serial adapter
+- Configure the receiver in the SITL as usual
+- While starting SITL from configurator, enable "Serial receiver" option
+
+The SITL offers a built-in option for forwarding the host's serial port to the SITL UART.
+
+Please note that 100000(SBUS) and 420000(CRSF) are non-standart baud rates which may not be supported by some USB-to-serial adapters. FDTI and CH340 should work. CP2102/9 does not work.
 
-The Configurator offers a built-in option for forwarding the serial data to the SITL TCP port, if SITL is started manually the following option can be used:
 
-The connection can then be established with a programme that forwards the serial data unaltered to TCP, e.g. with the Python script tcp_serial_redirect.py (https://github.com/Scavanger/TCP-Serial-Redirect)
-If necessary, please download the required runtime environment from https://www.python.org/.
-Please use the linked version, which has a smaller buffer, otherwise the control response is relatively slow.
+#### Example SBUS:
+For this you need a USB-to-serial adapter, receiver with inverter, or receiver which can output inverted SBUS (normal UART). 
 
-### Example SBUS:
-For this you need a FT232 module. With FT-Prog (https://ftdichip.com/utilities/) the signals can be inverted: Devices->Scan and Parse, then Hardware Specific -> Invert RS232 Signals -> Invert RXD.
+SBUS protocol is inverted UART.
+
+Receiver's SBUS output should be connected to the USB-to-serial adapter's RX pin (via inverter).
+
+With FT-Prog (https://ftdichip.com/utilities/) the signal can be inverted by adapter: Devices->Scan and Parse, then Hardware Specific -> Invert RS232 Signals -> Invert RXD.
 
 ![SITL-SBUS-FT232](assets/SITL-SBUS-FT232.png)
 
-For SBUS, the command line arguments of the python script are:
-```python tcp_serial_redirect.py --parity E --stopbits 2 -c 127.0.0.1:[INAV-UART-PORT] COMXX 100000```
+![SITL-SBUS](assets/serial_receiver_sbus.png)
 
 ### Telemetry
+In the SITL configuration, enable serial receiver on some port and configure receiver type "Serial", "SBUS".
+
+#### Example CRSF:
+
+On receiver side, CRSF is normal UART.
+
+Connect receiver's RX/TX pins (and GND, 5V of course) to USB-To-Serial adapter's TX/RX pins (RX to TX, TX to RX).
+
+![SITL-SBUS](assets/serial_receiver_crsf.png)
+
+In the SITL configuration, enable serial receiver on some port and configure receiver type "Serial", "CRSF".
+
+### Proxy Flight controller
+
+The last, but probably the most easiest way to connect receiver to the SITL, is to use any inav/betaflight Flight controler as proxy.
+
+Connect receiver of any type to FC and configure FC to the point where channels are correctly updated in the "Receiver" tab. Inav and Betaflight are supported.
 
-LTM and MAVLink telemetry are supported, either as a discrete function or shared with MSP.
+You also can use your plane/quad ( if receiver is powered from USB).
+
+![SITL-SBUS](assets/serial_receiver_proxy.png)
+
+In the SITL configuration, select "Receiver type: SIM" regardles of the kind of receiver used.
 
-RX Telemetry via a return channel through the receiver is not yet supported by SITL.
 
 ## OSD
 For the OSD the program INAV-Sim-OSD is available: https://github.com/Scavanger/INAV-SIM-OSD.
@@ -91,6 +134,8 @@ For this, activate MSP-Displayport on a UART/TCP port and connect to the corresp
 
 Note: INAV-Sim-OSD only works if the simulator is in window mode.
 
+*With INAV-X-Plane-HITL plugin, OSD is supported natively.*
+
 ## Command line
 
 The command line options are only necessary if the SITL executable is started by hand.
@@ -103,7 +148,7 @@ If SITL is started without command line options, only a serial MSP / CLI connect
 
 ```--path``` Path and file name to config file. If not present, eeprom.bin in the current directory is used. Example: ```C:\INAV_SITL\flying-wing.bin```, ```/home/user/sitl-eeproms/test-eeprom.bin```.
 
-```--sim=[sim]``` Select the simulator. xp = X-Plane, rf = RealFlight. Example: ```--sim=xp```
+```--sim=[sim]``` Select the simulator. xp = X-Plane, rf = RealFlight. Example: ```--sim=xp```. If not specified, configurator-only mode is started. Omit for usage with INAV-X-Plane-HITL plugin.
 
 ```--simip=[ip]``` Hostname or IP address of the simulator, if you specify a simulator with "--sim" and omit this option IPv4 localhost (`127.0.0.1`) will be used. Example: ```--simip=172.65.21.15```, ```--simip acme-sims.org```, ```--sim ::1```.
 
@@ -118,6 +163,18 @@ To assign motor1 to virtual receiver channel 1, servo 1 to channel 2, and servo2
 ```--chanmap:M01-01,S01-02,S02-03```
 Please also read the documentation of the individual simulators.
 
+```--serialport``` Use serial receiver or proxy FC connected to host's serial port, f.e. ```--serialportCOM5``` or ```--serialportdev/ttyACM3```
+
+```--serialuart``` Map serial receiver to SITL UART, f.e. ```--serialuart=3``` for UART3. Omit if using ```--fcproxy```.
+
+```--baudrate``` Serial receiver baudrate (default: 115200)
+
+```--stopbits=[None|One|Two]``` Serial receiver stopbits (default: One)
+    
+```--parity=[Even|None|Odd]``` Serial receiver parity (default: None)
+
+```--fcproxy``` Use inav/betaflight FC as a proxy for serial receiver.
+
 ```--help``` Displays help for the command line options.
 
 For options that take an argument, either form `--flag=value` or `--flag value` may be used.
@@ -125,46 +182,13 @@ For options that take an argument, either form `--flag=value` or `--flag value`
 ## Running SITL
 It is recommended to start the tools in the following order:
 1. Simulator, aircraft should be ready for take-off
-2. INAV-SITL
+2. SITL
 3. OSD
-4. serial redirect for RC input
-
-## Compile
-
-### Linux and FreeBSD:
-Almost like normal, ruby, cmake and make are also required.
-With cmake, the option "-DSITL=ON" must be specified.
-
-```
-mkdir build_SITL
-cd build_SITL
-cmake -DSITL=ON ..
-make
-```
-
-### Windows:
-Compile under cygwin, then as in Linux.
-Copy cygwin1.dll into the directory, or include cygwin's /bin/ directory in the environment variable PATH.
-
-If the build fails (segfault, possibly out of memory), adding `-DCMAKE_BUILD_TYPE=MinRelSize` to the `cmake` command may help.
-
-#### Build manager
-
-`ninja` may also be used (parallel builds without `-j $(nproc)`):
-
-```
-cmake -GNinja -DSITL=ON ..
-ninja
-```
-
-### Compiler requirements
 
-* Modern GCC. Must be a *real* GCC, macOS faking it with clang will not work. GCC 10 to GCC 13 are known to work.
-* Unix sockets networking. Cygwin is required on Windows (vice `winsock`).
-* Pthreads
+For INav-X-Plane-HITL plugin:
+1. SITL (Run in configurator-only mode)
+2. X-Plane
 
-## Supported environments
+# #Forwarding serial data for other UART
 
-* Linux on x86_64, ia-32, Aarch64 (e.g. Rpi4), RISCV64 (e.g. VisionFive2)
-* Windows on x86_64
-* FreeBSD (x86_64 at least).
+Other UARTs can then be mapped to host's serial port using external tool, which can be found in directories ```inav-configurator\resources\sitl\linux\Ser2TCP```, ```inav-configurator\resources\sitl\windows\Ser2TCP.exe```
diff --git a/docs/SITL/assets/serial_receiver_crsf.png b/docs/SITL/assets/serial_receiver_crsf.png
new file mode 100644
index 0000000000000000000000000000000000000000..55ae6af98b62a85984c02020ab6280473c3c3848
GIT binary patch
literal 6993
zcmZvBc{G&&`#vdz3>8_@V<}3MExQ@J%9fB2vW4tRvJ69F5T=rSH})(cS+Z|q%f9c+
z*rORUm>J9b^!~m-pWpBN&iAk9Joh>G{oLn%uIt>_>v}~z(b2d>$3aIyL2>EPL)E7g
z6z2d63d*~*RA-S#LChUz4|XrL=U)1*ue{z`de~AZ*|=KSazApmw6lF`YiZ-_-fb&?
zmJ#$s%RsFsf;UnuvIKoZVVJW{A{`tY<QP({l{%70XK#HyMAF8_hKZhG;ml!kb8{2i
zz=wjuv2=}iFh^P`8X6ipoXVT|7Sz|*7tu{Jdtaj)+oK~TR#a3(>W&!6ae`GZ&F&Ki
z;-1<nKXA$$nwy)GDwq+E?vYS;B&=48ii*c4B#4Q=DS7jRg2KnqvACwD#;|C{(6iKp
z!t`}QZ=|Mo%Fw}`;L@*-&Q0$n)0`B(_RqtOl*d=luOGl8+PAy52y%|O{l9iUwobLi
zI){mh&ur}{c5lS@9v%**;#X!5dgF=vn4a<6$;ruSq%m=$4ZSq^dwe)VKUu{j7-^zg
zUG+Zwnd?k@GNPuo<DlpFJ~4mr=X&#pmgP<Ka@X4Ce&s@s&hsoHuA_&vie9N-BW~<9
zhcrzrpAC6E?<tXpqj=Z(G;bY8B(4r-6AlU3KRav0VH^>cJG#0!z2ab<nOcz<7zkVH
zYg;FGqI#zm4t}4UoS@NYGMP*y64^rsOV9q7-6K^c1E1;NX%VIz-F2OpK7G5*Tf@`E
z>-X{5<p&%{Q9WJOH(uGbL)}U4pOc1?tY5tI5s2-sJ&fEhTObt`NhuD7hwn8abg3E)
zq|6{qq@qo~pPN_T^*_6!LwO#|jiC$%(LxpXJK>(+Xx=$-cyU0b<#58jv=s%n7B)wT
zZ0(ytkq-)RLlGO4EE*qgq#<skG88;tq&f!-`aX?L|1mW5BV7qY88^7CmpidYCHPbz
zYiL}3q@NC2@5?+C=9iIfI1HpvNvBnpy7S2TpxJIJQ}^PAAb3%7hQPrN;xqMCSbcFj
z+}3s@44uX6g2#tT59_BRHbe7QBa8ZIyjD~#k(GAewJ}4_cmkv36D(go<6OS2rP~mm
z6T|CHDt3mB5*P1_o1{)>ha!l_O@xa1{H-e%GLm~T8S7U9`YW6492gDqKNmWrI9aIo
z;;<+o%VrDy^wYaC2^r)3>3jTq>112{=KV^2e&D^H(*`c3uXO~8ZXb~%I|fDq*tq#V
zEp|^{72PKn-wdEK+mYu0UZv>upG3%UN|HgKA6g|{`Q7{h8&r!~WeIwE(q+edFG~0O
z-uLGA)DX$S-I;3*<fnxJOC;M@8!B3S+TC97muaeal0y-c$%C@(D0n?F;@R30JSgB>
zfAE$(go`Uc*snHjPyccmW-@$0%gFa3VP%z|Z+grGeLuFv8(5%w6>^%$HOVlurub&Q
zY1LiD{N~+-rht)&?iF~~9cmnzXx|Ph+D7K2A?h>MSzP`01j!=cj>3fN#(jTtgnR>>
z@Yg3$^ESN6lBJ}ZDj(F3yHLE-KE*+x$A&&AwPMb3NQAxn)vTRj&rCno(lV9R6t-h9
zjhVFT1%QbV)J*e#p;H|RqE=En_y33gB7})R@xO;_!abCwVbMLP<Jn1wpo5dkjrS7#
zfT#5;mhI?x3ymIIFP9OZbx;H^v%EviMnEy6-BuJq5;uUj>3A93kj)2Aoz!;f73;=x
zH>?Ht0Rd}(Ns%K8@px&aa(^l;gISRy8u3^m!RQ&-Jer5fK?E$g_(`^W&;2v$3N#2&
zNhF{|35rKnt9+KO7R@w!`YEg>Q27WxvLbn$71H|SbO&XK8F~&ipyMvA=|<rQqNM8R
zPL15{?LuIGdwYP!gbB=nKs;_5$ZZ|iJggO13TCy*kM7b4T{zS)9334s@L9C(6|5Ku
zLnBie8BgV19J<$W!YG|tzVqP7I8WI$6bgDywpqhL*$u|?cbxwn=@=yt@+5>FOQn2t
z?jL3@Qq?Iz>s`L4nIjopIDj&X$`O+YZ|XTA@bqgq4NcCaIJXgkbV=@6jT$z9X6})*
z{gAA{Nv3gw=Y^ht6!G}d4rFp(^gzPQs6v|&JGNq9OLYb_bzrRU=8GV%uWa<H?9ZD6
zrhjVtyPVP1zC0~47G~i=6?BcBs`^e<{&IRT2o$5zaxa1P=pHuOZscUrG2>JE?G6;Z
zMn{?ISNu_b<v`^?m&f93;^e~2%#n?M_5HkEe}A6xr00{^K~r0>k8Nb^39lGFoQ#BK
zq-Ntt;6i8V(W#A%fQ;+0y~cjeIQPm+jZ36j64dpLy+yc|dF*fZCz2n9!QYq83$b*I
zGSBGwY-yhtM@;trT;Ol`-c;P8K~BuQH+LE?dGERLn%rT}CE~l?aLG@(R7k{YBc;aO
z=Da*%iLwFv>u%*2EmY_WtmzvE!%q3ZjCpsSV3F&g;dD1Jj(9bKH@>xL{iAO2@DA-O
z31O{=Sl+O;V!L)vlp}->WW_AYU`QK)1$dKL>y*LCyZ*WgJMD@ibFUaLWc&s>OL_86
zUjVev#m5{Rjji!15K}u})V7*#$C?jl>fQSFyXmq}{P2MKc6+?ZjoDP|uTFNuZ<#n(
zG!^Pw-|qS#S0Waz0msY2YUEVKnERJ!cJAGLW^Fl+y)^{9bfj$U4hJGHcc32o^d{xY
zqB|<Bt=rIw-T}kwzU=dK{g~|4(s4qaanXafuVNbh{OZxRZz7drOOCE3@26q*=T2)D
zLsm`#<&-RLDr{ov)9e<U3s>AOeS2Z<v}E04QDXQ0z($U*TiP^Zzt)y#@59ehi9j!T
zFO0*Ni9iFUMF&t?srFFc+?~_$k-)=wfymny_xn*3*2y<u3U?u90}LIjcV5%^?n+11
z^jVBf`K;N#_MMOxjttw+J+G2T1R_R8R2^w##6}z2S(6=pQkirzTa}m98#Kx^7!fsk
z`cZqGG{nFJ-sDCemTZ7+0Y1wD()q%I$=-NQ{5q+M$%=9IixBnw_WeqE<MGDhh)W&t
zs-ShQ6VHN`Nv?!n#d6}`IuiGzeA<;#>~3n)tkM-IqpvlYfDe4@p9G_yz@{Kh94oGY
zH^HZy{NU4(>kT4+#i0aaSNV^_{k~JpLExYs&5{?|9<i7o{7w!u?<cYPhC89RvrFwy
z$MEWWG5J%0(~T*$u<E{T_?|usyxlnO>09%#cA14smEy}Zns$<`(URs1PnxH1g8EpV
z{QSJHra#T%dL11+Jk)J{bZ8yoDfp0VQsj232W3756W-CtoJhyKVg*Mcw_SF$pAXPx
zTuoyhs_#*5|J(LemM<>Q;ovDq)V%Y7{BfazO@EU#kjk+0T&sU~!CRj2=Mfgj&gi=B
zI)ao0qdoQ^(s}EzTe>sm{pQ#+rl>YxHaQaC)9iJ8*rYdr)6bdOw<!{d8qMjNrMkmT
z!5<qfDFF|oSI3J%VgXmW;nI@K0b1Z<EZ@s$0NPmudFe5|ua#S0`UT~oPJ4TXJ!@>d
zkKk?w^||4Pd`n*UfYK^@$v?(m=kBWSKw&-~xLh}(zn}U$AGzxF-XXi?`8qa{zDo0*
zwAH`zE~Ni{ODA^SxwR9Xt}cWS@Ojqk-Pn_!Jr8pTNY54Ss5@4)Y51@lecaXwZ@e2Q
z@m1#r$#Xf#q{1UME_5w5&`ICa@AQ*#k1-C^4ex=>!eopl&RMaja}`SLtS#ObsB(`N
zR~Am(iMmOZYHRUi0CFC%jQ{%PXu0v)YZAKlCU8DHM&lyb$Nb)v)9@c6Ww;i7V)Vj|
z=fsb_YGm7VF08|g)U8rlvW7zVtxZAdG=40?X;sBrMZSLdPMLfnqv=ugDCMB!+G-uw
zBSC|Giyv3i$sL{WX~V%X2hVz8<A*uE7q9&CxDq%@`Nqj*UXy6NrgxXwl&rRwRd-|5
zc1bSF(8glz6D^o|>xywdEv2m-pM}%OwH8!=@ei*t;vMZ@$6gtHt@@*yxE9JGY|ZX=
zzMsBV+nD<WBV7Mb)0+jk@B6MGUs${ycf#5Vo=~GxTGq6n6ND%-1LUq67z86^i6<R6
zkG7U(n!t>ZzneHyc9H6kM#+}Yd=mOZ2J8kCGl&P#{&yp1$}Ur#-p0U{j)Mjp75`^1
z2e~i+gp&(`Z8U*Rp*`5(BS>Zibn#TQAcj~_2VvMQ6VchowBRIK7Pl74_LhdK8|?GT
zzYV;`=RWr2#r6q$xd2^|_f}%E^H$$$wEc9ioXFX79y@>+7RTIQXFKmn4WQRQ9^G|(
z%Cpd|S91aSJ235I2B^*<vzTM-tvK=H%H7rPDw9Dz(}xdv#3*>?-q!rt+2MMYhTw%I
zj#iV}@boY?y($R<J%%?td<j8d#&R-|;r@f7C<;a^W^GP)CR&?Rrb7f_RHwkl@=9wl
zzbI9E5J#eMGu*Cq7TJOdCgZGSZtJsY&E5bXpt`TAGk%!9wvvV3=*?4E<<{qfu^Gw0
z5)GF?SA>>8eCoY)Gc0~htn)oG(e8bmIHCBSr^NaSpGCTUc`I3<<U?OYY-!G7o6BWB
zj{BCwdd)P^^b?2nm~vdEjHCd#%sbX@$a$#^hmlfN+2hIo44RQ9f8B#)%|?M&QbO`v
zUNE*64L4jZ1HnVFaZDqW+Da4ro>QfHk*}jz?vtd-1%6S%Y@xnuILqHd=#Zu?Z}=W!
z{*|{sA$;DMpYHJ<4`e(R(wA}JRHPz6CU8i0sF<7~{uL3~t)X<B>AoMICc!RgkoPD`
zb{1(D!wZg5;DDYFRm-;LGrJ0Qxddoiw}zJ11n^uDYopEvA6v@DLK?Iuk1g>NhD~lM
zep0T{fF#&QD$Ii}xzO8oE+Pu>j%5>nE4-$%fRvYR)HZU}xz1ZV=|pk?0qJmwkC2?b
zk)s%W|D!$UAcI~&9Suy0owY{F*@=93t*fhJ3u(2NrKL8}<)gFnc-Llq&dT!C64Ro|
z+UOAqP4w5&4a^IAs11-Yx0EjYxi^bH{ct@&7yA3deB}2xAHzV;z<qBYVBp>9tDs6~
z9we#<a#q;B?yT_gSz#WIpy1z(e@2drpK?{j-kW!bm^!*I3mAy)S89RHBY*VIBHi*+
zCqI{m!zBf!YuM{ppb}h>gFGUbuABR@X_1_}AnS(DE&lR?uu#FhB?m|jJ-}ldwB4BR
zl$(9XHGW!h<B3ZVBueeZR@n^&+`C9mGYHG3de=tt?!tsK{hgN&!5gH>juAxN$%$=}
zloKadF?eD9pL5iP>=^@<p0J-WkjEefG@HQ?w@UTyzgoE<h7bgyJGNwkaDTu5uZYg%
zJYyg%xCaxw;F{%e#z0Vk5KseSD6$4O&H-yeX44OBZWSsB3hCF?54t7A&`2;wcB)N*
zj+~|%R3Co{q<j#1rh8p+aN~RB=ujm7*z>LekNIqpMsAEvm4Cg=bc!J|z~Q`^->{^7
z>&c{ljI6MspG<?>C=IZN`!w(|KcKAI&C6DQ126Ho&8uPkx}Ps2EV1y6g)~3%e;sAm
zs0W5WEaT_sl@_R0NF0}t$)Hw4omiPwP!1}42$!A$cAMP?tZnSoknCuIoFy%z&o3xc
zZ5|rGU9%1A?G1)}=X@m#I0<t{enyIfq(}@tb+*IhI#fiBeaRIUJ$}{ZxSx^KB}m1Y
zckH6djxVWKYkW|(9+F;WK7V3McJbsnx2uiX1V?{Ec1!KK=RvgkFz`p6Q>5Yn=bCx~
zzDwEqW=K<!b0|C2F9C2E6fd-opCKB;UX-Yjq4Z^`HcZwW{l)LAQ;EO(bN+BkzRK*O
zr!#rmsg;m`|NUbEjfCl3%~f|F5eoSEz}#hA_TDVpcFFZ{!o4Z?EOL~@x9&SloPuq4
zn&RYT@?5ol5kJP6dVB2zVh4L-cGehIHn;7t?xHX@jdF+<De0R-MP2;{)Q0*8gXuL+
z;7(gLU-D<ii{Nu%b04PdeOIW&8SI3hliqoI2y^pDPubK51Zlsu%xte~?8&)5?qYJw
z{eImBd+QfZ!d{thOcFvDqJ=`9-QO!IQ~-`h&5sTn%KYJ@leBl4u)F~%Lh;c8#LG)Q
zb&7to_YjiA=>ZPQg&4>BESVY(0qTj-qHU{c16s3lC1N%>XJ_bn)k{4Do{3>j*U;$c
z@LX}u4|PjXp`e+5Ht`qc*$sjLnLEBtS$~_6^{Fx|zaL#5zpMfcRwlDMh+zJ(kFe_%
zKSi7B`-2%4ZXYU`FUiLYKKy+fHiM5iM-#o3u%oGQlo%!c{emt;tF18RKt;6GKck)p
z%)XRIcPcWVs_%N_Dj{xU^mwZ7%VJ0^Z_k^SxQb5Dh+Ek5M2-r3|CdOBA3*Q5lJkG6
z9;zLl{2faY9ag0z7ljbHdyt_A?P8dW0?yzu(Ss5oMu;=b(RY_mGm4Wpd^Y56+1qDA
zjJqWw+{ssU3Ied-;i{BSvns;}$CjVvM^f-pCZ0x#s|4an@1tN7m+gL`)plqs$oSf8
z;ZLx>GQkkpD&_^bG8$Xy%8J3%yL}KXaMx<6&`T241!y7>&X@0Mgx1tggoT^2k_I-+
zjfVfIf8lA2lRX2)uQxyLXWZnw%9rh-MQ+3}4-?(Oso}4%c$EdWg+&#D@<U`&FR5(<
z3N7EaXrG!-8jCMDiFpE3ovJHVRj-#=xD3g=<4l0mvQB*8YDx?f=X&IkY^l-e7WQW>
zob;m<j&5rp^2y}yYV}{kWcl&bCsp7r=%5Z!Rab#g*p$vy!oY2CBL3$m+retVy^`9z
z<1e};*p*QcZa#}Uu^?uMjvC2b5T{QM_#GFdL!!pg2+FeDh<rm$RhJ@9^0I7W;Nuh1
z9vS?k(x1ZkxRj;StXqM!-Uolkjhlm}83j{~8$|fnMV!Z}$U+}Y64%T<t6+qt1#kuW
zoOJ)t)>BzA5!%Iy;wuizY4Ug&#ywOsd2~O4e6pHC<gONTw*C|hI<pP_wU}sjLGOAn
zdS!>#%zsr{WPaESK8nq%fWD~gKf(hiIt&FZgxgqr1{D?$=yvCm6)$yaOy!d?punRM
zSSIxfa&`n((V<HY=J<#h)+JxmImM5g67WC)mN4k*mU-M}ytD~<LbYe<Pa4oWOH_>R
z_<wAQMJnh&+Go%ZM+tKvIPwK5+FSg`{*np<%{DQ{Ev>v+FRy$iyL+BnDjM-)dZ~r7
zg;2|hf>*|$0~S}TptQ)X`KOG)!tN2}951-ba2)b131Q0+R0{^nr)c)ajgMb~k4()?
z*X8IEi)+9Q%HS?zvBU^_Dr0On%gfzs(}N43<c>wnTAPI8$}wrJf^jIm`kwE+OlqmK
zE2-qItvpXYloiI8Z`6${f<;^)Cz)k<Z9r>SFMmCu|7C^AeZgCz-65@IU{gYTe$g&|
zBhGX2@^h~bn>|0I5{*BPeC>mLzvvL8(2zrrPBg#Vmizue=?mWM$m{Tk$YN0-y}C)0
zqOJ{JU0HiGU;h1had(-99>S*&p_SX>%yTbiSeUpm$0|A89v87$DOM^~t4P=*YFeFh
z>wN1xV0)9h;P-|2*?52TJy9;E&izPG^$T8t;*5td)al2Mr?NdEVV+e#$9L#gfpP;(
z8bVk=MG4D5pAOS{@ZmTFmlaRD?Igo^g~b?m{Dop8=Ihn>G*L0rtNEj^q6E6^{JB|q
zXEx_u(j5p@Ss8#+#DSVH&$iP8#w&Hy;NeR7a3_D?foBIE4t3dw9tmzIS0_*mt3vuO
zENK1;*)um%3LCs|m?d_$L%9pC12s)dg1t27|Fuy5)xuu%|LX?UXsq<^`Nv%$%F7kT
z+1m~NT1HlS`?x8}>RmhoLaYhhnQQa`>pqw^8pf)5y_oHFce{OHfjjYe9!kGaaOpG~
z*AkbmIA<mWgtWD_At##Q(A#_d9ND^AIZ`dMH`;ks-1||WN6x&xD;h>mN6xiG<%L>G
zzcGX2@mv|HQNuZ%&>VpbIm5dZKR-T?4mH*g>mRoZNKc?LZAp?bQD&&6e$LpCvAvAU
zEeqB43{V);$QIBTc`;y2-1K{OyHV{btv)<WZXPebM7dnM!Zf6YEtfl`WASl5`@rgG
z?@NmXjELcy{hR5ObW1_m34tD=IoL%~TbLZvfNXnaz;c`8Y6#aaA*d>g`4s?pIkW}5
z&Ry2kvc#&KE4cJNR_WFyO$yCJR~hE3kUSOMKwAC6s#ZeF*gnI7$iy#{C>c4<KOyxH
z@EoI5MIp}v3k_CZygnX%_yjvYNxC@z;m03a+h)len!U^Y6NGSdwJ<-Fi2w<>+J?c2
zM@1CbD|+t`hxMMVa>D4->_dyjuG3!A26C&<!mZ|skSbd)bpAL&@75|QO4=)ZJ0GK6
z+Iw0OORDa_!U9oMWiL@bQVv4vG^2!T!FkK@jV6O!-&)L{`-{jM#O6Pq|0pd3!!uFF
zWN=1v3);99szU)4TVl|E*uO|s#0OqPoP)8ogJ^f?|Ft$ZI?qIzMdlfx4o&|8icEC|
zs9hjrD-q-u=p77UxNg}D??!Rm*VHA^t+g`eH4--1U1SP}eP&!2LrN|%T0L^+X?fvJ
z4%d}!WzdSm9vK^$nqfr!(xCW(@(EKHSWvCWwIr|=O#9L%d@BgiXenw=rjr$CIkpT(
zAnh3aG+!~^d4(BzkUrNEe&V-;EE%}70>_84-%4SdU%QS=(2O1}MT1m`n_-Ar3k?6Q
zTl)GP@S}1*4)xrzi-3-(cg9eSlb>AA5?dUrsD~5Tkl1!RMcGJNsim!l2DaZelx0rn
za%Z59vtuHwhKXA7U)pZ^68+=?jI9GTO)--sO61mH`+?gRPEPP!3c<Qz>ku;Chas@f
zP!WzXm@!bXcPbKz*f(}wI=UDF@_4^AbtgH26afhIHx3PBk)5X^VT@wnQR9<#L~M(t
z<|)q#&C+uHckQ{3V@rmH1)a?PErGz)Z(vQbtpP~Tu*$HJ0!UBbWHc~NI~)D~XzGHV
ze+bR+51~;sySiaLdz4E59%BD1t<P>S7OCC|LD6nvnaZmSU}{r0$xP)chApVHzX_8P
zw?E!~(|rq?Dof+f-Ib>l3`bMF>;I1g|2jyf7op%GUjhyHiRqqQ-%vbK(@`x|e);bI
E0Q<EpbpQYW

literal 0
HcmV?d00001

diff --git a/docs/SITL/assets/serial_receiver_proxy.png b/docs/SITL/assets/serial_receiver_proxy.png
new file mode 100644
index 0000000000000000000000000000000000000000..6f22f5dbac66e8473f7bc5d4f73dd38417e99af3
GIT binary patch
literal 7107
zcmZvBcUTkO)-DJVK_x^`nlLCuih_cIv;?F{jZy?bL{tQ%3DON9QbG@?GywtWMUY-K
z^j@S%7clgi5K2Nqa`W?^bI(1`_x&}`-m}-tp1t0+*1O&bzh|IzmW79fj*jlE_FZ*D
zI=T}89UZ;IDF)h%_FG6h?ZWM@Vdie+^1|KA+RcVe^|{Ma8v$)6Yg-#b8|&vjuH80D
zG>f<Q^o%un!UYcw4kB(u(X2|Bj_6FXSB{Rlk4S|D1*@y8%OwukCUi%{)q-ilqx*(L
zVjsCHY-ni6p=22yYD7Gk?d$71oXnjrecRO3bnip&5vEY<w)E`m>`1nwWpG`n;AQ{n
z`I&uUM9<McJZ+~ziHV6e(3_6ALvs1kGSNLUP-ntY4S_(Yt*tHgxaa6lY*ILF^12iq
zeCx&ynU{&Z(FT4gLkD7kB}EQSCUnYajtWRq)q=j6Uzo-4){U-pf}&&2K>v1Z%Vf(3
z$CtOlr`PwBdsYsIQui=3@06Yr_wl`>ISGA)sjP>@)z-y@iOpZb6B85ry>V3)!Hbys
zOmp~W6X)r+PpI0uFr$>sePZ6vhGld_^Pjc$z1};fev5xPm)G_&#2zBPUDG0anYg;!
z6!c^K50Qwcd)Kj&YltTj4RcqPettPTJj88pEE9+EM10=Z;`|iG{#iz9d4|7#Kp$~=
z?qIXCcXFYxwRW!S`1p8nagj=;5{X2oX&jE`uSsonRb%g|&9rC>bM1tV@7L~UUlM)C
zq;b;8mHx*1nCwuD0?{$q?t$Xk_QYD5Z9f;g`HXhn>@0T~WnVe3aGO)Wj@&}{ff_uy
zEoTCQr`6B+y7WRA`ARQv&@(~>u=Ii8QwWv*4rE-IYOEG>>vHR=xYx%^T{$ssiPpF{
z6<3up9W#XcC0~^>h|mihM9o@d>;%9@cx&+fW!v^5-s>qdmVWG~?_&o(go?@f)0^8f
z3-+cA(8o&aauq?~nCX#T(`TXsQYEg|3Zia8NwMmFP|fn49_rCR!|~&(&_7G}1fh>4
z)W(It@vedwKDY`!NJp(Lf%ZMelEAA|;jNmcR|B+mUGwgfz4Kd|-X#lj<gW1V+a3&S
zQyeAH64YFRfZlSyhI6FdW8xKaS%ONymv~S)&};L_?DYf6qj~vzA9QkZy^KlkO9wl9
zPrlyZL-#2W#J-z-mbK-76`vT;3nMsL$227wBd%*ksT}SGec$T~;-s!1bllkSPyGjy
z85BzB0p<`b2B@g2eoD8Sl*w8oQO`$#<nVAebDEzI=Fj^ntNTu4pG40qYuekb^mO%#
z<zbfu?I{QC>*$*=t!i^uD$uM+#q#kn*MXVPq+amjsjbs+@3hSk`WIc4HVJgM|HW>~
z`e%k)KK`EuOM4$XXOTZ@oQGzlFh-&h*9t(N373dr8)LA9G`L=CZ%CP`>)^%wZc+`0
z+^9zR(eX4VMKS^q&l9CO%q|42`%3lWo(->cL_J4Kpu^%lDAoa&c{Hn!(i4<lXNV-!
zLla=l`(>Q$$GP%3B%#SX1Cn&CY#vi1PAyk_)HQCS<174qX!?<l;^D>o<dBVxd&eS7
z=t0%I0gGnPe?T+Oa02+r*^YdhA^sOc|BPwCX!_5RxBp=4e@>@rVlX+wgxftkxw~bR
zw~0XX&AM8~JI*3N<cED+5}B}w#dddgx1T#z7l;tzWkj{B?avpq=0O<V&fJFbR9%Z4
z$m;$q$s^jv+tF#rpd<q3gR));(4eccM%-ip;Io6W<S;t75rW<5Yo9;Mm~J8ISl$jT
z1|?~vGn7?^$_UJ7u>zXR?O_M)?OHw4KQ@EFi+RpaCE}Ww_9C!4=gV=p=Q4M`sk8r2
zmNlKW0zye|J1gJ~?Sbxo%J>QozvBfnP)lb&CTnQtbuvMX;<w#EFa!BY$;F|V&^xx$
z$#r#ggoy@LhRL<)#jMYsy40DSRnf#=J`$7v90(ZWk*ibz5%Tp<ZA&rI&W!Hg=awNd
zFGxT8>XN^Toz=f%b36>Sub7zUG1}~bdFE$%(K3PX<mdh$%)ruu>f0Gr;YV`N7)hgf
z21IHMYW8<!-TtWZ6|Wk7^l9~Vzm#VQB_fLYBdO+vHTJJQKj^NGtP1ykEwL!SOe`q<
z1w|d}Tx;#z5hK?hs|)W6@8TTgg%8qGBi`0WP*$hA!dg;l3VabWc<pnh?=^6K>oU4B
z-lemOf^tTYD6Jo%R^g`H%xQ~Borzr)UG2NL%&Mw=ufYbe8xOhO!^~NPb9uWaT?R<w
zHkm*jRm=^;daOoXKPqv#(046eGWtie+?saPqs0q-j(7L2CIyL^PbG^*%&u9ps7?yS
zW4+ip&iOJ$HtM#hJ9jSFUq7Fg;WtQmQ=Nt~tt4_KyN}s?YnV%YU%WZgJUtd#`S!7G
z1mY?AJ|M2M5%XQTf2q34=L210%1@pb>gg8Pxa8vx#EG<J6%AXsV%&M9UrxK6nOlUV
zvnK~*ALWd5rd{v9EvV7ZC0Va4L^f%tQQv0W!`rW>VL7Rpb2pJvv8V)>DO8JEpGxmn
z*>~?mfO41Vn3$t&U8uRxRlh^DybIJ#0$ewL0@q(Pd4P>5elLql7rPN#rh_@?%YDr6
zS0&2w^o^`BS5kJ4bs=`EbV@GQaOlmYq01H#aT?(luf3n%x|+wr;5u|DcIzCZ2-49V
z12n(6#0l`?`tKyeZQckAoq5yh{`|F5J12jHi{=Vv2y|5ZtHq-EX@jc~azPqZ%)q!p
z&64q}=XL!G9AAM=10Coomk1}ahFirC?B;udztIQ}7kfxK^y})}tZ<9g?#H_dV!tlj
z@x8Jh36i|&zc+h0<c@raHahoB`6i-Lp;JcDWoUXU)53~609l&fLtNa%yHn?D$?i^<
zQ;ao6rTdl~+iiS@a&Wblt4qp8jrlUp*Mzw||2$o^c&K!(QC3iG-<ZqH8MEctV9$L?
zh>6v%v{D{DPSnyIkA#{H{wiHRX4Sgsp!S=Y?{?##51sdrU*_A8Uo3HT!$mXdyP1Pd
zM|EuL&xrzVe)~6K<9K_(MU`2|P`-Z8Tw@vDP5x<)7R$@lKEnp#i-hbGknimK?NSLS
zFo%@8fRD>}g!iD6PVb;vcNW&_OJ65?N&(*AsamA{b~EKGsPn+;%PbDdW|Aa-_4K<4
z>zYGZkc-48L%#f_!W%5uH+6fHPs&IZ4<|V$Z9)&D){%H4KIC&T{9|VChs(huH*ji;
zBF3MuqFy?QT07mpav^M0t>Ea77!bh&^7w-AT%VydTSyG}X#E-=R?I`~M*bwN3bMFe
z$a{nD-C3O6|IM#gryDF{B78Z;w2GZb+csGLWF9Ztq6y(W=TmeIL3L4U9Uzb3Ri7u3
z+}se<>YeR?!5%7Gk$_@>{@klXP2+3h)SwR!N-m@wPj48%y1ckYM*tq>-E2&AG!Q+r
z858BNgA=)UA(brnr(bydg-^A5*VMOnU=PtN{%jd7Q|h=&gZi2P66{3|tHmIrE<mot
z>?{2ea}i+oh(E=!BnElxG!_n+wu92gzhdw0UPathxvKOMi#ukAWPADx{SZ5ly9V{F
z{eD2MSzSsE;F6E13qkju<L1u!#QciW&gg;>!K0@Ylm}@f*&2!Uu(TZnu8U0_bN2Yz
z>DEc6-8K8wF!Q}CGZDOT&t3Lp&{V3c&mBG*hR3YB6>94AC8$Z>kKI;)2ymJLBlS8<
z5u`3+KE^l(TtPxMJEaz>*Pg`F;CuPkgH*EGE`&3u*V*cw6}X1{!>^ifiM|Y*-wd+B
z&Ty%VwOTzcCuA7=Z_IhkBJ!8YspW0oPwLG1*u!C+LG;@vyPm$Mb~!_h<$@(X<9lY;
zY8wJ1{Id?8I%ux|Zh7TJNwRafo0Lc-*e~C%#$M9UqI@PHOS!MIK!3GZ@Mz}Us4h`s
z={~PLoT`D#Y~Y`<$qd4ZmkZU(weWFnv)Aba2kw6Cx<b3VTlW146unCRm>2OD`|~o+
zbuZ9xw4wvd)YgJ2EKN7pAG&2ack)rcqAFFEu>S^5Jy529U9{DQ3!<BJdB3s2>tZyI
znJGbizalPz8sLY0S~^U`{!D<s1(TtmzXe#o3A7D<w~h@YozC<{%u}A{O9E{Fsn_~#
zAoBfra1en$(=YH}Roapl+GClRC%ryjkGn4!+1h&!I8*-^;cm&#&v=(pe4!ra>YaQ7
zn8&yyQX61VT=v%u?7Z6gDAy-5@?$C-9;U27^cPAgS}|>7YOb?~A9^bcvp)GRHp|l6
zKgiM?`8*rAu#9v4O}QRDcDTkE^dx2>==dT_00*D=sNyY_ns=zQAntvinnX~~`hCCg
z52z%oiIJEgF8#AqQ3+KbUZKO@fHFhO-$i!c+(qKXaL9*-CK67j-WIZM;S*4C@QQtD
z+SI8hUKw*YD=skwMzg=4os5N^+Ic(`e5z9@ckbddroE;<7HEsi0K^IE*#P@J5+t2d
z&~|I(*-Y{>ey@B7-X+h(j@ikuhr)lh4WyuESS+55Vv)?L9e{`<skLjUeAqK&uCzl=
zHx!dqF2p3Q@f&7x;iP2s`EtU%tU)^C*WVu!5ECLdo(bExsYYpkh+%<Fc(y5g>F+$4
ztq#;6*8S}LtYM`?zD{P=Y{ixW=0@$EnNJrhBjUZyBlr^2>yNElY!8re2+`^D*4#I0
z{fzAt`;qR?!X^;UgI~w$JINSZpUV>L*)_Zc96<P>t7mHzQv=Rahf7`Y7b0Wy0waKz
zFY(r0sJzx^qwu#M$>l?ZvjOsvus)as+1i-YQZ3>1i(2=Jlz$uXik#&22ZSo};>&KY
z&UDjzy0^Bid|-mJ28Nae#jl;S;pLvk;@aC#h$!#t?u%i>LrcV>mat~txwr+X?rz-i
z^e4*oG=AtyQfqJ#*}8cAj#svVzc(Sn`OFH3k`d73$95jpD00JK+q@s?q3+>UsXzEc
zTRv1>ea~dIHymIpyCkZ&SwuE|xA4*m3T8Wv;z1xnVSVNyu)Y`!II9poS<>Ym82xAl
zD$WWF>uA0Z9b)zq^@!a9((8A@iP~qhKElqdNtkvcU$@QlXt9RP><uB|V!v?%yB<4e
zq38TPI6;vz?CBT);~(#Q8*zKb;AB{c)>-H}p_K!LNBS)9BPTt`*6x%{N7Lsc?V6I3
zz<DHik}m3eO#vJqY2dM^>YG0zJ->xELtsCPeel5D5Smt42^&sxdtuamnZ>CQvs$#^
zFwj}Q7_`f3EqswAt{v@DaBm}xhxRoh_5R}hYsMsoTZ^spcZZry6@}<9l%yWmx$?j=
zQq2Pu`D+3>%MCvyOP#lsok8cX(AJMc2hRpZ6BVBeK>U|AaF=S+xTsJ4112r5?+af$
zMWfw+dA1EckQ9`mK#S@-`TRgFjJvQG#Q6Vc_dRwTeE<icRp7sfV<?z_SMwk=;gvIJ
z>42zm&k~A7+{h0n5)c7N821<BvC)-8rzL27?`d%2T0<`;!~4<4!%9@*xNdgGZ+K?H
z0*zm<5L=as$`<2F56J8ia)MUx*ynyJd}v82sE%&_!oSrkxaubHc>&V=X6s7+i20k+
zJgpxu=7ZA1XQq6MzT#635)L2O6e3F+M!nL<?c3<KFcY1kruh3yU>VaT@au7WrB8Jr
zG1JQD7S60gqiSor(!P+WB&slZZ9Hc}U1rogJ1(7*(0w4;n*78IEgx?fDx+-gNgo?`
z2?I52_-6UovjOS#r{#}YvF2jQsA%7+1ixKD4W2MuS=;fl@cU_b@{6KbT)K9OjE2(_
z!OFSl8Bjpcf%x2~-W4^|H~P@ckgt*JxQ+8})r2$uFu2L%j49wn(*aWZO8eBEE>|Ds
zhjAgDcQvUcHSsUbD5Vp-LfdGhcxEH#j+bF+I~CCjXK`f1a1A)N$(>&MBAZ%vFNe>E
zRpeq~;!I1PClVE>Q75r=xwWEr>w!I5<FNMCw1cK1TiI;X0%1^HcKpFrhLqtBV{x68
zKGOBkn!>%Wbvn?Qg_c&NZ`;wqPwX1u_Qc8>rP-BOFb%wDNPy{WmsQ%kH8j$ia+K3l
zwIn}Oy4pp{R`aVcF>Jz9DuJ`_S80oEsw-|mK4}2PWG|JorZ=?wY^Mz6WmDIMz53m<
zsx<g}!t%2#f`!Mnx$%L~yAAhA^JA9Fw;TE)FmrPY@DKZ)2CA*Y#v2yi!uzhb<bcKE
z0heV9i+~mI4rzO^hIP6^81Oqh&;M0~zTBmn|KfCjd>}09CNFz+Z)AKj;~STvSp8{V
zLRc41HUY9}P*_#mVeu?W?pz&nk95^SR3vz?Ihf`;StgGo0sKg;l3cGwDEzUD>6Y%Z
z(i~!}RmO0DCifZgv)`_oAB&8MzaHJEsKyr(GVHMq%NXUc-ESvR>>P`3Ot$ZTAKj+$
z)^zt5y3pd=YX2rnxv;*D@?NV5%2{P<w81h@m-<-sEdpDh|5iO)S1kr)g8UtZ883$O
z4va<stia~(9lj*~Nxw1#%-b{}QZI;*IbQxp>ya^O&<<+MwAqHT%W>&9=>)T0O6x=J
zh$>Z@^&=mbd<kA|UHRM}@ZfWcqnY>UJcf~S-M(k<!JW^)l-M|h#phsWSVvz@&$r><
zr|w`g*YA|JBN{Em?Sd4(L-^9xmGaKmhIJX*P!_Pwl2VTJGUp2BTixQAO9>rcw+a#V
zn^iY~-Pm}FOu~5lZbKX?c20*g24nHM3elymhHti3k*b{LbnrO|luYhvLDsrxJR*w;
zpyjr!P^iz5RC#3$XWvTsNb51gX`?)Dpc;=rqVCA$ouwB2dJ1)?w%<ryi!+d!NzUL@
zDCFB$wgk6pVaEnHQ|px*jGK|$A@^jK*3rg<;Ry$OXj?@kQPyMumPKUi+0onzLYu_9
z=*zUDvgKe)YXyHm3{Mwu569uN+iiKnl5dpGKZ4ZdbRS2i93B-Uf)%OQHgJ!eXbvHx
zoFmQ7n~<Lza98C3Rq_rjt9Su@Ex4(Zcxd&5*c?s+mB+HR6;PolelBX@mxYwgG-eIG
zHv`L?vx268Jtgpu7(u{_m*4jaT!<kV>ZL{`U@2j|ELeKz%iS_%_xE!Tn>wTC84f|J
z9t68zq|;K%lTttJiA~4dwB%P}77wQ~kNF&d`y5>afGgfd@X`G}U^Nuy+LK3Bk&8n4
z!l_~pV8@_(AB4IC?D&fL#v1HrKc*GGm;R`IB;dsFf0o692S)-98sSvP-(lo`i5&k|
z|Hz13W}pmX$2G}sfByKl0#b4os14vNZ0+<-boE(3Aut||QtYr{!K^faXbQkeCupwW
zGhgu%7DxO+(^c9)4LzQ4*<}njQ_~|pfY(T@^%QkgCMs^arUo$-K3#*ki5YYHRGQ1#
z4N}w%y>irF_Jzz57B(*^Lt2@Yl_yAxCvJZ8sOCp9TS^1p^;Dd#du`RPeCEx$<C2<x
zmq!pn$*0+BWs>T`C^@yV8x@38v<6hL*U1f=WB#RBf9bZK@uD!kEN(MRCuHYG>V5&&
z<y+^N(Z0@m9b<cU@Zi^)+)v=Jam1zpEU`5w`&Nnh<*W!EWOzj5-a!aKG-@Rb6+K;U
z6Bm8<_`Gi(Mc2THlN;6n`qNI6AbOuDvc(D9qg*m#s|qH5(<{rAn9q8C9mrl)ACBER
z(|N8N>Dr9AYqZwhKi~N0nZo7hjaz_fmHKXA_0mg1Ed3X(A<jCg%|u4+nyzZKR7j+w
z)om(QmwLdNaND(od_?HUCE&G_G<CkY?j^H@Evm>hDZi^I3<!+^cvF+1V6<~$hLDZ_
zr^cV;kJC{F93)57*)tVedrzcncxGQ5BsQd!`wi(%C|ICcgG(g+qh3)|KTOwr=s1uV
z&=}<yhr9Zn86R&3{ma|Ee-XD&XgUs|!A#)4MXwC0w_rk66TpOHxI;SeuaaFai}=_e
z3Ag!2)X92u0huTa<#PW{Sglu5Iu$Ev+~R@3V5Lt&n>yDB6I=Qz7I25u;)^fE7VZ33
zmv?bd0GyZ|N{Va;iOB`kH@t+N%gV|kO3bS%b3Cj^zY^uv3;)r;a6Bo5!>LWBiW!nu
zwNqXQDOi7U<ub5558ObzdMB-P3*+M~e9T6_b`7`{UT5R<Tgx`HI2D5Ikj=tJHU@LE
zM1`O5g_*W%A<;p(7|H}u5F&xA<WQ7^S*CP}$R(e!U;lP&@@BhikB*kJp1ThrHuaQM
zPd?@bZcuA|A<XCi_m2`X0njad$G^1HExn2)HMCv3#j5@gd9f0XuzM!mkDN%c@<)m{
zUWV?ngapssy{93fshGs&%D)iYg-u*sBv7tFnsLv3%{svL;#KC{u)E4AYw+ta#3uXY
znW-oeJNbSpSl|NxEdqfOe#&qd5*B%G2~bTAP;Z-JSpsApP|C_?9vnf1-K+vpMb1_o
z<L3dUD(S4OmG`CR{@Qt~rLnJ9ej`PW1rW23q)G#Zk2zKS5>I^vIrNHj!Nr0naF|r%
zT?_Bj)gv*)hm^X2B3i#J>T}*?fc5Sg8v=1@ywjZ=dz^?zUpxUVkpKvOtbcXTfq3u?
zz&N(RaL9S*A87oOf&M>ed<y$tQ89nWna>IkjxQ8;g0`t|*LMR+of#s_LFwt13t$E<
zF6;spXVTF1HCTMP1(I7w2#fZ-_<6*8dT^e`fILCN%WgW1|MWm^!Q3hFn8V2Y>fCv?
z=x81^4^;C)`_YSUikDB!jIKgRMZR}oql^fG7CR>gjEB&A9e9KCBPaA<lx|thvMsEQ
zhLHMOI{DC<e~`N&-4*(0s^pbosCt4`yu{Wn@=c+4sC@12{83{(q;X|BJZtz<Fjy^S
zmecW4n+u*%>$jA*I+|xX;6CO0RPJ>)xGVM*UbS@gmAl;qXa{pp@WGica7Qjd+ud&z
znaNYPu*e}qaZLD@4-)Jd44DR%4mI994ZLe<SLlohCTA1alnSZE(C7Qd);9r%)62V7
zwvNyVNNBmyO{so%nCMs+sk76e4W)a4EIXfiDs$k|75|GOnwCUl1P~SJ`XZ%%gxEUa
zi@%^Lpw9tg)%7u@3cH+)e_dq)JqaviUIPk?wFyl-um7so?gKQEEkJWZEt>5gC%{A3
zcj^C<@c$OKX=(l4BgDshT=#NxWPxzjk4(k|F~RLf$c74IVB=7h;0rUvMecAeMxT$d
p-l9-}Rjz->{;6X$9RuC7h9?0m;teNg|JUfWH4M~C?mT(-e*lHFgdG3?

literal 0
HcmV?d00001

diff --git a/docs/SITL/assets/serial_receiver_sbus.png b/docs/SITL/assets/serial_receiver_sbus.png
new file mode 100644
index 0000000000000000000000000000000000000000..ce4ebcce1690df41de20165d5f73fedf4d640a48
GIT binary patch
literal 6803
zcmaJ`cQl;cw<aX=A%-M^2t!1O=tQE;M2}8%g6N_qO0+SeL}!TJyAdrB5~4F0y@aSy
z$EZ<8ong$}$@ly2cklh@uJx|<p0m$>)_Kp~`<(sk^G4}tsa&JJO-)8dc1`t};&U>x
zOF%L*@`qO`ND|d|4Bey`i<gqIm!7-5myeZ)9hsc1yR{vMs;iZQ-E%uDTfaAbb}}Ty
zI~@&urD)OEfoSee=rb~d;`Ot$larHT1G2Sh=Z%exGaTs|?8lz9x3_<sJAI+^+_`$K
zEkYl6GCwvpb~;lU%)&DF`Q6~)pmtKfdD!>pzO(USm$SZT=(nZ$W31L=kuy~JNXl~u
zxkoOgV~@+`%0HHi#r2CTJBx^jVbS&T^Yf``X`&+D)!sS{4GqrDl?Gm)$jH16%IBQY
z24d8Gv&K#y1Xou$yXLxpDux##jN~U#t1rncA`#oYTUcr5lHs9Ur$-SHkIWJ}XF8Hy
z=C+R0`!-GnQcuUSk5IpG$A_tdrvnou)6>(fVG^@NCb$j6>hkpO$sY=)!6AB?MP^#L
zhHeX8Ik1NB-6#FOk8$wPruFuij+M>TmEN_@<J!gkmgaEWVYjAn{wk^&jomnC4{e)T
zAz2uQJ6tb)eu%@7h4efxT}O`=Vo$M{y`44Ok3-yH$pre9P5$BxD*H=56bfA)M645g
z))v<WacjtdnTv~y)zwuZk%+_LzK*<sl94fgR8^GI_nrNn1GBt7vw%$Epj3Q%^JYhv
z3bSVKL_hzkeMYsLle4$aOk+Trsrm!V%&co(3rw7NCcEf4jK9S8oIwYaXAvjdHY~h8
zbP5>}UKcZJl^?}4hUk$61Fk}zE>SpL23Gf7r8E)W{DFIYY|!|S-uAgq$w&!CBx;=e
zr_*y(5nzm>JdT4Q4t7=RivGiW@?gM*+4Hz(+7fd&GAMzg4E~uZ{?wrS+s6#IWo5;8
zDiFw8B$t~1KH*J(|G70S{P4#kK}D*VJ4uSY#j?PnS3&4c?As^js%+8qeKS(;GYrE8
zpRxC&l<#$-o^|EI;_!h@<}EuaJC;(q(;G3i0Z)Lk-YdunE`}?&lEoFQP{XLi3Bt4N
zg@<!{eC}yIip(roMjjKl(2E9qMB9NYdjmc$=;;Z#+I!79NdWOhzNAi;T5+jtVPN0v
zA?B^zOe=COu`lSpXUa9Z6Ogm$N=lI3L!xOzQ8%pqB21V6V$<Y)P!L!$fM}A35!BKg
zti?&6WhWlV>dVg^iagjaLr|&fB&juvwzux<ceCn!dU*4C-M4ku=RGj}_n7Nd`WBjj
zE6eghDi7UXYaB^>d)_KT@5D?7eVl65(HwzWB5(d+ON`gS=SwJBK^!qQvG?ofFqlT`
z=Asmqn?;*bXLq8QCVFtMB+iphJ+{&Cdxpz3584u|zfeQ7vRg+`5rg0j*kl?5Mj=2&
zyxBb6JpIwyy$+SRQ+JbJ^h|xf&+A8e7HhDa9?|`ldq-FXE#(=YGro+{!~(<rd>Mx%
zvhJ-+sI&W7^Q+(;J)^d5X51S<C;VcCyyIWAJIjq?fGNh$GvYH2U^USVsXbDHkomHc
z`0>wR`MFn~^x#?ZhE}?n$0FE3I;l4c)F6&O0r7VvT>{c$-w84T`6z$>Prsdt@_(uP
z3An%2{*M@SkI!=%@688{`JO@i2+#QCoZUXW%JtX=h8%>W5eT%lm}FOi)+(0;kMAm7
zcPN}LkMadsF$!BY^=&m)B}uj~7^a1?si9*REAK!q(|ROmp^7Z9SX=0~Wq3A?LrpD7
z)6+!autp}7witme3su<~9Yk7DL*4<TK~O57qvaq(|Emzy5(VyK20$I^FGL3#ZTrgW
z<n(eP!>1Q46$@)5zvs4(;f1~<xWX~IduRT9f?Sbjj+Ly-MTgk3!}Aw>jDd=6Bl9uw
z-dyJ!ilD8F);zV6-Ct4hE!&`{*o|kP-}-N>*u0YbSjNa@+K~)l4KJvuTrNqqBgev)
z2=afd_^&0MC_wiM=$KZh!cH##V=k{?tnci&jI0vd3vo~`$e>L^U7y*j2D|Z!ot-v;
zUB6Qag>n|V3~ERAVW-)i@on;~N#1yzbC4qa^7K`_Nh1r;@TQ&rn48{+jG0|O+|5oo
zc}F`{loh_MvU4~Mn|S1f&utzFT=Y^F<N{XU!u&r&?p|_1`)x97iEJ(0`)$~`y>w5o
zn`X-=N-uY4z6rD)8#Pme_5EFTp6HmOORP`!!?T!N%N7C#>@Btt9sRx{;knLx5sq$5
zmcAFkpE@9)`&S>G_}&Z53e;`YF5PHuRpXnZ^OL9sw5kn9qPMz2KAco6U$ztONTq|T
zZSD5Z)5vW;kw%<~^4|-iQm-2rRi|2v?;L-5H~D+jElWbcunIcG3#9Cz;l24+JnwjR
zih&`va5_PNnOV0-&Fkfn;Vnk%?f4d*<YrH2i$0+k&FR}}Pua89Yd#Q?u=_4L`PM2U
zZ#y6Q@|TTLS<Q_BPb9~ot^Mz~l68}LtGeDM4rh!+o<@)OE4%e5ZwRkyZG;k~3V9XQ
zdsU_c7N@MiBBS0v`|@8V4IQzrPkma34y!OlaQ_Ob7=vi=%NB|l?K)aH53ifA7_8_A
zJu9n8ek<YcK;z-Jp`>N57vo9zI!$Y~WyfgIyuzBR6}Ef!v)c8HI)3emhYQ1^8&95b
zjMC5Tyc<R2-eOOmY+?|eQ|>D^+@{9oS>+Ks7P6y{HR4mgt>L>%967~D-U>v|#@>1x
z1UEgj_1LQD_%I#T|1&<$RW{SdNvY;ow&Tfq=<>yuW(cUaD;ptQUj{WwsB1({cqp+O
zZZI9P6nq$Tu)WLTV0Mny>Z0<=bH+Ci+DA$el_wFB<O_sfQ!UbfJ1z@sCqL$bj%QV(
zc`dbuk!ci}><~#o@O*zUYUQT)HNP~x{|oz|dq6;7`3v^kQYD10>w{1$p7QvGH8?zG
z^1iLHte>?Xk83@v?EtfU`8my;xQuz>lqm68pMT=}DmQh{Vzy|<+3>m>(3f-;;k?n8
zdHAAtqT^F8&{?lRt2M5bwZg(x7zY3EWi8%tY&d@C{@fTBSLRM{c3%?9!?JQ*tS?T^
zY~<>k>8>JV{}62h>!x9O267BK0qJD@IJO~<8ts8)x{#K`%Qp@&=$1}0Nu7lt$#2B#
z)XFXDm7U3)(7V&Grr9=1ma3HW45_TYJI4Mx$`mqZ!^`?+8A-(amO+@=9V_5I#wy_X
z$1UnVDB!d8F4)+>6F|{9?IQ_`xdg;4H{Bpxf>P93;RD9*+f;Azy}VYYp+!}f*Xy`;
zZ2EF)>+zwVrwWiQwOVoG7r0l@r@6rk9bvfG&#FT#%R?2@e-W3~2O^3?e#F6U3kLwh
zKS{L!pj#s>huXtbTnds)NvQMsDR;u&9b&j6k4HLp72+1C%LB)jJ?8Of@0a$-jojG9
z4f5FP?FZLfky$rxU3shG4$&IDfAdx>p6lfhcm0~<2_0v#t8sS(Z+|4PB2=bgDeEc-
zTZ7rK+3%Q2L$V7a0TMO>%)%?i*a8i|`3IjiW9fBL#hx>QO}isbgWa)P#A86tdjMTz
zIFbPZsUN&eF{!Lbvn3~ovWb{=9;jCmgyW0qx~+F-h48Vl3ojqlDwyYA0&6z&IIe`I
zj_n%3Z|Hk=3l+l4jxGI7b`jrJx4%$4IPhO@nv$qHC}8Z4w;$@B&Gv8%X49#Ea@U)C
zkKY;j7Wokk%sBocNu-DLPIMebIJ=6q8|i-JdL-BVo3LCcG3=#Wc3CVh@wEY=0~cA2
zwyzh;byhG3wm8H9@6ca^abaKjNj|#5nYz5r^ay0k*LJa&#eD;4ExFgmUtZyIdmSz%
z)7e_qB$7IJj(fFYX)6XSWL>di)b>N~Do^}sGvF}}sA=Jyympw=PZ-DwTL`Mz=ZvAg
z%*YL=`}|HjAl*A!b6S^bwZ_j_2)&-AnA<37z1c=-8-Q$LpRlufKgw4q<Z0%GF4MM^
z;-XAO$3o&CloDvxiyHDP^BkaC33_r*6EDZL0A!Q|az&u@9Jkr4TA}5Sl$c&jjVwDY
zJU()lodTtw*T=%PFZd}Wo$)W<-Kpz4J~nc{zEos2g#^9k8)gU3qEFLSetc873?!xK
zzq7~eJHRg><;;HYISr>Y=yi~&?En?!ze!t%6+<3$6N})0K6SMG?`+N#4!{#Gksyoi
ze;`CE5@Z(`I-3p)KS%6yroNqJGbn{#7_aNIIZS<d^oe);ue}uFeqw_MZ#53;0o8}N
zVYsh-d*fM~c^9Y|@Rllk7*#Yz5I*+4IDAYSA{h=i@H9Seven_Vh|+B^fY1g-MV~rT
zi0c@}Z#AVuA~7k}x7Q>%x(TPnoT6l$^B%s{EkVSBWyk~;@k_D_tdT(ajB@n1hjjuh
z-zmL-DJq99pAbK;zI-fE$g4+pDLD4VO2>mX{;;<ruJZb|s)PI@98~s(nB|;DYU0?d
z(8h;X>^wrF1`x;vaa?Dlm1h7%CYd>K<~Vo=KkW#*pW1t==nX*_QpaC!UNDt47E)o0
zE#v>v(&$L6{9?RBJ+x@}2;?7e6#B9vA}JeCp-SuCT$<*L<CtTpQp|xxWpSS-!~$1M
zyiBnBAZ&*6V3d`o1&9%1+=Hu+1JefF5XT{2gXD#PQS0qATvuvg)N@(~9rMhWq<{~8
zzKHzHQZ5>m$iUrZJP=_~*g+K_w?l^phF(5C*cX9(RlFIA<t*(qe-<>SMG!9G3t}E?
zP1sSBJI}4K2Qr4j%CT~&EzS2{n-cors<AS^E+XP@gO09yljiG{46jdgqXwph!>VDw
z-fMLMJkRCt+csWP48S77)}a2owOGW40W0l94}b<($*LfEc_>*agN6ZvaJp`@TKyH`
z$Q>POGZ*&2f&0m{ZtK1!JE$4p$U&s1VZ8hB!YLjIc^y=7VfEsM0YTUe0aVt{`)t0v
zd5PtFdZ0cnsCndusKQ@%wz^*u(zG>j5q<Z<Ik1TCKy*1ySS{PQQIk9|6LhjBwusRp
z6rNgkH9%HuV5f%FQV*b45mcT8t7fMVaO~lep!C2BaI6y+(Ti7@uHG+=iG85~;=2Om
zbV{{XSDJ+5EN*er{8g)^J~$gRhRv6S=cOM2pjKRa;%ckA$+g4uFeU!Fv3OabaKXz!
zbD;0W67#;k!iiBV_D#oy)u54RZ?;w)SjQX`eF5C)L?(#0AHO)~J%Hty(RmULX?U4_
z0(<TW9UGkj&z^QaHV30Om9_TTcjte&&^&k<iItl|YUM)fcJ_oSDy`;DD5&uus=xPJ
z)e^-im7D~GMEH8+0l)HSXAa2a{-wkd%8+*8WrA=dkbaiw9|}GCewm`J?I{UzYW4?0
zB%e75K1b$Dtf7tw!pP0RYR-qM=(oq9?*lZl;btw+q#n63WAkk*pio6xFGB!0xG|hW
zU&nEXO7@!78q@}Eu`<epfK2)xIFb09PD9$~ih7fEosiG#^jccjbj$X6xRk8Z-dq3Q
zjx?a~#h4C(fS$6swS8R_AgLS^@tuWLMAj90WMRKG#jyNS(T2s-+H54eFcK%uE5%Ma
z#;3dNBfq9A2ffJWnaiSekm+Eslj5Xx=v4I}UrOY(6C8wZ2?{lJ6y%hZV+!1^V5_@$
zHEw^-^XcE=ZxuH$t;w|B;{!r#TO3dFSRSfUuo|4r?+1>;4}M}N#dBa*@*#jWbV4@f
z!3@t)1m`*}YfyTWZFXyU_&|(h48ORt=uynC)GU!v$O_jE+~K3PjO7s}%0uAn0G&2v
z{qaNJYx_SOs_EV}hkNCm-?-GR%9jsB$IfRQ_C+#LO&<hB#ntEB#7ue3dfsnah;c4I
z?TXe1lwUZ61#7CF&Aa@V_P~5B4|L8Pj_?Y96UhW;c^}wH5dK}^^ww_-+Z)U1WyVAU
zd@x^c^MreH1w!f7#Poe^>%`gMarT%zi<#{MjWuAXmIoyAmKW#*DCKCB8AChtVeaRd
z+J>`gM0rlK={A__nv3%H#kgszs_OH}o5k*$F)I;t)LFjOfyrEMBaTv&%t6d<5~U<F
zn%b|hY%p?WxjHX%wL9x{0cuhwebrr7mA3#HJ3N`ou~7ih0?V74X#xF5!96>*)ACI}
zUf6Cdq2GRO6+bim03eY6SzAZD`(e1)UCWZ5`qIld6{Z2HhHi=xaPK9>{TleSO>oui
zC4e3yIGjJDLYmvvJbo#Z&mv)2D+3i_^+W;VJj{8`AwC*-1-N?9DJ5*R5NH)Zg!NRf
z-PPlP_I|CyG8i~tINSr?rI1)riF4OfKNDIYoSG6uH0<AhfQhY_JDf!PWrPN1!^%wU
z*G~aVmz`>=cxo-y4zGb^e3%RZ1#*M<5AX=#Y^xF`OQ~;R%Y$kQCaDJ2nrxIl(tbQ9
z?Xg$9KG?Y=35qUotC-AH_oOLXsvmU3RG#D|Ug4JR+za0e4dQ!Ksf4I6eRN^walQl*
zej*Oy&Cg~?)H_Ec+v2fz^fWG*px<I>`la@cbb=IT8!KZqaE`4VNYjKL;fo;<ffa=l
zOIk#RuurD05e{fr&VDRl4(L$X=!UIkn`WC;lE&pH$UTWA{eRCccaiouijT<BhpgUj
zVmQy&Hb9w;dx3m!$ZJgOzmd}WsC<S<pO4ZmHb3w+6;oQRmN*r1Tik{>vlQGjaJIZk
z?M|d(kRRTP4v+{+*E^K6N2bOYFQGK1ki2s9N!5L}S&;3TjW+Rpr)PRNj(SnM^C)W2
z{`sB1i|wzhzy|QuFUv*ddqOgK4sB0)&=jXU3f~}g#*1f1z#T2xG6eR?@_0}_9SU)>
z36|FNo*>Cd!p{L`BYR)9iNR6{u%bKyD(8IDukdUWth0`>f}Ux=s@NiFPr`{$*?J?;
zo8Ssk^S5apfx`RNpW6T5qW+&kc72EnU$Xou4ohAT82rzmIWvh(NZBHg<E<1%wO*rC
zPX%uSWrq-<<RKVOrg(tSKQ$}|SNoC6N@S62%N~`NAC}Y|?a>GX8H$jVdP6ETcwQ7}
z?6KwzJQ|&As@62}4?xWx{lf0Qap=>rSIiK=`0T#h@~bQ+HgB$m3Uz-lvds}dTFGm!
z+2p5TMCiTaddzQ(1k&CTId1j(%w+AK3-JxvcG>`zR7pnkL`diD$FAi-_DhWKR%+s&
zXz1G};f$jI8a(nDPu?ez$G=anZpOS9-6r915uy(@DjrXA1Dja4Aq_X{dl=Py^%+pY
zx`~?ZmQR4&g5e?3TL-Piz@>3Ut+@1D2SUxQnAC4;D&nE8KJP-?4*<C#k=&4{IpW>n
znS;p84l1sqUL3DP3?NT9WjICVFa%aj_P9C?;@fpyWa+xAIt@xieU|C2wLmI$%TdL7
z-!Xkq_~ilc?oD4p2R1>y!TTF?L{zkk65df4+J6Y!v^$u|jk}h(bF@S@AReN2<}lOR
z%l#j72t)!W+U)cu(`Mc}jx$!JHN|hk8$hM6hQzy%y-C#X4;##h2jE))f4E~P#~<#<
z`G2_M-%j`X3;PaCjklr)bJPj_69)M-2$hmge&zXeB=@F7`%m-hT8(zE6n*mZ6@GfA
z^A}Sb3}H{Ye}N@oYNQkNe`v=)F$PUMM;GA=hS4@D?AqTUVdpVDRO4OzTO`u)i+;-u
z1Y9ZTl@?dblka;e-vSkr_z*7!f@<>FaNh!Vsw({zCK`QRpn%Sd5!{GuGP@4GpUVE-
z_TiT#?aJ=LEk)64twCIiy3PAhNPH^2!mh~I*o6`9Rj%iyuOvNQKpRt3ku-|rkG**<
zFmLqIhu-=0bri*{+Bn=seK8w~kOKMnwiBABw|nDQQE26*4j(=CHirLHj@~{Tsowi?
z=ezIhHSXhdPS@cVWu22fuxBT)J+cWENhm72lda#@wy|lrRV+b(D`W-Plc0%zNevE@
z)o{k8-w{$d`+{~^70xR0slGy9xcr6^xF}G;p6ZowPBQ@Burpo-c9)n45RLn!2Nt;}
zq8=_4eT6jiJTy6*GG~X;1F0h7$Q9koop8S0QaqL8^1DK&M6{<y0b#(iidguiRsCHR
zmGeOAI^5)z>|NOzQ5aC0^*Z<}y=)RLbsN5~>1GMKi<0GciJTrw$A?lp2T=mkLSSx_
zLkY#=WkipIMay;->4V_K^}maQvH}u&;phzq{Ia|{g9$!Q;xq$s{Lg*gY$O0%)DFCY
zp^!hn^iPe^C`@86Oe6>%O7jN-<}f220?ZRVkkP~13cS_QRSJ-o-zR*8vYz44doGOt
z3<Co}IH_tW6xOp4$DM-zvK3DS$9FQ=yTrp5Be*rp_C>0{@k51al|8q{J57c28v`Qr
zUAqXScYXlAQUKA1@`dqWT1*XbR8FXGkh5e95(dvvik)`<gzZ2Y+Sf)@zgd{JMIuL>
z)C@;}Jp%{hu5DX>gPF<6&osG#xlQpoynI!02LMrzsD#i2DZ%jN6X7Rc9B2fN@m9r}
z1=5-MOIB;47uPz+3Wg2<Cxy~xyZ0TaHm`e5P87$3V^?iFHRdxPV>wiqzhcpnnQ0OO
z;1{U*#NOq503MS*B#YjSfFR~R<YdSPBGhm_^p2JB+vv5Doim}CT$fX;dav+Tiuflr
z;8;QJE8$+QQ$ZQFRwy0xt7pX`7*sJqxY43THGg<^>&!UpcDm}qpw8iseC(UXMayRO
zIXZolEYjBf$5$Qyudn_+uSxDph=PfduJ^0H3?pqH50e_Zs&N3LVpayqFUqjpNOcsd
w!ddi^+$~KH8A>3%SM0yovYE3_WE5oY#^62)TLa%n|3Ju8m9!MA<zI&U7qV@<t^fc4

literal 0
HcmV?d00001

diff --git a/docs/development/Building SITL.md b/docs/development/Building SITL.md
new file mode 100644
index 00000000000..33ba7ee3d5c
--- /dev/null
+++ b/docs/development/Building SITL.md	
@@ -0,0 +1,39 @@
+## Building SITL
+
+### Linux and FreeBSD:
+Almost like normal, ruby, cmake and make are also required.
+With cmake, the option "-DSITL=ON" must be specified.
+
+```
+mkdir build_SITL
+cd build_SITL
+cmake -DSITL=ON ..
+make
+```
+
+### Windows:
+Compile under cygwin, then as in Linux.
+Copy cygwin1.dll into the directory, or include cygwin's /bin/ directory in the environment variable PATH.
+
+If the build fails (segfault, possibly out of memory), adding `-DCMAKE_BUILD_TYPE=MinRelSize` to the `cmake` command may help.
+
+#### Build manager
+
+`ninja` may also be used (parallel builds without `-j $(nproc)`):
+
+```
+cmake -GNinja -DSITL=ON ..
+ninja
+```
+
+### Compiler requirements
+
+* Modern GCC. Must be a *real* GCC, macOS faking it with clang will not work. GCC 10 to GCC 13 are known to work.
+* Unix sockets networking. Cygwin is required on Windows (vice `winsock`).
+* Pthreads
+
+## Supported environments
+
+* Linux on x86_64, ia-32, Aarch64 (e.g. Rpi4), RISCV64 (e.g. VisionFive2)
+* Windows on x86_64
+* FreeBSD (x86_64 at least).
diff --git a/src/main/drivers/serial_tcp.c b/src/main/drivers/serial_tcp.c
index 48522508ce8..765f8308cd3 100644
--- a/src/main/drivers/serial_tcp.c
+++ b/src/main/drivers/serial_tcp.c
@@ -43,6 +43,7 @@
 
 #include "drivers/serial.h"
 #include "drivers/serial_tcp.h"
+#include "target/SITL/serial_proxy.h"
 
 static const struct serialPortVTable tcpVTable[];
 static tcpPort_t tcpPorts[SERIAL_PORT_COUNT];
@@ -118,6 +119,23 @@ static tcpPort_t *tcpReConfigure(tcpPort_t *port, uint32_t id)
     return port;
 }
 
+void tcpReceiveBytes( tcpPort_t *port, const uint8_t* buffer, ssize_t recvSize ) {
+    for (ssize_t i = 0; i < recvSize; i++) {
+        if (port->serialPort.rxCallback) {
+            port->serialPort.rxCallback((uint16_t)buffer[i], port->serialPort.rxCallbackData);
+        } else {
+            pthread_mutex_lock(&port->receiveMutex);
+            port->serialPort.rxBuffer[port->serialPort.rxBufferHead] = buffer[i];
+            port->serialPort.rxBufferHead = (port->serialPort.rxBufferHead + 1) % port->serialPort.rxBufferSize;
+            pthread_mutex_unlock(&port->receiveMutex);
+        }
+    }
+}
+
+void tcpReceiveBytesEx( int portIndex, const uint8_t* buffer, ssize_t recvSize ) {
+    tcpReceiveBytes( &tcpPorts[portIndex], buffer, recvSize );
+}
+
 int tcpReceive(tcpPort_t *port)
 {
     char addrbuf[IPADDRESS_PRINT_BUFLEN];
@@ -162,22 +180,12 @@ int tcpReceive(tcpPort_t *port)
         return 0;
     }
 
-    for (ssize_t i = 0; i < recvSize; i++) {
-
-        if (port->serialPort.rxCallback) {
-            port->serialPort.rxCallback((uint16_t)buffer[i], port->serialPort.rxCallbackData);
-        } else {
-            pthread_mutex_lock(&port->receiveMutex);
-            port->serialPort.rxBuffer[port->serialPort.rxBufferHead] = buffer[i];
-            port->serialPort.rxBufferHead = (port->serialPort.rxBufferHead + 1) % port->serialPort.rxBufferSize;
-            pthread_mutex_unlock(&port->receiveMutex);
-        }
-    }
-
     if (recvSize < 0) {
         recvSize = 0;
     }
 
+    tcpReceiveBytes( port, buffer, recvSize );
+
     return (int)recvSize;
 }
 
@@ -240,9 +248,21 @@ void tcpWritBuf(serialPort_t *instance, const void *data, int count)
     send(port->clientSocketFd, data, count, 0);
 }
 
+int getTcpPortIndex(const serialPort_t *instance) {
+    for (int i = 0; i < SERIAL_PORT_COUNT; i++) {
+        if ( &(tcpPorts[i].serialPort) == instance) return i;
+    }
+    return -1;
+}
+
 void tcpWrite(serialPort_t *instance, uint8_t ch)
 {
     tcpWritBuf(instance, (void*)&ch, 1);
+
+    int index = getTcpPortIndex(instance);
+    if ( !serialFCProxy && serialProxyIsConnected() && (index == (serialUartIndex-1)) ) {
+            serialProxyWriteData( (unsigned char *)&ch, 1);
+    }
 }
 
 uint32_t tcpTotalRxBytesWaiting(const serialPort_t *instance)
@@ -263,6 +283,10 @@ uint32_t tcpTotalRxBytesWaiting(const serialPort_t *instance)
     return count;
 }
 
+uint32_t tcpRXBytesFree(int portIndex) {
+    return tcpPorts[portIndex].serialPort.rxBufferSize - tcpTotalRxBytesWaiting( &tcpPorts[portIndex].serialPort);
+}
+
 uint32_t tcpTotalTxBytesFree(const serialPort_t *instance)
 {
     UNUSED(instance);
@@ -272,7 +296,6 @@ uint32_t tcpTotalTxBytesFree(const serialPort_t *instance)
 bool isTcpTransmitBufferEmpty(const serialPort_t *instance)
 {
     UNUSED(instance);
-
     return true;
 }
 
diff --git a/src/main/drivers/serial_tcp.h b/src/main/drivers/serial_tcp.h
index d27610eb805..7f394b8ccd3 100644
--- a/src/main/drivers/serial_tcp.h
+++ b/src/main/drivers/serial_tcp.h
@@ -26,6 +26,8 @@
 #include <netinet/in.h>
 #include <netdb.h>
 
+#include "drivers/serial.h"
+
 #define BASE_IP_ADDRESS 5760
 #define TCP_BUFFER_SIZE 2048
 #define TCP_MAX_PACKET_SIZE 65535
@@ -50,5 +52,7 @@ typedef struct
 
 serialPort_t *tcpOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, void *rxCallbackData, uint32_t baudRate, portMode_t mode, portOptions_t options);
 
-void tcpSend(tcpPort_t *port);
-int tcpReceive(tcpPort_t *port);
+extern void tcpSend(tcpPort_t *port);
+extern int tcpReceive(tcpPort_t *port);
+extern void tcpReceiveBytesEx( int portIndex, const uint8_t* buffer, ssize_t recvSize );
+extern uint32_t tcpRXBytesFree(int portIndex);
\ No newline at end of file
diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index 2839a338f22..b1352c1ce27 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -147,6 +147,10 @@
 
 #include "telemetry/telemetry.h"
 
+#if defined(SITL_BUILD)
+#include "target/SITL/serial_proxy.h"
+#endif
+
 #ifdef USE_HARDWARE_REVISION_DETECTION
 #include "hardware_revision.h"
 #endif
@@ -223,6 +227,10 @@ void init(void)
     flashDeviceInitialized = flashInit();
 #endif
 
+#if defined(SITL_BUILD)
+    serialProxyInit();
+#endif
+
     initEEPROM();
     ensureEEPROMContainsValidData();
     suspendRxSignal();
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index 246d65c0a7e..406622af38f 100755
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -93,6 +93,10 @@
 
 #include "config/feature.h"
 
+#if defined(SITL_BUILD)
+#include "target/SITL/serial_proxy.h"
+#endif
+
 void taskHandleSerial(timeUs_t currentTimeUs)
 {
     UNUSED(currentTimeUs);
@@ -421,6 +425,10 @@ void fcTasksInit(void)
 #if defined(USE_SMARTPORT_MASTER)
     setTaskEnabled(TASK_SMARTPORT_MASTER, true);
 #endif
+
+#if defined(SITL_BUILD)
+    serialProxyStart();
+#endif
 }
 
 cfTask_t cfTasks[TASK_COUNT] = {
diff --git a/src/main/main.c b/src/main/main.c
index c002fbab25c..c303602dcbc 100644
--- a/src/main/main.c
+++ b/src/main/main.c
@@ -27,6 +27,11 @@
 
 #include "scheduler/scheduler.h"
 
+#if defined(SITL_BUILD)
+#include "target/SITL/serial_proxy.h"
+#endif
+
+
 #ifdef SOFTSERIAL_LOOPBACK
 serialPort_t *loopbackPort;
 #endif
@@ -65,6 +70,9 @@ int main(void)
     loopbackInit();
 
     while (true) {
+#if defined(SITL_BUILD)
+        serialProxyProcess();
+#endif
         scheduler();
         processLoopback();
     }
diff --git a/src/main/rx/sim.c b/src/main/rx/sim.c
index a2a336f612e..62d9a9b864c 100644
--- a/src/main/rx/sim.c
+++ b/src/main/rx/sim.c
@@ -31,15 +31,14 @@
 
 static uint16_t channels[MAX_SUPPORTED_RC_CHANNEL_COUNT];
 static bool hasNewData = false;
+static uint16_t rssi = 0;
 
-static uint16_t rxSimReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t chan)
-{
+static uint16_t rxSimReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t chan) {
     UNUSED(rxRuntimeConfigPtr);
     return channels[chan];
 }
 
-void rxSimSetChannelValue(uint16_t* values, uint8_t count)
-{
+void rxSimSetChannelValue(uint16_t* values, uint8_t count) {
     for (size_t i = 0; i < count; i++) {    
         channels[i] = values[i];
     }
@@ -47,10 +46,11 @@ void rxSimSetChannelValue(uint16_t* values, uint8_t count)
     hasNewData = true;
 }
 
-static uint8_t rxSimFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
-{
+static uint8_t rxSimFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) {
     UNUSED(rxRuntimeConfig);
     
+    lqTrackerSet(rxRuntimeConfig->lqTracker, rssi);
+
     if (!hasNewData) {    
         return RX_FRAME_PENDING;
     }
@@ -59,8 +59,7 @@ static uint8_t rxSimFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
     return RX_FRAME_COMPLETE;
 }
 
-void rxSimInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
-{
+void rxSimInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) {
     UNUSED(rxConfig);
 
     rxRuntimeConfig->channelCount = MAX_SUPPORTED_RC_CHANNEL_COUNT;
@@ -68,4 +67,8 @@ void rxSimInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
     rxRuntimeConfig->rcReadRawFn = rxSimReadRawRC;
     rxRuntimeConfig->rcFrameStatusFn = rxSimFrameStatus;
 }
+
+void rxSimSetRssi(uint16_t value) {
+    rssi = value;
+}
 #endif
diff --git a/src/main/rx/sim.h b/src/main/rx/sim.h
index 88342acad73..9c8ff36d1b0 100644
--- a/src/main/rx/sim.h
+++ b/src/main/rx/sim.h
@@ -20,4 +20,5 @@
 #include "rx/rx.h"
 
 void rxSimSetChannelValue(uint16_t* values, uint8_t count);
+void rxSimSetRssi(uint16_t value);
 void rxSimInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
diff --git a/src/main/target/SITL/serial_proxy.c b/src/main/target/SITL/serial_proxy.c
new file mode 100644
index 00000000000..281fefd3ede
--- /dev/null
+++ b/src/main/target/SITL/serial_proxy.c
@@ -0,0 +1,784 @@
+/*
+ * 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 "serial_proxy.h"
+
+#if defined(SITL_BUILD)
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/select.h>
+#include <time.h>
+#include <unistd.h>
+
+#include "platform.h"
+
+#include <sys/types.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <unistd.h>
+#include <unistd.h>
+
+#include "drivers/time.h"
+#include "msp/msp_serial.h"
+#include "msp/msp_protocol.h"
+#include "common/crc.h"
+#include "rx/sim.h"
+
+#include <sys/ioctl.h>
+
+#ifdef  __FreeBSD__
+# define __BSD_VISIBLE 1
+#endif
+
+#ifdef __linux__
+#include <asm/termbits.h>
+#ifndef TCGETS2
+#include <asm-generic/ioctls.h>
+#endif
+#else
+#include <termios.h>
+#endif
+
+#ifdef __APPLE__
+#include <IOKit/serial/ioss.h>
+#endif
+
+#include "drivers/serial_tcp.h"
+
+#define SYM_BEGIN       '$'
+#define SYM_PROTO_V1    'M'
+#define SYM_PROTO_V2    'X'
+#define SYM_FROM_MWC    '>'
+#define SYM_TO_MWC      '<'
+#define SYM_UNSUPPORTED '!'
+
+#define JUMBO_FRAME_MIN_SIZE  255
+#define MAX_MSP_MESSAGE 1024
+#define RX_CONFIG_SIZE 24
+
+typedef enum {
+    DS_IDLE,
+    DS_PROTO_IDENTIFIER,
+    DS_DIRECTION_V1,
+    DS_DIRECTION_V2,
+    DS_FLAG_V2,
+    DS_PAYLOAD_LENGTH_V1,
+    DS_PAYLOAD_LENGTH_JUMBO_LOW,
+    DS_PAYLOAD_LENGTH_JUMBO_HIGH,
+    DS_PAYLOAD_LENGTH_V2_LOW,
+    DS_PAYLOAD_LENGTH_V2_HIGH,
+    DS_CODE_V1,
+    DS_CODE_JUMBO_V1,
+    DS_CODE_V2_LOW,
+    DS_CODE_V2_HIGH,
+    DS_PAYLOAD_V1,
+    DS_PAYLOAD_V2,
+    DS_CHECKSUM_V1,
+    DS_CHECKSUM_V2,
+} TDecoderState;
+
+static TDecoderState decoderState = DS_IDLE;
+
+typedef enum {
+    RXC_IDLE = 0,
+    RXC_RQ   = 1,
+    RXC_DONE = 2
+} TRXConfigState;
+
+static TRXConfigState rxConfigState = RXC_IDLE;
+
+static int message_length_expected;
+static unsigned char message_buffer[MAX_MSP_MESSAGE];
+static int message_length_received;
+static int unsupported;
+static int code;
+static int message_direction;
+static uint8_t message_checksum;
+static int reqCount = 0;
+static uint16_t rssi = 0;
+static uint8_t rxConfigBuffer[RX_CONFIG_SIZE];
+
+static timeMs_t lastWarning = 0;
+
+int serialUartIndex = -1;
+char serialPort[64] = "";
+int serialBaudRate = 115200;
+OptSerialStopBits_e serialStopBits = OPT_SERIAL_STOP_BITS_ONE;  //0:None|1:One|2:OnePointFive|3:Two
+OptSerialParity_e serialParity = OPT_SERIAL_PARITY_NONE;
+bool serialFCProxy = false;
+
+#define FC_PROXY_REQUEST_PERIOD_MIN_MS 20  //inav can handle 100 msp messages per second max
+#define FC_PROXY_REQUEST_PERIOD_TIMEOUT_MS 200
+#define FC_PROXY_REQUEST_PERIOD_RSSI_MS 300
+#define SERIAL_BUFFER_SIZE 256
+
+#if defined(__CYGWIN__)
+#include <windows.h>
+static HANDLE hSerial;
+#else
+static int fd;
+#endif
+
+static bool connected = false;
+static bool started = false;
+
+static timeMs_t nextProxyRequestTimeout = 0;
+static timeMs_t nextProxyRequestMin = 0;
+static timeMs_t nextProxyRequestRssi = 0;
+
+static timeMs_t lastVisit = 0;
+
+#if !defined(__CYGWIN__)
+#if !defined( __linux__) && !defined(__APPLE__)
+static int rate_to_constant(int baudrate)
+{
+#ifdef __FreeBSD__
+    return baudrate;
+#else
+#define B(x) case x: return B##x
+    switch (baudrate) {
+            B(50);
+            B(75);
+            B(110);
+            B(134);
+            B(150);
+            B(200);
+            B(300);
+            B(600);
+            B(1200);
+            B(1800);
+            B(2400);
+            B(4800);
+            B(9600);
+            B(19200);
+            B(38400);
+            B(57600);
+            B(115200);
+            B(230400);
+        default:
+            return 0;
+    }
+#undef B
+#endif
+}
+#endif
+
+static void close_serial(void)
+{
+#ifdef __linux__
+    ioctl(fd, TCFLSH, TCIOFLUSH);
+#else
+    tcflush(fd, TCIOFLUSH);
+#endif
+    close(fd);
+}
+
+static int set_fd_speed(void)
+{
+    int res = -1;
+#ifdef __linux__
+    // Just user BOTHER for everything (allows non-standard speeds)
+    struct termios2 t;
+    if ((res = ioctl(fd, TCGETS2, &t)) != -1) {
+        t.c_cflag &= ~CBAUD;
+        t.c_cflag |= BOTHER;
+        t.c_ospeed = t.c_ispeed = serialBaudRate;
+        res = ioctl(fd, TCSETS2, &t);
+    }
+#elif __APPLE__
+    speed_t speed = serialBaudRate;
+    res = ioctl(fd, IOSSIOSPEED, &speed);
+#else
+    int speed = rate_to_constant(serialBaudRate);
+    struct termios term;
+    if (tcgetattr(fd, &term)) return -1;
+    if (speed == 0) {
+        res = -1;
+    } else {
+        if (cfsetispeed(&term, speed) != -1) {
+            cfsetospeed(&term, speed);
+            res = tcsetattr(fd, TCSANOW, &term);
+        }
+        if (res != -1) {
+            memset(&term, 0, sizeof(term));
+            res = (tcgetattr(fd, &term));
+        }
+    }
+#endif
+    return res;
+}
+
+static int set_attributes(void)
+{
+    struct termios tio;
+    memset (&tio, 0, sizeof(tio));
+    int res = -1;
+#ifdef __linux__
+    res = ioctl(fd, TCGETS, &tio);
+#else
+    res = tcgetattr(fd, &tio);
+#endif
+    if (res != -1) {
+        // cfmakeraw ...
+        tio.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
+        tio.c_oflag &= ~OPOST;
+        tio.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
+        tio.c_cflag &= ~(CSIZE | PARENB);
+        tio.c_cflag |=  CS8 | CREAD | CLOCAL;
+        tio.c_cc[VTIME] = 0;
+        tio.c_cc[VMIN] = 0;
+
+        switch (serialStopBits) {
+            case OPT_SERIAL_STOP_BITS_ONE:
+                tio.c_cflag &= ~CSTOPB;
+                break;
+            case OPT_SERIAL_STOP_BITS_TWO:
+                tio.c_cflag |= CSTOPB;
+                break;
+            case OPT_SERIAL_STOP_BITS_INVALID:
+                break;
+        }
+
+        switch (serialParity) {
+            case OPT_SERIAL_PARITY_EVEN:
+                tio.c_cflag |= PARENB;
+                tio.c_cflag &= ~PARODD;
+                break;
+            case OPT_SERIAL_PARITY_NONE:
+                tio.c_cflag &= ~PARENB;
+                tio.c_cflag &= ~PARODD;
+                break;
+            case OPT_SERIAL_PARITY_ODD:
+                tio.c_cflag |= PARENB;
+                tio.c_cflag |= PARODD;
+                break;
+            case OPT_SERIAL_PARITY_INVALID:
+                break;
+        }
+#ifdef __linux__
+        res = ioctl(fd, TCSETS, &tio);
+#else
+        res = tcsetattr(fd, TCSANOW, &tio);
+#endif
+    }
+    if (res != -1) {
+        res = set_fd_speed();
+    }
+    return res;
+}
+#endif
+
+void serialProxyInit(void)
+{
+    char portName[64 + 20];
+    if ( strlen(serialPort) < 1) {
+        return;
+    }
+    connected = false;
+
+#if defined(__CYGWIN__)
+    sprintf(portName, "\\\\.\\%s", serialPort);
+
+    hSerial = CreateFile(portName,
+                         GENERIC_READ | GENERIC_WRITE,
+                         0,
+                         NULL,
+                         OPEN_EXISTING,
+                         FILE_ATTRIBUTE_NORMAL,
+                         NULL);
+
+    if (hSerial == INVALID_HANDLE_VALUE) {
+        if (GetLastError() == ERROR_FILE_NOT_FOUND) {
+            fprintf(stderr, "[SERIALPROXY] ERROR: Sserial port was not attached. Reason: %s not available.\n", portName);
+        } else {
+            fprintf(stderr, "[SERIALPROXY] ERROR: Can not connect to serial port, unknown error.\n");
+        }
+        return;
+    } else {
+        DCB dcbSerialParams = { 0 };
+        if (!GetCommState(hSerial, &dcbSerialParams)) {
+            fprintf(stderr, "[SERIALPROXY] ALERT: failed to get current serial parameters!\n");
+        } else {
+            dcbSerialParams.BaudRate = serialBaudRate;
+            dcbSerialParams.ByteSize = 8;
+
+            // Disable software flow control (XON/XOFF)
+            dcbSerialParams.fOutX = FALSE;
+            dcbSerialParams.fInX = FALSE;
+
+            // Disable hardware flow control (RTS/CTS)
+            dcbSerialParams.fRtsControl = RTS_CONTROL_DISABLE;
+            dcbSerialParams.fDtrControl = DTR_CONTROL_DISABLE;
+
+            // Disable any special processing of bytes
+            dcbSerialParams.fBinary = TRUE;
+
+            switch (serialStopBits) {
+                case OPT_SERIAL_STOP_BITS_ONE:
+                    dcbSerialParams.StopBits = ONESTOPBIT;
+                    break;
+                case OPT_SERIAL_STOP_BITS_TWO:
+                    dcbSerialParams.StopBits = TWOSTOPBITS;
+                    break;
+                case OPT_SERIAL_STOP_BITS_INVALID:
+                    break;
+            }
+
+            switch (serialParity) {
+                case OPT_SERIAL_PARITY_EVEN:
+                    dcbSerialParams.Parity = EVENPARITY;
+                    break;
+                case OPT_SERIAL_PARITY_NONE:
+                    dcbSerialParams.Parity = NOPARITY;
+                    break;
+                case OPT_SERIAL_PARITY_ODD:
+                    dcbSerialParams.Parity = ODDPARITY;
+                    break;
+                case OPT_SERIAL_PARITY_INVALID:
+                    break;
+            }
+
+            if (!SetCommState(hSerial, &dcbSerialParams)) {
+                fprintf(stderr, "[SERIALPROXY] ALERT: Could not set Serial Port parameters\n");
+            } else {
+                COMMTIMEOUTS comTimeOut;
+                comTimeOut.ReadIntervalTimeout = MAXDWORD;
+                comTimeOut.ReadTotalTimeoutMultiplier = 0;
+                comTimeOut.ReadTotalTimeoutConstant = 0;
+                comTimeOut.WriteTotalTimeoutMultiplier = 0;
+                comTimeOut.WriteTotalTimeoutConstant = 0;
+                SetCommTimeouts(hSerial, &comTimeOut);
+            }
+        }
+    }
+#else
+    strcpy(portName, serialPort); // because, windows ...
+    fd = open(serialPort, O_RDWR | O_NOCTTY);
+    if (fd != -1) {
+        int res = 1;
+        res = set_attributes();
+        if (res == -1) {
+            fprintf(stderr, "[SERIALPROXY] ALERT: Failed to configure device: %s %s\n", portName, strerror(errno));
+            close(fd);
+            fd = -1;
+        }
+    } else {
+        fprintf(stderr, "[SERIALPROXY] ERROR: Can not connect to serial port %s\n", portName);
+        return;
+    }
+#endif
+    connected = true;
+
+    if ( !serialFCProxy ) {
+        fprintf(stderr, "[SERIALPROXY] Connected %s to UART%d\n", portName, serialUartIndex);
+    } else {
+        fprintf(stderr, "[SERIALPROXY] Using proxy flight controller on %s\n", portName);
+    }
+}
+
+void serialProxyStart(void)
+{
+    started = true;
+}
+
+void serialProxyClose(void)
+{
+    if (connected) {
+        connected = false;
+#if defined(__CYGWIN__)
+        CloseHandle(hSerial);
+#else
+        close_serial();
+#endif
+        started = false;
+        nextProxyRequestTimeout = 0;
+        nextProxyRequestMin = 0;
+        nextProxyRequestRssi = 0;
+        lastWarning = 0;
+        lastVisit = 0;
+    }
+}
+
+static bool canOutputWarning(void)
+{
+    if ( millis() > lastWarning ) {
+        lastWarning = millis() + 5000;
+        return true;
+    }
+    return false;
+}
+
+int serialProxyReadData(unsigned char *buffer, unsigned int nbChar)
+{
+    if (!connected) return 0;
+
+#if defined(__CYGWIN__)
+    COMSTAT status;
+    DWORD errors;
+    DWORD bytesRead;
+
+    ClearCommError(hSerial, &errors, &status);
+    if (status.cbInQue > 0) {
+        unsigned int toRead = (status.cbInQue > nbChar) ? nbChar : status.cbInQue;
+        if (ReadFile(hSerial, buffer, toRead, &bytesRead, NULL) && (bytesRead != 0)) {
+            return bytesRead;
+        }
+    }
+    return 0;
+#else
+    if (nbChar == 0) return 0;
+    int bytesRead = read(fd, buffer, nbChar);
+    return bytesRead;
+#endif
+}
+
+bool serialProxyWriteData(unsigned char *buffer, unsigned int nbChar)
+{
+    if (!connected) return false;
+
+#if defined(__CYGWIN__)
+    COMSTAT status;
+    DWORD errors;
+    DWORD bytesSent;
+    if (!WriteFile(hSerial, (void *)buffer, nbChar, &bytesSent, 0)) {
+        ClearCommError(hSerial, &errors, &status);
+        if ( canOutputWarning() ) {
+            fprintf(stderr, "[SERIALPROXY] ERROR: Can not write to serial port\n");
+        }
+        return false;
+    }
+    if ( bytesSent != nbChar ) {
+        if ( canOutputWarning() ) {
+            fprintf(stderr, "[SERIALPROXY] ERROR: Can not write to serial port\n");
+        }
+    }
+#else
+    ssize_t l = write(fd, buffer, nbChar);
+    if ( l != (ssize_t)nbChar ) {
+        if ( canOutputWarning() ) {
+            fprintf(stderr, "[SERIALPROXY] ERROR: Can not write to serial port\n");
+        }
+        return false;
+    }
+#endif
+    return true;
+}
+
+bool serialProxyIsConnected(void)
+{
+    return connected;
+}
+
+static void mspSendCommand(int cmd, unsigned char *data, int dataLen)
+{
+    uint8_t msgBuf[MAX_MSP_MESSAGE] = { '$', 'X', '<' };
+    int msgLen = 3;
+
+    mspHeaderV2_t *hdrV2 = (mspHeaderV2_t *)&msgBuf[msgLen];
+    hdrV2->flags = 0;
+    hdrV2->cmd = cmd;
+    hdrV2->size = dataLen;
+    msgLen += sizeof(mspHeaderV2_t);
+
+    for ( int i = 0; i < dataLen; i++ ) {
+        msgBuf[msgLen++] = data[i];
+    }
+
+    uint8_t crc = crc8_dvb_s2_update(0, (uint8_t *)hdrV2, sizeof(mspHeaderV2_t));
+    crc = crc8_dvb_s2_update(crc, data, dataLen);
+    msgBuf[msgLen] = crc;
+    msgLen++;
+
+    serialProxyWriteData((unsigned char *)&msgBuf, msgLen);
+}
+
+static void mspRequestChannels(void)
+{
+    mspSendCommand(MSP_RC, NULL, 0);
+}
+
+static void mspRequestRssi(void)
+{
+    mspSendCommand(MSP_ANALOG, NULL, 0);
+}
+
+static void requestRXConfigState(void)
+{
+    mspSendCommand(MSP_RX_CONFIG, NULL, 0);
+    rxConfigState = RXC_RQ;
+    fprintf(stderr, "[SERIALPROXY] Requesting RX config from proxy FC...\n");
+}
+
+static void processMessage(void)
+{
+    if ( code == MSP_RC ) {
+        if ( reqCount > 0 ) reqCount--;
+        int count = message_length_received / 2;
+        if ( count <= MAX_SUPPORTED_RC_CHANNEL_COUNT) {
+            uint16_t *channels = (uint16_t *)(&message_buffer[0]);
+            //AETR => AERT
+            uint v = channels[2];
+            channels[2] = channels[3];
+            channels[3] = v;
+            if ( rssi > 0 ) {
+                rxSimSetChannelValue(channels, count);
+            }
+        }
+    } else if ( code == MSP_ANALOG ) {
+        if ( reqCount > 0 ) reqCount--;
+        if ( message_length_received >= 7 ) {
+            rssi = *((uint16_t *)(&message_buffer[3]));
+            rxSimSetRssi( rssi );
+        }
+    } else if ( code == MSP_RX_CONFIG ) {
+        memcpy( &(rxConfigBuffer[0]), &(message_buffer[0]), RX_CONFIG_SIZE );
+        *((uint16_t *) & (rxConfigBuffer[1])) = 2500; //maxcheck
+        *((uint16_t *) & (rxConfigBuffer[5])) = 500; //mincheck
+
+        mspSendCommand( MSP_SET_RX_CONFIG, rxConfigBuffer, RX_CONFIG_SIZE );
+        rxConfigState = RXC_DONE;
+        fprintf(stderr, "[SERIALPROXY] Setting RX config on proxy FC...\n");
+    }
+}
+
+static void decodeProxyMessages(unsigned char *data, int len)
+{
+    for (int i = 0; i < len; i++) {
+        switch (decoderState) {
+            case DS_IDLE: // sync char 1
+                if (data[i] == SYM_BEGIN) {
+                    decoderState = DS_PROTO_IDENTIFIER;
+                }
+                break;
+
+            case DS_PROTO_IDENTIFIER: // sync char 2
+                switch (data[i]) {
+                    case SYM_PROTO_V1:
+                        decoderState = DS_DIRECTION_V1;
+                        break;
+                    case SYM_PROTO_V2:
+                        decoderState = DS_DIRECTION_V2;
+                        break;
+                    default:
+                        //unknown protocol
+                        decoderState = DS_IDLE;
+                }
+                break;
+
+            case DS_DIRECTION_V1: // direction (should be >)
+
+            case DS_DIRECTION_V2:
+                unsupported = 0;
+                switch (data[i]) {
+                    case SYM_FROM_MWC:
+                        message_direction = 1;
+                        break;
+                    case SYM_TO_MWC:
+                        message_direction = 0;
+                        break;
+                    case SYM_UNSUPPORTED:
+                        unsupported = 1;
+                        break;
+                }
+                decoderState = decoderState == DS_DIRECTION_V1 ? DS_PAYLOAD_LENGTH_V1 : DS_FLAG_V2;
+                break;
+
+            case DS_FLAG_V2:
+                // Ignored for now
+                decoderState = DS_CODE_V2_LOW;
+                break;
+
+            case DS_PAYLOAD_LENGTH_V1:
+                message_length_expected = data[i];
+
+                if (message_length_expected == JUMBO_FRAME_MIN_SIZE) {
+                    decoderState = DS_CODE_JUMBO_V1;
+                } else {
+                    message_length_received = 0;
+                    decoderState = DS_CODE_V1;
+                }
+                break;
+
+            case DS_PAYLOAD_LENGTH_V2_LOW:
+                message_length_expected = data[i];
+                decoderState = DS_PAYLOAD_LENGTH_V2_HIGH;
+                break;
+
+            case DS_PAYLOAD_LENGTH_V2_HIGH:
+                message_length_expected |= data[i] << 8;
+                message_length_received = 0;
+                if (message_length_expected <= MAX_MSP_MESSAGE) {
+                    decoderState = message_length_expected > 0 ? DS_PAYLOAD_V2 : DS_CHECKSUM_V2;
+                } else {
+                    //too large payload
+                    decoderState = DS_IDLE;
+                }
+                break;
+
+            case DS_CODE_V1:
+            case DS_CODE_JUMBO_V1:
+                code = data[i];
+                if (message_length_expected > 0) {
+                    // process payload
+                    if (decoderState == DS_CODE_JUMBO_V1) {
+                        decoderState = DS_PAYLOAD_LENGTH_JUMBO_LOW;
+                    } else {
+                        decoderState = DS_PAYLOAD_V1;
+                    }
+                } else {
+                    // no payload
+                    decoderState = DS_CHECKSUM_V1;
+                }
+                break;
+
+            case DS_CODE_V2_LOW:
+                code = data[i];
+                decoderState = DS_CODE_V2_HIGH;
+                break;
+
+            case DS_CODE_V2_HIGH:
+                code |= data[i] << 8;
+                decoderState = DS_PAYLOAD_LENGTH_V2_LOW;
+                break;
+
+            case DS_PAYLOAD_LENGTH_JUMBO_LOW:
+                message_length_expected = data[i];
+                decoderState = DS_PAYLOAD_LENGTH_JUMBO_HIGH;
+                break;
+
+            case DS_PAYLOAD_LENGTH_JUMBO_HIGH:
+                message_length_expected |= data[i] << 8;
+                message_length_received = 0;
+                decoderState = DS_PAYLOAD_V1;
+                break;
+
+            case DS_PAYLOAD_V1:
+            case DS_PAYLOAD_V2:
+                message_buffer[message_length_received] = data[i];
+                message_length_received++;
+
+                if (message_length_received >= message_length_expected) {
+                    decoderState = decoderState == DS_PAYLOAD_V1 ? DS_CHECKSUM_V1 : DS_CHECKSUM_V2;
+                }
+                break;
+
+            case DS_CHECKSUM_V1:
+                if (message_length_expected >= JUMBO_FRAME_MIN_SIZE) {
+                    message_checksum = JUMBO_FRAME_MIN_SIZE;
+                } else {
+                    message_checksum = message_length_expected;
+                }
+                message_checksum ^= code;
+                if (message_length_expected >= JUMBO_FRAME_MIN_SIZE) {
+                    message_checksum ^= message_length_expected & 0xFF;
+                    message_checksum ^= (message_length_expected & 0xFF00) >> 8;
+                }
+                for (int ii = 0; ii < message_length_received; ii++) {
+                    message_checksum ^= message_buffer[ii];
+                }
+                if (message_checksum == data[i]) {
+                    processMessage();
+                }
+                decoderState = DS_IDLE;
+                break;
+
+            case DS_CHECKSUM_V2:
+                message_checksum = 0;
+                message_checksum = crc8_dvb_s2(message_checksum, 0); // flag
+                message_checksum = crc8_dvb_s2(message_checksum, code & 0xFF);
+                message_checksum = crc8_dvb_s2(message_checksum, (code & 0xFF00) >> 8);
+                message_checksum = crc8_dvb_s2(message_checksum, message_length_expected & 0xFF);
+                message_checksum = crc8_dvb_s2(message_checksum, (message_length_expected & 0xFF00) >> 8);
+                for (int ii = 0; ii < message_length_received; ii++) {
+                    message_checksum = crc8_dvb_s2(message_checksum, message_buffer[ii]);
+                }
+                if (message_checksum == data[i]) {
+                    processMessage();
+                }
+                decoderState = DS_IDLE;
+                break;
+
+            default:
+                break;
+        }
+    }
+}
+
+void serialProxyProcess(void)
+{
+
+    if (!started) return;
+    if (!connected) return;
+
+    if ((millis() - lastVisit) > 500) {
+        if ( lastVisit > 0 ) {
+            fprintf(stderr, "timeout=%dms\n", millis() - lastVisit);
+        }
+    }
+    lastVisit = millis();
+
+    if ( serialFCProxy ) {
+        //first we have to change FC min_check and max_check thresholds to avoid activating stick commands on proxy FC
+        if ( rxConfigState == RXC_IDLE ) {
+            requestRXConfigState();
+        } else if ( rxConfigState == RXC_DONE ) {
+            if ( (nextProxyRequestTimeout <= millis()) || ((reqCount == 0) && (nextProxyRequestMin <= millis()))) {
+                nextProxyRequestTimeout = millis() + FC_PROXY_REQUEST_PERIOD_TIMEOUT_MS;
+                nextProxyRequestMin = millis() + FC_PROXY_REQUEST_PERIOD_MIN_MS;
+                mspRequestChannels();
+                reqCount++;
+            }
+
+            if ( nextProxyRequestRssi <= millis()) {
+                nextProxyRequestRssi = millis() + FC_PROXY_REQUEST_PERIOD_RSSI_MS;
+                mspRequestRssi();
+                reqCount++;
+            }
+        }
+
+        unsigned char buf[SERIAL_BUFFER_SIZE];
+        int count = serialProxyReadData(buf, SERIAL_BUFFER_SIZE);
+        if (count == 0) return;
+        decodeProxyMessages(buf, count);
+
+    } else {
+
+        if ( serialUartIndex == -1 )  return;
+        unsigned char buf[SERIAL_BUFFER_SIZE];
+        uint32_t avail = tcpRXBytesFree(serialUartIndex - 1);
+        if ( avail == 0 ) return;
+        if (avail > SERIAL_BUFFER_SIZE) avail = SERIAL_BUFFER_SIZE;
+
+        int count = serialProxyReadData(buf, avail);
+        if (count == 0) return;
+
+        tcpReceiveBytesEx( serialUartIndex - 1, buf, count);
+    }
+
+}
+#endif
diff --git a/src/main/target/SITL/serial_proxy.h b/src/main/target/SITL/serial_proxy.h
new file mode 100644
index 00000000000..5cb97516399
--- /dev/null
+++ b/src/main/target/SITL/serial_proxy.h
@@ -0,0 +1,64 @@
+/*
+ * 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 <stdint.h>
+#include <stdbool.h>
+#include <stddef.h>
+
+#include <platform.h>
+
+#if defined(SITL_BUILD)
+
+typedef enum
+{
+    OPT_SERIAL_STOP_BITS_ONE,
+    OPT_SERIAL_STOP_BITS_TWO,
+    OPT_SERIAL_STOP_BITS_INVALID
+} OptSerialStopBits_e;
+
+typedef enum
+{
+    OPT_SERIAL_PARITY_EVEN,
+    OPT_SERIAL_PARITY_NONE,
+    OPT_SERIAL_PARITY_ODD,
+    OPT_SERIAL_PARITY_INVALID
+} OptSerialParity_e;
+
+
+extern int serialUartIndex; ///1 for UART1
+extern char serialPort[64];
+extern int serialBaudRate;
+extern OptSerialStopBits_e serialStopBits;
+extern OptSerialParity_e serialParity;
+extern bool serialFCProxy;
+
+extern void serialProxyInit(void);
+extern void serialProxyStart(void);
+extern void serialProxyProcess(void);
+extern void serialProxyClose(void);
+extern bool serialProxyIsConnected(void);
+extern bool serialProxyWriteData(unsigned char *buffer, unsigned int nbChar);
+
+#endif
\ No newline at end of file
diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c
index e8861eec56d..e473c636981 100644
--- a/src/main/target/SITL/target.c
+++ b/src/main/target/SITL/target.c
@@ -56,6 +56,8 @@
 #include "target/SITL/sim/realFlight.h"
 #include "target/SITL/sim/xplane.h"
 
+#include "target/SITL/serial_proxy.h"
+
 // More dummys
 const int timerHardwareCount = 0;
 timerHardware_t timerHardware[1];
@@ -170,19 +172,47 @@ bool parseMapping(char* mapStr)
     return true;
 }
 
+OptSerialStopBits_e parseStopBits(const char* optarg){
+    if ( strcmp(optarg, "One") == 0 ) {
+        return OPT_SERIAL_STOP_BITS_ONE;
+    } else if ( strcmp(optarg, "Two") == 0 ) {
+        return OPT_SERIAL_STOP_BITS_TWO;
+    } else  {
+        return OPT_SERIAL_STOP_BITS_INVALID;
+    }
+}
+
+OptSerialParity_e parseParity(const char* optarg){
+    if ( strcmp(optarg, "Even") == 0 ) {
+        return OPT_SERIAL_PARITY_EVEN;
+    } else if ( strcmp(optarg, "None") == 0 ) {
+        return OPT_SERIAL_PARITY_NONE;
+    } else if ( strcmp(optarg, "Odd") == 0 ) {
+        return OPT_SERIAL_PARITY_ODD;
+    } else  {
+        return OPT_SERIAL_PARITY_INVALID;
+    }
+}
+
 void printCmdLineOptions(void)
 {
     printVersion();
     fprintf(stderr, "Avaiable options:\n");
-    fprintf(stderr, "--path=[path]                        Path and filename of eeprom.bin. If not specified 'eeprom.bin' in program directory is used.\n");
-    fprintf(stderr, "--sim=[rf|xp]                        Simulator interface: rf = RealFligt, xp = XPlane. Example: --sim=rf\n");
-    fprintf(stderr, "--simip=[ip]                         IP-Address oft the simulator host. If not specified localhost (127.0.0.1) is used.\n");
-    fprintf(stderr, "--simport=[port]                     Port oft the simulator host.\n");
-    fprintf(stderr, "--useimu                             Use IMU sensor data from the simulator instead of using attitude data from the simulator directly (experimental, not recommended).\n");
-    fprintf(stderr, "--chanmap=[mapstring]                Channel mapping. Maps INAVs motor and servo PWM outputs to the virtual receiver output in the simulator.\n");
-    fprintf(stderr, "                                     The mapstring has the following format: M(otor)|S(servo)<INAV-OUT>-<RECEIVER-OUT>,... All numbers must have two digits\n");
-    fprintf(stderr, "                                     For example: Map motor 1 to virtal receiver output 1, servo 1 to output 2 and servo 2 to output 3:\n");
-    fprintf(stderr, "                                     --chanmap=M01-01,S01-02,S02-03\n");
+    fprintf(stderr, "--path=[path]                  Path and filename of eeprom.bin. If not specified 'eeprom.bin' in program directory is used.\n");
+    fprintf(stderr, "--sim=[rf|xp]                  Simulator interface: rf = RealFligt, xp = XPlane. Example: --sim=rf\n");
+    fprintf(stderr, "--simip=[ip]                   IP-Address oft the simulator host. If not specified localhost (127.0.0.1) is used.\n");
+    fprintf(stderr, "--simport=[port]               Port oft the simulator host.\n");
+    fprintf(stderr, "--useimu                       Use IMU sensor data from the simulator instead of using attitude data from the simulator directly (experimental, not recommended).\n");
+    fprintf(stderr, "--serialuart=[uart]            UART number on which serial receiver is configured in SITL, f.e. 3 for UART3\n");
+    fprintf(stderr, "--serialport=[serialport]      Host's serial port to which serial receiver/proxy FC is connected, f.e. COM3, /dev/ttyACM3\n");
+    fprintf(stderr, "--baudrate=[baudrate]          Serial receiver baudrate (default: 115200).\n");
+    fprintf(stderr, "--stopbits=[None|One|Two]      Serial receiver stopbits (default: One).\n");
+    fprintf(stderr, "--parity=[Even|None|Odd]       Serial receiver parity (default: None).\n");
+    fprintf(stderr, "--fcproxy                      Use inav/betaflight FC as a proxy for serial receiver.\n");
+    fprintf(stderr, "--chanmap=[mapstring]          Channel mapping. Maps INAVs motor and servo PWM outputs to the virtual receiver output in the simulator.\n");
+    fprintf(stderr, "                               The mapstring has the following format: M(otor)|S(servo)<INAV-OUT>-<RECEIVER-OUT>,... All numbers must have two digits\n");
+    fprintf(stderr, "                               For example: Map motor 1 to virtal receiver output 1, servo 1 to output 2 and servo 2 to output 3:\n");
+    fprintf(stderr, "                               --chanmap=M01-01,S01-02,S02-03\n");
 }
 
 void parseArguments(int argc, char *argv[])
@@ -202,7 +232,13 @@ void parseArguments(int argc, char *argv[])
             {"simport", required_argument, 0, 'p'},
             {"help", no_argument, 0, 'h'},
             {"path", required_argument, 0, 'e'},
-	    {"version", no_argument, 0, 'v'},
+            {"version", no_argument, 0, 'v'},
+            {"serialuart", required_argument, 0, '0'},
+            {"serialport", required_argument, 0, '1'},
+            {"baudrate", required_argument, 0, '2'},
+            {"stopbits", required_argument, 0, '3'},
+            {"parity", required_argument, 0, '4'},
+            {"fcproxy", no_argument, 0, '5'},
             {NULL, 0, NULL, 0}
         };
 
@@ -242,10 +278,54 @@ void parseArguments(int argc, char *argv[])
                     fprintf(stderr, "[EEPROM] Invalid path, using eeprom file in program directory\n.");
                 }
                 break;
-	    case 'v':
-		printVersion();
-		exit(0);
-	    default:
+            case 'v':
+                printVersion();
+                exit(0);
+            case '0':
+                serialUartIndex = atoi(optarg);
+                if ( (serialUartIndex<1) || (serialUartIndex>8) ) {
+                    fprintf(stderr, "[serialuart] Invalid argument\n.");
+                    exit(0);
+                }
+                break;
+            case '1':
+                if ( (strlen(optarg)<1) || (strlen(optarg)>63) ) {
+                    fprintf(stderr, "[serialport] Invalid argument\n.");
+                    exit(0);
+                } else {
+                    strcpy( serialPort, optarg );
+                }
+                break;
+            case '2':
+                serialBaudRate = atoi(optarg);
+                if ( serialBaudRate < 1200 )
+                {
+                    fprintf(stderr, "[baudrate] Invalid argument\n.");
+                    exit(0);
+                }
+                break;
+            case '3':
+                serialStopBits = parseStopBits(optarg);
+                if ( serialStopBits == OPT_SERIAL_STOP_BITS_INVALID )
+                {
+                    fprintf(stderr, "[stopbits] Invalid argument\n.");
+                    exit(0);
+                }
+                break;
+
+            case '4':
+                serialParity = parseParity(optarg);
+                if ( serialParity== OPT_SERIAL_PARITY_INVALID )
+                {
+                    fprintf(stderr, "[parity] Invalid argument\n.");
+                    exit(0);
+                }
+                break;
+            case '5':
+                serialFCProxy = true;
+                break;
+
+            default:
                 printCmdLineOptions();
                 exit(0);
         }
@@ -304,6 +384,7 @@ void systemReset(void)
 #else
     closefrom(3);
 #endif
+    serialProxyClose();
     execvp(c_argv[0], c_argv); // restart
 }
 

From f3f0397e07003eb3dc510f23e27fdd07192a2fdd Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Mon, 29 Apr 2024 15:02:20 +0200
Subject: [PATCH 080/323] Revert enforcing mode on TIM3

---
 src/main/target/MAMBAF405_2022A/config.c | 3 ---
 1 file changed, 3 deletions(-)

diff --git a/src/main/target/MAMBAF405_2022A/config.c b/src/main/target/MAMBAF405_2022A/config.c
index ba3a5344c20..d632230c240 100644
--- a/src/main/target/MAMBAF405_2022A/config.c
+++ b/src/main/target/MAMBAF405_2022A/config.c
@@ -57,7 +57,4 @@ void targetConfiguration(void)
     
     serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_MSP;
     serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].msp_baudrateIndex = BAUD_115200;
-
-    // To improve backwards compatibility with INAV versions 6.x and older
-    timerOverridesMutable(timer2id(TIM3))->outputMode = OUTPUT_MODE_MOTORS;
 }

From ab7ab945a2f1b45578ff54baab1e3256b7310eff Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Mon, 29 Apr 2024 17:37:25 +0200
Subject: [PATCH 081/323] Looks like inav never had the pins for the NANO
 variant correct.

---
 src/main/target/FLYWOOF745/target.c | 29 +++++++++++++++++++----------
 1 file changed, 19 insertions(+), 10 deletions(-)

diff --git a/src/main/target/FLYWOOF745/target.c b/src/main/target/FLYWOOF745/target.c
index a82a708f51a..04c083ac94f 100644
--- a/src/main/target/FLYWOOF745/target.c
+++ b/src/main/target/FLYWOOF745/target.c
@@ -30,17 +30,26 @@
 #include "drivers/pinio.h"
 
 timerHardware_t timerHardware[] = {
-    // DEF_TIM(TIM1, CH3, PE13, TIM_USE_PPM,                      0, 1), // PPM, DMA2_ST6
+// DEF_TIM(TIM1, CH3, PE13, TIM_USE_PPM,                      0, 1), // PPM,
+// DMA2_ST6
 
-    DEF_TIM(TIM3, CH4, PB1,  TIM_USE_OUTPUT_AUTO,    0, 0), // M1 , DMA1_ST2
-    DEF_TIM(TIM8, CH4, PC9,  TIM_USE_OUTPUT_AUTO,    0, 0), // M2 , DMA2_ST7
-    DEF_TIM(TIM3, CH3, PB0,  TIM_USE_OUTPUT_AUTO,    0, 0), // M3 , DMA1_ST7
-    DEF_TIM(TIM1, CH2, PE11, TIM_USE_OUTPUT_AUTO,    0, 1), // M4 , DMA2_ST4
-    DEF_TIM(TIM1, CH1, PE9,  TIM_USE_OUTPUT_AUTO,    0, 2), // M5 , DMA2_ST2
-    DEF_TIM(TIM5, CH4, PA3,  TIM_USE_OUTPUT_AUTO,    0, 0), // M6 , DMA1_ST1
-    DEF_TIM(TIM3, CH1, PB4,  TIM_USE_OUTPUT_AUTO,    0, 0), // M7 , DMA1_ST4
-    DEF_TIM(TIM3, CH2, PB5,  TIM_USE_OUTPUT_AUTO,    0, 0), // M8 , DMA1_ST5
-    DEF_TIM(TIM4, CH1, PD12, TIM_USE_LED,            0, 0), // LED_STRIP, DMA1_ST0
+#ifdef FLYWOOF745
+    DEF_TIM(TIM3, CH4, PB1,  TIM_USE_OUTPUT_AUTO, 0, 0),   // M1 , DMA1_ST2
+    DEF_TIM(TIM8, CH4, PC9,  TIM_USE_OUTPUT_AUTO, 0, 0),   // M2 , DMA2_ST7
+    DEF_TIM(TIM3, CH3, PB0,  TIM_USE_OUTPUT_AUTO, 0, 0),   // M3 , DMA1_ST7
+    DEF_TIM(TIM1, CH2, PE11, TIM_USE_OUTPUT_AUTO, 0, 1),  // M4 , DMA2_ST4
+    DEF_TIM(TIM1, CH1, PE9,  TIM_USE_OUTPUT_AUTO, 0, 2),   // M5 , DMA2_ST2
+#else /* FLYWOOF745NANO */
+    DEF_TIM(TIM3, CH3, PB0,  TIM_USE_OUTPUT_AUTO, 0, 0),   // M1 , DMA1_ST7
+    DEF_TIM(TIM3, CH4, PB1,  TIM_USE_OUTPUT_AUTO, 0, 0),   // M2 , DMA1_ST2
+    DEF_TIM(TIM1, CH1, PE9,  TIM_USE_OUTPUT_AUTO, 0, 2),   // M3 , DMA2_ST2
+    DEF_TIM(TIM1, CH2, PE11, TIM_USE_OUTPUT_AUTO, 0, 1),  // M4 , DMA2_ST4
+    DEF_TIM(TIM8, CH4, PC9,  TIM_USE_OUTPUT_AUTO, 0, 0),   // M5 , DMA2_ST7
+#endif
+    DEF_TIM(TIM5, CH4, PA3,  TIM_USE_OUTPUT_AUTO, 0, 0),  // M6 , DMA1_ST1
+    DEF_TIM(TIM3, CH1, PB4,  TIM_USE_OUTPUT_AUTO, 0, 0),  // M7 , DMA1_ST4
+    DEF_TIM(TIM3, CH2, PB5,  TIM_USE_OUTPUT_AUTO, 0, 0),  // M8 , DMA1_ST5
+    DEF_TIM(TIM4, CH1, PD12, TIM_USE_LED, 0, 0),         // LED_STRIP, DMA1_ST0
 };
 
 const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

From 72b5fab0cc7977ad7662690d7bb4f306fe2ba36f Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Mon, 29 Apr 2024 15:02:20 +0200
Subject: [PATCH 082/323] Revert enforcing mode on TIM3

---
 src/main/target/MAMBAF405_2022A/config.c | 3 ---
 1 file changed, 3 deletions(-)

diff --git a/src/main/target/MAMBAF405_2022A/config.c b/src/main/target/MAMBAF405_2022A/config.c
index ba3a5344c20..d632230c240 100644
--- a/src/main/target/MAMBAF405_2022A/config.c
+++ b/src/main/target/MAMBAF405_2022A/config.c
@@ -57,7 +57,4 @@ void targetConfiguration(void)
     
     serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_MSP;
     serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].msp_baudrateIndex = BAUD_115200;
-
-    // To improve backwards compatibility with INAV versions 6.x and older
-    timerOverridesMutable(timer2id(TIM3))->outputMode = OUTPUT_MODE_MOTORS;
 }

From 14755bdd566014bcff0786b0e8f75cea9f0241ed Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Mon, 29 Apr 2024 19:48:50 +0200
Subject: [PATCH 083/323] Unify the gyro filter settings

---
 docs/Settings.md              | 88 +++++++++++++++++++++++++++++++----
 src/main/fc/settings.yaml     | 19 ++++----
 src/main/flight/dynamic_lpf.c |  2 +-
 src/main/flight/ez_tune.c     |  2 +-
 src/main/sensors/gyro.c       |  7 ++-
 src/main/sensors/gyro.h       |  9 +++-
 6 files changed, 99 insertions(+), 28 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index 7ed9437ced1..3c6037fc31c 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -1562,6 +1562,76 @@ For developer ground test use. Disables motors, sets heading status = Trusted on
 
 ---
 
+### gyro_adaptive_filter_hpf_hz
+
+High pass filter cutoff frequency
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 10 | 1 | 50 |
+
+---
+
+### gyro_adaptive_filter_integrator_threshold_high
+
+High threshold for adaptive filter integrator
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 4 | 1 | 10 |
+
+---
+
+### gyro_adaptive_filter_integrator_threshold_low
+
+Low threshold for adaptive filter integrator
+
+| Default | Min | Max |
+| --- | --- | --- |
+| -2 | -10 | 0 |
+
+---
+
+### gyro_adaptive_filter_max_hz
+
+Maximum frequency for adaptive filter
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 150 | 100 | 500 |
+
+---
+
+### gyro_adaptive_filter_min_hz
+
+Minimum frequency for adaptive filter
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 50 | 30 | 250 |
+
+---
+
+### gyro_adaptive_filter_std_lpf_hz
+
+Standard deviation low pass filter cutoff frequency
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 2 | 0 | 10 |
+
+---
+
+### gyro_adaptive_filter_target
+
+Target value for adaptive filter
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 3.5 | 1 | 6 |
+
+---
+
 ### gyro_anti_aliasing_lpf_hz
 
 Gyro processing anti-aliasing filter cutoff frequency. In normal operation this filter setting should never be changed. In Hz
@@ -1602,33 +1672,33 @@ Minimum frequency of the gyro Dynamic LPF
 
 ---
 
-### gyro_main_lpf_hz
+### gyro_filter_mode
 
-Software based gyro main lowpass filter. Value is cutoff frequency (Hz)
+Specifies the type of the software LPF of the gyro signals.
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 60 | 0 | 500 |
+| STATIC |  |  |
 
 ---
 
-### gyro_to_use
+### gyro_main_lpf_hz
 
-On multi-gyro targets, allows to choose which gyro to use. 0 = first gyro, 1 = second gyro
+Software based gyro main lowpass filter. Value is cutoff frequency (Hz)
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 0 | 0 | 2 |
+| 60 | 0 | 500 |
 
 ---
 
-### gyro_use_dyn_lpf
+### gyro_to_use
 
-Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro LPF based on the throttle position.
+On multi-gyro targets, allows to choose which gyro to use. 0 = first gyro, 1 = second gyro
 
 | Default | Min | Max |
 | --- | --- | --- |
-| OFF | OFF | ON |
+| 0 | 0 | 2 |
 
 ---
 
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 40ca93c829d..f5e8df233e3 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -191,6 +191,9 @@ tables:
   - name: led_pin_pwm_mode
     values: ["SHARED_LOW", "SHARED_HIGH", "LOW", "HIGH"]
     enum: led_pin_pwm_mode_e
+  - name: gyro_filter_mode
+    values: ["STATIC", "DYNAMIC", "ADAPTIVE"]
+    enum: gyroFilterType_e
 
 constants:
   RPYL_PID_MIN: 0
@@ -226,11 +229,11 @@ groups:
         field: gyro_main_lpf_hz
         min: 0
         max: 500
-      - name: gyro_use_dyn_lpf
-        description: "Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro LPF based on the throttle position."
-        default_value: OFF
-        field: useDynamicLpf
-        type: bool
+      - name: gyro_filter_mode
+        description: "Specifies the type of the software LPF of the gyro signals."
+        default_value: "STATIC"
+        field: gyroFilterMode
+        table: gyro_filter_mode
       - name: gyro_dyn_lpf_min_hz
         description: "Minimum frequency of the gyro Dynamic LPF"
         default_value: 200
@@ -330,12 +333,6 @@ groups:
         field: gravity_cmss_cal
         min: 0
         max: 2000
-      - name: gyro_adaptive_filter
-        description: "Enable adaptive filter for gyro data"
-        default_value: OFF
-        field: adaptiveFilterEnabled
-        type: bool
-        condition: USE_ADAPTIVE_FILTER
       - name: gyro_adaptive_filter_target
         description: "Target value for adaptive filter"
         default_value: 3.5
diff --git a/src/main/flight/dynamic_lpf.c b/src/main/flight/dynamic_lpf.c
index 0e5b4f6ef28..31e594312ab 100644
--- a/src/main/flight/dynamic_lpf.c
+++ b/src/main/flight/dynamic_lpf.c
@@ -37,7 +37,7 @@ static float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLp
 
 void dynamicLpfGyroTask(void) {
 
-    if (!gyroConfig()->useDynamicLpf) {
+    if (!gyroConfig()->gyroFilterMode != GYRO_FILTER_MODE_DYNAMIC) {
         return;
     }
 
diff --git a/src/main/flight/ez_tune.c b/src/main/flight/ez_tune.c
index 4bde5b645e8..e67e3fe9b47 100644
--- a/src/main/flight/ez_tune.c
+++ b/src/main/flight/ez_tune.c
@@ -109,7 +109,7 @@ void ezTuneUpdate(void) {
 #endif
 
         //Disable dynamic LPF
-        gyroConfigMutable()->useDynamicLpf = 0;
+        gyroConfigMutable()->gyroFilterMode = GYRO_FILTER_MODE_STATIC;
 
         //Setup PID controller
 
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index fd2036ef9a2..7f7d1d171d9 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -96,7 +96,7 @@ EXTENDED_FASTRAM secondaryDynamicGyroNotchState_t secondaryDynamicGyroNotchState
 
 #endif
 
-PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 9);
+PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 10);
 
 PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .gyro_anti_aliasing_lpf_hz = SETTING_GYRO_ANTI_ALIASING_LPF_HZ_DEFAULT,
@@ -105,7 +105,6 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .gyro_to_use = SETTING_GYRO_TO_USE_DEFAULT,
 #endif
     .gyro_main_lpf_hz = SETTING_GYRO_MAIN_LPF_HZ_DEFAULT,
-    .useDynamicLpf = SETTING_GYRO_USE_DYN_LPF_DEFAULT,
     .gyroDynamicLpfMinHz = SETTING_GYRO_DYN_LPF_MIN_HZ_DEFAULT,
     .gyroDynamicLpfMaxHz = SETTING_GYRO_DYN_LPF_MAX_HZ_DEFAULT,
     .gyroDynamicLpfCurveExpo = SETTING_GYRO_DYN_LPF_CURVE_EXPO_DEFAULT,
@@ -124,7 +123,6 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .gyro_zero_cal = {SETTING_GYRO_ZERO_X_DEFAULT, SETTING_GYRO_ZERO_Y_DEFAULT, SETTING_GYRO_ZERO_Z_DEFAULT},
     .gravity_cmss_cal = SETTING_INS_GRAVITY_CMSS_DEFAULT,
 #ifdef USE_ADAPTIVE_FILTER
-    .adaptiveFilterEnabled = SETTING_GYRO_ADAPTIVE_FILTER_DEFAULT,
     .adaptiveFilterTarget = SETTING_GYRO_ADAPTIVE_FILTER_TARGET_DEFAULT,
     .adaptiveFilterMinHz = SETTING_GYRO_ADAPTIVE_FILTER_MIN_HZ_DEFAULT,
     .adaptiveFilterMaxHz = SETTING_GYRO_ADAPTIVE_FILTER_MAX_HZ_DEFAULT,
@@ -132,6 +130,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .adaptiveFilterHpfHz = SETTING_GYRO_ADAPTIVE_FILTER_HPF_HZ_DEFAULT,
     .adaptiveFilterIntegratorThresholdHigh = SETTING_GYRO_ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD_HIGH_DEFAULT,
     .adaptiveFilterIntegratorThresholdLow  = SETTING_GYRO_ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD_LOW_DEFAULT,
+    .gyroFilterMode = SETTING_GYRO_FILTER_MODE_DEFAULT,
 #endif
 );
 
@@ -261,7 +260,7 @@ static void gyroInitFilters(void)
     initGyroFilter(&gyroLpf2ApplyFn, gyroLpf2State, gyroConfig()->gyro_main_lpf_hz, getLooptime());
 
 #ifdef USE_ADAPTIVE_FILTER
-    if (gyroConfig()->adaptiveFilterEnabled) {
+    if (gyroConfig()->gyroFilterMode == GYRO_FILTER_MODE_ADAPTIVE) {
         adaptiveFilterSetDefaultFrequency(gyroConfig()->gyro_main_lpf_hz, gyroConfig()->adaptiveFilterMinHz, gyroConfig()->adaptiveFilterMaxHz);
     }
 #endif
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index 75ff7983372..7f767e370d1 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -52,6 +52,12 @@ typedef enum {
     DYNAMIC_NOTCH_MODE_3D
 } dynamicGyroNotchMode_e;
 
+typedef enum {
+    GYRO_FILTER_MODE_STATIC = 0,
+    GYRO_FILTER_MODE_DYNAMIC = 1,
+    GYRO_FILTER_MODE_ADAPTIVE = 2
+} gyroFilterMode_e;
+
 typedef struct gyro_s {
     bool initialized;
     uint32_t targetLooptime;
@@ -69,7 +75,6 @@ typedef struct gyroConfig_s {
     uint8_t  gyro_to_use;
 #endif
     uint16_t gyro_main_lpf_hz;
-    uint8_t useDynamicLpf;
     uint16_t gyroDynamicLpfMinHz;
     uint16_t gyroDynamicLpfMaxHz;
     uint8_t gyroDynamicLpfCurveExpo;
@@ -88,7 +93,6 @@ typedef struct gyroConfig_s {
     int16_t gyro_zero_cal[XYZ_AXIS_COUNT];
     float gravity_cmss_cal;
 #ifdef USE_ADAPTIVE_FILTER
-    uint8_t adaptiveFilterEnabled;
     float adaptiveFilterTarget;
     uint16_t adaptiveFilterMinHz;
     uint16_t adaptiveFilterMaxHz;
@@ -96,6 +100,7 @@ typedef struct gyroConfig_s {
     float adaptiveFilterHpfHz;
     float adaptiveFilterIntegratorThresholdHigh;
     float adaptiveFilterIntegratorThresholdLow;
+    uint8_t gyroFilterMode;
 #endif
 } gyroConfig_t;
 

From 0bf3d3c3c421c0759ab97f14567361688b2924db Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Pawe=C5=82=20Spychalski?= <pspychalski@gmail.com>
Date: Mon, 29 Apr 2024 19:34:18 +0200
Subject: [PATCH 084/323] Merge pull request #9990 from
 iNavFlight/mmosca-flywoof745-fixes

Match F745 and F745NANO motor pins to BF target
---
 src/main/target/FLYWOOF745/target.c | 29 +++++++++++++++++++----------
 1 file changed, 19 insertions(+), 10 deletions(-)

diff --git a/src/main/target/FLYWOOF745/target.c b/src/main/target/FLYWOOF745/target.c
index a82a708f51a..04c083ac94f 100644
--- a/src/main/target/FLYWOOF745/target.c
+++ b/src/main/target/FLYWOOF745/target.c
@@ -30,17 +30,26 @@
 #include "drivers/pinio.h"
 
 timerHardware_t timerHardware[] = {
-    // DEF_TIM(TIM1, CH3, PE13, TIM_USE_PPM,                      0, 1), // PPM, DMA2_ST6
+// DEF_TIM(TIM1, CH3, PE13, TIM_USE_PPM,                      0, 1), // PPM,
+// DMA2_ST6
 
-    DEF_TIM(TIM3, CH4, PB1,  TIM_USE_OUTPUT_AUTO,    0, 0), // M1 , DMA1_ST2
-    DEF_TIM(TIM8, CH4, PC9,  TIM_USE_OUTPUT_AUTO,    0, 0), // M2 , DMA2_ST7
-    DEF_TIM(TIM3, CH3, PB0,  TIM_USE_OUTPUT_AUTO,    0, 0), // M3 , DMA1_ST7
-    DEF_TIM(TIM1, CH2, PE11, TIM_USE_OUTPUT_AUTO,    0, 1), // M4 , DMA2_ST4
-    DEF_TIM(TIM1, CH1, PE9,  TIM_USE_OUTPUT_AUTO,    0, 2), // M5 , DMA2_ST2
-    DEF_TIM(TIM5, CH4, PA3,  TIM_USE_OUTPUT_AUTO,    0, 0), // M6 , DMA1_ST1
-    DEF_TIM(TIM3, CH1, PB4,  TIM_USE_OUTPUT_AUTO,    0, 0), // M7 , DMA1_ST4
-    DEF_TIM(TIM3, CH2, PB5,  TIM_USE_OUTPUT_AUTO,    0, 0), // M8 , DMA1_ST5
-    DEF_TIM(TIM4, CH1, PD12, TIM_USE_LED,            0, 0), // LED_STRIP, DMA1_ST0
+#ifdef FLYWOOF745
+    DEF_TIM(TIM3, CH4, PB1,  TIM_USE_OUTPUT_AUTO, 0, 0),   // M1 , DMA1_ST2
+    DEF_TIM(TIM8, CH4, PC9,  TIM_USE_OUTPUT_AUTO, 0, 0),   // M2 , DMA2_ST7
+    DEF_TIM(TIM3, CH3, PB0,  TIM_USE_OUTPUT_AUTO, 0, 0),   // M3 , DMA1_ST7
+    DEF_TIM(TIM1, CH2, PE11, TIM_USE_OUTPUT_AUTO, 0, 1),  // M4 , DMA2_ST4
+    DEF_TIM(TIM1, CH1, PE9,  TIM_USE_OUTPUT_AUTO, 0, 2),   // M5 , DMA2_ST2
+#else /* FLYWOOF745NANO */
+    DEF_TIM(TIM3, CH3, PB0,  TIM_USE_OUTPUT_AUTO, 0, 0),   // M1 , DMA1_ST7
+    DEF_TIM(TIM3, CH4, PB1,  TIM_USE_OUTPUT_AUTO, 0, 0),   // M2 , DMA1_ST2
+    DEF_TIM(TIM1, CH1, PE9,  TIM_USE_OUTPUT_AUTO, 0, 2),   // M3 , DMA2_ST2
+    DEF_TIM(TIM1, CH2, PE11, TIM_USE_OUTPUT_AUTO, 0, 1),  // M4 , DMA2_ST4
+    DEF_TIM(TIM8, CH4, PC9,  TIM_USE_OUTPUT_AUTO, 0, 0),   // M5 , DMA2_ST7
+#endif
+    DEF_TIM(TIM5, CH4, PA3,  TIM_USE_OUTPUT_AUTO, 0, 0),  // M6 , DMA1_ST1
+    DEF_TIM(TIM3, CH1, PB4,  TIM_USE_OUTPUT_AUTO, 0, 0),  // M7 , DMA1_ST4
+    DEF_TIM(TIM3, CH2, PB5,  TIM_USE_OUTPUT_AUTO, 0, 0),  // M8 , DMA1_ST5
+    DEF_TIM(TIM4, CH1, PD12, TIM_USE_LED, 0, 0),         // LED_STRIP, DMA1_ST0
 };
 
 const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

From e0f383bae3752c436453b13779cf164396a8a5c5 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Mon, 29 Apr 2024 20:15:12 +0200
Subject: [PATCH 085/323] Revert to macos13 in release_7.1.1 to fix build
 without backporting all fixes

---
 .github/workflows/ci.yml | 312 +++++++++++++++++++--------------------
 1 file changed, 156 insertions(+), 156 deletions(-)

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index 09e27045ca6..94dc8bf344a 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -1,156 +1,156 @@
-name: Build firmware
-# Don't enable CI on push, just on PR. If you
-# are working on the main repo and want to trigger
-# a CI build submit a draft PR.
-on: pull_request
-
-jobs:
-  build:
-    runs-on: ubuntu-latest
-    strategy:
-        matrix:
-          id: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9]
-
-    steps:
-      - uses: actions/checkout@v3
-      - name: Install dependencies
-        run: sudo apt-get update && sudo apt-get -y install ninja-build
-      - name: Setup environment
-        env:
-          ACTIONS_ALLOW_UNSECURE_COMMANDS: true
-        run: |
-          # This is the hash of the commit for the PR
-          # when the action is triggered by PR, empty otherwise
-          COMMIT_ID=${{ github.event.pull_request.head.sha }}
-          # This is the hash of the commit when triggered by push
-          # but the hash of refs/pull/<n>/merge, which is different
-          # from the hash of the latest commit in the PR, that's
-          # why we try github.event.pull_request.head.sha first
-          COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
-          BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
-          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }')
-          echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
-          echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
-      - uses: actions/cache@v1
-        with:
-          path: downloads
-          key: ${{ runner.os }}-downloads-${{ hashFiles('CMakeLists.txt') }}-${{ hashFiles('**/cmake/*')}}
-      - name: Build targets (${{ matrix.id }})
-        run: mkdir -p build && cd build && cmake -DWARNINGS_AS_ERRORS=ON -DCI_JOB_INDEX=${{ matrix.id }} -DCI_JOB_COUNT=${{ strategy.job-total }} -DBUILD_SUFFIX=${{ env.BUILD_SUFFIX }} -G Ninja .. && ninja ci
-      - name: Upload artifacts
-        uses: actions/upload-artifact@v3
-        with:
-          name: ${{ env.BUILD_NAME }}
-          path: ./build/*.hex
-
-  build-SITL-Linux:
-    runs-on: ubuntu-latest
-    steps:
-      - uses: actions/checkout@v3
-      - name: Install dependencies
-        run: sudo apt-get update && sudo apt-get -y install ninja-build
-      - name: Setup environment
-        env:
-          ACTIONS_ALLOW_UNSECURE_COMMANDS: true
-        run: |
-          # This is the hash of the commit for the PR
-          # when the action is triggered by PR, empty otherwise
-          COMMIT_ID=${{ github.event.pull_request.head.sha }}
-          # This is the hash of the commit when triggered by push
-          # but the hash of refs/pull/<n>/merge, which is different
-          # from the hash of the latest commit in the PR, that's
-          # why we try github.event.pull_request.head.sha first
-          COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
-          BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
-          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }')
-          echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
-          echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
-      - name: Build SITL
-        run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja
-      - name: Upload artifacts
-        uses: actions/upload-artifact@v3
-        with:
-          name: ${{ env.BUILD_NAME }}_SITL
-          path: ./build_SITL/*_SITL
-
-  build-SITL-Mac:
-    runs-on: macos-latest
-    steps:
-      - uses: actions/checkout@v3
-      - name: Install dependencies
-        run: |
-          brew install cmake ninja ruby
-
-      - name: Setup environment
-        env:
-          ACTIONS_ALLOW_UNSECURE_COMMANDS: true
-        run: |
-          # This is the hash of the commit for the PR
-          # when the action is triggered by PR, empty otherwise
-          COMMIT_ID=${{ github.event.pull_request.head.sha }}
-          # This is the hash of the commit when triggered by push
-          # but the hash of refs/pull/<n>/merge, which is different
-          # from the hash of the latest commit in the PR, that's
-          # why we try github.event.pull_request.head.sha first
-          COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
-          BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
-          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }')
-          echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
-          echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
-      - name: Build SITL
-        run: |
-          mkdir -p build_SITL && cd build_SITL
-          cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja ..
-          ninja
-
-      - name: Upload artifacts
-        uses: actions/upload-artifact@v3
-        with:
-          name: ${{ env.BUILD_NAME }}_SITL-MacOS
-          path: ./build_SITL/*_SITL
-
-  build-SITL-Windows:
-    runs-on: windows-latest
-    defaults:
-      run:
-        shell: C:\tools\cygwin\bin\bash.exe -o igncr '{0}'
-    steps:
-      - uses: actions/checkout@v3
-      - name: Setup Cygwin
-        uses: egor-tensin/setup-cygwin@v4
-        with:
-          packages: cmake ruby ninja gcc-g++
-      - name: Setup environment
-        env:
-          ACTIONS_ALLOW_UNSECURE_COMMANDS: true
-        run: |
-          # This is the hash of the commit for the PR
-          # when the action is triggered by PR, empty otherwise
-          COMMIT_ID=${{ github.event.pull_request.head.sha }}
-          # This is the hash of the commit when triggered by push
-          # but the hash of refs/pull/<n>/merge, which is different
-          # from the hash of the latest commit in the PR, that's
-          # why we try github.event.pull_request.head.sha first
-          COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
-          BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
-          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }')
-          echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
-          echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
-      - name: Build SITL
-        run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja
-      - name: Upload artifacts
-        uses: actions/upload-artifact@v3
-        with:
-          name: ${{ env.BUILD_NAME }}_SITL-WIN
-          path: ./build_SITL/*.exe
-
-
-  test:
-    needs: [build]
-    runs-on: ubuntu-latest
-    steps:
-      - uses: actions/checkout@v3
-      - name: Install dependencies
-        run: sudo apt-get update && sudo apt-get -y install ninja-build
-      - name: Run Tests
-        run: mkdir -p build && cd build && cmake -DTOOLCHAIN=none -G Ninja .. && ninja check
+name: Build firmware
+# Don't enable CI on push, just on PR. If you
+# are working on the main repo and want to trigger
+# a CI build submit a draft PR.
+on: pull_request
+
+jobs:
+  build:
+    runs-on: ubuntu-latest
+    strategy:
+        matrix:
+          id: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9]
+
+    steps:
+      - uses: actions/checkout@v3
+      - name: Install dependencies
+        run: sudo apt-get update && sudo apt-get -y install ninja-build
+      - name: Setup environment
+        env:
+          ACTIONS_ALLOW_UNSECURE_COMMANDS: true
+        run: |
+          # This is the hash of the commit for the PR
+          # when the action is triggered by PR, empty otherwise
+          COMMIT_ID=${{ github.event.pull_request.head.sha }}
+          # This is the hash of the commit when triggered by push
+          # but the hash of refs/pull/<n>/merge, which is different
+          # from the hash of the latest commit in the PR, that's
+          # why we try github.event.pull_request.head.sha first
+          COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
+          BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
+          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }')
+          echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
+          echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
+      - uses: actions/cache@v1
+        with:
+          path: downloads
+          key: ${{ runner.os }}-downloads-${{ hashFiles('CMakeLists.txt') }}-${{ hashFiles('**/cmake/*')}}
+      - name: Build targets (${{ matrix.id }})
+        run: mkdir -p build && cd build && cmake -DWARNINGS_AS_ERRORS=ON -DCI_JOB_INDEX=${{ matrix.id }} -DCI_JOB_COUNT=${{ strategy.job-total }} -DBUILD_SUFFIX=${{ env.BUILD_SUFFIX }} -G Ninja .. && ninja ci
+      - name: Upload artifacts
+        uses: actions/upload-artifact@v3
+        with:
+          name: ${{ env.BUILD_NAME }}
+          path: ./build/*.hex
+
+  build-SITL-Linux:
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v3
+      - name: Install dependencies
+        run: sudo apt-get update && sudo apt-get -y install ninja-build
+      - name: Setup environment
+        env:
+          ACTIONS_ALLOW_UNSECURE_COMMANDS: true
+        run: |
+          # This is the hash of the commit for the PR
+          # when the action is triggered by PR, empty otherwise
+          COMMIT_ID=${{ github.event.pull_request.head.sha }}
+          # This is the hash of the commit when triggered by push
+          # but the hash of refs/pull/<n>/merge, which is different
+          # from the hash of the latest commit in the PR, that's
+          # why we try github.event.pull_request.head.sha first
+          COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
+          BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
+          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }')
+          echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
+          echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
+      - name: Build SITL
+        run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja
+      - name: Upload artifacts
+        uses: actions/upload-artifact@v3
+        with:
+          name: ${{ env.BUILD_NAME }}_SITL
+          path: ./build_SITL/*_SITL
+
+  build-SITL-Mac:
+    runs-on: macos-13
+    steps:
+      - uses: actions/checkout@v3
+      - name: Install dependencies
+        run: |
+          brew install cmake ninja ruby
+
+      - name: Setup environment
+        env:
+          ACTIONS_ALLOW_UNSECURE_COMMANDS: true
+        run: |
+          # This is the hash of the commit for the PR
+          # when the action is triggered by PR, empty otherwise
+          COMMIT_ID=${{ github.event.pull_request.head.sha }}
+          # This is the hash of the commit when triggered by push
+          # but the hash of refs/pull/<n>/merge, which is different
+          # from the hash of the latest commit in the PR, that's
+          # why we try github.event.pull_request.head.sha first
+          COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
+          BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
+          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }')
+          echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
+          echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
+      - name: Build SITL
+        run: |
+          mkdir -p build_SITL && cd build_SITL
+          cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja ..
+          ninja
+
+      - name: Upload artifacts
+        uses: actions/upload-artifact@v3
+        with:
+          name: ${{ env.BUILD_NAME }}_SITL-MacOS
+          path: ./build_SITL/*_SITL
+
+  build-SITL-Windows:
+    runs-on: windows-latest
+    defaults:
+      run:
+        shell: C:\tools\cygwin\bin\bash.exe -o igncr '{0}'
+    steps:
+      - uses: actions/checkout@v3
+      - name: Setup Cygwin
+        uses: egor-tensin/setup-cygwin@v4
+        with:
+          packages: cmake ruby ninja gcc-g++
+      - name: Setup environment
+        env:
+          ACTIONS_ALLOW_UNSECURE_COMMANDS: true
+        run: |
+          # This is the hash of the commit for the PR
+          # when the action is triggered by PR, empty otherwise
+          COMMIT_ID=${{ github.event.pull_request.head.sha }}
+          # This is the hash of the commit when triggered by push
+          # but the hash of refs/pull/<n>/merge, which is different
+          # from the hash of the latest commit in the PR, that's
+          # why we try github.event.pull_request.head.sha first
+          COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
+          BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
+          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }')
+          echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
+          echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
+      - name: Build SITL
+        run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja
+      - name: Upload artifacts
+        uses: actions/upload-artifact@v3
+        with:
+          name: ${{ env.BUILD_NAME }}_SITL-WIN
+          path: ./build_SITL/*.exe
+
+
+  test:
+    needs: [build]
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v3
+      - name: Install dependencies
+        run: sudo apt-get update && sudo apt-get -y install ninja-build
+      - name: Run Tests
+        run: mkdir -p build && cd build && cmake -DTOOLCHAIN=none -G Ninja .. && ninja check

From 6669af879e7c5072f37c0e482aac7d0a31fda572 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Mon, 29 Apr 2024 20:17:11 +0200
Subject: [PATCH 086/323] back to unix line endings

---
 .github/workflows/ci.yml | 312 +++++++++++++++++++--------------------
 1 file changed, 156 insertions(+), 156 deletions(-)

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index 94dc8bf344a..6fddc02df85 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -1,156 +1,156 @@
-name: Build firmware
-# Don't enable CI on push, just on PR. If you
-# are working on the main repo and want to trigger
-# a CI build submit a draft PR.
-on: pull_request
-
-jobs:
-  build:
-    runs-on: ubuntu-latest
-    strategy:
-        matrix:
-          id: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9]
-
-    steps:
-      - uses: actions/checkout@v3
-      - name: Install dependencies
-        run: sudo apt-get update && sudo apt-get -y install ninja-build
-      - name: Setup environment
-        env:
-          ACTIONS_ALLOW_UNSECURE_COMMANDS: true
-        run: |
-          # This is the hash of the commit for the PR
-          # when the action is triggered by PR, empty otherwise
-          COMMIT_ID=${{ github.event.pull_request.head.sha }}
-          # This is the hash of the commit when triggered by push
-          # but the hash of refs/pull/<n>/merge, which is different
-          # from the hash of the latest commit in the PR, that's
-          # why we try github.event.pull_request.head.sha first
-          COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
-          BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
-          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }')
-          echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
-          echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
-      - uses: actions/cache@v1
-        with:
-          path: downloads
-          key: ${{ runner.os }}-downloads-${{ hashFiles('CMakeLists.txt') }}-${{ hashFiles('**/cmake/*')}}
-      - name: Build targets (${{ matrix.id }})
-        run: mkdir -p build && cd build && cmake -DWARNINGS_AS_ERRORS=ON -DCI_JOB_INDEX=${{ matrix.id }} -DCI_JOB_COUNT=${{ strategy.job-total }} -DBUILD_SUFFIX=${{ env.BUILD_SUFFIX }} -G Ninja .. && ninja ci
-      - name: Upload artifacts
-        uses: actions/upload-artifact@v3
-        with:
-          name: ${{ env.BUILD_NAME }}
-          path: ./build/*.hex
-
-  build-SITL-Linux:
-    runs-on: ubuntu-latest
-    steps:
-      - uses: actions/checkout@v3
-      - name: Install dependencies
-        run: sudo apt-get update && sudo apt-get -y install ninja-build
-      - name: Setup environment
-        env:
-          ACTIONS_ALLOW_UNSECURE_COMMANDS: true
-        run: |
-          # This is the hash of the commit for the PR
-          # when the action is triggered by PR, empty otherwise
-          COMMIT_ID=${{ github.event.pull_request.head.sha }}
-          # This is the hash of the commit when triggered by push
-          # but the hash of refs/pull/<n>/merge, which is different
-          # from the hash of the latest commit in the PR, that's
-          # why we try github.event.pull_request.head.sha first
-          COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
-          BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
-          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }')
-          echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
-          echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
-      - name: Build SITL
-        run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja
-      - name: Upload artifacts
-        uses: actions/upload-artifact@v3
-        with:
-          name: ${{ env.BUILD_NAME }}_SITL
-          path: ./build_SITL/*_SITL
-
-  build-SITL-Mac:
-    runs-on: macos-13
-    steps:
-      - uses: actions/checkout@v3
-      - name: Install dependencies
-        run: |
-          brew install cmake ninja ruby
-
-      - name: Setup environment
-        env:
-          ACTIONS_ALLOW_UNSECURE_COMMANDS: true
-        run: |
-          # This is the hash of the commit for the PR
-          # when the action is triggered by PR, empty otherwise
-          COMMIT_ID=${{ github.event.pull_request.head.sha }}
-          # This is the hash of the commit when triggered by push
-          # but the hash of refs/pull/<n>/merge, which is different
-          # from the hash of the latest commit in the PR, that's
-          # why we try github.event.pull_request.head.sha first
-          COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
-          BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
-          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }')
-          echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
-          echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
-      - name: Build SITL
-        run: |
-          mkdir -p build_SITL && cd build_SITL
-          cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja ..
-          ninja
-
-      - name: Upload artifacts
-        uses: actions/upload-artifact@v3
-        with:
-          name: ${{ env.BUILD_NAME }}_SITL-MacOS
-          path: ./build_SITL/*_SITL
-
-  build-SITL-Windows:
-    runs-on: windows-latest
-    defaults:
-      run:
-        shell: C:\tools\cygwin\bin\bash.exe -o igncr '{0}'
-    steps:
-      - uses: actions/checkout@v3
-      - name: Setup Cygwin
-        uses: egor-tensin/setup-cygwin@v4
-        with:
-          packages: cmake ruby ninja gcc-g++
-      - name: Setup environment
-        env:
-          ACTIONS_ALLOW_UNSECURE_COMMANDS: true
-        run: |
-          # This is the hash of the commit for the PR
-          # when the action is triggered by PR, empty otherwise
-          COMMIT_ID=${{ github.event.pull_request.head.sha }}
-          # This is the hash of the commit when triggered by push
-          # but the hash of refs/pull/<n>/merge, which is different
-          # from the hash of the latest commit in the PR, that's
-          # why we try github.event.pull_request.head.sha first
-          COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
-          BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
-          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }')
-          echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
-          echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
-      - name: Build SITL
-        run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja
-      - name: Upload artifacts
-        uses: actions/upload-artifact@v3
-        with:
-          name: ${{ env.BUILD_NAME }}_SITL-WIN
-          path: ./build_SITL/*.exe
-
-
-  test:
-    needs: [build]
-    runs-on: ubuntu-latest
-    steps:
-      - uses: actions/checkout@v3
-      - name: Install dependencies
-        run: sudo apt-get update && sudo apt-get -y install ninja-build
-      - name: Run Tests
-        run: mkdir -p build && cd build && cmake -DTOOLCHAIN=none -G Ninja .. && ninja check
+name: Build firmware
+# Don't enable CI on push, just on PR. If you
+# are working on the main repo and want to trigger
+# a CI build submit a draft PR.
+on: pull_request
+
+jobs:
+  build:
+    runs-on: ubuntu-latest
+    strategy:
+        matrix:
+          id: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9]
+
+    steps:
+      - uses: actions/checkout@v3
+      - name: Install dependencies
+        run: sudo apt-get update && sudo apt-get -y install ninja-build
+      - name: Setup environment
+        env:
+          ACTIONS_ALLOW_UNSECURE_COMMANDS: true
+        run: |
+          # This is the hash of the commit for the PR
+          # when the action is triggered by PR, empty otherwise
+          COMMIT_ID=${{ github.event.pull_request.head.sha }}
+          # This is the hash of the commit when triggered by push
+          # but the hash of refs/pull/<n>/merge, which is different
+          # from the hash of the latest commit in the PR, that's
+          # why we try github.event.pull_request.head.sha first
+          COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
+          BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
+          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }')
+          echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
+          echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
+      - uses: actions/cache@v1
+        with:
+          path: downloads
+          key: ${{ runner.os }}-downloads-${{ hashFiles('CMakeLists.txt') }}-${{ hashFiles('**/cmake/*')}}
+      - name: Build targets (${{ matrix.id }})
+        run: mkdir -p build && cd build && cmake -DWARNINGS_AS_ERRORS=ON -DCI_JOB_INDEX=${{ matrix.id }} -DCI_JOB_COUNT=${{ strategy.job-total }} -DBUILD_SUFFIX=${{ env.BUILD_SUFFIX }} -G Ninja .. && ninja ci
+      - name: Upload artifacts
+        uses: actions/upload-artifact@v3
+        with:
+          name: ${{ env.BUILD_NAME }}
+          path: ./build/*.hex
+
+  build-SITL-Linux:
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v3
+      - name: Install dependencies
+        run: sudo apt-get update && sudo apt-get -y install ninja-build
+      - name: Setup environment
+        env:
+          ACTIONS_ALLOW_UNSECURE_COMMANDS: true
+        run: |
+          # This is the hash of the commit for the PR
+          # when the action is triggered by PR, empty otherwise
+          COMMIT_ID=${{ github.event.pull_request.head.sha }}
+          # This is the hash of the commit when triggered by push
+          # but the hash of refs/pull/<n>/merge, which is different
+          # from the hash of the latest commit in the PR, that's
+          # why we try github.event.pull_request.head.sha first
+          COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
+          BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
+          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }')
+          echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
+          echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
+      - name: Build SITL
+        run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja
+      - name: Upload artifacts
+        uses: actions/upload-artifact@v3
+        with:
+          name: ${{ env.BUILD_NAME }}_SITL
+          path: ./build_SITL/*_SITL
+
+  build-SITL-Mac:
+    runs-on: macos-13
+    steps:
+      - uses: actions/checkout@v3
+      - name: Install dependencies
+        run: |
+          brew install cmake ninja ruby
+
+      - name: Setup environment
+        env:
+          ACTIONS_ALLOW_UNSECURE_COMMANDS: true
+        run: |
+          # This is the hash of the commit for the PR
+          # when the action is triggered by PR, empty otherwise
+          COMMIT_ID=${{ github.event.pull_request.head.sha }}
+          # This is the hash of the commit when triggered by push
+          # but the hash of refs/pull/<n>/merge, which is different
+          # from the hash of the latest commit in the PR, that's
+          # why we try github.event.pull_request.head.sha first
+          COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
+          BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
+          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }')
+          echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
+          echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
+      - name: Build SITL
+        run: |
+          mkdir -p build_SITL && cd build_SITL
+          cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja ..
+          ninja
+
+      - name: Upload artifacts
+        uses: actions/upload-artifact@v3
+        with:
+          name: ${{ env.BUILD_NAME }}_SITL-MacOS
+          path: ./build_SITL/*_SITL
+
+  build-SITL-Windows:
+    runs-on: windows-latest
+    defaults:
+      run:
+        shell: C:\tools\cygwin\bin\bash.exe -o igncr '{0}'
+    steps:
+      - uses: actions/checkout@v3
+      - name: Setup Cygwin
+        uses: egor-tensin/setup-cygwin@v4
+        with:
+          packages: cmake ruby ninja gcc-g++
+      - name: Setup environment
+        env:
+          ACTIONS_ALLOW_UNSECURE_COMMANDS: true
+        run: |
+          # This is the hash of the commit for the PR
+          # when the action is triggered by PR, empty otherwise
+          COMMIT_ID=${{ github.event.pull_request.head.sha }}
+          # This is the hash of the commit when triggered by push
+          # but the hash of refs/pull/<n>/merge, which is different
+          # from the hash of the latest commit in the PR, that's
+          # why we try github.event.pull_request.head.sha first
+          COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
+          BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
+          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }')
+          echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
+          echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
+      - name: Build SITL
+        run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja
+      - name: Upload artifacts
+        uses: actions/upload-artifact@v3
+        with:
+          name: ${{ env.BUILD_NAME }}_SITL-WIN
+          path: ./build_SITL/*.exe
+
+
+  test:
+    needs: [build]
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v3
+      - name: Install dependencies
+        run: sudo apt-get update && sudo apt-get -y install ninja-build
+      - name: Run Tests
+        run: mkdir -p build && cd build && cmake -DTOOLCHAIN=none -G Ninja .. && ninja check

From 59d9160597b4370fcf234e692d8f1b1fbf8b4c4d Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Mon, 29 Apr 2024 20:17:55 +0200
Subject: [PATCH 087/323] Unix line endings

---
 src/main/target/TAKERF722SE/target.c |  96 +++----
 src/main/target/TAKERF722SE/target.h | 372 +++++++++++++--------------
 2 files changed, 234 insertions(+), 234 deletions(-)

diff --git a/src/main/target/TAKERF722SE/target.c b/src/main/target/TAKERF722SE/target.c
index 247efb84e52..d67062a9fbc 100644
--- a/src/main/target/TAKERF722SE/target.c
+++ b/src/main/target/TAKERF722SE/target.c
@@ -1,48 +1,48 @@
-/*
-* This file is part of INAV Project.
-*
-* 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/io.h"
-
-#include "drivers/dma.h"
-#include "drivers/timer.h"
-#include "drivers/timer_def.h"
-
-
-
-
-
-timerHardware_t timerHardware[] = {
-
-    DEF_TIM(TIM3, CH1, PB4,  TIM_USE_OUTPUT_AUTO, 0, 0),
-    DEF_TIM(TIM3, CH2, PB5,  TIM_USE_OUTPUT_AUTO, 0, 0),
-    DEF_TIM(TIM3, CH3, PB0,  TIM_USE_OUTPUT_AUTO, 0, 0),
-    DEF_TIM(TIM3, CH4, PB1,  TIM_USE_OUTPUT_AUTO, 0, 0),
-
-    DEF_TIM(TIM4, CH1, PB6,  TIM_USE_OUTPUT_AUTO, 0, 0),
-    DEF_TIM(TIM4, CH2, PB7,  TIM_USE_OUTPUT_AUTO, 0, 0),
-    DEF_TIM(TIM8, CH3, PC8,  TIM_USE_OUTPUT_AUTO, 0, 0),
-    DEF_TIM(TIM8, CH4, PC9,  TIM_USE_OUTPUT_AUTO, 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.
+*
+* 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/io.h"
+
+#include "drivers/dma.h"
+#include "drivers/timer.h"
+#include "drivers/timer_def.h"
+
+
+
+
+
+timerHardware_t timerHardware[] = {
+
+    DEF_TIM(TIM3, CH1, PB4,  TIM_USE_OUTPUT_AUTO, 0, 0),
+    DEF_TIM(TIM3, CH2, PB5,  TIM_USE_OUTPUT_AUTO, 0, 0),
+    DEF_TIM(TIM3, CH3, PB0,  TIM_USE_OUTPUT_AUTO, 0, 0),
+    DEF_TIM(TIM3, CH4, PB1,  TIM_USE_OUTPUT_AUTO, 0, 0),
+
+    DEF_TIM(TIM4, CH1, PB6,  TIM_USE_OUTPUT_AUTO, 0, 0),
+    DEF_TIM(TIM4, CH2, PB7,  TIM_USE_OUTPUT_AUTO, 0, 0),
+    DEF_TIM(TIM8, CH3, PC8,  TIM_USE_OUTPUT_AUTO, 0, 0),
+    DEF_TIM(TIM8, CH4, PC9,  TIM_USE_OUTPUT_AUTO, 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/TAKERF722SE/target.h b/src/main/target/TAKERF722SE/target.h
index 61ab442653d..817dc535f03 100644
--- a/src/main/target/TAKERF722SE/target.h
+++ b/src/main/target/TAKERF722SE/target.h
@@ -1,186 +1,186 @@
-/*
- * 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.
- *
- * 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 "GEPR"
-
-#define USBD_PRODUCT_STRING     "TAKERF722SE"
-
-#define LED0                    PC14
-
-
-// *************** BEEPER ************************
-
-#define BEEPER                  PC13
-#define BEEPER_INVERTED
-
-
-// *************** LEDSTRIP ************************
-#define USE_LED_STRIP
-#define WS2811_PIN                  PA8
-
-
-// *************** UART *****************************
-#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            PB10
-#define UART3_RX_PIN            PB11
-
-#define USE_UART4
-#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 SERIAL_PORT_COUNT       7
-
-#define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
-#define SERIALRX_PROVIDER       SERIALRX_SBUS
-#define SERIALRX_UART           SERIAL_PORT_USART2
-
-
-// *************** SPI Bus **********************
-
-#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   	    PB14
-#define SPI2_MOSI_PIN   	    PB15
-
-#define USE_SPI_DEVICE_3
-#define SPI3_SCK_PIN            PC10
-#define SPI3_MISO_PIN           PC11
-#define SPI3_MOSI_PIN           PB2
-#define SPI3_SCK_AF             GPIO_AF6_SPI3
-#define SPI3_MISO_AF            GPIO_AF6_SPI3
-#define SPI3_MOSI_AF            GPIO_AF7_SPI3
-
-
-// *************** Gyro & ACC **********************
-
-#define USE_IMU_MPU6000
-#define IMU_MPU6000_ALIGN       CW0_DEG
-#define MPU6000_CS_PIN          PA4
-#define MPU6000_SPI_BUS         BUS_SPI1
-
-
-#define USE_IMU_ICM42605
-#define IMU_ICM42605_ALIGN      CW0_DEG
-#define ICM42605_CS_PIN         PA4
-#define ICM42605_SPI_BUS        BUS_SPI1
-
-// *************** 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 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 USE_FLASHFS
-
-#define USE_FLASH_M25P16
-#define M25P16_CS_PIN           PA15
-#define M25P16_SPI_BUS          BUS_SPI3
-
-#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
-
-// *************** OSD *****************************
-
-#define USE_MAX7456
-#define MAX7456_SPI_BUS         BUS_SPI2
-#define MAX7456_CS_PIN          PB12
-
-// *************** ADC *****************************
-
-#define USE_ADC
-#define ADC_INSTANCE                ADC1
-#define ADC1_DMA_STREAM             DMA2_Stream0
-#define ADC_CHANNEL_1_PIN           PC3
-#define ADC_CHANNEL_2_PIN           PC0
-#define ADC_CHANNEL_3_PIN           PC2
-
-#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)
-
-
-
-
-//**************************************************
-
-#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        8
-
-#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.
+ *
+ * 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 "GEPR"
+
+#define USBD_PRODUCT_STRING     "TAKERF722SE"
+
+#define LED0                    PC14
+
+
+// *************** BEEPER ************************
+
+#define BEEPER                  PC13
+#define BEEPER_INVERTED
+
+
+// *************** LEDSTRIP ************************
+#define USE_LED_STRIP
+#define WS2811_PIN                  PA8
+
+
+// *************** UART *****************************
+#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            PB10
+#define UART3_RX_PIN            PB11
+
+#define USE_UART4
+#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 SERIAL_PORT_COUNT       7
+
+#define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER       SERIALRX_SBUS
+#define SERIALRX_UART           SERIAL_PORT_USART2
+
+
+// *************** SPI Bus **********************
+
+#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   	    PB14
+#define SPI2_MOSI_PIN   	    PB15
+
+#define USE_SPI_DEVICE_3
+#define SPI3_SCK_PIN            PC10
+#define SPI3_MISO_PIN           PC11
+#define SPI3_MOSI_PIN           PB2
+#define SPI3_SCK_AF             GPIO_AF6_SPI3
+#define SPI3_MISO_AF            GPIO_AF6_SPI3
+#define SPI3_MOSI_AF            GPIO_AF7_SPI3
+
+
+// *************** Gyro & ACC **********************
+
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW0_DEG
+#define MPU6000_CS_PIN          PA4
+#define MPU6000_SPI_BUS         BUS_SPI1
+
+
+#define USE_IMU_ICM42605
+#define IMU_ICM42605_ALIGN      CW0_DEG
+#define ICM42605_CS_PIN         PA4
+#define ICM42605_SPI_BUS        BUS_SPI1
+
+// *************** 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 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 USE_FLASHFS
+
+#define USE_FLASH_M25P16
+#define M25P16_CS_PIN           PA15
+#define M25P16_SPI_BUS          BUS_SPI3
+
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+
+// *************** OSD *****************************
+
+#define USE_MAX7456
+#define MAX7456_SPI_BUS         BUS_SPI2
+#define MAX7456_CS_PIN          PB12
+
+// *************** ADC *****************************
+
+#define USE_ADC
+#define ADC_INSTANCE                ADC1
+#define ADC1_DMA_STREAM             DMA2_Stream0
+#define ADC_CHANNEL_1_PIN           PC3
+#define ADC_CHANNEL_2_PIN           PC0
+#define ADC_CHANNEL_3_PIN           PC2
+
+#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)
+
+
+
+
+//**************************************************
+
+#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        8
+
+#define USE_DSHOT
+#define USE_ESC_SENSOR

From 61ac48fc2e6f8006e9ce1d0b8e2b274b39e8c24a Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Mon, 29 Apr 2024 20:23:29 +0200
Subject: [PATCH 088/323] fix warning

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

diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c
index 7b2590ff70b..5769b56fbdb 100644
--- a/src/main/flight/mixer_profile.c
+++ b/src/main/flight/mixer_profile.c
@@ -79,7 +79,7 @@ void pgResetFn_mixerProfiles(mixerProfile_t *instance)
     }
 }
 
-void activateMixerConfig(){
+void activateMixerConfig(void){
     currentMixerProfileIndex = getConfigMixerProfile();
     currentMixerConfig = *mixerConfig();
     nextMixerProfileIndex = (currentMixerProfileIndex + 1) % MAX_MIXER_PROFILE_COUNT;

From 3fd5f286516de7bd924ade4d34c62413ba5b0fbe Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Mon, 29 Apr 2024 20:33:50 +0200
Subject: [PATCH 089/323] Fix warnings

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

diff --git a/src/main/io/displayport_msp_osd.c b/src/main/io/displayport_msp_osd.c
index e1e6cb29676..0d797e1f756 100644
--- a/src/main/io/displayport_msp_osd.c
+++ b/src/main/io/displayport_msp_osd.c
@@ -525,7 +525,7 @@ void mspOsdSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn)
     }
 }
 
-mspPort_t *getMspOsdPort()
+mspPort_t *getMspOsdPort(void)
 {
     if (mspPort.port) {
         return &mspPort;

From 3af52f7769aef555f0860ff4eb1b65da67ed3a5e Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Mon, 29 Apr 2024 20:44:10 +0200
Subject: [PATCH 090/323] More warning fixes

---
 src/main/build/build_config.h | 2 +-
 src/main/cms/cms_types.h      | 8 +++++++-
 2 files changed, 8 insertions(+), 2 deletions(-)

diff --git a/src/main/build/build_config.h b/src/main/build/build_config.h
index d97e6d300ce..75bb46668d7 100644
--- a/src/main/build/build_config.h
+++ b/src/main/build/build_config.h
@@ -40,7 +40,7 @@
 #endif
 
 #ifdef __APPLE__
-#define FASTRAM                     __attribute__ ((section("__DATA,__.fastram_bss"), aligned(4)))
+#define FASTRAM                     __attribute__ ((section("__DATA,__.fastram_bss"), aligned(8)))
 #else
 #define FASTRAM                     __attribute__ ((section(".fastram_bss"), aligned(4)))
 #endif
diff --git a/src/main/cms/cms_types.h b/src/main/cms/cms_types.h
index 9b2b06646c5..a07e55eaaa4 100644
--- a/src/main/cms/cms_types.h
+++ b/src/main/cms/cms_types.h
@@ -26,6 +26,12 @@
 
 //type of elements
 
+#ifndef __APPLE__
+#define OSD_ENTRY_ATTR __attribute__((packed))
+#else
+#define OSD_ENTRY_ATTR
+#endif
+
 typedef enum
 {
     OME_Label,
@@ -71,7 +77,7 @@ typedef struct
     const void * const data;
     const uint8_t type; // from OSD_MenuElement
     uint8_t flags;
-} __attribute__((packed)) OSD_Entry;
+} OSD_ENTRY_ATTR OSD_Entry;
 
 // Bits in flags
 #define PRINT_VALUE    (1 << 0)  // Value has been changed, need to redraw

From a469daae88e46ba3c47e913854de3d0df04b18c4 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Mon, 29 Apr 2024 20:50:24 +0200
Subject: [PATCH 091/323] macos latest

---
 .github/workflows/ci.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index 6fddc02df85..09e27045ca6 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -74,7 +74,7 @@ jobs:
           path: ./build_SITL/*_SITL
 
   build-SITL-Mac:
-    runs-on: macos-13
+    runs-on: macos-latest
     steps:
       - uses: actions/checkout@v3
       - name: Install dependencies

From fe0f08959db5e5a43e29737f9f8fcf3b5d2b052f Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 30 Apr 2024 15:22:02 +0200
Subject: [PATCH 092/323] Compiloation fix

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

diff --git a/src/main/flight/dynamic_lpf.c b/src/main/flight/dynamic_lpf.c
index 31e594312ab..2c10613ede7 100644
--- a/src/main/flight/dynamic_lpf.c
+++ b/src/main/flight/dynamic_lpf.c
@@ -37,7 +37,7 @@ static float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLp
 
 void dynamicLpfGyroTask(void) {
 
-    if (!gyroConfig()->gyroFilterMode != GYRO_FILTER_MODE_DYNAMIC) {
+    if (gyroConfig()->gyroFilterMode != GYRO_FILTER_MODE_DYNAMIC) {
         return;
     }
 

From 09d404f06493e71cf3a96b153c7b0cd909129918 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Wed, 1 May 2024 19:58:53 +0100
Subject: [PATCH 093/323] nav hold fix

---
 src/main/navigation/navigation.c         | 43 ++++++++++++++----------
 src/main/navigation/navigation_private.h |  1 -
 2 files changed, 26 insertions(+), 18 deletions(-)

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index b443dc49941..eca17c2d656 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -424,7 +424,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_POSHOLD_3D_INITIALIZE,
         .onEntry = navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE,
         .timeoutMs = 0,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT,
         .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_POSHOLD_MODE,
         .mwState = MW_NAV_STATE_HOLD_INFINIT,
         .mwError = MW_NAV_ERROR_NONE,
@@ -439,7 +439,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_POSHOLD_3D_IN_PROGRESS,
         .onEntry = navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_RC_ALT | NAV_RC_POS | NAV_RC_YAW,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_RC_ALT | NAV_RC_POS | NAV_RC_YAW,
         .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_POSHOLD_MODE,
         .mwState = MW_NAV_STATE_HOLD_INFINIT,
         .mwError = MW_NAV_ERROR_NONE,
@@ -595,7 +595,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_RTH_CLIMB_TO_SAFE_ALT,
         .onEntry = navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,     // allow pos adjustment while climbind to safe alt
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,     // allow pos adjustment while climbind to safe alt
         .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
         .mwState = MW_NAV_STATE_RTH_CLIMB,
         .mwError = MW_NAV_ERROR_WAIT_FOR_RTH_ALT,
@@ -635,7 +635,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_RTH_HEAD_HOME,
         .onEntry = navOnEnteringState_NAV_STATE_RTH_HEAD_HOME,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
         .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
         .mwState = MW_NAV_STATE_RTH_ENROUTE,
         .mwError = MW_NAV_ERROR_NONE,
@@ -656,7 +656,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_RTH_LOITER_PRIOR_TO_LANDING,
         .onEntry = navOnEnteringState_NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING,
         .timeoutMs = 500,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
         .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
         .mwState = MW_NAV_STATE_LAND_SETTLE,
         .mwError = MW_NAV_ERROR_NONE,
@@ -677,7 +677,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_RTH_LOITER_ABOVE_HOME,
         .onEntry = navOnEnteringState_NAV_STATE_RTH_LOITER_ABOVE_HOME,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT,
         .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
         .mwState = MW_NAV_STATE_HOVER_ABOVE_HOME,
         .mwError = MW_NAV_ERROR_NONE,
@@ -696,7 +696,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_RTH_LANDING,
         .onEntry = navOnEnteringState_NAV_STATE_RTH_LANDING,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
         .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
         .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
         .mwError = MW_NAV_ERROR_LANDING,
@@ -717,7 +717,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_RTH_FINISHING,
         .onEntry = navOnEnteringState_NAV_STATE_RTH_FINISHING,
         .timeoutMs = 0,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
         .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
         .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
         .mwError = MW_NAV_ERROR_LANDING,
@@ -828,7 +828,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME,                             // There is no state for timed hold?
         .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
         .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
         .mwState = MW_NAV_STATE_HOLD_TIMED,
         .mwError = MW_NAV_ERROR_NONE,
@@ -849,7 +849,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_WAYPOINT_RTH_LAND,
         .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
         .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
         .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
         .mwError = MW_NAV_ERROR_LANDING,
@@ -887,7 +887,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_WAYPOINT_FINISHED,
         .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP | NAV_AUTO_WP_DONE,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP | NAV_AUTO_WP_DONE,
         .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
         .mwState = MW_NAV_STATE_WP_ENROUTE,
         .mwError = MW_NAV_ERROR_FINISH,
@@ -908,7 +908,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_INITIALIZE,
         .onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
         .timeoutMs = 0,
-        .stateFlags = NAV_CTL_EMERG | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE,
+        .stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
         .mapToFlightModes = 0,
         .mwState = MW_NAV_STATE_EMERGENCY_LANDING,
         .mwError = MW_NAV_ERROR_LANDING,
@@ -926,7 +926,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_IN_PROGRESS,
         .onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_EMERG | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE,
+        .stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
         .mapToFlightModes = 0,
         .mwState = MW_NAV_STATE_EMERGENCY_LANDING,
         .mwError = MW_NAV_ERROR_LANDING,
@@ -944,7 +944,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_FINISHED,
         .onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINISHED,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_EMERG | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE,
+        .stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
         .mapToFlightModes = 0,
         .mwState = MW_NAV_STATE_LANDED,
         .mwError = MW_NAV_ERROR_LANDING,
@@ -1053,7 +1053,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_FW_LANDING_CLIMB_TO_LOITER,
         .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_CLIMB_TO_LOITER,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH,
         .mapToFlightModes = NAV_FW_AUTOLAND,
         .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
         .mwError = MW_NAV_ERROR_NONE,
@@ -1074,7 +1074,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_FW_LANDING_LOITER,
         .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_LOITER,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH,
         .mapToFlightModes = NAV_FW_AUTOLAND,
         .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
         .mwError = MW_NAV_ERROR_NONE,
@@ -4081,7 +4081,16 @@ bool isLastMissionWaypoint(void)
 /* Checks if Nav hold position is active */
 bool isNavHoldPositionActive(void)
 {
-    return navGetCurrentStateFlags() & NAV_CTL_HOLD;
+    // WP mode last WP hold and Timed hold positions
+    if (FLIGHT_MODE(NAV_WP_MODE)) {
+        return isLastMissionWaypoint() || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME;
+    }
+    // RTH mode (spiral climb and Home positions but excluding RTH Trackback point positions) and POSHOLD mode
+    // Also hold position during emergency landing if position valid
+    return (FLIGHT_MODE(NAV_RTH_MODE) && !posControl.flags.rthTrackbackActive) ||
+            FLIGHT_MODE(NAV_POSHOLD_MODE) ||
+            (posControl.navState == NAV_STATE_FW_LANDING_CLIMB_TO_LOITER || posControl.navState == NAV_STATE_FW_LANDING_LOITER) ||
+            navigationIsExecutingAnEmergencyLanding();
 }
 
 float getActiveSpeed(void)
diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
index 015c9e371c5..5d23d36fc59 100644
--- a/src/main/navigation/navigation_private.h
+++ b/src/main/navigation/navigation_private.h
@@ -334,7 +334,6 @@ typedef enum {
     NAV_AUTO_WP_DONE        = (1 << 15),    // Waypoint mission reached the last waypoint and is idling
 
     NAV_MIXERAT             = (1 << 16),    // MIXERAT in progress
-    NAV_CTL_HOLD            = (1 << 17),    // Nav loiter active at position or will be when position reached
 } navigationFSMStateFlags_t;
 
 typedef struct {

From 2a62aff039be188f132317a5e1d3bc58c5924b83 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Fri, 3 May 2024 11:46:33 +0100
Subject: [PATCH 094/323] WP altitude enforce hold fix

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

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index eca17c2d656..cedd3b3e9e3 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -4081,10 +4081,11 @@ bool isLastMissionWaypoint(void)
 /* Checks if Nav hold position is active */
 bool isNavHoldPositionActive(void)
 {
-    // WP mode last WP hold and Timed hold positions
-    if (FLIGHT_MODE(NAV_WP_MODE)) {
-        return isLastMissionWaypoint() || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME;
-    }
+    // WP mode last WP hold and Timed/Alt Enforce hold positions
+    return isLastMissionWaypoint() ||
+           NAV_Status.state == MW_NAV_STATE_HOLD_TIMED ||
+           posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME;
+
     // RTH mode (spiral climb and Home positions but excluding RTH Trackback point positions) and POSHOLD mode
     // Also hold position during emergency landing if position valid
     return (FLIGHT_MODE(NAV_RTH_MODE) && !posControl.flags.rthTrackbackActive) ||

From d40bc0dc0cdfe055690f4fa53ef820046bae9715 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Fri, 3 May 2024 21:44:09 +0100
Subject: [PATCH 095/323] improvements

---
 src/main/navigation/navigation.c             | 29 +++++++++++---------
 src/main/navigation/navigation_fixedwing.c   | 10 ++-----
 src/main/navigation/navigation_multicopter.c | 11 +++-----
 3 files changed, 23 insertions(+), 27 deletions(-)

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 09271e5f10f..bef708ecb1b 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -1898,16 +1898,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
                     setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING);
 
                     // Use linear climb between WPs arriving at WP altitude when within 10% of total distance to WP
-                    // Update climbrate until within 25% of total distance to WP, then hold constant
-                    static float climbRate = 0;
-                    if (posControl.wpDistance > 0.25f * posControl.wpInitialDistance) {
+                    // Update climb rate until within 100cm of total climb xy distance to WP, then hold constant
+                    float climbRate = 0.0f;
+                    if (posControl.wpDistance - 0.1f * posControl.wpInitialDistance > 100.0f) {
                         climbRate = posControl.actualState.velXY * (posControl.activeWaypoint.pos.z - posControl.actualState.abs.pos.z) /
                                     (posControl.wpDistance - 0.1f * posControl.wpInitialDistance);
                     }
-
-                    if (fabsf(climbRate) >= (STATE(AIRPLANE) ? navConfig()->fw.max_auto_climb_rate : navConfig()->mc.max_auto_climb_rate)) {
-                        climbRate = 0;
-                    }
                     updateClimbRateToAltitudeController(climbRate, posControl.activeWaypoint.pos.z, ROC_TO_ALT_TARGET);
 
                     if(STATE(MULTIROTOR)) {
@@ -3401,14 +3397,20 @@ bool isProbablyStillFlying(void)
 float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros)
 {
     const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding();
-
     float maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_auto_climb_rate : navConfig()->fw.max_auto_climb_rate;
+
+    if (posControl.flags.rocToAltMode == ROC_TO_ALT_CONSTANT) {
+        if (posControl.flags.isAdjustingAltitude) {
+            maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_manual_climb_rate : navConfig()->fw.max_manual_climb_rate;
+        }
+
+        return constrainf(posControl.desiredState.climbRateDemand, -maxClimbRate, maxClimbRate);
+    }
+
     if (posControl.desiredState.climbRateDemand) {
-        maxClimbRate = ABS(posControl.desiredState.climbRateDemand);
+        maxClimbRate = constrainf(ABS(posControl.desiredState.climbRateDemand), 0.0f, maxClimbRate);
     } else if (emergLandingIsActive) {
         maxClimbRate = navConfig()->general.emerg_descent_rate;
-    } else if (posControl.flags.isAdjustingAltitude) {
-        maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_manual_climb_rate : navConfig()->fw.max_manual_climb_rate;
     }
 
     const float targetAltitudeError = targetAltitude - navGetCurrentActualPositionAndVelocity()->pos.z;
@@ -3420,8 +3422,8 @@ float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros)
         targetVel = pidProfile()->fwAltControlResponseFactor * targetAltitudeError / 100.0f;
     }
 
-    if (emergLandingIsActive && targetAltitudeError > -50) {
-        return -100;    // maintain 1 m/s descent during emerg landing when approaching T/O altitude
+    if (emergLandingIsActive && targetAltitudeError > -50.0f) {
+        return -100.0f;    // maintain 1 m/s descent during emerg landing when within 50cm of min speed landing altitude target
     } else {
         return constrainf(targetVel, -maxClimbRate, maxClimbRate);
     }
@@ -3439,6 +3441,7 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt
 
     if (mode == ROC_TO_ALT_CURRENT) {
         posControl.desiredState.pos.z = navGetCurrentActualPositionAndVelocity()->pos.z;
+        desiredClimbRate = 0.0f;
     } else if (mode == ROC_TO_ALT_TARGET) {
         posControl.desiredState.pos.z = targetAltitude;
     }
diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index 0eaa41ef165..68714ca4803 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -133,11 +133,7 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
 {
     static pt1Filter_t velzFilterState;
 
-    float desiredClimbRate = posControl.desiredState.climbRateDemand;
-
-    if (posControl.flags.rocToAltMode != ROC_TO_ALT_CONSTANT) {
-        desiredClimbRate = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros);
-    }
+    float desiredClimbRate = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros);
 
     // Reduce max allowed climb pitch if performing loiter (stall prevention)
     if (needToCalculateCircularLoiter && desiredClimbRate > 0.0f) {
@@ -770,8 +766,8 @@ void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
     rcCommand[THROTTLE] = setDesiredThrottle(currentBatteryProfile->failsafe_throttle, true);
 
     if (posControl.flags.estAltStatus >= EST_USABLE) {
-        // target min descent rate 10m above takeoff altitude
-        updateClimbRateToAltitudeController(0, 1000.0f, ROC_TO_ALT_TARGET);
+        // target min descent rate at distance 2 x emerg descent rate above takeoff altitude
+        updateClimbRateToAltitudeController(0, 2.0f * navConfig()->general.emerg_descent_rate, 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 41d1f7a5642..b108ef6aa31 100644
--- a/src/main/navigation/navigation_multicopter.c
+++ b/src/main/navigation/navigation_multicopter.c
@@ -76,11 +76,7 @@ float getSqrtControllerVelocity(float targetAltitude, timeDelta_t deltaMicros)
 // Position to velocity controller for Z axis
 static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
 {
-    float targetVel = posControl.desiredState.climbRateDemand;
-
-    if (posControl.flags.rocToAltMode != ROC_TO_ALT_CONSTANT) {
-        targetVel = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros);
-    }
+    float targetVel = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros);
 
     posControl.pids.pos[Z].output_constrained = targetVel;      // only used for Blackbox and OSD info
 
@@ -141,6 +137,7 @@ bool adjustMulticopterAltitudeFromRCInput(void)
     }
     else {
         const int16_t rcThrottleAdjustment = applyDeadbandRescaled(rcCommand[THROTTLE] - altHoldThrottleRCZero, rcControlsConfig()->alt_hold_deadband, -500, 500);
+
         if (rcThrottleAdjustment) {
             // set velocity proportional to stick movement
             float rcClimbRate;
@@ -928,8 +925,8 @@ 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(0, 500.0f, ROC_TO_ALT_TARGET);
+            // target min descent rate at distance 2 x emerg descent rate above takeoff altitude
+            updateClimbRateToAltitudeController(0, 2.0f * navConfig()->general.emerg_descent_rate, ROC_TO_ALT_TARGET);
             updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate);
             updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate);
         }

From 83de3f941d3b7cf247a9fbf5bb9469bf761f90de Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Sat, 4 May 2024 11:25:58 +0100
Subject: [PATCH 096/323] change loiter control logic

---
 src/main/navigation/navigation.c           | 54 ++++++++++++----------
 src/main/navigation/navigation_fixedwing.c |  9 ++--
 src/main/navigation/navigation_private.h   |  1 +
 3 files changed, 36 insertions(+), 28 deletions(-)

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index cedd3b3e9e3..1d24f9ae57b 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -424,7 +424,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_POSHOLD_3D_INITIALIZE,
         .onEntry = navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE,
         .timeoutMs = 0,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT,
         .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_POSHOLD_MODE,
         .mwState = MW_NAV_STATE_HOLD_INFINIT,
         .mwError = MW_NAV_ERROR_NONE,
@@ -439,7 +439,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_POSHOLD_3D_IN_PROGRESS,
         .onEntry = navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_RC_ALT | NAV_RC_POS | NAV_RC_YAW,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_RC_ALT | NAV_RC_POS | NAV_RC_YAW,
         .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_POSHOLD_MODE,
         .mwState = MW_NAV_STATE_HOLD_INFINIT,
         .mwError = MW_NAV_ERROR_NONE,
@@ -595,7 +595,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_RTH_CLIMB_TO_SAFE_ALT,
         .onEntry = navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,     // allow pos adjustment while climbind to safe alt
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,     // allow pos adjustment while climbing to safe alt
         .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
         .mwState = MW_NAV_STATE_RTH_CLIMB,
         .mwError = MW_NAV_ERROR_WAIT_FOR_RTH_ALT,
@@ -656,7 +656,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_RTH_LOITER_PRIOR_TO_LANDING,
         .onEntry = navOnEnteringState_NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING,
         .timeoutMs = 500,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
         .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
         .mwState = MW_NAV_STATE_LAND_SETTLE,
         .mwError = MW_NAV_ERROR_NONE,
@@ -677,7 +677,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_RTH_LOITER_ABOVE_HOME,
         .onEntry = navOnEnteringState_NAV_STATE_RTH_LOITER_ABOVE_HOME,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT,
         .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
         .mwState = MW_NAV_STATE_HOVER_ABOVE_HOME,
         .mwError = MW_NAV_ERROR_NONE,
@@ -696,7 +696,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_RTH_LANDING,
         .onEntry = navOnEnteringState_NAV_STATE_RTH_LANDING,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
         .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
         .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
         .mwError = MW_NAV_ERROR_LANDING,
@@ -717,7 +717,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_RTH_FINISHING,
         .onEntry = navOnEnteringState_NAV_STATE_RTH_FINISHING,
         .timeoutMs = 0,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
         .mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
         .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
         .mwError = MW_NAV_ERROR_LANDING,
@@ -828,7 +828,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME,                             // There is no state for timed hold?
         .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
         .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
         .mwState = MW_NAV_STATE_HOLD_TIMED,
         .mwError = MW_NAV_ERROR_NONE,
@@ -849,7 +849,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_WAYPOINT_RTH_LAND,
         .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
         .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
         .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
         .mwError = MW_NAV_ERROR_LANDING,
@@ -887,7 +887,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_WAYPOINT_FINISHED,
         .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP | NAV_AUTO_WP_DONE,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP | NAV_AUTO_WP_DONE,
         .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
         .mwState = MW_NAV_STATE_WP_ENROUTE,
         .mwError = MW_NAV_ERROR_FINISH,
@@ -926,7 +926,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_IN_PROGRESS,
         .onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
+        .stateFlags =  NAV_CTL_HOLD | NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
         .mapToFlightModes = 0,
         .mwState = MW_NAV_STATE_EMERGENCY_LANDING,
         .mwError = MW_NAV_ERROR_LANDING,
@@ -944,7 +944,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_FINISHED,
         .onEntry = navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINISHED,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
+        .stateFlags =  NAV_CTL_HOLD | NAV_CTL_EMERG | NAV_REQUIRE_ANGLE,
         .mapToFlightModes = 0,
         .mwState = MW_NAV_STATE_LANDED,
         .mwError = MW_NAV_ERROR_LANDING,
@@ -1053,7 +1053,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_FW_LANDING_CLIMB_TO_LOITER,
         .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_CLIMB_TO_LOITER,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH,
         .mapToFlightModes = NAV_FW_AUTOLAND,
         .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
         .mwError = MW_NAV_ERROR_NONE,
@@ -1074,7 +1074,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_FW_LANDING_LOITER,
         .onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_LOITER,
         .timeoutMs = 10,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_HOLD | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH,
         .mapToFlightModes = NAV_FW_AUTOLAND,
         .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
         .mwError = MW_NAV_ERROR_NONE,
@@ -4081,17 +4081,21 @@ bool isLastMissionWaypoint(void)
 /* Checks if Nav hold position is active */
 bool isNavHoldPositionActive(void)
 {
-    // WP mode last WP hold and Timed/Alt Enforce hold positions
-    return isLastMissionWaypoint() ||
-           NAV_Status.state == MW_NAV_STATE_HOLD_TIMED ||
-           posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME;
-
-    // RTH mode (spiral climb and Home positions but excluding RTH Trackback point positions) and POSHOLD mode
-    // Also hold position during emergency landing if position valid
-    return (FLIGHT_MODE(NAV_RTH_MODE) && !posControl.flags.rthTrackbackActive) ||
-            FLIGHT_MODE(NAV_POSHOLD_MODE) ||
-            (posControl.navState == NAV_STATE_FW_LANDING_CLIMB_TO_LOITER || posControl.navState == NAV_STATE_FW_LANDING_LOITER) ||
-            navigationIsExecutingAnEmergencyLanding();
+    /* If the current Nav state isn't flagged as a hold point (NAV_CTL_HOLD) then
+     * waypoints are assumed to be hold points by default unless excluded as defined here */
+
+    if (navGetCurrentStateFlags() & NAV_CTL_HOLD) {
+        return true;
+    }
+
+    if (FLIGHT_MODE(NAV_WP_MODE)) {
+        return posControl.waypointList[posControl.activeWaypointIndex].action != NAV_WP_ACTION_WAYPOINT || isLastMissionWaypoint();
+    }
+
+    return posControl.navState != NAV_STATE_FW_LANDING_APPROACH &&
+           posControl.navState != NAV_STATE_FW_LANDING_GLIDE &&
+           posControl.navState != NAV_STATE_FW_LANDING_FLARE &&
+           !posControl.flags.rthTrackbackActive;
 }
 
 float getActiveSpeed(void)
diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index 2dfebdf8d98..e9991203578 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -290,9 +290,12 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
     // Detemine if a circular loiter is required.
     // For waypoints only use circular loiter when angular visibility is > 30 degs, otherwise head straight toward target
     #define TAN_15DEG    0.26795f
-    needToCalculateCircularLoiter = isNavHoldPositionActive() &&
-                                     (distanceToActualTarget <= (navLoiterRadius / TAN_15DEG)) &&
-                                     (distanceToActualTarget > 50.0f);
+
+    bool loiterApproachActive = isNavHoldPositionActive() &&
+                                distanceToActualTarget <= (navLoiterRadius / TAN_15DEG) &&
+                                distanceToActualTarget > 50.0f;
+    needToCalculateCircularLoiter = loiterApproachActive || (navGetCurrentStateFlags() & NAV_CTL_HOLD);
+
     //if vtol landing is required, fly straight to homepoint
     if ((posControl.navState == NAV_STATE_RTH_HEAD_HOME) && navigationRTHAllowsLanding() && checkMixerATRequired(MIXERAT_REQUEST_LAND)){
         needToCalculateCircularLoiter = false;
diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
index 5d23d36fc59..ddd2bb908da 100644
--- a/src/main/navigation/navigation_private.h
+++ b/src/main/navigation/navigation_private.h
@@ -334,6 +334,7 @@ typedef enum {
     NAV_AUTO_WP_DONE        = (1 << 15),    // Waypoint mission reached the last waypoint and is idling
 
     NAV_MIXERAT             = (1 << 16),    // MIXERAT in progress
+    NAV_CTL_HOLD            = (1 << 17),    // Nav loiter active at position
 } navigationFSMStateFlags_t;
 
 typedef struct {

From 8ad1e5dbdec4373d5330f281a17a9b34da6ac6db Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Sat, 4 May 2024 13:41:21 +0100
Subject: [PATCH 097/323] simplify hold state logic

---
 src/main/navigation/navigation.c | 5 +----
 1 file changed, 1 insertion(+), 4 deletions(-)

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 1d24f9ae57b..070bd1b4b83 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -4092,10 +4092,7 @@ bool isNavHoldPositionActive(void)
         return posControl.waypointList[posControl.activeWaypointIndex].action != NAV_WP_ACTION_WAYPOINT || isLastMissionWaypoint();
     }
 
-    return posControl.navState != NAV_STATE_FW_LANDING_APPROACH &&
-           posControl.navState != NAV_STATE_FW_LANDING_GLIDE &&
-           posControl.navState != NAV_STATE_FW_LANDING_FLARE &&
-           !posControl.flags.rthTrackbackActive;
+    return !FLIGHT_MODE(NAV_FW_AUTOLAND) && !posControl.flags.rthTrackbackActive;
 }
 
 float getActiveSpeed(void)

From ab07785fa9c14d05f37ccdc490b61260027df60e Mon Sep 17 00:00:00 2001
From: Manuel Ilg <29724064+manuelilg@users.noreply.github.com>
Date: Sun, 5 May 2024 13:35:12 +0200
Subject: [PATCH 098/323] Fix channel selection for DMA2 Stream6. To use
 Channel6 which is only for TIM1_CH3 instead of the Channel0 which is for
 TIM1_CH1, TIM1_CH2 and TIM1_CH3.

---
 src/main/target/DALRCF405/target.c   | 2 +-
 src/main/target/FLYWOOF411/target.c  | 4 ++--
 src/main/target/HAKRCF405V2/target.c | 2 +-
 src/main/target/HAKRCF722V2/target.c | 2 +-
 4 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/main/target/DALRCF405/target.c b/src/main/target/DALRCF405/target.c
index 816ad3cdeca..e428504c1bc 100644
--- a/src/main/target/DALRCF405/target.c
+++ b/src/main/target/DALRCF405/target.c
@@ -26,7 +26,7 @@ timerHardware_t timerHardware[] = {
 
     DEF_TIM(TIM3, CH3, PB0,  TIM_USE_OUTPUT_AUTO, 0, 0),   // S1 (1,7)
     DEF_TIM(TIM8, CH1, PC6,  TIM_USE_OUTPUT_AUTO, 0, 0),   // S2 (2,2)
-    DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0, 0),   // S3 (2,6)
+    DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0, 1),   // S3 (2,6)
     DEF_TIM(TIM1, CH1, PA8,  TIM_USE_OUTPUT_AUTO, 0, 1),   // S4 (2,1)  (2.3 2.6)
     DEF_TIM(TIM8, CH3, PC8,  TIM_USE_OUTPUT_AUTO, 0, 1),   // S5 (2,4)  (2.2)
     DEF_TIM(TIM3, CH4, PB1,  TIM_USE_OUTPUT_AUTO, 0, 0),   // S6 (1,2)
diff --git a/src/main/target/FLYWOOF411/target.c b/src/main/target/FLYWOOF411/target.c
index 3946ad0b404..764d29e3144 100644
--- a/src/main/target/FLYWOOF411/target.c
+++ b/src/main/target/FLYWOOF411/target.c
@@ -39,8 +39,8 @@ timerHardware_t timerHardware[] = {
 #else
     DEF_TIM(TIM1, CH1, PA8,  TIM_USE_OUTPUT_AUTO,  0, 1), // S1_OUT 2,1
     DEF_TIM(TIM1, CH2, PA9,  TIM_USE_OUTPUT_AUTO,  0, 1), // S2_OUT 2,2
-    DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO,  0, 0), // S3_OUT 2,6
-    DEF_TIM(TIM3, CH3, PB0,  TIM_USE_MOTOR                ,  0, 0), // S4_OUT 1,7
+    DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO,  0, 1), // S3_OUT 2,6
+    DEF_TIM(TIM3, CH3, PB0,  TIM_USE_MOTOR      ,  0, 0), // S4_OUT 1,7
 
     DEF_TIM(TIM3, CH4, PB1,  TIM_USE_ANY,   0, 0), //  RSSI   1,2
     DEF_TIM(TIM5, CH4, PA3,  TIM_USE_ANY,   0, 1), //  RX2    1,0
diff --git a/src/main/target/HAKRCF405V2/target.c b/src/main/target/HAKRCF405V2/target.c
index a44aabc0142..a0e79ff5b88 100644
--- a/src/main/target/HAKRCF405V2/target.c
+++ b/src/main/target/HAKRCF405V2/target.c
@@ -26,7 +26,7 @@ timerHardware_t timerHardware[] = {
     DEF_TIM(TIM8, CH4, PC9,  TIM_USE_OUTPUT_AUTO, 0, 0), // S2_OUT D2_ST7
     DEF_TIM(TIM1, CH1, PA8,  TIM_USE_OUTPUT_AUTO, 0, 1), // S3_OUT D2_ST1
     DEF_TIM(TIM1, CH2, PA9,  TIM_USE_OUTPUT_AUTO, 0, 1), // S4_OUT D2_ST2
-    DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0, 0), // S5_OUT D2_ST6
+    DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0, 1), // S5_OUT D2_ST6
     DEF_TIM(TIM3, CH1, PB4,  TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT D1_ST4
     DEF_TIM(TIM3, CH3, PB0,  TIM_USE_OUTPUT_AUTO, 0, 0), // S7_OUT D1_ST7
     DEF_TIM(TIM3, CH4, PB1,  TIM_USE_OUTPUT_AUTO, 0, 0), // S8_OUT D1_ST2
diff --git a/src/main/target/HAKRCF722V2/target.c b/src/main/target/HAKRCF722V2/target.c
index 02098ce755d..9738db56d0c 100644
--- a/src/main/target/HAKRCF722V2/target.c
+++ b/src/main/target/HAKRCF722V2/target.c
@@ -39,7 +39,7 @@ timerHardware_t timerHardware[] = {
     DEF_TIM(TIM1, CH2, PA9,  TIM_USE_OUTPUT_AUTO, 0, 1), // S4
     DEF_TIM(TIM3, CH3, PB0,  TIM_USE_OUTPUT_AUTO, 0, 0), // S5
     DEF_TIM(TIM3, CH4, PB1,  TIM_USE_OUTPUT_AUTO, 0, 0), // S6
-    DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0, 0), // S7
+    DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0, 1), // S7
     DEF_TIM(TIM3, CH1, PB4,  TIM_USE_OUTPUT_AUTO, 0, 0), // S8
 
     DEF_TIM(TIM2, CH2, PB3, TIM_USE_LED, 0, 0), // 2812LED

From 1dc91058761816cc2427495e48f373393e62cf32 Mon Sep 17 00:00:00 2001
From: bishko <23706061+bishko@users.noreply.github.com>
Date: Sun, 5 May 2024 13:06:13 +0100
Subject: [PATCH 099/323] Create Building in Gitpod.md (#10004)

Gitpod offer by far the easierst and fastest method for building INAV targets. I wrote a small .md for those who want to have a step-by-step guide.
---
 docs/development/Building in Gitpod.md | 15 +++++++++++++++
 1 file changed, 15 insertions(+)
 create mode 100644 docs/development/Building in Gitpod.md

diff --git a/docs/development/Building in Gitpod.md b/docs/development/Building in Gitpod.md
new file mode 100644
index 00000000000..4434b734433
--- /dev/null
+++ b/docs/development/Building in Gitpod.md	
@@ -0,0 +1,15 @@
+# Building in Gitpod
+
+Gitpod offers an online build environment for building INAV targets.
+## Setting up the environment
+
+1.  Go to https://gitpod.io/new
+1.  Paste `https://github.com/iNavFlight/inav/tree/master` into the field called "Select a repository"
+1.  Cick on the link that shows in the drop down and Gitpod will atomatically selects the adequate Editor and Browser
+1.  Leave the other fields as default and click "Continue". Your build environment will be created.
+1.  At the bottom of the page, you will see a command line. Type `make [TARGET]` and wait for the target to be built.
+1.  Your new target will be located in a folder called `bin`, which can be found at the top left of the page.
+
+NOTE: You can use this method to build your forks as well. Just paste in the link to your fork and follow the rest of the steps.
+
+You are done!

From 32ba12297524e04305106e0667043b9b57055337 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Mon, 6 May 2024 09:00:46 +0100
Subject: [PATCH 100/323] Add comments

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

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 070bd1b4b83..b00442895b6 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -4088,10 +4088,12 @@ bool isNavHoldPositionActive(void)
         return true;
     }
 
+    // No hold required for basic WP type unless it's the last mission waypoint
     if (FLIGHT_MODE(NAV_WP_MODE)) {
         return posControl.waypointList[posControl.activeWaypointIndex].action != NAV_WP_ACTION_WAYPOINT || isLastMissionWaypoint();
     }
 
+    // No hold required for Trackback WPs or for fixed wing autoland WPs not flagged as hold points (returned above if they are)
     return !FLIGHT_MODE(NAV_FW_AUTOLAND) && !posControl.flags.rthTrackbackActive;
 }
 

From feedd3f4c68a6f1bc4f5cbec3979503106b12e16 Mon Sep 17 00:00:00 2001
From: Roman Lut <11955117+RomanLut@users.noreply.github.com>
Date: Mon, 6 May 2024 15:04:13 +0200
Subject: [PATCH 101/323] fix ist8308 compass

---
 src/main/drivers/compass/compass_ist8308.c | 3 ++-
 src/main/drivers/compass/compass_ist8310.c | 2 +-
 2 files changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/main/drivers/compass/compass_ist8308.c b/src/main/drivers/compass/compass_ist8308.c
index 739c0965e9d..75d7386350d 100644
--- a/src/main/drivers/compass/compass_ist8308.c
+++ b/src/main/drivers/compass/compass_ist8308.c
@@ -121,8 +121,9 @@ static bool ist8308Read(magDev_t * mag)
         return false;
     }
 
+    // Invert Y axis to co convert from left to right coordinate system
     mag->magADCRaw[X] = (int16_t)(buf[1] << 8 | buf[0]) * LSB2FSV;
-    mag->magADCRaw[Y] = (int16_t)(buf[3] << 8 | buf[2]) * LSB2FSV;
+    mag->magADCRaw[Y] = -(int16_t)(buf[3] << 8 | buf[2]) * LSB2FSV;
     mag->magADCRaw[Z] = (int16_t)(buf[5] << 8 | buf[4]) * LSB2FSV;
 
     return true;
diff --git a/src/main/drivers/compass/compass_ist8310.c b/src/main/drivers/compass/compass_ist8310.c
index bbd4687eadc..c7b5b0b1249 100644
--- a/src/main/drivers/compass/compass_ist8310.c
+++ b/src/main/drivers/compass/compass_ist8310.c
@@ -139,7 +139,7 @@ static bool ist8310Read(magDev_t * mag)
         return false;
     }
 
-    // Looks like datasheet is incorrect and we need to invert Y axis to conform to right hand rule
+    // Invert Y axis to co convert from left to right coordinate system
     mag->magADCRaw[X] =  (int16_t)(buf[1] << 8 | buf[0]) * LSB2FSV;
     mag->magADCRaw[Y] = -(int16_t)(buf[3] << 8 | buf[2]) * LSB2FSV;
     mag->magADCRaw[Z] =  (int16_t)(buf[5] << 8 | buf[4]) * LSB2FSV;

From 6eb637743d920ef0ae54f55620654a5ed5fb1c90 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 7 May 2024 17:07:08 +0200
Subject: [PATCH 102/323] Compilation fix

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

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 0ab403a8ae5..711ee05f37a 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -1451,7 +1451,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
             bool trackbackActive = navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_ON ||
                                    (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_FS && posControl.flags.forcedRTHActivated);
 
-            if (trackbackActive && posControl.activeRthTBPointIndex >= 0 && !isWaypointMissionRTHActive()) {
+            if (trackbackActive && rth_trackback.activePointIndex >= 0 && !isWaypointMissionRTHActive()) {
                 rthTrackBackUpdate(true);  // save final trackpoint for altitude and max trackback distance reference
                 posControl.flags.rthTrackbackActive = true;
                 calculateAndSetActiveWaypointToLocalPosition(getRthTrackBackPosition());

From a6e256c2ca996c16bcf5724590e5044f7aaf2f96 Mon Sep 17 00:00:00 2001
From: bishko <23706061+bishko@users.noreply.github.com>
Date: Tue, 7 May 2024 16:08:42 +0100
Subject: [PATCH 103/323] Update Building in Gitpod.md

Adding a few points that are required to get HEX files.
---
 docs/development/Building in Gitpod.md | 12 ++++++++----
 1 file changed, 8 insertions(+), 4 deletions(-)

diff --git a/docs/development/Building in Gitpod.md b/docs/development/Building in Gitpod.md
index 4434b734433..fb0203a529f 100644
--- a/docs/development/Building in Gitpod.md	
+++ b/docs/development/Building in Gitpod.md	
@@ -1,14 +1,18 @@
 # Building in Gitpod
 
 Gitpod offers an online build environment for building INAV targets.
-## Setting up the environment
+## Setting up the environment and building targets
 
 1.  Go to https://gitpod.io/new
-1.  Paste `https://github.com/iNavFlight/inav/tree/master` into the field called "Select a repository"
-1.  Cick on the link that shows in the drop down and Gitpod will atomatically selects the adequate Editor and Browser
+1.  Paste `https://github.com/iNavFlight/inav/tree/[version]` into the field called "Select a repository". 
+1.  Ensure that you substitute [version] (e.g. 7.1.0) with the version number of INAV that you want to build.
+1.  Cick on the link that shows in the drop down and Gitpod will atomatically selects the adequate Editor and Browser.
 1.  Leave the other fields as default and click "Continue". Your build environment will be created.
 1.  At the bottom of the page, you will see a command line. Type `make [TARGET]` and wait for the target to be built.
-1.  Your new target will be located in a folder called `bin`, which can be found at the top left of the page.
+1.  Once the build has finished, navigate to the build folder using `cd build`.
+1.  Once in the folder, run `objcopy -O ihex -R .eeprom [TARGET].elf [TARGET].hex` to convert the `.elf` file to a `.hex` file.
+1.  Your new target `.hex` binary will be located in a folder called `bin`, which can be found at the top left of the page.
+
 
 NOTE: You can use this method to build your forks as well. Just paste in the link to your fork and follow the rest of the steps.
 

From 01303686fd19e8e51925e860ba8f146850ba2915 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Tue, 7 May 2024 22:47:44 +0100
Subject: [PATCH 104/323] fixes

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

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 944dcd7db41..e647fe475e3 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -169,7 +169,7 @@ static EXTENDED_FASTRAM bool angleHoldIsLevel = false;
 static EXTENDED_FASTRAM float fixedWingLevelTrim;
 static EXTENDED_FASTRAM pidController_t fixedWingLevelTrimController;
 
-PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 7);
+PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 8);
 
 PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .bank_mc = {
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 8ea4a9a3713..19178ddb1bd 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -1664,7 +1664,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LOITER_PRIOR_TO_LAN
     if (!pauseLanding && ((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, 0, ROC_TO_ALT_CURRENT);
-        return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; // success = land
+        return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_LOITER_ABOVE_HOME; // success = land
     } else {
         fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL);
         setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
@@ -1920,7 +1920,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
                     setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING);
 
                     // Use linear climb between WPs arriving at WP altitude when within 10% of total distance to WP
-                    // Update climb rate until within 100cm of total climb xy distance to WP, then hold constant
+                    // Update climb rate until within 100cm of total climb xy distance to WP
                     float climbRate = 0.0f;
                     if (posControl.wpDistance - 0.1f * posControl.wpInitialDistance > 100.0f) {
                         climbRate = posControl.actualState.velXY * (posControl.activeWaypoint.pos.z - posControl.actualState.abs.pos.z) /

From 75ea2ce41798451259def2229698e5a2e97d3c2f Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sat, 11 May 2024 08:35:19 +0200
Subject: [PATCH 105/323] Fix line ends

---
 .../target/GEPRCF745_BT_HD/CMakeLists.txt     |   2 +-
 src/main/target/GEPRCF745_BT_HD/config.c      | 126 +++---
 src/main/target/GEPRCF745_BT_HD/target.c      | 104 ++---
 src/main/target/GEPRCF745_BT_HD/target.h      | 396 +++++++++---------
 4 files changed, 314 insertions(+), 314 deletions(-)

diff --git a/src/main/target/GEPRCF745_BT_HD/CMakeLists.txt b/src/main/target/GEPRCF745_BT_HD/CMakeLists.txt
index 815e08923a8..6d067ab102c 100644
--- a/src/main/target/GEPRCF745_BT_HD/CMakeLists.txt
+++ b/src/main/target/GEPRCF745_BT_HD/CMakeLists.txt
@@ -1 +1 @@
-target_stm32f745xg(GEPRCF745_BT_HD)
+target_stm32f745xg(GEPRCF745_BT_HD)
diff --git a/src/main/target/GEPRCF745_BT_HD/config.c b/src/main/target/GEPRCF745_BT_HD/config.c
index f228c3e023f..d2f972270a2 100644
--- a/src/main/target/GEPRCF745_BT_HD/config.c
+++ b/src/main/target/GEPRCF745_BT_HD/config.c
@@ -1,63 +1,63 @@
-/*
- * 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 <stdbool.h>
-#include <stdint.h>
-
-#include <platform.h>
-
-#include "common/axis.h"
-
-#include "config/config_master.h"
-#include "config/feature.h"
-
-#include "drivers/sensor.h"
-#include "drivers/pwm_esc_detect.h"
-#include "drivers/pwm_output.h"
-#include "drivers/serial.h"
-
-#include "fc/rc_controls.h"
-
-#include "flight/failsafe.h"
-#include "flight/mixer.h"
-#include "flight/pid.h"
-
-#include "rx/rx.h"
-
-#include "io/serial.h"
-
-#include "sensors/battery.h"
-#include "sensors/sensors.h"
-
-#include "telemetry/telemetry.h"
-
-#include "fc/fc_msp_box.h"
-
-#include "io/piniobox.h"
-
-
-#define BLUETOOTH_MSP_BAUDRATE      BAUD_115200
-
-void targetConfiguration(void)
-{
-    pinioBoxConfigMutable()->permanentId[0] = BOXARM;
-    pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER1;
-
-    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART3)].functionMask = FUNCTION_MSP;
-    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART3)].msp_baudrateIndex = BLUETOOTH_MSP_BAUDRATE;
-}
-
+/*
+ * 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 <stdbool.h>
+#include <stdint.h>
+
+#include <platform.h>
+
+#include "common/axis.h"
+
+#include "config/config_master.h"
+#include "config/feature.h"
+
+#include "drivers/sensor.h"
+#include "drivers/pwm_esc_detect.h"
+#include "drivers/pwm_output.h"
+#include "drivers/serial.h"
+
+#include "fc/rc_controls.h"
+
+#include "flight/failsafe.h"
+#include "flight/mixer.h"
+#include "flight/pid.h"
+
+#include "rx/rx.h"
+
+#include "io/serial.h"
+
+#include "sensors/battery.h"
+#include "sensors/sensors.h"
+
+#include "telemetry/telemetry.h"
+
+#include "fc/fc_msp_box.h"
+
+#include "io/piniobox.h"
+
+
+#define BLUETOOTH_MSP_BAUDRATE      BAUD_115200
+
+void targetConfiguration(void)
+{
+    pinioBoxConfigMutable()->permanentId[0] = BOXARM;
+    pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER1;
+
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART3)].functionMask = FUNCTION_MSP;
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART3)].msp_baudrateIndex = BLUETOOTH_MSP_BAUDRATE;
+}
+
diff --git a/src/main/target/GEPRCF745_BT_HD/target.c b/src/main/target/GEPRCF745_BT_HD/target.c
index 57a8c2a8914..6fe88bde011 100644
--- a/src/main/target/GEPRCF745_BT_HD/target.c
+++ b/src/main/target/GEPRCF745_BT_HD/target.c
@@ -1,52 +1,52 @@
-/*
-* This file is part of INAV Project.
-*
-* 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/sensor.h"
-
-
-BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000,  DEVHW_MPU6000,  IMU1_SPI_BUS,   IMU1_CS_PIN,   NONE,   0,  DEVFLAGS_NONE,  IMU1_ALIGN);
-BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000_2,  DEVHW_MPU6000, IMU2_SPI_BUS,   IMU2_CS_PIN,   NONE,   1,  DEVFLAGS_NONE,  IMU2_ALIGN);
-
-BUSDEV_REGISTER_SPI_TAG(busdev_icm42688,  DEVHW_MPU6000,  IMU1_SPI_BUS,   IMU1_CS_PIN,   NONE,   0,  DEVFLAGS_NONE,  IMU1_ALIGN);
-BUSDEV_REGISTER_SPI_TAG(busdev_icm42688_2, DEVHW_ICM42605, IMU2_SPI_BUS,   IMU2_CS_PIN,   NONE,   1,  DEVFLAGS_NONE,  IMU2_ALIGN);
-
-
-
-timerHardware_t timerHardware[] = {
-
-    DEF_TIM(TIM3, CH3, PB0,  TIM_USE_OUTPUT_AUTO, 0, 0),
-    DEF_TIM(TIM3, CH4, PB1,  TIM_USE_OUTPUT_AUTO, 0, 0),
-    DEF_TIM(TIM3, CH2, PB5,  TIM_USE_OUTPUT_AUTO, 0, 0),
-    DEF_TIM(TIM3, CH1, PB4,  TIM_USE_OUTPUT_AUTO, 0, 0),
-
-    DEF_TIM(TIM4, CH1, PD12, TIM_USE_OUTPUT_AUTO, 0, 0),
-    DEF_TIM(TIM4, CH2, PD13, TIM_USE_OUTPUT_AUTO, 0, 0),
-    DEF_TIM(TIM8, CH3, PC8,  TIM_USE_OUTPUT_AUTO, 0, 0),
-    DEF_TIM(TIM8, CH4, PC9,  TIM_USE_OUTPUT_AUTO, 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.
+*
+* 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/sensor.h"
+
+
+BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000,  DEVHW_MPU6000,  IMU1_SPI_BUS,   IMU1_CS_PIN,   NONE,   0,  DEVFLAGS_NONE,  IMU1_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000_2,  DEVHW_MPU6000, IMU2_SPI_BUS,   IMU2_CS_PIN,   NONE,   1,  DEVFLAGS_NONE,  IMU2_ALIGN);
+
+BUSDEV_REGISTER_SPI_TAG(busdev_icm42688,  DEVHW_MPU6000,  IMU1_SPI_BUS,   IMU1_CS_PIN,   NONE,   0,  DEVFLAGS_NONE,  IMU1_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_icm42688_2, DEVHW_ICM42605, IMU2_SPI_BUS,   IMU2_CS_PIN,   NONE,   1,  DEVFLAGS_NONE,  IMU2_ALIGN);
+
+
+
+timerHardware_t timerHardware[] = {
+
+    DEF_TIM(TIM3, CH3, PB0,  TIM_USE_OUTPUT_AUTO, 0, 0),
+    DEF_TIM(TIM3, CH4, PB1,  TIM_USE_OUTPUT_AUTO, 0, 0),
+    DEF_TIM(TIM3, CH2, PB5,  TIM_USE_OUTPUT_AUTO, 0, 0),
+    DEF_TIM(TIM3, CH1, PB4,  TIM_USE_OUTPUT_AUTO, 0, 0),
+
+    DEF_TIM(TIM4, CH1, PD12, TIM_USE_OUTPUT_AUTO, 0, 0),
+    DEF_TIM(TIM4, CH2, PD13, TIM_USE_OUTPUT_AUTO, 0, 0),
+    DEF_TIM(TIM8, CH3, PC8,  TIM_USE_OUTPUT_AUTO, 0, 0),
+    DEF_TIM(TIM8, CH4, PC9,  TIM_USE_OUTPUT_AUTO, 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/GEPRCF745_BT_HD/target.h b/src/main/target/GEPRCF745_BT_HD/target.h
index 340f7bbe390..3d61f663dd8 100644
--- a/src/main/target/GEPRCF745_BT_HD/target.h
+++ b/src/main/target/GEPRCF745_BT_HD/target.h
@@ -1,198 +1,198 @@
-/*
- * This file is part of INAV Project.
- *
- * 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 "GEPR"
-#define USBD_PRODUCT_STRING     "GEPRCF745_BT_HD"
-
-#define USE_TARGET_CONFIG
-
-#define LED0                    PC13
-
-#define BEEPER                  PD2
-#define BEEPER_INVERTED
-
-
-//**************** SPI BUS *************************
-#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           PB14
-#define SPI2_MOSI_PIN           PB15
-
-#define USE_SPI_DEVICE_3
-#define SPI3_SCK_PIN            PC10
-#define SPI3_MISO_PIN   	    PC11
-#define SPI3_MOSI_PIN   	    PC12
-
-#define USE_SPI_DEVICE_4
-#define SPI4_SCK_PIN            PE2
-#define SPI4_MISO_PIN           PE5
-#define SPI4_MOSI_PIN           PE6
-
-
-
-// *************** Gyro & ACC **********************
-
-#define USE_DUAL_GYRO
-#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS
-#define USE_IMU_MPU6000
-#define USE_IMU_ICM42605
-
-
-// *****IMU1 MPU6000 & ICM42688 ON  SPI1 **************
-#define IMU1_ALIGN              CW180_DEG
-#define IMU1_SPI_BUS            BUS_SPI1
-#define IMU1_CS_PIN             PA4
-
-
-
-// *****IMU2 MPU6000 & ICM42688 ON  SPI2 **************
-#define IMU2_ALIGN              CW0_DEG
-#define IMU2_SPI_BUS            BUS_SPI2
-#define IMU2_CS_PIN             PB12
-
-
-// *************** 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 USE_SDCARD
-#define USE_SDCARD_SPI
-#define SDCARD_SPI_BUS          BUS_SPI3
-#define SDCARD_CS_PIN           PA15
-
-#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
-
-
-// ************PINIO to disable BT*****************
-#define USE_PINIO
-#define USE_PINIOBOX
-#define PINIO1_PIN                  PE13  
-#define PINIO1_FLAGS                PINIO_FLAGS_INVERTED
-
-
-// ************PINIO to other
-#define PINIO2_PIN                  PC14               //Enable vsw
-
-
-// *************** OSD *****************************
-#define USE_MAX7456
-#define MAX7456_SPI_BUS         BUS_SPI4
-#define MAX7456_CS_PIN          PE4
-
-// *************** 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 USE_UART7
-#define UART7_RX_PIN            PE7
-#define UART7_TX_PIN            PE8
-
-#define USE_UART8
-#define UART8_RX_PIN            PE0
-#define UART8_TX_PIN            PE1
-
-#define SERIAL_PORT_COUNT       8
-
-#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           PC3
-#define ADC_CHANNEL_2_PIN           PC5
-#define ADC_CHANNEL_3_PIN           PC2
-
-#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)
-
-#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 0xffff
-#define TARGET_IO_PORTE 0xffff
-
-#define MAX_PWM_OUTPUT_PORTS        8
-
-#define USE_DSHOT
-#define USE_ESC_SENSOR
+/*
+ * This file is part of INAV Project.
+ *
+ * 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 "GEPR"
+#define USBD_PRODUCT_STRING     "GEPRCF745_BT_HD"
+
+#define USE_TARGET_CONFIG
+
+#define LED0                    PC13
+
+#define BEEPER                  PD2
+#define BEEPER_INVERTED
+
+
+//**************** SPI BUS *************************
+#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           PB14
+#define SPI2_MOSI_PIN           PB15
+
+#define USE_SPI_DEVICE_3
+#define SPI3_SCK_PIN            PC10
+#define SPI3_MISO_PIN   	    PC11
+#define SPI3_MOSI_PIN   	    PC12
+
+#define USE_SPI_DEVICE_4
+#define SPI4_SCK_PIN            PE2
+#define SPI4_MISO_PIN           PE5
+#define SPI4_MOSI_PIN           PE6
+
+
+
+// *************** Gyro & ACC **********************
+
+#define USE_DUAL_GYRO
+#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS
+#define USE_IMU_MPU6000
+#define USE_IMU_ICM42605
+
+
+// *****IMU1 MPU6000 & ICM42688 ON  SPI1 **************
+#define IMU1_ALIGN              CW180_DEG
+#define IMU1_SPI_BUS            BUS_SPI1
+#define IMU1_CS_PIN             PA4
+
+
+
+// *****IMU2 MPU6000 & ICM42688 ON  SPI2 **************
+#define IMU2_ALIGN              CW0_DEG
+#define IMU2_SPI_BUS            BUS_SPI2
+#define IMU2_CS_PIN             PB12
+
+
+// *************** 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 USE_SDCARD
+#define USE_SDCARD_SPI
+#define SDCARD_SPI_BUS          BUS_SPI3
+#define SDCARD_CS_PIN           PA15
+
+#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
+
+
+// ************PINIO to disable BT*****************
+#define USE_PINIO
+#define USE_PINIOBOX
+#define PINIO1_PIN                  PE13  
+#define PINIO1_FLAGS                PINIO_FLAGS_INVERTED
+
+
+// ************PINIO to other
+#define PINIO2_PIN                  PC14               //Enable vsw
+
+
+// *************** OSD *****************************
+#define USE_MAX7456
+#define MAX7456_SPI_BUS         BUS_SPI4
+#define MAX7456_CS_PIN          PE4
+
+// *************** 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 USE_UART7
+#define UART7_RX_PIN            PE7
+#define UART7_TX_PIN            PE8
+
+#define USE_UART8
+#define UART8_RX_PIN            PE0
+#define UART8_TX_PIN            PE1
+
+#define SERIAL_PORT_COUNT       8
+
+#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           PC3
+#define ADC_CHANNEL_2_PIN           PC5
+#define ADC_CHANNEL_3_PIN           PC2
+
+#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)
+
+#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 0xffff
+#define TARGET_IO_PORTE 0xffff
+
+#define MAX_PWM_OUTPUT_PORTS        8
+
+#define USE_DSHOT
+#define USE_ESC_SENSOR

From d251b2eca3db7cdee04a9e739290503b7efd9e52 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sat, 11 May 2024 20:54:44 +0200
Subject: [PATCH 106/323] First round of changes

Need to make sure it shows up in configurator
---
 src/main/drivers/pwm_mapping.c | 7 +++++++
 src/main/drivers/timer.h       | 1 +
 src/main/fc/cli.c              | 3 +++
 src/main/flight/mixer.h        | 3 ++-
 4 files changed, 13 insertions(+), 1 deletion(-)

diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c
index 0486eb5ac19..3f9b713c6cd 100644
--- a/src/main/drivers/pwm_mapping.c
+++ b/src/main/drivers/pwm_mapping.c
@@ -47,6 +47,7 @@ enum {
     MAP_TO_NONE,
     MAP_TO_MOTOR_OUTPUT,
     MAP_TO_SERVO_OUTPUT,
+    MAP_TO_LED_OUTPUT
 };
 
 typedef struct {
@@ -335,6 +336,8 @@ void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerU
             type = MAP_TO_SERVO_OUTPUT;
         } else if (TIM_IS_MOTOR(timHw->usageFlags) && !pwmHasServoOnTimer(timOutputs, timHw->tim)) {
             type = MAP_TO_MOTOR_OUTPUT;
+        } else if (TIM_IS_LED(timHw->usageFlags) && !pwmHasMotorOnTimer(timOutputs, timHw->tim) && !pwmHasServoOnTimer(timOutputs, timHw->tim)) {
+            type = MAP_TO_LED_OUTPUT;
         }
 
         switch(type) {
@@ -348,6 +351,10 @@ void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerU
                 timOutputs->timServos[timOutputs->maxTimServoCount++] = timHw;
                 pwmClaimTimer(timHw->tim, timHw->usageFlags);
                 break;
+            case MAP_TO_LED_OUTPUT:
+                timHw->usageFlags &= TIM_USE_LED;
+                pwmClaimTimer(timHw->tim, timHw->usageFlags);
+                break;
             default:
                 break;
         }
diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h
index a207aff0c56..dec602f3c93 100644
--- a/src/main/drivers/timer.h
+++ b/src/main/drivers/timer.h
@@ -123,6 +123,7 @@ typedef enum {
 
 #define TIM_IS_MOTOR(flags) ((flags) & TIM_USE_MOTOR)
 #define TIM_IS_SERVO(flags) ((flags) & TIM_USE_SERVO)
+#define TIM_IS_LED(flags) ((flags) & TIM_USE_LED)
 
 #define TIM_IS_MOTOR_ONLY(flags) (TIM_IS_MOTOR(flags) && !TIM_IS_SERVO(flags))
 #define TIM_IS_SERVO_ONLY(flags) (!TIM_IS_MOTOR(flags) && TIM_IS_SERVO(flags))
diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index 97de1bd6ed1..3bedf7d324d 100644
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -163,6 +163,7 @@ static const char * outputModeNames[] = {
     "AUTO",
     "MOTORS",
     "SERVOS",
+    "LED",
     NULL
 };
 
@@ -2821,6 +2822,8 @@ static void cliTimerOutputMode(char *cmdline)
                     mode = OUTPUT_MODE_MOTORS;
                 } else if(!sl_strcasecmp("SERVOS", tok)) {
                     mode = OUTPUT_MODE_SERVOS;
+                } else if(!sl_strcasecmp("LED", tok)) {
+                    mode = OUTPUT_MODE_LED;
                 } else {
                     cliShowParseError();
                     return;
diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h
index 0d40fdc11ec..8af82173069 100644
--- a/src/main/flight/mixer.h
+++ b/src/main/flight/mixer.h
@@ -48,7 +48,8 @@ typedef enum {
 typedef enum {
     OUTPUT_MODE_AUTO     = 0,
     OUTPUT_MODE_MOTORS,
-    OUTPUT_MODE_SERVOS
+    OUTPUT_MODE_SERVOS,
+    OUTPUT_MODE_LED
 } outputMode_e;
 
 typedef struct motorAxisCorrectionLimits_s {

From 35ce764405515216a6742cecdc36f81a755de0f6 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Sat, 11 May 2024 21:29:58 +0100
Subject: [PATCH 107/323] Fix max climb rate during loiter

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

diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index db9f4d67283..71a7f99fc34 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -135,9 +135,9 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
 
     float desiredClimbRate = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros);
 
-    // Reduce max allowed climb pitch if performing loiter (stall prevention)
-    if (needToCalculateCircularLoiter && desiredClimbRate > 0.0f) {
-        desiredClimbRate *= 0.67f;
+    // Reduce max allowed climb rate by 2/3 if performing loiter (stall prevention)
+    if (needToCalculateCircularLoiter && desiredClimbRate > 0.67f * navConfig()->fw.max_auto_climb_rate) {
+        desiredClimbRate = 0.67f * navConfig()->fw.max_auto_climb_rate;
     }
 
     // Here we use negative values for dive for better clarity

From f82b3fc26ebfa5ba8b09cb2666e6b78e3351ea66 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sat, 11 May 2024 23:48:51 +0200
Subject: [PATCH 108/323] Allow led pin to be re-used as servo or motor more
 easily.

---
 src/main/drivers/pwm_mapping.c      | 16 ++++++++--------
 src/main/drivers/timer.h            |  6 +++---
 src/main/fc/fc_msp.c                | 11 +++++++++++
 src/main/msp/msp_protocol_v2_inav.h |  1 +
 4 files changed, 23 insertions(+), 11 deletions(-)

diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c
index 3f9b713c6cd..b9ce0624592 100644
--- a/src/main/drivers/pwm_mapping.c
+++ b/src/main/drivers/pwm_mapping.c
@@ -214,16 +214,16 @@ static bool checkPwmTimerConflicts(const timerHardware_t *timHw)
 static void timerHardwareOverride(timerHardware_t * timer) {
     switch (timerOverrides(timer2id(timer->tim))->outputMode) {
         case OUTPUT_MODE_MOTORS:
-            if (TIM_IS_SERVO(timer->usageFlags)) {
-                timer->usageFlags &= ~TIM_USE_SERVO;
-                timer->usageFlags |= TIM_USE_MOTOR;
-            }
+            timer->usageFlags &= ~(TIM_USE_SERVO|TIM_USE_LED);
+            timer->usageFlags |= TIM_USE_MOTOR;
             break;
         case OUTPUT_MODE_SERVOS:
-            if (TIM_IS_MOTOR(timer->usageFlags)) {
-                timer->usageFlags &= ~TIM_USE_MOTOR;
-                timer->usageFlags |= TIM_USE_SERVO;
-            }
+            timer->usageFlags &= ~(TIM_USE_MOTOR|TIM_USE_LED);
+            timer->usageFlags |= TIM_USE_SERVO;
+            break;
+        case OUTPUT_MODE_LED:
+            timer->usageFlags &= ~(TIM_USE_MOTOR|TIM_USE_SERVO);
+            timer->usageFlags |= TIM_USE_LED;
             break;
     }
 }
diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h
index dec602f3c93..d87e0400d52 100644
--- a/src/main/drivers/timer.h
+++ b/src/main/drivers/timer.h
@@ -113,9 +113,9 @@ typedef enum {
     TIM_USE_MOTOR           = (1 << 2),     // Motor output
     TIM_USE_SERVO           = (1 << 3),     // Servo output
     TIM_USE_MC_CHNFW        = (1 << 4),     // Deprecated and not used after removal of CHANNEL_FORWARDING feature
-    //TIM_USE_FW_MOTOR        = (1 << 5),   // We no longer differentiate mc from fw on pwm allocation
-    //TIM_USE_FW_SERVO        = (1 << 6),
-    TIM_USE_LED             = (1 << 24),
+    //TIM_USE_FW_MOTOR      = (1 << 5),     // We no longer differentiate mc from fw on pwm allocation
+    //TIM_USE_FW_SERVO      = (1 << 6),
+    TIM_USE_LED             = (1 << 24),    // Remapping needs to be in the lower 8 bits.
     TIM_USE_BEEPER          = (1 << 25),
 } timerUsageFlag_e;
 
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index d44a6e1fcad..2c0210617e8 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -1586,6 +1586,17 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
                 sbufWriteU8(dst, timerHardware[i].usageFlags);
             }
         break;
+    case MSP2_INAV_OUTPUT_MAPPING_EXT2:
+        for (uint8_t i = 0; i < timerHardwareCount; ++i)
+            if (!(timerHardware[i].usageFlags & (TIM_USE_PPM | TIM_USE_PWM))) {
+                #if defined(SITL_BUILD)
+                sbufWriteU8(dst, i);
+                #else
+                sbufWriteU8(dst, timer2id(timerHardware[i].tim));
+                #endif
+                sbufWriteU32(dst, timerHardware[i].usageFlags);
+            }
+        break;
     
     case MSP2_INAV_MC_BRAKING:
 #ifdef USE_MR_BRAKING_MODE
diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h
index 3312e5bc25d..1b64b3f604d 100755
--- a/src/main/msp/msp_protocol_v2_inav.h
+++ b/src/main/msp/msp_protocol_v2_inav.h
@@ -34,6 +34,7 @@
 #define MSP2_INAV_OUTPUT_MAPPING_EXT            0x200D
 #define MSP2_INAV_TIMER_OUTPUT_MODE             0x200E
 #define MSP2_INAV_SET_TIMER_OUTPUT_MODE         0x200F
+#define MSP2_INAV_OUTPUT_MAPPING_EXT2           0x210D
 
 #define MSP2_INAV_MIXER                         0x2010
 #define MSP2_INAV_SET_MIXER                     0x2011

From 0c44cf5b0d14862b05b3ce6feed4454385239d71 Mon Sep 17 00:00:00 2001
From: rmaia <9812730+rmaia3d@users.noreply.github.com>
Date: Sun, 12 May 2024 00:45:43 -0300
Subject: [PATCH 109/323] BFCOMPAT: Add text label (AT) for auto-throttle and
 BF font symbols for pitch and roll

---
 src/main/io/displayport_msp_bf_compat.c | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/src/main/io/displayport_msp_bf_compat.c b/src/main/io/displayport_msp_bf_compat.c
index 4219d2b2db8..1f6ca45bd81 100644
--- a/src/main/io/displayport_msp_bf_compat.c
+++ b/src/main/io/displayport_msp_bf_compat.c
@@ -328,28 +328,28 @@ uint8_t getBfCharacter(uint8_t ch, uint8_t page)
 
     case SYM_ZERO_HALF_LEADING_DOT:
         return BF_SYM_ZERO_HALF_LEADING_DOT;
+*/
 
     case SYM_AUTO_THR0:
-        return BF_SYM_AUTO_THR0;
+        return 'A';
 
     case SYM_AUTO_THR1:
-        return BF_SYM_AUTO_THR1;
+        return 'T';
 
     case SYM_ROLL_LEFT:
-        return BF_SYM_ROLL_LEFT;
+        return BF_SYM_ROLL;
 
     case SYM_ROLL_LEVEL:
-        return BF_SYM_ROLL_LEVEL;
+        return BF_SYM_ROLL;
 
     case SYM_ROLL_RIGHT:
-        return BF_SYM_ROLL_RIGHT;
+        return BF_SYM_ROLL;
 
     case SYM_PITCH_UP:
-        return BF_SYM_PITCH_UP;
+        return BF_SYM_PITCH;
 
     case SYM_PITCH_DOWN:
-        return BF_SYM_PITCH_DOWN;
- */
+        return BF_SYM_PITCH;
 
     case SYM_GFORCE:
         return 'G';

From ce9ea2d8fcb17c946f26cab49a0b906fc205c849 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sun, 12 May 2024 11:08:53 +0200
Subject: [PATCH 110/323] Add obsolete warning to MSP2_INAV_OUTPUT_MAPPING_EXT

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

diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index e099d11f648..75e18ef3681 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -1575,6 +1575,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
             }
         break;
 
+    // Obsolete, replaced by MSP2_INAV_OUTPUT_MAPPING_EXT2
     case MSP2_INAV_OUTPUT_MAPPING_EXT:
         for (uint8_t i = 0; i < timerHardwareCount; ++i)
             if (!(timerHardware[i].usageFlags & (TIM_USE_PPM | TIM_USE_PWM))) {
@@ -1583,6 +1584,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
                 #else
                 sbufWriteU8(dst, timer2id(timerHardware[i].tim));
                 #endif
+                // usageFlags is u32, cuts out the higher 24bits
                 sbufWriteU8(dst, timerHardware[i].usageFlags);
             }
         break;
diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h
index 1b64b3f604d..7c10a4df990 100755
--- a/src/main/msp/msp_protocol_v2_inav.h
+++ b/src/main/msp/msp_protocol_v2_inav.h
@@ -31,7 +31,7 @@
 #define MSP2_INAV_OUTPUT_MAPPING                0x200A
 #define MSP2_INAV_MC_BRAKING                    0x200B
 #define MSP2_INAV_SET_MC_BRAKING                0x200C
-#define MSP2_INAV_OUTPUT_MAPPING_EXT            0x200D
+#define MSP2_INAV_OUTPUT_MAPPING_EXT            0x200D // Obsolete, replaced by MSP2_INAV_OUTPUT_MAPPING_EXT2
 #define MSP2_INAV_TIMER_OUTPUT_MODE             0x200E
 #define MSP2_INAV_SET_TIMER_OUTPUT_MODE         0x200F
 #define MSP2_INAV_OUTPUT_MAPPING_EXT2           0x210D

From ac3277656142d63d3c43c46729c53647da6a41a5 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sun, 12 May 2024 13:21:11 +0200
Subject: [PATCH 111/323] vimrx synthax on by default

---
 .vimrc | 1 +
 1 file changed, 1 insertion(+)

diff --git a/.vimrc b/.vimrc
index 547d37812cb..4808f6bb38d 100644
--- a/.vimrc
+++ b/.vimrc
@@ -5,5 +5,6 @@ set expandtab
 set bs=2
 set sw=4
 set ts=4
+syn on
 
 

From 4f6220b0fe7ef3cf264f3660caaaabe7421b09d1 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sun, 12 May 2024 13:21:29 +0200
Subject: [PATCH 112/323] Change conflict checking to rely on timerUsageFlag

Previously it used IO_TAG to check for the LED pin directly
---
 src/main/drivers/pwm_mapping.c | 12 +++++++++---
 1 file changed, 9 insertions(+), 3 deletions(-)

diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c
index b9ce0624592..e953c86b96e 100644
--- a/src/main/drivers/pwm_mapping.c
+++ b/src/main/drivers/pwm_mapping.c
@@ -168,10 +168,16 @@ static bool checkPwmTimerConflicts(const timerHardware_t *timHw)
 
 #if defined(USE_LED_STRIP)
     if (feature(FEATURE_LED_STRIP)) {
-        const timerHardware_t * ledTimHw = timerGetByTag(IO_TAG(WS2811_PIN), TIM_USE_ANY);
-        if (ledTimHw != NULL && timHw->tim == ledTimHw->tim) {
-            return true;
+        for (int i = 0; i < timerHardwareCount; i++) {
+            if (timHw->tim == timerHardware[i].tim && timerHardware[i].usageFlags & TIM_USE_LED) {
+				return true;
+            }
         }
+
+        //const timerHardware_t * ledTimHw = timerGetByTag(IO_TAG(WS2811_PIN), TIM_USE_ANY);
+        //if (ledTimHw != NULL && timHw->tim == ledTimHw->tim) {
+        //    return true;
+        //}
     }
 #endif
 

From b838606528e4830a13cb2416b8bc934d53172fb4 Mon Sep 17 00:00:00 2001
From: rmaia <9812730+rmaia3d@users.noreply.github.com>
Date: Sun, 12 May 2024 11:28:10 -0300
Subject: [PATCH 113/323] Keep throttle symbol, just prepending with the letter
 A when auto-throttle is engaged

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

diff --git a/src/main/io/displayport_msp_bf_compat.c b/src/main/io/displayport_msp_bf_compat.c
index 1f6ca45bd81..e7f75ec2fa5 100644
--- a/src/main/io/displayport_msp_bf_compat.c
+++ b/src/main/io/displayport_msp_bf_compat.c
@@ -334,7 +334,7 @@ uint8_t getBfCharacter(uint8_t ch, uint8_t page)
         return 'A';
 
     case SYM_AUTO_THR1:
-        return 'T';
+        return BF_SYM_THR;
 
     case SYM_ROLL_LEFT:
         return BF_SYM_ROLL;

From 1dd0d873da670c2a9074e923ba4d8eef6cdef571 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Mon, 13 May 2024 01:11:48 +0200
Subject: [PATCH 114/323] Prefer default LED pin

This changes led hardware setup to prefer original led pin, but fall
back to reassigned LED pin if that was changed to something else.
---
 src/main/drivers/light_ws2811strip.c | 10 +++++++---
 1 file changed, 7 insertions(+), 3 deletions(-)

diff --git a/src/main/drivers/light_ws2811strip.c b/src/main/drivers/light_ws2811strip.c
index cb56a7fe79a..fe5f405d032 100644
--- a/src/main/drivers/light_ws2811strip.c
+++ b/src/main/drivers/light_ws2811strip.c
@@ -124,6 +124,10 @@ void ws2811LedStripInit(void)
 {
     const timerHardware_t * timHw = timerGetByTag(IO_TAG(WS2811_PIN), TIM_USE_ANY);
 
+    if (!(timHw->usageFlags & TIM_USE_LED)) { // Check if it has not been reassigned
+        timHw = timerGetByUsageFlag(TIM_USE_LED); // Get first pin marked as LED
+    }
+
     if (timHw == NULL) {
         return;
     }
@@ -133,14 +137,14 @@ void ws2811LedStripInit(void)
         return;
     }
 
-    ws2811IO = IOGetByTag(IO_TAG(WS2811_PIN));
+    ws2811IO = IOGetByTag(timHw->tag); //IOGetByTag(IO_TAG(WS2811_PIN));
     IOInit(ws2811IO, OWNER_LED_STRIP, RESOURCE_OUTPUT, 0);
     IOConfigGPIOAF(ws2811IO, IOCFG_AF_PP_FAST, timHw->alternateFunction);
 
-    if ( ledPinConfig()->led_pin_pwm_mode == LED_PIN_PWM_MODE_LOW ) {
+    if (ledPinConfig()->led_pin_pwm_mode == LED_PIN_PWM_MODE_LOW) {
         ledConfigurePWM();
         *timerCCR(ws2811TCH) = 0;
-    } else if ( ledPinConfig()->led_pin_pwm_mode == LED_PIN_PWM_MODE_HIGH ) {
+    } else if (ledPinConfig()->led_pin_pwm_mode == LED_PIN_PWM_MODE_HIGH) {
         ledConfigurePWM();
         *timerCCR(ws2811TCH) = 100;
     } else {

From 6f4ff6b4de8567645abd09e10ab9d2cb63febba7 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Mon, 13 May 2024 17:32:14 +0100
Subject: [PATCH 115/323] Fix trackback error

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

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 711ee05f37a..1354d818a0d 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -1448,10 +1448,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
         }
         else {
             // Switch to RTH trackback
-            bool trackbackActive = navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_ON ||
-                                   (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_FS && posControl.flags.forcedRTHActivated);
-
-            if (trackbackActive && rth_trackback.activePointIndex >= 0 && !isWaypointMissionRTHActive()) {
+            if (rthTrackBackIsActive() && rth_trackback.activePointIndex >= 0 && !isWaypointMissionRTHActive()) {
                 rthTrackBackUpdate(true);  // save final trackpoint for altitude and max trackback distance reference
                 posControl.flags.rthTrackbackActive = true;
                 calculateAndSetActiveWaypointToLocalPosition(getRthTrackBackPosition());
@@ -3200,7 +3197,7 @@ bool rthAltControlStickOverrideCheck(uint8_t axis)
         (axis == ROLL && STATE(MULTIROTOR) && !posControl.flags.rthTrackbackActive)) {
         return false;
     }
-    
+
     static timeMs_t rthOverrideStickHoldStartTime[2];
 
     if (rxGetChannelValue(axis) > rxConfig()->maxcheck) {

From 6aeb5c3e7c0dcf2a7afe3166849c094ac3149477 Mon Sep 17 00:00:00 2001
From: Julio Cesar <juliocesar@MacBook-Pro-de-Julio.local>
Date: Mon, 13 May 2024 21:58:30 -0300
Subject: [PATCH 116/323] Make the 'declination.py' file generate Declination,
 Inclination and Intensity.

---
 .../navigation/navigation_declination_gen.c   | 120 +++---
 src/main/target/common.h                      |   2 -
 src/utils/declination.py                      | 345 ++++++++++++------
 3 files changed, 299 insertions(+), 168 deletions(-)

diff --git a/src/main/navigation/navigation_declination_gen.c b/src/main/navigation/navigation_declination_gen.c
index c7d632cd87c..65b44cfcbcb 100644
--- a/src/main/navigation/navigation_declination_gen.c
+++ b/src/main/navigation/navigation_declination_gen.c
@@ -1,63 +1,77 @@
 /* this file is automatically generated by src/utils/declination.py - DO NOT EDIT! */
 
 
-/* Updated on 2024-04-19 12:54:15.842704 */
+/* Updated on 2024-05-13 21:53:14.731603 */
 
 
-#include <stdint.h>
+const float SAMPLING_RES = 10;
+const float SAMPLING_MIN_LAT = -90;
+const float SAMPLING_MAX_LAT = 90;
+const float SAMPLING_MIN_LON = -180;
+const float SAMPLING_MAX_LON = 180;
 
-
-
-#if defined(NAV_AUTO_MAG_DECLINATION_PRECISE)
-#define SAMPLING_RES		10.00000f
-#define SAMPLING_MIN_LON	-180.00000f
-#define SAMPLING_MAX_LON	180.00000f
-#define SAMPLING_MIN_LAT	-90.00000f
-#define SAMPLING_MAX_LAT	90.00000f
-
-static const float declination_table[19][37] = {
-    {148.63040f,138.63040f,128.63040f,118.63040f,108.63040f,98.63040f,88.63040f,78.63040f,68.63040f,58.63040f,48.63040f,38.63040f,28.63040f,18.63040f,8.63040f,-1.36960f,-11.36960f,-21.36960f,-31.36960f,-41.36960f,-51.36960f,-61.36960f,-71.36960f,-81.36960f,-91.36960f,-101.36960f,-111.36960f,-121.36960f,-131.36960f,-141.36960f,-151.36960f,-161.36960f,-171.36960f,178.63040f,168.63040f,158.63040f,148.63040f},
-    {128.87816f,116.70769f,105.62443f,95.48127f,86.09683f,77.29367f,68.91718f,60.84302f,52.97822f,45.25811f,37.64003f,30.09478f,22.59702f,15.11676f,7.61418f,0.03947f,-7.66207f,-15.54157f,-23.63819f,-31.97410f,-40.55489f,-49.37538f,-58.42917f,-67.71936f,-77.26833f,-87.12519f,-97.37007f,-108.11458f,-119.49579f,-131.65830f,-144.71588f,-158.68662f,-173.41452f,171.47213f,156.49935f,142.18469f,128.87816f},
-    {85.86755f,77.88182f,71.42849f,65.89217f,60.86962f,56.04972f,51.17842f,46.06489f,40.60132f,34.77619f,28.67056f,22.43282f,16.23087f,10.18755f,4.31917f,-1.49067f,-7.45797f,-13.81467f,-20.71641f,-28.18762f,-36.12471f,-44.34676f,-52.66473f,-60.93937f,-69.11381f,-77.22546f,-85.41334f,-93.94372f,-103.29064f,-114.35005f,-128.97727f,-151.06023f,174.83520f,137.67818f,112.23020f,96.52063f,85.86756f},
-    {48.43188f,46.98353f,45.36982f,43.80186f,42.32760f,40.82382f,38.99678f,36.46379f,32.90329f,28.18877f,22.45599f,16.10387f,9.71432f,3.86202f,-1.14650f,-5.45997f,-9.64078f,-14.38658f,-20.18766f,-27.11174f,-34.80967f,-42.71859f,-50.31107f,-57.23008f,-63.28782f,-68.39719f,-72.47622f,-75.31221f,-76.30788f,-73.74072f,-61.55512f,-18.96160f,29.20071f,44.46666f,48.67974f,49.28540f,48.43188f},
-    {31.58850f,31.74835f,31.39924f,30.87678f,30.43574f,30.19800f,29.98360f,29.24010f,27.23113f,23.37602f,17.54538f,10.24099f,2.57680f,-4.11819f,-8.98330f,-12.08874f,-14.32383f,-16.91831f,-20.97106f,-26.92020f,-34.17714f,-41.56631f,-48.09571f,-53.19469f,-56.53278f,-57.82558f,-56.66031f,-52.27310f,-43.55594f,-29.95085f,-13.24765f,2.54212f,14.60798f,22.72224f,27.72553f,30.44935f,31.58850f},
-    {22.79850f,23.32533f,23.34746f,23.05694f,22.66226f,22.42885f,22.46289f,22.41289f,21.37705f,18.23370f,12.30982f,4.02530f,-4.93197f,-12.42012f,-17.23389f,-19.62803f,-20.56810f,-20.93312f,-21.98947f,-25.41090f,-31.19026f,-37.44166f,-42.49725f,-45.51856f,-46.02918f,-43.74555f,-38.55838f,-30.56411f,-20.73773f,-11.07131f,-2.79308f,4.21937f,10.21441f,15.15242f,18.92948f,21.44642f,22.79850f},
-    {17.16987f,17.68582f,17.85385f,17.73377f,17.34906f,16.86130f,16.52747f,16.34574f,15.56790f,12.74839f,6.74610f,-2.00418f,-11.22826f,-18.37193f,-22.46233f,-24.19496f,-24.47888f,-23.22908f,-20.72783f,-19.60150f,-21.98233f,-26.43395f,-30.43203f,-32.35125f,-31.47499f,-27.95264f,-22.40690f,-15.51422f,-8.63620f,-3.35146f,0.42144f,3.87188f,7.48128f,10.95260f,13.91406f,16.01563f,17.16987f},
-    {13.46149f,13.75829f,13.85281f,13.81813f,13.52893f,12.94281f,12.32698f,11.88976f,11.00215f,8.11359f,1.99478f,-6.62228f,-15.08744f,-21.05798f,-23.92224f,-24.34577f,-23.04831f,-19.84546f,-14.88685f,-10.38631f,-9.06427f,-11.31395f,-15.06340f,-17.65505f,-17.71488f,-15.54256f,-11.89991f,-7.27581f,-2.87289f,-0.16517f,1.24684f,2.97092f,5.51371f,8.29954f,10.80542f,12.58786f,13.46149f},
-    {11.19370f,11.20122f,11.07257f,11.02175f,10.82482f,10.26986f,9.62418f,9.09716f,7.97068f,4.77978f,-1.35469f,-9.31946f,-16.55524f,-21.15274f,-22.50861f,-21.01857f,-17.63827f,-13.15823f,-8.29683f,-4.01764f,-1.51484f,-1.87967f,-4.62279f,-7.47003f,-8.63091f,-8.05888f,-6.24442f,-3.42551f,-0.62936f,0.67783f,0.89029f,1.80510f,3.93778f,6.48671f,8.83523f,10.50781f,11.19370f},
-    {9.96310f,9.79689f,9.46606f,9.40362f,9.31492f,8.84240f,8.22130f,7.55735f,6.02850f,2.43377f,-3.55150f,-10.60044f,-16.54723f,-19.75861f,-19.59018f,-16.58982f,-12.16100f,-7.67553f,-3.93189f,-0.91522f,1.34973f,1.87637f,0.22653f,-2.15228f,-3.61566f,-3.90720f,-3.29932f,-1.83286f,-0.23997f,0.20367f,-0.19418f,0.32122f,2.29252f,4.85536f,7.33778f,9.21090f,9.96310f},
-    {9.18888f,9.24336f,8.98253f,9.05281f,9.17191f,8.83598f,8.10836f,7.00345f,4.80464f,0.69064f,-5.15618f,-11.30079f,-15.93916f,-17.74937f,-16.42349f,-12.86230f,-8.44857f,-4.38855f,-1.42704f,0.70613f,2.51072f,3.31152f,2.34785f,0.48172f,-0.89199f,-1.49634f,-1.59901f,-1.18437f,-0.66243f,-0.94842f,-1.78088f,-1.62463f,0.11657f,2.73547f,5.55820f,7.95812f,9.18888f},
-    {8.09034f,8.95675f,9.30400f,9.80188f,10.27100f,10.13647f,9.19791f,7.38846f,4.25004f,-0.60600f,-6.53915f,-11.93135f,-15.26054f,-15.79066f,-13.79060f,-10.27561f,-6.25103f,-2.56551f,0.07346f,1.80766f,3.24155f,4.05015f,3.51428f,2.10305f,0.93136f,0.25053f,-0.27213f,-0.70545f,-1.24346f,-2.37500f,-3.75122f,-4.07408f,-2.71911f,-0.15132f,2.99380f,6.02278f,8.09034f},
-    {6.27792f,8.37723f,9.88296f,11.16334f,12.08487f,12.15767f,11.03142f,8.51947f,4.34248f,-1.47518f,-7.78911f,-12.71269f,-15.01118f,-14.61457f,-12.25578f,-8.87216f,-5.13675f,-1.63793f,0.99774f,2.71417f,3.98293f,4.78698f,4.67468f,3.82712f,2.93970f,2.18792f,1.25958f,-0.01231f,-1.71794f,-3.92282f,-6.03566f,-6.90283f,-5.95491f,-3.51614f,-0.20102f,3.30598f,6.27792f},
-    {4.15672f,7.47883f,10.31556f,12.59798f,14.06987f,14.34572f,13.08042f,9.97537f,4.77447f,-2.16465f,-9.14687f,-14.00316f,-15.77819f,-14.88455f,-12.28640f,-8.84845f,-5.11869f,-1.55612f,1.34605f,3.41756f,4.91977f,6.01696f,6.56254f,6.51350f,6.02774f,5.09138f,3.48456f,1.07344f,-2.08240f,-5.60383f,-8.56486f,-9.89888f,-9.20525f,-6.83312f,-3.41374f,0.42499f,4.15672f},
-    {2.38746f,6.60961f,10.51138f,13.76537f,15.94256f,16.59043f,15.27093f,11.52705f,5.06990f,-3.36003f,-11.36932f,-16.51244f,-18.15167f,-17.03678f,-14.21651f,-10.51524f,-6.47146f,-2.49805f,1.03216f,3.92789f,6.27640f,8.24229f,9.81368f,10.78355f,10.85070f,9.68870f,7.04509f,2.91449f,-2.25894f,-7.44786f,-11.30963f,-12.92043f,-12.20858f,-9.70263f,-6.08438f,-1.92562f,2.38746f},
-    {1.14371f,5.95768f,10.55039f,14.56595f,17.51248f,18.77049f,17.56825f,12.99646f,4.54568f,-6.33792f,-15.91121f,-21.43028f,-22.88293f,-21.44177f,-18.22110f,-13.99911f,-9.28961f,-4.46489f,0.18470f,4.47851f,8.37200f,11.87759f,14.90388f,17.13814f,18.04159f,16.93503f,13.18748f,6.66358f,-1.56678f,-9.20492f,-14.20053f,-15.96059f,-14.99797f,-12.15657f,-8.19328f,-3.64820f,1.14371f},
-    {-0.35955f,4.92766f,9.99739f,14.51863f,17.99503f,19.64821f,18.23668f,11.99980f,-0.09657f,-14.74873f,-25.55246f,-30.33910f,-30.58104f,-28.00627f,-23.75441f,-18.51083f,-12.70165f,-6.61593f,-0.46628f,5.58665f,11.41580f,16.88997f,21.81133f,25.83916f,28.40134f,28.57592f,24.99531f,16.28607f,3.28542f,-9.10495f,-16.59733f,-19.05958f,-18.04316f,-14.91590f,-10.58486f,-5.61478f,-0.35955f},
-    {-5.35521f,-0.10619f,4.69597f,8.54683f,10.65268f,9.65875f,3.42678f,-9.71040f,-26.04578f,-37.63969f,-42.28407f,-42.02380f,-38.82980f,-33.88684f,-27.86664f,-21.16414f,-14.02720f,-6.62426f,0.91924f,8.49972f,16.02040f,23.37681f,30.43782f,37.01500f,42.80483f,47.26542f,49.32663f,46.69337f,34.75696f,10.85970f,-10.89085f,-20.36338f,-21.94791f,-19.71753f,-15.63466f,-10.66363f,-5.35521f},
-    {-166.99999f,-156.99999f,-146.99999f,-136.99999f,-126.99999f,-116.99999f,-106.99999f,-96.99999f,-86.99999f,-76.99999f,-66.99999f,-56.99999f,-46.99999f,-36.99999f,-26.99999f,-16.99999f,-6.99999f,3.00001f,13.00001f,23.00001f,33.00000f,43.00000f,53.00000f,63.00000f,73.00000f,83.00000f,93.00000f,103.00000f,113.00000f,123.00000f,133.00000f,143.00000f,153.00000f,163.00000f,173.00000f,-177.00000f,-167.00000f}
+const float declination_table[19][37] = {
+    {148.83402f,138.83401f,128.83401f,118.83402f,108.83402f,98.83402f,88.83402f,78.83402f,68.83402f,58.83402f,48.83402f,38.83402f,28.83402f,18.83402f,8.83402f,-1.16598f,-11.16598f,-21.16598f,-31.16598f,-41.16598f,-51.16598f,-61.16598f,-71.16598f,-81.16598f,-91.16598f,-101.16598f,-111.16598f,-121.16598f,-131.16598f,-141.16598f,-151.16598f,-161.16598f,-171.16598f,178.83402f,168.83402f,158.83402f,148.83402f},
+    {129.09306f,116.89412f,105.78898f,95.63017f,86.23504f,77.42476f,69.04348f,60.96591f,53.09841f,45.37592f,37.75568f,30.20867f,22.70998f,15.23027f,7.73037f,0.16098f,-7.53238f,-15.40109f,-23.48496f,-31.80703f,-40.37375f,-49.18062f,-58.22152f,-67.49946f,-77.03640f,-86.88079f,-97.11212f,-107.84156f,-119.20626f,-131.35191f,-144.39481f,-158.35719f,-173.08769f,171.78274f,156.78187f,142.43310f,129.09306f},
+    {85.81367f,77.83602f,71.40003f,65.88433f,60.88263f,56.08204f,51.22755f,46.12774f,40.67419f,34.85457f,28.74909f,22.50583f,16.29363f,10.23812f,4.36029f,-1.45095f,-7.40778f,-13.74178f,-20.61213f,-28.04922f,-35.95530f,-44.15322f,-52.45486f,-60.71951f,-68.88732f,-76.99183f,-85.16746f,-93.67473f,-102.97950f,-113.96429f,-128.46293f,-150.36073f,175.55488f,138.00554f,112.28051f,96.48319f,85.81367f},
+    {48.22805f,46.81921f,45.24300f,43.71189f,42.27245f,40.80022f,39.00177f,36.49549f,32.95972f,28.26545f,22.54379f,16.18846f,9.77818f,3.88971f,-1.16066f,-5.50500f,-9.68832f,-14.40052f,-20.13990f,-26.99385f,-34.63319f,-42.50584f,-50.08478f,-57.00760f,-63.07978f,-68.20773f,-72.30336f,-75.14845f,-76.14220f,-73.56875f,-61.46573f,-19.66028f,28.39616f,43.98783f,48.35027f,49.03265f,48.22805f},
+    {31.42931f,31.60530f,31.28145f,30.79047f,30.38024f,30.16777f,29.97487f,29.25597f,27.27948f,23.46202f,17.66378f,10.37176f,2.68673f,-4.06559f,-9.01317f,-12.19989f,-14.47861f,-17.04830f,-21.00672f,-26.83158f,-33.98769f,-41.32654f,-47.85214f,-52.97945f,-56.36644f,-57.72068f,-56.62201f,-52.29959f,-43.64045f,-30.08348f,-13.41611f,2.35315f,14.41455f,22.53506f,27.54777f,30.28028f,31.42931f},
+    {22.67985f,23.21169f,23.25494f,22.99742f,22.63545f,22.42369f,22.46877f,22.43187f,21.42571f,18.33186f,12.46440f,4.21692f,-4.75068f,-12.30560f,-17.22721f,-19.74039f,-20.77499f,-21.16946f,-22.14607f,-25.39336f,-31.01102f,-37.19196f,-42.26227f,-45.34969f,-45.95366f,-43.77299f,-38.67388f,-30.72261f,-20.87969f,-11.16390f,-2.84711f,4.17466f,10.15622f,15.07307f,18.83144f,21.33469f,22.67985f},
+    {17.07334f,17.59062f,17.77934f,17.69577f,17.34929f,16.88674f,16.56217f,16.38656f,15.63204f,12.86517f,6.93562f,-1.75751f,-10.98436f,-18.20308f,-22.41588f,-24.27993f,-24.67590f,-23.50719f,-21.00605f,-19.72369f,-21.87960f,-26.21214f,-30.23195f,-32.25069f,-31.49417f,-28.07385f,-22.58093f,-15.67611f,-8.73630f,-3.38117f,0.43868f,3.90117f,7.49078f,10.92679f,13.85484f,15.93237f,17.07334f},
+    {13.37426f,13.67217f,13.78744f,13.78998f,13.54222f,12.98780f,12.38852f,11.96065f,11.09372f,8.25499f,2.21162f,-6.34723f,-14.82596f,-20.89032f,-23.89434f,-24.45752f,-23.26867f,-20.14461f,-15.21492f,-10.62227f,-9.09952f,-11.18829f,-14.91638f,-17.58833f,-17.75073f,-15.65321f,-12.03643f,-7.38772f,-2.92537f,-0.15156f,1.30857f,3.04660f,5.56513f,8.30463f,10.76538f,12.51642f,13.37426f},
+    {11.10969f,11.11944f,11.01236f,10.99728f,10.84097f,10.32286f,9.70494f,9.19687f,8.09157f,4.94277f,-1.12864f,-9.05484f,-16.32980f,-21.04376f,-22.55109f,-21.19496f,-17.89373f,-13.44411f,-8.58084f,-4.24867f,-1.62827f,-1.86509f,-4.55045f,-7.42469f,-8.64775f,-8.12536f,-6.32716f,-3.48850f,-0.64489f,0.72027f,0.97976f,1.90952f,4.01623f,6.51266f,8.80749f,10.44185f,11.10969f},
+    {9.88349f,9.72040f,9.41289f,9.38439f,9.33291f,8.89921f,8.31537f,7.68011f,6.17272f,2.60695f,-3.33977f,-10.37899f,-16.38823f,-19.72660f,-19.70115f,-16.80477f,-12.41185f,-7.91607f,-4.15177f,-1.10381f,1.22204f,1.82879f,0.23517f,-2.13784f,-3.62813f,-3.94630f,-3.34494f,-1.85961f,-0.22644f,0.26762f,-0.08803f,0.44077f,2.38763f,4.89878f,7.32401f,9.15291f,9.88349f},
+    {9.12464f,9.18177f,8.94457f,9.04553f,9.19618f,8.89744f,8.21223f,7.14260f,4.96349f,0.86055f,-4.97931f,-11.14357f,-15.85640f,-17.78532f,-16.57645f,-13.08478f,-8.67898f,-4.59243f,-1.60427f,0.55144f,2.39028f,3.23916f,2.31654f,0.46434f,-0.91732f,-1.53003f,-1.62589f,-1.18564f,-0.62403f,-0.86624f,-1.66562f,-1.50145f,0.21812f,2.79294f,5.56329f,7.91823f,9.12464f},
+    {8.06290f,8.93172f,9.29767f,9.81684f,10.30693f,10.20311f,9.30721f,7.53634f,4.41412f,-0.45099f,-6.41113f,-11.85029f,-15.25565f,-15.88209f,-13.96742f,-10.49677f,-6.46933f,-2.75504f,-0.08743f,1.66921f,3.12874f,3.96903f,3.45981f,2.05876f,0.88578f,0.20951f,-0.29117f,-0.68669f,-1.18101f,-2.27487f,-3.63054f,-3.95562f,-2.62221f,-0.08793f,3.01927f,6.01454f,8.06290f},
+    {6.31041f,8.41617f,9.93058f,11.21483f,12.13996f,12.23034f,11.14011f,8.66524f,4.49928f,-1.34733f,-7.71989f,-12.71272f,-15.08305f,-14.75641f,-12.45343f,-9.09705f,-5.35725f,-1.83470f,0.82823f,2.57001f,3.86519f,4.69588f,4.60243f,3.76104f,2.87601f,2.13967f,1.24833f,0.02788f,-1.62859f,-3.80231f,-5.90990f,-6.79346f,-5.87079f,-3.45535f,-0.15720f,3.34006f,6.31041f},
+    {4.26294f,7.59975f,10.43666f,12.70491f,14.16020f,14.43398f,13.18770f,10.10768f,4.90586f,-2.08386f,-9.15208f,-14.09196f,-15.93051f,-15.08268f,-12.51517f,-9.09089f,-5.35759f,-1.77919f,1.14514f,3.24292f,4.77489f,5.90082f,6.46683f,6.42854f,5.95440f,5.04613f,3.49019f,1.14215f,-1.96091f,-5.46021f,-8.43365f,-9.79880f,-9.13330f,-6.77311f,-3.34752f,0.50970f,4.26294f},
+    {2.57464f,6.81988f,10.72127f,13.95370f,16.10049f,16.72460f,15.39548f,11.64478f,5.15172f,-3.36781f,-11.49173f,-16.72206f,-18.40831f,-17.31430f,-14.50140f,-10.79977f,-6.74992f,-2.76545f,0.78143f,3.70100f,6.08013f,8.07892f,9.67963f,10.67456f,10.77058f,9.65347f,7.07600f,3.02043f,-2.09809f,-7.27683f,-11.16806f,-12.81861f,-12.13043f,-9.62141f,-5.97603f,-1.77704f,2.57464f},
+    {1.43503f,6.27921f,10.88019f,14.88261f,17.80063f,19.02158f,17.77297f,13.12636f,4.53718f,-6.54055f,-16.27050f,-21.85552f,-23.31365f,-21.85455f,-18.61101f,-14.36797f,-9.64057f,-4.79964f,-0.13239f,4.18385f,8.10605f,11.64519f,14.70734f,16.97990f,17.92977f,16.88820f,13.23164f,6.81146f,-1.34665f,-8.97876f,-14.01257f,-15.81005f,-14.85926f,-12.00033f,-7.99733f,-3.40265f,1.43502f},
+    {0.13094f,5.45479f,10.54962f,15.08235f,18.55436f,20.17754f,18.67574f,12.20150f,-0.36226f,-15.49225f,-26.46004f,-31.19380f,-31.33251f,-28.66356f,-24.33699f,-19.03627f,-13.18344f,-7.06335f,-0.88475f,5.19490f,11.05071f,16.55252f,21.50321f,25.56424f,28.16988f,28.41286f,24.95220f,16.43585f,3.63861f,-8.67079f,-16.18909f,-18.69045f,-17.68753f,-14.54612f,-10.18137f,-5.16795f,0.13094f},
+    {-4.14830f,1.12345f,5.96040f,9.84415f,11.94596f,10.80871f,4.02869f,-10.36405f,-27.94912f,-39.79617f,-44.16477f,-43.57950f,-40.12657f,-34.99189f,-28.83083f,-22.02403f,-14.80922f,-7.34799f,0.23881f,7.85050f,15.39197f,22.75942f,29.82156f,36.38924f,42.15868f,46.59144f,48.63821f,46.09967f,34.72041f,12.13936f,-8.98865f,-18.65098f,-20.47841f,-18.40133f,-14.39834f,-9.45818f,-4.14830f},
+    {-169.79948f,-159.79948f,-149.79948f,-139.79948f,-129.79948f,-119.79948f,-109.79948f,-99.79948f,-89.79948f,-79.79948f,-69.79948f,-59.79948f,-49.79948f,-39.79948f,-29.79948f,-19.79948f,-9.79948f,0.20052f,10.20052f,20.20052f,30.20052f,40.20052f,50.20052f,60.20052f,70.20052f,80.20052f,90.20052f,100.20052f,110.20052f,120.20052f,130.20052f,140.20052f,150.20052f,160.20052f,170.20052f,-179.79948f,-169.79948f}
 };
 
-#else /* !NAV_AUTO_MAG_DECLINATION_PRECISE */
-#define SAMPLING_RES		10.00000f
-#define SAMPLING_MIN_LON	-180.00000f
-#define SAMPLING_MAX_LON	180.00000f
-#define SAMPLING_MIN_LAT	-60.00000f
-#define SAMPLING_MAX_LAT	60.00000f
-
-static const int8_t declination_table[13][37] = {
-    {48,47,45,44,42,41,39,36,33,28,22,16,10,4,-1,-5,-10,-14,-20,-27,-35,-43,-50,-57,-63,-68,-72,-75,-76,-74,-62,-19,29,44,49,49,48},
-    {32,32,31,31,30,30,30,29,27,23,18,10,3,-4,-9,-12,-14,-17,-21,-27,-34,-42,-48,-53,-57,-58,-57,-52,-44,-30,-13,3,15,23,28,30,32},
-    {23,23,23,23,23,22,22,22,21,18,12,4,-5,-12,-17,-20,-21,-21,-22,-25,-31,-37,-42,-46,-46,-44,-39,-31,-21,-11,-3,4,10,15,19,21,23},
-    {17,18,18,18,17,17,17,16,16,13,7,-2,-11,-18,-22,-24,-24,-23,-21,-20,-22,-26,-30,-32,-31,-28,-22,-16,-9,-3,0,4,7,11,14,16,17},
-    {13,14,14,14,14,13,12,12,11,8,2,-7,-15,-21,-24,-24,-23,-20,-15,-10,-9,-11,-15,-18,-18,-16,-12,-7,-3,0,1,3,6,8,11,13,13},
-    {11,11,11,11,11,10,10,9,8,5,-1,-9,-17,-21,-23,-21,-18,-13,-8,-4,-2,-2,-5,-7,-9,-8,-6,-3,-1,1,1,2,4,6,9,11,11},
-    {10,10,9,9,9,9,8,8,6,2,-4,-11,-17,-20,-20,-17,-12,-8,-4,-1,1,2,0,-2,-4,-4,-3,-2,0,0,0,0,2,5,7,9,10},
-    {9,9,9,9,9,9,8,7,5,1,-5,-11,-16,-18,-16,-13,-8,-4,-1,1,3,3,2,0,-1,-1,-2,-1,-1,-1,-2,-2,0,3,6,8,9},
-    {8,9,9,10,10,10,9,7,4,-1,-7,-12,-15,-16,-14,-10,-6,-3,0,2,3,4,4,2,1,0,0,-1,-1,-2,-4,-4,-3,0,3,6,8},
-    {6,8,10,11,12,12,11,9,4,-1,-8,-13,-15,-15,-12,-9,-5,-2,1,3,4,5,5,4,3,2,1,0,-2,-4,-6,-7,-6,-4,0,3,6},
-    {4,7,10,13,14,14,13,10,5,-2,-9,-14,-16,-15,-12,-9,-5,-2,1,3,5,6,7,7,6,5,3,1,-2,-6,-9,-10,-9,-7,-3,0,4},
-    {2,7,11,14,16,17,15,12,5,-3,-11,-17,-18,-17,-14,-11,-6,-2,1,4,6,8,10,11,11,10,7,3,-2,-7,-11,-13,-12,-10,-6,-2,2},
-    {1,6,11,15,18,19,18,13,5,-6,-16,-21,-23,-21,-18,-14,-9,-4,0,4,8,12,15,17,18,17,13,7,-2,-9,-14,-16,-15,-12,-8,-4,1}
+const float inclination_table[19][37] = {
+    {-72.02070f,-72.02071f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02071f,-72.02070f,-72.02071f},
+    {-78.23208f,-77.46612f,-76.54604f,-75.51344f,-74.40408f,-73.25043f,-72.08420f,-70.93779f,-69.84384f,-68.83332f,-67.93241f,-67.15960f,-66.52407f,-66.02648f,-65.66205f,-65.42519f,-65.31392f,-65.33255f,-65.49161f,-65.80511f,-66.28626f,-66.94289f,-67.77395f,-68.76776f,-69.90201f,-71.14489f,-72.45664f,-73.79090f,-75.09549f,-76.31306f,-77.38240f,-78.24183f,-78.83644f,-79.12876f,-79.10879f,-78.79618f,-78.23208f},
+    {-80.80007f,-78.97830f,-77.14756f,-75.29021f,-73.38193f,-71.40769f,-69.37743f,-67.33857f,-65.37896f,-63.61481f,-62.16302f,-61.10460f,-60.45316f,-60.14557f,-60.06470f,-60.08698f,-60.13217f,-60.19219f,-60.32958f,-60.65151f,-61.27183f,-62.27481f,-63.69257f,-65.50302f,-67.64474f,-70.03800f,-72.60073f,-75.25455f,-77.92173f,-80.51496f,-82.91263f,-84.88736f,-85.95658f,-85.63251f,-84.30522f,-82.60297f,-80.80007f},
+    {-77.45516f,-75.43606f,-73.49604f,-71.57699f,-69.59653f,-67.45749f,-65.08505f,-62.48902f,-59.81600f,-57.35291f,-55.46368f,-54.46054f,-54.44855f,-55.23685f,-56.40404f,-57.49090f,-58.18917f,-58.42457f,-58.34278f,-58.24952f,-58.51231f,-59.42525f,-61.10217f,-63.46936f,-66.34503f,-69.53469f,-72.88656f,-76.29320f,-79.66218f,-82.88130f,-85.73520f,-87.38955f,-86.36194f,-84.15583f,-81.83294f,-79.58616f,-77.45517f},
+    {-71.58214f,-69.61565f,-67.71488f,-65.86479f,-64.01505f,-62.04510f,-59.77482f,-57.07400f,-54.02405f,-51.03212f,-48.80491f,-48.09823f,-49.26412f,-51.92460f,-55.16562f,-58.07793f,-60.10267f,-61.01513f,-60.84749f,-59.97822f,-59.15799f,-59.18575f,-60.45940f,-62.86069f,-65.97595f,-69.36736f,-72.71868f,-75.80268f,-78.35851f,-80.06262f,-80.67983f,-80.25871f,-79.08901f,-77.46953f,-75.59576f,-73.59787f,-71.58215f},
+    {-64.39807f,-62.40221f,-60.40960f,-58.42549f,-56.48151f,-54.55795f,-52.49863f,-50.03954f,-47.03044f,-43.80319f,-41.39118f,-41.16169f,-43.78688f,-48.51954f,-53.83971f,-58.60413f,-62.35170f,-64.81766f,-65.60605f,-64.62579f,-62.64650f,-61.08659f,-61.05364f,-62.68316f,-65.34905f,-68.24163f,-70.78236f,-72.62117f,-73.51797f,-73.52965f,-73.02048f,-72.24528f,-71.21594f,-69.88980f,-68.25886f,-66.38361f,-64.39807f},
+    {-55.02826f,-52.87120f,-50.69073f,-48.44731f,-46.19513f,-44.04849f,-42.00465f,-39.74123f,-36.79801f,-33.33110f,-30.77621f,-31.26765f,-35.68330f,-42.58861f,-49.78570f,-56.05256f,-61.22645f,-65.24992f,-67.52021f,-67.43115f,-65.26549f,-62.35447f,-60.45735f,-60.49007f,-61.96843f,-63.81210f,-65.26064f,-65.90429f,-65.53604f,-64.51130f,-63.52424f,-62.77292f,-61.94296f,-60.76846f,-59.16172f,-57.17787f,-55.02826f},
+    {-42.23934f,-39.72248f,-37.30606f,-34.84704f,-32.29950f,-29.84715f,-27.62882f,-25.25073f,-22.02346f,-18.13398f,-15.56621f,-17.09131f,-23.52381f,-32.78684f,-42.11183f,-49.91489f,-55.93881f,-60.34177f,-62.85919f,-63.04596f,-60.93631f,-57.41244f,-54.21948f,-52.82417f,-53.10885f,-54.02972f,-54.81946f,-54.92826f,-53.94911f,-52.38029f,-51.27875f,-50.79557f,-50.20937f,-49.05107f,-47.23568f,-44.84941f,-42.23934f},
+    {-25.31616f,-22.26353f,-19.64432f,-17.13765f,-14.48794f,-11.90452f,-9.56551f,-6.93532f,-3.35246f,0.62534f,2.69744f,0.20162f,-7.49683f,-18.49333f,-29.80260f,-38.99555f,-45.22087f,-48.81124f,-50.28975f,-49.88122f,-47.49942f,-43.57896f,-39.80201f,-37.81589f,-37.58213f,-38.12733f,-38.79073f,-38.91636f,-37.84560f,-36.15384f,-35.28930f,-35.35164f,-35.11620f,-33.90633f,-31.71407f,-28.67497f,-25.31616f},
+    {-5.22365f,-1.68002f,0.98130f,3.28459f,5.73141f,8.13291f,10.33797f,12.92226f,16.28019f,19.52644f,20.64919f,17.75076f,10.26968f,-0.68527f,-12.39654f,-21.81573f,-27.55757f,-29.98547f,-30.24089f,-29.18412f,-26.62224f,-22.51726f,-18.50023f,-16.36665f,-16.02599f,-16.47452f,-17.18522f,-17.56335f,-16.82114f,-15.49276f,-15.21405f,-16.04947f,-16.38744f,-15.33409f,-12.92178f,-9.31548f,-5.22365f},
+    {14.65727f,18.24457f,20.73560f,22.68750f,24.74090f,26.83478f,28.82717f,31.06453f,33.64927f,35.73650f,35.91607f,33.06931f,26.82267f,17.82545f,8.14938f,0.35249f,-4.22224f,-5.65846f,-5.08244f,-3.67265f,-1.30171f,2.36894f,6.00997f,7.96495f,8.31670f,8.01335f,7.45026f,6.96852f,7.19717f,7.76654f,7.31456f,5.77666f,4.68883f,5.07867f,7.05547f,10.50432f,14.65727f},
+    {31.00203f,34.03206f,36.22486f,37.93041f,39.71929f,41.67672f,43.64451f,45.63057f,47.51149f,48.64179f,48.13476f,45.46014f,40.64785f,34.34474f,27.90343f,22.77471f,19.78522f,19.08118f,19.96093f,21.39191f,23.28877f,25.96005f,28.61507f,30.09860f,30.42073f,30.29938f,30.04062f,29.74069f,29.65970f,29.53545f,28.53162f,26.62276f,24.91817f,24.37385f,25.31100f,27.72542f,31.00203f},
+    {43.33608f,45.45851f,47.28808f,48.92364f,50.69187f,52.68920f,54.75852f,56.70372f,58.24993f,58.87748f,58.03709f,55.61656f,52.01961f,47.93440f,44.13769f,41.24459f,39.61639f,39.39065f,40.22833f,41.47069f,42.89022f,44.59331f,46.23881f,47.24171f,47.57154f,47.63935f,47.66249f,47.62919f,47.50096f,47.00810f,45.71114f,43.68014f,41.65440f,40.38784f,40.27252f,41.37670f,43.33608f},
+    {53.10131f,54.36161f,55.82606f,57.45873f,59.32146f,61.39014f,63.50628f,65.43734f,66.85905f,67.31408f,66.44780f,64.38450f,61.68268f,58.96721f,56.68844f,55.08044f,54.25566f,54.24982f,54.89128f,55.82780f,56.83438f,57.87845f,58.85920f,59.59271f,60.06042f,60.40887f,60.71534f,60.90716f,60.81057f,60.15911f,58.74748f,56.73559f,54.64618f,53.02448f,52.20550f,52.28132f,53.10131f},
+    {61.90363f,62.59082f,63.73000f,65.25974f,67.10373f,69.13404f,71.16923f,72.97878f,74.24745f,74.58969f,73.79370f,72.08739f,69.99806f,68.01780f,66.44167f,65.38391f,64.86230f,64.84287f,65.21557f,65.80372f,66.46017f,67.12970f,67.80720f,68.49179f,69.18909f,69.89166f,70.52514f,70.92481f,70.86509f,70.14456f,68.73300f,66.86155f,64.92449f,63.29260f,62.20331f,61.74779f,61.90363f},
+    {70.57319f,70.97549f,71.81891f,73.05700f,74.60513f,76.33503f,78.07584f,79.60158f,80.60606f,80.76720f,79.99878f,78.58008f,76.92858f,75.37566f,74.11340f,73.22087f,72.70518f,72.52979f,72.62677f,72.91213f,73.31657f,73.81370f,74.42101f,75.17239f,76.07666f,77.07532f,78.01176f,78.63384f,78.66938f,77.98598f,76.70086f,75.10445f,73.50638f,72.14451f,71.16426f,70.63412f,70.57319f},
+    {78.82351f,79.06440f,79.60559f,80.41612f,81.44382f,82.60991f,83.79747f,84.82323f,85.41014f,85.29919f,84.52961f,83.39228f,82.15604f,80.99185f,79.99787f,79.22370f,78.68527f,78.37555f,78.27394f,78.35635f,78.60546f,79.01788f,79.60326f,80.37283f,81.31875f,82.38760f,83.44917f,84.26774f,84.54142f,84.12292f,83.18704f,82.03917f,80.91872f,79.97342f,79.28584f,78.89722f,78.82351f},
+    {85.92404f,85.99807f,86.21370f,86.55320f,86.98729f,87.46743f,87.90712f,88.15645f,88.05609f,87.61718f,86.98684f,86.28678f,85.58976f,84.94098f,84.37090f,83.90045f,83.54357f,83.30879f,83.20077f,83.22179f,83.37274f,83.65354f,84.06234f,84.59369f,85.23581f,85.96691f,86.74942f,87.51716f,88.14109f,88.39226f,88.15089f,87.63861f,87.08412f,86.59723f,86.22810f,86.00055f,85.92404f},
+    {88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f}
 };
 
-#endif
+const float intensity_table[19][37] = {
+    {0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f},
+    {0.60562f,0.59923f,0.59133f,0.58213f,0.57184f,0.56070f,0.54892f,0.53679f,0.52458f,0.51262f,0.50121f,0.49067f,0.48128f,0.47330f,0.46697f,0.46249f,0.46006f,0.45986f,0.46203f,0.46665f,0.47372f,0.48312f,0.49460f,0.50778f,0.52217f,0.53717f,0.55217f,0.56652f,0.57964f,0.59102f,0.60027f,0.60715f,0.61152f,0.61342f,0.61294f,0.61026f,0.60562f},
+    {0.62993f,0.61663f,0.60163f,0.58513f,0.56721f,0.54792f,0.52741f,0.50597f,0.48417f,0.46274f,0.44252f,0.42424f,0.40840f,0.39524f,0.38483f,0.37722f,0.37262f,0.37149f,0.37444f,0.38211f,0.39496f,0.41303f,0.43591f,0.46271f,0.49220f,0.52288f,0.55321f,0.58160f,0.60660f,0.62705f,0.64217f,0.65166f,0.65569f,0.65477f,0.64966f,0.64113f,0.62993f},
+    {0.61859f,0.59954f,0.57951f,0.55859f,0.53652f,0.51281f,0.48703f,0.45921f,0.43010f,0.40125f,0.37460f,0.35184f,0.33383f,0.32034f,0.31043f,0.30313f,0.29816f,0.29624f,0.29887f,0.30789f,0.32478f,0.35003f,0.38290f,0.42162f,0.46385f,0.50716f,0.54918f,0.58755f,0.62009f,0.64502f,0.66138f,0.66918f,0.66921f,0.66276f,0.65127f,0.63614f,0.61859f},
+    {0.58434f,0.56134f,0.53820f,0.51511f,0.49174f,0.46715f,0.44010f,0.40984f,0.37699f,0.34382f,0.31364f,0.28967f,0.27359f,0.26453f,0.25966f,0.25610f,0.25262f,0.24997f,0.25053f,0.25779f,0.27518f,0.30435f,0.34417f,0.39141f,0.44206f,0.49253f,0.54000f,0.58194f,0.61593f,0.64015f,0.65395f,0.65794f,0.65353f,0.64245f,0.62626f,0.60641f,0.58434f},
+    {0.53921f,0.51448f,0.48995f,0.46595f,0.44250f,0.41888f,0.39359f,0.36521f,0.33366f,0.30097f,0.27115f,0.24895f,0.23726f,0.23477f,0.23677f,0.23885f,0.23920f,0.23803f,0.23691f,0.23986f,0.25296f,0.28060f,0.32245f,0.37391f,0.42855f,0.48085f,0.52753f,0.56643f,0.59562f,0.61429f,0.62316f,0.62341f,0.61626f,0.60311f,0.58508f,0.56327f,0.53921f},
+    {0.48785f,0.46379f,0.43988f,0.41636f,0.39367f,0.37182f,0.34998f,0.32674f,0.30099f,0.27346f,0.24780f,0.22959f,0.22251f,0.22507f,0.23198f,0.23916f,0.24558f,0.25057f,0.25278f,0.25389f,0.26060f,0.28058f,0.31692f,0.36595f,0.41928f,0.46891f,0.51065f,0.54231f,0.56238f,0.57214f,0.57493f,0.57236f,0.56426f,0.55095f,0.53309f,0.51149f,0.48785f},
+    {0.43209f,0.41089f,0.38995f,0.36941f,0.34979f,0.33155f,0.31470f,0.29826f,0.28044f,0.26067f,0.24164f,0.22828f,0.22418f,0.22862f,0.23770f,0.24867f,0.26122f,0.27397f,0.28296f,0.28650f,0.28895f,0.29863f,0.32271f,0.36133f,0.40653f,0.44908f,0.48374f,0.50731f,0.51774f,0.51826f,0.51530f,0.51075f,0.50252f,0.48985f,0.47321f,0.45336f,0.43209f},
+    {0.37890f,0.36283f,0.34739f,0.33271f,0.31922f,0.30724f,0.29696f,0.28780f,0.27809f,0.26658f,0.25432f,0.24439f,0.23994f,0.24247f,0.25117f,0.26398f,0.27930f,0.29517f,0.30771f,0.31403f,0.31546f,0.31823f,0.33068f,0.35590f,0.38846f,0.42051f,0.44683f,0.46322f,0.46680f,0.46142f,0.45465f,0.44832f,0.43950f,0.42721f,0.41228f,0.39568f,0.37890f},
+    {0.34114f,0.33192f,0.32348f,0.31616f,0.31061f,0.30672f,0.30410f,0.30225f,0.29980f,0.29492f,0.28700f,0.27755f,0.26956f,0.26653f,0.27083f,0.28123f,0.29446f,0.30791f,0.31935f,0.32678f,0.32987f,0.33216f,0.33985f,0.35562f,0.37648f,0.39779f,0.41572f,0.42632f,0.42691f,0.42012f,0.41134f,0.40209f,0.39087f,0.37775f,0.36429f,0.35181f,0.34114f},
+    {0.32818f,0.32516f,0.32319f,0.32283f,0.32517f,0.32980f,0.33535f,0.34062f,0.34397f,0.34296f,0.33615f,0.32475f,0.31211f,0.30265f,0.30010f,0.30443f,0.31265f,0.32241f,0.33228f,0.34067f,0.34700f,0.35327f,0.36224f,0.37416f,0.38773f,0.40158f,0.41356f,0.42060f,0.42076f,0.41455f,0.40362f,0.38928f,0.37303f,0.35700f,0.34334f,0.33369f,0.32818f},
+    {0.33981f,0.34000f,0.34266f,0.34809f,0.35721f,0.36937f,0.38240f,0.39400f,0.40180f,0.40293f,0.39568f,0.38157f,0.36502f,0.35110f,0.34343f,0.34233f,0.34605f,0.35337f,0.36304f,0.37282f,0.38187f,0.39163f,0.40273f,0.41394f,0.42487f,0.43601f,0.44625f,0.45294f,0.45399f,0.44803f,0.43420f,0.41393f,0.39124f,0.37028f,0.35389f,0.34377f,0.33981f},
+    {0.37236f,0.37289f,0.37826f,0.38816f,0.40237f,0.41956f,0.43724f,0.45274f,0.46333f,0.46592f,0.45864f,0.44316f,0.42447f,0.40808f,0.39741f,0.39283f,0.39339f,0.39860f,0.40735f,0.41725f,0.42701f,0.43745f,0.44902f,0.46095f,0.47308f,0.48570f,0.49764f,0.50631f,0.50895f,0.50305f,0.48719f,0.46316f,0.43591f,0.41060f,0.39068f,0.37789f,0.37236f},
+    {0.42245f,0.42215f,0.42846f,0.44064f,0.45724f,0.47601f,0.49435f,0.50993f,0.52038f,0.52304f,0.51631f,0.50148f,0.48288f,0.46550f,0.45264f,0.44515f,0.44269f,0.44488f,0.45083f,0.45875f,0.46742f,0.47714f,0.48867f,0.50225f,0.51759f,0.53373f,0.54863f,0.55948f,0.56339f,0.55795f,0.54232f,0.51859f,0.49131f,0.46537f,0.44426f,0.42977f,0.42245f},
+    {0.48319f,0.48226f,0.48758f,0.49842f,0.51308f,0.52913f,0.54416f,0.55622f,0.56358f,0.56459f,0.55836f,0.54575f,0.52954f,0.51323f,0.49956f,0.48985f,0.48445f,0.48324f,0.48569f,0.49085f,0.49796f,0.50710f,0.51894f,0.53390f,0.55144f,0.56981f,0.58634f,0.59810f,0.60259f,0.59827f,0.58527f,0.56571f,0.54318f,0.52144f,0.50331f,0.49032f,0.48319f},
+    {0.53929f,0.53799f,0.54070f,0.54691f,0.55552f,0.56497f,0.57363f,0.58010f,0.58328f,0.58236f,0.57702f,0.56775f,0.55586f,0.54317f,0.53137f,0.52173f,0.51505f,0.51167f,0.51159f,0.51458f,0.52039f,0.52900f,0.54059f,0.55506f,0.57159f,0.58845f,0.60334f,0.61392f,0.61846f,0.61626f,0.60790f,0.59512f,0.58032f,0.56589f,0.55363f,0.54459f,0.53929f},
+    {0.57278f,0.57090f,0.57068f,0.57187f,0.57403f,0.57656f,0.57878f,0.58007f,0.57991f,0.57795f,0.57409f,0.56849f,0.56162f,0.55414f,0.54681f,0.54040f,0.53554f,0.53271f,0.53223f,0.53423f,0.53873f,0.54564f,0.55475f,0.56559f,0.57735f,0.58891f,0.59900f,0.60643f,0.61040f,0.61066f,0.60758f,0.60203f,0.59514f,0.58801f,0.58156f,0.57637f,0.57278f},
+    {0.57900f,0.57728f,0.57584f,0.57463f,0.57360f,0.57263f,0.57160f,0.57039f,0.56890f,0.56707f,0.56489f,0.56241f,0.55976f,0.55710f,0.55465f,0.55263f,0.55127f,0.55074f,0.55121f,0.55273f,0.55530f,0.55883f,0.56314f,0.56798f,0.57300f,0.57785f,0.58216f,0.58562f,0.58802f,0.58928f,0.58943f,0.58865f,0.58716f,0.58523f,0.58309f,0.58097f,0.57900f},
+    {0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f}
+};
\ No newline at end of file
diff --git a/src/main/target/common.h b/src/main/target/common.h
index b550df42d0f..0b878067c45 100644
--- a/src/main/target/common.h
+++ b/src/main/target/common.h
@@ -117,8 +117,6 @@
 
 #define NAV_NON_VOLATILE_WAYPOINT_CLI
 
-#define NAV_AUTO_MAG_DECLINATION_PRECISE
-
 #define USE_D_BOOST
 #define USE_ANTIGRAVITY
 
diff --git a/src/utils/declination.py b/src/utils/declination.py
index 44f5c9f21db..da0f805b392 100755
--- a/src/utils/declination.py
+++ b/src/utils/declination.py
@@ -1,133 +1,252 @@
-#!/usr/bin/env python
-
-# Ported from https://github.com/ArduPilot/ardupilot/tree/master/libraries/AP_Declination/generate
-# Run this script with python3!
-# To install the igrf module, use python3 -m pip install --user igrf12
-# Note that it requires a fortran compiler
-
-'''
-generate field tables from IGRF12
+#!/usr/bin/env python3
 '''
+generate field tables from IGRF13. Note that this requires python3
 
-import collections
-import datetime
-import pathlib
-import sys
-
-import igrf
-import numpy as np
+To run the generator you need the igrf module. Install like this:
 
-SAMPLING_RES = 10
-SAMPLING_MIN_LAT = -90
-SAMPLING_MAX_LAT = 90
-SAMPLING_MIN_LON = -180
-SAMPLING_MAX_LON = 180
+    python3 -m pip install --user igrf
 
-# This is used for flash constrained environments. We limit
-# the latitude range to [-60, 60], so the values fit in an int8_t
-SAMPLING_COMPACT_MIN_LAT = -60
-SAMPLING_COMPACT_MAX_LAT = 60
+And run:
 
-PREPROCESSOR_SYMBOL = 'NAV_AUTO_MAG_DECLINATION_PRECISE'
+    python3 src/utils/declination.py
 
-Query = collections.namedtuple('Query', ['date', 'res', 'min_lat', 'max_lat', 'min_lon', 'max_lon'])
-Result = collections.namedtuple('Result', ['query', 'lats', 'lons', 'declination', 'inclination', 'intensity'])
+It will updates the navigation_declination_gen.c code
+'''
 
-def write_table(f, name, table, compact):
+import igrf
+import numpy as np
+import datetime
+import pathlib
+from math import sin, cos, sqrt
+import math
+
+class Vector3(object):
+    '''a vector'''
+    def __init__(self, x=None, y=None, z=None):
+        if x is not None and y is not None and z is not None:
+            self.x = float(x)
+            self.y = float(y)
+            self.z = float(z)
+        elif x is not None and len(x) == 3:
+            self.x = float(x[0])
+            self.y = float(x[1])
+            self.z = float(x[2])
+        elif x is not None:
+            raise ValueError('bad initializer')
+        else:
+            self.x = float(0)
+            self.y = float(0)
+            self.z = float(0)
+
+    def length(self):
+        return sqrt(self.x**2 + self.y**2 + self.z**2)
+
+    def __sub__(self, other):
+        return Vector3(self.x - other.x, self.y - other.y, self.z - other.z)
+
+    def __mul__(self, other):
+        if isinstance(other, (int, float)):
+            return Vector3(self.x * other, self.y * other, self.z * other)
+        elif isinstance(other, Vector3):
+            return self.x * other.x + self.y * other.y + self.z * other.z
+        else:
+            raise ValueError('Multiplication with unsupported type')
+
+class Matrix3(object):
+    '''a 3x3 matrix, intended as a rotation matrix'''
+    def __init__(self, a=None, b=None, c=None):
+        if a is not None and b is not None and c is not None:
+            self.a = a.copy()
+            self.b = b.copy()
+            self.c = c.copy()
+        else:
+            self.identity()
+
+    def identity(self):
+        self.a = Vector3(1,0,0)
+        self.b = Vector3(0,1,0)
+        self.c = Vector3(0,0,1)
+
+    def from_euler(self, roll, pitch, yaw):
+        '''fill the matrix from Euler angles in radians'''
+        cp = cos(pitch)
+        sp = sin(pitch)
+        sr = sin(roll)
+        cr = cos(roll)
+        sy = sin(yaw)
+        cy = cos(yaw)
+
+        self.a.x = cp * cy
+        self.a.y = (sr * sp * cy) - (cr * sy)
+        self.a.z = (cr * sp * cy) + (sr * sy)
+        self.b.x = cp * sy
+        self.b.y = (sr * sp * sy) + (cr * cy)
+        self.b.z = (cr * sp * sy) - (sr * cy)
+        self.c.x = -sp
+        self.c.y = sr * cp
+        self.c.z = cr * cp
+
+    def __mul__(self, vector):
+        if isinstance(vector, Vector3):
+            x = self.a.x * vector.x + self.a.y * vector.y + self.a.z * vector.z
+            y = self.b.x * vector.x + self.b.y * vector.y + self.b.z * vector.z
+            z = self.c.x * vector.x + self.c.y * vector.y + self.c.z * vector.z
+            return Vector3(x, y, z)
+        else:
+            raise ValueError('Multiplication with unsupported type')
+
+def write_table(f, name, table):
     '''write one table'''
-
-    if compact:
-        format_entry = lambda x: '%d' % round(x)
-        table_type = 'int8_t'
-    else:
-        table_type = 'float'
-        format_entry = lambda x: '%.5ff' % x
-
-    num_lat = len(table)
-    num_lon = len(table[0])
-
-    f.write("static const %s %s[%u][%u] = {\n" %
-                (table_type, name, num_lat, num_lon))
-    for i in range(num_lat):
+    f.write("const float %s[%u][%u] = {\n" %
+                (name, NUM_LAT, NUM_LON))
+    for i in range(NUM_LAT):
         f.write("    {")
-        for j in range(num_lon):
-            f.write(format_entry(table[i][j]))
-            if j != num_lon - 1:
+        for j in range(NUM_LON):
+            f.write("%.5ff" % table[i][j])
+            if j != NUM_LON-1:
                 f.write(",")
         f.write("}")
-        if i != num_lat - 1:
+        if i != NUM_LAT-1:
             f.write(",")
         f.write("\n")
     f.write("};\n\n")
 
-def declination_tables(query):
-    lats = np.arange(query.min_lat, query.max_lat + query.res, query.res)
-    lons = np.arange(query.min_lon, query.max_lon + query.res, query.res)
-
-    num_lat = lats.size
-    num_lon = lons.size
-
-    intensity = np.empty((num_lat, num_lon))
-    inclination = np.empty((num_lat, num_lon))
-    declination = np.empty((num_lat, num_lon))
-
-    for i, lat in enumerate(lats):
-        for j, lon in enumerate(lons):
-            mag = igrf.igrf(date, glat=lat, glon=lon, alt_km=0., isv=0, itype=1)
-            intensity[i][j] = mag.total / 1e5
-            inclination[i][j] = mag.incl
-            declination[i][j] = mag.decl
-
-    return Result(query=query, lats=lats, lons=lons,
-        declination=declination, inclination=inclination, intensity=intensity)
-
-def generate_constants(f, query):
-    f.write('#define SAMPLING_RES\t\t%.5ff\n' % query.res)
-    f.write('#define SAMPLING_MIN_LON\t%.5ff\n' % query.min_lon)
-    f.write('#define SAMPLING_MAX_LON\t%.5ff\n' % query.max_lon)
-    f.write('#define SAMPLING_MIN_LAT\t%.5ff\n' % query.min_lat)
-    f.write('#define SAMPLING_MAX_LAT\t%.5ff\n' % query.max_lat)
-    f.write('\n')
-
-def generate_tables(f, query, compact):
-    result = declination_tables(query)
-    write_table(f, 'declination_table', result.declination, compact)
-
-    # We're not using these tables for now
-    #if not compact:
-    #    write_table(f, 'inclination_table', result.inclination, False)
-    #    write_table(f, 'intensity_table', result.intensity, False)
-
-def generate_code(f, date):
-
-    compact_query = Query(date=date, res=SAMPLING_RES,
-        min_lat=SAMPLING_COMPACT_MIN_LAT, max_lat=SAMPLING_COMPACT_MAX_LAT,
-        min_lon=SAMPLING_MIN_LON, max_lon=SAMPLING_MAX_LON)
+date = datetime.datetime.now()
 
-    precise_query = Query(date=date, res=SAMPLING_RES,
-        min_lat=SAMPLING_MIN_LAT, max_lat=SAMPLING_MAX_LAT,
-        min_lon=SAMPLING_MIN_LON, max_lon=SAMPLING_MAX_LON)
+SAMPLING_RES = 10
+SAMPLING_MIN_LAT = -90
+SAMPLING_MAX_LAT = 90
+SAMPLING_MIN_LON = -180
+SAMPLING_MAX_LON = 180
 
+lats = np.arange(SAMPLING_MIN_LAT, SAMPLING_MAX_LAT+SAMPLING_RES, SAMPLING_RES)
+lons = np.arange(SAMPLING_MIN_LON, SAMPLING_MAX_LON+SAMPLING_RES, SAMPLING_RES)
+
+NUM_LAT = lats.size
+NUM_LON = lons.size
+
+intensity_table = np.empty((NUM_LAT, NUM_LON))
+inclination_table = np.empty((NUM_LAT, NUM_LON))
+declination_table = np.empty((NUM_LAT, NUM_LON))
+
+max_error = 0
+max_error_pos = None
+max_error_field = None
+
+def get_igrf(lat, lon):
+    '''return field as [declination_deg, inclination_deg, intensity_gauss]'''
+    mag = igrf.igrf(date, glat=lat, glon=lon, alt_km=0., isv=0, itype=1)
+    intensity = float(mag.total/1e5)
+    inclination = float(mag.incl)
+    declination = float(mag.decl)
+    return [declination, inclination, intensity]
+
+def interpolate_table(table, latitude_deg, longitude_deg):
+    '''interpolate inside a table for a given lat/lon in degrees'''
+    # round down to nearest sampling resolution
+    min_lat = int(math.floor(latitude_deg / SAMPLING_RES) * SAMPLING_RES)
+    min_lon = int(math.floor(longitude_deg / SAMPLING_RES) * SAMPLING_RES)
+
+    # find index of nearest low sampling point
+    min_lat_index = int(math.floor((min_lat - SAMPLING_MIN_LAT) / SAMPLING_RES))
+    min_lon_index = int(math.floor((min_lon - SAMPLING_MIN_LON) / SAMPLING_RES))
+
+    # calculate intensity
+    data_sw = table[min_lat_index][min_lon_index]
+    data_se = table[min_lat_index][min_lon_index + 1]
+    data_ne = table[min_lat_index + 1][min_lon_index + 1]
+    data_nw = table[min_lat_index + 1][min_lon_index]
+
+    # perform bilinear interpolation on the four grid corners
+    data_min = ((longitude_deg - min_lon) / SAMPLING_RES) * (data_se - data_sw) + data_sw
+    data_max = ((longitude_deg - min_lon) / SAMPLING_RES) * (data_ne - data_nw) + data_nw
+
+    value = ((latitude_deg - min_lat) / SAMPLING_RES) * (data_max - data_min) + data_min
+    return value
+
+def interpolate_field(latitude_deg, longitude_deg):
+    '''calculate magnetic field intensity and orientation, interpolating in tables
+
+    returns array [declination_deg, inclination_deg, intensity] or None'''
+    # limit to table bounds
+    if latitude_deg < SAMPLING_MIN_LAT or latitude_deg >= SAMPLING_MAX_LAT:
+        return None
+    if longitude_deg < SAMPLING_MIN_LON or longitude_deg >= SAMPLING_MAX_LON:
+        return None
+
+    intensity_gauss = interpolate_table(intensity_table, latitude_deg, longitude_deg)
+    declination_deg = interpolate_table(declination_table, latitude_deg, longitude_deg)
+    inclination_deg = interpolate_table(inclination_table, latitude_deg, longitude_deg)
+
+    return [declination_deg, inclination_deg, intensity_gauss]
+
+def field_to_Vector3(mag):
+    '''return mGauss field from dec, inc and intensity'''
+    R = Matrix3()
+    mag_ef = Vector3(mag[2]*1000.0, 0.0, 0.0)
+    R.from_euler(0.0, -math.radians(mag[1]), math.radians(mag[0]))
+    return R * mag_ef
+
+def test_error(lat, lon):
+    '''check for error from lat, lon'''
+    global max_error, max_error_pos, max_error_field
+    mag1 = get_igrf(lat, lon)
+    mag2 = interpolate_field(lat, lon)
+    if mag2 is None:
+        return
+    ef1 = field_to_Vector3(mag1)
+    ef2 = field_to_Vector3(mag2)
+    err = (ef1 - ef2).length()
+    if err > max_error or err > 100:
+        print(lat, lon, err, ef1, ef2)
+        max_error = err
+        max_error_pos = (lat, lon)
+        max_error_field = ef1 - ef2
+
+def test_max_error(lat, lon):
+    '''check for maximum error from lat, lon over SAMPLING_RES range'''
+    steps = 3
+    delta = SAMPLING_RES/steps
+    for i in range(steps):
+        for j in range(steps):
+            lat2 = lat + i * delta
+            lon2 = lon + j * delta
+            if lat2 > SAMPLING_MAX_LAT or lon2 > SAMPLING_MAX_LON:
+                continue
+            if lat2 < SAMPLING_MIN_LAT or lon2 < SAMPLING_MIN_LON:
+                continue
+            test_error(lat2, lon2)
+
+for i, lat in enumerate(lats):
+    for j, lon in enumerate(lons):
+        mag = get_igrf(lat, lon)
+        declination_table[i][j] = mag[0]
+        inclination_table[i][j] = mag[1]
+        intensity_table[i][j] = mag[2]
+
+with open(pathlib.Path(__file__).parent / '..' / 'main' / 'navigation' / 'navigation_declination_gen.c', 'w') as f:
     f.write('/* this file is automatically generated by src/utils/declination.py - DO NOT EDIT! */\n\n\n')
     f.write('/* Updated on %s */\n\n\n' % date)
-    f.write('#include <stdint.h>\n\n')
-
-
-    f.write('\n\n#if defined(%s)\n' % PREPROCESSOR_SYMBOL)
-    generate_constants(f, precise_query)
-    generate_tables(f, precise_query, False)
-    # We're not using these tables for now
-    # write_table(f, 'inclination_table', inclination_table)
-    # write_table(f, 'intensity_table', intensity_table)
-    f.write('#else /* !%s */\n' % PREPROCESSOR_SYMBOL)
-    generate_constants(f, compact_query)
-    generate_tables(f, compact_query, True)
-    f.write('#endif\n')
-
-if __name__ == '__main__':
-
-    output = pathlib.PurePath(__file__).parent / '..' / 'main'  / 'navigation' / 'navigation_declination_gen.c'
-    date = datetime.datetime.now()
 
-    with open(output, 'w') as f:
-        generate_code(f, date)
+    f.write('''const float SAMPLING_RES = %u;
+const float SAMPLING_MIN_LAT = %u;
+const float SAMPLING_MAX_LAT = %u;
+const float SAMPLING_MIN_LON = %u;
+const float SAMPLING_MAX_LON = %u;
+
+''' % (SAMPLING_RES,
+           SAMPLING_MIN_LAT,
+           SAMPLING_MAX_LAT,
+           SAMPLING_MIN_LON,
+           SAMPLING_MAX_LON))
+
+    write_table(f, 'declination_table', declination_table)
+    write_table(f, 'inclination_table', inclination_table)
+    write_table(f, 'intensity_table', intensity_table)
+
+print("Checking for maximum error")
+for lat in range(-60, 60, 1):
+    for lon in range(-180, 180, 1):
+        test_max_error(lat, lon)
+print("Generated with max error %.2f %s at (%.2f,%.2f)" % (
+    max_error, max_error_field, max_error_pos[0], max_error_pos[1]))

From c9779c2dee475b3627d8cfa469963e2e27a5ea1c Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Tue, 14 May 2024 11:42:13 +0100
Subject: [PATCH 117/323] rth trackback tweaks

---
 src/main/navigation/navigation.c    |  8 ++------
 src/main/navigation/rth_trackback.c | 15 ++++++++-------
 src/main/navigation/rth_trackback.h |  2 +-
 3 files changed, 11 insertions(+), 14 deletions(-)

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index c776ce13f54..cc4e269ef4b 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -301,10 +301,6 @@ bool validateRTHSanityChecker(void);
 void updateHomePosition(void);
 bool abortLaunchAllowed(void);
 
-// static bool rthAltControlStickOverrideCheck(unsigned axis);
-// static void updateRthTrackback(bool forceSaveTrackPoint);
-// static fpVector3_t * rthGetTrackbackPos(void);
-
 #ifdef USE_FW_AUTOLAND
 static float getLandAltitude(void);
 static int32_t calcWindDiff(int32_t heading, int32_t windHeading);
@@ -1449,7 +1445,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
         }
         else {
             // Switch to RTH trackback
-            if (rthTrackBackIsActive() && rth_trackback.activePointIndex >= 0 && !isWaypointMissionRTHActive()) {
+            if (rthTrackBackCanBeActivated() && rth_trackback.activePointIndex >= 0 && !isWaypointMissionRTHActive()) {
                 rthTrackBackUpdate(true);  // save final trackpoint for altitude and max trackback distance reference
                 posControl.flags.rthTrackbackActive = true;
                 calculateAndSetActiveWaypointToLocalPosition(getRthTrackBackPosition());
@@ -1568,7 +1564,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_TRACKBACK(navigatio
         return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
     }
 
-    if (rthTrackBackSetNewPosition()) {
+    if (!rthTrackBackSetNewPosition()) {
         return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE;
     }
 
diff --git a/src/main/navigation/rth_trackback.c b/src/main/navigation/rth_trackback.c
index 37467d9b385..dfdcf801f7c 100644
--- a/src/main/navigation/rth_trackback.c
+++ b/src/main/navigation/rth_trackback.c
@@ -37,9 +37,10 @@
 
 rth_trackback_t rth_trackback;
 
-bool rthTrackBackIsActive(void) 
+bool rthTrackBackCanBeActivated(void)
 {
-    return navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_ON ||  (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_FS && posControl.flags.forcedRTHActivated);
+    return posControl.flags.estPosStatus >= EST_USABLE &&
+           (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_ON || (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_FS && posControl.flags.forcedRTHActivated));
 }
 
 void rthTrackBackUpdate(bool forceSaveTrackPoint)
@@ -127,7 +128,7 @@ void rthTrackBackUpdate(bool forceSaveTrackPoint)
 bool rthTrackBackSetNewPosition(void)
 {
     if (posControl.flags.estPosStatus == EST_NONE) {
-        return false;
+        return false;   // will fall back to RTH initialize allowing full RTH to handle position loss correctly
     }
 
     const int32_t distFromStartTrackback = CENTIMETERS_TO_METERS(calculateDistanceToDestination(&rth_trackback.pointsList[rth_trackback.lastSavedIndex]));
@@ -142,7 +143,7 @@ bool rthTrackBackSetNewPosition(void)
     if (rth_trackback.activePointIndex < 0 || cancelTrackback) {
         rth_trackback.WrapAroundCounter = rth_trackback.activePointIndex = -1;
         posControl.flags.rthTrackbackActive = false;
-        return true;    // Procede to home after final trackback point
+        return false;    // No more trackback points to set, procede to home
     }
 
     if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.bearing)) {
@@ -161,7 +162,7 @@ bool rthTrackBackSetNewPosition(void)
         setDesiredPosition(getRthTrackBackPosition(), 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
     }
 
-    return false;
+    return true;
 }
 
 fpVector3_t *getRthTrackBackPosition(void)
@@ -174,9 +175,9 @@ fpVector3_t *getRthTrackBackPosition(void)
     return &rth_trackback.pointsList[rth_trackback.activePointIndex];
 }
 
-void resetRthTrackBack(void) 
+void resetRthTrackBack(void)
 {
     rth_trackback.activePointIndex = -1;
     posControl.flags.rthTrackbackActive = false;
-    rth_trackback.WrapAroundCounter = -1;   
+    rth_trackback.WrapAroundCounter = -1;
 }
\ No newline at end of file
diff --git a/src/main/navigation/rth_trackback.h b/src/main/navigation/rth_trackback.h
index e3c71d0d45e..f3337bb4b79 100644
--- a/src/main/navigation/rth_trackback.h
+++ b/src/main/navigation/rth_trackback.h
@@ -35,7 +35,7 @@ typedef struct
 
 extern rth_trackback_t rth_trackback;
 
-bool rthTrackBackIsActive(void);
+bool rthTrackBackCanBeActivated(void);
 bool rthTrackBackSetNewPosition(void);
 void rthTrackBackUpdate(bool forceSaveTrackPoint);
 fpVector3_t *getRthTrackBackPosition(void);

From 9c64e9b65d1321af35acace2696001a2aff58f40 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Wed, 15 May 2024 11:44:47 +0100
Subject: [PATCH 118/323] baro altitude estimation fix

---
 docs/Settings.md                              | 10 ------
 src/main/fc/fc_msp_box.c                      |  2 +-
 src/main/fc/settings.yaml                     |  5 ---
 src/main/navigation/navigation.h              |  2 --
 .../navigation/navigation_pos_estimator.c     | 31 +++++++++++--------
 5 files changed, 19 insertions(+), 31 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index 30533330adf..14eb3b3dfce 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -1812,16 +1812,6 @@ Allows to chose when the home position is reset. Can help prevent resetting home
 
 ---
 
-### inav_use_gps_no_baro
-
-Defines if INAV should use only use GPS data for altitude estimation when barometer is not available. If set to ON, INAV will allow GPS assisted modes and RTH even when there is no barometer installed.
-
-| Default | Min | Max |
-| --- | --- | --- |
-| ON | OFF | ON |
-
----
-
 ### inav_w_acc_bias
 
 Weight for accelerometer drift estimation
diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c
index 2745f7e041b..7ffbbefd95e 100644
--- a/src/main/fc/fc_msp_box.c
+++ b/src/main/fc/fc_msp_box.c
@@ -215,7 +215,7 @@ void initActiveBoxIds(void)
 
     bool navReadyAltControl = getHwBarometerStatus() != HW_SENSOR_NONE;
 #ifdef USE_GPS
-    navReadyAltControl = navReadyAltControl || (feature(FEATURE_GPS) && (STATE(AIRPLANE) || positionEstimationConfig()->use_gps_no_baro));
+    navReadyAltControl = navReadyAltControl || feature(FEATURE_GPS);
 
     const bool navFlowDeadReckoning = sensors(SENSOR_OPFLOW) && sensors(SENSOR_ACC) && positionEstimationConfig()->allow_dead_reckoning;
     bool navReadyPosControl = sensors(SENSOR_ACC) && feature(FEATURE_GPS);
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index dfa7bb97d35..898c1b7be7a 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -2271,11 +2271,6 @@ groups:
         field: gravity_calibration_tolerance
         min: 0
         max: 255
-      - name: inav_use_gps_no_baro
-        description: "Defines if INAV should use only use GPS data for altitude estimation when barometer is not available. If set to ON, INAV will allow GPS assisted modes and RTH even when there is no barometer installed."
-        field: use_gps_no_baro
-        type: bool
-        default_value: ON
       - name: inav_allow_dead_reckoning
         description: "Defines if INAV will dead-reckon over short GPS outages. May also be useful for indoors OPFLOW navigation"
         default_value: OFF
diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
index 0613a19c24e..276fb61cc1e 100644
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@@ -260,8 +260,6 @@ typedef struct positionEstimationConfig_s {
     float max_eph_epv;  // Max estimated position error acceptable for estimation (cm)
     float baro_epv;     // Baro position error
 
-    uint8_t use_gps_no_baro;
-
 #ifdef USE_GPS_FIX_ESTIMATION
     uint8_t allow_gps_fix_estimation;
 #endif
diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
index 19018983cc5..30fa4012e46 100755
--- a/src/main/navigation/navigation_pos_estimator.c
+++ b/src/main/navigation/navigation_pos_estimator.c
@@ -56,7 +56,7 @@
 navigationPosEstimator_t posEstimator;
 static float initialBaroAltitudeOffset = 0.0f;
 
-PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 6);
+PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 7);
 
 PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
         // Inertial position estimator parameters
@@ -64,7 +64,6 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
         .reset_altitude_type = SETTING_INAV_RESET_ALTITUDE_DEFAULT,
         .reset_home_type = SETTING_INAV_RESET_HOME_DEFAULT,
         .gravity_calibration_tolerance = SETTING_INAV_GRAVITY_CAL_TOLERANCE_DEFAULT,  // 5 cm/s/s calibration error accepted (0.5% of gravity)
-        .use_gps_no_baro = SETTING_INAV_USE_GPS_NO_BARO_DEFAULT,                      // Use GPS altitude if no baro is available on all aircrafts
         .allow_dead_reckoning = SETTING_INAV_ALLOW_DEAD_RECKONING_DEFAULT,
 
         .max_surface_altitude = SETTING_INAV_MAX_SURFACE_ALTITUDE_DEFAULT,
@@ -180,12 +179,12 @@ void onNewGPSData(void)
     newLLH.lon = gpsSol.llh.lon;
     newLLH.alt = gpsSol.llh.alt;
 
-    if (sensors(SENSOR_GPS) 
+    if (sensors(SENSOR_GPS)
 #ifdef USE_GPS_FIX_ESTIMATION
             || STATE(GPS_ESTIMATED_FIX)
 #endif
         ) {
-        if (!(STATE(GPS_FIX) 
+        if (!(STATE(GPS_FIX)
 #ifdef USE_GPS_FIX_ESTIMATION
                 || STATE(GPS_ESTIMATED_FIX)
 #endif
@@ -553,15 +552,20 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
     DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount());            // Clip count
 
     bool correctOK = false;
-    
-    //ignore baro if difference is too big, baro is probably wrong
-    const float gpsBaroResidual = ctx->newFlags & EST_GPS_Z_VALID ? fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt) : 0.0f;
-    //fade out the baro to prevent sudden jump
-    const float start_epv = positionEstimationConfig()->max_eph_epv;
-    const float end_epv = positionEstimationConfig()->max_eph_epv * 2.0f;
-    const float wBaro = scaleRangef(constrainf(gpsBaroResidual, start_epv, end_epv), start_epv, end_epv, 1.0f, 0.0f);
-    //use both baro and gps
-    if ((ctx->newFlags & EST_BARO_VALID) && (!positionEstimationConfig()->use_gps_no_baro) && (wBaro > 0.01f)) {
+
+    float wBaro = 0.0f;
+    if (ctx->newFlags & EST_BARO_VALID) {
+        // Ignore baro if difference is too big, baro is probably wrong
+        const float gpsBaroResidual = ctx->newFlags & EST_GPS_Z_VALID ? fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt) : 0.0f;
+
+        // Fade out the baro to prevent sudden jump
+        const float start_epv = positionEstimationConfig()->max_eph_epv;
+        const float end_epv = positionEstimationConfig()->max_eph_epv * 2.0f;
+        wBaro = scaleRangef(constrainf(gpsBaroResidual, start_epv, end_epv), start_epv, end_epv, 1.0f, 0.0f);
+    }
+
+    // Always use Baro if no GPS otherwise only use if accuracy OK compared to GPS
+    if (wBaro > 0.01f) {
         timeUs_t currentTimeUs = micros();
 
         if (!ARMING_FLAG(ARMED)) {
@@ -597,6 +601,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
 
         correctOK = true;
     }
+
     if (ctx->newFlags & EST_GPS_Z_VALID) {
         // Reset current estimate to GPS altitude if estimate not valid
         if (!(ctx->newFlags & EST_Z_VALID)) {

From 283baa7c0a28685ec184dd1f57a822009c46480b Mon Sep 17 00:00:00 2001
From: Darren Lines <darren@darrenlines.uk>
Date: Wed, 15 May 2024 20:01:08 +0100
Subject: [PATCH 119/323] Initial changes

---
 src/main/CMakeLists.txt                       |   4 +-
 src/main/drivers/osd.h                        |   4 +-
 src/main/drivers/osd_symbols.h                |   6 +-
 src/main/io/bf_osd_symbols.h                  | 163 ----
 src/main/io/displayport_msp_bf_compat.c       | 748 ------------------
 src/main/io/displayport_msp_dji_compat.c      | 713 +++++++++++++++++
 ..._compat.h => displayport_msp_dji_compat.h} |  12 +-
 src/main/io/displayport_msp_osd.c             |  18 +-
 src/main/io/dji_osd_symbols.h                 | 164 ++++
 src/main/io/osd.c                             | 127 +--
 src/main/io/osd_grid.c                        |   8 +-
 src/main/io/osd_hud.c                         |   4 +-
 src/main/io/osd_utils.c                       |   4 +-
 13 files changed, 948 insertions(+), 1027 deletions(-)
 delete mode 100644 src/main/io/bf_osd_symbols.h
 delete mode 100644 src/main/io/displayport_msp_bf_compat.c
 create mode 100644 src/main/io/displayport_msp_dji_compat.c
 rename src/main/io/{displayport_msp_bf_compat.h => displayport_msp_dji_compat.h} (69%)
 create mode 100644 src/main/io/dji_osd_symbols.h

diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt
index 28c068966f8..c87abc1782d 100755
--- a/src/main/CMakeLists.txt
+++ b/src/main/CMakeLists.txt
@@ -500,8 +500,8 @@ main_sources(COMMON_SRC
     io/displayport_max7456.h
     io/displayport_msp.c
     io/displayport_msp.h
-    io/displayport_msp_bf_compat.c
-    io/displayport_msp_bf_compat.h
+    io/displayport_msp_dji_compat.c
+    io/displayport_msp_dji_compat.h
     io/displayport_oled.c
     io/displayport_oled.h
     io/displayport_msp_osd.c
diff --git a/src/main/drivers/osd.h b/src/main/drivers/osd.h
index e9ae4d5ef3f..a8b0bbc8e42 100644
--- a/src/main/drivers/osd.h
+++ b/src/main/drivers/osd.h
@@ -49,8 +49,8 @@ typedef enum {
     VIDEO_SYSTEM_HDZERO,
     VIDEO_SYSTEM_DJIWTF,
     VIDEO_SYSTEM_AVATAR,
-    VIDEO_SYSTEM_BFCOMPAT,
-    VIDEO_SYSTEM_BFCOMPAT_HD
+    VIDEO_SYSTEM_DJICOMPAT,
+    VIDEO_SYSTEM_DJICOMPAT_HD
 } videoSystem_e;
 
 typedef enum {
diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h
index 91cdcbb5f6b..90c0bc97131 100644
--- a/src/main/drivers/osd_symbols.h
+++ b/src/main/drivers/osd_symbols.h
@@ -44,9 +44,9 @@
 #define SYM_DBM                     0x13  // 019 dBm
 #define SYM_SNR                     0x14  // 020 SNR
 
-#define SYM_AH_DIRECTION_UP         0x15  // 021 Arrow up AHI
-#define SYM_AH_DIRECTION_DOWN       0x16  // 022 Arrow down AHI
-#define SYM_DIRECTION               0x17  // 023 to 030, directional little arrows
+#define SYM_AH_DECORATION_UP        0x15  // 021 Arrow up AHI
+#define SYM_AH_DECORATION_DOWN      0x16  // 022 Arrow down AHI
+#define SYM_DECORATION              0x17  // 023 to 030, directional little arrows
 
 #define SYM_VOLT                    0x1F  // 031 VOLTS
 #define SYM_MAH                     0x99  // 153 mAh
diff --git a/src/main/io/bf_osd_symbols.h b/src/main/io/bf_osd_symbols.h
deleted file mode 100644
index a63b9470fb3..00000000000
--- a/src/main/io/bf_osd_symbols.h
+++ /dev/null
@@ -1,163 +0,0 @@
-/* @file max7456_symbols.h
- * @brief max7456 symbols for the mwosd font set
- *
- * @author Nathan Tsoi nathan@vertile.com
- *
- * Copyright (C) 2016 Nathan Tsoi
- *
- * This program 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.
- *
- * This program 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
-
-//Misc
-#define BF_SYM_NONE                    0x00
-#define BF_SYM_END_OF_FONT             0xFF
-#define BF_SYM_BLANK                   0x20
-#define BF_SYM_HYPHEN                  0x2D
-#define BF_SYM_BBLOG                   0x10
-#define BF_SYM_HOMEFLAG                0x11
-#define BF_SYM_RPM                     0x12
-#define BF_SYM_ROLL                    0x14
-#define BF_SYM_PITCH                   0x15
-#define BF_SYM_TEMPERATURE             0x7A
-
-// GPS and navigation
-#define BF_SYM_LAT                     0x89
-#define BF_SYM_LON                     0x98
-#define BF_SYM_ALTITUDE                0x7F
-#define BF_SYM_TOTAL_DISTANCE          0x71
-#define BF_SYM_OVER_HOME               0x05
-
-// RSSI
-#define BF_SYM_RSSI                    0x01
-#define BF_SYM_LINK_QUALITY            0x7B
-
-// Throttle Position (%)
-#define BF_SYM_THR                     0x04
-
-// Unit Icons (Metric)
-#define BF_SYM_M                       0x0C
-#define BF_SYM_KM                      0x7D
-#define BF_SYM_C                       0x0E
-
-// Unit Icons (Imperial)
-#define BF_SYM_FT                      0x0F
-#define BF_SYM_MILES                   0x7E
-#define BF_SYM_F                       0x0D
-
-// Heading Graphics
-#define BF_SYM_HEADING_N               0x18
-#define BF_SYM_HEADING_S               0x19
-#define BF_SYM_HEADING_E               0x1A
-#define BF_SYM_HEADING_W               0x1B
-#define BF_SYM_HEADING_DIVIDED_LINE    0x1C
-#define BF_SYM_HEADING_LINE            0x1D
-
-// AH Center screen Graphics
-#define BF_SYM_AH_CENTER_LINE          0x72
-#define BF_SYM_AH_CENTER               0x73
-#define BF_SYM_AH_CENTER_LINE_RIGHT    0x74
-#define BF_SYM_AH_RIGHT                0x02
-#define BF_SYM_AH_LEFT                 0x03
-#define BF_SYM_AH_DECORATION           0x13
-
-// Satellite Graphics
-#define BF_SYM_SAT_L                   0x1E
-#define BF_SYM_SAT_R                   0x1F
-
-// Direction arrows
-#define BF_SYM_ARROW_SOUTH             0x60
-#define BF_SYM_ARROW_2                 0x61
-#define BF_SYM_ARROW_3                 0x62
-#define BF_SYM_ARROW_4                 0x63
-#define BF_SYM_ARROW_EAST              0x64
-#define BF_SYM_ARROW_6                 0x65
-#define BF_SYM_ARROW_7                 0x66
-#define BF_SYM_ARROW_8                 0x67
-#define BF_SYM_ARROW_NORTH             0x68
-#define BF_SYM_ARROW_10                0x69
-#define BF_SYM_ARROW_11                0x6A
-#define BF_SYM_ARROW_12                0x6B
-#define BF_SYM_ARROW_WEST              0x6C
-#define BF_SYM_ARROW_14                0x6D
-#define BF_SYM_ARROW_15                0x6E
-#define BF_SYM_ARROW_16                0x6F
-
-#define BF_SYM_ARROW_SMALL_UP          0x75
-#define BF_SYM_ARROW_SMALL_DOWN        0x76
-
-// AH Bars
-#define BF_SYM_AH_BAR9_0               0x80
-#define BF_SYM_AH_BAR9_1               0x81
-#define BF_SYM_AH_BAR9_2               0x82
-#define BF_SYM_AH_BAR9_3               0x83
-#define BF_SYM_AH_BAR9_4               0x84
-#define BF_SYM_AH_BAR9_5               0x85
-#define BF_SYM_AH_BAR9_6               0x86
-#define BF_SYM_AH_BAR9_7               0x87
-#define BF_SYM_AH_BAR9_8               0x88
-
-// Progress bar
-#define BF_SYM_PB_START                0x8A
-#define BF_SYM_PB_FULL                 0x8B
-#define BF_SYM_PB_HALF                 0x8C
-#define BF_SYM_PB_EMPTY                0x8D
-#define BF_SYM_PB_END                  0x8E
-#define BF_SYM_PB_CLOSE                0x8F
-
-// Batt evolution
-#define BF_SYM_BATT_FULL               0x90
-#define BF_SYM_BATT_5                  0x91
-#define BF_SYM_BATT_4                  0x92
-#define BF_SYM_BATT_3                  0x93
-#define BF_SYM_BATT_2                  0x94
-#define BF_SYM_BATT_1                  0x95
-#define BF_SYM_BATT_EMPTY              0x96
-
-// Batt Icons
-#define BF_SYM_MAIN_BATT               0x97
-
-// Voltage and amperage
-#define BF_SYM_VOLT                    0x06
-#define BF_SYM_AMP                     0x9A
-#define BF_SYM_MAH                     0x07
-#define BF_SYM_WATT                    0x57  // 0x57 is 'W'
-
-// Time
-#define BF_SYM_ON_M                    0x9B
-#define BF_SYM_FLY_M                   0x9C
-
-// Speed
-#define BF_SYM_SPEED                   0x70
-#define BF_SYM_KPH                     0x9E
-#define BF_SYM_MPH                     0x9D
-#define BF_SYM_MPS                     0x9F
-#define BF_SYM_FTPS                    0x99
-
-// Menu cursor
-#define BF_SYM_CURSOR                  BF_SYM_AH_LEFT
-
-// Stick overlays
-#define BF_SYM_STICK_OVERLAY_SPRITE_HIGH 0x08
-#define BF_SYM_STICK_OVERLAY_SPRITE_MID  0x09
-#define BF_SYM_STICK_OVERLAY_SPRITE_LOW  0x0A
-#define BF_SYM_STICK_OVERLAY_CENTER      0x0B
-#define BF_SYM_STICK_OVERLAY_VERTICAL    0x16
-#define BF_SYM_STICK_OVERLAY_HORIZONTAL  0x17
-
-// GPS degree/minute/second symbols
-#define BF_SYM_GPS_DEGREE              BF_SYM_STICK_OVERLAY_SPRITE_HIGH  // kind of looks like the degree symbol
-#define BF_SYM_GPS_MINUTE              0x27 // '
-#define BF_SYM_GPS_SECOND              0x22 // "
diff --git a/src/main/io/displayport_msp_bf_compat.c b/src/main/io/displayport_msp_bf_compat.c
deleted file mode 100644
index e7f75ec2fa5..00000000000
--- a/src/main/io/displayport_msp_bf_compat.c
+++ /dev/null
@@ -1,748 +0,0 @@
-/*
- * This file is part of INAV Project.
- *
- * 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.
- *
- * 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 "platform.h"
-
-#if defined(USE_OSD) && defined(USE_MSP_DISPLAYPORT)
-
-#ifndef DISABLE_MSP_BF_COMPAT
-
-#include "io/displayport_msp_bf_compat.h"
-#include "io/bf_osd_symbols.h"
-#include "drivers/osd_symbols.h"
-
-uint8_t getBfCharacter(uint8_t ch, uint8_t page)
-{
-    uint16_t ech = ch | ((page & 0x3)<< 8) ;
-
-    if (ech >= 0x20 && ech <= 0x5F) { // ASCII range
-        return ch;
-    }
-
-    if (ech >= SYM_AH_DECORATION_MIN && ech <= SYM_AH_DECORATION_MAX) {
-        return BF_SYM_AH_DECORATION;
-    }
-
-    switch (ech) {
-    case SYM_RSSI:
-        return BF_SYM_RSSI;
-
-    case SYM_LQ:
-        return BF_SYM_LINK_QUALITY;
-
-    case SYM_LAT:
-        return BF_SYM_LAT;
-
-    case SYM_LON:
-        return BF_SYM_LON;
-
-    case SYM_SAT_L:
-        return BF_SYM_SAT_L;
-
-    case SYM_SAT_R:
-        return BF_SYM_SAT_R;
-
-    case SYM_HOME_NEAR:
-        return BF_SYM_HOMEFLAG;
-
-    case SYM_DEGREES:
-        return BF_SYM_GPS_DEGREE;
-
-/*
-    case SYM_HEADING:
-        return BF_SYM_HEADING;
-
-    case SYM_SCALE:
-        return BF_SYM_SCALE;
-
-    case SYM_HDP_L:
-        return BF_SYM_HDP_L;
-
-    case SYM_HDP_R:
-        return BF_SYM_HDP_R;
-*/
-    case SYM_HOME:
-        return BF_SYM_HOMEFLAG;
-
-    case SYM_2RSS:
-        return BF_SYM_RSSI;
-
-/*
-    case SYM_DB:
-        return BF_SYM_DB
-
-    case SYM_DBM:
-        return BF_SYM_DBM;
-
-    case SYM_SNR:
-        return BF_SYM_SNR;
-
-    case SYM_AH_DECORATION_UP:
-        return BF_SYM_AH_DECORATION_UP;
-
-    case SYM_AH_DECORATION_DOWN:
-        return BF_SYM_AH_DECORATION_DOWN;
-*/
-    case SYM_DIRECTION:
-        return BF_SYM_ARROW_NORTH;
-    
-    case SYM_DIRECTION + 1: // NE pointing arrow
-        return BF_SYM_ARROW_7;
-
-    case SYM_DIRECTION + 2: // E pointing arrow
-        return BF_SYM_ARROW_EAST;
-
-    case SYM_DIRECTION + 3: // SE pointing arrow
-        return BF_SYM_ARROW_3;
-
-    case SYM_DIRECTION + 4: // S pointing arrow
-        return BF_SYM_ARROW_SOUTH;
-
-    case SYM_DIRECTION + 5: // SW pointing arrow
-        return BF_SYM_ARROW_15;
-
-    case SYM_DIRECTION + 6: // W pointing arrow
-        return BF_SYM_ARROW_WEST;
-
-    case SYM_DIRECTION + 7: // NW pointing arrow
-        return BF_SYM_ARROW_11;
-
-    case SYM_VOLT:
-        return BF_SYM_VOLT;
-
-    case SYM_MAH:
-        return BF_SYM_MAH;
-
-    case SYM_AH_KM:
-        return BF_SYM_KM;
-
-    case SYM_AH_MI:
-        return BF_SYM_MILES;
-/*
-    case SYM_VTX_POWER:
-        return BF_SYM_VTX_POWER;
-
-    case SYM_AH_NM:
-        return BF_SYM_AH_NM;
-
-    case SYM_MAH_NM_0:
-        return BF_SYM_MAH_NM_0;
-
-    case SYM_MAH_NM_1:
-        return BF_SYM_MAH_NM_1;
-
-    case SYM_MAH_KM_0:
-        return BF_SYM_MAH_KM_0;
-
-    case SYM_MAH_KM_1:
-        return BF_SYM_MAH_KM_1;
-
-    case SYM_MILLIOHM:
-        return BF_SYM_MILLIOHM;
-*/
-    case SYM_BATT_FULL:
-        return BF_SYM_BATT_FULL;
-
-    case SYM_BATT_5:
-        return BF_SYM_BATT_5;
-
-    case SYM_BATT_4:
-        return BF_SYM_BATT_4;
-
-    case SYM_BATT_3:
-        return BF_SYM_BATT_3;
-
-    case SYM_BATT_2:
-        return BF_SYM_BATT_2;
-
-    case SYM_BATT_1:
-        return BF_SYM_BATT_1;
-
-    case SYM_BATT_EMPTY:
-        return BF_SYM_BATT_EMPTY;
-
-    case SYM_AMP:
-        return BF_SYM_AMP;
-/*
-    case SYM_WH:
-        return BF_SYM_WH;
-
-    case SYM_WH_KM:
-        return BF_SYM_WH_KM;
-
-    case SYM_WH_MI:
-        return BF_SYM_WH_MI;
-
-    case SYM_WH_NM:
-        return BF_SYM_WH_NM;
-*/
-    case SYM_WATT:
-        return BF_SYM_WATT;
-/*
-    case SYM_MW:
-        return BF_SYM_MW;
-
-    case SYM_KILOWATT:
-        return BF_SYM_KILOWATT;
-*/
-    case SYM_FT:
-        return BF_SYM_FT;
-
-    case SYM_ALT_FT:
-        return BF_SYM_FT;
-
-    case SYM_ALT_M:
-        return BF_SYM_M;
-
-    case SYM_TOTAL:
-        return BF_SYM_TOTAL_DISTANCE;
-/*
-    case SYM_ALT_KM:
-        return BF_SYM_ALT_KM;
-
-    case SYM_ALT_KFT:
-        return BF_SYM_ALT_KFT;
-
-    case SYM_DIST_M:
-        return BF_SYM_DIST_M;
-
-    case SYM_DIST_KM:
-        return BF_SYM_DIST_KM;
-
-    case SYM_DIST_FT:
-        return BF_SYM_DIST_FT;
-
-    case SYM_DIST_MI:
-        return BF_SYM_DIST_MI;
-
-    case SYM_DIST_NM:
-        return BF_SYM_DIST_NM;
-*/
-    case SYM_M:
-        return BF_SYM_M;
-
-    case SYM_KM:
-        return BF_SYM_KM;
-
-    case SYM_MI:
-        return BF_SYM_MILES;
-/*
-    case SYM_NM:
-        return BF_SYM_NM;
-*/
-    case SYM_WIND_HORIZONTAL:
-        return 'W';     // W for wind
-
-/*
-    case SYM_WIND_VERTICAL:
-        return BF_SYM_WIND_VERTICAL;
-
-    case SYM_3D_KT:
-        return BF_SYM_3D_KT;
-*/
-    case SYM_AIR:
-        return 'A';     // A for airspeed
-
-    case SYM_3D_KMH:
-        return BF_SYM_KPH;
-
-    case SYM_3D_MPH:
-        return BF_SYM_MPH;
-
-    case SYM_RPM:
-        return BF_SYM_RPM;
-
-    case SYM_FTS:
-        return BF_SYM_FTPS;
-/*
-    case SYM_100FTM:
-        return BF_SYM_100FTM;
-*/
-    case SYM_MS:
-        return BF_SYM_MPS;
-
-    case SYM_KMH:
-        return BF_SYM_KPH;
-
-    case SYM_MPH:
-        return BF_SYM_MPH;
-/*
-    case SYM_KT:
-        return BF_SYM_KT
-
-    case SYM_MAH_MI_0:
-        return BF_SYM_MAH_MI_0;
-
-    case SYM_MAH_MI_1:
-        return BF_SYM_MAH_MI_1;
-*/
-    case SYM_THR:
-        return BF_SYM_THR;
-
-    case SYM_TEMP_F:
-        return BF_SYM_F;
-
-    case SYM_TEMP_C:
-        return BF_SYM_C;
-
-    case SYM_BLANK:
-        return BF_SYM_BLANK;
-/*
-    case SYM_ON_H:
-        return BF_SYM_ON_H;
-
-    case SYM_FLY_H:
-        return BF_SYM_FLY_H;
-*/
-    case SYM_ON_M:
-        return BF_SYM_ON_M;
-
-    case SYM_FLY_M:
-        return BF_SYM_FLY_M;
-/*
-    case SYM_GLIDESLOPE:
-        return BF_SYM_GLIDESLOPE;
-
-    case SYM_WAYPOINT:
-        return BF_SYM_WAYPOINT;
-
-    case SYM_CLOCK:
-        return BF_SYM_CLOCK;
-
-    case SYM_ZERO_HALF_TRAILING_DOT:
-        return BF_SYM_ZERO_HALF_TRAILING_DOT;
-
-    case SYM_ZERO_HALF_LEADING_DOT:
-        return BF_SYM_ZERO_HALF_LEADING_DOT;
-*/
-
-    case SYM_AUTO_THR0:
-        return 'A';
-
-    case SYM_AUTO_THR1:
-        return BF_SYM_THR;
-
-    case SYM_ROLL_LEFT:
-        return BF_SYM_ROLL;
-
-    case SYM_ROLL_LEVEL:
-        return BF_SYM_ROLL;
-
-    case SYM_ROLL_RIGHT:
-        return BF_SYM_ROLL;
-
-    case SYM_PITCH_UP:
-        return BF_SYM_PITCH;
-
-    case SYM_PITCH_DOWN:
-        return BF_SYM_PITCH;
-
-    case SYM_GFORCE:
-        return 'G';
-
-/*
-    case SYM_GFORCE_X:
-        return BF_SYM_GFORCE_X;
-
-    case SYM_GFORCE_Y:
-        return BF_SYM_GFORCE_Y;
-
-    case SYM_GFORCE_Z:
-        return BF_SYM_GFORCE_Z;
-
-    case SYM_BARO_TEMP:
-        return BF_SYM_BARO_TEMP;
-
-    case SYM_IMU_TEMP:
-        return BF_SYM_IMU_TEMP;
-
-    case SYM_TEMP:
-        return BF_SYM_TEMP;
-
-    case SYM_TEMP_SENSOR_FIRST:
-        return BF_SYM_TEMP_SENSOR_FIRST;
-
-    case SYM_ESC_TEMP:
-        return BF_SYM_ESC_TEMP;
-
-    case SYM_TEMP_SENSOR_LAST:
-        return BF_SYM_TEMP_SENSOR_LAST;
-
-    case TEMP_SENSOR_SYM_COUNT:
-        return BF_TEMP_SENSOR_SYM_COUNT;
-*/
-    case SYM_HEADING_N:
-        return BF_SYM_HEADING_N;
-
-    case SYM_HEADING_S:
-        return BF_SYM_HEADING_S;
-
-    case SYM_HEADING_E:
-        return BF_SYM_HEADING_E;
-
-    case SYM_HEADING_W:
-        return BF_SYM_HEADING_W;
-
-    case SYM_HEADING_DIVIDED_LINE:
-        return BF_SYM_HEADING_DIVIDED_LINE;
-
-    case SYM_HEADING_LINE:
-        return BF_SYM_HEADING_LINE;
-/*
-    case SYM_MAX:
-        return BF_SYM_MAX;
-
-    case SYM_PROFILE:
-        return BF_SYM_PROFILE;
-
-    case SYM_SWITCH_INDICATOR_LOW:
-        return BF_SYM_SWITCH_INDICATOR_LOW;
-
-    case SYM_SWITCH_INDICATOR_MID:
-        return BF_SYM_SWITCH_INDICATOR_MID;
-
-    case SYM_SWITCH_INDICATOR_HIGH:
-        return BF_SYM_SWITCH_INDICATOR_HIGH;
-
-    case SYM_AH:
-        return BF_SYM_AH;
-
-    case SYM_GLIDE_DIST:
-        return BF_SYM_GLIDE_DIST;
-
-    case SYM_GLIDE_MINS:
-        return BF_SYM_GLIDE_MINS;
-
-    case SYM_AH_V_FT_0:
-        return BF_SYM_AH_V_FT_0;
-
-    case SYM_AH_V_FT_1:
-        return BF_SYM_AH_V_FT_1;
-
-    case SYM_AH_V_M_0:
-        return BF_SYM_AH_V_M_0;
-
-    case SYM_AH_V_M_1:
-        return BF_SYM_AH_V_M_1;
-
-    case SYM_FLIGHT_MINS_REMAINING:
-        return BF_SYM_FLIGHT_MINS_REMAINING;
-
-    case SYM_FLIGHT_HOURS_REMAINING:
-        return BF_SYM_FLIGHT_HOURS_REMAINING;
-
-    case SYM_GROUND_COURSE:
-        return BF_SYM_GROUND_COURSE;
-
-    case SYM_CROSS_TRACK_ERROR:
-        return BF_SYM_CROSS_TRACK_ERROR;
-
-    case SYM_LOGO_START:
-        return BF_SYM_LOGO_START;
-
-    case SYM_LOGO_WIDTH:
-        return BF_SYM_LOGO_WIDTH;
-
-    case SYM_LOGO_HEIGHT:
-        return BF_SYM_LOGO_HEIGHT;
-*/
-    case SYM_AH_LEFT:
-        return BF_SYM_AH_LEFT;
-
-    case SYM_AH_RIGHT:
-        return BF_SYM_AH_RIGHT;
-
-/*
-    case SYM_AH_DECORATION_COUNT:
-        return BF_SYM_AH_DECORATION_COUNT;
-*/
-    case SYM_AH_CH_LEFT:
-    case SYM_AH_CH_TYPE3:
-    case SYM_AH_CH_TYPE4:
-    case SYM_AH_CH_TYPE5:
-    case SYM_AH_CH_TYPE6:
-    case SYM_AH_CH_TYPE7:
-    case SYM_AH_CH_TYPE8:
-    case SYM_AH_CH_AIRCRAFT1:
-        return BF_SYM_AH_CENTER_LINE;
-
-    case SYM_AH_CH_RIGHT:
-    case (SYM_AH_CH_TYPE3+2):
-    case (SYM_AH_CH_TYPE4+2):
-    case (SYM_AH_CH_TYPE5+2):
-    case (SYM_AH_CH_TYPE6+2):
-    case (SYM_AH_CH_TYPE7+2):
-    case (SYM_AH_CH_TYPE8+2):
-    case SYM_AH_CH_AIRCRAFT3:
-        return BF_SYM_AH_CENTER_LINE_RIGHT;
-    
-    case SYM_AH_CH_AIRCRAFT0:
-    case SYM_AH_CH_AIRCRAFT4:
-        return ' ';
-
-    case SYM_ARROW_UP:
-        return BF_SYM_ARROW_NORTH;
-
-    case SYM_ARROW_2:
-        return BF_SYM_ARROW_8;
-
-    case SYM_ARROW_3:
-        return BF_SYM_ARROW_7;
-
-    case SYM_ARROW_4:
-        return BF_SYM_ARROW_6;
-
-    case SYM_ARROW_RIGHT:
-        return BF_SYM_ARROW_EAST;
-
-    case SYM_ARROW_6:
-        return BF_SYM_ARROW_4;
-
-    case SYM_ARROW_7:
-        return BF_SYM_ARROW_3;
-
-    case SYM_ARROW_8:
-        return BF_SYM_ARROW_2;
-
-    case SYM_ARROW_DOWN:
-        return BF_SYM_ARROW_SOUTH;
-
-    case SYM_ARROW_10:
-        return BF_SYM_ARROW_16;
-
-    case SYM_ARROW_11:
-        return BF_SYM_ARROW_15;
-
-    case SYM_ARROW_12:
-        return BF_SYM_ARROW_14;
-
-    case SYM_ARROW_LEFT:
-        return BF_SYM_ARROW_WEST;
-
-    case SYM_ARROW_14:
-        return BF_SYM_ARROW_12;
-
-    case SYM_ARROW_15:
-        return BF_SYM_ARROW_11;
-
-    case SYM_ARROW_16:
-        return BF_SYM_ARROW_10;
-
-    case SYM_AH_H_START:
-        return BF_SYM_AH_BAR9_0;
-
-    case (SYM_AH_H_START+1):
-        return BF_SYM_AH_BAR9_1;
-
-    case (SYM_AH_H_START+2):
-        return BF_SYM_AH_BAR9_2;
-
-    case (SYM_AH_H_START+3):
-        return BF_SYM_AH_BAR9_3;
-
-    case (SYM_AH_H_START+4):
-        return BF_SYM_AH_BAR9_4;
-
-    case (SYM_AH_H_START+5):
-        return BF_SYM_AH_BAR9_5;
-
-    case (SYM_AH_H_START+6):
-        return BF_SYM_AH_BAR9_6;
-
-    case (SYM_AH_H_START+7):
-        return BF_SYM_AH_BAR9_7;
-
-    case (SYM_AH_H_START+8):
-        return BF_SYM_AH_BAR9_8;
-
-    // BF does not have vertical artificial horizon. replace with middle horizontal one
-    case SYM_AH_V_START:
-    case (SYM_AH_V_START+1):
-    case (SYM_AH_V_START+2):
-    case (SYM_AH_V_START+3):
-    case (SYM_AH_V_START+4):
-    case (SYM_AH_V_START+5):
-        return BF_SYM_AH_BAR9_4;
-
-    // BF for ESP_RADAR Symbols
-    case SYM_HUD_CARDINAL:
-        return BF_SYM_ARROW_SOUTH;
-    case SYM_HUD_CARDINAL + 1:
-        return BF_SYM_ARROW_16;
-    case SYM_HUD_CARDINAL + 2:
-        return BF_SYM_ARROW_15;
-    case SYM_HUD_CARDINAL + 3:
-        return BF_SYM_ARROW_WEST;
-    case SYM_HUD_CARDINAL + 4:
-        return BF_SYM_ARROW_12;
-    case SYM_HUD_CARDINAL + 5:
-        return BF_SYM_ARROW_11;
-    case SYM_HUD_CARDINAL + 6:
-        return BF_SYM_ARROW_NORTH;
-    case SYM_HUD_CARDINAL + 7:
-        return BF_SYM_ARROW_7;
-    case SYM_HUD_CARDINAL + 8:
-        return BF_SYM_ARROW_6;
-    case SYM_HUD_CARDINAL + 9:
-        return BF_SYM_ARROW_EAST;
-    case SYM_HUD_CARDINAL + 10:
-        return BF_SYM_ARROW_3;
-    case SYM_HUD_CARDINAL + 11:
-        return BF_SYM_ARROW_2;
-
-    case SYM_HUD_ARROWS_R3:
-        return BF_SYM_AH_RIGHT;
-    case SYM_HUD_ARROWS_L3:
-        return BF_SYM_AH_LEFT;
-
-    case SYM_HUD_SIGNAL_0:
-        return BF_SYM_AH_BAR9_1;
-    case SYM_HUD_SIGNAL_1:
-        return BF_SYM_AH_BAR9_3;
-    case SYM_HUD_SIGNAL_2:
-        return BF_SYM_AH_BAR9_4;
-    case SYM_HUD_SIGNAL_3:
-        return BF_SYM_AH_BAR9_5;
-    case SYM_HUD_SIGNAL_4:
-        return BF_SYM_AH_BAR9_7;
-/*
-    case SYM_VARIO_UP_2A:
-        return BF_SYM_VARIO_UP_2A;
-
-    case SYM_VARIO_UP_1A:
-        return BF_SYM_VARIO_UP_1A;
-
-    case SYM_VARIO_DOWN_1A:
-        return BF_SYM_VARIO_DOWN_1A;
-
-    case SYM_VARIO_DOWN_2A:
-        return BF_SYM_VARIO_DOWN_2A;
-*/
-    case SYM_ALT:
-        return BF_SYM_ALTITUDE;
-/*
-    case SYM_HUD_SIGNAL_0:
-        return BF_SYM_HUD_SIGNAL_0;
-
-    case SYM_HUD_SIGNAL_1:
-        return BF_SYM_HUD_SIGNAL_1;
-
-    case SYM_HUD_SIGNAL_2:
-        return BF_SYM_HUD_SIGNAL_2;
-
-    case SYM_HUD_SIGNAL_3:
-        return BF_SYM_HUD_SIGNAL_3;
-
-    case SYM_HUD_SIGNAL_4:
-        return BF_SYM_HUD_SIGNAL_4;
-
-    case SYM_HOME_DIST:
-        return BF_SYM_HOME_DIST;
-*/
-
-    case SYM_AH_CH_CENTER:
-    case (SYM_AH_CH_TYPE3+1):
-    case (SYM_AH_CH_TYPE4+1):
-    case (SYM_AH_CH_TYPE5+1):
-    case (SYM_AH_CH_TYPE6+1):
-    case (SYM_AH_CH_TYPE7+1):
-    case (SYM_AH_CH_TYPE8+1):
-    case SYM_AH_CH_AIRCRAFT2:
-        return BF_SYM_AH_CENTER;
-/*
-    case SYM_FLIGHT_DIST_REMAINING:
-        return BF_SYM_FLIGHT_DIST_REMAINING;
-
-    case SYM_AH_CH_TYPE3:
-        return BF_SYM_AH_CH_TYPE3;
-
-    case SYM_AH_CH_TYPE4:
-        return BF_SYM_AH_CH_TYPE4;
-
-    case SYM_AH_CH_TYPE5:
-        return BF_SYM_AH_CH_TYPE5;
-
-    case SYM_AH_CH_TYPE6:
-        return BF_SYM_AH_CH_TYPE6;
-
-    case SYM_AH_CH_TYPE7:
-        return BF_SYM_AH_CH_TYPE7;
-
-    case SYM_AH_CH_TYPE8:
-        return BF_SYM_AH_CH_TYPE8;
-
-    case SYM_AH_CH_AIRCRAFT0:
-        return BF_SYM_AH_CH_AIRCRAFT0;
-
-    case SYM_AH_CH_AIRCRAFT1:
-        return BF_SYM_AH_CH_AIRCRAFT1;
-
-    case SYM_AH_CH_AIRCRAFT2:
-        return BF_SYM_AH_CH_AIRCRAFT2;
-
-    case SYM_AH_CH_AIRCRAFT3:
-        return BF_SYM_AH_CH_AIRCRAFT3;
-
-    case SYM_AH_CH_AIRCRAFT4:
-        return BF_SYM_AH_CH_AIRCRAFT4;
-
-    case SYM_HUD_ARROWS_L1:
-        return BF_SYM_HUD_ARROWS_L1;
-
-    case SYM_HUD_ARROWS_L2:
-        return BF_SYM_HUD_ARROWS_L2;
-
-    case SYM_HUD_ARROWS_L3:
-        return BF_SYM_HUD_ARROWS_L3;
-
-    case SYM_HUD_ARROWS_R1:
-        return BF_SYM_HUD_ARROWS_R1;
-
-    case SYM_HUD_ARROWS_R2:
-        return BF_SYM_HUD_ARROWS_R2;
-
-    case SYM_HUD_ARROWS_R3:
-        return BF_SYM_HUD_ARROWS_R3;
-
-    case SYM_HUD_ARROWS_U1:
-        return BF_SYM_HUD_ARROWS_U1;
-
-    case SYM_HUD_ARROWS_U2:
-        return BF_SYM_HUD_ARROWS_U2;
-
-    case SYM_HUD_ARROWS_U3:
-        return BF_SYM_HUD_ARROWS_U3;
-
-    case SYM_HUD_ARROWS_D1:
-        return BF_SYM_HUD_ARROWS_D1;
-
-    case SYM_HUD_ARROWS_D2:
-        return BF_SYM_HUD_ARROWS_D2;
-
-    case SYM_HUD_ARROWS_D3:
-        return BF_SYM_HUD_ARROWS_D3;
-*/
-    default:
-        break;
-    }
-
-    return '?'; // Missing/not mapped character
-}
-
-#endif
-
-#endif
diff --git a/src/main/io/displayport_msp_dji_compat.c b/src/main/io/displayport_msp_dji_compat.c
new file mode 100644
index 00000000000..de4fa8656fa
--- /dev/null
+++ b/src/main/io/displayport_msp_dji_compat.c
@@ -0,0 +1,713 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * 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.
+ *
+ * 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 "platform.h"
+
+#if defined(USE_OSD) && defined(USE_MSP_DISPLAYPORT)
+
+#ifndef DISABLE_MSP_DJI_COMPAT
+
+#include "io/displayport_msp_dji_compat.h"
+#include "io/dji_osd_symbols.h"
+#include "drivers/osd_symbols.h"
+
+uint8_t getDJICharacter(uint8_t ch, uint8_t page)
+{
+    uint16_t ech = ch | ((page & 0x3)<< 8) ;
+
+    if (ech >= 0x20 && ech <= 0x5F) { // ASCII range
+        return ch;
+    }
+
+    if (ech >= SYM_AH_DECORATION_MIN && ech <= SYM_AH_DECORATION_MAX) {
+        return DJI_SYM_AH_DECORATION;
+    }
+
+    switch (ech) {
+        case SYM_RSSI:
+            return DJI_SYM_RSSI;
+
+/*
+        case SYM_LQ:
+            return DJI_SYM_LINK_QUALITY;
+*/
+
+        case SYM_LAT:
+            return DJI_SYM_LAT;
+
+        case SYM_LON:
+            return DJI_SYM_LON;
+
+        case SYM_SAT_L:
+            return DJI_SYM_SAT_L;
+
+        case SYM_SAT_R:
+            return DJI_SYM_SAT_R;
+
+        case SYM_HOME_NEAR:
+            return DJI_SYM_HOMEFLAG;
+
+        case SYM_DEGREES:
+            return DJI_SYM_GPS_DEGREE;
+
+/*
+        case SYM_HEADING:
+            return DJI_SYM_HEADING;
+
+        case SYM_SCALE:
+            return DJI_SYM_SCALE;
+
+        case SYM_HDP_L:
+            return DJI_SYM_HDP_L;
+
+        case SYM_HDP_R:
+            return DJI_SYM_HDP_R;
+*/
+        case SYM_HOME:
+            return DJI_SYM_HOMEFLAG;
+
+        case SYM_2RSS:
+            return DJI_SYM_RSSI;
+
+/*
+        case SYM_DB:
+            return DJI_SYM_DB
+
+        case SYM_DBM:
+            return DJI_SYM_DBM;
+
+        case SYM_SNR:
+            return DJI_SYM_SNR;
+*/
+
+        case SYM_AH_DECORATION_UP:
+            return DJI_SYM_ARROW_SMALL_UP;
+
+        case SYM_AH_DECORATION_DOWN:
+            return DJI_SYM_ARROW_SMALL_DOWN;
+
+        case SYM_DECORATION:
+            return DJI_SYM_ARROW_SMALL_UP;
+        
+        case SYM_DECORATION + 1: // NE pointing arrow
+            return DJI_SYM_ARROW_7;
+
+        case SYM_DECORATION + 2: // E pointing arrow
+            return DJI_SYM_ARROW_EAST;
+
+        case SYM_DECORATION + 3: // SE pointing arrow
+            return DJI_SYM_ARROW_3;
+
+        case SYM_DECORATION + 4: // S pointing arrow
+            return DJI_SYM_ARROW_SOUTH;
+
+        case SYM_DECORATION + 5: // SW pointing arrow
+            return DJI_SYM_ARROW_15;
+
+        case SYM_DECORATION + 6: // W pointing arrow
+            return DJI_SYM_ARROW_WEST;
+
+        case SYM_DECORATION + 7: // NW pointing arrow
+            return DJI_SYM_ARROW_11;
+
+        case SYM_VOLT:
+            return DJI_SYM_VOLT;
+
+        case SYM_MAH:
+            return DJI_SYM_MAH;
+
+        case SYM_AH_KM:
+            return 'K';
+
+        case SYM_AH_MI:
+            return 'M';
+/*
+        case SYM_VTX_POWER:
+            return DJI_SYM_VTX_POWER;
+
+        case SYM_AH_NM:
+            return DJI_SYM_AH_NM;
+
+        case SYM_MAH_NM_0:
+            return DJI_SYM_MAH_NM_0;
+
+        case SYM_MAH_NM_1:
+            return DJI_SYM_MAH_NM_1;
+
+        case SYM_MAH_KM_0:
+            return DJI_SYM_MAH_KM_0;
+
+        case SYM_MAH_KM_1:
+            return DJI_SYM_MAH_KM_1;
+
+        case SYM_MILLIOHM:
+            return DJI_SYM_MILLIOHM;
+*/
+        case SYM_BATT_FULL:
+            return DJI_SYM_BATT_FULL;
+
+        case SYM_BATT_5:
+            return DJI_SYM_BATT_5;
+
+        case SYM_BATT_4:
+            return DJI_SYM_BATT_4;
+
+        case SYM_BATT_3:
+            return DJI_SYM_BATT_3;
+
+        case SYM_BATT_2:
+            return DJI_SYM_BATT_2;
+
+        case SYM_BATT_1:
+            return DJI_SYM_BATT_1;
+
+        case SYM_BATT_EMPTY:
+            return DJI_SYM_BATT_EMPTY;
+
+        case SYM_AMP:
+            return DJI_SYM_AMP;
+/*
+        case SYM_WH:
+            return DJI_SYM_WH;
+
+        case SYM_WH_KM:
+            return DJI_SYM_WH_KM;
+
+        case SYM_WH_MI:
+            return DJI_SYM_WH_MI;
+
+        case SYM_WH_NM:
+            return DJI_SYM_WH_NM;
+*/
+        case SYM_WATT:
+            return DJI_SYM_WATT;
+/*
+        case SYM_MW:
+            return DJI_SYM_MW;
+
+        case SYM_KILOWATT:
+            return DJI_SYM_KILOWATT;
+*/
+        case SYM_FT:
+            return DJI_SYM_FT;
+
+        case SYM_ALT_FT:
+            return DJI_SYM_FT;
+
+        case SYM_ALT_M:
+            return DJI_SYM_M;
+
+        case SYM_TOTAL:
+            return DJI_SYM_FLY_H;
+/*
+
+        case SYM_ALT_KM:
+            return DJI_SYM_ALT_KM;
+
+        case SYM_ALT_KFT:
+            return DJI_SYM_ALT_KFT;
+
+        case SYM_DIST_M:
+            return DJI_SYM_DIST_M;
+
+        case SYM_DIST_KM:
+            return DJI_SYM_DIST_KM;
+
+        case SYM_DIST_FT:
+            return DJI_SYM_DIST_FT;
+
+        case SYM_DIST_MI:
+            return DJI_SYM_DIST_MI;
+
+        case SYM_DIST_NM:
+            return DJI_SYM_DIST_NM;
+*/
+        case SYM_M:
+            return DJI_SYM_M;
+
+        case SYM_KM:
+            return 'K';
+
+        case SYM_MI:
+            return 'M';
+/*
+        case SYM_NM:
+            return DJI_SYM_NM;
+*/
+        case SYM_WIND_HORIZONTAL:
+            return 'W';     // W for wind
+
+/*
+        case SYM_WIND_VERTICAL:
+            return DJI_SYM_WIND_VERTICAL;
+
+        case SYM_3D_KT:
+            return DJI_SYM_3D_KT;
+*/
+        case SYM_AIR:
+            return 'A';     // A for airspeed
+
+        case SYM_3D_KMH:
+            return DJI_SYM_KPH;
+
+        case SYM_3D_MPH:
+            return DJI_SYM_MPH;
+
+        case SYM_RPM:
+            return DJI_SYM_RPM;
+
+        case SYM_FTS:
+            return DJI_SYM_FTPS;
+/*
+        case SYM_100FTM:
+            return DJI_SYM_100FTM;
+*/
+        case SYM_MS:
+            return DJI_SYM_MPS;
+
+        case SYM_KMH:
+            return DJI_SYM_KPH;
+
+        case SYM_MPH:
+            return DJI_SYM_MPH;
+/*
+        case SYM_KT:
+            return DJI_SYM_KT
+
+        case SYM_MAH_MI_0:
+            return DJI_SYM_MAH_MI_0;
+
+        case SYM_MAH_MI_1:
+            return DJI_SYM_MAH_MI_1;
+*/
+        case SYM_THR:
+            return DJI_SYM_THR;
+
+        case SYM_TEMP_F:
+            return DJI_SYM_F;
+
+        case SYM_TEMP_C:
+            return DJI_SYM_C;
+
+        case SYM_BLANK:
+            return DJI_SYM_BLANK;
+
+        case SYM_ON_H:
+            return DJI_SYM_ON_H;
+
+        case SYM_FLY_H:
+            return DJI_SYM_FLY_H;
+
+        case SYM_ON_M:
+            return DJI_SYM_ON_M;
+
+        case SYM_FLY_M:
+            return DJI_SYM_FLY_M;
+/*
+        case SYM_GLIDESLOPE:
+            return DJI_SYM_GLIDESLOPE;
+
+        case SYM_WAYPOINT:
+            return DJI_SYM_WAYPOINT;
+
+        case SYM_CLOCK:
+            return DJI_SYM_CLOCK;
+
+        case SYM_ZERO_HALF_TRAILING_DOT:
+            return DJI_SYM_ZERO_HALF_TRAILING_DOT;
+
+        case SYM_ZERO_HALF_LEADING_DOT:
+            return DJI_SYM_ZERO_HALF_LEADING_DOT;
+*/
+
+        case SYM_AUTO_THR0:
+            return 'A';
+
+        case SYM_AUTO_THR1:
+            return DJI_SYM_THR;
+
+        case SYM_ROLL_LEFT:
+            return DJI_SYM_ROLL;
+
+        case SYM_ROLL_LEVEL:
+            return DJI_SYM_ROLL;
+
+        case SYM_ROLL_RIGHT:
+            return DJI_SYM_ROLL;
+
+        case SYM_PITCH_UP:
+            return DJI_SYM_PITCH;
+
+        case SYM_PITCH_DOWN:
+            return DJI_SYM_PITCH;
+
+        case SYM_GFORCE:
+            return 'G';
+
+/*
+        case SYM_GFORCE_X:
+            return DJI_SYM_GFORCE_X;
+
+        case SYM_GFORCE_Y:
+            return DJI_SYM_GFORCE_Y;
+
+        case SYM_GFORCE_Z:
+            return DJI_SYM_GFORCE_Z;
+*/
+        case SYM_BARO_TEMP:
+            return DJI_SYM_TEMPERATURE;
+
+        case SYM_IMU_TEMP:
+            return DJI_SYM_TEMPERATURE;
+
+        case SYM_TEMP:
+            return DJI_SYM_TEMPERATURE;
+
+        case SYM_ESC_TEMP:
+            return DJI_SYM_TEMPERATURE;
+/*
+        case SYM_TEMP_SENSOR_FIRST:
+            return DJI_SYM_TEMP_SENSOR_FIRST;
+
+        case SYM_TEMP_SENSOR_LAST:
+            return DJI_SYM_TEMP_SENSOR_LAST;
+
+        case TEMP_SENSOR_SYM_COUNT:
+            return DJI_TEMP_SENSOR_SYM_COUNT;
+*/
+        case SYM_HEADING_N:
+            return DJI_SYM_HEADING_N;
+
+        case SYM_HEADING_S:
+            return DJI_SYM_HEADING_S;
+
+        case SYM_HEADING_E:
+            return DJI_SYM_HEADING_E;
+
+        case SYM_HEADING_W:
+            return DJI_SYM_HEADING_W;
+
+        case SYM_HEADING_DIVIDED_LINE:
+            return DJI_SYM_HEADING_DIVIDED_LINE;
+
+        case SYM_HEADING_LINE:
+            return DJI_SYM_HEADING_LINE;
+
+        case SYM_MAX:
+            return DJI_SYM_MAX;
+/*
+        case SYM_PROFILE:
+            return DJI_SYM_PROFILE;
+*/
+        case SYM_SWITCH_INDICATOR_LOW:
+            return DJI_SYM_STICK_OVERLAY_SPRITE_LOW;
+
+        case SYM_SWITCH_INDICATOR_MID:
+            return DJI_SYM_STICK_OVERLAY_SPRITE_MID;
+
+        case SYM_SWITCH_INDICATOR_HIGH:
+            return DJI_SYM_STICK_OVERLAY_SPRITE_HIGH;
+/*
+        case SYM_AH:
+            return DJI_SYM_AH;
+
+        case SYM_GLIDE_DIST:
+            return DJI_SYM_GLIDE_DIST;
+
+        case SYM_GLIDE_MINS:
+            return DJI_SYM_GLIDE_MINS;
+
+        case SYM_AH_V_FT_0:
+            return DJI_SYM_AH_V_FT_0;
+
+        case SYM_AH_V_FT_1:
+            return DJI_SYM_AH_V_FT_1;
+
+        case SYM_AH_V_M_0:
+            return DJI_SYM_AH_V_M_0;
+
+        case SYM_AH_V_M_1:
+            return DJI_SYM_AH_V_M_1;
+
+        case SYM_FLIGHT_MINS_REMAINING:
+            return DJI_SYM_FLIGHT_MINS_REMAINING;
+
+        case SYM_FLIGHT_HOURS_REMAINING:
+            return DJI_SYM_FLIGHT_HOURS_REMAINING;
+
+        case SYM_GROUND_COURSE:
+            return DJI_SYM_GROUND_COURSE;
+
+        case SYM_CROSS_TRACK_ERROR:
+            return DJI_SYM_CROSS_TRACK_ERROR;
+
+        case SYM_LOGO_START:
+            return DJI_SYM_LOGO_START;
+
+        case SYM_LOGO_WIDTH:
+            return DJI_SYM_LOGO_WIDTH;
+
+        case SYM_LOGO_HEIGHT:
+            return DJI_SYM_LOGO_HEIGHT;
+*/
+        case SYM_AH_LEFT:
+            return DJI_SYM_AH_LEFT;
+
+        case SYM_AH_RIGHT:
+            return DJI_SYM_AH_RIGHT;
+
+/*
+        case SYM_AH_DECORATION_COUNT:
+            return DJI_SYM_AH_DECORATION_COUNT;
+*/
+        case SYM_AH_CH_LEFT:
+        case SYM_AH_CH_TYPE3:
+        case SYM_AH_CH_TYPE4:
+        case SYM_AH_CH_TYPE5:
+        case SYM_AH_CH_TYPE6:
+        case SYM_AH_CH_TYPE7:
+        case SYM_AH_CH_TYPE8:
+        case SYM_AH_CH_AIRCRAFT1:
+            return DJI_SYM_CROSSHAIR_LEFT;
+
+        case SYM_AH_CH_CENTER:
+        case (SYM_AH_CH_TYPE3+1):
+        case (SYM_AH_CH_TYPE4+1):
+        case (SYM_AH_CH_TYPE5+1):
+        case (SYM_AH_CH_TYPE6+1):
+        case (SYM_AH_CH_TYPE7+1):
+        case (SYM_AH_CH_TYPE8+1):
+        case SYM_AH_CH_AIRCRAFT2:
+            return DJI_SYM_CROSSHAIR_CENTRE;
+
+        case SYM_AH_CH_RIGHT:
+        case (SYM_AH_CH_TYPE3+2):
+        case (SYM_AH_CH_TYPE4+2):
+        case (SYM_AH_CH_TYPE5+2):
+        case (SYM_AH_CH_TYPE6+2):
+        case (SYM_AH_CH_TYPE7+2):
+        case (SYM_AH_CH_TYPE8+2):
+        case SYM_AH_CH_AIRCRAFT3:
+            return DJI_SYM_CROSSHAIR_RIGHT;
+        
+        case SYM_AH_CH_AIRCRAFT0:
+        case SYM_AH_CH_AIRCRAFT4:
+            return DJI_SYM_BLANK;
+
+        case SYM_ARROW_UP:
+            return DJI_SYM_ARROW_NORTH;
+
+        case SYM_ARROW_2:
+            return DJI_SYM_ARROW_8;
+
+        case SYM_ARROW_3:
+            return DJI_SYM_ARROW_7;
+
+        case SYM_ARROW_4:
+            return DJI_SYM_ARROW_6;
+
+        case SYM_ARROW_RIGHT:
+            return DJI_SYM_ARROW_EAST;
+
+        case SYM_ARROW_6:
+            return DJI_SYM_ARROW_4;
+
+        case SYM_ARROW_7:
+            return DJI_SYM_ARROW_3;
+
+        case SYM_ARROW_8:
+            return DJI_SYM_ARROW_2;
+
+        case SYM_ARROW_DOWN:
+            return DJI_SYM_ARROW_SOUTH;
+
+        case SYM_ARROW_10:
+            return DJI_SYM_ARROW_16;
+
+        case SYM_ARROW_11:
+            return DJI_SYM_ARROW_15;
+
+        case SYM_ARROW_12:
+            return DJI_SYM_ARROW_14;
+
+        case SYM_ARROW_LEFT:
+            return DJI_SYM_ARROW_WEST;
+
+        case SYM_ARROW_14:
+            return DJI_SYM_ARROW_12;
+
+        case SYM_ARROW_15:
+            return DJI_SYM_ARROW_11;
+
+        case SYM_ARROW_16:
+            return DJI_SYM_ARROW_10;
+
+        case SYM_AH_H_START:
+            return DJI_SYM_AH_BAR9_0;
+
+        case (SYM_AH_H_START+1):
+            return DJI_SYM_AH_BAR9_1;
+
+        case (SYM_AH_H_START+2):
+            return DJI_SYM_AH_BAR9_2;
+
+        case (SYM_AH_H_START+3):
+            return DJI_SYM_AH_BAR9_3;
+
+        case (SYM_AH_H_START+4):
+            return DJI_SYM_AH_BAR9_4;
+
+        case (SYM_AH_H_START+5):
+            return DJI_SYM_AH_BAR9_5;
+
+        case (SYM_AH_H_START+6):
+            return DJI_SYM_AH_BAR9_6;
+
+        case (SYM_AH_H_START+7):
+            return DJI_SYM_AH_BAR9_7;
+
+        case (SYM_AH_H_START+8):
+            return DJI_SYM_AH_BAR9_8;
+
+        // DJI does not have vertical artificial horizon. replace with middle horizontal one
+        case SYM_AH_V_START:
+        case (SYM_AH_V_START+1):
+        case (SYM_AH_V_START+2):
+        case (SYM_AH_V_START+3):
+        case (SYM_AH_V_START+4):
+        case (SYM_AH_V_START+5):
+            return DJI_SYM_AH_BAR9_4;
+
+        // DJI for ESP_RADAR Symbols
+        case SYM_HUD_CARDINAL:
+            return DJI_SYM_ARROW_SOUTH;
+        case SYM_HUD_CARDINAL + 1:
+            return DJI_SYM_ARROW_16;
+        case SYM_HUD_CARDINAL + 2:
+            return DJI_SYM_ARROW_15;
+        case SYM_HUD_CARDINAL + 3:
+            return DJI_SYM_ARROW_WEST;
+        case SYM_HUD_CARDINAL + 4:
+            return DJI_SYM_ARROW_12;
+        case SYM_HUD_CARDINAL + 5:
+            return DJI_SYM_ARROW_11;
+        case SYM_HUD_CARDINAL + 6:
+            return DJI_SYM_ARROW_NORTH;
+        case SYM_HUD_CARDINAL + 7:
+            return DJI_SYM_ARROW_7;
+        case SYM_HUD_CARDINAL + 8:
+            return DJI_SYM_ARROW_6;
+        case SYM_HUD_CARDINAL + 9:
+            return DJI_SYM_ARROW_EAST;
+        case SYM_HUD_CARDINAL + 10:
+            return DJI_SYM_ARROW_3;
+        case SYM_HUD_CARDINAL + 11:
+            return DJI_SYM_ARROW_2;
+
+        case SYM_HUD_SIGNAL_0:
+            return DJI_SYM_AH_BAR9_1;
+        case SYM_HUD_SIGNAL_1:
+            return DJI_SYM_AH_BAR9_3;
+        case SYM_HUD_SIGNAL_2:
+            return DJI_SYM_AH_BAR9_4;
+        case SYM_HUD_SIGNAL_3:
+            return DJI_SYM_AH_BAR9_5;
+        case SYM_HUD_SIGNAL_4:
+            return DJI_SYM_AH_BAR9_7;
+
+        case SYM_VARIO_UP_2A:
+            return DJI_SYM_ARROW_SMALL_UP;
+
+        case SYM_VARIO_UP_1A:
+            return DJI_SYM_ARROW_SMALL_UP;
+
+        case SYM_VARIO_DOWN_1A:
+            return DJI_SYM_ARROW_SMALL_DOWN;
+
+        case SYM_VARIO_DOWN_2A:
+            return DJI_SYM_ARROW_SMALL_DOWN;
+
+        case SYM_ALT:
+            return DJI_SYM_ALTITUDE;
+/*
+        case SYM_HUD_SIGNAL_0:
+            return DJI_SYM_HUD_SIGNAL_0;
+
+        case SYM_HUD_SIGNAL_1:
+            return DJI_SYM_HUD_SIGNAL_1;
+
+        case SYM_HUD_SIGNAL_2:
+            return DJI_SYM_HUD_SIGNAL_2;
+
+        case SYM_HUD_SIGNAL_3:
+            return DJI_SYM_HUD_SIGNAL_3;
+
+        case SYM_HUD_SIGNAL_4:
+            return DJI_SYM_HUD_SIGNAL_4;
+
+        case SYM_HOME_DIST:
+            return DJI_SYM_HOME_DIST;
+
+        case SYM_FLIGHT_DIST_REMAINING:
+            return DJI_SYM_FLIGHT_DIST_REMAINING;
+*/
+        case SYM_HUD_ARROWS_L1:
+            return DJI_SYM_ARROW_SMALL_LEFT;
+
+        case SYM_HUD_ARROWS_L2:
+            return DJI_SYM_ARROW_SMALL_LEFT;
+
+        case SYM_HUD_ARROWS_L3:
+            return DJI_SYM_ARROW_SMALL_LEFT;
+
+        case SYM_HUD_ARROWS_R1:
+            return DJI_SYM_ARROW_SMALL_RIGHT;
+
+        case SYM_HUD_ARROWS_R2:
+            return DJI_SYM_ARROW_SMALL_RIGHT;
+
+        case SYM_HUD_ARROWS_R3:
+            return DJI_SYM_ARROW_SMALL_RIGHT;
+
+        case SYM_HUD_ARROWS_U1:
+            return DJI_SYM_ARROW_SMALL_UP;
+
+        case SYM_HUD_ARROWS_U2:
+            return DJI_SYM_ARROW_SMALL_UP;
+
+        case SYM_HUD_ARROWS_U3:
+            return DJI_SYM_ARROW_SMALL_UP;
+
+        case SYM_HUD_ARROWS_D1:
+            return DJI_SYM_ARROW_SMALL_DOWN;
+
+        case SYM_HUD_ARROWS_D2:
+            return DJI_SYM_ARROW_SMALL_DOWN;
+
+        case SYM_HUD_ARROWS_D3:
+            return DJI_SYM_ARROW_SMALL_DOWN;
+
+        default:
+            break;
+    }
+
+    return '?'; // Missing/not mapped character
+}
+
+#endif
+
+#endif
diff --git a/src/main/io/displayport_msp_bf_compat.h b/src/main/io/displayport_msp_dji_compat.h
similarity index 69%
rename from src/main/io/displayport_msp_bf_compat.h
rename to src/main/io/displayport_msp_dji_compat.h
index 95dab7b518d..6380770aa00 100644
--- a/src/main/io/displayport_msp_bf_compat.h
+++ b/src/main/io/displayport_msp_dji_compat.h
@@ -22,15 +22,15 @@
 
 #include "platform.h"
 
-#if defined(USE_OSD) && defined(USE_MSP_DISPLAYPORT) && !defined(DISABLE_MSP_BF_COMPAT)
+#if defined(USE_OSD) && defined(USE_MSP_DISPLAYPORT) && !defined(DISABLE_MSP_DJI_COMPAT)
 #include "osd.h"
-uint8_t getBfCharacter(uint8_t ch, uint8_t page);
-#define isBfCompatibleVideoSystem(osdConfigPtr) (osdConfigPtr->video_system == VIDEO_SYSTEM_BFCOMPAT || osdConfigPtr->video_system == VIDEO_SYSTEM_BFCOMPAT_HD)
+uint8_t getDJICharacter(uint8_t ch, uint8_t page);
+#define isDJICompatibleVideoSystem(osdConfigPtr) (osdConfigPtr->video_system == VIDEO_SYSTEM_DJICOMPAT || osdConfigPtr->video_system == VIDEO_SYSTEM_DJICOMPAT_HD)
 #else
-#define getBfCharacter(x, page) (x)
+#define getDJICharacter(x, page) (x)
 #ifdef OSD_UNIT_TEST
-#define isBfCompatibleVideoSystem(osdConfigPtr) (true)
+#define isDJICompatibleVideoSystem(osdConfigPtr) (true)
 #else
-#define isBfCompatibleVideoSystem(osdConfigPtr) (false)
+#define isDJICompatibleVideoSystem(osdConfigPtr) (false)
 #endif
 #endif
\ No newline at end of file
diff --git a/src/main/io/displayport_msp_osd.c b/src/main/io/displayport_msp_osd.c
index d587104e9b9..2e425e6aecb 100644
--- a/src/main/io/displayport_msp_osd.c
+++ b/src/main/io/displayport_msp_osd.c
@@ -51,7 +51,7 @@
 #include "msp/msp_serial.h"
 
 #include "displayport_msp_osd.h"
-#include "displayport_msp_bf_compat.h"
+#include "displayport_msp_dji_compat.h"
 
 #define FONT_VERSION 3
 
@@ -307,7 +307,7 @@ static int drawScreen(displayPort_t *displayPort) // 250Hz
         uint8_t len = 4;
         do {
             bitArrayClr(dirty, pos);
-            subcmd[len] = isBfCompatibleVideoSystem(osdConfig()) ? getBfCharacter(screen[pos++], page): screen[pos++];
+            subcmd[len] = isDJICompatibleVideoSystem(osdConfig()) ? getDJICharacter(screen[pos++], page): screen[pos++];
             len++;
 
             if (bitArrayGet(dirty, pos)) {
@@ -315,7 +315,7 @@ static int drawScreen(displayPort_t *displayPort) // 250Hz
             }
         } while (next == pos && next < endOfLine && getAttrPage(attrs[next]) == page && getAttrBlink(attrs[next]) == blink);
 
-        if (!isBfCompatibleVideoSystem(osdConfig())) {
+        if (!isDJICompatibleVideoSystem(osdConfig())) {
             attributes |= (page << DISPLAYPORT_MSP_ATTR_FONTPAGE);
         }
 
@@ -465,7 +465,7 @@ displayPort_t* mspOsdDisplayPortInit(const videoSystem_e videoSystem)
     if (mspOsdSerialInit()) {
         switch(videoSystem) {
         case VIDEO_SYSTEM_AUTO:
-        case VIDEO_SYSTEM_BFCOMPAT:
+        case VIDEO_SYSTEM_DJICOMPAT:
         case VIDEO_SYSTEM_PAL:
             currentOsdMode = SD_3016;
             screenRows = PAL_ROWS;
@@ -486,7 +486,7 @@ displayPort_t* mspOsdDisplayPortInit(const videoSystem_e videoSystem)
             screenRows = DJI_ROWS;
             screenCols = DJI_COLS;
             break;
-        case VIDEO_SYSTEM_BFCOMPAT_HD:
+        case VIDEO_SYSTEM_DJICOMPAT_HD:
         case VIDEO_SYSTEM_AVATAR:
             currentOsdMode = HD_5320;
             screenRows = AVATAR_ROWS;
@@ -500,10 +500,10 @@ displayPort_t* mspOsdDisplayPortInit(const videoSystem_e videoSystem)
         init();
         displayInit(&mspOsdDisplayPort, &mspOsdVTable);
 
-        if (osdVideoSystem == VIDEO_SYSTEM_BFCOMPAT) {
-            mspOsdDisplayPort.displayPortType = "MSP DisplayPort: BetaFlight Compatability mode";
-        } else if (osdVideoSystem == VIDEO_SYSTEM_BFCOMPAT_HD) {
-            mspOsdDisplayPort.displayPortType = "MSP DisplayPort: BetaFlight Compatability mode (HD)";
+        if (osdVideoSystem == VIDEO_SYSTEM_DJICOMPAT) {
+            mspOsdDisplayPort.displayPortType = "MSP DisplayPort: DJI Compatability mode";
+        } else if (osdVideoSystem == VIDEO_SYSTEM_DJICOMPAT_HD) {
+            mspOsdDisplayPort.displayPortType = "MSP DisplayPort: DJI Compatability mode (HD)";
         } else {
             mspOsdDisplayPort.displayPortType = "MSP DisplayPort";
         }
diff --git a/src/main/io/dji_osd_symbols.h b/src/main/io/dji_osd_symbols.h
new file mode 100644
index 00000000000..66972a810e7
--- /dev/null
+++ b/src/main/io/dji_osd_symbols.h
@@ -0,0 +1,164 @@
+/* @file max7456_symbols.h
+ * @brief max7456 symbols for the mwosd font set
+ *
+ * @author Nathan Tsoi nathan@vertile.com
+ *
+ * Copyright (C) 2016 Nathan Tsoi
+ *
+ * This program 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.
+ *
+ * This program 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
+
+//Misc
+#define DJI_SYM_NONE                    0x00
+#define DJI_SYM_END_OF_FONT             0xFF
+#define DJI_SYM_BLANK                   0x20
+#define DJI_SYM_HYPHEN                  0x2D
+#define DJI_SYM_BBLOG                   0x10
+#define DJI_SYM_HOMEFLAG                0x11
+#define DJI_SYM_RPM                     0x12
+#define DJI_SYM_ROLL                    0x14
+#define DJI_SYM_PITCH                   0x15
+#define DJI_SYM_TEMPERATURE             0x7A
+#define DJI_SYM_MAX                     0x24
+
+// GPS and navigation
+#define DJI_SYM_LAT                     0x89
+#define DJI_SYM_LON                     0x98
+#define DJI_SYM_ALTITUDE                0x7F
+#define DJI_SYM_OVER_HOME               0x05
+
+// RSSI
+#define DJI_SYM_RSSI                    0x01
+
+// Throttle Position (%)
+#define DJI_SYM_THR                     0x04
+
+// Unit Icons (Metric)
+#define DJI_SYM_M                       0x0C
+#define DJI_SYM_C                       0x0E
+
+// Unit Icons (Imperial)
+#define DJI_SYM_F                       0x0D
+#define DJI_SYM_FT                      0x0F
+
+// Heading Graphics
+#define DJI_SYM_HEADING_N               0x18
+#define DJI_SYM_HEADING_S               0x19
+#define DJI_SYM_HEADING_E               0x1A
+#define DJI_SYM_HEADING_W               0x1B
+#define DJI_SYM_HEADING_DIVIDED_LINE    0x1C
+#define DJI_SYM_HEADING_LINE            0x1D
+
+// AH Center screen Graphics
+#define DJI_SYM_CROSSHAIR_LEFT          0x72
+#define DJI_SYM_CROSSHAIR_CENTRE        0x73
+#define DJI_SYM_CROSSHAIR_RIGHT         0x74
+#define DJI_SYM_AH_RIGHT                0x02
+#define DJI_SYM_AH_LEFT                 0x03
+#define DJI_SYM_AH_DECORATION           0x13
+#define DJI_SYM_SMALL_CROSSHAIR         0x7E
+
+// Satellite Graphics
+#define DJI_SYM_SAT_L                   0x1E
+#define DJI_SYM_SAT_R                   0x1F
+
+// Direction arrows
+#define DJI_SYM_ARROW_SOUTH             0x60
+#define DJI_SYM_ARROW_2                 0x61
+#define DJI_SYM_ARROW_3                 0x62
+#define DJI_SYM_ARROW_4                 0x63
+#define DJI_SYM_ARROW_EAST              0x64
+#define DJI_SYM_ARROW_6                 0x65
+#define DJI_SYM_ARROW_7                 0x66
+#define DJI_SYM_ARROW_8                 0x67
+#define DJI_SYM_ARROW_NORTH             0x68
+#define DJI_SYM_ARROW_10                0x69
+#define DJI_SYM_ARROW_11                0x6A
+#define DJI_SYM_ARROW_12                0x6B
+#define DJI_SYM_ARROW_WEST              0x6C
+#define DJI_SYM_ARROW_14                0x6D
+#define DJI_SYM_ARROW_15                0x6E
+#define DJI_SYM_ARROW_16                0x6F
+
+#define DJI_SYM_ARROW_SMALL_UP          0x75
+#define DJI_SYM_ARROW_SMALL_DOWN        0x76
+#define DJI_SYM_ARROW_SMALL_RIGHT       0x77
+#define DJI_SYM_ARROW_SMALL_LEFT        0x78
+
+// AH Bars
+#define DJI_SYM_AH_BAR9_0               0x80
+#define DJI_SYM_AH_BAR9_1               0x81
+#define DJI_SYM_AH_BAR9_2               0x82
+#define DJI_SYM_AH_BAR9_3               0x83
+#define DJI_SYM_AH_BAR9_4               0x84
+#define DJI_SYM_AH_BAR9_5               0x85
+#define DJI_SYM_AH_BAR9_6               0x86
+#define DJI_SYM_AH_BAR9_7               0x87
+#define DJI_SYM_AH_BAR9_8               0x88
+
+// Progress bar
+#define DJI_SYM_PB_START                0x8A
+#define DJI_SYM_PB_FULL                 0x8B
+#define DJI_SYM_PB_HALF                 0x8C
+#define DJI_SYM_PB_EMPTY                0x8D
+#define DJI_SYM_PB_END                  0x8E
+#define DJI_SYM_PB_CLOSE                0x8F
+
+// Batt evolution
+#define DJI_SYM_BATT_FULL               0x90
+#define DJI_SYM_BATT_5                  0x91
+#define DJI_SYM_BATT_4                  0x92
+#define DJI_SYM_BATT_3                  0x93
+#define DJI_SYM_BATT_2                  0x94
+#define DJI_SYM_BATT_1                  0x95
+#define DJI_SYM_BATT_EMPTY              0x96
+
+// Batt Icons
+#define DJI_SYM_MAIN_BATT               0x97
+
+// Voltage and amperage
+#define DJI_SYM_VOLT                    0x06
+#define DJI_SYM_AMP                     0x9A
+#define DJI_SYM_MAH                     0x07
+#define DJI_SYM_WATT                    0x57  // 0x57 is 'W'
+
+// Time
+#define DJI_SYM_ON_H                    0x70
+#define DJI_SYM_FLY_H                   0x71
+#define DJI_SYM_ON_M                    0x9B
+#define DJI_SYM_FLY_M                   0x9C
+
+// Speed
+#define DJI_SYM_KPH                     0x9E
+#define DJI_SYM_MPH                     0x9D
+#define DJI_SYM_MPS                     0x9F
+#define DJI_SYM_FTPS                    0x99
+
+// Menu cursor
+#define DJI_SYM_CURSOR                  DJI_SYM_AH_LEFT
+
+// Stick overlays
+#define DJI_SYM_STICK_OVERLAY_SPRITE_HIGH 0x08
+#define DJI_SYM_STICK_OVERLAY_SPRITE_MID  0x09
+#define DJI_SYM_STICK_OVERLAY_SPRITE_LOW  0x0A
+#define DJI_SYM_STICK_OVERLAY_CENTER      0x0B
+#define DJI_SYM_STICK_OVERLAY_VERTICAL    0x16
+#define DJI_SYM_STICK_OVERLAY_HORIZONTAL  0x17
+
+// GPS degree/minute/second symbols
+#define DJI_SYM_GPS_DEGREE              DJI_SYM_STICK_OVERLAY_SPRITE_HIGH  // kind of looks like the degree symbol
+#define DJI_SYM_GPS_MINUTE              0x27 // '
+#define DJI_SYM_GPS_SECOND              0x22 // "
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 56245ea04af..f3f00ae7bdb 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -70,7 +70,7 @@
 #include "io/osd_common.h"
 #include "io/osd_hud.h"
 #include "io/osd_utils.h"
-#include "io/displayport_msp_bf_compat.h"
+#include "io/displayport_msp_dji_compat.h"
 #include "io/vtx.h"
 #include "io/vtx_string.h"
 
@@ -253,51 +253,6 @@ bool osdIsNotMetric(void) {
     return !(osdConfig()->units == OSD_UNIT_METRIC || osdConfig()->units == OSD_UNIT_METRIC_MPH);
 }
 
-/*
- * Aligns text to the left side. Adds spaces at the end to keep string length unchanged.
- */
-/* -- Currently unused --
-static void osdLeftAlignString(char *buff)
-{
-    uint8_t sp = 0, ch = 0;
-    uint8_t len = strlen(buff);
-    while (buff[sp] == ' ') sp++;
-    for (ch = 0; ch < (len - sp); ch++) buff[ch] = buff[ch + sp];
-    for (sp = ch; sp < len; sp++) buff[sp] = ' ';
-}*/
-
-/*
- * This is a simplified distance conversion code that does not use any scaling
- * but is fully compatible with the DJI G2 MSP Displayport OSD implementation.
- * (Based on osdSimpleAltitudeSymbol() implementation)
- */
-/* void osdSimpleDistanceSymbol(char *buff, int32_t dist) {
-
-    int32_t convertedDistance;
-    char suffix;
-
-    switch ((osd_unit_e)osdConfig()->units) {
-        case OSD_UNIT_UK:
-            FALLTHROUGH;
-        case OSD_UNIT_GA:
-            FALLTHROUGH;
-        case OSD_UNIT_IMPERIAL:
-            convertedDistance = CENTIMETERS_TO_FEET(dist);
-            suffix = SYM_ALT_FT;
-            break;
-        case OSD_UNIT_METRIC_MPH:
-            FALLTHROUGH;
-        case OSD_UNIT_METRIC:
-            convertedDistance = CENTIMETERS_TO_METERS(dist);
-            suffix = SYM_ALT_M; // Intentionally use the altitude symbol, as the distance symbol is not defined in BFCOMPAT mode
-            break;
-    }
-
-    tfp_sprintf(buff, "%5d", (int) convertedDistance); // 5 digits, allowing up to 99999 meters/feet, which should be plenty for 99.9% of use cases
-    buff[5] = suffix;
-    buff[6] = '\0';
-} */
-
 /**
  * Converts distance into a string based on the current unit system
  * prefixed by a a symbol to indicate the unit used.
@@ -313,12 +268,12 @@ static void osdFormatDistanceSymbol(char *buff, int32_t dist, uint8_t decimals)
     uint8_t symbol_mi = SYM_DIST_MI;
     uint8_t symbol_nm = SYM_DIST_NM;
 
-#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())) {
+#ifndef DISABLE_MSP_DJI_COMPAT   // IF DJICOMPAT is not supported, there's no need to check for it and change the values
+    if (isDJICompatibleVideoSystem(osdConfig())) {
         // Add one digit so up no switch to scaled decimal occurs above 99
         digits = 4U;
         sym_index = 4U;
-        // Use altitude symbols on purpose, as it seems distance symbols are not defined in BFCOMPAT mode
+        // Use altitude symbols on purpose, as it seems distance symbols are not defined in DJICOMPAT mode
         symbol_m = SYM_ALT_M;
         symbol_km = SYM_ALT_KM;
         symbol_ft = SYM_ALT_FT;
@@ -570,8 +525,8 @@ 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  
-    if (isBfCompatibleVideoSystem(osdConfig())) {
+#ifndef DISABLE_MSP_DJI_COMPAT   // IF DJICOMPAT is not supported, there's no need to check for it and change the values  
+    if (isDJICompatibleVideoSystem(osdConfig())) {
         totalDigits++;
         digits++;
         symbolIndex++;
@@ -792,21 +747,21 @@ static void osdFormatCoordinate(char *buff, char sym, int32_t val)
     int32_t decimalPart = abs(val % (int)GPS_DEGREES_DIVIDER);
     STATIC_ASSERT(GPS_DEGREES_DIVIDER == 1e7, adjust_max_decimal_digits);
     int decimalDigits;
-    bool bfcompat = false;  // Assume BFCOMPAT mode is no enabled
+    bool djiCompat = false;  // Assume DJICOMPAT mode is no enabled
 
-#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it
-        if(isBfCompatibleVideoSystem(osdConfig())) {
-            bfcompat = true;
+#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it
+        if(isDJICompatibleVideoSystem(osdConfig())) {
+            djiCompat = true;
         }
 #endif
 
-    if (!bfcompat) {
+    if (!djiCompat) {
         decimalDigits = tfp_sprintf(buff + 1 + integerDigits, "%07d", (int)decimalPart);
         // Embbed the decimal separator
         buff[1 + integerDigits - 1] += SYM_ZERO_HALF_TRAILING_DOT - '0';
         buff[1 + integerDigits] += SYM_ZERO_HALF_LEADING_DOT - '0';
     } else {
-        // BFCOMPAT mode enabled
+        // DJICOMPAT mode enabled
         decimalDigits = tfp_sprintf(buff + 1 + integerDigits, ".%06d", (int)decimalPart);
     }
     // Fill up to coordinateLength with zeros
@@ -1752,8 +1707,8 @@ static bool osdDrawSingleElement(uint8_t item)
 
     case OSD_MAIN_BATT_VOLTAGE: {
         uint8_t base_digits = 2U;
-#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it
-        if(isBfCompatibleVideoSystem(osdConfig())) {
+#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it
+        if(isDJICompatibleVideoSystem(osdConfig())) {
             base_digits = 3U;   // Add extra digit to account for decimal point taking an extra character space
         }
 #endif
@@ -1763,8 +1718,8 @@ static bool osdDrawSingleElement(uint8_t item)
 
     case OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE: {
         uint8_t base_digits = 2U;
-#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it
-        if(isBfCompatibleVideoSystem(osdConfig())) {
+#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it
+        if(isDJICompatibleVideoSystem(osdConfig())) {
             base_digits = 3U;   // Add extra digit to account for decimal point taking an extra character space
         }
 #endif
@@ -1787,9 +1742,9 @@ static bool osdDrawSingleElement(uint8_t item)
     case OSD_MAH_DRAWN: {
         uint8_t mah_digits = osdConfig()->mAh_precision; // Initialize to config value
 
-#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it
-        if (isBfCompatibleVideoSystem(osdConfig())) {
-            //BFcompat is unable to work with scaled values and it only has mAh symbol to work with
+#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it
+        if (isDJICompatibleVideoSystem(osdConfig())) {
+            //DJIcompat is unable to work with scaled values and it only has mAh symbol to work with
             tfp_sprintf(buff, "%5d", (int)getMAhDrawn());   // Use 5 digits to allow packs below 100Ah
             buff[5] = SYM_MAH;
             buff[6] = '\0';
@@ -1828,9 +1783,9 @@ static bool osdDrawSingleElement(uint8_t item)
         else if (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH) {
             uint8_t mah_digits = osdConfig()->mAh_precision; // Initialize to config value
 
-#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it
-            if (isBfCompatibleVideoSystem(osdConfig())) {
-                //BFcompat is unable to work with scaled values and it only has mAh symbol to work with
+#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it
+            if (isDJICompatibleVideoSystem(osdConfig())) {
+                //DJIcompat is unable to work with scaled values and it only has mAh symbol to work with
                 tfp_sprintf(buff, "%5d", (int)getBatteryRemainingCapacity());   // Use 5 digits to allow packs below 100Ah
                 buff[5] = SYM_MAH;
                 buff[6] = '\0';
@@ -2139,8 +2094,8 @@ static bool osdDrawSingleElement(uint8_t item)
             buff[1] = SYM_HDP_R;
             int32_t centiHDOP = 100 * gpsSol.hdop / HDOP_SCALE;
             uint8_t digits = 2U;
-#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())) {
+#ifndef DISABLE_MSP_DJI_COMPAT   // IF DJICOMPAT is not supported, there's no need to check for it and change the values
+            if (isDJICompatibleVideoSystem(osdConfig())) {
                 digits = 3U;
             }
 #endif
@@ -2799,8 +2754,8 @@ static bool osdDrawSingleElement(uint8_t item)
                 // time will be longer than 99 minutes. If it is, it will show 99:^^
                 if (glideTime > (99 * 60) + 59) {
                     tfp_sprintf(buff + 1, "%02d:", (int)(glideTime / 60));
-                    buff[4] = SYM_DIRECTION;
-                    buff[5] = SYM_DIRECTION;
+                    buff[4] = SYM_DECORATION;
+                    buff[5] = SYM_DECORATION;
                 } else {
                     tfp_sprintf(buff + 1, "%02d:%02d", (int)(glideTime / 60), (int)(glideTime % 60));
                 }
@@ -3147,8 +3102,8 @@ static bool osdDrawSingleElement(uint8_t item)
     case OSD_MAIN_BATT_CELL_VOLTAGE:
         {
             uint8_t base_digits = 3U;
-#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it
-            if(isBfCompatibleVideoSystem(osdConfig())) {
+#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it
+            if(isDJICompatibleVideoSystem(osdConfig())) {
                 base_digits = 4U;   // Add extra digit to account for decimal point taking an extra character space
             }
 #endif
@@ -3159,8 +3114,8 @@ static bool osdDrawSingleElement(uint8_t item)
     case OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE:
         {
             uint8_t base_digits = 3U;
-#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it
-            if(isBfCompatibleVideoSystem(osdConfig())) {
+#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it
+            if(isDJICompatibleVideoSystem(osdConfig())) {
                 base_digits = 4U;   // Add extra digit to account for decimal point taking an extra character space
             }
 #endif
@@ -3215,8 +3170,8 @@ static bool osdDrawSingleElement(uint8_t item)
             timeUs_t currentTimeUs = micros();
             timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated);
             uint8_t digits = 3U;
-#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())) {
+#ifndef DISABLE_MSP_DJI_COMPAT   // IF DJICOMPAT is not supported, there's no need to check for it and change the values
+            if (isDJICompatibleVideoSystem(osdConfig())) {
                 // Increase number of digits so values above 99 don't get scaled by osdFormatCentiNumber
                 digits = 4U;
             }
@@ -3248,7 +3203,7 @@ static bool osdDrawSingleElement(uint8_t item)
                     }
                     if (!efficiencyValid) {
                         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] = SYM_MAH_MI_0;        // This will overwrite the "-" at buff[3] if not in DJICOMPAT mode
                         buff[digits + 1] = SYM_MAH_MI_1;
                         buff[digits + 2] = '\0';
                     }
@@ -3418,7 +3373,7 @@ static bool osdDrawSingleElement(uint8_t item)
             horizontalWindSpeed = getEstimatedHorizontalWindSpeed(&angle);
             int16_t windDirection = osdGetHeadingAngle( CENTIDEGREES_TO_DEGREES((int)angle) - DECIDEGREES_TO_DEGREES(attitude.values.yaw) + 22);
             buff[0] = SYM_WIND_HORIZONTAL;
-            buff[1] = SYM_DIRECTION + (windDirection*2 / 90);
+            buff[1] = SYM_DECORATION + (windDirection*2 / 90);
             osdFormatWindSpeedStr(buff + 2, horizontalWindSpeed, valid);
             break;
         }
@@ -3435,10 +3390,10 @@ static bool osdDrawSingleElement(uint8_t item)
             float verticalWindSpeed;
             verticalWindSpeed = -getEstimatedWindSpeed(Z);  //from NED to NEU
             if (verticalWindSpeed < 0) {
-                buff[1] = SYM_AH_DIRECTION_DOWN;
+                buff[1] = SYM_AH_DECORATION_DOWN;
                 verticalWindSpeed = -verticalWindSpeed;
             } else {
-                buff[1] = SYM_AH_DIRECTION_UP;
+                buff[1] = SYM_AH_DECORATION_UP;
             }
             osdFormatWindSpeedStr(buff + 2, verticalWindSpeed, valid);
             break;
@@ -3552,7 +3507,7 @@ static bool osdDrawSingleElement(uint8_t item)
             } else {
                 referenceSymbol = '-';
             }
-            displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_DIRECTION);
+            displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_DECORATION);
             displayWriteChar(osdDisplayPort, elemPosX, elemPosY + 1, referenceSymbol);
             return true;
         }
@@ -4186,8 +4141,8 @@ uint8_t drawLogos(bool singular, uint8_t row) {
     uint8_t logoColOffset = 0;
     bool usePilotLogo = (osdConfig()->use_pilot_logo && osdDisplayIsHD());
 
-#ifndef DISABLE_MSP_BF_COMPAT   // IF BFCOMPAT is in use, the pilot logo cannot be used, due to font issues.
-    if (isBfCompatibleVideoSystem(osdConfig()))
+#ifndef DISABLE_MSP_DJI_COMPAT   // IF DJICOMPAT is in use, the pilot logo cannot be used, due to font issues.
+    if (isDJICompatibleVideoSystem(osdConfig()))
         usePilotLogo = false;
 #endif
 
@@ -4727,8 +4682,8 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b
 
     tfp_sprintf(outBuff, ": ");
     uint8_t digits = 3U;    // Total number of digits (including decimal point)
-#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())) {
+#ifndef DISABLE_MSP_DJI_COMPAT   // IF DJICOMPAT is not supported, there's no need to check for it and change the values
+    if (isDJICompatibleVideoSystem(osdConfig())) {
         // Add one digit so no switch to scaled decimal occurs above 99
         digits = 4U;
     }
diff --git a/src/main/io/osd_grid.c b/src/main/io/osd_grid.c
index 5d7e8736d91..9e79194498d 100644
--- a/src/main/io/osd_grid.c
+++ b/src/main/io/osd_grid.c
@@ -315,16 +315,16 @@ void osdGridDrawSidebars(displayPort_t *display)
     // Arrows
     if (osdConfig()->sidebar_scroll_arrows) {
         displayWriteChar(display, elemPosX - hudwidth, elemPosY - hudheight - 1,
-            left.arrow == OSD_SIDEBAR_ARROW_UP ? SYM_AH_DIRECTION_UP : SYM_BLANK);
+            left.arrow == OSD_SIDEBAR_ARROW_UP ? SYM_AH_DECORATION_UP : SYM_BLANK);
 
         displayWriteChar(display, elemPosX + hudwidth, elemPosY - hudheight - 1,
-            right.arrow == OSD_SIDEBAR_ARROW_UP ? SYM_AH_DIRECTION_UP : SYM_BLANK);
+            right.arrow == OSD_SIDEBAR_ARROW_UP ? SYM_AH_DECORATION_UP : SYM_BLANK);
 
         displayWriteChar(display, elemPosX - hudwidth, elemPosY + hudheight + 1,
-            left.arrow == OSD_SIDEBAR_ARROW_DOWN ? SYM_AH_DIRECTION_DOWN : SYM_BLANK);
+            left.arrow == OSD_SIDEBAR_ARROW_DOWN ? SYM_AH_DECORATION_DOWN : SYM_BLANK);
 
         displayWriteChar(display, elemPosX + hudwidth, elemPosY + hudheight + 1,
-            right.arrow == OSD_SIDEBAR_ARROW_DOWN ? SYM_AH_DIRECTION_DOWN : SYM_BLANK);
+            right.arrow == OSD_SIDEBAR_ARROW_DOWN ? SYM_AH_DECORATION_DOWN : SYM_BLANK);
     }
 
     // Draw AH sides
diff --git a/src/main/io/osd_hud.c b/src/main/io/osd_hud.c
index 07bc181d1c1..8a6a68f467a 100644
--- a/src/main/io/osd_hud.c
+++ b/src/main/io/osd_hud.c
@@ -207,7 +207,7 @@ void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitu
 
     if (poiType == 1) { // POI from the ESP radar
         error_x = hudWrap360(poiP1 - DECIDEGREES_TO_DEGREES(osdGetHeading()));
-        osdHudWrite(poi_x - 1, poi_y, SYM_DIRECTION + ((error_x + 22) / 45) % 8, 1);
+        osdHudWrite(poi_x - 1, poi_y, SYM_DECORATION + ((error_x + 22) / 45) % 8, 1);
         osdHudWrite(poi_x + 1, poi_y, SYM_HUD_SIGNAL_0 + poiP2, 1);
     }
     else if (poiType == 2) { // Waypoint,
@@ -248,7 +248,7 @@ void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitu
             tfp_sprintf(buff, "%3d", altc);
         }
 
-        buff[0] = (poiAltitude >= 0) ? SYM_AH_DIRECTION_UP : SYM_AH_DIRECTION_DOWN;
+        buff[0] = (poiAltitude >= 0) ? SYM_AH_DECORATION_UP : SYM_AH_DECORATION_DOWN;
     } else { // Display the distance by default 
         switch ((osd_unit_e)osdConfig()->units) {
             case OSD_UNIT_UK:
diff --git a/src/main/io/osd_utils.c b/src/main/io/osd_utils.c
index 6675be8783b..9c9fb0608a0 100644
--- a/src/main/io/osd_utils.c
+++ b/src/main/io/osd_utils.c
@@ -20,7 +20,7 @@
 #include "common/maths.h"
 #include "common/typeconversion.h"
 #include "drivers/osd_symbols.h"
-#include "io/displayport_msp_bf_compat.h"
+#include "io/displayport_msp_dji_compat.h"
 
 #if defined(USE_OSD) || defined(OSD_UNIT_TEST)
 
@@ -45,7 +45,7 @@ bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int ma
     int decimals = maxDecimals;
     bool negative = false;
     bool scaled = false;
-    bool explicitDecimal = isBfCompatibleVideoSystem(osdConfig());
+    bool explicitDecimal = isDJICompatibleVideoSystem(osdConfig());
 
     buff[length] = '\0';
 

From 10d7dd58c42bfab845172e98104c0254663d6552 Mon Sep 17 00:00:00 2001
From: Scavanger <andreas.kanzler@outlook.com>
Date: Wed, 15 May 2024 22:20:16 -0300
Subject: [PATCH 120/323] snapp

---
 docs/Broken USB recovery.md | 118 ++++++++++++++++++++++++++++++++++++
 1 file changed, 118 insertions(+)
 create mode 100644 docs/Broken USB recovery.md

diff --git a/docs/Broken USB recovery.md b/docs/Broken USB recovery.md
new file mode 100644
index 00000000000..46db627c83e
--- /dev/null
+++ b/docs/Broken USB recovery.md	
@@ -0,0 +1,118 @@
+# Broken USB recovery 
+
+It is possible to flash INAV without USB over UART 1 or 3.
+
+## Prerequisites:
+- USB/UART adapter (FT232, CP2102, etc.)
+- STM32 Cube Programmer (https://www.st.com/en/development-tools/stm32cubeprog.html)
+
+To gain access to the FC via Configurator, MSP must be activated on a UART as standard. Some FCs already have this enabled by default, if not a custom firmware must be created.
+
+The following targets have MSP activated on a UART by default:
+
+| Target | Standard MSP Port |
+|-----------| ----------- |
+| AOCODARCF4V3 | UART5 |
+| ATOMRCF405NAVI_DELUXE | UART1 |
+| FF_F35_LIGHTNING | UART1 |
+| FLYCOLORF7V2 | UART4 |
+| GEPRCF405_BT_HD | UART5* |
+| GEPRCF722_BT_HD | UART4* |
+| IFLIGHT_BLITZ_F7_AIO | UART1 |
+| JHEMCUF405WING | UART6 |
+| JHEMCUH743HD | UART4 |
+| KAKUTEH7 | UART1 and UART2* |
+| KAKUTEH7WING | UART6 |
+| MAMBAF405_2022A | UART4 |
+| MAMBAF405US | UART4 |
+| MAMBAF722 | UART4 |
+| MAMBAF722 APP | UART4*|
+| MAMBAF722WING | UART4 |
+| MAMBAF722_X8 | UART4 |
+| MAMBAH743 | UART4* |
+| MATEKF405SE | UART1 |
+| NEUTRONRCH743BT | UART3* |
+| SDMODELH7V1 | UART1 and UART2 |
+| SKYSTARSH743HD | UART4 |
+| SPEEDYBEEF4 | UART5* |
+| SPEEDYBEEF405MINI | UART4* |
+| SPEEDYBEEF405V3 | UART4* |
+| SPEEDYBEEF405V4 | UART4* |
+| SPEEDYBEEF405WING | UART6 |
+| SPEEDYBEEF7 | UART6 |
+| SPRACINGF4EVO | UART1 |
+| TMOTORF7V2 | UART5 |
+
+(*) No Pads/Pins, Port is used interally (Bluetooth)
+
+## Custom firmware:
+
+If the FC does not have MSP activated on a UART by default or does not have a connector for it, a custom firmware must be built. 
+The following procedure describes the process under Windows 10/11:
+
+Please read [Building in Windows 2010 or 11 with Linux Subsystem](https://github.com/iNavFlight/inav/blob/master/docs/development/Building%20in%20Windows%2010%20or%2011%20with%20Linux%20Subsystem.md)
+and follow the instructions up to "Building with Make".
+
+To activate MSP by default, go to the directory `src/main/target/[your target]`.
+If no config.c exists, create a new text file with this name and the following content:
+
+```
+/*
+ * 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 "io/serial.h"
+
+void targetConfiguration(void)
+{
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USARTX)].functionMask = FUNCTION_MSP;
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USARTX)].msp_baudrateIndex = BAUD_115200;
+}
+
+```
+
+If the file already exists, add the following lines in the function `void targetConfiguration(void)` (before the last `}`)
+
+```
+serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USARTX)].functionMask = FUNCTION_MSP;
+serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USARTX)].msp_baudrateIndex = BAUD_115200;
+```
+
+Replace the X in SERIAL_PORT_USARTX (in both lines) with the number of UART/serial port on which MSP is to be activated.
+
+Example:
+For UART 2: `SERIAL_PORT_USART2`
+For UART 3: `SERIAL_PORT_USART3`
+etc.
+
+Save the file and build the firmware as described in the document above.
+
+## Flashing via Uart:
+
+1. Disconnect ALL peripherals and the USB Cable from the FC. To power the FC use a battery or use the 5V provided from the USB/Serial Converter. 
+2. Connect UART 1 or 3 (other UARTS will not work) and GND to the USB/Serial converter (RX -> TX, TX -> RX)
+3. Keep the boot/dfu button pressed
+4. Switch on the FC / supply with power
+5. Start STM32 CubeProgrammer and go to "Erasing & Programming", second option in the menu.
+6. Select UART (blue dropdown field) and select the COM port of the USB/Serial adapter and press "Connect". The corresponding processor should now be displayed below.
+7. Click on "Full flash erase". This is also necessary if you are flashing the same firmware version that was previously on the FC, otherwise MSP may not be activated on the UART.
+8. Under "Download" load the previously created firmware (`INAV_X.X.X_[Your Target].hex`) or the standard firmware if UART is already activated there. The option "Verify programming" is optional but recommended. Make sure that "Skip flash erase while programming" is NOT activated.
+9. Click "Start Programming"
+
+After the process is completed, switch the FC off and on again and then the Configurator can connect to the FC via USB/serial adapter and the previously configured UART.

From 3d9d1f3b91aa255c80919d90d89dbfa755894a64 Mon Sep 17 00:00:00 2001
From: Darren Lines <darren@darrenlines.uk>
Date: Thu, 16 May 2024 07:43:56 +0100
Subject: [PATCH 121/323] Update docs

---
 docs/Betaflight 4.3 compatible OSD.md | 50 ---------------------------
 docs/DJI compatible OSD.md            | 50 +++++++++++++++++++++++++++
 2 files changed, 50 insertions(+), 50 deletions(-)
 delete mode 100644 docs/Betaflight 4.3 compatible OSD.md
 create mode 100644 docs/DJI compatible OSD.md

diff --git a/docs/Betaflight 4.3 compatible OSD.md b/docs/Betaflight 4.3 compatible OSD.md
deleted file mode 100644
index 0e9644dae24..00000000000
--- a/docs/Betaflight 4.3 compatible OSD.md	
+++ /dev/null
@@ -1,50 +0,0 @@
-# Betaflight 4.3 compatible MSP DisplayPort OSD (DJI O3 "Canvas Mode")
-
-INAV 6.0 includes a special mode for MSP DisplayPort that supports incomplete implementations of MSP DisplayPort that only support BetaFlight, like the DJI O3 Air Unit. INAV 6.1 expands this to include HD canvas sizes from BetaFlight 4.4.
-
-Different flight controllers have different OSD symbols and elements and require different fonts. BetaFlight's font is a single page and supports a maximum of 256 glyphs, INAV's font is currently 2 pages and supports up to 512 different glyphs.
-
-While there is some overlap between the glyphs in BetaFlight and INAV, it is not possible to perform a 1 to 1 mapping for all the them. In cases where there is no suitable glyph in the BetaFlight font, a question mark `?` will be displayed.
-
-This mode can be enabled by selecting BF43COMPAT or BFHDCOMPAT as video format in the OSD tab of the configurator or by typing the following command on the CLI:
-
-`set osd_video_system = BF43COMPAT`
-
-or
-
-`set osd_video_system = BFHDCOMPAT`
-
-## Limitations
-
-* Canvas size needs to be manually changed to HD on the Display menu in DJI's goggles (you may need a firmware update) and set as BFHDCOMPAT in the OSD tab of the configurator.
-* Unsupported Glyphs show up as `?`
-
-## FAQ
-
-### I see a lot of `?` on my OSD.
-
-That is expected, when your INAV OSD widgets use glyphs that don't have a suitable mapping in BetaFlight's font.
-
-### Does it work with the G2 and Original Air Unit/Vista?
-
-Yes.
-
-### Is this a replacement for WTFOS?
-
-Not exactly. WTFOS is a full implementation of MSP-Displayport for rooted Air Unit/Vista/Googles V2 and actually works much better than BetaFlight compatibility mode, being able to display all INAV's glyphs.
-
-### Can INAV fix DJI's product?
-
-No. OSD renderinng happens on the googles/air unit side of things. Please ask DJI to fix their incomplete MSP DisplayPort implemenation. You can probably request it in [DJI's forum](https://forum.dji.com/forum.php?mod=forumdisplay&fid=129&filter=typeid&typeid=767).
-
-### BetaFlight X.Y now has more symbols, can you update INAV?
-
-Maybe. If a future version of BetaFlight includes more Glyphs that can be mapped into INAV it is fairly simple to add the mapping, but the problem with DJI's implemenation persists. Even if we update the mapping, if DJI does not update the fonts on their side the problem will persist.
-
-### Can you replace glyph `X` with text `x description`?
-
-While it might technically be possible to replace some glyphs with text in multiple cells, it will introduce a lot of complexity in the OSD rendering and configuration for something we hope is a temporary workaround.
-
-### Does DJI support Canvas Mode?
-
-Actually, no. What DJI calls Canvas Mode is actually MSP DisplayPort and is a character based OSD.
diff --git a/docs/DJI compatible OSD.md b/docs/DJI compatible OSD.md
new file mode 100644
index 00000000000..6af25670306
--- /dev/null
+++ b/docs/DJI compatible OSD.md	
@@ -0,0 +1,50 @@
+# DJI compatible MSP DisplayPort OSD (DJI O3 "Canvas Mode")
+
+INAV 6.0 includes a special mode for MSP DisplayPort that supports DJI's incomplete implementations of MSP DisplayPort. This can be found on products like the DJI O3 Air Unit. INAV 6.1 expands this to include HD canvas sizes from BetaFlight 4.4.
+
+Different flight controller firmware have different OSD symbols and elements and require different fonts. BetaFlight's font is a single page and supports a maximum of 256 glyphs, INAV's font is currently 2 pages and supports up to 512 different glyphs. DJI's font is single page and based, but not the same as, BetaFlight's font.
+
+While there is some overlap between the glyphs in DJI and INAV, it is not possible to perform a 1 to 1 mapping for all the them. In cases where there is no suitable glyph in the DJI font, a question mark `?` will be displayed.
+
+This mode can be enabled by selecting DJI43COMPAT or DJIHDCOMPAT as video format in the OSD tab of the configurator or by typing the following command on the CLI:
+
+`set osd_video_system = DJI43COMPAT`
+
+or
+
+`set osd_video_system = DJIHDCOMPAT`
+
+## Limitations
+
+* Canvas size needs to be manually changed to HD on the Display menu in DJI's goggles (you may need a firmware update) and set as DJIHDCOMPAT in the OSD tab of the configurator.
+* Unsupported Glyphs show up as `?`
+
+## FAQ
+
+### I see a lot of `?` on my OSD.
+
+That is expected. When your INAV OSD widgets use glyphs that don't have a suitable mapping in DJI's font.
+
+### Does it work with the G2 and Original Air Unit/Vista?
+
+Yes.
+
+### Is this a replacement for WTFOS?
+
+Not exactly. WTFOS is a full implementation of MSP-Displayport for rooted Air Unit/Vista/Googles V2 and actually works much better than DJI compatibility mode. It can use all of INAV's OSD elements as intended. If you have the option of WTFOS or DJI compatability mode. WTFOS is the best option.
+
+### Can INAV fix DJI's product?
+
+No. OSD renderinng happens on the googles/air unit side of things. Please ask DJI to fix their incomplete MSP DisplayPort implemenation. You can probably request it in [DJI's forum](https://forum.dji.com/forum.php?mod=forumdisplay&fid=129&filter=typeid&typeid=767). To see what you're missing out on with O3. Check out what WTFOS did with the original system. Not only could the pilots upload the fonts of their choosing (who doesn't want a cool SneakyFPV font on their OSD). But there were no problems supporting and firmware. Plus, there was even an option to save the OSD to a file and overlay that over your DVR video. If you're reading this far. Please recommend to DJI that they fix their product, to at least what was possible with WTFOS.
+
+### DJI's font now has more symbols, can you update INAV?
+
+Maybe. If a future version of DJI's font includes more Glyphs that can be mapped into INAV. It is fairly simple to add the mapping. However, the best solution would be full support of MSP DisplayPort by DJI. Then there will never be an issue with missing icons. As the latest INAV font would be able to be uploaded on to the goggles.
+
+### Can you replace glyph `X` with text `x description`?
+
+While it might technically be possible to replace some glyphs with text in multiple cells, it will introduce a lot of complexity in the OSD rendering and configuration for something we hope is a temporary workaround.
+
+### Does DJI support Canvas Mode?
+
+Actually, no. What DJI calls Canvas Mode is actually MSP DisplayPort and is a character based OSD. Currently, the only true implementaion of Canvas Mode is with FrSKY PixelOSD. This was found on some F722 flight controllers from Matek.

From a818190ed12761ea7977f29f4d963c2a51310064 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sat, 18 May 2024 13:49:04 +0200
Subject: [PATCH 122/323] Use letter `Q` for DJI LinkQuality OSD element

---
 src/main/io/displayport_msp_dji_compat.c | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/src/main/io/displayport_msp_dji_compat.c b/src/main/io/displayport_msp_dji_compat.c
index de4fa8656fa..46e14c00470 100644
--- a/src/main/io/displayport_msp_dji_compat.c
+++ b/src/main/io/displayport_msp_dji_compat.c
@@ -41,10 +41,8 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page)
         case SYM_RSSI:
             return DJI_SYM_RSSI;
 
-/*
         case SYM_LQ:
-            return DJI_SYM_LINK_QUALITY;
-*/
+            return 'Q';
 
         case SYM_LAT:
             return DJI_SYM_LAT;

From ec7ebd1c4b3c66155896c0615e3c0f11590b62d7 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sat, 18 May 2024 14:15:59 +0200
Subject: [PATCH 123/323] Do not render DJI logo on OSD when using DJI Compat
 modes

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

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index f3f00ae7bdb..62186908ff9 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -4140,10 +4140,13 @@ uint8_t drawLogos(bool singular, uint8_t row) {
     uint8_t logoRow = row;
     uint8_t logoColOffset = 0;
     bool usePilotLogo = (osdConfig()->use_pilot_logo && osdDisplayIsHD());
+    bool useINAVLogo = (singular && !usePilotLogo) || !singular;
 
 #ifndef DISABLE_MSP_DJI_COMPAT   // IF DJICOMPAT is in use, the pilot logo cannot be used, due to font issues.
-    if (isDJICompatibleVideoSystem(osdConfig()))
+    if (isDJICompatibleVideoSystem(osdConfig())) {
         usePilotLogo = false;
+        useINAVLogo = false;
+    }
 #endif
 
     uint8_t logoSpacing = osdConfig()->inav_to_pilot_logo_spacing;
@@ -4160,7 +4163,7 @@ uint8_t drawLogos(bool singular, uint8_t row) {
     }
 
     // Draw INAV logo
-    if ((singular && !usePilotLogo) || !singular) {
+    if (useINAVLogo) {
         unsigned logo_c = SYM_LOGO_START;
         uint8_t logo_x = logoColOffset;
         for (uint8_t lRow = 0; lRow < SYM_LOGO_HEIGHT; lRow++) {
@@ -4191,7 +4194,7 @@ uint8_t drawLogos(bool singular, uint8_t row) {
     }
 
     return logoRow;
-    }
+}
 
 #ifdef USE_STATS
 uint8_t drawStat_Stats(uint8_t statNameX, uint8_t row, uint8_t statValueX, bool isBootStats)

From 3efb86c848123475d904f97ab6ee1ecb3716a0f3 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sat, 18 May 2024 14:16:29 +0200
Subject: [PATCH 124/323] Do not render DJI logo on OSD when using DJI compat
 mode

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

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 62186908ff9..b2407b2325b 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -4181,9 +4181,9 @@ uint8_t drawLogos(bool singular, uint8_t row) {
         logoRow = row;
         if (singular) {
             logo_x = logoColOffset;
-    } else {
-            logo_x = logoColOffset + SYM_LOGO_WIDTH + logoSpacing;
-    }
+        } else {
+                logo_x = logoColOffset + SYM_LOGO_WIDTH + logoSpacing;
+        }
 
         for (uint8_t lRow = 0; lRow < SYM_LOGO_HEIGHT; lRow++) {
             for (uint8_t lCol = 0; lCol < SYM_LOGO_WIDTH; lCol++) {
@@ -4193,6 +4193,10 @@ uint8_t drawLogos(bool singular, uint8_t row) {
         }
     }
 
+    if (!usePilotLogo && !useINAVLogo) {
+        logoRow += SYM_LOGO_HEIGHT;
+    }
+
     return logoRow;
 }
 

From 3615e6fb0981d7e03125fe454e8acaff7b1f6a34 Mon Sep 17 00:00:00 2001
From: dzaro-dev <23706061+dzaro-dev@users.noreply.github.com>
Date: Sat, 18 May 2024 16:58:22 +0100
Subject: [PATCH 125/323] Adding Crosshair Styles

---
 src/main/io/dji_osd_symbols.h | 20 ++++++++++++++++++++
 1 file changed, 20 insertions(+)

diff --git a/src/main/io/dji_osd_symbols.h b/src/main/io/dji_osd_symbols.h
index 66972a810e7..3ef5a6c5e9b 100644
--- a/src/main/io/dji_osd_symbols.h
+++ b/src/main/io/dji_osd_symbols.h
@@ -71,6 +71,26 @@
 #define DJI_SYM_AH_DECORATION           0x13
 #define DJI_SYM_SMALL_CROSSHAIR         0x7E
 
+// Crosshair Styles
+#define DJI_SYM_AH_CH_TYPE3             0x00
+#define DJI_SYM_AH_CH_TYPE3_1           0x7E
+#define DJI_SYM_AH_CH_TYPE3_2           0x00
+#define DJI_SYM_AH_CH_TYPE4             0x2D
+#define DJI_SYM_AH_CH_TYPE4_1           0x7E
+#define DJI_SYM_AH_CH_TYPE4_2           0x2D
+#define DJI_SYM_AH_CH_TYPE5             0x17
+#define DJI_SYM_AH_CH_TYPE5_1           0x7E
+#define DJI_SYM_AH_CH_TYPE5_2           0x17
+#define DJI_SYM_AH_CH_TYPE6             0x00
+#define DJI_SYM_AH_CH_TYPE6_1           0x09
+#define DJI_SYM_AH_CH_TYPE6_2           0x00
+#define DJI_SYM_AH_CH_TYPE7             0x78
+#define DJI_SYM_AH_CH_TYPE7_1           0x7E
+#define DJI_SYM_AH_CH_TYPE7_2           0x77
+#define DJI_SYM_AH_CH_TYPE8             0x02
+#define DJI_SYM_AH_CH_TYPE8_1           0x7E
+#define DJI_SYM_AH_CH_TYPE8_2           0x03
+
 // Satellite Graphics
 #define DJI_SYM_SAT_L                   0x1E
 #define DJI_SYM_SAT_R                   0x1F

From cc4b3db899a995eae8514c63309d985338efa2d3 Mon Sep 17 00:00:00 2001
From: dzaro-dev <23706061+dzaro-dev@users.noreply.github.com>
Date: Sat, 18 May 2024 17:02:59 +0100
Subject: [PATCH 126/323] Adding Crosshair Styles

---
 src/main/io/displayport_msp_dji_compat.c | 49 ++++++++++++++++++++----
 1 file changed, 42 insertions(+), 7 deletions(-)

diff --git a/src/main/io/displayport_msp_dji_compat.c b/src/main/io/displayport_msp_dji_compat.c
index de4fa8656fa..93ec60b9ce8 100644
--- a/src/main/io/displayport_msp_dji_compat.c
+++ b/src/main/io/displayport_msp_dji_compat.c
@@ -469,40 +469,75 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page)
 
         case SYM_AH_RIGHT:
             return DJI_SYM_AH_RIGHT;
-
 /*
         case SYM_AH_DECORATION_COUNT:
             return DJI_SYM_AH_DECORATION_COUNT;
 */
         case SYM_AH_CH_LEFT:
+        case SYM_AH_CH_AIRCRAFT1:
+            return DJI_SYM_CROSSHAIR_LEFT;
+
         case SYM_AH_CH_TYPE3:
+            return DJI_SYM_AH_CH_TYPE3;
+        
         case SYM_AH_CH_TYPE4:
+            return DJI_SYM_AH_CH_TYPE4;
+        
         case SYM_AH_CH_TYPE5:
+            return DJI_SYM_AH_CH_TYPE5;
+        
         case SYM_AH_CH_TYPE6:
+            return DJI_SYM_AH_CH_TYPE6;
+        
         case SYM_AH_CH_TYPE7:
+            return DJI_SYM_AH_CH_TYPE7;
+        
         case SYM_AH_CH_TYPE8:
-        case SYM_AH_CH_AIRCRAFT1:
-            return DJI_SYM_CROSSHAIR_LEFT;
+            return DJI_SYM_AH_CH_TYPE8;
 
         case SYM_AH_CH_CENTER:
+        case SYM_AH_CH_AIRCRAFT2:
+            return DJI_SYM_CROSSHAIR_CENTRE;
+
         case (SYM_AH_CH_TYPE3+1):
+            return DJI_SYM_AH_CH_TYPE3_1;
+        
         case (SYM_AH_CH_TYPE4+1):
+            return DJI_SYM_AH_CH_TYPE4_1;
+        
         case (SYM_AH_CH_TYPE5+1):
+            return DJI_SYM_AH_CH_TYPE5_1;
+        
         case (SYM_AH_CH_TYPE6+1):
+            return DJI_SYM_AH_CH_TYPE6_1;
+        
         case (SYM_AH_CH_TYPE7+1):
+            return DJI_SYM_AH_CH_TYPE7_1;
+        
         case (SYM_AH_CH_TYPE8+1):
-        case SYM_AH_CH_AIRCRAFT2:
-            return DJI_SYM_CROSSHAIR_CENTRE;
+            return DJI_SYM_AH_CH_TYPE8_1;
 
         case SYM_AH_CH_RIGHT:
+        case SYM_AH_CH_AIRCRAFT3:
+            return DJI_SYM_CROSSHAIR_RIGHT;
+
         case (SYM_AH_CH_TYPE3+2):
+            return DJI_SYM_AH_CH_TYPE3_2;
+        
         case (SYM_AH_CH_TYPE4+2):
+            return DJI_SYM_AH_CH_TYPE4_2;
+        
         case (SYM_AH_CH_TYPE5+2):
+            return DJI_SYM_AH_CH_TYPE5_2;
+        
         case (SYM_AH_CH_TYPE6+2):
+            return DJI_SYM_AH_CH_TYPE6_2;
+        
         case (SYM_AH_CH_TYPE7+2):
+            return DJI_SYM_AH_CH_TYPE7_2;
+        
         case (SYM_AH_CH_TYPE8+2):
-        case SYM_AH_CH_AIRCRAFT3:
-            return DJI_SYM_CROSSHAIR_RIGHT;
+            return DJI_SYM_AH_CH_TYPE8_2;
         
         case SYM_AH_CH_AIRCRAFT0:
         case SYM_AH_CH_AIRCRAFT4:

From 5e85fe07b1d8ca589390c060f915c9c64ecb26ba Mon Sep 17 00:00:00 2001
From: Darren Lines <darren@darrenlines.uk>
Date: Sat, 18 May 2024 20:31:04 +0100
Subject: [PATCH 127/323] Allow pilots the option to hide question marks from
 the OSD.

---
 docs/Settings.md                         | 10 ++++++
 src/main/fc/settings.yaml                | 41 +++---------------------
 src/main/io/displayport_msp_dji_compat.c |  2 +-
 src/main/io/osd.c                        |  5 ++-
 src/main/io/osd.h                        |  5 ++-
 5 files changed, 24 insertions(+), 39 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index 30533330adf..29a745544e4 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -4412,6 +4412,16 @@ Value under which the OSD axis g force indicators will blink (g)
 
 ---
 
+### osd_highlight_djis_missing_font_symbols
+
+Show question marks where there is no symbol in the DJI font to represent the INAV OSD element's symbol. When off, blank spaces will be used. Only relevent for DJICOMPAT modes.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| ON | OFF | ON |
+
+---
+
 ### osd_home_position_arm_screen
 
 Should home position coordinates be displayed on the arming screen.
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index dfa7bb97d35..d276aca274d 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -3453,7 +3453,6 @@ groups:
         min: 8
         max: 11
         default_value: 9
-
       - name: osd_adsb_distance_warning
         description: "Distance in meters of ADSB aircraft that is displayed"
         default_value: 20000
@@ -3478,7 +3477,6 @@ groups:
         min: 0
         max: 64000
         type: uint16_t
-
       - name: osd_estimations_wind_compensation
         description: "Use wind estimation for remaining flight time/distance estimation"
         default_value: ON
@@ -3491,12 +3489,10 @@ groups:
         condition: USE_WIND_ESTIMATOR
         field: estimations_wind_mps
         type: bool
-
       - name: osd_failsafe_switch_layout
         description: "If enabled the OSD automatically switches to the first layout during failsafe"
         default_value: OFF
         type: bool
-
       - name: osd_plus_code_digits
         description: "Numer of plus code digits before shortening with `osd_plus_code_short`. Precision at the equator: 10=13.9x13.9m; 11=2.8x3.5m; 12=56x87cm; 13=11x22cm."
         field: plus_code_digits
@@ -3508,213 +3504,186 @@ groups:
         field: plus_code_short
         default_value: "0"
         table: osd_plus_code_short
-
       - name: osd_ahi_style
         description: "Sets OSD Artificial Horizon style \"DEFAULT\" or \"LINE\" for the FrSky Graphical OSD."
         field: ahi_style
         default_value: "DEFAULT"
         table: osd_ahi_style
         type: uint8_t
-
       - name: osd_force_grid
         field: force_grid
         type: bool
         default_value: OFF
         description: Force OSD to work in grid mode even if the OSD device supports pixel level access (mainly used for development)
-
       - name: osd_ahi_bordered
         field: ahi_bordered
         type: bool
         description: Shows a border/corners around the AHI region (pixel OSD only)
         default_value: OFF
-
       - name: osd_ahi_width
         field: ahi_width
         max: 255
         description: AHI width in pixels (pixel OSD only)
         default_value: 132
-
       - name: osd_ahi_height
         field: ahi_height
         max: 255
         description: AHI height in pixels (pixel OSD only)
         default_value: 162
-
       - name: osd_ahi_vertical_offset
         field: ahi_vertical_offset
         min: -128
         max: 127
         description: AHI vertical offset from center (pixel OSD only)
         default_value: -18
-
       - name: osd_sidebar_horizontal_offset
         field: sidebar_horizontal_offset
         min: -128
         max: 127
         default_value: 0
         description: Sidebar horizontal offset from default position. Positive values move the sidebars closer to the edges.
-
       - name: osd_left_sidebar_scroll_step
         field: left_sidebar_scroll_step
         max: 255
         default_value: 0
         description: How many units each sidebar step represents. 0 means the default value for the scroll type.
-
       - name: osd_right_sidebar_scroll_step
         field: right_sidebar_scroll_step
         max: 255
         default_value: 0
         description: Same as left_sidebar_scroll_step, but for the right sidebar
-
       - name: osd_sidebar_height
         field: sidebar_height
         min: 0
         max: 5
         default_value: 3
         description: Height of sidebars in rows. 0 leaves only the level indicator arrows (Not for pixel OSD)
-
       - name: osd_ahi_pitch_interval
         field: ahi_pitch_interval
         min: 0
         max: 30
         default_value: 0
         description: Draws AHI at increments of the set pitch interval over the full pitch range. AHI line is drawn with ends offset when pitch first exceeds interval with offset increasing with increasing pitch. Offset direction changes between climb and dive. Set to 0 to disable (Not for pixel OSD)
-
       - name: osd_home_position_arm_screen
         type: bool
         default_value: ON
         description: Should home position coordinates be displayed on the arming screen.
-
       - name: osd_pan_servo_index
         description: Index of the pan servo, used to adjust osd home heading direction based on camera pan. Note that this feature does not work with continiously rotating servos.
         field: pan_servo_index
         min: 0
         max: 16
         default_value: 0
-
       - name: osd_pan_servo_pwm2centideg
         description: Centidegrees of pan servo rotation us PWM signal. A servo with 180 degrees of rotation from 1000 to 2000 us PWM typically needs `18` for this setting. Change sign to inverse direction.
         field: pan_servo_pwm2centideg
         default_value: 0
         min: -36
         max: 36
-
       - name: osd_pan_servo_offcentre_warning
         description: Degrees either side of the pan servo centre; where it is assumed camera is wanted to be facing forwards, but isn't at 0. If in this range and not 0 for longer than 10 seconds, the pan servo offset OSD element will blink. 0 means the warning is disabled.
         field: pan_servo_offcentre_warning
         min: 0
         max: 45
         default_value: 10
-
       - name: osd_pan_servo_indicator_show_degrees
         description: Show the degress of offset from centre on the pan servo OSD display element.
         field: pan_servo_indicator_show_degrees
         type: bool
         default_value: OFF
-
       - name: osd_esc_rpm_precision
         description: Number of characters used to display the RPM value.
         field: esc_rpm_precision
         min: 3
         max: 6
         default_value: 3
-
       - name: osd_mah_precision
         description: Number of digits used for mAh precision. Currently used by mAh Used and Battery Remaining Capacity
         field: mAh_precision
         min: 4
         max: 6
         default_value: 4
-
       - name: osd_use_pilot_logo
         description: Use custom pilot logo with/instead of the INAV logo. The pilot logo must be characters 473 to 511
         field: use_pilot_logo
         type: bool
         default_value: OFF
-
       - name: osd_inav_to_pilot_logo_spacing
         description: The space between the INAV and pilot logos, if `osd_use_pilot_logo` is `ON`. This number may be adjusted so that it fits the odd/even col width displays. For example, if using an odd column width display, such as Walksnail, and this is set to 4. 1 will be added so that the logos are equally spaced from the centre of the screen.
         field: inav_to_pilot_logo_spacing
         min: 0
         max: 20
         default_value: 8
-
       - name: osd_arm_screen_display_time
         description: Amount of time to display the arm screen [ms]
         field: arm_screen_display_time
         min: 1000
         max: 5000
         default_value: 1500
-
       - name: osd_switch_indicator_zero_name
         description: "Character to use for OSD switch incicator 0."
         field: osd_switch_indicator0_name
         type: string
         max: 5
         default_value: "FLAP"
-
       - name: osd_switch_indicator_one_name
         description: "Character to use for OSD switch incicator 1."
         field: osd_switch_indicator1_name
         type: string
         max: 5
         default_value: "GEAR"
-
       - name: osd_switch_indicator_two_name
         description: "Character to use for OSD switch incicator 2."
         field: osd_switch_indicator2_name
         type: string
         max: 5
         default_value: "CAM"
-
       - name: osd_switch_indicator_three_name
         description: "Character to use for OSD switch incicator 3."
         field: osd_switch_indicator3_name
         type: string
         max: 5
         default_value: "LIGT"
-
       - name: osd_switch_indicator_zero_channel
         description: "RC Channel to use for OSD switch indicator 0."
         field: osd_switch_indicator0_channel
         min: 5
         max: MAX_SUPPORTED_RC_CHANNEL_COUNT
         default_value: 5
-
       - name: osd_switch_indicator_one_channel
         description: "RC Channel to use for OSD switch indicator 1."
         field: osd_switch_indicator1_channel
         min: 5
         max: MAX_SUPPORTED_RC_CHANNEL_COUNT
         default_value: 5
-
       - name: osd_switch_indicator_two_channel
         description: "RC Channel to use for OSD switch indicator 2."
         field: osd_switch_indicator2_channel
         min: 5
         max: MAX_SUPPORTED_RC_CHANNEL_COUNT
         default_value: 5
-
       - name: osd_switch_indicator_three_channel
         description: "RC Channel to use for OSD switch indicator 3."
         field: osd_switch_indicator3_channel
         min: 5
         max: MAX_SUPPORTED_RC_CHANNEL_COUNT
         default_value: 5
-
       - name: osd_switch_indicators_align_left
         description: "Align text to left of switch indicators"
         field: osd_switch_indicators_align_left
         type: bool
         default_value: ON
-
       - name: osd_system_msg_display_time
         description: System message display cycle time for multiple messages (milliseconds).
         field: system_msg_display_time
         default_value: 1000
         min: 500
         max: 5000
-
+      - name: osd_highlight_djis_missing_font_symbols
+        description: Show question marks where there is no symbol in the DJI font to represent the INAV OSD element's symbol. When off, blank spaces will be used. Only relevent for DJICOMPAT modes.
+        field: highlight_djis_fuckup
+        default_value: ON
+        type: bool
   - name: PG_OSD_COMMON_CONFIG
     type: osdCommonConfig_t
     headers: ["io/osd_common.h"]
diff --git a/src/main/io/displayport_msp_dji_compat.c b/src/main/io/displayport_msp_dji_compat.c
index de4fa8656fa..e6c2d0b3c03 100644
--- a/src/main/io/displayport_msp_dji_compat.c
+++ b/src/main/io/displayport_msp_dji_compat.c
@@ -705,7 +705,7 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page)
             break;
     }
 
-    return '?'; // Missing/not mapped character
+    return (osdConfig()->highlight_djis_fuckup) ? '?' : SYM_BLANK; // Missing/not mapped character
 }
 
 #endif
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index f3f00ae7bdb..881a8b4e767 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -223,7 +223,7 @@ static bool osdDisplayHasCanvas;
 
 #define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees)
 
-PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 10);
+PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 11);
 PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 1);
 
 void osdStartedSaveProcess(void) {
@@ -3874,6 +3874,9 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
     .airspeed_alarm_min = SETTING_OSD_AIRSPEED_ALARM_MIN_DEFAULT,
     .airspeed_alarm_max = SETTING_OSD_AIRSPEED_ALARM_MAX_DEFAULT,
 #endif
+#ifndef DISABLE_MSP_DJI_COMPAT
+    .highlight_djis_fuckup = SETTING_OSD_HIGHLIGHT_DJIS_MISSING_FONT_SYMBOLS_DEFAULT,
+#endif
 
     .video_system = SETTING_OSD_VIDEO_SYSTEM_DEFAULT,
     .row_shiftdown = SETTING_OSD_ROW_SHIFTDOWN_DEFAULT,
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index ca5dca66137..ed60e26e4b8 100644
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -457,7 +457,10 @@ typedef struct osdConfig_s {
     bool            use_pilot_logo;                     // If enabled, the pilot logo (last 40 characters of page 2 font) will be used with the INAV logo.
     uint8_t         inav_to_pilot_logo_spacing;         // The space between the INAV and pilot logos, if pilot logo is used. This number may be adjusted so that it fits the odd/even col width.
     uint16_t        arm_screen_display_time;            // Length of time the arm screen is displayed
-    #ifdef USE_ADSB
+#ifndef DISABLE_MSP_DJI_COMPAT
+    bool            highlight_djis_fuckup;              // If enabled, show question marks where there is no character in DJI's font to represent an OSD element symbol
+#endif
+#ifdef USE_ADSB
     uint16_t adsb_distance_warning;                     // in metres
     uint16_t adsb_distance_alert;                       // in metres
     uint16_t adsb_ignore_plane_above_me_limit;          // in metres

From 135406decf4e63ef89fc4afc47885a54b7b5949d Mon Sep 17 00:00:00 2001
From: dzaro-dev <23706061+dzaro-dev@users.noreply.github.com>
Date: Sun, 19 May 2024 00:44:12 +0100
Subject: [PATCH 128/323] Update dji_osd_symbols.h as per request

---
 src/main/io/dji_osd_symbols.h | 40 +++++++++++++++++------------------
 1 file changed, 20 insertions(+), 20 deletions(-)

diff --git a/src/main/io/dji_osd_symbols.h b/src/main/io/dji_osd_symbols.h
index 3ef5a6c5e9b..2489bb176d9 100644
--- a/src/main/io/dji_osd_symbols.h
+++ b/src/main/io/dji_osd_symbols.h
@@ -71,26 +71,6 @@
 #define DJI_SYM_AH_DECORATION           0x13
 #define DJI_SYM_SMALL_CROSSHAIR         0x7E
 
-// Crosshair Styles
-#define DJI_SYM_AH_CH_TYPE3             0x00
-#define DJI_SYM_AH_CH_TYPE3_1           0x7E
-#define DJI_SYM_AH_CH_TYPE3_2           0x00
-#define DJI_SYM_AH_CH_TYPE4             0x2D
-#define DJI_SYM_AH_CH_TYPE4_1           0x7E
-#define DJI_SYM_AH_CH_TYPE4_2           0x2D
-#define DJI_SYM_AH_CH_TYPE5             0x17
-#define DJI_SYM_AH_CH_TYPE5_1           0x7E
-#define DJI_SYM_AH_CH_TYPE5_2           0x17
-#define DJI_SYM_AH_CH_TYPE6             0x00
-#define DJI_SYM_AH_CH_TYPE6_1           0x09
-#define DJI_SYM_AH_CH_TYPE6_2           0x00
-#define DJI_SYM_AH_CH_TYPE7             0x78
-#define DJI_SYM_AH_CH_TYPE7_1           0x7E
-#define DJI_SYM_AH_CH_TYPE7_2           0x77
-#define DJI_SYM_AH_CH_TYPE8             0x02
-#define DJI_SYM_AH_CH_TYPE8_1           0x7E
-#define DJI_SYM_AH_CH_TYPE8_2           0x03
-
 // Satellite Graphics
 #define DJI_SYM_SAT_L                   0x1E
 #define DJI_SYM_SAT_R                   0x1F
@@ -182,3 +162,23 @@
 #define DJI_SYM_GPS_DEGREE              DJI_SYM_STICK_OVERLAY_SPRITE_HIGH  // kind of looks like the degree symbol
 #define DJI_SYM_GPS_MINUTE              0x27 // '
 #define DJI_SYM_GPS_SECOND              0x22 // "
+
+// Crosshair Styles
+#define DJI_SYM_AH_CH_TYPE3             DJI_SYM_NONE
+#define DJI_SYM_AH_CH_TYPE3_1           DJI_SYM_SMALL_CROSSHAIR
+#define DJI_SYM_AH_CH_TYPE3_2           DJI_SYM_NONE
+#define DJI_SYM_AH_CH_TYPE4             DJI_SYM_HYPHEN
+#define DJI_SYM_AH_CH_TYPE4_1           DJI_SYM_SMALL_CROSSHAIR
+#define DJI_SYM_AH_CH_TYPE4_2           DJI_SYM_HYPHEN
+#define DJI_SYM_AH_CH_TYPE5             DJI_SYM_STICK_OVERLAY_HORIZONTAL
+#define DJI_SYM_AH_CH_TYPE5_1           DJI_SYM_SMALL_CROSSHAIR
+#define DJI_SYM_AH_CH_TYPE5_2           DJI_SYM_STICK_OVERLAY_HORIZONTAL
+#define DJI_SYM_AH_CH_TYPE6             DJI_SYM_NONE
+#define DJI_SYM_AH_CH_TYPE6_1           DJI_SYM_STICK_OVERLAY_SPRITE_MID
+#define DJI_SYM_AH_CH_TYPE6_2           DJI_SYM_NONE
+#define DJI_SYM_AH_CH_TYPE7             DJI_SYM_ARROW_SMALL_LEFT
+#define DJI_SYM_AH_CH_TYPE7_1           DJI_SYM_SMALL_CROSSHAIR
+#define DJI_SYM_AH_CH_TYPE7_2           DJI_SYM_ARROW_SMALL_RIGHT
+#define DJI_SYM_AH_CH_TYPE8             DJI_SYM_AH_LEFT
+#define DJI_SYM_AH_CH_TYPE8_1           DJI_SYM_SMALL_CROSSHAIR
+#define DJI_SYM_AH_CH_TYPE8_2           DJI_SYM_AH_RIGHT

From 655b325fcef1f550046a1cb4f643f62fbe48aeb3 Mon Sep 17 00:00:00 2001
From: Darren Lines <darren@darrenlines.uk>
Date: Sun, 19 May 2024 08:33:14 +0100
Subject: [PATCH 129/323] Tidy up DJI crosshairs

Re-ordered to keep the same crosshairs together.
---
 src/main/io/displayport_msp_dji_compat.c | 82 ++++++++++--------------
 src/main/io/dji_osd_symbols.h            | 20 ------
 2 files changed, 34 insertions(+), 68 deletions(-)

diff --git a/src/main/io/displayport_msp_dji_compat.c b/src/main/io/displayport_msp_dji_compat.c
index 93ec60b9ce8..ba6f7399c66 100644
--- a/src/main/io/displayport_msp_dji_compat.c
+++ b/src/main/io/displayport_msp_dji_compat.c
@@ -476,72 +476,58 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page)
         case SYM_AH_CH_LEFT:
         case SYM_AH_CH_AIRCRAFT1:
             return DJI_SYM_CROSSHAIR_LEFT;
-
-        case SYM_AH_CH_TYPE3:
-            return DJI_SYM_AH_CH_TYPE3;
-        
-        case SYM_AH_CH_TYPE4:
-            return DJI_SYM_AH_CH_TYPE4;
-        
-        case SYM_AH_CH_TYPE5:
-            return DJI_SYM_AH_CH_TYPE5;
-        
-        case SYM_AH_CH_TYPE6:
-            return DJI_SYM_AH_CH_TYPE6;
-        
-        case SYM_AH_CH_TYPE7:
-            return DJI_SYM_AH_CH_TYPE7;
-        
-        case SYM_AH_CH_TYPE8:
-            return DJI_SYM_AH_CH_TYPE8;
-
         case SYM_AH_CH_CENTER:
         case SYM_AH_CH_AIRCRAFT2:
             return DJI_SYM_CROSSHAIR_CENTRE;
-
-        case (SYM_AH_CH_TYPE3+1):
-            return DJI_SYM_AH_CH_TYPE3_1;
-        
-        case (SYM_AH_CH_TYPE4+1):
-            return DJI_SYM_AH_CH_TYPE4_1;
-        
-        case (SYM_AH_CH_TYPE5+1):
-            return DJI_SYM_AH_CH_TYPE5_1;
-        
-        case (SYM_AH_CH_TYPE6+1):
-            return DJI_SYM_AH_CH_TYPE6_1;
-        
-        case (SYM_AH_CH_TYPE7+1):
-            return DJI_SYM_AH_CH_TYPE7_1;
-        
-        case (SYM_AH_CH_TYPE8+1):
-            return DJI_SYM_AH_CH_TYPE8_1;
-
         case SYM_AH_CH_RIGHT:
         case SYM_AH_CH_AIRCRAFT3:
             return DJI_SYM_CROSSHAIR_RIGHT;
+        
+        case SYM_AH_CH_AIRCRAFT0:
+        case SYM_AH_CH_AIRCRAFT4:
+            return DJI_SYM_BLANK;
 
+        case SYM_AH_CH_TYPE3:
+            return DJI_SYM_NONE;
+        case (SYM_AH_CH_TYPE3+1):
+            return DJI_SYM_SMALL_CROSSHAIR;
         case (SYM_AH_CH_TYPE3+2):
-            return DJI_SYM_AH_CH_TYPE3_2;
+            return DJI_SYM_NONE;
         
+        case SYM_AH_CH_TYPE4:
+            return DJI_SYM_HYPHEN;
+        case (SYM_AH_CH_TYPE4+1):
+            return DJI_SYM_SMALL_CROSSHAIR;
         case (SYM_AH_CH_TYPE4+2):
-            return DJI_SYM_AH_CH_TYPE4_2;
+            return DJI_SYM_HYPHEN;
         
+        case SYM_AH_CH_TYPE5:
+            return DJI_SYM_STICK_OVERLAY_HORIZONTAL;
+        case (SYM_AH_CH_TYPE5+1):
+            return DJI_SYM_SMALL_CROSSHAIR;
         case (SYM_AH_CH_TYPE5+2):
-            return DJI_SYM_AH_CH_TYPE5_2;
+            return DJI_SYM_STICK_OVERLAY_HORIZONTAL;
         
+        case SYM_AH_CH_TYPE6:
+            return DJI_SYM_NONE;
+        case (SYM_AH_CH_TYPE6+1):
+            return DJI_SYM_STICK_OVERLAY_SPRITE_MID;
         case (SYM_AH_CH_TYPE6+2):
-            return DJI_SYM_AH_CH_TYPE6_2;
+            return DJI_SYM_NONE;
         
+        case SYM_AH_CH_TYPE7:
+            return DJI_SYM_ARROW_SMALL_LEFT;
+        case (SYM_AH_CH_TYPE7+1):
+            return DJI_SYM_SMALL_CROSSHAIR;
         case (SYM_AH_CH_TYPE7+2):
-            return DJI_SYM_AH_CH_TYPE7_2;
+            return DJI_SYM_ARROW_SMALL_RIGHT;
         
+        case SYM_AH_CH_TYPE8:
+            return DJI_SYM_AH_LEFT;
+        case (SYM_AH_CH_TYPE8+1):
+            return DJI_SYM_SMALL_CROSSHAIR;
         case (SYM_AH_CH_TYPE8+2):
-            return DJI_SYM_AH_CH_TYPE8_2;
-        
-        case SYM_AH_CH_AIRCRAFT0:
-        case SYM_AH_CH_AIRCRAFT4:
-            return DJI_SYM_BLANK;
+            return DJI_SYM_AH_RIGHT;
 
         case SYM_ARROW_UP:
             return DJI_SYM_ARROW_NORTH;
diff --git a/src/main/io/dji_osd_symbols.h b/src/main/io/dji_osd_symbols.h
index 2489bb176d9..66972a810e7 100644
--- a/src/main/io/dji_osd_symbols.h
+++ b/src/main/io/dji_osd_symbols.h
@@ -162,23 +162,3 @@
 #define DJI_SYM_GPS_DEGREE              DJI_SYM_STICK_OVERLAY_SPRITE_HIGH  // kind of looks like the degree symbol
 #define DJI_SYM_GPS_MINUTE              0x27 // '
 #define DJI_SYM_GPS_SECOND              0x22 // "
-
-// Crosshair Styles
-#define DJI_SYM_AH_CH_TYPE3             DJI_SYM_NONE
-#define DJI_SYM_AH_CH_TYPE3_1           DJI_SYM_SMALL_CROSSHAIR
-#define DJI_SYM_AH_CH_TYPE3_2           DJI_SYM_NONE
-#define DJI_SYM_AH_CH_TYPE4             DJI_SYM_HYPHEN
-#define DJI_SYM_AH_CH_TYPE4_1           DJI_SYM_SMALL_CROSSHAIR
-#define DJI_SYM_AH_CH_TYPE4_2           DJI_SYM_HYPHEN
-#define DJI_SYM_AH_CH_TYPE5             DJI_SYM_STICK_OVERLAY_HORIZONTAL
-#define DJI_SYM_AH_CH_TYPE5_1           DJI_SYM_SMALL_CROSSHAIR
-#define DJI_SYM_AH_CH_TYPE5_2           DJI_SYM_STICK_OVERLAY_HORIZONTAL
-#define DJI_SYM_AH_CH_TYPE6             DJI_SYM_NONE
-#define DJI_SYM_AH_CH_TYPE6_1           DJI_SYM_STICK_OVERLAY_SPRITE_MID
-#define DJI_SYM_AH_CH_TYPE6_2           DJI_SYM_NONE
-#define DJI_SYM_AH_CH_TYPE7             DJI_SYM_ARROW_SMALL_LEFT
-#define DJI_SYM_AH_CH_TYPE7_1           DJI_SYM_SMALL_CROSSHAIR
-#define DJI_SYM_AH_CH_TYPE7_2           DJI_SYM_ARROW_SMALL_RIGHT
-#define DJI_SYM_AH_CH_TYPE8             DJI_SYM_AH_LEFT
-#define DJI_SYM_AH_CH_TYPE8_1           DJI_SYM_SMALL_CROSSHAIR
-#define DJI_SYM_AH_CH_TYPE8_2           DJI_SYM_AH_RIGHT

From 508b3ddc904b9c1ddf9194fd4d51e4eaf3b063e9 Mon Sep 17 00:00:00 2001
From: Darren Lines <darren@darrenlines.uk>
Date: Sun, 19 May 2024 09:07:16 +0100
Subject: [PATCH 130/323] Remove unused define from DJI OSD symbols

I searched the code and DJI_SYM_CURSOR isn't used anywhere. The only OSD use of cursor at all is in the CMS, which just uses the '>' character.

So this define can go. After that the dji_osd_symbols.h file is a pure character to variable map.
---
 src/main/io/dji_osd_symbols.h | 3 ---
 1 file changed, 3 deletions(-)

diff --git a/src/main/io/dji_osd_symbols.h b/src/main/io/dji_osd_symbols.h
index 2489bb176d9..35a034c7f54 100644
--- a/src/main/io/dji_osd_symbols.h
+++ b/src/main/io/dji_osd_symbols.h
@@ -147,9 +147,6 @@
 #define DJI_SYM_MPS                     0x9F
 #define DJI_SYM_FTPS                    0x99
 
-// Menu cursor
-#define DJI_SYM_CURSOR                  DJI_SYM_AH_LEFT
-
 // Stick overlays
 #define DJI_SYM_STICK_OVERLAY_SPRITE_HIGH 0x08
 #define DJI_SYM_STICK_OVERLAY_SPRITE_MID  0x09

From 536c5c3958f8623d02cf3c46ae9956b005a381b6 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 19 May 2024 20:24:18 +0200
Subject: [PATCH 131/323] Make Iterm Lock configurable

---
 src/main/fc/settings.yaml | 12 ++++++++++++
 src/main/flight/pid.c     |  9 ++++++---
 src/main/flight/pid.h     |  5 +++++
 3 files changed, 23 insertions(+), 3 deletions(-)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 307f6d9cb53..3abe94e12c9 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -2227,6 +2227,18 @@ groups:
         field: fixedWingLevelTrimGain
         min: 0
         max: 20
+      - name: fw_iterm_lock_time_max_ms
+        description: Defines max time in milliseconds for how long ITerm Lock will shut down Iterm after sticks are release
+        default_value: 500
+        field: fwItermLockTimeMaxMs
+        min: 100
+        max: 1000
+      - name: fw_iterm_lock_rate_threshold
+        description: Defines rate percentage when full P I and D attenuation should happen. 100 disables Iterm Lock for P and D term
+        field: fwItermLockRateLimit
+        default_value: 40
+        min: 10
+        max: 100
 
   - name: PG_PID_AUTOTUNE_CONFIG
     type: pidAutotuneConfig_t
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 479d8e8d346..711b3a0af07 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -178,7 +178,7 @@ static EXTENDED_FASTRAM bool angleHoldIsLevel = false;
 static EXTENDED_FASTRAM float fixedWingLevelTrim;
 static EXTENDED_FASTRAM pidController_t fixedWingLevelTrimController;
 
-PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 8);
+PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 9);
 
 PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .bank_mc = {
@@ -312,6 +312,8 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .smithPredictorDelay = SETTING_SMITH_PREDICTOR_DELAY_DEFAULT,
         .smithPredictorFilterHz = SETTING_SMITH_PREDICTOR_LPF_HZ_DEFAULT,
 #endif
+        .fwItermLockTimeMaxMs = SETTING_FW_ITERM_LOCK_TIME_MAX_MS_DEFAULT,
+        .fwItermLockRateLimit = SETTING_FW_ITERM_LOCK_RATE_THRESHOLD_DEFAULT,
 );
 
 bool pidInitFilters(void)
@@ -748,7 +750,8 @@ static void nullRateController(pidState_t *pidState, float dT, float dT_inv) {
 
 static void fwRateAttenuation(pidState_t *pidState, const float rateTarget, const float rateError) {
     const float maxRate = currentControlRateProfile->stabilized.rates[pidState->axis] * 10.0f;
-    const float dampingFactor = attenuation(rateTarget, maxRate / 2.5f);
+
+    const float dampingFactor = attenuation(rateTarget, maxRate * pidProfile()->fwItermLockRateLimit / 100.0f);
 
     /*
      * Iterm damping is applied (down to 0) when:
@@ -770,7 +773,7 @@ static void fwRateAttenuation(pidState_t *pidState, const float rateTarget, cons
         pidState->attenuation.targetOverThresholdTimeMs = 0;
     }
 
-    pidState->attenuation.aI = MIN(dampingFactor, (errorThresholdReached && (millis() - pidState->attenuation.targetOverThresholdTimeMs) < 500) ? 0.0f : 1.0f);
+    pidState->attenuation.aI = MIN(dampingFactor, (errorThresholdReached && (millis() - pidState->attenuation.targetOverThresholdTimeMs) < pidProfile()->fwItermLockTimeMaxMs) ? 0.0f : 1.0f);
 
     //P & D damping factors are always the same and based on current damping factor
     pidState->attenuation.aP = dampingFactor;
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index c8df7231ed6..93e713b61a9 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -155,6 +155,11 @@ typedef struct pidProfile_s {
     float smithPredictorDelay;
     uint16_t smithPredictorFilterHz;
 #endif
+
+
+    uint16_t fwItermLockTimeMaxMs;
+    uint8_t fwItermLockRateLimit;
+
 } pidProfile_t;
 
 typedef struct pidAutotuneConfig_s {

From 2118cebf255d2cce359863636d68f19ad6538c0e Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 19 May 2024 20:24:41 +0200
Subject: [PATCH 132/323] Iterm Lock configurability

---
 src/main/fc/settings.yaml | 6 ++++++
 src/main/flight/pid.c     | 3 ++-
 src/main/flight/pid.h     | 1 +
 3 files changed, 9 insertions(+), 1 deletion(-)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 3abe94e12c9..a2d2195bde0 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -2239,6 +2239,12 @@ groups:
         default_value: 40
         min: 10
         max: 100
+      - name: fw_iterm_lock_engage_threshold
+        description: Defines error rate (in percents of max rate) when Iterm Lock is engaged when sticks are release. Iterm Lock will stay active until error drops below this number
+        default_value: 10
+        min: 5
+        max: 25
+        field: fwItermLockEngageThreshold
 
   - name: PG_PID_AUTOTUNE_CONFIG
     type: pidAutotuneConfig_t
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 711b3a0af07..a9d1291877f 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -314,6 +314,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
 #endif
         .fwItermLockTimeMaxMs = SETTING_FW_ITERM_LOCK_TIME_MAX_MS_DEFAULT,
         .fwItermLockRateLimit = SETTING_FW_ITERM_LOCK_RATE_THRESHOLD_DEFAULT,
+        .fwItermLockEngageThreshold = SETTING_FW_ITERM_LOCK_ENGAGE_THRESHOLD_DEFAULT,
 );
 
 bool pidInitFilters(void)
@@ -761,7 +762,7 @@ static void fwRateAttenuation(pidState_t *pidState, const float rateTarget, cons
      */
 
     //If error is greater than 10% or max rate
-    const bool errorThresholdReached = fabsf(rateError) > maxRate * 0.1f;
+    const bool errorThresholdReached = fabsf(rateError) > maxRate * pidProfile()->fwItermLockEngageThreshold / 100.0f;
 
     //If stick (setpoint) was moved above threshold in the last 500ms
     if (fabsf(rateTarget) > maxRate * 0.2f) {
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index 93e713b61a9..26aeb86990d 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -159,6 +159,7 @@ typedef struct pidProfile_s {
 
     uint16_t fwItermLockTimeMaxMs;
     uint8_t fwItermLockRateLimit;
+    uint8_t fwItermLockEngageThreshold;
 
 } pidProfile_t;
 

From fc9457f4708b616594de728681ae072bc03bcda6 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 19 May 2024 20:25:52 +0200
Subject: [PATCH 133/323] Docs update

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

diff --git a/docs/Settings.md b/docs/Settings.md
index 9f3baaed662..96141fa65d2 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -1302,6 +1302,36 @@ Fixed-wing rate stabilisation I-gain for YAW
 
 ---
 
+### fw_iterm_lock_engage_threshold
+
+Defines error rate (in percents of max rate) when Iterm Lock is engaged when sticks are release. Iterm Lock will stay active until error drops below this number
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 10 | 5 | 25 |
+
+---
+
+### fw_iterm_lock_rate_threshold
+
+Defines rate percentage when full P I and D attenuation should happen. 100 disables Iterm Lock for P and D term
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 40 | 10 | 100 |
+
+---
+
+### fw_iterm_lock_time_max_ms
+
+Defines max time in milliseconds for how long ITerm Lock will shut down Iterm after sticks are release
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 500 | 100 | 1000 |
+
+---
+
 ### fw_level_pitch_gain
 
 I-gain for the pitch trim for self-leveling flight modes. Higher values means that AUTOTRIM will be faster but might introduce oscillations

From 5a2b1ee9a7a21090c2586fe9d8b8088ee98760a4 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Tue, 21 May 2024 11:39:23 +0100
Subject: [PATCH 134/323] move activewp and rxupdaterate fields

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

diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index 727488cc1b1..4433e7b4aba 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -402,13 +402,15 @@ static const blackboxSimpleFieldDefinition_t blackboxGpsHFields[] = {
 
 // Rarely-updated fields
 static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = {
+    {"activeWpNumber",        -1, UNSIGNED, PREDICT(0),      ENCODING(UNSIGNED_VB)},    
     {"flightModeFlags",       -1, UNSIGNED, PREDICT(0),      ENCODING(UNSIGNED_VB)},
     {"stateFlags",            -1, UNSIGNED, PREDICT(0),      ENCODING(UNSIGNED_VB)},
 
     {"failsafePhase",         -1, UNSIGNED, PREDICT(0),      ENCODING(TAG2_3S32)},
     {"rxSignalReceived",      -1, UNSIGNED, PREDICT(0),      ENCODING(TAG2_3S32)},
     {"rxFlightChannelsValid", -1, UNSIGNED, PREDICT(0),      ENCODING(TAG2_3S32)},
-
+    {"rxUpdateRate",          -1, UNSIGNED, PREDICT(PREVIOUS),      ENCODING(UNSIGNED_VB)},
+    
     {"hwHealthStatus",        -1, UNSIGNED, PREDICT(0),      ENCODING(UNSIGNED_VB)},
     {"powerSupplyImpedance",  -1, UNSIGNED, PREDICT(0),      ENCODING(UNSIGNED_VB)},
     {"sagCompensatedVBat",    -1, UNSIGNED, PREDICT(0),      ENCODING(UNSIGNED_VB)},
@@ -436,8 +438,6 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = {
     {"escRPM",                -1, UNSIGNED, PREDICT(0),             ENCODING(UNSIGNED_VB)},
     {"escTemperature",        -1, SIGNED,   PREDICT(PREVIOUS),      ENCODING(SIGNED_VB)},
 #endif
-    {"rxUpdateRate",          -1, UNSIGNED, PREDICT(PREVIOUS),      ENCODING(UNSIGNED_VB)},
-    {"activeWpNumber",        -1, UNSIGNED, PREDICT(0),      ENCODING(UNSIGNED_VB)},
 };
 
 typedef enum BlackboxState {
@@ -1259,7 +1259,8 @@ static void writeSlowFrame(void)
     int32_t values[3];
 
     blackboxWrite('S');
-
+    
+    blackboxWriteUnsignedVB(slowHistory.activeWpNumber);
     blackboxWriteUnsignedVB(slowHistory.flightModeFlags);
     blackboxWriteUnsignedVB(slowHistory.stateFlags);
 
@@ -1268,9 +1269,11 @@ static void writeSlowFrame(void)
      */
     values[0] = slowHistory.failsafePhase;
     values[1] = slowHistory.rxSignalReceived ? 1 : 0;
-    values[2] = slowHistory.rxFlightChannelsValid ? 1 : 0;
+    values[2] = slowHistory.rxFlightChannelsValid ? 1 : 0;    
     blackboxWriteTag2_3S32(values);
 
+    blackboxWriteUnsignedVB(slowHistory.rxUpdateRate);    
+
     blackboxWriteUnsignedVB(slowHistory.hwHealthStatus);
 
     blackboxWriteUnsignedVB(slowHistory.powerSupplyImpedance);
@@ -1296,8 +1299,6 @@ static void writeSlowFrame(void)
     blackboxWriteUnsignedVB(slowHistory.escRPM);
     blackboxWriteSignedVB(slowHistory.escTemperature);
 #endif
-    blackboxWriteUnsignedVB(slowHistory.rxUpdateRate);
-    blackboxWriteUnsignedVB(slowHistory.activeWpNumber);
 
     blackboxSlowFrameIterationTimer = 0;
 }
@@ -1307,6 +1308,7 @@ static void writeSlowFrame(void)
  */
 static void loadSlowState(blackboxSlowState_t *slow)
 {
+    slow->activeWpNumber = getActiveWpNumber();
     memcpy(&slow->flightModeFlags, &rcModeActivationMask, sizeof(slow->flightModeFlags)); //was flightModeFlags;
     // Also log Nav auto enabled flight modes rather than just those selected by boxmode
     if (FLIGHT_MODE(ANGLE_MODE)) {
@@ -1328,6 +1330,7 @@ static void loadSlowState(blackboxSlowState_t *slow)
     slow->failsafePhase = failsafePhase();
     slow->rxSignalReceived = rxIsReceivingSignal();
     slow->rxFlightChannelsValid = rxAreFlightChannelsValid();
+    slow->rxUpdateRate = getRcUpdateFrequency();    
     slow->hwHealthStatus = (getHwGyroStatus()           << 2 * 0) |     // Pack hardware health status into a bit field.
                            (getHwAccelerometerStatus()  << 2 * 1) |     // Use raw hardwareSensorStatus_e values and pack them using 2 bits per value
                            (getHwCompassStatus()        << 2 * 2) |     // Report GYRO in 2 lowest bits, then ACC, COMPASS, BARO, GPS, RANGEFINDER and PITOT
@@ -1379,9 +1382,6 @@ static void loadSlowState(blackboxSlowState_t *slow)
     slow->escRPM = escSensor->rpm;
     slow->escTemperature = escSensor->temperature;
 #endif
-
-    slow->rxUpdateRate = getRcUpdateFrequency();
-    slow->activeWpNumber = getActiveWpNumber();
 }
 
 /**

From 946241d995ca8b7c8c762330a31b9d2ed9f1d1be Mon Sep 17 00:00:00 2001
From: Darren Lines <darren@darrenlines.uk>
Date: Tue, 21 May 2024 21:41:30 +0100
Subject: [PATCH 135/323] Made it more boring

---
 src/main/fc/settings.yaml                | 2 +-
 src/main/io/displayport_msp_dji_compat.c | 2 +-
 src/main/io/osd.c                        | 2 +-
 src/main/io/osd.h                        | 2 +-
 4 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index d276aca274d..fa21316463e 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -3681,7 +3681,7 @@ groups:
         max: 5000
       - name: osd_highlight_djis_missing_font_symbols
         description: Show question marks where there is no symbol in the DJI font to represent the INAV OSD element's symbol. When off, blank spaces will be used. Only relevent for DJICOMPAT modes.
-        field: highlight_djis_fuckup
+        field: highlight_djis_missing_characters
         default_value: ON
         type: bool
   - name: PG_OSD_COMMON_CONFIG
diff --git a/src/main/io/displayport_msp_dji_compat.c b/src/main/io/displayport_msp_dji_compat.c
index e6c2d0b3c03..cf57b49be67 100644
--- a/src/main/io/displayport_msp_dji_compat.c
+++ b/src/main/io/displayport_msp_dji_compat.c
@@ -705,7 +705,7 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page)
             break;
     }
 
-    return (osdConfig()->highlight_djis_fuckup) ? '?' : SYM_BLANK; // Missing/not mapped character
+    return (osdConfig()->highlight_djis_missing_characters) ? '?' : SYM_BLANK; // Missing/not mapped character
 }
 
 #endif
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 881a8b4e767..34135ce2fd7 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -3875,7 +3875,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
     .airspeed_alarm_max = SETTING_OSD_AIRSPEED_ALARM_MAX_DEFAULT,
 #endif
 #ifndef DISABLE_MSP_DJI_COMPAT
-    .highlight_djis_fuckup = SETTING_OSD_HIGHLIGHT_DJIS_MISSING_FONT_SYMBOLS_DEFAULT,
+    .highlight_djis_missing_characters = SETTING_OSD_HIGHLIGHT_DJIS_MISSING_FONT_SYMBOLS_DEFAULT,
 #endif
 
     .video_system = SETTING_OSD_VIDEO_SYSTEM_DEFAULT,
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index ed60e26e4b8..7ac14054994 100644
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -458,7 +458,7 @@ typedef struct osdConfig_s {
     uint8_t         inav_to_pilot_logo_spacing;         // The space between the INAV and pilot logos, if pilot logo is used. This number may be adjusted so that it fits the odd/even col width.
     uint16_t        arm_screen_display_time;            // Length of time the arm screen is displayed
 #ifndef DISABLE_MSP_DJI_COMPAT
-    bool            highlight_djis_fuckup;              // If enabled, show question marks where there is no character in DJI's font to represent an OSD element symbol
+    bool            highlight_djis_missing_characters;  // If enabled, show question marks where there is no character in DJI's font to represent an OSD element symbol
 #endif
 #ifdef USE_ADSB
     uint16_t adsb_distance_warning;                     // in metres

From b4dfc9302a84b31f03e79ed179c8754aab99fb76 Mon Sep 17 00:00:00 2001
From: HishamGhosheh <hisham.ghosheh@gmail.com>
Date: Thu, 23 May 2024 01:57:25 +0200
Subject: [PATCH 136/323] Add JHEF405PRO (aka GHF405AIO-HD) support

---
 src/main/target/JHEF405PRO/CMakeLists.txt |   1 +
 src/main/target/JHEF405PRO/target.c       |  34 +++++
 src/main/target/JHEF405PRO/target.h       | 154 ++++++++++++++++++++++
 3 files changed, 189 insertions(+)
 create mode 100644 src/main/target/JHEF405PRO/CMakeLists.txt
 create mode 100644 src/main/target/JHEF405PRO/target.c
 create mode 100644 src/main/target/JHEF405PRO/target.h

diff --git a/src/main/target/JHEF405PRO/CMakeLists.txt b/src/main/target/JHEF405PRO/CMakeLists.txt
new file mode 100644
index 00000000000..eb7f0108832
--- /dev/null
+++ b/src/main/target/JHEF405PRO/CMakeLists.txt
@@ -0,0 +1 @@
+target_stm32f405xg(JHEF405PRO)
diff --git a/src/main/target/JHEF405PRO/target.c b/src/main/target/JHEF405PRO/target.c
new file mode 100644
index 00000000000..23c1604c0ca
--- /dev/null
+++ b/src/main/target/JHEF405PRO/target.c
@@ -0,0 +1,34 @@
+/*
+ * 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 <platform.h>
+#include "drivers/io.h"
+#include "drivers/pwm_mapping.h"
+#include "drivers/timer.h"
+
+timerHardware_t timerHardware[] = {    
+    DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0),  // S1
+    DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0),  // S2
+    DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, 0, 1),  // S3
+    DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, 0, 0),  // S4
+
+    DEF_TIM(TIM4, CH3, PB8, TIM_USE_PPM, 0, 0),       // PPM
+    DEF_TIM(TIM1, CH2, PA9, TIM_USE_LED,  0, 1 ),     //LED
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
diff --git a/src/main/target/JHEF405PRO/target.h b/src/main/target/JHEF405PRO/target.h
new file mode 100644
index 00000000000..43547cd2efa
--- /dev/null
+++ b/src/main/target/JHEF405PRO/target.h
@@ -0,0 +1,154 @@
+/*
+ * This 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.
+ *
+ * This software 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 software.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#define TARGET_BOARD_IDENTIFIER "JHEF405PRO"
+#define USBD_PRODUCT_STRING  "JHEF405PRO"
+
+#define LED0                PC14
+#define BEEPER              PC13
+#define BEEPER_INVERTED
+
+// *************** SPI1 Gyro & ACC **********************
+#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_IMU_ICM42605
+#define IMU_ICM42605_ALIGN      CW90_DEG
+#define ICM42605_CS_PIN         PB12
+#define ICM42605_SPI_BUS        BUS_SPI1
+
+#define USE_EXTI
+#define GYRO_INT_EXTI       PB13
+#define USE_MPU_DATA_READY_SIGNAL
+
+// *************** SPI3 OSD & Flash ***************************
+#define USE_SPI_DEVICE_3
+#define SPI3_SCK_PIN        PC10
+#define SPI3_MISO_PIN       PC11
+#define SPI3_MOSI_PIN       PC12
+
+// OSD
+#define USE_OSD
+#define USE_MAX7456
+#define MAX7456_SPI_BUS     BUS_SPI3
+#define MAX7456_CS_PIN      PB14
+
+// Flash
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+#define USE_FLASHFS
+#define USE_FLASH_M25P16
+#define M25P16_CS_PIN           PB3
+#define M25P16_SPI_BUS          BUS_SPI3
+
+// *************** I2C /Baro/Mag *********************
+#define USE_I2C
+#define USE_I2C_DEVICE_1
+#define I2C1_SCL                PB8
+#define I2C1_SDA                PB9
+#define DEFAULT_I2C_BUS         BUS_I2C1
+
+// Baro
+#define USE_BARO
+#define BARO_I2C_BUS            DEFAULT_I2C_BUS
+#define USE_BARO_DPS310
+
+// Mag
+#define USE_MAG
+#define USE_MAG_ALL
+#define MAG_I2C_BUS             DEFAULT_I2C_BUS
+
+// Temperature
+#define TEMPERATURE_I2C_BUS     DEFAULT_I2C_BUS
+
+// Range finder
+#define USE_RANGEFINDER
+#define USE_RANGEFINDER_HCSR04_I2C
+#define RANGEFINDER_I2C_BUS     DEFAULT_I2C_BUS
+
+// PITOT
+#define PITOT_I2C_BUS           DEFAULT_I2C_BUS
+
+// *************** UART *****************************
+#define USB_IO
+#define USE_VCP
+#define VBUS_SENSING_PIN        PA8 // TODO validate
+#define VBUS_SENSING_ENABLED
+
+#define USE_UART1
+#define UART1_RX_PIN            PA10
+#define UART1_TX_PIN            PB6
+
+#define USE_UART2
+#define UART2_RX_PIN            PD6
+#define UART2_TX_PIN            PD5
+
+#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
+
+// *************** ADC ***************************
+#define USE_ADC
+#define ADC_INSTANCE                ADC1
+#define ADC1_DMA_STREAM             DMA2_Stream0
+#define ADC_CHANNEL_1_PIN           PC3
+#define ADC_CHANNEL_2_PIN           PC2
+#define ADC_CHANNEL_3_PIN           PC0
+#define VBAT_ADC_CHANNEL            ADC_CHN_1
+#define CURRENT_METER_ADC_CHANNEL   ADC_CHN_2
+#define RSSI_ADC_CHANNEL            ADC_CHN_3
+#define CURRENT_METER_SCALE         250
+
+// ***************  OTHERS *************************
+#define DEFAULT_FEATURES        (FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_OSD | FEATURE_TELEMETRY)
+
+// Rx defaults
+#define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER       SERIALRX_CRSF
+#define SERIALRX_UART           SERIAL_PORT_USART2
+
+
+// *************** LED2812 ************************
+#define USE_LED_STRIP
+#define WS2811_PIN              PA9
+
+
+
+#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
+
+//TIMER
+#define MAX_PWM_OUTPUT_PORTS       4
+
+#define USE_DSHOT

From b7a24063af29560cc5a95cfdbbdded5ea3d638f4 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Thu, 23 May 2024 09:43:41 +0200
Subject: [PATCH 137/323] Add PSA about ICM426xx changes in INAV 7.1

Some users have reported issues with INAV 7.1+ when upgrading from previews versions in aircraft that had this gyro.

Recalibrating the gyro, or redoing the setup in INAV 7.1 seems to have fixed most of them.
---
 readme.md | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/readme.md b/readme.md
index a70c42d4c86..a83ba6c6cfd 100644
--- a/readme.md
+++ b/readme.md
@@ -6,6 +6,10 @@
 
 > INAV 7 is the last INAV official release available for F411 based flight controllers. The next milestone, INAV 8 will not be available for F411 boards.
 
+# ICM426xx IMUs PSA
+
+> The filtering settings for the ICM426xx has changed to matche what is used by Ardupilot and Betaflight in INAV 7.1. When upgrading from older versions you may need to recalibrate the Accelerometer and if you are not using INAV's default tune you may also want to check if the tune is still good.
+
 ![INAV](http://static.rcgroups.net/forums/attachments/6/1/0/3/7/6/a9088858-102-inav.png)
 
 # PosHold, Navigation and RTH without compass PSA

From e37ed9f270ba1729463e4d01f29923584d70c9fd Mon Sep 17 00:00:00 2001
From: HishamGhosheh <hisham.ghosheh@gmail.com>
Date: Thu, 23 May 2024 10:27:55 +0200
Subject: [PATCH 138/323] Add SKIP_RELEASES to JHEF405PRO

---
 src/main/target/JHEF405PRO/CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/target/JHEF405PRO/CMakeLists.txt b/src/main/target/JHEF405PRO/CMakeLists.txt
index eb7f0108832..939221cf744 100644
--- a/src/main/target/JHEF405PRO/CMakeLists.txt
+++ b/src/main/target/JHEF405PRO/CMakeLists.txt
@@ -1 +1 @@
-target_stm32f405xg(JHEF405PRO)
+target_stm32f405xg(JHEF405PRO SKIP_RELEASES)

From 39bd781552867e2e22a176cac222498270bfed2d Mon Sep 17 00:00:00 2001
From: Scavanger <andreas.kanzler@outlook.com>
Date: Thu, 23 May 2024 10:51:41 -0300
Subject: [PATCH 139/323] USB-Rescue

---
 cmake/at32.cmake            |  5 ++++
 cmake/sitl.cmake            |  4 +++
 cmake/stm32.cmake           |  5 ++++
 docs/Broken USB recovery.md | 51 +++++--------------------------------
 src/main/fc/config.c        |  8 ++++++
 5 files changed, 28 insertions(+), 45 deletions(-)

diff --git a/cmake/at32.cmake b/cmake/at32.cmake
index 54e178deb7b..6ac70bc528e 100644
--- a/cmake/at32.cmake
+++ b/cmake/at32.cmake
@@ -326,6 +326,11 @@ function(target_at32)
 
     math(EXPR hse_value "${hse_mhz} * 1000000")
     list(APPEND target_definitions "HSE_VALUE=${hse_value}")
+
+    if (MSP_UART) 
+        list(APPEND target_definitions "MSP_UART=${MSP_UART}")
+    endif()
+
     if(args_COMPILE_DEFINITIONS)
         list(APPEND target_definitions ${args_COMPILE_DEFINITIONS})
     endif()
diff --git a/cmake/sitl.cmake b/cmake/sitl.cmake
index ee43aa9a93a..39e6456830a 100644
--- a/cmake/sitl.cmake
+++ b/cmake/sitl.cmake
@@ -99,6 +99,10 @@ function (target_sitl name)
     math(EXPR hse_value "${hse_mhz} * 1000000")
     list(APPEND target_definitions "HSE_VALUE=${hse_value}")
 
+    if (MSP_UART) 
+        list(APPEND target_definitions "MSP_UART=${MSP_UART}")
+    endif()
+
     string(TOLOWER ${PROJECT_NAME} lowercase_project_name)
     set(binary_name ${lowercase_project_name}_${FIRMWARE_VERSION}_${name})
     if(DEFINED BUILD_SUFFIX AND NOT "" STREQUAL "${BUILD_SUFFIX}")
diff --git a/cmake/stm32.cmake b/cmake/stm32.cmake
index f02185e9aff..c4534a30500 100644
--- a/cmake/stm32.cmake
+++ b/cmake/stm32.cmake
@@ -333,6 +333,11 @@ function(target_stm32)
 
     math(EXPR hse_value "${hse_mhz} * 1000000")
     list(APPEND target_definitions "HSE_VALUE=${hse_value}")
+
+    if (MSP_UART) 
+        list(APPEND target_definitions "MSP_UART=${MSP_UART}")
+    endif()
+
     if(args_COMPILE_DEFINITIONS)
         list(APPEND target_definitions ${args_COMPILE_DEFINITIONS})
     endif()
diff --git a/docs/Broken USB recovery.md b/docs/Broken USB recovery.md
index 46db627c83e..3406653b981 100644
--- a/docs/Broken USB recovery.md	
+++ b/docs/Broken USB recovery.md	
@@ -53,55 +53,16 @@ The following procedure describes the process under Windows 10/11:
 Please read [Building in Windows 2010 or 11 with Linux Subsystem](https://github.com/iNavFlight/inav/blob/master/docs/development/Building%20in%20Windows%2010%20or%2011%20with%20Linux%20Subsystem.md)
 and follow the instructions up to "Building with Make".
 
-To activate MSP by default, go to the directory `src/main/target/[your target]`.
-If no config.c exists, create a new text file with this name and the following content:
-
-```
-/*
- * 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 "io/serial.h"
-
-void targetConfiguration(void)
-{
-    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USARTX)].functionMask = FUNCTION_MSP;
-    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USARTX)].msp_baudrateIndex = BAUD_115200;
-}
-
-```
-
-If the file already exists, add the following lines in the function `void targetConfiguration(void)` (before the last `}`)
-
-```
-serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USARTX)].functionMask = FUNCTION_MSP;
-serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USARTX)].msp_baudrateIndex = BAUD_115200;
-```
-
-Replace the X in SERIAL_PORT_USARTX (in both lines) with the number of UART/serial port on which MSP is to be activated.
+In the step 'prepare build environment' add the option `-DMSP_UART=SERIAL_PORT_USARTX` to `cmake`
+
+Replace the X in SERIAL_PORT_USARTX with the number of UART/serial port on which MSP is to be activated.
 
 Example:
-For UART 2: `SERIAL_PORT_USART2`
-For UART 3: `SERIAL_PORT_USART3`
+For UART 2: `cmake -DMSP_UART=SERIAL_PORT_USART2 ..`
+For UART 3: `cmake -DMSP_UART=SERIAL_PORT_USART3 ..`
 etc.
 
-Save the file and build the firmware as described in the document above.
+Build the firmware as described in the document above (`make [YOUR_TARGET]`).
 
 ## Flashing via Uart:
 
diff --git a/src/main/fc/config.c b/src/main/fc/config.c
index c9a50c60a25..e5f164988df 100755
--- a/src/main/fc/config.c
+++ b/src/main/fc/config.c
@@ -284,6 +284,14 @@ void createDefaultConfig(void)
     featureSet(FEATURE_AIRMODE);
 
     targetConfiguration();
+
+#ifdef MSP_UART
+    int port = findSerialPortIndexByIdentifier(MSP_UART);
+    if (port) {
+        serialConfigMutable()->portConfigs[port].functionMask = FUNCTION_MSP;
+        serialConfigMutable()->portConfigs[port].msp_baudrateIndex = BAUD_115200;
+    }
+#endif
 }
 
 void resetConfigs(void)

From a974ccc85b6f3f79a67f58149c6b960b603e15eb Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Fri, 24 May 2024 10:01:08 -0500
Subject: [PATCH 140/323] revert buggy inav_use_gps_no_baro to default off

---
 src/main/fc/settings.yaml | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index d2d8c5c433e..de413d456af 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -2288,10 +2288,10 @@ groups:
         field: use_gps_velned
         type: bool
       - name: inav_use_gps_no_baro
-        description: "Defines if INAV should use only use GPS data for altitude estimation when barometer is not available. If set to ON, INAV will allow GPS assisted modes and RTH even when there is no barometer installed."
+        description: "Defines if INAV should use only use GPS data for altitude estimation and not barometer. If set to ON, INAV will allow GPS assisted modes and RTH even when there is no barometer installed."
         field: use_gps_no_baro
         type: bool
-        default_value: ON
+        default_value: OFF
       - name: inav_allow_dead_reckoning
         description: "Defines if INAV will dead-reckon over short GPS outages. May also be useful for indoors OPFLOW navigation"
         default_value: OFF

From 4e1e59eb1f16ccfea2d4d4728bdf094eabcced66 Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Fri, 24 May 2024 13:07:07 -0500
Subject: [PATCH 141/323] inav_use_gps_no_baro Settings.md

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

diff --git a/docs/Settings.md b/docs/Settings.md
index 9c31498ef19..9817422595e 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -1844,11 +1844,11 @@ Allows to chose when the home position is reset. Can help prevent resetting home
 
 ### inav_use_gps_no_baro
 
-Defines if INAV should use only use GPS data for altitude estimation when barometer is not available. If set to ON, INAV will allow GPS assisted modes and RTH even when there is no barometer installed.
+Defines if INAV should use only use GPS data for altitude estimation and not barometer. If set to ON, INAV will allow GPS assisted modes and RTH even when there is no barometer installed.
 
 | Default | Min | Max |
 | --- | --- | --- |
-| ON | OFF | ON |
+| OFF | OFF | ON |
 
 ---
 

From 9be624037c2c812b3e08a352e3d952febdf77dcb Mon Sep 17 00:00:00 2001
From: WizzardDr <93602460+WizzardDr@users.noreply.github.com>
Date: Fri, 24 May 2024 22:13:13 +0100
Subject: [PATCH 142/323] Skip `pollVersion()` and `pollGnssCapabilities()` if
 `gps_auto_config = OFF`

Modified gps_ublox.c to skip `pollVersion()` and `pollGnssCapabilities()` if `gps_auto_config = OFF`. This is to support an RX only UART connection of the GPS module.
---
 src/main/io/gps_ublox.c | 37 +++++++++++++++++++------------------
 1 file changed, 19 insertions(+), 18 deletions(-)

diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c
index 8375774e044..8c071613289 100755
--- a/src/main/io/gps_ublox.c
+++ b/src/main/io/gps_ublox.c
@@ -1048,25 +1048,26 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread)
     gpsState.hwVersion = UBX_HW_VERSION_UNKNOWN;
     gpsState.autoConfigStep = 0;
 
-    do {
-        pollVersion();
-        gpsState.autoConfigStep++;
-        ptWaitTimeout((gpsState.hwVersion != UBX_HW_VERSION_UNKNOWN), GPS_CFG_CMD_TIMEOUT_MS);
-    } while(gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN);
-
-    gpsState.autoConfigStep = 0;
-    ubx_capabilities.supported = ubx_capabilities.enabledGnss = ubx_capabilities.defaultGnss = 0;
-    // M7 and earlier will never get pass this step, so skip it (#9440).
-    // UBLOX documents that this is M8N and later
-    if (gpsState.hwVersion > UBX_HW_VERSION_UBLOX7) {
-	do {
-	    pollGnssCapabilities();
-	    gpsState.autoConfigStep++;
-	    ptWaitTimeout((ubx_capabilities.capMaxGnss != 0), GPS_CFG_CMD_TIMEOUT_MS);
-	} while (gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && ubx_capabilities.capMaxGnss == 0);
-    }
     // Configure GPS module if enabled
     if (gpsState.gpsConfig->autoConfig) {
+        do {
+            pollVersion();
+            gpsState.autoConfigStep++;
+            ptWaitTimeout((gpsState.hwVersion != UBX_HW_VERSION_UNKNOWN), GPS_CFG_CMD_TIMEOUT_MS);
+        } while(gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN);
+
+        gpsState.autoConfigStep = 0;
+        ubx_capabilities.supported = ubx_capabilities.enabledGnss = ubx_capabilities.defaultGnss = 0;
+        // M7 and earlier will never get pass this step, so skip it (#9440).
+        // UBLOX documents that this is M8N and later
+        if (gpsState.hwVersion > UBX_HW_VERSION_UBLOX7) {
+        do {
+            pollGnssCapabilities();
+            gpsState.autoConfigStep++;
+            ptWaitTimeout((ubx_capabilities.capMaxGnss != 0), GPS_CFG_CMD_TIMEOUT_MS);
+        } while (gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && ubx_capabilities.capMaxGnss == 0);
+        }
+
         // Configure GPS
         ptSpawn(gpsConfigure);
     }
@@ -1079,7 +1080,7 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread)
         ptSemaphoreWait(semNewDataReady);
         gpsProcessNewSolutionData(false);
 
-        if ((gpsState.gpsConfig->provider == GPS_UBLOX || gpsState.gpsConfig->provider == GPS_UBLOX7PLUS)) {
+        if ((gpsState.gpsConfig->autoConfig) && (gpsState.gpsConfig->provider == GPS_UBLOX || gpsState.gpsConfig->provider == GPS_UBLOX7PLUS)) {
             if ((millis() - gpsState.lastCapaPoolMs) > GPS_CAPA_INTERVAL) {
                 gpsState.lastCapaPoolMs = millis();
 

From 9081429453e7ac6bb4d3f527cb4e74cc7a958ea5 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Fri, 24 May 2024 23:20:55 +0100
Subject: [PATCH 143/323] Include trusted alt for healthy position

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

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index cc4e269ef4b..e3b491af6ae 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -4437,7 +4437,7 @@ uint32_t distanceToFirstWP(void)
 
 bool navigationPositionEstimateIsHealthy(void)
 {
-    return (posControl.flags.estPosStatus >= EST_USABLE) && STATE(GPS_FIX_HOME);
+    return posControl.flags.estPosStatus >= EST_USABLE && posControl.flags.estAltStatus >= EST_USABLE && STATE(GPS_FIX_HOME);
 }
 
 navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)

From 943e16f81f03f12a41a4156cff885133945da9ba Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Sun, 26 May 2024 14:02:35 +0100
Subject: [PATCH 144/323] Update system messages

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

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index f3f00ae7bdb..e48a70ef001 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -525,7 +525,7 @@ void osdFormatAltitudeSymbol(char *buff, int32_t alt)
         buff[0] = ' ';
     }
 
-#ifndef DISABLE_MSP_DJI_COMPAT   // IF DJICOMPAT is not supported, there's no need to check for it and change the values  
+#ifndef DISABLE_MSP_DJI_COMPAT   // IF DJICOMPAT is not supported, there's no need to check for it and change the values
     if (isDJICompatibleVideoSystem(osdConfig())) {
         totalDigits++;
         digits++;
@@ -620,7 +620,7 @@ 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)
@@ -631,7 +631,7 @@ char *osdFormatTrimWhiteSpace(char *buff)
     while(isspace((unsigned char)*buff)) buff++;
 
     // All spaces?
-    if(*buff == 0)  
+    if(*buff == 0)
     return buff;
 
     // Trim trailing spaces
@@ -1098,7 +1098,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]);
@@ -1322,7 +1322,7 @@ static void osdDrawMap(int referenceHeading, uint16_t referenceSym, uint16_t cen
         }
     }
 
-    if (STATE(GPS_FIX) 
+    if (STATE(GPS_FIX)
 #ifdef USE_GPS_FIX_ESTIMATION
             || STATE(GPS_ESTIMATED_FIX)
 #endif
@@ -1841,7 +1841,7 @@ static bool osdDrawSingleElement(uint8_t item)
         if (STATE(GPS_ESTIMATED_FIX)) {
             strcpy(buff + 2, "ES");
             TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
-        } else 
+        } else
 #endif
         if (!STATE(GPS_FIX)) {
             hardwareSensorStatus_e sensorStatus = getHwGPSStatus();
@@ -1900,10 +1900,10 @@ static bool osdDrawSingleElement(uint8_t item)
 
     case OSD_HOME_DIR:
         {
-            if ((STATE(GPS_FIX) 
+            if ((STATE(GPS_FIX)
 #ifdef USE_GPS_FIX_ESTIMATION
                     || STATE(GPS_ESTIMATED_FIX)
-#endif                
+#endif
                     ) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) {
                 if (GPS_distanceToHome < (navConfig()->general.min_rth_distance / 100) ) {
                     displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_HOME_NEAR);
@@ -2540,18 +2540,18 @@ static bool osdDrawSingleElement(uint8_t item)
         osdCrosshairPosition(&elemPosX, &elemPosY);
         osdHudDrawCrosshair(osdGetDisplayPortCanvas(), elemPosX, elemPosY);
 
-        if (osdConfig()->hud_homing && (STATE(GPS_FIX) 
-#ifdef USE_GPS_FIX_ESTIMATION        
+        if (osdConfig()->hud_homing && (STATE(GPS_FIX)
+#ifdef USE_GPS_FIX_ESTIMATION
             || STATE(GPS_ESTIMATED_FIX)
-#endif            
+#endif
             ) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) {
             osdHudDrawHoming(elemPosX, elemPosY);
         }
 
-        if ((STATE(GPS_FIX) 
-#ifdef USE_GPS_FIX_ESTIMATION        
+        if ((STATE(GPS_FIX)
+#ifdef USE_GPS_FIX_ESTIMATION
             || STATE(GPS_ESTIMATED_FIX)
-#endif            
+#endif
             ) && isImuHeadingValid()) {
 
             if (osdConfig()->hud_homepoint || osdConfig()->hud_radar_disp > 0 || osdConfig()->hud_wp_disp > 0) {
@@ -3176,10 +3176,10 @@ static bool osdDrawSingleElement(uint8_t item)
                 digits = 4U;
             }
 #endif
-            if ((STATE(GPS_FIX) 
+            if ((STATE(GPS_FIX)
 #ifdef USE_GPS_FIX_ESTIMATION
                 || STATE(GPS_ESTIMATED_FIX)
-#endif                
+#endif
                 ) && gpsSol.groundSpeed > 0) {
                 if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
                     value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f,
@@ -3202,7 +3202,7 @@ static bool osdDrawSingleElement(uint8_t item)
                         tfp_sprintf(buff + strlen(buff), "%c", 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 DJICOMPAT mode
                         buff[digits + 1] = SYM_MAH_MI_1;
                         buff[digits + 2] = '\0';
@@ -3216,7 +3216,7 @@ static bool osdDrawSingleElement(uint8_t item)
                         tfp_sprintf(buff + strlen(buff), "%c", 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';
@@ -3232,7 +3232,7 @@ static bool osdDrawSingleElement(uint8_t item)
                         tfp_sprintf(buff + strlen(buff), "%c", 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';
@@ -3407,7 +3407,7 @@ static bool osdDrawSingleElement(uint8_t item)
             STATIC_ASSERT(GPS_DEGREES_DIVIDER == OLC_DEG_MULTIPLIER, invalid_olc_deg_multiplier);
             int digits = osdConfig()->plus_code_digits;
             int digitsRemoved = osdConfig()->plus_code_short * 2;
-            if ((STATE(GPS_FIX) 
+            if ((STATE(GPS_FIX)
 #ifdef USE_GPS_FIX_ESTIMATION
                     || STATE(GPS_ESTIMATED_FIX)
 #endif
@@ -4204,7 +4204,7 @@ uint8_t drawStat_Stats(uint8_t statNameX, uint8_t row, uint8_t statValueX, bool
             displayWrite(osdDisplayPort, statNameX, row, "ODOMETER:");
         else
             displayWrite(osdDisplayPort, statNameX, row, "ODOMETER");
-        
+
         switch (osdConfig()->units) {
             case OSD_UNIT_UK:
                 FALLTHROUGH;
@@ -4217,7 +4217,7 @@ uint8_t drawStat_Stats(uint8_t statNameX, uint8_t row, uint8_t statValueX, bool
                     tfp_sprintf(string_buffer, ": %d", statTotalDist);
                     buffLen = 3 + sizeof(statTotalDist);
                 }
-                
+
                 string_buffer[buffLen++] = SYM_MI;
                 break;
             default:
@@ -4243,8 +4243,8 @@ uint8_t drawStat_Stats(uint8_t statNameX, uint8_t row, uint8_t statValueX, bool
                     uint16_t statTotalDist = (uint16_t)(statsConfig()->stats_total_dist / METERS_PER_KILOMETER);
                     tfp_sprintf(string_buffer, ": %d", statTotalDist);
                     buffLen = 3 + sizeof(statTotalDist);
-                }   
-                
+                }
+
                 string_buffer[buffLen++] = SYM_KM;
                 break;
         }
@@ -4255,13 +4255,13 @@ uint8_t drawStat_Stats(uint8_t statNameX, uint8_t row, uint8_t statValueX, bool
             displayWrite(osdDisplayPort, statNameX, ++row, "TOTAL TIME:");
         else
             displayWrite(osdDisplayPort, statNameX, ++row, "TOTAL TIME");
-        
+
         uint32_t tot_mins = statsConfig()->stats_total_time / 60;
         if (isBootStats)
             tfp_sprintf(string_buffer, "%d:%02dH:M%c", (int)(tot_mins / 60), (int)(tot_mins % 60), '\0');
         else
             tfp_sprintf(string_buffer, ": %d:%02d H:M%c", (int)(tot_mins / 60), (int)(tot_mins % 60), '\0');
-        
+
         displayWrite(osdDisplayPort, statValueX-(isBootStats ? 7 : 0), row,  string_buffer);
 
 #ifdef USE_ADC
@@ -4486,7 +4486,7 @@ static void osdUpdateStats(void)
         if (escTemperatureValid) {
             if (stats.min_esc_temp > escSensor->temperature)
                 stats.min_esc_temp = escSensor->temperature;
-            
+
             if (stats.max_esc_temp < escSensor->temperature)
                 stats.max_esc_temp = escSensor->temperature;
         }
@@ -4560,7 +4560,7 @@ uint8_t drawStat_MaxDistanceFromHome(uint8_t col, uint8_t row, uint8_t statValX)
     } else {
         displayWrite(osdDisplayPort, col, row, "MAX DISTANCE FROM ");
     valueXOffset = 18;
-    } 
+    }
     displayWriteChar(osdDisplayPort, col + valueXOffset, row, SYM_HOME);
     tfp_sprintf(buff, ": ");
     osdFormatDistanceStr(buff + 2, stats.max_distance * 100);
@@ -4576,10 +4576,10 @@ uint8_t drawStat_Speed(uint8_t col, uint8_t row, uint8_t statValX)
     uint8_t multiValueXOffset = 0;
 
     displayWrite(osdDisplayPort, col, row, "MAX/AVG SPEED");
-    
+
     osdFormatVelocityStr(buff2, stats.max_3D_speed, true, false);
     tfp_sprintf(buff, ": %s/", osdFormatTrimWhiteSpace(buff2));
-    multiValueXOffset = strlen(buff); 
+    multiValueXOffset = strlen(buff);
     displayWrite(osdDisplayPort, statValX, row, buff);
 
     osdGenerateAverageVelocityStr(buff2);
@@ -4612,8 +4612,8 @@ uint8_t drawStat_BatteryVoltage(uint8_t col, uint8_t row, uint8_t statValX)
     tfp_sprintf(buff, ": ");
     osdFormatCentiNumber(buff + 2, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2, false);
     strcat(osdFormatTrimWhiteSpace(buff), "/");
-    multiValueXOffset = strlen(buff); 
-    // AverageCell 
+    multiValueXOffset = strlen(buff);
+    // AverageCell
     osdFormatCentiNumber(buff + multiValueXOffset, stats.min_voltage / getBatteryCellCount(), 0, 2, 0, 3, false);
     tfp_sprintf(buff + strlen(buff), "%c", SYM_VOLT);
 
@@ -4645,7 +4645,7 @@ uint8_t drawStat_MaximumPowerAndCurrent(uint8_t col, uint8_t row, uint8_t statVa
 uint8_t drawStat_UsedEnergy(uint8_t col, uint8_t row, uint8_t statValX)
 {
     char    buff[12];
-    
+
     if (osdDisplayIsHD())
         displayWrite(osdDisplayPort, col, row, "USED ENERGY FLT/TOT");
     else
@@ -4674,7 +4674,7 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b
     int32_t totalDistance = getTotalTravelDistance();
     bool moreThanAh = false;
     bool efficiencyValid = totalDistance >= 10000;
-    
+
     if (osdDisplayIsHD())
         displayWrite(osdDisplayPort, col, row, "AVG EFFICIENCY FLT/TOT");
     else
@@ -4698,18 +4698,18 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b
                         moreThanAh = osdFormatCentiNumber(buff, (int32_t)((getMAhDrawn() - stats.flightStartMAh) * 10000.0f * METERS_PER_MILE / totalDistance), 1000, 0, 2, digits, false);
                         strcat(outBuff, osdFormatTrimWhiteSpace(buff));
                         if (osdDisplayIsHD()) {
-                            if (!moreThanAh) 
+                            if (!moreThanAh)
                                 tfp_sprintf(outBuff + strlen(outBuff), "%c%c", SYM_MAH_MI_0, SYM_MAH_MI_1);
-                            else 
+                            else
                                 tfp_sprintf(outBuff + strlen(outBuff), "%c", SYM_AH_MI);
-                            
+
                             moreThanAh = false;
                         }
 
                         strcat(outBuff, "/");
                         moreThanAh = moreThanAh || osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_MILE / totalDistance), 1000, 0, 2, digits, false);
                         strcat(outBuff, osdFormatTrimWhiteSpace(buff));
-                        
+
                         if (!moreThanAh)
                             tfp_sprintf(outBuff + strlen(outBuff), "%c%c", SYM_MAH_MI_0, SYM_MAH_MI_1);
                         else
@@ -4736,11 +4736,11 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b
                         moreThanAh = osdFormatCentiNumber(buff, (int32_t)((getMAhDrawn()-stats.flightStartMAh) * 10000.0f * METERS_PER_NAUTICALMILE / totalDistance), 1000, 0, 2, digits, false);
                         strcat(outBuff, osdFormatTrimWhiteSpace(buff));
                          if (osdDisplayIsHD()) {
-                            if (!moreThanAh) 
+                            if (!moreThanAh)
                                 tfp_sprintf(outBuff + strlen(outBuff), "%c%c", SYM_MAH_NM_0, SYM_MAH_NM_1);
-                            else 
+                            else
                                 tfp_sprintf(outBuff + strlen(outBuff), "%c", SYM_AH_NM);
-                            
+
                             moreThanAh = false;
                         }
 
@@ -4781,11 +4781,11 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b
                 moreThanAh = osdFormatCentiNumber(buff, (int32_t)((getMAhDrawn() - stats.flightStartMAh) * 10000000.0f / totalDistance), 1000, 0, 2, digits, false);
                 strcat(outBuff, osdFormatTrimWhiteSpace(buff));
                 if (osdDisplayIsHD()) {
-                    if (!moreThanAh) 
+                    if (!moreThanAh)
                         tfp_sprintf(outBuff + strlen(outBuff), "%c%c", SYM_MAH_KM_0, SYM_MAH_KM_1);
-                    else 
+                    else
                         tfp_sprintf(outBuff + strlen(outBuff), "%c", SYM_AH_KM);
-                    
+
                     moreThanAh = false;
                 }
 
@@ -4838,7 +4838,7 @@ uint8_t drawStat_RXStats(uint8_t col, uint8_t row, uint8_t statValX)
     tfp_sprintf(buff, ": ");
     itoa(stats.min_rssi, buff + 2, 10);
     strcat(osdFormatTrimWhiteSpace(buff), "%");
-    
+
     if (rxConfig()->serialrx_provider == SERIALRX_CRSF) {
         strcat(osdFormatTrimWhiteSpace(buff), "/");
         multiValueXOffset = strlen(buff);
@@ -4881,9 +4881,9 @@ uint8_t drawStat_ESCTemperature(uint8_t col, uint8_t row, uint8_t statValX)
 {
     char buff[12];
     displayWrite(osdDisplayPort, col, row, "MIN/MAX ESC TEMP");
-    tfp_sprintf(buff, ": %3d/%3d%c", 
-                ((osdConfig()->units == OSD_UNIT_IMPERIAL) ? (int16_t)(stats.min_esc_temp * 9 / 5.0f + 320) : stats.min_esc_temp), 
-                ((osdConfig()->units == OSD_UNIT_IMPERIAL) ? (int16_t)(stats.max_esc_temp * 9 / 5.0f + 320) : stats.max_esc_temp), 
+    tfp_sprintf(buff, ": %3d/%3d%c",
+                ((osdConfig()->units == OSD_UNIT_IMPERIAL) ? (int16_t)(stats.min_esc_temp * 9 / 5.0f + 320) : stats.min_esc_temp),
+                ((osdConfig()->units == OSD_UNIT_IMPERIAL) ? (int16_t)(stats.max_esc_temp * 9 / 5.0f + 320) : stats.max_esc_temp),
                 ((osdConfig()->units == OSD_UNIT_IMPERIAL) ? SYM_TEMP_F : SYM_TEMP_C));
     displayWrite(osdDisplayPort, statValX, row++, buff);
 
@@ -4904,10 +4904,10 @@ uint8_t drawStat_GForce(uint8_t col, uint8_t row, uint8_t statValX)
         displayWrite(osdDisplayPort, col, row, "MAX G-FORCE");
     else
         displayWrite(osdDisplayPort, col, row, "MAX/MIN Z/MAX Z G-FORCE");
-        
+
     tfp_sprintf(outBuff, ": ");
     osdFormatCentiNumber(buff, max_gforce * 100, 0, 2, 0, 3, false);
-    
+
     if (!osdDisplayIsHD()) {
         strcat(outBuff, osdFormatTrimWhiteSpace(buff));
         displayWrite(osdDisplayPort, statValX, row++, outBuff);
@@ -4922,7 +4922,7 @@ uint8_t drawStat_GForce(uint8_t col, uint8_t row, uint8_t statValX)
     osdFormatCentiNumber(buff, acc_extremes_min * 100, 0, 2, 0, 4, false);
     strcat(outBuff, osdFormatTrimWhiteSpace(buff));
     strcat(outBuff, "/");
-    
+
     osdFormatCentiNumber(buff, acc_extremes_max * 100, 0, 2, 0, 3, false);
     strcat(outBuff, osdFormatTrimWhiteSpace(buff));
     displayWrite(osdDisplayPort, statValX, row++, outBuff);
@@ -4949,7 +4949,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
 
     const uint8_t statNameX = (osdDisplayPort->cols - (osdDisplayIsHD() ? 41 : 28)) / 2;
     const uint8_t statValuesX = osdDisplayPort->cols - statNameX - (osdDisplayIsHD() ? 15 : 11);
-    
+
     if (page > 1)
         page = 0;
 
@@ -5023,9 +5023,9 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
                 if (feature(FEATURE_BLACKBOX)) {
                     char buff[12];
                     displayWrite(osdDisplayPort, statNameX, row, "BLACKBOX FILE");
-                    
+
                     tfp_sprintf(buff, ": %u/%u", stats.min_sats, stats.max_sats);
-                    
+
                     int32_t logNumber = blackboxGetLogNumber();
                     if (logNumber >= 0)
                         tfp_sprintf(buff, ": %05ld ", logNumber);
@@ -5043,7 +5043,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
                 break;
         }
     }
-    
+
     row = drawStat_DisarmMethod(statNameX, row, statValuesX);
 
     // The following has been commented out as it will be added in #9688
@@ -5399,27 +5399,27 @@ static void osdRefresh(timeUs_t currentTimeUs)
 
              // 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;
                 }
             }
@@ -5462,7 +5462,7 @@ static void osdRefresh(timeUs_t currentTimeUs)
             displayHeartbeat(osdDisplayPort);
             isThrottleHigh = checkStickPosition(THR_HI);
         }
-        
+
         return;
     }
 
@@ -5631,24 +5631,19 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
 
     if (buff != NULL) {
         const char *message = NULL;
-        char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH+1)]; //warning: shared buffer. Make sure it is used by single message in code below!
-        // We might have up to 5 messages to show.
-        const char *messages[5];
+        // Warning: messageBuf is shared. Make sure it is used by single message in code below!
+        char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH + 1)];
+        // We might have up to 6 messages to show.
+        const char *messages[6];
         unsigned messageCount = 0;
         const char *failsafeInfoMessage = NULL;
         const char *invertedInfoMessage = NULL;
 
         if (ARMING_FLAG(ARMED)) {
-#ifdef USE_FW_AUTOLAND
-            if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding() || FLIGHT_MODE(NAV_FW_AUTOLAND)) {
-                if (isWaypointMissionRTHActive() && !posControl.fwLandState.landWp) {
-#else
-            if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
-                if (isWaypointMissionRTHActive()) {
-#endif
-                    // if RTH activated whilst WP mode selected, remind pilot to cancel WP mode to exit RTH
-                    messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_WP_RTH_CANCEL);
-                }
+            if (STATE(LANDING_DETECTED)) {
+                messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_LANDED);
+            } else if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
+                // Returns maximum of 5 messages
                 if (navGetCurrentStateFlags() & NAV_AUTO_WP_DONE) {
                     messages[messageCount++] = STATE(LANDING_DETECTED) ? OSD_MESSAGE_STR(OSD_MSG_WP_LANDED) : OSD_MESSAGE_STR(OSD_MSG_WP_FINISHED);
                 } else if (NAV_Status.state == MW_NAV_STATE_WP_ENROUTE) {
@@ -5670,36 +5665,27 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
 
                         messages[messageCount++] = messageBuf;
                     }
-                } else if (NAV_Status.state == MW_NAV_STATE_LAND_SETTLE && posControl.landingDelay > 0) {
-                    uint16_t remainingHoldSec = MS2S(posControl.landingDelay - millis());
-                    tfp_sprintf(messageBuf, "LANDING DELAY: %3u SECONDS", remainingHoldSec);
-
-                    messages[messageCount++] = messageBuf;
                 }
-
                 else {
-#ifdef USE_FW_AUTOLAND
-                    if (canFwLandingBeCancelled()) {
-                         messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MOVE_STICKS);
-                    } else if (!FLIGHT_MODE(NAV_FW_AUTOLAND)) {
-#endif
-                        const char *navStateMessage = navigationStateMessage();
-                        if (navStateMessage) {
-                            messages[messageCount++] = navStateMessage;
-                        }
-#ifdef USE_FW_AUTOLAND
+                    const char *navStateMessage = navigationStateMessage();
+                    if (navStateMessage) {
+                        messages[messageCount++] = navStateMessage;
                     }
-#endif
                 }
-
 #if defined(USE_SAFE_HOME)
                 const char *safehomeMessage = divertingToSafehomeMessage();
                 if (safehomeMessage) {
                     messages[messageCount++] = safehomeMessage;
                 }
 #endif
-                if (FLIGHT_MODE(FAILSAFE_MODE)) {
-                    // In FS mode while being armed too
+                if (FLIGHT_MODE(FAILSAFE_MODE)) {   // In FS mode while armed
+                    if (NAV_Status.state == MW_NAV_STATE_LAND_SETTLE && posControl.landingDelay > 0) {
+                        uint16_t remainingHoldSec = MS2S(posControl.landingDelay - millis());
+                        tfp_sprintf(messageBuf, "LANDING DELAY: %3u SECONDS", remainingHoldSec);
+
+                        messages[messageCount++] = messageBuf;
+                    }
+
                     const char *failsafePhaseMessage = osdFailsafePhaseMessage();
                     failsafeInfoMessage = osdFailsafeInfoMessage();
 
@@ -5709,56 +5695,52 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                     if (failsafeInfoMessage) {
                         messages[messageCount++] = failsafeInfoMessage;
                     }
+                } else if (isWaypointMissionRTHActive()) {
+                    // if RTH activated whilst WP mode selected, remind pilot to cancel WP mode to exit RTH
+                    messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_WP_RTH_CANCEL);
                 }
             } else {    /* messages shown only when Failsafe, WP, RTH or Emergency Landing not active */
-                if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) {
-                    messages[messageCount++] = navConfig()->fw.launch_manual_throttle ? OSD_MESSAGE_STR(OSD_MSG_AUTOLAUNCH_MANUAL) :
-                                                                                        OSD_MESSAGE_STR(OSD_MSG_AUTOLAUNCH);
-                    const char *launchStateMessage = fixedWingLaunchStateMessage();
-                    if (launchStateMessage) {
-                        messages[messageCount++] = launchStateMessage;
-                    }
-                } else {
-                    if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) {
-                        // ALTHOLD might be enabled alongside ANGLE/HORIZON/ANGLEHOLD/ACRO
-                        // when it doesn't require ANGLE mode (required only in FW
-                        // right now). If it requires ANGLE, its display is handled by OSD_FLYMODE.
-                        messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALTITUDE_HOLD);
-                    }
-                    if (STATE(MULTIROTOR) && FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
-                        if (posControl.cruise.multicopterSpeed >= 50.0f) {
-                            char buf[6];
-                            osdFormatVelocityStr(buf, posControl.cruise.multicopterSpeed, false, false);
-                            tfp_sprintf(messageBuf, "(SPD %s)", buf);
-                        } else {
-                            strcpy(messageBuf, "(HOLD)");
-                        }
-                        messages[messageCount++] = messageBuf;
-                    }
-                    if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM) && !feature(FEATURE_FW_AUTOTRIM)) {
-                        messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTRIM);
-                    }
-                    if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) {
-                        messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTUNE);
-                        if (FLIGHT_MODE(MANUAL_MODE)) {
-                            messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTUNE_ACRO);
+                if (STATE(AIRPLANE)) {  // Returns maximum of 3 messages
+#ifdef USE_FW_AUTOLAND
+                    if (canFwLandingBeCancelled()) {
+                         messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MOVE_STICKS);
+                    } else
+#endif
+                    if (navGetCurrentStateFlags() & NAV_CTL_LAUNCH) {
+                        messages[messageCount++] = navConfig()->fw.launch_manual_throttle ? OSD_MESSAGE_STR(OSD_MSG_AUTOLAUNCH_MANUAL) :
+                                                                                            OSD_MESSAGE_STR(OSD_MSG_AUTOLAUNCH);
+                        const char *launchStateMessage = fixedWingLaunchStateMessage();
+                        if (launchStateMessage) {
+                            messages[messageCount++] = launchStateMessage;
                         }
-                    }
-                    if (isFixedWingLevelTrimActive()) {
-                            messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOLEVEL);
-                    }
-                    if (FLIGHT_MODE(HEADFREE_MODE)) {
-                        messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_HEADFREE);
-                    }
-                    if (FLIGHT_MODE(SOARING_MODE)) {
+                    } else if (FLIGHT_MODE(SOARING_MODE)) {
                         messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_NAV_SOARING);
+                    } else {
+                        strcpy(messageBuf, "(AUTO-");
+                        if (isFixedWingLevelTrimActive()) {
+                            strcat(messageBuf, OSD_MSG_AUTOLEVEL);
+                        }
+                        if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM) && !feature(FEATURE_FW_AUTOTRIM)) {
+                            if (messageBuf[6] != '\0') {
+                                strcat(messageBuf, ":");
+                            }
+                            strcat(messageBuf, OSD_MSG_AUTOTRIM);
+                        }
+                        if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) {
+                            if (messageBuf[6] != '\0') {
+                                strcat(messageBuf, ":");
+                            }
+                            strcat(messageBuf, OSD_MSG_AUTOTUNE);
+                            if (FLIGHT_MODE(MANUAL_MODE)) {
+                                messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTUNE_ACRO);
+                            }
+                        }
+                        if (messageBuf[6] != '\0') {
+                            strcat(messageBuf, ")");
+                            messages[messageCount++] = messageBuf;
+                        }
                     }
-                    if (posControl.flags.wpMissionPlannerActive) {
-                        messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MISSION_PLANNER);
-                    }
-                    if (STATE(LANDING_DETECTED)) {
-                        messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_LANDED);
-                    }
+
                     if (IS_RC_MODE_ACTIVE(BOXANGLEHOLD)) {
                         int8_t navAngleHoldAxis = navCheckActiveAngleHoldAxis();
                         if (isAngleHoldLevel()) {
@@ -5769,40 +5751,58 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                             messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_PITCH);
                         }
                     }
+                } else if (STATE(MULTIROTOR)) {     // Returns maximum of 1 messages
+                    if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
+                        if (posControl.cruise.multicopterSpeed >= 50.0f) {
+                            char buf[6];
+                            osdFormatVelocityStr(buf, posControl.cruise.multicopterSpeed, false, false);
+                            tfp_sprintf(messageBuf, "(SPD %s)", buf);
+                        } else {
+                            strcpy(messageBuf, "(HOLD)");
+                        }
+                        messages[messageCount++] = messageBuf;
+                    } else if (FLIGHT_MODE(HEADFREE_MODE)) {
+                        messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_HEADFREE);
+                    }
+                }
+
+                if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) {
+                    // ALTHOLD might be enabled alongside ANGLE/HORIZON/ANGLEHOLD/ACRO
+                    // when it doesn't require ANGLE mode (required only in FW
+                    // right now). If it requires ANGLE, its display is handled by OSD_FLYMODE.
+                    messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALTITUDE_HOLD);
+                }
+
+                if (posControl.flags.wpMissionPlannerActive) {
+                    messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MISSION_PLANNER);
                 }
             }
-        } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) {
+        } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) {    // Returns maximum of 2 messages
             unsigned invalidIndex;
 
             // Check if we're unable to arm for some reason
             if (ARMING_FLAG(ARMING_DISABLED_INVALID_SETTING) && !settingsValidate(&invalidIndex)) {
+                const setting_t *setting = settingGet(invalidIndex);
+                settingGetName(setting, messageBuf);
+                for (int ii = 0; messageBuf[ii]; ii++) {
+                    messageBuf[ii] = sl_toupper(messageBuf[ii]);
+                }
+                invertedInfoMessage = messageBuf;
+                messages[messageCount++] = invertedInfoMessage;
 
-                    const setting_t *setting = settingGet(invalidIndex);
-                    settingGetName(setting, messageBuf);
-                    for (int ii = 0; messageBuf[ii]; ii++) {
-                        messageBuf[ii] = sl_toupper(messageBuf[ii]);
-                    }
-                    invertedInfoMessage = messageBuf;
-                    messages[messageCount++] = invertedInfoMessage;
-
-                    invertedInfoMessage = OSD_MESSAGE_STR(OSD_MSG_INVALID_SETTING);
-                    messages[messageCount++] = invertedInfoMessage;
-
+                invertedInfoMessage = OSD_MESSAGE_STR(OSD_MSG_INVALID_SETTING);
+                messages[messageCount++] = invertedInfoMessage;
             } else {
-
-                    invertedInfoMessage = OSD_MESSAGE_STR(OSD_MSG_UNABLE_ARM);
-                    messages[messageCount++] = invertedInfoMessage;
-
-                    // Show the reason for not arming
-                    messages[messageCount++] = osdArmingDisabledReasonMessage();
-
+                invertedInfoMessage = OSD_MESSAGE_STR(OSD_MSG_UNABLE_ARM);
+                messages[messageCount++] = invertedInfoMessage;
+                // Show the reason for not arming
+                messages[messageCount++] = osdArmingDisabledReasonMessage();
             }
         } else if (!ARMING_FLAG(ARMED)) {
             if (isWaypointListValid()) {
                 messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_WP_MISSION_LOADED);
             }
         }
-
         /* Messages that are shown regardless of Arming state */
 
         // The following has been commented out as it will be added in #9688
@@ -5834,7 +5834,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
             } else if (message == invertedInfoMessage) {
                 TEXT_ATTRIBUTES_ADD_INVERTED(elemAttr);
             }
-            // We're shoing either failsafePhaseMessage or
+            // We're showing either failsafePhaseMessage or
             // navStateMessage. Don't BLINK here since
             // having this text available might be crucial
             // during a lost aircraft recovery and blinking

From 7a4f6cbfc6cd41aac32f1e1182583d121ee8d6c2 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Mon, 27 May 2024 17:20:15 +0100
Subject: [PATCH 145/323] Comments + change FW auto messages

---
 src/main/io/osd.c | 38 ++++++++++++++++++++++----------------
 src/main/io/osd.h |  6 +++---
 2 files changed, 25 insertions(+), 19 deletions(-)

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index e48a70ef001..ff91d559629 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -2380,8 +2380,9 @@ static bool osdDrawSingleElement(uint8_t item)
                 p = " WP ";
             else if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && navigationRequiresAngleMode()) {
                 // If navigationRequiresAngleMode() returns false when ALTHOLD is active,
-                // it means it can be combined with ANGLE, HORIZON, ANGLEHOLD, ACRO, etc...
+                // it means it can be combined with ANGLE, HORIZON, ACRO, etc...
                 // and its display is handled by OSD_MESSAGES rather than OSD_FLYMODE.
+                // (Currently only applies to multirotor).
                 p = " AH ";
             }
             else if (FLIGHT_MODE(ANGLE_MODE))
@@ -5631,11 +5632,14 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
 
     if (buff != NULL) {
         const char *message = NULL;
-        // Warning: messageBuf is shared. Make sure it is used by single message in code below!
+        /* Warning: messageBuf is shared, use accordingly */
         char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH + 1)];
-        // We might have up to 6 messages to show.
-        const char *messages[6];
+
+        /* Warning, ensure number of messages returned does not exceed messages array size
+         * Messages array set 1 larger than maximum expected message count of 6 */
+        const char *messages[7];
         unsigned messageCount = 0;
+
         const char *failsafeInfoMessage = NULL;
         const char *invertedInfoMessage = NULL;
 
@@ -5643,7 +5647,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
             if (STATE(LANDING_DETECTED)) {
                 messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_LANDED);
             } else if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
-                // Returns maximum of 5 messages
+                /* RETURNS MAXIMUM OF 5 MESSAGES */
                 if (navGetCurrentStateFlags() & NAV_AUTO_WP_DONE) {
                     messages[messageCount++] = STATE(LANDING_DETECTED) ? OSD_MESSAGE_STR(OSD_MSG_WP_LANDED) : OSD_MESSAGE_STR(OSD_MSG_WP_FINISHED);
                 } else if (NAV_Status.state == MW_NAV_STATE_WP_ENROUTE) {
@@ -5699,8 +5703,9 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                     // if RTH activated whilst WP mode selected, remind pilot to cancel WP mode to exit RTH
                     messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_WP_RTH_CANCEL);
                 }
-            } else {    /* messages shown only when Failsafe, WP, RTH or Emergency Landing not active */
-                if (STATE(AIRPLANE)) {  // Returns maximum of 3 messages
+            } else {    /* Messages shown only when Failsafe, WP, RTH or Emergency Landing not active */
+                /* RETURNS MAXIMUM OF 4 MESSAGES */
+                if (STATE(AIRPLANE)) {      /* RETURNS MAXIMUM OF 3 MESSAGES */
 #ifdef USE_FW_AUTOLAND
                     if (canFwLandingBeCancelled()) {
                          messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MOVE_STICKS);
@@ -5751,7 +5756,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                             messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_PITCH);
                         }
                     }
-                } else if (STATE(MULTIROTOR)) {     // Returns maximum of 1 messages
+                } else if (STATE(MULTIROTOR)) {     /* RETURNS MAXIMUM OF 2 MESSAGES */
                     if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
                         if (posControl.cruise.multicopterSpeed >= 50.0f) {
                             char buf[6];
@@ -5764,20 +5769,20 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                     } else if (FLIGHT_MODE(HEADFREE_MODE)) {
                         messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_HEADFREE);
                     }
-                }
+                    if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) {
+                        /* If ALTHOLD is separately enabled for multirotor together with ANGL/HORIZON/ACRO modes
+                         * then ANGL/HORIZON/ACRO are indicated by the OSD_FLYMODE field.
+                         * In this case indicate ALTHOLD is active via a system message */
 
-                if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) {
-                    // ALTHOLD might be enabled alongside ANGLE/HORIZON/ANGLEHOLD/ACRO
-                    // when it doesn't require ANGLE mode (required only in FW
-                    // right now). If it requires ANGLE, its display is handled by OSD_FLYMODE.
-                    messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALTITUDE_HOLD);
+                        messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALTITUDE_HOLD);
+                    }
                 }
 
                 if (posControl.flags.wpMissionPlannerActive) {
                     messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MISSION_PLANNER);
                 }
             }
-        } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) {    // Returns maximum of 2 messages
+        } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) {    /* RETURNS MAXIMUM OF 2 MESSAGES */
             unsigned invalidIndex;
 
             // Check if we're unable to arm for some reason
@@ -5803,7 +5808,8 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                 messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_WP_MISSION_LOADED);
             }
         }
-        /* Messages that are shown regardless of Arming state */
+
+        /* Messages that are shown regardless of Arming state - RETURNS MAXIMUM OF 1 MESSAGES */
 
         // The following has been commented out as it will be added in #9688
         // uint16_t rearmMs = (emergInflightRearmEnabled()) ? emergencyInFlightRearmTimeMS() : 0;
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index ca5dca66137..3a8f0b98403 100644
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -109,10 +109,10 @@
 #define OSD_MSG_AUTOLAUNCH          "AUTOLAUNCH"
 #define OSD_MSG_AUTOLAUNCH_MANUAL   "AUTOLAUNCH (MANUAL)"
 #define OSD_MSG_ALTITUDE_HOLD       "(ALTITUDE HOLD)"
-#define OSD_MSG_AUTOTRIM            "(AUTOTRIM)"
-#define OSD_MSG_AUTOTUNE            "(AUTOTUNE)"
+#define OSD_MSG_AUTOTRIM            "TRIM"
+#define OSD_MSG_AUTOTUNE            "TUNE"
 #define OSD_MSG_AUTOTUNE_ACRO       "SWITCH TO ACRO"
-#define OSD_MSG_AUTOLEVEL           "(AUTO LEVEL TRIM)"
+#define OSD_MSG_AUTOLEVEL           "LEVELTRIM"
 #define OSD_MSG_HEADFREE            "(HEADFREE)"
 #define OSD_MSG_NAV_SOARING         "(SOARING)"
 #define OSD_MSG_UNABLE_ARM          "UNABLE TO ARM"

From 5d7d4adbc5b6f61a47b727caf7e00cfce2e11da9 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Mon, 27 May 2024 19:49:40 +0100
Subject: [PATCH 146/323] Update osd.c

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

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index ff91d559629..78c8316f369 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -5647,7 +5647,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
             if (STATE(LANDING_DETECTED)) {
                 messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_LANDED);
             } else if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
-                /* RETURNS MAXIMUM OF 5 MESSAGES */
+                /* ADDS MAXIMUM OF 5 MESSAGES */
                 if (navGetCurrentStateFlags() & NAV_AUTO_WP_DONE) {
                     messages[messageCount++] = STATE(LANDING_DETECTED) ? OSD_MESSAGE_STR(OSD_MSG_WP_LANDED) : OSD_MESSAGE_STR(OSD_MSG_WP_FINISHED);
                 } else if (NAV_Status.state == MW_NAV_STATE_WP_ENROUTE) {
@@ -5704,8 +5704,8 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                     messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_WP_RTH_CANCEL);
                 }
             } else {    /* Messages shown only when Failsafe, WP, RTH or Emergency Landing not active */
-                /* RETURNS MAXIMUM OF 4 MESSAGES */
-                if (STATE(AIRPLANE)) {      /* RETURNS MAXIMUM OF 3 MESSAGES */
+                /* ADDS MAXIMUM OF 4 MESSAGES */
+                if (STATE(AIRPLANE)) {      /* ADDS MAXIMUM OF 3 MESSAGES */
 #ifdef USE_FW_AUTOLAND
                     if (canFwLandingBeCancelled()) {
                          messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MOVE_STICKS);
@@ -5756,7 +5756,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                             messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_PITCH);
                         }
                     }
-                } else if (STATE(MULTIROTOR)) {     /* RETURNS MAXIMUM OF 2 MESSAGES */
+                } else if (STATE(MULTIROTOR)) {     /* ADDS MAXIMUM OF 2 MESSAGES */
                     if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
                         if (posControl.cruise.multicopterSpeed >= 50.0f) {
                             char buf[6];
@@ -5782,7 +5782,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                     messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MISSION_PLANNER);
                 }
             }
-        } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) {    /* RETURNS MAXIMUM OF 2 MESSAGES */
+        } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) {    /* ADDS MAXIMUM OF 2 MESSAGES */
             unsigned invalidIndex;
 
             // Check if we're unable to arm for some reason
@@ -5809,7 +5809,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
             }
         }
 
-        /* Messages that are shown regardless of Arming state - RETURNS MAXIMUM OF 1 MESSAGES */
+        /* Messages that are shown regardless of Arming state - ADDS MAXIMUM OF 1 MESSAGES */
 
         // The following has been commented out as it will be added in #9688
         // uint16_t rearmMs = (emergInflightRearmEnabled()) ? emergencyInFlightRearmTimeMS() : 0;

From 3051c90cbc1db49d6f448f5a82a78bc37e178a6f Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Mon, 27 May 2024 20:22:14 +0100
Subject: [PATCH 147/323] Update osd.c

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

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 78c8316f369..552687a4c62 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -5722,14 +5722,14 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                         messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_NAV_SOARING);
                     } else {
                         strcpy(messageBuf, "(AUTO-");
-                        if (isFixedWingLevelTrimActive()) {
-                            strcat(messageBuf, OSD_MSG_AUTOLEVEL);
-                        }
                         if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM) && !feature(FEATURE_FW_AUTOTRIM)) {
+                            strcat(messageBuf, OSD_MSG_AUTOTRIM);
+                        }
+                        if (isFixedWingLevelTrimActive()) {
                             if (messageBuf[6] != '\0') {
                                 strcat(messageBuf, ":");
                             }
-                            strcat(messageBuf, OSD_MSG_AUTOTRIM);
+                            strcat(messageBuf, OSD_MSG_AUTOLEVEL);
                         }
                         if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) {
                             if (messageBuf[6] != '\0') {

From 3802e1df1b141d5dda9b159169bf617215677d50 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Tue, 28 May 2024 21:20:47 +0100
Subject: [PATCH 148/323] Add FW tune/trim mode priority

---
 src/main/fc/rc_modes.c         | 16 +++++++++++-
 src/main/fc/rc_modes.h         |  1 +
 src/main/flight/pid.c          |  4 +--
 src/main/flight/pid_autotune.c |  5 ++--
 src/main/flight/servos.c       | 24 ++++++++---------
 src/main/io/osd.c              | 47 ++++++++++++----------------------
 src/main/io/osd.h              |  6 ++---
 7 files changed, 52 insertions(+), 51 deletions(-)

diff --git a/src/main/fc/rc_modes.c b/src/main/fc/rc_modes.c
index 741308755ca..b593bddcc6a 100755
--- a/src/main/fc/rc_modes.c
+++ b/src/main/fc/rc_modes.c
@@ -114,7 +114,6 @@ void processAirmode(void) {
     } else if (STATE(MULTIROTOR)) {
         processAirmodeMultirotor();
     }
-
 }
 
 bool isUsingNavigationModes(void)
@@ -122,6 +121,21 @@ bool isUsingNavigationModes(void)
     return isUsingNAVModes;
 }
 
+bool isFwAutoModeActive(boxId_e mode)
+{
+    /* Sets activation priority of fixed wing auto tune/trim modes: Autotune -> Autotrim -> Autolevel */
+
+    if (mode == BOXAUTOTUNE) {
+        return IS_RC_MODE_ACTIVE(BOXAUTOTUNE);
+    } else if (mode == BOXAUTOTRIM) {
+        return IS_RC_MODE_ACTIVE(BOXAUTOTRIM) && !IS_RC_MODE_ACTIVE(BOXAUTOTUNE);
+    } else if (mode == BOXAUTOLEVEL) {
+        return IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) && !IS_RC_MODE_ACTIVE(BOXAUTOTUNE) && !IS_RC_MODE_ACTIVE(BOXAUTOTRIM);
+    }
+
+    return false;
+}
+
 bool IS_RC_MODE_ACTIVE(boxId_e boxId)
 {
     return bitArrayGet(rcModeActivationMask.bits, boxId);
diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h
index 04aea681bc9..ad2c4422252 100644
--- a/src/main/fc/rc_modes.h
+++ b/src/main/fc/rc_modes.h
@@ -140,3 +140,4 @@ bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range);
 
 void updateActivatedModes(void);
 void updateUsedModeActivationConditionFlags(void);
+bool isFwAutoModeActive(boxId_e mode);
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index a9d1291877f..dc15f07ad6f 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -1380,7 +1380,7 @@ pidBank_t * pidBankMutable(void) {
 
 bool isFixedWingLevelTrimActive(void)
 {
-    return IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) && !areSticksDeflected() &&
+    return isFwAutoModeActive(BOXAUTOLEVEL) && !areSticksDeflected() &&
            (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) &&
            !FLIGHT_MODE(SOARING_MODE) && !FLIGHT_MODE(MANUAL_MODE) &&
            !navigationIsControllingAltitude() && !(navCheckActiveAngleHoldAxis() == FD_PITCH && !angleHoldIsLevel);
@@ -1404,7 +1404,7 @@ void updateFixedWingLevelTrim(timeUs_t currentTimeUs)
     previousArmingState = ARMING_FLAG(ARMED);
 
     // return if not active or disarmed
-    if (!IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) || !ARMING_FLAG(ARMED)) {
+    if (!isFwAutoModeActive(BOXAUTOLEVEL) || !ARMING_FLAG(ARMED)) {
         return;
     }
 
diff --git a/src/main/flight/pid_autotune.c b/src/main/flight/pid_autotune.c
index af6896f4233..c4be8101bc4 100755
--- a/src/main/flight/pid_autotune.c
+++ b/src/main/flight/pid_autotune.c
@@ -40,6 +40,7 @@
 #include "fc/config.h"
 #include "fc/controlrate_profile.h"
 #include "fc/rc_controls.h"
+#include "fc/rc_modes.h"
 #include "fc/rc_adjustments.h"
 #include "fc/runtime_config.h"
 #include "fc/settings.h"
@@ -130,7 +131,7 @@ void autotuneStart(void)
 
 void autotuneUpdateState(void)
 {
-    if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE) && STATE(AIRPLANE) && ARMING_FLAG(ARMED)) {
+    if (isFwAutoModeActive(BOXAUTOTUNE) && STATE(AIRPLANE) && ARMING_FLAG(ARMED)) {
         if (!FLIGHT_MODE(AUTO_TUNE)) {
             autotuneStart();
             ENABLE_FLIGHT_MODE(AUTO_TUNE);
@@ -202,7 +203,7 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa
 
         if ((tuneCurrent[axis].updateCount & 25) == 0 && tuneCurrent[axis].updateCount >= AUTOTUNE_FIXED_WING_MIN_SAMPLES) {
             if (pidAutotuneConfig()->fw_rate_adjustment != FIXED  && !FLIGHT_MODE(ANGLE_MODE)) { // Rate discovery is not possible in ANGLE mode
-                
+
                 // Target 80% control surface deflection to leave some room for P and I to work
                 float pidSumTarget = (pidAutotuneConfig()->fw_max_rate_deflection / 100.0f) * pidSumLimit;
 
diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c
index f38a4ea108c..f88f4cd2cf1 100755
--- a/src/main/flight/servos.c
+++ b/src/main/flight/servos.c
@@ -82,7 +82,7 @@ void Reset_servoMixers(servoMixer_t *instance)
 #ifdef USE_PROGRAMMING_FRAMEWORK
             ,.conditionId = -1
 #endif
-        );       
+        );
     }
 }
 
@@ -96,7 +96,7 @@ void pgResetFn_servoParams(servoParam_t *instance)
             .max = DEFAULT_SERVO_MAX,
             .middle = DEFAULT_SERVO_MIDDLE,
             .rate = 100
-        );        
+        );
     }
 }
 
@@ -194,7 +194,7 @@ void servosInit(void)
 }
 
 int getServoCount(void)
-{   
+{
     if (mixerUsesServos) {
         return 1 + maxServoIndex - minServoIndex;
     }
@@ -246,7 +246,7 @@ static void filterServos(void)
 void writeServos(void)
 {
     filterServos();
-    
+
 #if !defined(SITL_BUILD)
     int servoIndex = 0;
     bool zeroServoValue = false;
@@ -449,7 +449,7 @@ void processServoAutotrimMode(void)
     static int32_t servoMiddleAccum[MAX_SUPPORTED_SERVOS];
     static int32_t servoMiddleAccumCount[MAX_SUPPORTED_SERVOS];
 
-    if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM)) {
+    if (isFwAutoModeActive(BOXAUTOTRIM)) {
         switch (trimState) {
             case AUTOTRIM_IDLE:
                 if (ARMING_FLAG(ARMED)) {
@@ -544,7 +544,7 @@ void processServoAutotrimMode(void)
 void processContinuousServoAutotrim(const float dT)
 {
     static timeMs_t lastUpdateTimeMs;
-    static servoAutotrimState_e trimState = AUTOTRIM_IDLE;    
+    static servoAutotrimState_e trimState = AUTOTRIM_IDLE;
     static uint32_t servoMiddleUpdateCount;
 
     const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, fast_fsqrtf(vectorNormSquared(&imuMeasuredRotationBF)), SERVO_AUTOTRIM_FILTER_CUTOFF, dT);
@@ -556,16 +556,16 @@ void processContinuousServoAutotrim(const float dT)
             const bool planeIsFlyingStraight = rotRateMagnitudeFiltered <= DEGREES_TO_RADIANS(servoConfig()->servo_autotrim_rotation_limit);
             const bool noRotationCommanded = targetRateMagnitudeFiltered <= servoConfig()->servo_autotrim_rotation_limit;
             const bool sticksAreCentered = !areSticksDeflected();
-            const bool planeIsFlyingLevel = ABS(attitude.values.pitch + DEGREES_TO_DECIDEGREES(getFixedWingLevelTrim())) <= SERVO_AUTOTRIM_ATTITUDE_LIMIT 
+            const bool planeIsFlyingLevel = ABS(attitude.values.pitch + DEGREES_TO_DECIDEGREES(getFixedWingLevelTrim())) <= SERVO_AUTOTRIM_ATTITUDE_LIMIT
                                             && ABS(attitude.values.roll) <= SERVO_AUTOTRIM_ATTITUDE_LIMIT;
             if (
-                planeIsFlyingStraight && 
-                noRotationCommanded && 
+                planeIsFlyingStraight &&
+                noRotationCommanded &&
                 planeIsFlyingLevel &&
                 sticksAreCentered &&
-                !FLIGHT_MODE(MANUAL_MODE) && 
+                !FLIGHT_MODE(MANUAL_MODE) &&
                 isGPSHeadingValid() // TODO: proper flying detection
-            ) { 
+            ) {
                 // Plane is flying straight and level: trim servos
                 for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
                     // For each stabilized axis, add 5 units of I-term to all associated servo midpoints
@@ -610,7 +610,7 @@ void processContinuousServoAutotrim(const float dT)
     DEBUG_SET(DEBUG_AUTOTRIM, 1, servoMiddleUpdateCount);
     DEBUG_SET(DEBUG_AUTOTRIM, 3, MAX(RADIANS_TO_DEGREES(rotRateMagnitudeFiltered), targetRateMagnitudeFiltered));
     DEBUG_SET(DEBUG_AUTOTRIM, 5, axisPID_I[FD_ROLL]);
-    DEBUG_SET(DEBUG_AUTOTRIM, 7, axisPID_I[FD_PITCH]);    
+    DEBUG_SET(DEBUG_AUTOTRIM, 7, axisPID_I[FD_PITCH]);
 }
 
 void processServoAutotrim(const float dT) {
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 552687a4c62..207b3cb73ec 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -5632,10 +5632,10 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
 
     if (buff != NULL) {
         const char *message = NULL;
-        /* Warning: messageBuf is shared, use accordingly */
+        /* WARNING: messageBuf is shared, use accordingly */
         char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH + 1)];
 
-        /* Warning, ensure number of messages returned does not exceed messages array size
+        /* WARNING: ensure number of messages returned does not exceed messages array size
          * Messages array set 1 larger than maximum expected message count of 6 */
         const char *messages[7];
         unsigned messageCount = 0;
@@ -5647,7 +5647,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
             if (STATE(LANDING_DETECTED)) {
                 messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_LANDED);
             } else if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
-                /* ADDS MAXIMUM OF 5 MESSAGES */
+                /* ADDS MAXIMUM OF 5 MESSAGES TO TOTAL */
                 if (navGetCurrentStateFlags() & NAV_AUTO_WP_DONE) {
                     messages[messageCount++] = STATE(LANDING_DETECTED) ? OSD_MESSAGE_STR(OSD_MSG_WP_LANDED) : OSD_MESSAGE_STR(OSD_MSG_WP_FINISHED);
                 } else if (NAV_Status.state == MW_NAV_STATE_WP_ENROUTE) {
@@ -5704,8 +5704,8 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                     messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_WP_RTH_CANCEL);
                 }
             } else {    /* Messages shown only when Failsafe, WP, RTH or Emergency Landing not active */
-                /* ADDS MAXIMUM OF 4 MESSAGES */
-                if (STATE(AIRPLANE)) {      /* ADDS MAXIMUM OF 3 MESSAGES */
+                /* ADDS MAXIMUM OF 4 MESSAGES TO TOTAL */
+                if (STATE(AIRPLANE)) {      /* ADDS MAXIMUM OF 3 MESSAGES TO TOTAL */
 #ifdef USE_FW_AUTOLAND
                     if (canFwLandingBeCancelled()) {
                          messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MOVE_STICKS);
@@ -5720,30 +5720,15 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                         }
                     } else if (FLIGHT_MODE(SOARING_MODE)) {
                         messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_NAV_SOARING);
-                    } else {
-                        strcpy(messageBuf, "(AUTO-");
-                        if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM) && !feature(FEATURE_FW_AUTOTRIM)) {
-                            strcat(messageBuf, OSD_MSG_AUTOTRIM);
-                        }
-                        if (isFixedWingLevelTrimActive()) {
-                            if (messageBuf[6] != '\0') {
-                                strcat(messageBuf, ":");
-                            }
-                            strcat(messageBuf, OSD_MSG_AUTOLEVEL);
-                        }
-                        if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) {
-                            if (messageBuf[6] != '\0') {
-                                strcat(messageBuf, ":");
-                            }
-                            strcat(messageBuf, OSD_MSG_AUTOTUNE);
-                            if (FLIGHT_MODE(MANUAL_MODE)) {
-                                messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTUNE_ACRO);
-                            }
-                        }
-                        if (messageBuf[6] != '\0') {
-                            strcat(messageBuf, ")");
-                            messages[messageCount++] = messageBuf;
+                    } else if (isFwAutoModeActive(BOXAUTOTUNE)) {
+                        messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTUNE);
+                        if (FLIGHT_MODE(MANUAL_MODE)) {
+                            messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTUNE_ACRO);
                         }
+                    } else if (isFwAutoModeActive(BOXAUTOTRIM) && !feature(FEATURE_FW_AUTOTRIM)) {
+                        messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTRIM);
+                    } else if (isFixedWingLevelTrimActive()) {
+                        messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOLEVEL);
                     }
 
                     if (IS_RC_MODE_ACTIVE(BOXANGLEHOLD)) {
@@ -5756,7 +5741,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                             messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ANGLEHOLD_PITCH);
                         }
                     }
-                } else if (STATE(MULTIROTOR)) {     /* ADDS MAXIMUM OF 2 MESSAGES */
+                } else if (STATE(MULTIROTOR)) {     /* ADDS MAXIMUM OF 2 MESSAGES TO TOTAL */
                     if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
                         if (posControl.cruise.multicopterSpeed >= 50.0f) {
                             char buf[6];
@@ -5782,7 +5767,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                     messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MISSION_PLANNER);
                 }
             }
-        } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) {    /* ADDS MAXIMUM OF 2 MESSAGES */
+        } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) {    /* ADDS MAXIMUM OF 2 MESSAGES TO TOTAL */
             unsigned invalidIndex;
 
             // Check if we're unable to arm for some reason
@@ -5809,7 +5794,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
             }
         }
 
-        /* Messages that are shown regardless of Arming state - ADDS MAXIMUM OF 1 MESSAGES */
+        /* Messages that are shown regardless of Arming state - ADDS MAXIMUM OF 1 MESSAGES TO TOTAL */
 
         // The following has been commented out as it will be added in #9688
         // uint16_t rearmMs = (emergInflightRearmEnabled()) ? emergencyInFlightRearmTimeMS() : 0;
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index 3a8f0b98403..ca5dca66137 100644
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -109,10 +109,10 @@
 #define OSD_MSG_AUTOLAUNCH          "AUTOLAUNCH"
 #define OSD_MSG_AUTOLAUNCH_MANUAL   "AUTOLAUNCH (MANUAL)"
 #define OSD_MSG_ALTITUDE_HOLD       "(ALTITUDE HOLD)"
-#define OSD_MSG_AUTOTRIM            "TRIM"
-#define OSD_MSG_AUTOTUNE            "TUNE"
+#define OSD_MSG_AUTOTRIM            "(AUTOTRIM)"
+#define OSD_MSG_AUTOTUNE            "(AUTOTUNE)"
 #define OSD_MSG_AUTOTUNE_ACRO       "SWITCH TO ACRO"
-#define OSD_MSG_AUTOLEVEL           "LEVELTRIM"
+#define OSD_MSG_AUTOLEVEL           "(AUTO LEVEL TRIM)"
 #define OSD_MSG_HEADFREE            "(HEADFREE)"
 #define OSD_MSG_NAV_SOARING         "(SOARING)"
 #define OSD_MSG_UNABLE_ARM          "UNABLE TO ARM"

From 06eb0ef3c04135df62ae42ac87e9f1d9c9acd387 Mon Sep 17 00:00:00 2001
From: Fernando Schuindt <f.schuindtcs@gmail.com>
Date: Wed, 29 May 2024 01:23:33 -0300
Subject: [PATCH 149/323] Fix OSD mentioned twice on README.md feature list

---
 readme.md | 1 -
 1 file changed, 1 deletion(-)

diff --git a/readme.md b/readme.md
index a70c42d4c86..b8e9f21105d 100644
--- a/readme.md
+++ b/readme.md
@@ -51,7 +51,6 @@ Fly safe, fly smart with INAV 7.1 and a compass by your side!
 * SmartAudio and IRC Tramp VTX support
 * Telemetry: SmartPort, FPort, MAVlink, LTM, CRSF
 * Multi-color RGB LED Strip support
-* On Screen Display (OSD) - both character and pixel style
 * And many more!
 
 For a list of features, changes and some discussion please review consult the releases [page](https://github.com/iNavFlight/inav/releases) and the documentation.

From 2667c03850189242486e5554ae62bea741a797a3 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Thu, 30 May 2024 13:03:25 +0100
Subject: [PATCH 150/323] update flight modes

---
 src/main/blackbox/blackbox.c | 57 +++++++++++++++++++-----------------
 src/main/fc/fc_core.c        |  2 +-
 src/main/flight/pid.c        |  4 +--
 3 files changed, 33 insertions(+), 30 deletions(-)

diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index 4433e7b4aba..419b7b4add4 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -402,15 +402,22 @@ static const blackboxSimpleFieldDefinition_t blackboxGpsHFields[] = {
 
 // Rarely-updated fields
 static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = {
-    {"activeWpNumber",        -1, UNSIGNED, PREDICT(0),      ENCODING(UNSIGNED_VB)},    
+    /* "flightModeFlags" renamed internally to more correct ref of rcModeFlags, since it logs rc boxmode selections,
+     * but name kept for external compatibility reasons.
+     * "activeFlightModeFlags" logs actual active flight modes rather than rc boxmodes.
+     * 'active' should at least distinguish it from the existing "flightModeFlags" */
+     
+    {"activeWpNumber",        -1, UNSIGNED, PREDICT(0),      ENCODING(UNSIGNED_VB)},
     {"flightModeFlags",       -1, UNSIGNED, PREDICT(0),      ENCODING(UNSIGNED_VB)},
+    {"flightModeFlags2",      -1, UNSIGNED, PREDICT(0),      ENCODING(UNSIGNED_VB)},
+    {"activeFlightModeFlags", -1, UNSIGNED, PREDICT(0),      ENCODING(UNSIGNED_VB)},
     {"stateFlags",            -1, UNSIGNED, PREDICT(0),      ENCODING(UNSIGNED_VB)},
 
     {"failsafePhase",         -1, UNSIGNED, PREDICT(0),      ENCODING(TAG2_3S32)},
     {"rxSignalReceived",      -1, UNSIGNED, PREDICT(0),      ENCODING(TAG2_3S32)},
     {"rxFlightChannelsValid", -1, UNSIGNED, PREDICT(0),      ENCODING(TAG2_3S32)},
     {"rxUpdateRate",          -1, UNSIGNED, PREDICT(PREVIOUS),      ENCODING(UNSIGNED_VB)},
-    
+
     {"hwHealthStatus",        -1, UNSIGNED, PREDICT(0),      ENCODING(UNSIGNED_VB)},
     {"powerSupplyImpedance",  -1, UNSIGNED, PREDICT(0),      ENCODING(UNSIGNED_VB)},
     {"sagCompensatedVBat",    -1, UNSIGNED, PREDICT(0),      ENCODING(UNSIGNED_VB)},
@@ -533,7 +540,9 @@ typedef struct blackboxGpsState_s {
 
 // This data is updated really infrequently:
 typedef struct blackboxSlowState_s {
-    uint32_t flightModeFlags; // extend this data size (from uint16_t)
+    uint32_t rcModeFlags;
+    uint32_t rcModeFlags2;
+    uint32_t activeFlightModeFlags;
     uint32_t stateFlags;
     uint8_t failsafePhase;
     bool rxSignalReceived;
@@ -566,7 +575,7 @@ extern boxBitmask_t rcModeActivationMask;
 static BlackboxState blackboxState = BLACKBOX_STATE_DISABLED;
 
 static uint32_t blackboxLastArmingBeep = 0;
-static uint32_t blackboxLastFlightModeFlags = 0;
+static uint32_t blackboxLastRcModeFlags = 0;
 
 static struct {
     uint32_t headerIndex;
@@ -1259,9 +1268,11 @@ static void writeSlowFrame(void)
     int32_t values[3];
 
     blackboxWrite('S');
-    
+
     blackboxWriteUnsignedVB(slowHistory.activeWpNumber);
-    blackboxWriteUnsignedVB(slowHistory.flightModeFlags);
+    blackboxWriteUnsignedVB(slowHistory.rcModeFlags);
+    blackboxWriteUnsignedVB(slowHistory.rcModeFlags2);
+    blackboxWriteUnsignedVB(slowHistory.activeFlightModeFlags);
     blackboxWriteUnsignedVB(slowHistory.stateFlags);
 
     /*
@@ -1269,10 +1280,10 @@ static void writeSlowFrame(void)
      */
     values[0] = slowHistory.failsafePhase;
     values[1] = slowHistory.rxSignalReceived ? 1 : 0;
-    values[2] = slowHistory.rxFlightChannelsValid ? 1 : 0;    
+    values[2] = slowHistory.rxFlightChannelsValid ? 1 : 0;
     blackboxWriteTag2_3S32(values);
 
-    blackboxWriteUnsignedVB(slowHistory.rxUpdateRate);    
+    blackboxWriteUnsignedVB(slowHistory.rxUpdateRate);
 
     blackboxWriteUnsignedVB(slowHistory.hwHealthStatus);
 
@@ -1309,28 +1320,20 @@ static void writeSlowFrame(void)
 static void loadSlowState(blackboxSlowState_t *slow)
 {
     slow->activeWpNumber = getActiveWpNumber();
-    memcpy(&slow->flightModeFlags, &rcModeActivationMask, sizeof(slow->flightModeFlags)); //was flightModeFlags;
+
+    slow->rcModeFlags = rcModeActivationMask.bits[0];   // first 32 bits of boxId_e
+    slow->rcModeFlags2 = rcModeActivationMask.bits[1];  // remaining bits of boxId_e
+
     // Also log Nav auto enabled flight modes rather than just those selected by boxmode
-    if (FLIGHT_MODE(ANGLE_MODE)) {
-        slow->flightModeFlags |= (1 << BOXANGLE);
-    }
-    if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) {
-        slow->flightModeFlags |= (1 << BOXNAVALTHOLD);
-    }
-    if (FLIGHT_MODE(NAV_RTH_MODE)) {
-        slow->flightModeFlags |= (1 << BOXNAVRTH);
-    }
     if (navigationGetHeadingControlState() == NAV_HEADING_CONTROL_AUTO) {
-        slow->flightModeFlags |= (1 << BOXHEADINGHOLD);
-    }
-    if (navigationRequiresTurnAssistance()) {
-        slow->flightModeFlags |= (1 << BOXTURNASSIST);
+        slow->rcModeFlags |= (1 << BOXHEADINGHOLD);
     }
+    slow->activeFlightModeFlags = flightModeFlags;
     slow->stateFlags = stateFlags;
     slow->failsafePhase = failsafePhase();
     slow->rxSignalReceived = rxIsReceivingSignal();
     slow->rxFlightChannelsValid = rxAreFlightChannelsValid();
-    slow->rxUpdateRate = getRcUpdateFrequency();    
+    slow->rxUpdateRate = getRcUpdateFrequency();
     slow->hwHealthStatus = (getHwGyroStatus()           << 2 * 0) |     // Pack hardware health status into a bit field.
                            (getHwAccelerometerStatus()  << 2 * 1) |     // Use raw hardwareSensorStatus_e values and pack them using 2 bits per value
                            (getHwCompassStatus()        << 2 * 2) |     // Report GYRO in 2 lowest bits, then ACC, COMPASS, BARO, GPS, RANGEFINDER and PITOT
@@ -1498,7 +1501,7 @@ void blackboxStart(void)
      * it finally plays the beep for this arming event.
      */
     blackboxLastArmingBeep = getArmingBeepTimeMicros();
-    memcpy(&blackboxLastFlightModeFlags, &rcModeActivationMask, sizeof(blackboxLastFlightModeFlags)); // record startup status
+    memcpy(&blackboxLastRcModeFlags, &rcModeActivationMask, sizeof(blackboxLastRcModeFlags)); // record startup status
 
     blackboxSetState(BLACKBOX_STATE_PREPARE_LOG_FILE);
 }
@@ -2023,10 +2026,10 @@ static void blackboxCheckAndLogArmingBeep(void)
 static void blackboxCheckAndLogFlightMode(void)
 {
     // Use != so that we can still detect a change if the counter wraps
-    if (memcmp(&rcModeActivationMask, &blackboxLastFlightModeFlags, sizeof(blackboxLastFlightModeFlags))) {
+    if (memcmp(&rcModeActivationMask, &blackboxLastRcModeFlags, sizeof(blackboxLastRcModeFlags))) {
         flightLogEvent_flightMode_t eventData; // Add new data for current flight mode flags
-        eventData.lastFlags = blackboxLastFlightModeFlags;
-        memcpy(&blackboxLastFlightModeFlags, &rcModeActivationMask, sizeof(blackboxLastFlightModeFlags));
+        eventData.lastFlags = blackboxLastRcModeFlags;
+        memcpy(&blackboxLastRcModeFlags, &rcModeActivationMask, sizeof(blackboxLastRcModeFlags));
         memcpy(&eventData.flags, &rcModeActivationMask, sizeof(eventData.flags));
         blackboxLogEvent(FLIGHT_LOG_EVENT_FLIGHTMODE, (flightLogEventData_t *)&eventData);
     }
diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index 4b231548b99..d96c198f83d 100644
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -704,7 +704,7 @@ void processRx(timeUs_t currentTimeUs)
     }
 
     /* Turn assistant mode */
-    if (IS_RC_MODE_ACTIVE(BOXTURNASSIST)) {
+    if (IS_RC_MODE_ACTIVE(BOXTURNASSIST) || navigationRequiresTurnAssistance()) {
          ENABLE_FLIGHT_MODE(TURN_ASSISTANT);
     } else {
         DISABLE_FLIGHT_MODE(TURN_ASSISTANT);
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index a9d1291877f..cb6502b0107 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -1099,7 +1099,7 @@ void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis
     if (usedPidControllerType == PID_TYPE_PIFF && pidProfile()->fixedWingYawItermBankFreeze != 0 && axis == FD_YAW) {
         // Do not allow yaw I-term to grow when bank angle is too large
         float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll);
-        if (fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankFreeze && !(FLIGHT_MODE(AUTO_TUNE) || FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance())){
+        if (fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankFreeze && !(FLIGHT_MODE(AUTO_TUNE) || FLIGHT_MODE(TURN_ASSISTANT))) {
             pidState->itermFreezeActive = true;
         } else
         {
@@ -1242,7 +1242,7 @@ void FAST_CODE pidController(float dT)
     }
 
     // Apply Turn Assistance
-    if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
+    if (FLIGHT_MODE(TURN_ASSISTANT) && (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);

From 5191ec369485f4d4941cf96ac8143595863d8dbf Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Fri, 31 May 2024 09:56:39 +0200
Subject: [PATCH 151/323] Update readme.md

Fix small typo that sneaked in.
---
 readme.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/readme.md b/readme.md
index c18b029563c..fac247ef9ab 100644
--- a/readme.md
+++ b/readme.md
@@ -8,7 +8,7 @@
 
 # ICM426xx IMUs PSA
 
-> The filtering settings for the ICM426xx has changed to matche what is used by Ardupilot and Betaflight in INAV 7.1. When upgrading from older versions you may need to recalibrate the Accelerometer and if you are not using INAV's default tune you may also want to check if the tune is still good.
+> The filtering settings for the ICM426xx has changed to match what is used by Ardupilot and Betaflight in INAV 7.1. When upgrading from older versions you may need to recalibrate the Accelerometer and if you are not using INAV's default tune you may also want to check if the tune is still good.
 
 ![INAV](http://static.rcgroups.net/forums/attachments/6/1/0/3/7/6/a9088858-102-inav.png)
 

From 5a3e21dfbed2d91cf0f09fcd6ebf6b64b5443715 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Fri, 31 May 2024 16:41:39 +0200
Subject: [PATCH 152/323] Add extra infor about special labels

---
 src/main/drivers/pwm_mapping.h |  5 +++++
 src/main/fc/fc_msp.c           | 23 +++++++++++++++--------
 2 files changed, 20 insertions(+), 8 deletions(-)

diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h
index e9b3a0f9573..cfb96afadbf 100644
--- a/src/main/drivers/pwm_mapping.h
+++ b/src/main/drivers/pwm_mapping.h
@@ -53,6 +53,11 @@ typedef enum {
     SERVO_TYPE_SBUS_PWM
 } servoProtocolType_e;
 
+typedef enum {
+    PIN_LABEL_NONE = 0,
+    PIN_LABEL_LED
+} pinLabel_e;
+
 typedef enum {
     PWM_INIT_ERROR_NONE = 0,
     PWM_INIT_ERROR_TOO_MANY_MOTORS,
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 75e18ef3681..2398b0d4157 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -1589,15 +1589,22 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
             }
         break;
     case MSP2_INAV_OUTPUT_MAPPING_EXT2:
-        for (uint8_t i = 0; i < timerHardwareCount; ++i)
-            if (!(timerHardware[i].usageFlags & (TIM_USE_PPM | TIM_USE_PWM))) {
-                #if defined(SITL_BUILD)
-                sbufWriteU8(dst, i);
-                #else
-                sbufWriteU8(dst, timer2id(timerHardware[i].tim));
-                #endif
-                sbufWriteU32(dst, timerHardware[i].usageFlags);
+        {
+            ioTag_t led_tag = IO_TAG(WS2811_PIN);
+            for (uint8_t i = 0; i < timerHardwareCount; ++i)
+
+                if (!(timerHardware[i].usageFlags & (TIM_USE_PPM | TIM_USE_PWM))) {
+                    #if defined(SITL_BUILD)
+                    sbufWriteU8(dst, i);
+                    #else
+                    sbufWriteU8(dst, timer2id(timerHardware[i].tim));
+                    #endif
+                    sbufWriteU32(dst, timerHardware[i].usageFlags);
+                    // Extra label to help identify repurposed PINs.
+                    // Eventually, we can try to add more labels for PPM pins, etc.
+                    sbufWriteU8(dst, timerHardware[i].tag == led_tag ? PIN_LABEL_LED : PIN_LABEL_NONE);
             }
+        }
         break;
     
 

From 0b50f5519efe77a0aa51ca6bad35aba20330e80a Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Fri, 31 May 2024 17:33:37 +0200
Subject: [PATCH 153/323] Fix sitl build

---
 src/main/fc/fc_msp.c | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 2398b0d4157..44fefaa378a 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -1600,9 +1600,13 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
                     sbufWriteU8(dst, timer2id(timerHardware[i].tim));
                     #endif
                     sbufWriteU32(dst, timerHardware[i].usageFlags);
+                    #if defined(SITIL_BUILD)
+                    sbufWriteU8(dst, 0);
+                    #else
                     // Extra label to help identify repurposed PINs.
                     // Eventually, we can try to add more labels for PPM pins, etc.
                     sbufWriteU8(dst, timerHardware[i].tag == led_tag ? PIN_LABEL_LED : PIN_LABEL_NONE);
+                    #endif
             }
         }
         break;

From d78aa93c234183d34e908b3f1aafc2478b5951c4 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Fri, 31 May 2024 17:37:25 +0200
Subject: [PATCH 154/323] sitl can be a pain. One more fix :)

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

diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 44fefaa378a..5deb8efafb2 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -1590,7 +1590,9 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         break;
     case MSP2_INAV_OUTPUT_MAPPING_EXT2:
         {
+            #if defined(SITIL_BUILD)
             ioTag_t led_tag = IO_TAG(WS2811_PIN);
+            #endif
             for (uint8_t i = 0; i < timerHardwareCount; ++i)
 
                 if (!(timerHardware[i].usageFlags & (TIM_USE_PPM | TIM_USE_PWM))) {

From ad151352bd5c908029030237c6c51aa7ad0c900e Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Fri, 31 May 2024 17:41:41 +0200
Subject: [PATCH 155/323] s/defined/!defined/g

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

diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 5deb8efafb2..17b51f037a3 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -1590,7 +1590,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         break;
     case MSP2_INAV_OUTPUT_MAPPING_EXT2:
         {
-            #if defined(SITIL_BUILD)
+            #if !defined(SITIL_BUILD)
             ioTag_t led_tag = IO_TAG(WS2811_PIN);
             #endif
             for (uint8_t i = 0; i < timerHardwareCount; ++i)

From d33488d91a343411220242ca74280c5101a5494b Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Fri, 31 May 2024 17:45:26 +0200
Subject: [PATCH 156/323] Now, tripple checked. ;)

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

diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 17b51f037a3..f70c00f221b 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -1590,7 +1590,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         break;
     case MSP2_INAV_OUTPUT_MAPPING_EXT2:
         {
-            #if !defined(SITIL_BUILD)
+            #if !defined(SITL_BUILD)
             ioTag_t led_tag = IO_TAG(WS2811_PIN);
             #endif
             for (uint8_t i = 0; i < timerHardwareCount; ++i)
@@ -1602,7 +1602,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
                     sbufWriteU8(dst, timer2id(timerHardware[i].tim));
                     #endif
                     sbufWriteU32(dst, timerHardware[i].usageFlags);
-                    #if defined(SITIL_BUILD)
+                    #if defined(SITL_BUILD)
                     sbufWriteU8(dst, 0);
                     #else
                     // Extra label to help identify repurposed PINs.

From e88b85f4ea4e4eaf4e56d119de3d73ba66d45544 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Fri, 31 May 2024 18:12:42 +0200
Subject: [PATCH 157/323] Make sure LED is at the end of the PWM list

---
 src/main/target/AIRBOTF7/target.c              |  6 +++---
 src/main/target/ANYFCM7/target.c               | 15 ++++++++-------
 src/main/target/BLUEJAYF4/target.c             |  4 ++--
 src/main/target/F4BY/target.c                  | 18 ++++++++++--------
 src/main/target/FOXEERF745AIO/target.c         |  4 ++--
 src/main/target/FRSKYPILOT/target.c            |  4 ----
 src/main/target/IFLIGHT_BLITZ_ATF435/target.c  |  2 +-
 src/main/target/MATEKF405/target.c             |  2 +-
 src/main/target/NEUTRONRCF435MINI/target.c     |  6 +++---
 src/main/target/NEUTRONRCF435WING/target.c     |  3 +--
 src/main/target/OMNIBUSF4/CMakeLists.txt       |  1 -
 src/main/target/OMNIBUSF4/target.c             |  7 ++-----
 src/main/target/SAGEATF4/target.c              | 12 ++++++------
 .../target/SPEEDYBEEF405MINI/CMakeLists.txt    |  1 -
 src/main/target/SPEEDYBEEF405MINI/target.c     | 11 +++--------
 src/main/target/SPRACINGF7DUAL/target.c        |  4 +++-
 src/main/target/TAKERF722SE/target.c           |  2 +-
 src/main/target/TMOTORF7V2/target.c            |  2 +-
 src/main/target/TMOTORVELOXF7V2/target.c       |  2 +-
 19 files changed, 48 insertions(+), 58 deletions(-)

diff --git a/src/main/target/AIRBOTF7/target.c b/src/main/target/AIRBOTF7/target.c
index 22d2564821a..6cad05c41fd 100644
--- a/src/main/target/AIRBOTF7/target.c
+++ b/src/main/target/AIRBOTF7/target.c
@@ -42,13 +42,13 @@ BUSDEV_REGISTER_SPI_TAG(busdev_imu1_mpu6500,    DEVHW_MPU6500,  GYRO_1_SPI_BUS,
 #endif
 
 timerHardware_t timerHardware[] = {
-    DEF_TIM(TIM2,  CH1, PA15, TIM_USE_LED,                   0, 0), // LED
-    DEF_TIM(TIM1,  CH1, PA8,  TIM_USE_ANY,                   0, 0), // Cam control, SS, UNUSED
-
     DEF_TIM(TIM8,  CH3, PC8,  TIM_USE_OUTPUT_AUTO, 0, 0), //S1
     DEF_TIM(TIM4,  CH1, PB6,  TIM_USE_OUTPUT_AUTO, 0, 0), //S2
     DEF_TIM(TIM8,  CH4, PC9,  TIM_USE_OUTPUT_AUTO, 0, 0), //S3
     DEF_TIM(TIM4,  CH2, PB7,  TIM_USE_OUTPUT_AUTO, 0, 0), //S4
+							  //
+    DEF_TIM(TIM2,  CH1, PA15, TIM_USE_LED,                   0, 0), // LED
+    DEF_TIM(TIM1,  CH1, PA8,  TIM_USE_ANY,                   0, 0), // Cam control, SS, UNUSED
 };
 
 const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
diff --git a/src/main/target/ANYFCM7/target.c b/src/main/target/ANYFCM7/target.c
index 7dd30d2250b..aae6922a374 100644
--- a/src/main/target/ANYFCM7/target.c
+++ b/src/main/target/ANYFCM7/target.c
@@ -26,12 +26,6 @@
 #define TIM_EN_N    TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_N_CHANNEL
 
 timerHardware_t timerHardware[] = {
-    // DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM,   0, 0 ), // S1_IN
-    // DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM,                 0, 0 ), // S2_IN
-    // DEF_TIM(TIM8,  CH1, PC6,  TIM_USE_PWM,                 0, 0 ), // S3_IN DMA2_ST2 DMA2_ST2
-    // DEF_TIM(TIM8,  CH2, PC7,  TIM_USE_PWM,                 0, 0 ), // S4_IN DMA2_ST3 DMA2_ST2
-    // DEF_TIM(TIM8,  CH3, PC8,  TIM_USE_PWM,                 0, 0 ), // S5_IN DMA2_ST4 DMA2_ST2
-    // DEF_TIM(TIM8,  CH4, PC9,  TIM_USE_PWM,                 0, 0 ), // S6_IN DMA2_ST7
 
     DEF_TIM(TIM4,  CH3,  PB8,  TIM_USE_OUTPUT_AUTO,               0, 0 ), // S10_OUT 1 DMA1_ST7
     DEF_TIM(TIM2,  CH3,  PA2,  TIM_USE_OUTPUT_AUTO,               0, 0 ), // S6_OUT  2 DMA1_ST1
@@ -39,10 +33,17 @@ timerHardware_t timerHardware[] = {
     DEF_TIM(TIM2,  CH4,  PA3,  TIM_USE_OUTPUT_AUTO,               0, 1 ), // S1_OUT  4 DMA1_ST7 DMA1_ST6
     DEF_TIM(TIM5,  CH2,  PA1,  TIM_USE_OUTPUT_AUTO,               0, 0 ), // S2_OUT DMA1_ST4
     DEF_TIM(TIM1,  CH2N, PB0,  TIM_USE_OUTPUT_AUTO,               0, 1 ), // S3_OUT DMA2_ST6 DMA2_ST2
-    DEF_TIM(TIM3,  CH2,  PB5,  TIM_USE_OUTPUT_AUTO | TIM_USE_LED, 0, 0 ), // S4_OUT DMA1_ST5
+    DEF_TIM(TIM3,  CH2,  PB5,  TIM_USE_OUTPUT_AUTO,               0, 0 ), // S4_OUT DMA1_ST5 -- Used to have TIM_USE_LED
     DEF_TIM(TIM5,  CH1,  PA0,  TIM_USE_OUTPUT_AUTO,               0, 0 ), // S7_OUT DMA1_ST2
     DEF_TIM(TIM1,  CH3N, PB1,  TIM_USE_OUTPUT_AUTO,               0, 0 ), // S8_OUT DMA2_ST6
     DEF_TIM(TIM3,  CH1,  PB4,  TIM_USE_OUTPUT_AUTO,               0, 0 ), // S9_OUT DMA1_ST4
+
+    // DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM,   0, 0 ), // S1_IN
+    // DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM,                 0, 0 ), // S2_IN
+    // DEF_TIM(TIM8,  CH1, PC6,  TIM_USE_PWM,                 0, 0 ), // S3_IN DMA2_ST2 DMA2_ST2
+    // DEF_TIM(TIM8,  CH2, PC7,  TIM_USE_PWM,                 0, 0 ), // S4_IN DMA2_ST3 DMA2_ST2
+    // DEF_TIM(TIM8,  CH3, PC8,  TIM_USE_PWM,                 0, 0 ), // S5_IN DMA2_ST4 DMA2_ST2
+    // DEF_TIM(TIM8,  CH4, PC9,  TIM_USE_PWM,                 0, 0 ), // S6_IN DMA2_ST7
 };
 
 const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
diff --git a/src/main/target/BLUEJAYF4/target.c b/src/main/target/BLUEJAYF4/target.c
index ba48551f76c..37afb69b1ce 100644
--- a/src/main/target/BLUEJAYF4/target.c
+++ b/src/main/target/BLUEJAYF4/target.c
@@ -29,8 +29,8 @@ timerHardware_t timerHardware[] = {
     DEF_TIM(TIM5, CH2,  PA1, TIM_USE_OUTPUT_AUTO,               0, 0 ), // S2_OUT - DMA1_ST4
     DEF_TIM(TIM2, CH3,  PA2, TIM_USE_OUTPUT_AUTO,               0, 0 ), // S3_OUT - DMA1_ST1
     DEF_TIM(TIM9, CH2,  PA3, TIM_USE_OUTPUT_AUTO,               0, 0 ), // S4_OUT - no DMA
-    //DEF_TIM(TIM5, CH4,  PA3, TIM_USE_OUTPUT_AUTO,               0, 1 ), // S4_OUT - DMA1_ST3 (Could be DMA1_ST1 with dmaopt=0)
-    DEF_TIM(TIM3, CH4,  PB0, TIM_USE_OUTPUT_AUTO | TIM_USE_LED, 0, 0 ), // S5_OUT - DMA1_ST2
+    //DEF_TIM(TIM5, CH4,  PA3, TIM_USE_OUTPUT_AUTO,             0, 1 ), // S4_OUT - DMA1_ST3 (Could be DMA1_ST1 with dmaopt=0)
+    DEF_TIM(TIM3, CH4,  PB0, TIM_USE_OUTPUT_AUTO, 		0, 0 ), // S5_OUT - DMA1_ST2 -- used to be TIM_USE_LED
     DEF_TIM(TIM3, CH3,  PB1, TIM_USE_OUTPUT_AUTO,               0, 0 ), // S6_OUT - DMA1_ST7
 };
 
diff --git a/src/main/target/F4BY/target.c b/src/main/target/F4BY/target.c
index 8d1b4622f69..b1654e27ec0 100644
--- a/src/main/target/F4BY/target.c
+++ b/src/main/target/F4BY/target.c
@@ -6,14 +6,6 @@
 #include "drivers/timer.h"
 
 timerHardware_t timerHardware[] = {
-    // DEF_TIM(TIM3, CH4, PC9,  TIM_USE_PPM | TIM_USE_PWM,   0, 0), // S1_IN
-    // DEF_TIM(TIM3, CH3, PC8,  TIM_USE_PWM,   0, 0), // S2_IN
-    // DEF_TIM(TIM3, CH1, PC6,  TIM_USE_PWM,   0, 0), // S3_IN
-    // DEF_TIM(TIM3, CH2, PC7,  TIM_USE_PWM,   0, 0), // S4_IN
-    // DEF_TIM(TIM4, CH4, PD15, TIM_USE_PWM,   0, 0), // S5_IN
-    // DEF_TIM(TIM4, CH3, PD14, TIM_USE_PWM,   0, 0), // S6_IN
-    // DEF_TIM(TIM4, CH2, PD13, TIM_USE_PWM,   0, 0), // S7_IN
-    // DEF_TIM(TIM4, CH1, PD12, TIM_USE_PWM,   0, 0), // S8_IN
 
     DEF_TIM(TIM2, CH1, PA0,  TIM_USE_OUTPUT_AUTO, 0, 0), // S1_OUT
     DEF_TIM(TIM2, CH2, PA1,  TIM_USE_OUTPUT_AUTO, 0, 0), // S2_OUT
@@ -25,6 +17,16 @@ timerHardware_t timerHardware[] = {
     DEF_TIM(TIM1, CH4, PE14, TIM_USE_OUTPUT_AUTO, 0, 0), // S8_OUT
 
     DEF_TIM(TIM9, CH2, PE6,  TIM_USE_ANY, 0, 0), // sonar echo if needed
+
+
+    // DEF_TIM(TIM3, CH4, PC9,  TIM_USE_PPM | TIM_USE_PWM,   0, 0), // S1_IN
+    // DEF_TIM(TIM3, CH3, PC8,  TIM_USE_PWM,   0, 0), // S2_IN
+    // DEF_TIM(TIM3, CH1, PC6,  TIM_USE_PWM,   0, 0), // S3_IN
+    // DEF_TIM(TIM3, CH2, PC7,  TIM_USE_PWM,   0, 0), // S4_IN
+    // DEF_TIM(TIM4, CH4, PD15, TIM_USE_PWM,   0, 0), // S5_IN
+    // DEF_TIM(TIM4, CH3, PD14, TIM_USE_PWM,   0, 0), // S6_IN
+    // DEF_TIM(TIM4, CH2, PD13, TIM_USE_PWM,   0, 0), // S7_IN
+    // DEF_TIM(TIM4, CH1, PD12, TIM_USE_PWM,   0, 0), // S8_IN
 };
 
 const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
diff --git a/src/main/target/FOXEERF745AIO/target.c b/src/main/target/FOXEERF745AIO/target.c
index f724f65913c..382306a8d50 100644
--- a/src/main/target/FOXEERF745AIO/target.c
+++ b/src/main/target/FOXEERF745AIO/target.c
@@ -25,12 +25,12 @@
 
 timerHardware_t timerHardware[] = {
 
-    DEF_TIM(TIM1,  CH1, PA8, TIM_USE_LED, 0, 0),      // LED STRIP - D(2, 6, 0)
-
     DEF_TIM(TIM3, CH1, PB4,  TIM_USE_MOTOR, 0, 0), // M1 - D(1, 4, 5)
     DEF_TIM(TIM3, CH2, PB5,  TIM_USE_MOTOR, 0, 0), // M2 - D(1, 5, 5)
     DEF_TIM(TIM3, CH4, PB1,  TIM_USE_MOTOR, 0, 0), // M3 - D(1, 2, 5)
     DEF_TIM(TIM3, CH3, PB0,  TIM_USE_MOTOR, 0, 0), // M4 - D(1, 7, 5)
+						   //
+    DEF_TIM(TIM1,  CH1, PA8, TIM_USE_LED, 0, 0),      // LED STRIP - D(2, 6, 0)
 
 };
 
diff --git a/src/main/target/FRSKYPILOT/target.c b/src/main/target/FRSKYPILOT/target.c
index 50b82d7687e..95989f5f42a 100644
--- a/src/main/target/FRSKYPILOT/target.c
+++ b/src/main/target/FRSKYPILOT/target.c
@@ -39,13 +39,9 @@ timerHardware_t timerHardware[] = {
     DEF_TIM(TIM4,  CH2, PB7,  TIM_USE_OUTPUT_AUTO, 0, 0), // S7
     DEF_TIM(TIM4,  CH3, PB8,  TIM_USE_OUTPUT_AUTO, 0, 0), // S8
     DEF_TIM(TIM4,  CH4, PB9,  TIM_USE_OUTPUT_AUTO, 0, 0), // S9
-#ifdef FRSKYPILOT_LED
-    DEF_TIM(TIM2,  CH2, PA1,  TIM_USE_LED | TIM_USE_LED, 0, 0), // S10 now LED, S11 & S12 UART 3 only
-#else
     DEF_TIM(TIM2,  CH2, PA1,  TIM_USE_OUTPUT_AUTO, 0, 0), // S10
     DEF_TIM(TIM2,  CH3, PB10, TIM_USE_OUTPUT_AUTO, 0, 0), // S11
     DEF_TIM(TIM2,  CH4, PB11, TIM_USE_OUTPUT_AUTO, 0, 0), // S12
-#endif
 
     DEF_TIM(TIM5, CH1,  PA0,  TIM_USE_BEEPER, 0, 0),    // beeper
 };
diff --git a/src/main/target/IFLIGHT_BLITZ_ATF435/target.c b/src/main/target/IFLIGHT_BLITZ_ATF435/target.c
index 2de0fd50578..947d19c0d20 100644
--- a/src/main/target/IFLIGHT_BLITZ_ATF435/target.c
+++ b/src/main/target/IFLIGHT_BLITZ_ATF435/target.c
@@ -41,7 +41,7 @@ timerHardware_t timerHardware[] = {
     DEF_TIM(TMR4, CH1, PB6,  TIM_USE_OUTPUT_AUTO, 0,11),    // S5
     DEF_TIM(TMR4, CH2, PB7,  TIM_USE_OUTPUT_AUTO, 0,10),    // S6
     DEF_TIM(TMR2, CH3, PB10, TIM_USE_OUTPUT_AUTO, 0,5),     //S7
-	DEF_TIM(TMR2, CH4, PB11, TIM_USE_OUTPUT_AUTO, 0,6),     //S8
+    DEF_TIM(TMR2, CH4, PB11, TIM_USE_OUTPUT_AUTO, 0,6),     //S8
     
     DEF_TIM(TMR1, CH1, PA8,   TIM_USE_LED, 0, 0),  // LED STRIP
 };
diff --git a/src/main/target/MATEKF405/target.c b/src/main/target/MATEKF405/target.c
index 1706954b7f9..5b6f0ce559d 100644
--- a/src/main/target/MATEKF405/target.c
+++ b/src/main/target/MATEKF405/target.c
@@ -29,7 +29,7 @@ timerHardware_t timerHardware[] = {
     DEF_TIM(TIM8, CH3, PC8,  TIM_USE_OUTPUT_AUTO, 0, 1), // S3  UP(2,1)
     DEF_TIM(TIM8, CH4, PC9,  TIM_USE_OUTPUT_AUTO, 0, 0), // S4  UP(2,1)
 
-    DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO | TIM_USE_LED                        , 0, 0), // S5  UP(1,7)
+    DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0), // S5  UP(1,7) -- LED
 
     DEF_TIM(TIM1, CH1, PA8,  TIM_USE_OUTPUT_AUTO, 0, 0), // S6  UP(2,5)
     DEF_TIM(TIM4, CH3, PB8,  TIM_USE_OUTPUT_AUTO, 0, 0), // S7  D(1,7)!S5 UP(2,6)
diff --git a/src/main/target/NEUTRONRCF435MINI/target.c b/src/main/target/NEUTRONRCF435MINI/target.c
index 9400ae4be1b..0933322bb14 100644
--- a/src/main/target/NEUTRONRCF435MINI/target.c
+++ b/src/main/target/NEUTRONRCF435MINI/target.c
@@ -26,13 +26,13 @@
 
 timerHardware_t timerHardware[] = {
 
-    DEF_TIM(TMR1, CH1, PA8,  TIM_USE_ANY |TIM_USE_LED, 0,6),            // PWM1 - LED MCO1 DMA1 CH2
-
     DEF_TIM(TMR4, CH1, PB6,  TIM_USE_OUTPUT_AUTO, 0,0),   // motor1 DMA2 CH7
     DEF_TIM(TMR4, CH2, PB7,  TIM_USE_OUTPUT_AUTO, 0,2),   // motor2 DMA2 CH6
     DEF_TIM(TMR2, CH4, PA3,  TIM_USE_OUTPUT_AUTO, 0,1),   // motor3 DMA2 CH5
     DEF_TIM(TMR3, CH4, PB1,  TIM_USE_OUTPUT_AUTO, 0,3),   // motor4 DMA2 CH4
-
+							  //
+    DEF_TIM(TMR1, CH1, PA8,  TIM_USE_ANY |TIM_USE_LED, 0,6),            // PWM1 - LED MCO1 DMA1 CH2
+									//
 };
 
 const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
diff --git a/src/main/target/NEUTRONRCF435WING/target.c b/src/main/target/NEUTRONRCF435WING/target.c
index b1cd9031cc3..428654ebed3 100644
--- a/src/main/target/NEUTRONRCF435WING/target.c
+++ b/src/main/target/NEUTRONRCF435WING/target.c
@@ -26,8 +26,6 @@
 
 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_OUTPUT_AUTO, 0,0),   // motor1 DMA2 CH7
     DEF_TIM(TMR1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0,2),   // motor2 DMA2 CH6
     DEF_TIM(TMR2, CH4, PA3,  TIM_USE_OUTPUT_AUTO, 0,1),   // motor3 DMA2 CH5
@@ -38,6 +36,7 @@ timerHardware_t timerHardware[] = {
     DEF_TIM(TMR3, CH3, PB0,  TIM_USE_OUTPUT_AUTO, 0,8),  // PWM3 - OUT7  DMA2 CH3
     DEF_TIM(TMR2, CH3, PA2,  TIM_USE_OUTPUT_AUTO, 0,11), // PWM4 - OUT8  DMA1 CH7
 
+    DEF_TIM(TMR1, CH1, PA8,  TIM_USE_ANY |TIM_USE_LED, 0,7),            // PWM1 - LED MCO1 DMA1 CH2
 };
 
 const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
diff --git a/src/main/target/OMNIBUSF4/CMakeLists.txt b/src/main/target/OMNIBUSF4/CMakeLists.txt
index 72f3bbeca87..a6ccb483bf5 100644
--- a/src/main/target/OMNIBUSF4/CMakeLists.txt
+++ b/src/main/target/OMNIBUSF4/CMakeLists.txt
@@ -3,7 +3,6 @@ target_stm32f405xg(DYSF4PROV2)
 target_stm32f405xg(OMNIBUSF4)
 # the OMNIBUSF4SD has an SDCARD instead of flash, a BMP280 baro and therefore a slightly different ppm/pwm and SPI mapping
 target_stm32f405xg(OMNIBUSF4PRO)
-target_stm32f405xg(OMNIBUSF4PRO_LEDSTRIPM5)
 target_stm32f405xg(OMNIBUSF4V3_S5_S6_2SS)
 target_stm32f405xg(OMNIBUSF4V3_S5S6_SS)
 target_stm32f405xg(OMNIBUSF4V3_S6_SS)
diff --git a/src/main/target/OMNIBUSF4/target.c b/src/main/target/OMNIBUSF4/target.c
index 86485aa49fc..be85e9fbce3 100644
--- a/src/main/target/OMNIBUSF4/target.c
+++ b/src/main/target/OMNIBUSF4/target.c
@@ -41,15 +41,12 @@ timerHardware_t timerHardware[] = {
 #elif defined(OMNIBUSF4V3_S6_SS)
     DEF_TIM(TIM5,  CH2, PA1,  TIM_USE_OUTPUT_AUTO,                0, 0), // S5_OUT
     DEF_TIM(TIM1,  CH1, PA8,  TIM_USE_ANY,                                                           0, 0), // S6_OUT SOFTSERIAL
-#elif defined(OMNIBUSF4PRO_LEDSTRIPM5)
-    DEF_TIM(TIM5,  CH2, PA1,  TIM_USE_LED,                                                           0, 0), // S5_OUT LED strip
-    DEF_TIM(TIM1,  CH1, PA8,  TIM_USE_OUTPUT_AUTO,                0, 0), // S6_OUT
 #else
-    DEF_TIM(TIM5,  CH2, PA1,  TIM_USE_OUTPUT_AUTO | TIM_USE_LED,  0, 0), // S5_OUT MOTOR, SERVO or LED
+    DEF_TIM(TIM5,  CH2, PA1,  TIM_USE_OUTPUT_AUTO,  0, 0), // S5_OUT MOTOR, SERVO or LED
     DEF_TIM(TIM1,  CH1, PA8,  TIM_USE_OUTPUT_AUTO,                0, 0), // S6_OUT
 #endif
 
-#if (defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)) && !defined(OMNIBUSF4PRO_LEDSTRIPM5)
+#if (defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3))
     DEF_TIM(TIM4,  CH1, PB6,  TIM_USE_LED,                                                           0, 0), // LED strip for F4 V2 / F4-Pro-0X and later (RCD_CS for F4)
 #endif
 
diff --git a/src/main/target/SAGEATF4/target.c b/src/main/target/SAGEATF4/target.c
index 2d951598ae0..06f571c949b 100644
--- a/src/main/target/SAGEATF4/target.c
+++ b/src/main/target/SAGEATF4/target.c
@@ -26,19 +26,19 @@
 
 timerHardware_t timerHardware[] = {
 
-    // DEF_TIM(TMR5, CH4, PA3,  TIM_USE_PPM,  0, 4), 						// PPM UART2_RX_PIN DMA1 CH5
+	// DEF_TIM(TMR5, CH4, PA3,  TIM_USE_PPM,  0, 4), 						// PPM UART2_RX_PIN DMA1 CH5
 
- 	DEF_TIM(TMR4, CH1, PB6,  TIM_USE_MOTOR, 0,0),  // motor1 DMA1 CH1
- 	DEF_TIM(TMR4, CH2, PB7,  TIM_USE_MOTOR, 0,1),  // motor2 DMA1 CH2
- 	DEF_TIM(TMR4, CH3, PB8,  TIM_USE_MOTOR, 0,2), // motor3 DMA1 CH3
- 	DEF_TIM(TMR4, CH4, PB9,  TIM_USE_MOTOR, 0,3), // motor4 DMA1 CH4
+	DEF_TIM(TMR4, CH1, PB6,  TIM_USE_MOTOR, 0,0),  // motor1 DMA1 CH1
+	DEF_TIM(TMR4, CH2, PB7,  TIM_USE_MOTOR, 0,1),  // motor2 DMA1 CH2
+	DEF_TIM(TMR4, CH3, PB8,  TIM_USE_MOTOR, 0,2), // motor3 DMA1 CH3
+	DEF_TIM(TMR4, CH4, PB9,  TIM_USE_MOTOR, 0,3), // motor4 DMA1 CH4
 
 	DEF_TIM(TMR3, CH1, PC6,  TIM_USE_OUTPUT_AUTO, 0,8), 		// PWM1 - OUT5  DMA2 CH2 DMA2_CHANNEL1->ADC
 	DEF_TIM(TMR3, CH2, PC7,  TIM_USE_OUTPUT_AUTO, 0,9), 		// PWM2 - OUT6  DMA2 CH3
 	DEF_TIM(TMR3, CH3, PC8,  TIM_USE_OUTPUT_AUTO, 0,10), 		// PWM3 - OUT7  DMA2 CH4
 	DEF_TIM(TMR3, CH4, PC9,  TIM_USE_OUTPUT_AUTO, 0,11), 		// PWM4 - OUT8  DMA2 CH5
 
-    DEF_TIM(TMR2, CH3, PB10, TIM_USE_ANY | TIM_USE_LED, 0,5),           // PWM1 - LED MCO1 DMA1 CH6
+	DEF_TIM(TMR2, CH3, PB10, TIM_USE_ANY | TIM_USE_LED, 0,5),           // PWM1 - LED MCO1 DMA1 CH6
 	DEF_TIM(TMR5, CH4, PB11, TIM_USE_ANY | TIM_USE_BEEPER, 0,6), 		// PWM2 - BB DMA1 CH7
 };
 
diff --git a/src/main/target/SPEEDYBEEF405MINI/CMakeLists.txt b/src/main/target/SPEEDYBEEF405MINI/CMakeLists.txt
index e18c0cd5ada..c4105609952 100644
--- a/src/main/target/SPEEDYBEEF405MINI/CMakeLists.txt
+++ b/src/main/target/SPEEDYBEEF405MINI/CMakeLists.txt
@@ -1,2 +1 @@
 target_stm32f405xg(SPEEDYBEEF405MINI)
-target_stm32f405xg(SPEEDYBEEF405MINI_6OUTPUTS)
diff --git a/src/main/target/SPEEDYBEEF405MINI/target.c b/src/main/target/SPEEDYBEEF405MINI/target.c
index 3cc7fbbebba..b1e6e10f6f4 100644
--- a/src/main/target/SPEEDYBEEF405MINI/target.c
+++ b/src/main/target/SPEEDYBEEF405MINI/target.c
@@ -32,15 +32,10 @@ timerHardware_t timerHardware[] = {
     DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO,   1, 0), // S3
     DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO,   1, 0), // S4
 
-#ifdef SPEEDYBEEF405MINI_6OUTPUTS
-    DEF_TIM(TIM12,  CH1,    PB14,   TIM_USE_OUTPUT_AUTO,     1, 0), // CAM_CTRL
-    DEF_TIM(TIM1,   CH1,    PA8,    TIM_USE_OUTPUT_AUTO,     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
+    DEF_TIM(TIM12,  CH1,    PB14,   TIM_USE_ANY,   1, 0), // CAM_CTRL
+    DEF_TIM(TIM1,   CH1,    PA8,    TIM_USE_LED,   0, 0), // LED
 
-    DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY,                            0, 0), //TX2  softserial1_Tx
+    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/SPRACINGF7DUAL/target.c b/src/main/target/SPRACINGF7DUAL/target.c
index e94835b93c2..ead356956a4 100644
--- a/src/main/target/SPRACINGF7DUAL/target.c
+++ b/src/main/target/SPRACINGF7DUAL/target.c
@@ -56,7 +56,6 @@ timerHardware_t timerHardware[] = {
     DEF_TIM(TIM3,  CH4, PB1,  TIM_USE_MOTOR,               0, 0), // ESC 7
     DEF_TIM(TIM3,  CH3, PB0,  TIM_USE_MOTOR,               0, 0), // ESC 8
 
-    DEF_TIM(TIM2,  CH2, PA1,  TIM_USE_LED,                 0, 0), // LED Strip
     // Additional 2 PWM channels available on UART3 RX/TX pins
     // However, when using led strip the timer cannot be used, but no code appears to prevent that right now
     DEF_TIM(TIM2,  CH3, PB10, TIM_USE_MOTOR,               0, 0), // Shared with UART3 TX PIN and SPI3 TX (OSD)
@@ -67,6 +66,9 @@ timerHardware_t timerHardware[] = {
     // However, when using transponder the timer cannot be used, but no code appears to prevent that right now
     DEF_TIM(TIM1,  CH2, PA9,  TIM_USE_SERVO | TIM_USE_PWM, 0, 1), // PWM 3
     DEF_TIM(TIM1,  CH3, PA10, TIM_USE_SERVO | TIM_USE_PWM, 0, 1), // PWM 4
+
+
+    DEF_TIM(TIM2,  CH2, PA1,  TIM_USE_LED,                 0, 0), // LED Strip
 };
 
 const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
diff --git a/src/main/target/TAKERF722SE/target.c b/src/main/target/TAKERF722SE/target.c
index d67062a9fbc..c49e383a72f 100644
--- a/src/main/target/TAKERF722SE/target.c
+++ b/src/main/target/TAKERF722SE/target.c
@@ -40,7 +40,7 @@ timerHardware_t timerHardware[] = {
     DEF_TIM(TIM8, CH3, PC8,  TIM_USE_OUTPUT_AUTO, 0, 0),
     DEF_TIM(TIM8, CH4, PC9,  TIM_USE_OUTPUT_AUTO, 0, 0),
 
-    DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED,                         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/TMOTORF7V2/target.c b/src/main/target/TMOTORF7V2/target.c
index c21b0f0229a..df6004b7a2f 100644
--- a/src/main/target/TMOTORF7V2/target.c
+++ b/src/main/target/TMOTORF7V2/target.c
@@ -35,8 +35,8 @@ timerHardware_t timerHardware[] = {
     DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0),  // S7
     DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0),  // S8 
     
-    DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED,                         0, 0),  // LED
     DEF_TIM(TIM2, CH2, PB3, TIM_USE_PWM | TIM_USE_SERVO,      0, 0),  // "C.C"
+    DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED,                      0, 0),  // LED
 };
 
 const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
diff --git a/src/main/target/TMOTORVELOXF7V2/target.c b/src/main/target/TMOTORVELOXF7V2/target.c
index 7d9ae0d3292..b5f56ea6b79 100644
--- a/src/main/target/TMOTORVELOXF7V2/target.c
+++ b/src/main/target/TMOTORVELOXF7V2/target.c
@@ -37,7 +37,7 @@ timerHardware_t timerHardware[] = {
     DEF_TIM(TIM4, CH1, PB6,   TIM_USE_OUTPUT_AUTO, 0, 0),   // S7   UP1-6   D(1, 0, 2) // used to be fw motor
     DEF_TIM(TIM4, CH2, PB7,   TIM_USE_OUTPUT_AUTO, 0, 0),   // S8   UP1-6   D(1, 3, 2) // used to be fw motor
 
-    DEF_TIM(TIM1, CH1, PA8,     TIM_USE_LED,    0,  1), // LED
+    DEF_TIM(TIM1, CH1, PA8,     TIM_USE_LED,       0,  1), // LED
 };
 
 const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

From 243c6775999d15105fa0bb7e57ef00f92b2fc74a Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Fri, 31 May 2024 18:24:39 +0200
Subject: [PATCH 158/323] Some targets don't have led pin defined

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

diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index f70c00f221b..4ae42184816 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -1590,7 +1590,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         break;
     case MSP2_INAV_OUTPUT_MAPPING_EXT2:
         {
-            #if !defined(SITL_BUILD)
+            #if !defined(SITL_BUILD) && defined(WS2811_PIN)
             ioTag_t led_tag = IO_TAG(WS2811_PIN);
             #endif
             for (uint8_t i = 0; i < timerHardwareCount; ++i)
@@ -1602,7 +1602,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
                     sbufWriteU8(dst, timer2id(timerHardware[i].tim));
                     #endif
                     sbufWriteU32(dst, timerHardware[i].usageFlags);
-                    #if defined(SITL_BUILD)
+                    #if defined(SITL_BUILD) || !defined(WS2811_PIN)
                     sbufWriteU8(dst, 0);
                     #else
                     // Extra label to help identify repurposed PINs.

From 22697304b2173bd1305293b2bef4317c769033d3 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Fri, 31 May 2024 21:40:01 +0200
Subject: [PATCH 159/323] Update gps_ublox.c

Fix formatting.
---
 src/main/io/gps_ublox.c | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c
index 8c071613289..958de158d3e 100755
--- a/src/main/io/gps_ublox.c
+++ b/src/main/io/gps_ublox.c
@@ -1061,11 +1061,11 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread)
         // M7 and earlier will never get pass this step, so skip it (#9440).
         // UBLOX documents that this is M8N and later
         if (gpsState.hwVersion > UBX_HW_VERSION_UBLOX7) {
-        do {
-            pollGnssCapabilities();
-            gpsState.autoConfigStep++;
-            ptWaitTimeout((ubx_capabilities.capMaxGnss != 0), GPS_CFG_CMD_TIMEOUT_MS);
-        } while (gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && ubx_capabilities.capMaxGnss == 0);
+            do {
+                pollGnssCapabilities();
+                gpsState.autoConfigStep++;
+                ptWaitTimeout((ubx_capabilities.capMaxGnss != 0), GPS_CFG_CMD_TIMEOUT_MS);
+            } while (gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && ubx_capabilities.capMaxGnss == 0);
         }
 
         // Configure GPS

From 0e9ca2a58d8944f805d1089e20b23f8386206fcb Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Fri, 31 May 2024 21:49:43 -0500
Subject: [PATCH 160/323] update CMakeLists.txt to version 7.1.2

---
 CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index f74527e5f1b..3a2bd54e6e9 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -51,7 +51,7 @@ else()
     endif()
 endif()
 
-project(INAV VERSION 7.1.1)
+project(INAV VERSION 7.1.2)
 
 enable_language(ASM)
 

From eb676ece935821beac71e16d4aaf551b52d52fa9 Mon Sep 17 00:00:00 2001
From: flywoo <viper_rc@outlook.com>
Date: Sat, 1 Jun 2024 11:39:48 +0800
Subject: [PATCH 161/323] Added new targets FLYWOOF722PRO, F405HD, updated
 F745, F405PRO config

---
 src/main/target/FLYWOOF405PRO/CMakeLists.txt |   1 +
 src/main/target/FLYWOOF405PRO/config.c       |  34 ++++
 src/main/target/FLYWOOF405PRO/target.c       |   3 -
 src/main/target/FLYWOOF405PRO/target.h       |  19 ++-
 src/main/target/FLYWOOF722PRO/CMakeLists.txt |   1 +
 src/main/target/FLYWOOF722PRO/config.c       |  30 ++++
 src/main/target/FLYWOOF722PRO/target.c       |  48 ++++++
 src/main/target/FLYWOOF722PRO/target.h       | 158 +++++++++++++++++++
 src/main/target/FLYWOOF745/target.c          |   9 --
 src/main/target/FLYWOOF745/target.h          |  10 ++
 10 files changed, 296 insertions(+), 17 deletions(-)
 create mode 100644 src/main/target/FLYWOOF405PRO/config.c
 create mode 100644 src/main/target/FLYWOOF722PRO/CMakeLists.txt
 create mode 100644 src/main/target/FLYWOOF722PRO/config.c
 create mode 100644 src/main/target/FLYWOOF722PRO/target.c
 create mode 100644 src/main/target/FLYWOOF722PRO/target.h

diff --git a/src/main/target/FLYWOOF405PRO/CMakeLists.txt b/src/main/target/FLYWOOF405PRO/CMakeLists.txt
index 8447a117c00..2c5382d82ef 100644
--- a/src/main/target/FLYWOOF405PRO/CMakeLists.txt
+++ b/src/main/target/FLYWOOF405PRO/CMakeLists.txt
@@ -1 +1,2 @@
 target_stm32f405xg(FLYWOOF405PRO)
+target_stm32f405xg(FLYWOOF405HD)
\ No newline at end of file
diff --git a/src/main/target/FLYWOOF405PRO/config.c b/src/main/target/FLYWOOF405PRO/config.c
new file mode 100644
index 00000000000..dfe6c0ce100
--- /dev/null
+++ b/src/main/target/FLYWOOF405PRO/config.c
@@ -0,0 +1,34 @@
+/*
+ * 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 "drivers/pwm_output.h"
+#include "drivers/pwm_mapping.h"
+
+void targetConfiguration(void)
+{
+    // Make sure S1-S4 default to Motors
+
+    timerOverridesMutable(timer2id(TIM3))->outputMode = OUTPUT_MODE_MOTORS;
+    timerOverridesMutable(timer2id(TIM2))->outputMode = OUTPUT_MODE_MOTORS;
+}
diff --git a/src/main/target/FLYWOOF405PRO/target.c b/src/main/target/FLYWOOF405PRO/target.c
index c0f1c4eec2d..d4dd134dddc 100644
--- a/src/main/target/FLYWOOF405PRO/target.c
+++ b/src/main/target/FLYWOOF405PRO/target.c
@@ -24,15 +24,12 @@
 timerHardware_t timerHardware[] = {
     DEF_TIM(TIM3,  CH3, PB0,  TIM_USE_OUTPUT_AUTO,   0, 0), // S1 D(1,3,2)
     DEF_TIM(TIM3,  CH4, PB1,  TIM_USE_OUTPUT_AUTO,   0, 0), // S2 D(1,0,2)
-
     DEF_TIM(TIM2,  CH4, PA3,  TIM_USE_OUTPUT_AUTO,   0, 1), // S3 D(1,7,5)
     DEF_TIM(TIM2,  CH3, PA2,  TIM_USE_OUTPUT_AUTO,   0, 0), // S4 D(1,2,5)
     DEF_TIM(TIM3,  CH2, PB5,  TIM_USE_OUTPUT_AUTO,   0, 0), // S5 D(2,4,7)
     DEF_TIM(TIM8,  CH4, PC9,  TIM_USE_OUTPUT_AUTO,   0, 0), // S6
     DEF_TIM(TIM3,  CH1, PB4,  TIM_USE_OUTPUT_AUTO,   0, 0), // S7
     DEF_TIM(TIM8,  CH3, PC8,  TIM_USE_OUTPUT_AUTO,   0, 0), // S8
-
-
     DEF_TIM(TIM1,  CH2, PA9,  TIM_USE_LED,   0, 0), //2812LED  D(1,5,3)
     // DEF_TIM(TIM10, CH1, PB8,  TIM_USE_PPM,   0, 0), // PPM
 };
diff --git a/src/main/target/FLYWOOF405PRO/target.h b/src/main/target/FLYWOOF405PRO/target.h
index 14b5e9e03cb..a5a02c5fe84 100644
--- a/src/main/target/FLYWOOF405PRO/target.h
+++ b/src/main/target/FLYWOOF405PRO/target.h
@@ -17,9 +17,13 @@
 
 #pragma once
 
+#ifdef FLYWOOF405PRO
 #define TARGET_BOARD_IDENTIFIER "F4PR"
 #define USBD_PRODUCT_STRING  "FLYWOOF405PRO"
-
+#else
+#define TARGET_BOARD_IDENTIFIER "F4HD"
+#define USBD_PRODUCT_STRING  "FLYWOOF405HD"
+#endif
 
 #define LED0                    PC14  //Green
 #define BEEPER                  PC13
@@ -84,11 +88,12 @@
 #define SPI3_MISO_PIN   	    PC11
 #define SPI3_MOSI_PIN   	    PC12
 
+#ifdef FLYWOOF405PRO
 #define USE_OSD
 #define USE_MAX7456
 #define MAX7456_SPI_BUS         BUS_SPI3
-#define MAX7456_CS_PIN          PB14//
-
+#define MAX7456_CS_PIN          PB14
+#endif
 // *************** Onboard flash  ********************
 
 #define USE_FLASHFS
@@ -109,6 +114,10 @@
 #define UART1_TX_PIN            PB6
 #define UART1_RX_PIN            PA10
 
+#define USE_UART2
+#define UART2_TX_PIN            PD5
+#define UART2_RX_PIN            PD6
+
 #define USE_UART3
 #define UART3_TX_PIN            PB10
 #define UART3_RX_PIN            PB11
@@ -125,7 +134,7 @@
 #define UART6_TX_PIN            PC6
 #define UART6_RX_PIN            PC7
 
-#define SERIAL_PORT_COUNT       6
+#define SERIAL_PORT_COUNT       7
 
 #define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
 #define SERIALRX_PROVIDER       SERIALRX_SBUS
@@ -159,6 +168,6 @@
 #define TARGET_IO_PORTA         0xffff
 #define TARGET_IO_PORTB         0xffff
 #define TARGET_IO_PORTC         0xffff
-#define TARGET_IO_PORTD         (BIT(2))
+#define TARGET_IO_PORTD         0xffff
 
 #define MAX_PWM_OUTPUT_PORTS       8
diff --git a/src/main/target/FLYWOOF722PRO/CMakeLists.txt b/src/main/target/FLYWOOF722PRO/CMakeLists.txt
new file mode 100644
index 00000000000..1906be3d599
--- /dev/null
+++ b/src/main/target/FLYWOOF722PRO/CMakeLists.txt
@@ -0,0 +1 @@
+target_stm32f722xe(FLYWOOF722PRO)
diff --git a/src/main/target/FLYWOOF722PRO/config.c b/src/main/target/FLYWOOF722PRO/config.c
new file mode 100644
index 00000000000..ae386d4bc0a
--- /dev/null
+++ b/src/main/target/FLYWOOF722PRO/config.c
@@ -0,0 +1,30 @@
+/*
+ * 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/pwm_mapping.h"
+#include "fc/fc_msp_box.h"
+#include "io/piniobox.h"
+
+void targetConfiguration(void)
+{
+    pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1;
+	
+    timerOverridesMutable(timer2id(TIM3))->outputMode = OUTPUT_MODE_MOTORS;
+    timerOverridesMutable(timer2id(TIM2))->outputMode = OUTPUT_MODE_MOTORS;
+}
diff --git a/src/main/target/FLYWOOF722PRO/target.c b/src/main/target/FLYWOOF722PRO/target.c
new file mode 100644
index 00000000000..e8b78979f95
--- /dev/null
+++ b/src/main/target/FLYWOOF722PRO/target.c
@@ -0,0 +1,48 @@
+/*
+ * 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"
+
+
+BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000,     DEVHW_MPU6000,      MPU6000_SPI_BUS,    MPU6000_CS_PIN,     NONE,       0,  DEVFLAGS_NONE,  IMU_MPU6000_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_ICM42605,    DEVHW_ICM42605,     ICM42605_SPI_BUS,   ICM42605_CS_PIN,    NONE,      0,  DEVFLAGS_NONE,  IMU_ICM42605_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_BMI270,      DEVHW_BMI270,       BMI270_SPI_BUS,     BMI270_CS_PIN,      NONE,        0,  DEVFLAGS_NONE,  IMU_BMI270_ALIGN);
+
+timerHardware_t timerHardware[] = {
+    //DEF_TIM(TIM8,  CH1, PC6,  TIM_USE_PPM,0, 0),  // PPM&SBUS  
+
+    DEF_TIM(TIM3,  CH4, PB1,  TIM_USE_OUTPUT_AUTO, 0, 0),     // S1 - D(1,2)
+    DEF_TIM(TIM3,  CH1, PB4,  TIM_USE_OUTPUT_AUTO, 0, 0),     // S2 - D(1,4)
+    DEF_TIM(TIM2,  CH2, PB3,  TIM_USE_OUTPUT_AUTO, 0, 0),      //  S3 - D(1,6)
+    DEF_TIM(TIM2,  CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0),     //  S4 - D(1,5)  
+    DEF_TIM(TIM8,  CH3, PC8,  TIM_USE_OUTPUT_AUTO, 0, 0),      //  S5 - D(2,4) 
+    DEF_TIM(TIM8,  CH4, PC9,  TIM_USE_OUTPUT_AUTO, 0, 0),      //  S6 - D(2,1)
+    DEF_TIM(TIM4, CH1, PB6,   TIM_USE_OUTPUT_AUTO, 0, 0),        //  S7 - D(1,0)
+    DEF_TIM(TIM4, CH2, PB7,   TIM_USE_OUTPUT_AUTO, 0, 0),        //  S8 - D(1,3)
+
+    DEF_TIM(TIM1,  CH1, PA8,  TIM_USE_LED,                          0, 0),  // LED STRIP(1,5)
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
diff --git a/src/main/target/FLYWOOF722PRO/target.h b/src/main/target/FLYWOOF722PRO/target.h
new file mode 100644
index 00000000000..34d6de266c4
--- /dev/null
+++ b/src/main/target/FLYWOOF722PRO/target.h
@@ -0,0 +1,158 @@
+/*
+ * 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/>.
+ */
+
+#define TARGET_BOARD_IDENTIFIER "FWF7"
+#define USBD_PRODUCT_STRING     "FLYWOOF722PRO"
+
+/*** Indicators ***/
+#define LED0                    PC15
+#define USE_BEEPER		  
+#define BEEPER                  PC14
+#define BEEPER_INVERTED
+
+/*** SPI/I2C bus ***/
+#define USE_SPI
+#define USE_SPI_DEVICE_1        // Gyro 1/2
+#define SPI1_SCK_PIN            PA5
+#define SPI1_MISO_PIN           PA6
+#define SPI1_MOSI_PIN           PA7
+
+#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS
+
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW0_DEG
+#define MPU6000_SPI_BUS         BUS_SPI1
+#define MPU6000_CS_PIN          PA4
+
+#define USE_IMU_ICM42605
+#define IMU_ICM42605_ALIGN      CW270_DEG
+#define ICM42605_SPI_BUS        BUS_SPI1
+#define ICM42605_CS_PIN         PA4
+
+#define USE_IMU_BMI270
+#define IMU_BMI270_ALIGN        CW180_DEG
+#define BMI270_SPI_BUS          BUS_SPI1
+#define BMI270_CS_PIN           PA4
+
+
+#define USE_SPI_DEVICE_2        // MAX7456
+#define SPI2_SCK_PIN            PB13
+#define SPI2_MISO_PIN           PB14
+#define SPI2_MOSI_PIN           PB15
+
+#define USE_SPI_DEVICE_3        // FLASH
+#define SPI3_SCK_PIN            PC10
+#define SPI3_MISO_PIN           PC11 
+#define SPI3_MOSI_PIN           PB5
+
+#define USE_I2C
+#define USE_I2C_DEVICE_1        // I2C pads
+#define I2C1_SCL                PB8     
+#define I2C1_SDA                PB9 
+
+/*** Onboard flash ***/
+#define USE_FLASHFS
+#define USE_FLASH_M25P16
+#define M25P16_CS_PIN           PC13
+#define M25P16_SPI_BUS          BUS_SPI3
+
+/*** OSD ***/
+#define USE_OSD
+#define USE_MAX7456
+#define MAX7456_CS_PIN          PB12
+#define MAX7456_SPI_BUS         BUS_SPI2
+
+/*** Serial ports ***/
+#define USB_IO
+#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            PB10
+#define UART3_RX_PIN            PB11
+
+#define USE_UART4
+#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 SERIAL_PORT_COUNT       7
+
+/*** BARO & MAG ***/
+#define USE_BARO
+#define BARO_I2C_BUS            BUS_I2C1
+#define USE_BARO_BMP280
+#define USE_BARO_MS5611
+#define USE_BARO_DPS310
+#define USE_BARO_SPL06
+
+#define USE_MAG
+#define MAG_I2C_BUS             BUS_I2C1
+#define USE_MAG_ALL
+
+/*** ADC ***/
+#define USE_ADC
+#define ADC_CHANNEL_1_PIN               PC0
+#define ADC_CHANNEL_2_PIN               PC1
+#define ADC_CHANNEL_3_PIN               PC2
+
+#define CURRENT_METER_ADC_CHANNEL       ADC_CHN_1
+#define VBAT_ADC_CHANNEL                ADC_CHN_2
+#define RSSI_ADC_CHANNEL                ADC_CHN_3
+
+
+/*** PINIO ***/
+#define USE_PINIO
+#define USE_PINIOBOX
+#define PINIO1_PIN                      PB0
+
+/*** LED STRIP ***/
+#define USE_LED_STRIP
+#define WS2811_PIN                      PA8
+ 
+/*** Default settings ***/
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+#define CURRENT_METER_SCALE_DEFAULT     170
+#define SERIALRX_UART                   SERIAL_PORT_USART1
+#define DEFAULT_RX_TYPE                 RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER               SERIALRX_SBUS
+
+/*** Timer/PWM output ***/
+#define USE_SERIAL_4WAY_BLHELI_INTERFACE
+#define MAX_PWM_OUTPUT_PORTS            6
+#define USE_DSHOT
+#define USE_ESC_SENSOR
+
+/*** Used pins ***/
+#define TARGET_IO_PORTA         0xffff
+#define TARGET_IO_PORTB         0xffff
+#define TARGET_IO_PORTC         0xffff
+#define TARGET_IO_PORTD         (BIT(2))
diff --git a/src/main/target/FLYWOOF745/target.c b/src/main/target/FLYWOOF745/target.c
index 04c083ac94f..a68d3d3199f 100644
--- a/src/main/target/FLYWOOF745/target.c
+++ b/src/main/target/FLYWOOF745/target.c
@@ -32,20 +32,11 @@
 timerHardware_t timerHardware[] = {
 // DEF_TIM(TIM1, CH3, PE13, TIM_USE_PPM,                      0, 1), // PPM,
 // DMA2_ST6
-
-#ifdef FLYWOOF745
-    DEF_TIM(TIM3, CH4, PB1,  TIM_USE_OUTPUT_AUTO, 0, 0),   // M1 , DMA1_ST2
-    DEF_TIM(TIM8, CH4, PC9,  TIM_USE_OUTPUT_AUTO, 0, 0),   // M2 , DMA2_ST7
-    DEF_TIM(TIM3, CH3, PB0,  TIM_USE_OUTPUT_AUTO, 0, 0),   // M3 , DMA1_ST7
-    DEF_TIM(TIM1, CH2, PE11, TIM_USE_OUTPUT_AUTO, 0, 1),  // M4 , DMA2_ST4
-    DEF_TIM(TIM1, CH1, PE9,  TIM_USE_OUTPUT_AUTO, 0, 2),   // M5 , DMA2_ST2
-#else /* FLYWOOF745NANO */
     DEF_TIM(TIM3, CH3, PB0,  TIM_USE_OUTPUT_AUTO, 0, 0),   // M1 , DMA1_ST7
     DEF_TIM(TIM3, CH4, PB1,  TIM_USE_OUTPUT_AUTO, 0, 0),   // M2 , DMA1_ST2
     DEF_TIM(TIM1, CH1, PE9,  TIM_USE_OUTPUT_AUTO, 0, 2),   // M3 , DMA2_ST2
     DEF_TIM(TIM1, CH2, PE11, TIM_USE_OUTPUT_AUTO, 0, 1),  // M4 , DMA2_ST4
     DEF_TIM(TIM8, CH4, PC9,  TIM_USE_OUTPUT_AUTO, 0, 0),   // M5 , DMA2_ST7
-#endif
     DEF_TIM(TIM5, CH4, PA3,  TIM_USE_OUTPUT_AUTO, 0, 0),  // M6 , DMA1_ST1
     DEF_TIM(TIM3, CH1, PB4,  TIM_USE_OUTPUT_AUTO, 0, 0),  // M7 , DMA1_ST4
     DEF_TIM(TIM3, CH2, PB5,  TIM_USE_OUTPUT_AUTO, 0, 0),  // M8 , DMA1_ST5
diff --git a/src/main/target/FLYWOOF745/target.h b/src/main/target/FLYWOOF745/target.h
index c2b1937c611..d00c2455945 100644
--- a/src/main/target/FLYWOOF745/target.h
+++ b/src/main/target/FLYWOOF745/target.h
@@ -41,6 +41,16 @@
 #define MPU6000_CS_PIN          SPI4_NSS_PIN
 #define MPU6000_SPI_BUS         BUS_SPI4
 
+#define USE_IMU_BMI270
+#define IMU_BMI270_ALIGN       CW270_DEG
+#define BMI270_CS_PIN          SPI4_NSS_PIN
+#define BMI270_SPI_BUS         BUS_SPI4
+
+#define USE_IMU_ICM42605
+#define IMU_ICM42605_ALIGN      CW270_DEG
+#define ICM42605_CS_PIN         SPI4_NSS_PIN
+#define ICM42605_SPI_BUS        BUS_SPI4
+
 #define USB_IO
 #define USE_VCP
 #define VBUS_SENSING_ENABLED

From b976458ec1e21adff510ac43183945aa8c669012 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Sun, 2 Jun 2024 12:59:29 +0100
Subject: [PATCH 162/323] inverted crash detection

---
 docs/Settings.md                             | 10 ++++++++
 src/main/fc/settings.yaml                    |  6 +++++
 src/main/navigation/navigation.c             | 11 +++++----
 src/main/navigation/navigation.h             |  1 +
 src/main/navigation/navigation_multicopter.c | 26 ++++++++++++++++++++
 5 files changed, 49 insertions(+), 5 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index a1c339c1d8c..a6cd597ef17 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -3582,6 +3582,16 @@ Multicopter hover throttle hint for altitude controller. Should be set to approx
 
 ---
 
+### nav_mc_inverted_crash_detection
+
+Setting a value > 0 enables inverted crash detection for multirotors. It is intended for situations where the multirotor has crashed inverted on the ground and can't be manually disarmed due to loss of control. When enabled the setting defines the additional number of seconds before disarm beyond a minimum fixed time delay such that the minimum possible time delay before disarm is 3s.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | 0 | 15 |
+
+---
+
 ### nav_mc_manual_climb_rate
 
 Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s]
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index d9aa83c93cf..2dbda7569bb 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -2415,6 +2415,12 @@ groups:
         default_value: OFF
         field: general.flags.landing_bump_detection
         type: bool
+      - name: nav_mc_inverted_crash_detection
+        description: "Setting a value > 0 enables inverted crash detection for multirotors. It is intended for situations where the multirotor has crashed inverted on the ground and can't be manually disarmed due to loss of control. When enabled the setting defines the additional number of seconds before disarm beyond a minimum fixed time delay such that the minimum possible time delay before disarm is 3s."
+        default_value: 0
+        field: mc.inverted_crash_detection
+        min: 0
+        max: 15
       - 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"
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index cc4e269ef4b..a4a31370c63 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -175,7 +175,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
         .land_detect_sensitivity = SETTING_NAV_LAND_DETECT_SENSITIVITY_DEFAULT,                 // Changes sensitivity of landing detection
         .auto_disarm_delay = SETTING_NAV_AUTO_DISARM_DELAY_DEFAULT,                             // 2000 ms - time delay to disarm when auto disarm after landing enabled
         .rth_linear_descent_start_distance = SETTING_NAV_RTH_LINEAR_DESCENT_START_DISTANCE_DEFAULT,
-        .cruise_yaw_rate = SETTING_NAV_CRUISE_YAW_RATE_DEFAULT,                                // 20dps
+        .cruise_yaw_rate = SETTING_NAV_CRUISE_YAW_RATE_DEFAULT,                                 // 20dps
         .rth_fs_landing_delay = SETTING_NAV_RTH_FS_LANDING_DELAY_DEFAULT,                       // Delay before landing in FS. 0 = immedate landing
     },
 
@@ -193,13 +193,14 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
         .braking_boost_timeout = SETTING_NAV_MC_BRAKING_BOOST_TIMEOUT_DEFAULT,                   // Timout boost after 750ms
         .braking_boost_speed_threshold = SETTING_NAV_MC_BRAKING_BOOST_SPEED_THRESHOLD_DEFAULT,   // Boost can happen only above 1.5m/s
         .braking_boost_disengage_speed = SETTING_NAV_MC_BRAKING_BOOST_DISENGAGE_SPEED_DEFAULT,   // Disable boost at 1m/s
-        .braking_bank_angle = SETTING_NAV_MC_BRAKING_BANK_ANGLE_DEFAULT,                        // Max braking angle
+        .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
+        .althold_throttle_type = SETTING_NAV_MC_ALTHOLD_THROTTLE_DEFAULT,                        // STICK
+        .inverted_crash_detection = SETTING_NAV_MC_INVERTED_CRASH_DETECTION_DEFAULT,             // 0 - disarm time delay for inverted crash detection
     },
 
     // Fixed wing
diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
index 276fb61cc1e..1a19de7b34d 100644
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@@ -344,6 +344,7 @@ typedef struct navConfig_s {
         uint8_t posResponseExpo;                // Position controller expo (taret vel expo for MC)
         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
+        uint8_t inverted_crash_detection;       // Enables inverted crash detection, setting defines disarm time delay (0 = disabled)
     } mc;
 
     struct {
diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c
index b108ef6aa31..96d44252715 100644
--- a/src/main/navigation/navigation_multicopter.c
+++ b/src/main/navigation/navigation_multicopter.c
@@ -36,6 +36,7 @@
 #include "sensors/boardalignment.h"
 #include "sensors/gyro.h"
 
+#include "fc/fc_core.h"
 #include "fc/config.h"
 #include "fc/rc_controls.h"
 #include "fc/rc_curves.h"
@@ -794,11 +795,36 @@ static bool isLandingGbumpDetected(timeMs_t currentTimeMs)
     return false;
 }
 #endif
+bool isMulticopterCrashedInverted(void)
+{
+    static timeMs_t startTime = 0;
+
+    if ((ABS(attitude.values.roll) > 1000 || ABS(attitude.values.pitch) > 700) && fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) < MC_LAND_CHECK_VEL_Z_MOVING) {
+        if (startTime == 0) {
+            startTime = millis();
+        } else {
+            /* minimum 2s disarm delay + extra user set delay time. Min time of 3s given min user setting is 1s if enabled */
+            uint16_t disarmTimeDelay = 2000 + S2MS(navConfig()->mc.inverted_crash_detection);
+
+            return millis() - startTime > disarmTimeDelay;
+        }
+    } else {
+        startTime = 0;
+    }
+
+    return false;
+}
+
 bool isMulticopterLandingDetected(void)
 {
     DEBUG_SET(DEBUG_LANDING, 4, 0);
     DEBUG_SET(DEBUG_LANDING, 3, averageAbsGyroRates() * 100);
 
+    if (navConfig()->mc.inverted_crash_detection && isMulticopterCrashedInverted()) {
+        ENABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
+        disarm(DISARM_LANDING);
+    }
+
     const timeMs_t currentTimeMs = millis();
 
 #if defined(USE_BARO)

From 985fa2b440403674ae7c9abf9388abec810a70aa Mon Sep 17 00:00:00 2001
From: Darren Lines <darren@darrenlines.uk>
Date: Sun, 2 Jun 2024 16:01:52 +0100
Subject: [PATCH 163/323] Add battery profile to programming framework

---
 src/main/programming/logic_condition.c | 4 ++++
 src/main/programming/logic_condition.h | 1 +
 2 files changed, 5 insertions(+)

diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c
index 343db2989f5..f927ac18d1b 100644
--- a/src/main/programming/logic_condition.c
+++ b/src/main/programming/logic_condition.c
@@ -815,6 +815,10 @@ static int logicConditionGetFlightOperandValue(int operand) {
         case LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE: // int
             return getConfigProfile() + 1;
             break;
+
+        case LOGIC_CONDITION_OPERAND_FLIGHT_BATT_PROFILE: //int
+            return getConfigBatteryProfile() + 1;
+            break;
         
         case LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_MIXER_PROFILE: // int
             return currentMixerProfileIndex + 1;
diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h
index 3a0c0a87b1b..5defafaee67 100644
--- a/src/main/programming/logic_condition.h
+++ b/src/main/programming/logic_condition.h
@@ -142,6 +142,7 @@ typedef enum {
     LOGIC_CONDITION_OPERAND_FLIGHT_MIXER_TRANSITION_ACTIVE, //0,1           // 39
     LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_YAW, // deg                     // 40
     LOGIC_CONDITION_OPERAND_FLIGHT_FW_LAND_STATE,                           // 41
+    LOGIC_CONDITION_OPERAND_FLIGHT_BATT_PROFILE, // int                     // 42
 } logicFlightOperands_e;
 
 typedef enum {

From b9fd9e3f56d67a2ae4c265df4477d78554a218fa Mon Sep 17 00:00:00 2001
From: Darren Lines <darren@darrenlines.uk>
Date: Sun, 2 Jun 2024 19:02:38 +0100
Subject: [PATCH 164/323] Consolidate the old `PIDProfile` and `Profile` in to
 `Control Profile`

-  Consolidate the old `PIDProfile` and `Profile` in to `Control Profile`
- Improve consistency of the order `Control Profile`, `Mixer Profile`, and `Battery Profile`
- Added `mixer_profile` to the `diff`/`dump` commands
- Rename and update the profile documentation
- Update the programming framework documentaion
---
 docs/{Profiles.md => Control Profiles.md} |  49 ++---
 docs/Programming Framework.md             | 232 +++++++++++-----------
 src/main/fc/cli.c                         |  70 ++++---
 3 files changed, 177 insertions(+), 174 deletions(-)
 rename docs/{Profiles.md => Control Profiles.md} (64%)

diff --git a/docs/Profiles.md b/docs/Control Profiles.md
similarity index 64%
rename from docs/Profiles.md
rename to docs/Control Profiles.md
index 925de755f85..af41fed53b9 100644
--- a/docs/Profiles.md
+++ b/docs/Control Profiles.md	
@@ -1,56 +1,59 @@
-# Profiles
+# Control Profiles
 
 A profile is a set of configuration settings.
 
-Currently three profiles are supported. The default profile is profile `1`.
+Currently, INAV gives you three control profiles. The default control profile is `1`.
 
-## Changing profiles
+## Changing contorl profiles
 ### Stick Commands
-Profiles can be selected using a GUI or the following stick combinations:
+Control profiles can be selected using a GUI or the following stick combinations:
 
-| Profile | Throttle | Yaw   | Pitch  | Roll   |
-| ------- | -------- | ----- | ------ | ------ |
-| 1       | Down     | Left  | Middle | Left   |
-| 2       | Down     | Left  | Up     | Middle |
-| 3       | Down     | Left  | Middle | Right  |
+| Profile # | Throttle | Yaw   | Pitch  | Roll   |
+| -------- | -------- | ----- | ------ | ------ |
+| 1        | Down     | Left  | Middle | Left   |
+| 2        | Down     | Left  | Up     | Middle |
+| 3        | Down     | Left  | Middle | Right  |
 
 ### CLI
-The CLI `profile` command can also be used to change profiles:
+The CLI `control_profile` command can also be used to change control profiles:
 
 ```
-profile <index>
+control_profile <index>
 ```
 
 ### Programming (4.0.0 onwards)
-You can change profiles using the programming frame work. This allows a lot of flexability in how you change profiles.
+You can change control profiles using the programming frame work. This allows a lot of flexability in how you change profiles.
 
 For example, using a simple switch on channel 15.
 
 [![For example, using a simple switch](https://i.imgur.com/SS9CaaOl.png)](https://i.imgur.com/SS9CaaO.png)
 
-Or using the speed to change profiles. In this example: 
-- when lower than 25 cm/s (basically not flying), profiles are not effected.
-- Below 2682 cm/s (60 mph | 97 Km/h) use Profile 1
-- Above 5364 cm/s (120 mph | 193 Km/h) use Profile 3
-- Between 2683 and 5364 cm/s, use Profile 2
+Or using the speed to change control profiles. In this example: 
+- when lower than 25 cm/s (basically not flying), control profiles are not effected.
+- Below 2682 cm/s (60 mph | 97 Km/h) use control profile 1
+- Above 5364 cm/s (120 mph | 193 Km/h) use control profile 3
+- Between 2683 and 5364 cm/s, use control profile 2
 
 [![Using speed to change profiles](https://i.imgur.com/WjkuhhWl.png)](https://i.imgur.com/WjkuhhW.png)
 
-#### Configurator use with profile changing logic.
+> [!NOTE]
+> From INAV 8.0, the programming framework operator is **Set Control Profile** and the **Flight** Operand is **Active Control Profile**. Pre-INAV 8.0, they were **Set Profile** and **Active Profile** respectively.
 
-If you have logic conditions that change the profiles. You may find that if you manually change the profile using the drop down boxes in the top right of Configurator; that they switch back to a different profile. This is because the logic conditions are still running in the background. If this is the case, the simplest solutuion is to temporarily disable the switches that trigger the `set profile` operations. Remember to re-enable these switches after you have made your changes.
+#### Configurator use with control profile changing logic.
+
+If you have logic conditions that change the profiles. You may find that if you manually change the control profile; using the drop down boxes in the top right of Configurator. That they switch back to a different control profile. This is because the logic conditions are still running in the background. If this is the case, the simplest solutuion is to temporarily disable the switches that trigger the `Set Control Profile` operations. Remember to re-enable these switches after you have made your changes.
 
 [![Disabled SET PROFILE switches](https://i.imgur.com/AeH9ll7l.png)](https://i.imgur.com/AeH9ll7.png)
 
 ## Profile Contents
-The values contained within a profile can be seen by using the CLI `dump profile` command.
+The values contained within a control profile can be seen by using the CLI `dump control_profile` command.
 
 e.g
 ```
-# dump profile
+# dump control_profile
 
-# profile
-profile 1
+# control_profile
+control_profile 1
 
 set mc_p_pitch = 40
 set mc_i_pitch = 30
diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md
index 9f95fdb25d5..e0199cfd714 100644
--- a/docs/Programming Framework.md	
+++ b/docs/Programming Framework.md	
@@ -44,13 +44,13 @@ IPF can be edited using INAV Configurator user interface, or via CLI. To use COn
 
 | Operation ID  | Name                          | Notes |
 |---------------|-------------------------------|-------|
-| 0             | TRUE                          | Always evaluates as true |
-| 1             | EQUAL                         | Evaluates `false` if `false` or `0` |
-| 2             | GREATER_THAN                  | `true` if `Operand A` is a higher value than `Operand B` |
-| 3             | LOWER_THAN                    | `true` if `Operand A` is a lower value than `Operand B` |
-| 4             | LOW                           | `true` if `<1333` |
-| 5             | MID                           | `true` if `>=1333 and <=1666` |
-| 6             | HIGH                          | `true` if `>1666` |
+| 0             | True                          | Always evaluates as true |
+| 1             | Equal (A=B)                   | Evaluates `false` if `false` or `0` |
+| 2             | Greater Than (A>B)            | `true` if `Operand A` is a higher value than `Operand B` |
+| 3             | Lower Than (A<B)              | `true` if `Operand A` is a lower value than `Operand B` |
+| 4             | Low                           | `true` if `<1333` |
+| 5             | Mid                           | `true` if `>=1333 and <=1666` |
+| 6             | High                          | `true` if `>1666` |
 | 7             | AND                           | `true` if `Operand A` and `Operand B` are the same value or both `true` |
 | 8             | OR                            | `true` if `Operand A` and/or `OperandB` is `true` |
 | 9             | XOR                           | `true` if `Operand A` or `Operand B` is `true`, but not both |
@@ -58,105 +58,109 @@ IPF can be edited using INAV Configurator user interface, or via CLI. To use COn
 | 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            | 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 |
+| 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            | 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`  |
+| 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 |
-| 25            | SET_VTX_POWER_LEVEL           | Sets VTX power level. Accepted values are `0-3` for SmartAudio and `0-4` for Tramp protocol |
-| 26            | INVERT_ROLL                   | Inverts ROLL axis input for PID/PIFF controller |
-| 27            | INVERT_PITCH                  | Inverts PITCH axis input for PID/PIFF controller  |
-| 28            | INVERT_YAW                    | Inverts YAW axis input for PID/PIFF controller |
-| 29            | OVERRIDE_THROTTLE             | Override throttle value that is fed to the motors by mixer. Operand is scaled in us. `1000` means throttle cut, `1500` means half throttle |
-| 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            | 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`. Note operand A should normally be set as a "Value", NOT as "Get RC Channel"|
-| 39            | SET_HEADING_TARGET            | Sets heading-hold target to `Operand A`, in degrees. Value wraps-around. |
-| 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            | 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. |
-| 48            | DELAY                         | Delays activation after being triggered. This will return `true` when `Operand A` _is_ true, and the delay time in `Operand B` [ms] has been exceeded. |
-| 49            | TIMER                         | A simple on - off timer. `true` for the duration of `Operand A` [ms]. Then `false` for the duration of `Operand B` [ms]. |
-| 50            | DELTA                         | This returns `true` when the value of `Operand A` has changed by the value of `Operand B` or greater within 100ms. |
-| 51            | APPROX_EQUAL                  | `true` if `Operand B` is within 1% of `Operand A`. |
-| 52            | LED_PIN_PWM                   | Value `Operand A` from [`0` : `100`] starts PWM generation on LED Pin. See [LED pin PWM](LED%20pin%20PWM.md). Any other value stops PWM generation (stop to allow ws2812 LEDs updates in shared modes)|
+| 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 |
+| 25            | Set VTx Power Level           | Sets VTX power level. Accepted values are `0-3` for SmartAudio and `0-4` for Tramp protocol |
+| 26            | Invert Roll                   | Inverts ROLL axis input for PID/PIFF controller |
+| 27            | Invert Pitch                  | Inverts PITCH axis input for PID/PIFF controller  |
+| 28            | Invert Yaw                    | Inverts YAW axis input for PID/PIFF controller |
+| 29            | Override Throttlw             | Override throttle value that is fed to the motors by mixer. Operand is scaled in us. `1000` means throttle cut, `1500` means half throttle |
+| 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            | 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            | Override RC Channel           | Overrides channel set by `Operand A` to value of `Operand B`. Note operand A should normally be set as a "Value", NOT as "Get RC Channel"|
+| 39            | Set Heading Target            | Sets heading-hold target to `Operand A`, in centidegrees. Value wraps-around. |
+| 40            | Modulo                        | Modulo. Divide `Operand A` by `Operand B` and returns the remainder |
+| 41            | Override Loiter Radius        | 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 Control 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            | 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. |
+| 48            | Delay                         | Delays activation after being triggered. This will return `true` when `Operand A` _is_ true, and the delay time in `Operand B` [ms] has been exceeded. |
+| 49            | Timer                         | A simple on - off timer. `true` for the duration of `Operand A` [ms]. Then `false` for the duration of `Operand B` [ms]. |
+| 50            | Delta (|A| >= B)              | This returns `true` when the value of `Operand A` has changed by the value of `Operand B` or greater within 100ms. |
+| 51            | Approx Equals (A ~ B)         | `true` if `Operand B` is within 1% of `Operand A`. |
+| 52            | LED Pin PWM                   | Value `Operand A` from [`0` : `100`] starts PWM generation on LED Pin. See [LED pin PWM](LED%20pin%20PWM.md). Any other value stops PWM generation (stop to allow ws2812 LEDs updates in shared modes). |
+| 53            | Disable GPS Sensor Fix        | Disables the GNSS sensor fix. For testing GNSS failure. |
+| 54            | Mag calibration               | Trigger a magnetometer calibration. |
 
 ### Operands
 
 | Operand Type  | Name                  | Notes |
 |---------------|-----------------------|-------|
-| 0             | VALUE                 | Value derived from `value` field |
-| 1             | GET_RC_CHANNEL        | `value` points to RC channel number, indexed from 1 |
-| 2             | FLIGHT                | `value` points to flight parameter table |
-| 3             | FLIGHT_MODE           | `value` points to flight modes table |
-| 4             | LC                    | `value` points to other logic condition ID |
-| 5             | GVAR                  | Value stored in Global Variable indexed by `value`. `GVAR 1` means: value in GVAR 1 |
-| 5             | PID                   | Output of a Programming PID indexed by `value`. `PID 1` means: value in PID 1 |
-
-#### FLIGHT
-
-| Operand Value | Name                          | Notes |
-|---------------|-------------------------------|-------|
-| 0             | ARM_TIMER                     | in `seconds` |
-| 1             | HOME_DISTANCE                 | in `meters` |
-| 2             | TRIP_DISTANCE                 | in `meters` |
-| 3             | RSSI                          |  |
-| 4             | VBAT                          | in `Volts * 100`, eg. `12.1V` is `1210` |
-| 5             | CELL_VOLTAGE                  | in `Volts * 100`, eg. `12.1V` is `1210` |
-| 6             | CURRENT                       | in `Amps * 100`, eg. `9A` is `900` |
-| 7             | MAH_DRAWN                     | in `mAh`                              |
-| 8             | GPS_SATS                      |  |
-| 9             | GROUD_SPEED                   | in `cm/s` |
-| 10            | 3D_SPEED                      | in `cm/s` |
-| 11            | AIR_SPEED                     | in `cm/s` |
-| 12            | ALTITUDE                      | in `cm` |
-| 13            | VERTICAL_SPEED                | in `cm/s` |
-| 14            | TROTTLE_POS                   | in `%` |
-| 15            | ATTITUDE_ROLL                 | in `degrees` |
-| 16            | ATTITUDE_PITCH                | in `degrees` |
-| 17            | IS_ARMED                      | boolean `0`/`1` |
-| 18            | IS_AUTOLAUNCH                 | boolean `0`/`1` |
-| 19            | IS_ALTITUDE_CONTROL           | boolean `0`/`1` |
-| 20            | IS_POSITION_CONTROL           | boolean `0`/`1` |
-| 21            | IS_EMERGENCY_LANDING          | boolean `0`/`1` |
-| 22            | IS_RTH                        | boolean `0`/`1` |
-| 23            | IS_LANDING                    | boolean `0`/`1` |
-| 24            | IS_FAILSAFE                   | boolean `0`/`1` |
-| 25            | STABILIZED_ROLL               | Roll PID controller output `[-500:500]` |
-| 26            | STABILIZED_PITCH              | Pitch PID controller output `[-500:500]` |
-| 27            | STABILIZED_YAW                | Yaw PID controller output `[-500:500]` |
-| 28            | 3D HOME_DISTANCE              | in `meters`, calculated from HOME_DISTANCE and ALTITUDE using Pythagorean theorem |
-| 29            | CROSSFIRE LQ                  | Crossfire Link quality as returned by the CRSF protocol | 
-| 30            | CROSSFIRE SNR                 | Crossfire SNR as returned by the CRSF protocol |
-| 31            | GPS_VALID                     | boolean `0`/`1`. True when the GPS has a valid 3D Fix |
-| 32            | LOITER_RADIUS                 | The current loiter radius in cm. |
-| 33            | ACTIVE_PROFILE                | integer for the active config profile `[1..MAX_PROFILE_COUNT]` |
-| 34            | BATT_CELLS                    | Number of battery cells detected |
-| 35            | AGL_STATUS                    | boolean `1` when AGL can be trusted, `0` when AGL estimate can not be trusted |
-| 36            | AGL                           | integer Above The Groud Altitude in `cm` |
-| 37            | RANGEFINDER_RAW               | integer raw distance provided by the rangefinder in `cm` |
-| 38            | ACTIVE_MIXER_PROFILE          | Which mixers are currently active (for vtol etc) |
-| 39            | MIXER_TRANSITION_ACTIVE       | Currently switching between mixers (quad to plane etc) |
-| 40            | ATTITUDE_YAW                  | current heading (yaw) in `degrees` |
-| 41            | FW Land Sate                  | integer `1` - `5`, indicates the status of the FW landing, 0 Idle, 1 Downwind, 2 Base Leg, 3 Final Approach, 4 Glide, 5 Flare |
+| 0             | Value                 | Value derived from `value` field |
+| 1             | Get RC Channel        | `value` points to RC channel number, indexed from 1 |
+| 2             | Flight                | `value` points to **Flight** Parameters table |
+| 3             | Flight Mode           | `value` points to **Flight_Mode** table |
+| 4             | Logic Condition       | `value` points to other logic condition ID |
+| 5             | Get Global Variable   | Value stored in Global Variable indexed by `value`. `GVAR 1` means: value in GVAR 1 |
+| 5             | Programming PID       | Output of a Programming PID indexed by `value`. `PID 1` means: value in PID 1 |
+| 6             | Waypoints             | `value` points to the **Waypoint** parameter table |
+
+#### Flight Parameters
+
+| Operand Value | Name                                  | Notes |
+|---------------|---------------------------------------|-------|
+| 0             | ARM Timer [s]                         | Time since armed in `seconds` |
+| 1             | Home Distance [m]                     | distance from home in `meters` |
+| 2             | Trip distance [m]                     | Trip distance in `meters` |
+| 3             | RSSI                                  |  |
+| 4             | Vbat [centi-Volt] [1V = 100]          | VBAT Voltage in `Volts * 100`, eg. `12.1V` is `1210` |
+| 5             | Cell voltage [centi-Volt] [1V = 100]  | Average cell voltage in `Volts * 100`, eg. `12.1V` is `1210` |
+| 6             | Current [centi-Amp] [1A = 100]        | Current in `Amps * 100`, eg. `9A` is `900` |
+| 7             | Current drawn [mAh]                   | Total used current in `mAh` |
+| 8             | GPS Sats                              |  |
+| 9             | Ground speed [cm/s]                   | Ground speed in `cm/s` |
+| 10            | 3D speed [cm/s]                       | 3D speed in `cm/s` |
+| 11            | Air speed [cm/s]                      | Air speed in `cm/s` |
+| 12            | Altitude [cm]                         | Altitude in `cm` |
+| 13            | Vertical speed [cm/s]                 | Vertical speed in `cm/s` |
+| 14            | Throttle position [%]                 | Throttle position in `%` |
+| 15            | Roll [deg]                            | Roll attitude in `degrees` |
+| 16            | Pitch [deg]                           | Pitch attitude in `degrees` |
+| 17            | Is Armed                              | Is the system armed? boolean `0`/`1` |
+| 18            | Is Autolaunch                         | Is auto launch active? boolean `0`/`1` |
+| 19            | Is Controlling Altitude               | Is altitude being controlled? boolean `0`/`1` |
+| 20            | Is Controlling Position               | Is the position being controlled? boolean `0`/`1` |
+| 21            | Is Emergency Landing                  | Is the aircraft emergency landing? boolean `0`/`1` |
+| 22            | Is RTH                                | Is RTH active? boolean `0`/`1` |
+| 23            | Is Landing                            | Is the aircaft automatically landing? boolean `0`/`1` |
+| 24            | Is Failsafe                           | Is the flight controller in a failsafe? boolean `0`/`1` |
+| 25            | Stabilized Roll                       | Roll PID controller output `[-500:500]` |
+| 26            | Stabilized Pitch                      | Pitch PID controller output `[-500:500]` |
+| 27            | Stabilized Yaw                        | Yaw PID controller output `[-500:500]` |
+| 28            | 3D home distance [m]                  | 3D distance to home in `meters`. Calculated from Home distance and Altitude using Pythagorean theorem |
+| 29            | CRSF LQ                               | Link quality as returned by the CRSF protocol | 
+| 30            | CRSF SNR                              | SNR as returned by the CRSF protocol |
+| 31            | GPS Valid Fix                         | Boolean `0`/`1`. True when the GPS has a valid 3D Fix |
+| 32            | Loiter Radius [cm]                    | The current loiter radius in cm. |
+| 33            | Active Control Profile                | Integer for the active config profile `[1..MAX_PROFILE_COUNT]` |
+| 34            | Battery cells                         | Number of battery cells detected |
+| 35            | AGL status [0/1]                      | Boolean `1` when AGL can be trusted, `0` when AGL estimate can not be trusted |
+| 36            | AGL [cm]                              | Integer altitude above The Groud Altitude in `cm` |
+| 37            | Rangefinder [cm]                      | Integer raw distance provided by the rangefinder in `cm` |
+| 38            | Active Mixer Profile                  | Which mixer is currently active (for vtol etc) |
+| 39            | Mixer Transition Active               | Boolean `0`/`1`. Are we currently switching between mixers (quad to plane etc) |
+| 40            | Yaw [deg]                             | Current heading (yaw) in `degrees` |
+| 41            | FW Land Sate                          | Integer `1` - `5`, indicates the status of the FW landing, 0 Idle, 1 Downwind, 2 Base Leg, 3 Final Approach, 4 Glide, 5 Flare |
+| 42            | Current battery profile               | The active battery profile. Integer `[1..MAX_PROFILE_COUNT]` |
 
 #### FLIGHT_MODE
 
@@ -164,22 +168,22 @@ The flight mode operands return `true` when the mode is active. These are modes
 
 | Operand Value | Name              | Notes |
 |---------------|-------------------|-------|
-| 0             | FAILSAFE          | `true` when a **Failsafe** state has been triggered. |
-| 1             | MANUAL            | `true` when you are in the **Manual** flight mode. |
+| 0             | Failsafe          | `true` when a **Failsafe** state has been triggered. |
+| 1             | Manual            | `true` when you are in the **Manual** flight mode. |
 | 2             | RTH               | `true` when you are in the **Return to Home** flight mode. |
-| 3             | POSHOLD           | `true` when you are in the **Position Hold** or **Loiter** flight modes. |
-| 4             | CRUISE            | `true` when you are in the **Cruise** flight mode. |
-| 5             | ALTHOLD           | `true` when you the **Altitude Hold** flight mode modifier is active. |
-| 6             | ANGLE             | `true` when you are in the **Angle** flight mode. |
-| 7             | HORIZON           | `true` when you are in the **Horizon** flight mode. |
-| 8             | AIR               | `true` when you the **Airmode** flight mode modifier is active. |
-| 9             | USER1             | `true` when the **USER 1** mode is active. |
-| 10            | USER2             | `true` when the **USER 2** mode is active. |
-| 11            | COURSE_HOLD       | `true` when you are in the **Course Hold** flight mode. |
-| 12            | USER3             | `true` when the **USER 3** mode is active. |
-| 13            | USER4             | `true` when the **USER 4** mode is active. |
-| 14            | ACRO              | `true` when you are in the **Acro** flight mode. |
-| 15            | WAYPOINT_MISSION  | `true` when you are in the **WP Mission** flight mode. | 
+| 3             | Position Hold     | `true` when you are in the **Position Hold** or **Loiter** flight modes. |
+| 4             | Cruise            | `true` when you are in the **Cruise** flight mode. |
+| 5             | Altitude Hold     | `true` when you the **Altitude Hold** flight mode modifier is active. |
+| 6             | Angle             | `true` when you are in the **Angle** flight mode. |
+| 7             | Horizon           | `true` when you are in the **Horizon** flight mode. |
+| 8             | Air               | `true` when you the **Airmode** flight mode modifier is active. |
+| 9             | USER 1            | `true` when the **USER 1** mode is active. |
+| 10            | USER 2            | `true` when the **USER 2** mode is active. |
+| 11            | Course Hold       | `true` when you are in the **Course Hold** flight mode. |
+| 12            | USER 3            | `true` when the **USER 3** mode is active. |
+| 13            | USER 4            | `true` when the **USER 4** mode is active. |
+| 14            | Acro              | `true` when you are in the **Acro** flight mode. |
+| 15            | Waypoint Mission  | `true` when you are in the **WP Mission** flight mode. | 
 
 #### WAYPOINTS
 
@@ -335,5 +339,3 @@ choose *value* and enter the channel number. Choosing "get RC value" is a common
 which does something other than what you probably want.
 
 ![screenshot of override an RC channel with a value](./assets/images/ipf_set_get_rc_channel.png)
-
-
diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index 3bedf7d324d..aba1f58b905 100644
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -293,7 +293,7 @@ static void cliPutp(void *p, char ch)
 
 typedef enum {
     DUMP_MASTER = (1 << 0),
-    DUMP_PROFILE = (1 << 1),
+    DUMP_CONTROL_PROFILE = (1 << 1),
     DUMP_BATTERY_PROFILE = (1 << 2),
     DUMP_MIXER_PROFILE = (1 << 3),
     DUMP_ALL = (1 << 4),
@@ -3336,30 +3336,30 @@ static void cliPlaySound(char *cmdline)
     beeper(beeperModeForTableIndex(i));
 }
 
-static void cliProfile(char *cmdline)
+static void cliControlProfile(char *cmdline)
 {
     // CLI profile index is 1-based
     if (isEmpty(cmdline)) {
-        cliPrintLinef("profile %d", getConfigProfile() + 1);
+        cliPrintLinef("control_profile %d", getConfigProfile() + 1);
         return;
     } else {
         const int i = fastA2I(cmdline) - 1;
         if (i >= 0 && i < MAX_PROFILE_COUNT) {
             setConfigProfileAndWriteEEPROM(i);
-            cliProfile("");
+            cliControlProfile("");
         }
     }
 }
 
-static void cliDumpProfile(uint8_t profileIndex, uint8_t dumpMask)
+static void cliDumpControlProfile(uint8_t profileIndex, uint8_t dumpMask)
 {
     if (profileIndex >= MAX_PROFILE_COUNT) {
         // Faulty values
         return;
     }
     setConfigProfile(profileIndex);
-    cliPrintHashLine("profile");
-    cliPrintLinef("profile %d\r\n", getConfigProfile() + 1);
+    cliPrintHashLine("control_profile");
+    cliPrintLinef("control_profile %d\r\n", getConfigProfile() + 1);
     dumpAllValues(PROFILE_VALUE, dumpMask);
     dumpAllValues(CONTROL_RATE_VALUE, dumpMask);
     dumpAllValues(EZ_TUNE_VALUE, dumpMask);
@@ -3981,12 +3981,12 @@ static void printConfig(const char *cmdline, bool doDiff)
     const char *options;
     if ((options = checkCommand(cmdline, "master"))) {
         dumpMask = DUMP_MASTER; // only
-    } else if ((options = checkCommand(cmdline, "profile"))) {
-        dumpMask = DUMP_PROFILE; // only
-    } else if ((options = checkCommand(cmdline, "battery_profile"))) {
-        dumpMask = DUMP_BATTERY_PROFILE; // only
+    } else if ((options = checkCommand(cmdline, "control_profile"))) {
+        dumpMask = DUMP_CONTROL_PROFILE; // only
     } else if ((options = checkCommand(cmdline, "mixer_profile"))) {
         dumpMask = DUMP_MIXER_PROFILE; // only
+    } else if ((options = checkCommand(cmdline, "battery_profile"))) {
+        dumpMask = DUMP_BATTERY_PROFILE; // only
     } else if ((options = checkCommand(cmdline, "all"))) {
         dumpMask = DUMP_ALL;   // all profiles and rates
     } else {
@@ -3997,16 +3997,16 @@ static void printConfig(const char *cmdline, bool doDiff)
         dumpMask = dumpMask | DO_DIFF;
     }
 
-    const int currentProfileIndexSave = getConfigProfile();
-    const int currentBatteryProfileIndexSave = getConfigBatteryProfile();
+    const int currentControlProfileIndexSave = getConfigProfile();
     const int currentMixerProfileIndexSave = getConfigMixerProfile();
+    const int currentBatteryProfileIndexSave = getConfigBatteryProfile();
     backupConfigs();
     // reset all configs to defaults to do differencing
     resetConfigs();
     // restore the profile indices, since they should not be reset for proper comparison
-    setConfigProfile(currentProfileIndexSave);
-    setConfigBatteryProfile(currentBatteryProfileIndexSave);
+    setConfigProfile(currentControlProfileIndexSave);
     setConfigMixerProfile(currentMixerProfileIndexSave);
+    setConfigBatteryProfile(currentBatteryProfileIndexSave);
 
     if (checkCommand(options, "showdefaults")) {
         dumpMask = dumpMask | SHOW_DEFAULTS;   // add default values as comments for changed values
@@ -4128,25 +4128,25 @@ static void printConfig(const char *cmdline, bool doDiff)
 
         if (dumpMask & DUMP_ALL) {
             // dump all profiles
-            const int currentProfileIndexSave = getConfigProfile();
-            const int currentBatteryProfileIndexSave = getConfigBatteryProfile();
+            const int currentControlProfileIndexSave = getConfigProfile();
             const int currentMixerProfileIndexSave = getConfigMixerProfile();
+            const int currentBatteryProfileIndexSave = getConfigBatteryProfile();
+            for (int ii = 0; ii < MAX_PROFILE_COUNT; ++ii) {
+                cliDumpControlProfile(ii, dumpMask);
+            }
             for (int ii = 0; ii < MAX_MIXER_PROFILE_COUNT; ++ii) {
                 cliDumpMixerProfile(ii, dumpMask);
             }
-            for (int ii = 0; ii < MAX_PROFILE_COUNT; ++ii) {
-                cliDumpProfile(ii, dumpMask);
-            }
             for (int ii = 0; ii < MAX_BATTERY_PROFILE_COUNT; ++ii) {
                 cliDumpBatteryProfile(ii, dumpMask);
             }
-            setConfigProfile(currentProfileIndexSave);
-            setConfigBatteryProfile(currentBatteryProfileIndexSave);
+            setConfigProfile(currentControlProfileIndexSave);
             setConfigMixerProfile(currentMixerProfileIndexSave);
+            setConfigBatteryProfile(currentBatteryProfileIndexSave);
 
             cliPrintHashLine("restore original profile selection");
+            cliPrintLinef("profile %d", currentControlProfileIndexSave + 1);
             cliPrintLinef("mixer_profile %d", currentMixerProfileIndexSave + 1);
-            cliPrintLinef("profile %d", currentProfileIndexSave + 1);
             cliPrintLinef("battery_profile %d", currentBatteryProfileIndexSave + 1);
 
 #ifdef USE_CLI_BATCH
@@ -4154,18 +4154,19 @@ static void printConfig(const char *cmdline, bool doDiff)
 #endif
         } else {
             // dump just the current profiles
+            cliDumpControlProfile(getConfigProfile(), dumpMask);
             cliDumpMixerProfile(getConfigMixerProfile(), dumpMask);
-            cliDumpProfile(getConfigProfile(), dumpMask);
             cliDumpBatteryProfile(getConfigBatteryProfile(), dumpMask);
         }
     }
+
+    if (dumpMask & DUMP_CONTROL_PROFILE) {
+        cliDumpControlProfile(getConfigProfile(), dumpMask);
+    }
+
     if (dumpMask & DUMP_MIXER_PROFILE) {
         cliDumpMixerProfile(getConfigMixerProfile(), dumpMask);
     }
-    
-    if (dumpMask & DUMP_PROFILE) {
-        cliDumpProfile(getConfigProfile(), dumpMask);
-    }
 
     if (dumpMask & DUMP_BATTERY_PROFILE) {
         cliDumpBatteryProfile(getConfigBatteryProfile(), dumpMask);
@@ -4279,9 +4280,9 @@ const clicmd_t cmdTable[] = {
     CLI_COMMAND_DEF("defaults", "reset to defaults and reboot", NULL, cliDefaults),
     CLI_COMMAND_DEF("dfu", "DFU mode on reboot", NULL, cliDfu),
     CLI_COMMAND_DEF("diff", "list configuration changes from default",
-        "[master|battery_profile|profile|rates|all] {showdefaults}", cliDiff),
+        "[master|battery_profile|control_profile|mixer_profile|rates|all] {showdefaults}", cliDiff),
     CLI_COMMAND_DEF("dump", "dump configuration",
-        "[master|battery_profile|profile|rates|all] {showdefaults}", cliDump),
+        "[master|battery_profile|control_profile|mixer_profile|rates|all] {showdefaults}", cliDump),
 #ifdef USE_RX_ELERES
     CLI_COMMAND_DEF("eleres_bind", NULL, NULL, cliEleresBind),
 #endif // USE_RX_ELERES
@@ -4322,12 +4323,9 @@ const clicmd_t cmdTable[] = {
     CLI_COMMAND_DEF("msc", "switch into msc mode", NULL, cliMsc),
 #endif
     CLI_COMMAND_DEF("play_sound", NULL, "[<index>]\r\n", cliPlaySound),
-    CLI_COMMAND_DEF("profile", "change profile",
-        "[<index>]", cliProfile),
-    CLI_COMMAND_DEF("battery_profile", "change battery profile",
-        "[<index>]", cliBatteryProfile),
-    CLI_COMMAND_DEF("mixer_profile", "change mixer profile",
-        "[<index>]", cliMixerProfile),
+    CLI_COMMAND_DEF("control_profile", "change control profile", "[<index>]", cliControlProfile),
+    CLI_COMMAND_DEF("mixer_profile", "change mixer profile", "[<index>]", cliMixerProfile),
+    CLI_COMMAND_DEF("battery_profile", "change battery profile", "[<index>]", cliBatteryProfile),
     CLI_COMMAND_DEF("resource", "view currently used resources", NULL, cliResource),
     CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange),
 #if defined(USE_SAFE_HOME)

From c012de6ca6295d16ac5dc8074f9476e9e77037a9 Mon Sep 17 00:00:00 2001
From: Darren Lines <darren@darrenlines.uk>
Date: Sun, 2 Jun 2024 19:59:24 +0100
Subject: [PATCH 165/323] Make `battery_capacity_unit` a global setting

Remove `battery_capacity_unit` from battery profiles.
---
 docs/Battery.md                           |  3 ---
 src/main/fc/fc_msp.c                      | 16 ++++++++--------
 src/main/fc/settings.yaml                 | 12 ++++++------
 src/main/flight/rth_estimator.c           |  2 +-
 src/main/io/osd.c                         |  6 +++---
 src/main/sensors/battery.c                |  9 +++++----
 src/main/sensors/battery_config_structs.h | 21 +++++++++++----------
 7 files changed, 34 insertions(+), 35 deletions(-)

diff --git a/docs/Battery.md b/docs/Battery.md
index f6bdbd50391..e519c325410 100644
--- a/docs/Battery.md
+++ b/docs/Battery.md
@@ -201,7 +201,6 @@ Up to 3 battery profiles are supported. You can select the battery profile from
 - `vbat_max_cell_voltage`
 - `vbat_warning_cell_voltage`
 - `vbat_min_cell_voltage`
-- `battery_capacity_unit`
 - `battery_capacity`
 - `battery_capacity_warning`
 - `battery_capacity_critical`
@@ -253,7 +252,6 @@ feature BAT_PROF_AUTOSWITCH
 battery_profile 1
 
 set bat_cells = 3
-set battery_capacity_unit = MAH
 set battery_capacity = 2200
 set battery_capacity_warning = 440
 set battery_capacity_critical = 220
@@ -262,7 +260,6 @@ set battery_capacity_critical = 220
 battery_profile 2
 
 set bat_cells = 4
-set battery_capacity_unit = MAH
 set battery_capacity = 1500
 set battery_capacity_warning = 300
 set battery_capacity_critical = 150
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 4ae42184816..2109d3e08a9 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -856,7 +856,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         sbufWriteU32(dst, currentBatteryProfile->capacity.value);
         sbufWriteU32(dst, currentBatteryProfile->capacity.warning);
         sbufWriteU32(dst, currentBatteryProfile->capacity.critical);
-        sbufWriteU8(dst, currentBatteryProfile->capacity.unit);
+        sbufWriteU8(dst, batteryMetersConfig()->capacity_unit);
         break;
 
     case MSP2_INAV_MISC2:
@@ -895,7 +895,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         sbufWriteU32(dst, currentBatteryProfile->capacity.value);
         sbufWriteU32(dst, currentBatteryProfile->capacity.warning);
         sbufWriteU32(dst, currentBatteryProfile->capacity.critical);
-        sbufWriteU8(dst, currentBatteryProfile->capacity.unit);
+        sbufWriteU8(dst, batteryMetersConfig()->capacity_unit);
         break;
 
 #ifdef USE_GPS
@@ -2081,13 +2081,13 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             currentBatteryProfileMutable->capacity.value = sbufReadU32(src);
             currentBatteryProfileMutable->capacity.warning = sbufReadU32(src);
             currentBatteryProfileMutable->capacity.critical = sbufReadU32(src);
-            currentBatteryProfileMutable->capacity.unit = sbufReadU8(src);
+            batteryMetersConfigMutable()->capacity_unit = sbufReadU8(src);
             if ((batteryMetersConfig()->voltageSource != BAT_VOLTAGE_RAW) && (batteryMetersConfig()->voltageSource != BAT_VOLTAGE_SAG_COMP)) {
                 batteryMetersConfigMutable()->voltageSource = BAT_VOLTAGE_RAW;
                 return MSP_RESULT_ERROR;
             }
-            if ((currentBatteryProfile->capacity.unit != BAT_CAPACITY_UNIT_MAH) && (currentBatteryProfile->capacity.unit != BAT_CAPACITY_UNIT_MWH)) {
-                currentBatteryProfileMutable->capacity.unit = BAT_CAPACITY_UNIT_MAH;
+            if ((batteryMetersConfig()->capacity_unit != BAT_CAPACITY_UNIT_MAH) && (batteryMetersConfig()->capacity_unit != BAT_CAPACITY_UNIT_MWH)) {
+                batteryMetersConfigMutable()->capacity_unit = BAT_CAPACITY_UNIT_MAH;
                 return MSP_RESULT_ERROR;
             }
         } else
@@ -2120,13 +2120,13 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             currentBatteryProfileMutable->capacity.value = sbufReadU32(src);
             currentBatteryProfileMutable->capacity.warning = sbufReadU32(src);
             currentBatteryProfileMutable->capacity.critical = sbufReadU32(src);
-            currentBatteryProfileMutable->capacity.unit = sbufReadU8(src);
+            batteryMetersConfigMutable()->capacity_unit = sbufReadU8(src);
             if ((batteryMetersConfig()->voltageSource != BAT_VOLTAGE_RAW) && (batteryMetersConfig()->voltageSource != BAT_VOLTAGE_SAG_COMP)) {
                 batteryMetersConfigMutable()->voltageSource = BAT_VOLTAGE_RAW;
                 return MSP_RESULT_ERROR;
             }
-            if ((currentBatteryProfile->capacity.unit != BAT_CAPACITY_UNIT_MAH) && (currentBatteryProfile->capacity.unit != BAT_CAPACITY_UNIT_MWH)) {
-                currentBatteryProfileMutable->capacity.unit = BAT_CAPACITY_UNIT_MAH;
+            if ((batteryMetersConfig()->capacity_unit != BAT_CAPACITY_UNIT_MAH) && (batteryMetersConfig()->capacity_unit != BAT_CAPACITY_UNIT_MWH)) {
+                batteryMetersConfigMutable()->capacity_unit = BAT_CAPACITY_UNIT_MAH;
                 return MSP_RESULT_ERROR;
             }
         } else
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index d9aa83c93cf..3c192495751 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -890,6 +890,12 @@ groups:
         condition: USE_ADC
         min: 0
         max: 65535
+      - name: battery_capacity_unit
+        description: "Unit used for `battery_capacity`, `battery_capacity_warning` and `battery_capacity_critical` [MAH/MWH] (milliAmpere hour / milliWatt hour)."
+        default_value: "MAH"
+        field: capacity_unit
+        table: bat_capacity_unit
+        type: uint8_t
       - name: current_meter_scale
         description: "This sets the output voltage to current scaling for the current sensor in 0.1 mV/A steps. 400 is 40mV/A such as the ACS756 sensor outputs. 183 is the setting for the uberdistro with a 0.25mOhm shunt."
         default_value: :target
@@ -996,12 +1002,6 @@ groups:
         field: capacity.critical
         min: 0
         max: 4294967295
-      - name: battery_capacity_unit
-        description: "Unit used for `battery_capacity`, `battery_capacity_warning` and `battery_capacity_critical` [MAH/MWH] (milliAmpere hour / milliWatt hour)."
-        default_value: "MAH"
-        field: capacity.unit
-        table: bat_capacity_unit
-        type: uint8_t
       - name: controlrate_profile
         description: "Control rate profile to switch to when the battery profile is selected, 0 to disable and keep the currently selected control rate profile"
         default_value: 0
diff --git a/src/main/flight/rth_estimator.c b/src/main/flight/rth_estimator.c
index 708c71bd1cf..e0ca0fba20d 100644
--- a/src/main/flight/rth_estimator.c
+++ b/src/main/flight/rth_estimator.c
@@ -222,7 +222,7 @@ float calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount) {
 
     // check requirements
     const bool areBatterySettingsOK = feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER) && batteryWasFullWhenPluggedIn();
-    const bool areRTHEstimatorSettingsOK = batteryMetersConfig()->cruise_power > 0 && currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH &&currentBatteryProfile->capacity.value > 0 && navConfig()->fw.cruise_speed > 0;
+    const bool areRTHEstimatorSettingsOK = batteryMetersConfig()->cruise_power > 0 && batteryMetersConfig()->capacity_unit == BAT_CAPACITY_UNIT_MWH && currentBatteryProfile->capacity.value > 0 && navConfig()->fw.cruise_speed > 0;
     const bool isNavigationOK = navigationPositionEstimateIsHealthy() && isImuHeadingValid();
 
     if (!(areBatterySettingsOK && areRTHEstimatorSettingsOK && isNavigationOK)) {
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 34135ce2fd7..a3a298962a3 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -1780,7 +1780,7 @@ static bool osdDrawSingleElement(uint8_t item)
             tfp_sprintf(buff, "  NA");
         else if (!batteryWasFullWhenPluggedIn())
             tfp_sprintf(buff, "  NF");
-        else if (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH) {
+        else if (batteryMetersConfig()->capacity_unit == BAT_CAPACITY_UNIT_MAH) {
             uint8_t mah_digits = osdConfig()->mAh_precision; // Initialize to config value
 
 #ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it
@@ -1803,11 +1803,11 @@ static bool osdDrawSingleElement(uint8_t item)
                 buff[mah_digits + 1] = '\0';
                 unitsDrawn = true;
             }
-        } else // currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH
+        } else // batteryMetersConfig()->capacityUnit == BAT_CAPACITY_UNIT_MWH
             osdFormatCentiNumber(buff + 1, getBatteryRemainingCapacity() / 10, 0, 2, 0, 3, false);
 
         if (!unitsDrawn) {
-        buff[4] = currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH ? SYM_MAH : SYM_WH;
+        buff[4] = batteryMetersConfig()->capacity_unit == BAT_CAPACITY_UNIT_MAH ? SYM_MAH : SYM_WH;
         buff[5] = '\0';
         }
 
diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c
index 2ca7024694e..a29fa2e07cf 100644
--- a/src/main/sensors/battery.c
+++ b/src/main/sensors/battery.c
@@ -97,7 +97,7 @@ static int32_t mWhDrawn = 0;                    // energy (milliWatt hours) draw
 batteryState_e batteryState;
 const batteryProfile_t *currentBatteryProfile;
 
-PG_REGISTER_ARRAY_WITH_RESET_FN(batteryProfile_t, MAX_BATTERY_PROFILE_COUNT, batteryProfiles, PG_BATTERY_PROFILES, 2);
+PG_REGISTER_ARRAY_WITH_RESET_FN(batteryProfile_t, MAX_BATTERY_PROFILE_COUNT, batteryProfiles, PG_BATTERY_PROFILES, 3);
 
 void pgResetFn_batteryProfiles(batteryProfile_t *instance)
 {
@@ -118,7 +118,6 @@ void pgResetFn_batteryProfiles(batteryProfile_t *instance)
                 .value = SETTING_BATTERY_CAPACITY_DEFAULT,
                 .warning = SETTING_BATTERY_CAPACITY_WARNING_DEFAULT,
                 .critical = SETTING_BATTERY_CAPACITY_CRITICAL_DEFAULT,
-                .unit = SETTING_BATTERY_CAPACITY_UNIT_DEFAULT,
             },
 
             .controlRateProfile = 0,
@@ -167,7 +166,7 @@ void pgResetFn_batteryProfiles(batteryProfile_t *instance)
     }
 }
 
-PG_REGISTER_WITH_RESET_TEMPLATE(batteryMetersConfig_t, batteryMetersConfig, PG_BATTERY_METERS_CONFIG, 1);
+PG_REGISTER_WITH_RESET_TEMPLATE(batteryMetersConfig_t, batteryMetersConfig, PG_BATTERY_METERS_CONFIG, 2);
 
 PG_RESET_TEMPLATE(batteryMetersConfig_t, batteryMetersConfig,
 
@@ -186,6 +185,8 @@ PG_RESET_TEMPLATE(batteryMetersConfig_t, batteryMetersConfig,
 
     .voltageSource = SETTING_BAT_VOLTAGE_SRC_DEFAULT,
 
+    .capacity_unit = SETTING_BATTERY_CAPACITY_UNIT_DEFAULT,
+
     .cruise_power = SETTING_CRUISE_POWER_DEFAULT,
     .idle_power = SETTING_IDLE_POWER_DEFAULT,
     .rth_energy_margin = SETTING_RTH_ENERGY_MARGIN_DEFAULT,
@@ -405,7 +406,7 @@ void batteryUpdate(timeUs_t timeDelta)
 
         if ((currentBatteryProfile->capacity.value > 0) && batteryFullWhenPluggedIn) {
             uint32_t capacityDiffBetweenFullAndEmpty = currentBatteryProfile->capacity.value - currentBatteryProfile->capacity.critical;
-            int32_t drawn = (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH ? mWhDrawn : mAhDrawn);
+            int32_t drawn = (batteryMetersConfig()->capacity_unit == BAT_CAPACITY_UNIT_MWH ? mWhDrawn : mAhDrawn);
             batteryRemainingCapacity = (drawn > (int32_t)capacityDiffBetweenFullAndEmpty ? 0 : capacityDiffBetweenFullAndEmpty - drawn);
         }
 
diff --git a/src/main/sensors/battery_config_structs.h b/src/main/sensors/battery_config_structs.h
index 81eafef44ad..829ac8d8794 100644
--- a/src/main/sensors/battery_config_structs.h
+++ b/src/main/sensors/battery_config_structs.h
@@ -61,16 +61,18 @@ typedef struct batteryMetersConfig_s {
 #endif
 
     struct {
-        int16_t scale;          // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A
-        int16_t offset;         // offset of the current sensor in millivolt steps
-        currentSensor_e type;   // type of current meter used, either ADC or virtual
+        int16_t scale;                  // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A
+        int16_t offset;                 // offset of the current sensor in millivolt steps
+        currentSensor_e type;           // type of current meter used, either ADC or virtual
     } current;
 
     batVoltageSource_e voltageSource;
 
-    uint32_t cruise_power;      // power drawn by the motor(s) at cruise throttle/speed (cW)
-    uint16_t idle_power;        // power drawn by the system when the motor(s) are stopped (cW)
-    uint8_t rth_energy_margin;  // Energy that should be left after RTH (%), used for remaining time/distance before RTH
+    batCapacityUnit_e capacity_unit;    // Describes unit of capacity.value, capacity.warning and capacity.critical
+
+    uint32_t cruise_power;              // power drawn by the motor(s) at cruise throttle/speed (cW)
+    uint16_t idle_power;                // power drawn by the system when the motor(s) are stopped (cW)
+    uint8_t rth_energy_margin;          // Energy that should be left after RTH (%), used for remaining time/distance before RTH
 
     float throttle_compensation_weight;
 
@@ -90,10 +92,9 @@ typedef struct batteryProfile_s {
 #endif
 
     struct {
-        uint32_t value;                     // mAh or mWh (see capacity.unit)
-        uint32_t warning;                   // mAh or mWh (see capacity.unit)
-        uint32_t critical;                  // mAh or mWh (see capacity.unit)
-        batCapacityUnit_e unit;             // Describes unit of capacity.value, capacity.warning and capacity.critical
+        uint32_t value;                     // mAh or mWh (see batteryMetersConfig()->capacity_unit)
+        uint32_t warning;                   // mAh or mWh (see batteryMetersConfig()->capacity_unit)
+        uint32_t critical;                  // mAh or mWh (see batteryMetersConfig()->capacity_unit)
     } capacity;
 
     uint8_t controlRateProfile;

From 18c7ec44a51aedd3fcb3baae4bee381a5619772b Mon Sep 17 00:00:00 2001
From: Darren Lines <darren@darrenlines.uk>
Date: Sun, 2 Jun 2024 20:02:12 +0100
Subject: [PATCH 166/323] Fix missed CLI command in the diff

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

diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index aba1f58b905..18243fdefce 100644
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -4145,7 +4145,7 @@ static void printConfig(const char *cmdline, bool doDiff)
             setConfigBatteryProfile(currentBatteryProfileIndexSave);
 
             cliPrintHashLine("restore original profile selection");
-            cliPrintLinef("profile %d", currentControlProfileIndexSave + 1);
+            cliPrintLinef("control_profile %d", currentControlProfileIndexSave + 1);
             cliPrintLinef("mixer_profile %d", currentMixerProfileIndexSave + 1);
             cliPrintLinef("battery_profile %d", currentBatteryProfileIndexSave + 1);
 

From 48d05dcb50de2a1228b1fe9b299c9f5529db8d08 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sun, 2 Jun 2024 21:19:34 +0200
Subject: [PATCH 167/323] Initial draft for gimbal support.

Needs plumbing so we have a gimbal task running to send gumbal updates
---
 docs/Settings.md                      |  40 ++++++++++
 src/main/CMakeLists.txt               |   4 +
 src/main/config/parameter_group_ids.h |   3 +-
 src/main/drivers/gimbal_common.c      |  23 ++++++
 src/main/drivers/gimbal_common.h      |  37 ++++++++++
 src/main/fc/fc_msp_box.c              |  10 +++
 src/main/fc/rc_modes.h                |   8 +-
 src/main/fc/settings.yaml             |  28 +++++++
 src/main/io/gimbal_htk.c              | 101 ++++++++++++++++++++++++++
 src/main/io/gimbal_htk.h              |  42 +++++++++++
 src/main/io/serial.h                  |   1 +
 11 files changed, 294 insertions(+), 3 deletions(-)
 create mode 100644 src/main/drivers/gimbal_common.c
 create mode 100644 src/main/drivers/gimbal_common.h
 create mode 100644 src/main/io/gimbal_htk.c
 create mode 100644 src/main/io/gimbal_htk.h

diff --git a/docs/Settings.md b/docs/Settings.md
index a1c339c1d8c..5af90a7e15f 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -1462,6 +1462,46 @@ Yaw Iterm is frozen when bank angle is above this threshold [degrees]. This solv
 
 ---
 
+### gimabl_pitch_channel
+
+Gimbal pitch rc channel index. 0 is no channel.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | 0 | 32 |
+
+---
+
+### gimabl_roll_channel
+
+Gimbal roll rc channel index. 0 is no channel.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | 0 | 32 |
+
+---
+
+### gimabl_sensitivity
+
+Gimbal sensitivity is similar to gain and will affect how quickly the gimbal will react.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | -16 | 15 |
+
+---
+
+### gimabl_yaw_channel
+
+Gimbal yaw rc channel index. 0 is no channel.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | 0 | 32 |
+
+---
+
 ### gps_auto_baud
 
 Automatic configuration of GPS baudrate(The specified baudrate in configured in ports will be used) when used with UBLOX GPS
diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt
index c87abc1782d..31d09c716cf 100755
--- a/src/main/CMakeLists.txt
+++ b/src/main/CMakeLists.txt
@@ -177,6 +177,8 @@ main_sources(COMMON_SRC
     drivers/flash_m25p16.h
     drivers/flash_w25n01g.c
     drivers/flash_w25n01g.h
+    drivers/gimbal_common.h
+    drivers/gimbal_common.c
     drivers/io.c
     drivers/io.h
     drivers/io_pcf8574.c
@@ -354,6 +356,8 @@ main_sources(COMMON_SRC
     io/servo_sbus.h
     io/frsky_osd.c
     io/frsky_osd.h
+    io/gimbal_htk.c
+    io/gimbal_htk.h
     io/osd_dji_hd.c
     io/osd_dji_hd.h
     io/lights.c
diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h
index 822e85f137a..90085fae638 100644
--- a/src/main/config/parameter_group_ids.h
+++ b/src/main/config/parameter_group_ids.h
@@ -126,7 +126,8 @@
 #define PG_FW_AUTOLAND_CONFIG 1036
 #define PG_FW_AUTOLAND_APPROACH_CONFIG 1037
 #define PG_OSD_CUSTOM_ELEMENTS_CONFIG 1038
-#define PG_INAV_END PG_OSD_CUSTOM_ELEMENTS_CONFIG
+#define PG_GIMBAL_CONFIG 1039
+#define PG_INAV_END PG_GIMBAL_CONFIG
 
 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047
diff --git a/src/main/drivers/gimbal_common.c b/src/main/drivers/gimbal_common.c
new file mode 100644
index 00000000000..a5fbf38be76
--- /dev/null
+++ b/src/main/drivers/gimbal_common.c
@@ -0,0 +1,23 @@
+/*
+ * This file is part of INAV.
+ *
+ * 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 INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdint.h>
+#include <config/parameter_group_ids.h>
+
+#include "drivers/gimbal_common.h"
+
+PG_REGISTER(gimbalConfig_t, gimbalConfig, PG_GIMBAL_CONFIG, 0);
\ No newline at end of file
diff --git a/src/main/drivers/gimbal_common.h b/src/main/drivers/gimbal_common.h
new file mode 100644
index 00000000000..9ac2ad3d9bc
--- /dev/null
+++ b/src/main/drivers/gimbal_common.h
@@ -0,0 +1,37 @@
+/*
+ * This file is part of INAV.
+ *
+ * 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 INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+#include "config/feature.h"
+
+typedef struct gimbalConfig_s {
+    uint8_t yawChannel;
+    uint8_t pitchChannel;
+    uint8_t rollChannel;
+    uint8_t sensitivity;
+} gimbalConfig_t;
+
+PG_DECLARE(gimbalConfig_t, gimbalConfig);
+
+typedef enum {
+    GIMBAL_MODE_PITCH_ROLL_LOCK = 0,
+    GIMBAL_MODE_PITCH_LOCK = 1,
+    GIMBAL_MODE_FOLLOW = 2
+} gimbal_htk_mode_e;
\ No newline at end of file
diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c
index 7ffbbefd95e..77aacd2f41b 100644
--- a/src/main/fc/fc_msp_box.c
+++ b/src/main/fc/fc_msp_box.c
@@ -102,6 +102,10 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { .boxId = BOXMIXERPROFILE,     .boxName = "MIXER PROFILE 2",   .permanentId = 62 },
     { .boxId = BOXMIXERTRANSITION,  .boxName = "MIXER TRANSITION",  .permanentId = 63 },
     { .boxId = BOXANGLEHOLD,        .boxName = "ANGLE HOLD",        .permanentId = 64 },
+    { .boxId = BOXGIMBALLOCK,       .boxName = "GIMBAL LOCK",       .permanentId = 65 },
+    { .boxId = BOXGIMBALPLOCK,      .boxName = "GIMBAL LOCK P",     .permanentId = 66 },
+    { .boxId = BOXGIMBALPRLOCK,     .boxName = "GIMBAL LOCK P+R",   .permanentId = 66 },
+    { .boxId = BOXGIMBALCENTER,     .boxName = "GIMBAL CENTER",     .permanentId = 67 },
     { .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL,                .permanentId = 0xFF }
 };
 
@@ -431,6 +435,12 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION)), BOXMIXERTRANSITION);
 #endif
     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXANGLEHOLD)),       BOXANGLEHOLD);
+
+    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALLOCK)),      BOXGIMBALLOCK);
+    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALPLOCK)),     BOXGIMBALPLOCK);
+    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALPRLOCK)),    BOXGIMBALPRLOCK);
+    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)),    BOXGIMBALCENTER);
+
     memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t));
     for (uint32_t i = 0; i < activeBoxIdCount; i++) {
         if (activeBoxes[activeBoxIds[i]]) {
diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h
index 04aea681bc9..cac8528b6ad 100644
--- a/src/main/fc/rc_modes.h
+++ b/src/main/fc/rc_modes.h
@@ -78,9 +78,13 @@ typedef enum {
     BOXCHANGEMISSION = 50,
     BOXBEEPERMUTE    = 51,
     BOXMULTIFUNCTION = 52,
-    BOXMIXERPROFILE      = 53,
-    BOXMIXERTRANSITION   = 54,
+    BOXMIXERPROFILE  = 53,
+    BOXMIXERTRANSITION = 54,
     BOXANGLEHOLD     = 55,
+    BOXGIMBALLOCK    = 56,
+    BOXGIMBALPLOCK   = 57,
+    BOXGIMBALPRLOCK  = 58,
+    BOXGIMBALCENTER  = 59,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index d9aa83c93cf..a8fdbbd408c 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -4146,3 +4146,31 @@ groups:
         field: maxTailwind
         min: 0
         max: 3000
+  - name: PG_GIMBAL_CONFIG
+    type: gimbalConfig_t
+    headers: ["drivers/gimbal_common.h"]
+    members:
+      - name: gimabl_yaw_channel
+        description: "Gimbal yaw rc channel index. 0 is no channel."
+        default_value: 0
+        field: yawChannel
+        min: 0
+        max: 32
+      - name: gimabl_roll_channel
+        description: "Gimbal roll rc channel index. 0 is no channel."
+        default_value: 0
+        field: yawChannel
+        min: 0
+        max: 32
+      - name: gimabl_pitch_channel
+        description: "Gimbal pitch rc channel index. 0 is no channel."
+        default_value: 0
+        field: yawChannel
+        min: 0
+        max: 32
+      - name: gimabl_sensitivity
+        description: "Gimbal sensitivity is similar to gain and will affect how quickly the gimbal will react."
+        default_value: 0
+        field: sensitivity
+        min: -16
+        max: 15
\ No newline at end of file
diff --git a/src/main/io/gimbal_htk.c b/src/main/io/gimbal_htk.c
new file mode 100644
index 00000000000..0633423aec0
--- /dev/null
+++ b/src/main/io/gimbal_htk.c
@@ -0,0 +1,101 @@
+/*
+ * This file is part of INAV.
+ *
+ * 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 INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <common/crc.h>
+
+#include <drivers/gimbal_common.h>
+#include <io/gimbal_htk.h>
+
+#include <rx/rx.h>
+
+
+void gimbal_htk_update(void)
+{
+    const gimbalConfig_t *cfg = gimbalConfig();
+
+    GimbalHtkAttitudePkt_t attittude = {
+        .sync = {HTKATTITUDE_SYNC0, HTKATTITUDE_SYNC1},
+        .mode = GIMBAL_MODE_FOLLOW,
+    };
+
+    int yaw = 1500;
+    int pitch = 1500;
+    int roll = 1500;
+
+    if (rxAreFlightChannelsValid()) {
+        if (cfg->yawChannel > 0) {
+            yaw = rxGetChannelValue(cfg->yawChannel - 1);
+            // const rxChannelRangeConfig_t *channelRanges =
+            // rxChannelRangeConfigs(cfg->pitchChannel - 1);
+            if (yaw < 1000) {
+                yaw = 1000;
+            } else if (yaw > 2000) {
+                yaw = 2000;
+            }
+        }
+
+        if (cfg->pitchChannel > 0) {
+            pitch = rxGetChannelValue(cfg->pitchChannel - 1);
+            // const rxChannelRangeConfig_t *channelRanges =
+            // rxChannelRangeConfigs(cfg->pitchChannel - 1);
+            if (pitch < 1000) {
+                pitch = 1000;
+            } else if (pitch > 2000) {
+                pitch = 2000;
+            }
+        }
+
+        if (cfg->rollChannel > 0) {
+            roll = rxGetChannelValue(cfg->rollChannel - 1);
+            // const rxChannelRangeConfig_t *channelRanges =
+            // rxChannelRangeConfigs(cfg->pitchChannel - 1);
+            if (roll < 1000) {
+                roll = 1000;
+            } else if (roll > 2000) {
+                roll = 2000;
+            }
+        }
+    }
+
+    attittude.sensibility = gimbal_scale8(-16, 15, 0, 31, cfg->sensitivity);
+
+    attittude.yaw = gimbal_scale16(1000, 2000, 0, 4095, yaw);
+    attittude.pitch = gimbal_scale16(1000, 2000, 0, 4095, pitch);
+    attittude.roll = gimbal_scale16(1000, 2000, 0, 4095, roll);
+
+    uint16_t crc16 = 0;
+    uint8_t *b = (uint8_t *)&attittude;
+    for (uint8_t i = 0; i < sizeof(GimbalHtkAttitudePkt_t) - 2; i++) {
+        crc16 = crc16_ccitt(crc16, *(b + i));
+    }
+    attittude.crch = (crc16 >> 8) & 0xFF;
+    attittude.crcl = crc16 & 0xFF;
+
+    // Send new data
+}
+
+uint8_t gimbal_scale8(int8_t inputMin, int8_t inputMax, int8_t outputMin, int8_t outputMax, int8_t value)
+{
+    float m = (1.0f * outputMax - outputMin) / (inputMax - inputMin);
+    return (uint8_t)((outputMin + (m * (value - inputMin))) + 0.5f);
+}
+
+uint16_t gimbal_scale16(int16_t inputMin, int16_t inputMax, int16_t outputMin, int16_t outputMax, int16_t value)
+{
+    float m = (1.0f * outputMax - outputMin) / (inputMax - inputMin);
+    return (uint16_t)((outputMin + (m * (value - inputMin))) + 0.5f);
+}
\ No newline at end of file
diff --git a/src/main/io/gimbal_htk.h b/src/main/io/gimbal_htk.h
new file mode 100644
index 00000000000..1802ae33768
--- /dev/null
+++ b/src/main/io/gimbal_htk.h
@@ -0,0 +1,42 @@
+/*
+ * This file is part of INAV.
+ *
+ * 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 INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+#define GIMBAL_HTK_MODE_DEFAULT GIMBAL_HTK_MODE_FOLLOW
+
+#define HTKATTITUDE_SYNC0  0xA5
+#define HTKATTITUDE_SYNC1  0x5A
+typedef struct
+{
+	uint8_t  sync[2];   //data synchronization 0xA5, 0x5A
+	uint64_t mode:3;     //Gimbal Mode [0~7] [Only 0 1 2 modes are supported for the time being]
+	int64_t  sensibility:5;     //Cloud sensibility [-16~15]
+	uint64_t reserved:4;     //hold on to one's reserve
+	int64_t  roll:12;    //Roll angle [-2048~2047] => [-180~180]
+	int64_t  pitch:12;    //Pich angle [-2048~2047] => [-180~180]
+	int64_t  yaw:12;    //Yaw angle [-2048~2047] => [-180~180]
+	uint64_t crch:8;    //Data validation H
+	uint64_t crcl:8;    //Data validation L
+} __attribute__((packed)) GimbalHtkAttitudePkt_t;
+
+uint8_t gimbal_scale8(int8_t inputMin, int8_t inputMax, int8_t outputMin, int8_t outputMax, int8_t value);
+uint16_t gimbal_scale16(int16_t inputMin, int16_t inputMax, int16_t outputMin, int16_t outputMax, int16_t value);
+
+void gimbal_htk_update(void);
\ No newline at end of file
diff --git a/src/main/io/serial.h b/src/main/io/serial.h
index 7766679106b..dad56a9f4bc 100644
--- a/src/main/io/serial.h
+++ b/src/main/io/serial.h
@@ -57,6 +57,7 @@ typedef enum {
     FUNCTION_TELEMETRY_SMARTPORT_MASTER = (1 << 23), // 8388608
     FUNCTION_UNUSED_2                   = (1 << 24), // 16777216
     FUNCTION_MSP_OSD                    = (1 << 25), // 33554432
+    FUNCTION_HTK_GIMBAL                 = (1 << 26), // 67108864
 } serialPortFunction_e;
 
 #define FUNCTION_VTX_MSP FUNCTION_MSP_OSD

From 1834a67749117e9b7b6fbef4f55cc856eaea858f Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sun, 2 Jun 2024 23:35:34 +0200
Subject: [PATCH 168/323] Make it optional

Keep it out of F722 for testing builds, for now.
---
 src/main/drivers/gimbal_common.c |  8 ++++-
 src/main/drivers/gimbal_common.h | 10 +++++-
 src/main/fc/fc_msp_box.c         |  6 ++--
 src/main/fc/rc_modes.h           |  7 ++--
 src/main/fc/settings.yaml        |  1 +
 src/main/io/gimbal_htk.c         | 61 +++++++++++++++++++++++++++++---
 src/main/io/gimbal_htk.h         | 12 +++++--
 src/main/target/common.h         |  1 +
 8 files changed, 88 insertions(+), 18 deletions(-)

diff --git a/src/main/drivers/gimbal_common.c b/src/main/drivers/gimbal_common.c
index a5fbf38be76..ac883fd2e97 100644
--- a/src/main/drivers/gimbal_common.c
+++ b/src/main/drivers/gimbal_common.c
@@ -15,9 +15,15 @@
  * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include "platform.h"
+
+#ifdef USE_SERIAL_GIMBAL
+
 #include <stdint.h>
 #include <config/parameter_group_ids.h>
 
 #include "drivers/gimbal_common.h"
 
-PG_REGISTER(gimbalConfig_t, gimbalConfig, PG_GIMBAL_CONFIG, 0);
\ No newline at end of file
+PG_REGISTER(gimbalConfig_t, gimbalConfig, PG_GIMBAL_CONFIG, 0);
+
+#endif
\ No newline at end of file
diff --git a/src/main/drivers/gimbal_common.h b/src/main/drivers/gimbal_common.h
index 9ac2ad3d9bc..8b0d7f6a86f 100644
--- a/src/main/drivers/gimbal_common.h
+++ b/src/main/drivers/gimbal_common.h
@@ -17,6 +17,10 @@
 
 #pragma once
 
+#include "platform.h"
+
+#ifdef USE_SERIAL_GIMBAL
+
 #include <stdint.h>
 
 #include "config/feature.h"
@@ -34,4 +38,8 @@ typedef enum {
     GIMBAL_MODE_PITCH_ROLL_LOCK = 0,
     GIMBAL_MODE_PITCH_LOCK = 1,
     GIMBAL_MODE_FOLLOW = 2
-} gimbal_htk_mode_e;
\ No newline at end of file
+} gimbal_htk_mode_e;
+
+#define GIMBAL_MODE_DEFAULT = GIMBAL_MODE_FOLLOW;
+
+#endif
\ No newline at end of file
diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c
index 77aacd2f41b..ca384084866 100644
--- a/src/main/fc/fc_msp_box.c
+++ b/src/main/fc/fc_msp_box.c
@@ -102,9 +102,8 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { .boxId = BOXMIXERPROFILE,     .boxName = "MIXER PROFILE 2",   .permanentId = 62 },
     { .boxId = BOXMIXERTRANSITION,  .boxName = "MIXER TRANSITION",  .permanentId = 63 },
     { .boxId = BOXANGLEHOLD,        .boxName = "ANGLE HOLD",        .permanentId = 64 },
-    { .boxId = BOXGIMBALLOCK,       .boxName = "GIMBAL LOCK",       .permanentId = 65 },
-    { .boxId = BOXGIMBALPLOCK,      .boxName = "GIMBAL LOCK P",     .permanentId = 66 },
-    { .boxId = BOXGIMBALPRLOCK,     .boxName = "GIMBAL LOCK P+R",   .permanentId = 66 },
+    { .boxId = BOXGIMBALPLOCK,      .boxName = "GIMBAL LEVEL P",    .permanentId = 66 },
+    { .boxId = BOXGIMBALPRLOCK,     .boxName = "GIMBAL LEVEL P+R",  .permanentId = 66 },
     { .boxId = BOXGIMBALCENTER,     .boxName = "GIMBAL CENTER",     .permanentId = 67 },
     { .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL,                .permanentId = 0xFF }
 };
@@ -436,7 +435,6 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
 #endif
     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXANGLEHOLD)),       BOXANGLEHOLD);
 
-    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALLOCK)),      BOXGIMBALLOCK);
     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALPLOCK)),     BOXGIMBALPLOCK);
     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALPRLOCK)),    BOXGIMBALPRLOCK);
     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)),    BOXGIMBALCENTER);
diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h
index cac8528b6ad..fb517efa009 100644
--- a/src/main/fc/rc_modes.h
+++ b/src/main/fc/rc_modes.h
@@ -81,10 +81,9 @@ typedef enum {
     BOXMIXERPROFILE  = 53,
     BOXMIXERTRANSITION = 54,
     BOXANGLEHOLD     = 55,
-    BOXGIMBALLOCK    = 56,
-    BOXGIMBALPLOCK   = 57,
-    BOXGIMBALPRLOCK  = 58,
-    BOXGIMBALCENTER  = 59,
+    BOXGIMBALPLOCK   = 56,
+    BOXGIMBALPRLOCK  = 57,
+    BOXGIMBALCENTER  = 58,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index a8fdbbd408c..cc18a663626 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -4149,6 +4149,7 @@ groups:
   - name: PG_GIMBAL_CONFIG
     type: gimbalConfig_t
     headers: ["drivers/gimbal_common.h"]
+    condition: USE_SERIAL_GIMBAL
     members:
       - name: gimabl_yaw_channel
         description: "Gimbal yaw rc channel index. 0 is no channel."
diff --git a/src/main/io/gimbal_htk.c b/src/main/io/gimbal_htk.c
index 0633423aec0..727559de7c4 100644
--- a/src/main/io/gimbal_htk.c
+++ b/src/main/io/gimbal_htk.c
@@ -15,28 +15,74 @@
  * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include "platform.h"
+
+#ifdef USE_SERIAL_GIMBAL
+
 #include <common/crc.h>
 
 #include <drivers/gimbal_common.h>
+#include <drivers/serial.h>
+
 #include <io/gimbal_htk.h>
+#include <io/serial.h>
 
 #include <rx/rx.h>
+#include <fc/rc_modes.h>
+
+
+#define HTK_TX_BUFFER_SIZE 512
+static volatile uint8_t txBuffer[HTK_TX_BUFFER_SIZE];
+
+static serialPort_t *htkPort = NULL;
+
+bool gimbal_htk_detect(void)
+{
+    serialPortConfig_t *portConfig = findNextSerialPortConfig(FUNCTION_HTK_GIMBAL);
+
+    if (portConfig) {
+        htkPort = openSerialPort(portConfig->identifier, FUNCTION_HTK_GIMBAL, NULL, NULL,
+                115200, MODE_RXTX, SERIAL_NOT_INVERTED);
+
+        if (htkPort) {
+            htkPort->txBuffer = txBuffer;
+            htkPort->txBufferSize = HTK_TX_BUFFER_SIZE;
+            htkPort->txBufferTail = 0;
+            htkPort->txBufferHead = 0;
+
+            return true;
+        }
+    }
 
+    return false;
+}
 
 void gimbal_htk_update(void)
 {
-    const gimbalConfig_t *cfg = gimbalConfig();
+    if (!htkPort) {
+        return;
+    }
 
-    GimbalHtkAttitudePkt_t attittude = {
+    gimbalHtkAttitudePkt_t attittude = {
         .sync = {HTKATTITUDE_SYNC0, HTKATTITUDE_SYNC1},
         .mode = GIMBAL_MODE_FOLLOW,
     };
 
+    const gimbalConfig_t *cfg = gimbalConfig();
+
     int yaw = 1500;
     int pitch = 1500;
     int roll = 1500;
 
-    if (rxAreFlightChannelsValid()) {
+    if (IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)) {
+        attittude.mode = GIMBAL_MODE_FOLLOW;
+    } else if (IS_RC_MODE_ACTIVE(BOXGIMBALPLOCK)) {
+        attittude.mode = GIMBAL_MODE_PITCH_LOCK;
+    } else if (IS_RC_MODE_ACTIVE(BOXGIMBALPRLOCK)) {
+        attittude.mode = GIMBAL_MODE_PITCH_ROLL_LOCK;
+    }
+
+    if (rxAreFlightChannelsValid() && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)) {
         if (cfg->yawChannel > 0) {
             yaw = rxGetChannelValue(cfg->yawChannel - 1);
             // const rxChannelRangeConfig_t *channelRanges =
@@ -79,12 +125,15 @@ void gimbal_htk_update(void)
 
     uint16_t crc16 = 0;
     uint8_t *b = (uint8_t *)&attittude;
-    for (uint8_t i = 0; i < sizeof(GimbalHtkAttitudePkt_t) - 2; i++) {
+    for (uint8_t i = 0; i < sizeof(gimbalHtkAttitudePkt_t) - 2; i++) {
         crc16 = crc16_ccitt(crc16, *(b + i));
     }
     attittude.crch = (crc16 >> 8) & 0xFF;
     attittude.crcl = crc16 & 0xFF;
 
+    serialBeginWrite(htkPort);
+    //serialWriteBuf(htkPort, (uint8_t *)&attittude, sizeof(gimbalHtkAttitudePkt_t));
+    serialEndWrite(htkPort);
     // Send new data
 }
 
@@ -98,4 +147,6 @@ uint16_t gimbal_scale16(int16_t inputMin, int16_t inputMax, int16_t outputMin, i
 {
     float m = (1.0f * outputMax - outputMin) / (inputMax - inputMin);
     return (uint16_t)((outputMin + (m * (value - inputMin))) + 0.5f);
-}
\ No newline at end of file
+}
+
+#endif
\ No newline at end of file
diff --git a/src/main/io/gimbal_htk.h b/src/main/io/gimbal_htk.h
index 1802ae33768..b45b8f02651 100644
--- a/src/main/io/gimbal_htk.h
+++ b/src/main/io/gimbal_htk.h
@@ -17,13 +17,17 @@
 
 #pragma once
 
+#include "platform.h"
+
+#ifdef USE_SERIAL_GIMBAL
+
 #include <stdint.h>
 
 #define GIMBAL_HTK_MODE_DEFAULT GIMBAL_HTK_MODE_FOLLOW
 
 #define HTKATTITUDE_SYNC0  0xA5
 #define HTKATTITUDE_SYNC1  0x5A
-typedef struct
+typedef struct gimbalHtkAttitudePkt_s
 {
 	uint8_t  sync[2];   //data synchronization 0xA5, 0x5A
 	uint64_t mode:3;     //Gimbal Mode [0~7] [Only 0 1 2 modes are supported for the time being]
@@ -34,9 +38,11 @@ typedef struct
 	int64_t  yaw:12;    //Yaw angle [-2048~2047] => [-180~180]
 	uint64_t crch:8;    //Data validation H
 	uint64_t crcl:8;    //Data validation L
-} __attribute__((packed)) GimbalHtkAttitudePkt_t;
+} __attribute__((packed)) gimbalHtkAttitudePkt_t;
 
 uint8_t gimbal_scale8(int8_t inputMin, int8_t inputMax, int8_t outputMin, int8_t outputMax, int8_t value);
 uint16_t gimbal_scale16(int16_t inputMin, int16_t inputMax, int16_t outputMin, int16_t outputMax, int16_t value);
 
-void gimbal_htk_update(void);
\ No newline at end of file
+void gimbal_htk_update(void);
+
+#endif
\ No newline at end of file
diff --git a/src/main/target/common.h b/src/main/target/common.h
index 0b878067c45..676b1760409 100644
--- a/src/main/target/common.h
+++ b/src/main/target/common.h
@@ -193,6 +193,7 @@
 
 //Designed to free space of F722 and F411 MCUs
 #if (MCU_FLASH_SIZE > 512)
+#define USE_SERIAL_GIMBAL
 #define USE_VTX_FFPV
 #define USE_SERIALRX_SUMD
 #define USE_TELEMETRY_HOTT

From 4faed24fd752e68dd9daa7994377940f70d8cbe5 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sun, 2 Jun 2024 23:51:02 +0200
Subject: [PATCH 169/323] Business logic ok, still needs plumbing

---
 src/main/io/gimbal_htk.c | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/src/main/io/gimbal_htk.c b/src/main/io/gimbal_htk.c
index 727559de7c4..c2356910833 100644
--- a/src/main/io/gimbal_htk.c
+++ b/src/main/io/gimbal_htk.c
@@ -20,6 +20,7 @@
 #ifdef USE_SERIAL_GIMBAL
 
 #include <common/crc.h>
+#include <common/utils.h>
 
 #include <drivers/gimbal_common.h>
 #include <drivers/serial.h>
@@ -31,6 +32,8 @@
 #include <fc/rc_modes.h>
 
 
+STATIC_ASSERT(sizeof(gimbalHtkAttitudePkt_t) == 10, gimbalHtkAttitudePkt_t_size_not_10);
+
 #define HTK_TX_BUFFER_SIZE 512
 static volatile uint8_t txBuffer[HTK_TX_BUFFER_SIZE];
 
@@ -68,6 +71,7 @@ void gimbal_htk_update(void)
         .mode = GIMBAL_MODE_FOLLOW,
     };
 
+
     const gimbalConfig_t *cfg = gimbalConfig();
 
     int yaw = 1500;
@@ -132,7 +136,7 @@ void gimbal_htk_update(void)
     attittude.crcl = crc16 & 0xFF;
 
     serialBeginWrite(htkPort);
-    //serialWriteBuf(htkPort, (uint8_t *)&attittude, sizeof(gimbalHtkAttitudePkt_t));
+    serialWriteBuf(htkPort, (uint8_t *)&attittude, sizeof(gimbalHtkAttitudePkt_t));
     serialEndWrite(htkPort);
     // Send new data
 }

From 0e8486e45dab64fb030f0bc252d6f4e26e8c18f0 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Mon, 3 Jun 2024 22:58:42 +0200
Subject: [PATCH 170/323] rename some fields, add some plumbing for
 gimbal_common

---
 docs/Settings.md                 |  8 +++---
 src/main/drivers/gimbal_common.c | 45 ++++++++++++++++++++++++++++++++
 src/main/drivers/gimbal_common.h | 38 +++++++++++++++++++++++++--
 src/main/fc/settings.yaml        | 14 +++++-----
 src/main/io/gimbal_htk.c         |  8 +++---
 5 files changed, 96 insertions(+), 17 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index 5af90a7e15f..fb93cfb69ce 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -1462,9 +1462,9 @@ Yaw Iterm is frozen when bank angle is above this threshold [degrees]. This solv
 
 ---
 
-### gimabl_pitch_channel
+### gimabl_pan_channel
 
-Gimbal pitch rc channel index. 0 is no channel.
+Gimbal pan rc channel index. 0 is no channel.
 
 | Default | Min | Max |
 | --- | --- | --- |
@@ -1492,9 +1492,9 @@ Gimbal sensitivity is similar to gain and will affect how quickly the gimbal wil
 
 ---
 
-### gimabl_yaw_channel
+### gimabl_tilt_channel
 
-Gimbal yaw rc channel index. 0 is no channel.
+Gimbal tilt rc channel index. 0 is no channel.
 
 | Default | Min | Max |
 | --- | --- | --- |
diff --git a/src/main/drivers/gimbal_common.c b/src/main/drivers/gimbal_common.c
index ac883fd2e97..a7a2e275b07 100644
--- a/src/main/drivers/gimbal_common.c
+++ b/src/main/drivers/gimbal_common.c
@@ -20,10 +20,55 @@
 #ifdef USE_SERIAL_GIMBAL
 
 #include <stdint.h>
+#include <stdlib.h>
 #include <config/parameter_group_ids.h>
 
+#include "common/time.h"
+
 #include "drivers/gimbal_common.h"
 
+
 PG_REGISTER(gimbalConfig_t, gimbalConfig, PG_GIMBAL_CONFIG, 0);
 
+
+static gimbalDevice_t *commonGimbalDevice = NULL;
+
+void gimbalCommonInit(void)
+{
+}
+
+void gimbalCommonSetDevice(gimbalDevice_t *gimbalDevice)
+{
+    commonGimbalDevice = gimbalDevice;
+}
+
+gimbalDevice_t *gimbalCommonDevice(void)
+{
+    return commonGimbalDevice;
+}
+
+void gimbalCommonProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTimeUs)
+{
+    if (gimbalDevice && gimbalDevice->vTable->process) {
+        gimbalDevice->vTable->process(gimbalDevice, currentTimeUs);
+    }
+}
+
+gimbalDevType_e gimbalCommonGetDeviceType(gimbalDevice_t *gimbalDevice)
+{
+    if (!gimbalDevice || !gimbalDevice->vTable->getDeviceType) {
+        return GIMBAL_DEV_UNKNOWN;
+    }
+
+    return gimbalDevice->vTable->getDeviceType(gimbalDevice);
+}
+
+bool gimbalCommonDeviceIsReady(gimbalDevice_t *gimbalDevice)
+{
+    if (gimbalDevice && gimbalDevice->vTable->isReady) {
+        return gimbalDevice->vTable->isReady(gimbalDevice);
+    }
+    return false;
+}
+
 #endif
\ No newline at end of file
diff --git a/src/main/drivers/gimbal_common.h b/src/main/drivers/gimbal_common.h
index 8b0d7f6a86f..a2441184ed7 100644
--- a/src/main/drivers/gimbal_common.h
+++ b/src/main/drivers/gimbal_common.h
@@ -24,10 +24,35 @@
 #include <stdint.h>
 
 #include "config/feature.h"
+#include "common/time.h"
+
+
+typedef enum {
+    GIMBAL_DEV_UNSUPPORTED = 0,
+    GIMBAL_DEV_SERIAL,
+    GIMBAL_DEV_UNKNOWN=0xFF
+} gimbalDevType_e;
+
+struct gimbalVTable_s;
+
+typedef struct gimbalDevice_s {
+    const struct gimbalVTable_s *vTable;
+} gimbalDevice_t;
+
+// {set,get}BandAndChannel: band and channel are 1 origin
+// {set,get}PowerByIndex: 0 = Power OFF, 1 = device dependent
+// {set,get}PitMode: 0 = OFF, 1 = ON
+
+typedef struct gimbalVTable_s {
+    void (*process)(gimbalDevice_t *gimbalDevice, timeUs_t currentTimeUs);
+    gimbalDevType_e (*getDeviceType)(const gimbalDevice_t *gimablDevice);
+    bool (*isReady)(const gimbalDevice_t *gimbalDevice);
+} gimbalVTable_t;
+
 
 typedef struct gimbalConfig_s {
-    uint8_t yawChannel;
-    uint8_t pitchChannel;
+    uint8_t panChannel;
+    uint8_t tiltChannel;
     uint8_t rollChannel;
     uint8_t sensitivity;
 } gimbalConfig_t;
@@ -42,4 +67,13 @@ typedef enum {
 
 #define GIMBAL_MODE_DEFAULT = GIMBAL_MODE_FOLLOW;
 
+void gimbalCommonInit(void);
+void gimbalCommonSetDevice(gimbalDevice_t *gimbalDevice);
+gimbalDevice_t *gimbalCommonDevice(void);
+
+// VTable functions
+void gimbalCommonProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTimeUs);
+gimbalDevType_e gimbalCommonGetDeviceType(gimbalDevice_t *gimbalDevice);
+bool gimbalCommonIsReady(gimbalDevice_t *gimbalDevice);
+
 #endif
\ No newline at end of file
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index cc18a663626..2127c3505ae 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -4151,22 +4151,22 @@ groups:
     headers: ["drivers/gimbal_common.h"]
     condition: USE_SERIAL_GIMBAL
     members:
-      - name: gimabl_yaw_channel
-        description: "Gimbal yaw rc channel index. 0 is no channel."
+      - name: gimabl_pan_channel
+        description: "Gimbal pan rc channel index. 0 is no channel."
         default_value: 0
-        field: yawChannel
+        field: panChannel
         min: 0
         max: 32
       - name: gimabl_roll_channel
         description: "Gimbal roll rc channel index. 0 is no channel."
         default_value: 0
-        field: yawChannel
+        field: rollChannel
         min: 0
         max: 32
-      - name: gimabl_pitch_channel
-        description: "Gimbal pitch rc channel index. 0 is no channel."
+      - name: gimabl_tilt_channel
+        description: "Gimbal tilt rc channel index. 0 is no channel."
         default_value: 0
-        field: yawChannel
+        field: tiltChannel
         min: 0
         max: 32
       - name: gimabl_sensitivity
diff --git a/src/main/io/gimbal_htk.c b/src/main/io/gimbal_htk.c
index c2356910833..e3c97c8a425 100644
--- a/src/main/io/gimbal_htk.c
+++ b/src/main/io/gimbal_htk.c
@@ -87,8 +87,8 @@ void gimbal_htk_update(void)
     }
 
     if (rxAreFlightChannelsValid() && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)) {
-        if (cfg->yawChannel > 0) {
-            yaw = rxGetChannelValue(cfg->yawChannel - 1);
+        if (cfg->panChannel > 0) {
+            yaw = rxGetChannelValue(cfg->panChannel - 1);
             // const rxChannelRangeConfig_t *channelRanges =
             // rxChannelRangeConfigs(cfg->pitchChannel - 1);
             if (yaw < 1000) {
@@ -98,8 +98,8 @@ void gimbal_htk_update(void)
             }
         }
 
-        if (cfg->pitchChannel > 0) {
-            pitch = rxGetChannelValue(cfg->pitchChannel - 1);
+        if (cfg->tiltChannel > 0) {
+            pitch = rxGetChannelValue(cfg->tiltChannel - 1);
             // const rxChannelRangeConfig_t *channelRanges =
             // rxChannelRangeConfigs(cfg->pitchChannel - 1);
             if (pitch < 1000) {

From 307e35ad870b6bde9f4e2c4343884c7cd30b7fdb Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Tue, 4 Jun 2024 01:08:19 +0200
Subject: [PATCH 171/323] Current status.

Start plumbing task. Still need initialization
---
 src/main/drivers/gimbal_common.c | 17 +++++++++++++++++
 src/main/drivers/gimbal_common.h |  3 +++
 src/main/fc/fc_init.c            |  6 ++++++
 src/main/fc/fc_tasks.c           |  9 +++++++++
 src/main/scheduler/scheduler.h   |  3 +++
 5 files changed, 38 insertions(+)

diff --git a/src/main/drivers/gimbal_common.c b/src/main/drivers/gimbal_common.c
index a7a2e275b07..2a2ba396333 100644
--- a/src/main/drivers/gimbal_common.c
+++ b/src/main/drivers/gimbal_common.c
@@ -25,6 +25,8 @@
 
 #include "common/time.h"
 
+#include "fc/cli.h"
+
 #include "drivers/gimbal_common.h"
 
 
@@ -71,4 +73,19 @@ bool gimbalCommonDeviceIsReady(gimbalDevice_t *gimbalDevice)
     return false;
 }
 
+void taskUpdateGimbal(timeUs_t currentTimeUs)
+{
+    if (cliMode) {
+        return;
+    }
+
+    gimbalDevice_t *gimbalDevice = gimbalCommonDevice();
+
+    if(gimbalDevice) {
+        gimbalCommonProcess(gimbalDevice, currentTimeUs);
+    }
+}
+
+
+
 #endif
\ No newline at end of file
diff --git a/src/main/drivers/gimbal_common.h b/src/main/drivers/gimbal_common.h
index a2441184ed7..15d4099dc4a 100644
--- a/src/main/drivers/gimbal_common.h
+++ b/src/main/drivers/gimbal_common.h
@@ -76,4 +76,7 @@ void gimbalCommonProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTimeUs);
 gimbalDevType_e gimbalCommonGetDeviceType(gimbalDevice_t *gimbalDevice);
 bool gimbalCommonIsReady(gimbalDevice_t *gimbalDevice);
 
+
+void taskUpdateGimbal(timeUs_t currentTimeUs);
+
 #endif
\ No newline at end of file
diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index f2b368b8fcb..5feb7c739a4 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -53,6 +53,7 @@
 #include "drivers/exti.h"
 #include "drivers/io.h"
 #include "drivers/flash.h"
+#include "drivers/gimbal_common.h"
 #include "drivers/light_led.h"
 #include "drivers/nvic.h"
 #include "drivers/osd.h"
@@ -107,6 +108,7 @@
 #include "io/displayport_msp_osd.h"
 #include "io/displayport_srxl.h"
 #include "io/flashfs.h"
+#include "io/gimbal_htk.h"
 #include "io/gps.h"
 #include "io/ledstrip.h"
 #include "io/osd.h"
@@ -685,6 +687,10 @@ void init(void)
 
 #ifdef USE_DSHOT
     initDShotCommands();
+#endif
+
+#ifdef USE_SERIAL_GIMBAL
+
 #endif
 
     // Latch active features AGAIN since some may be modified by init().
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index 406622af38f..355d8824487 100755
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -34,6 +34,7 @@
 #include "drivers/serial.h"
 #include "drivers/stack_check.h"
 #include "drivers/pwm_mapping.h"
+#include "drivers/gimbal_common.h"
 
 #include "fc/cli.h"
 #include "fc/config.h"
@@ -680,4 +681,12 @@ cfTask_t cfTasks[TASK_COUNT] = {
         .desiredPeriod = TASK_PERIOD_HZ(TASK_AUX_RATE_HZ),          // 100Hz @10ms
         .staticPriority = TASK_PRIORITY_HIGH,
     },
+#ifdef USE_SERIAL_GIMBAL
+    [TASK_GIMBAL] = {
+        .taskName = "GIMBAL",
+        .taskFunc = taskUpdateGimbal,
+        .desiredPeriod = TASK_PERIOD_HZ(50),
+        .staticPriority = TASK_PRIORITY_LOW,
+    }
+#endif
 };
diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h
index 0d91876cb72..88b1ccf0384 100755
--- a/src/main/scheduler/scheduler.h
+++ b/src/main/scheduler/scheduler.h
@@ -121,6 +121,9 @@ typedef enum {
 #endif
 #ifdef USE_IRLOCK
     TASK_IRLOCK,
+#endif
+#ifdef USE_SERIAL_GIMBAL
+    TASK_GIMBAL,
 #endif
     /* Count of real tasks */
     TASK_COUNT,

From b8707b76cb0abed7372b5564a3b070e966cf6758 Mon Sep 17 00:00:00 2001
From: flywoo <viper_rc@outlook.com>
Date: Tue, 4 Jun 2024 11:09:23 +0800
Subject: [PATCH 172/323] Update F722PRO default features

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

diff --git a/src/main/target/FLYWOOF722PRO/target.h b/src/main/target/FLYWOOF722PRO/target.h
index 34d6de266c4..ae12a324701 100644
--- a/src/main/target/FLYWOOF722PRO/target.h
+++ b/src/main/target/FLYWOOF722PRO/target.h
@@ -140,6 +140,7 @@
  
 /*** Default settings ***/
 #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+#define DEFAULT_FEATURES  (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX)
 #define CURRENT_METER_SCALE_DEFAULT     170
 #define SERIALRX_UART                   SERIAL_PORT_USART1
 #define DEFAULT_RX_TYPE                 RX_TYPE_SERIAL

From b68619c6c8b3b096cfafbcaac53a80332b172cef Mon Sep 17 00:00:00 2001
From: flywoo <viper_rc@outlook.com>
Date: Tue, 4 Jun 2024 14:49:28 +0800
Subject: [PATCH 173/323] Update F722PRO target.h

---
 src/main/target/FLYWOOF722PRO/config.c | 1 -
 src/main/target/FLYWOOF722PRO/target.h | 2 +-
 2 files changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/main/target/FLYWOOF722PRO/config.c b/src/main/target/FLYWOOF722PRO/config.c
index ae386d4bc0a..57ecfa64ff6 100644
--- a/src/main/target/FLYWOOF722PRO/config.c
+++ b/src/main/target/FLYWOOF722PRO/config.c
@@ -24,7 +24,6 @@
 void targetConfiguration(void)
 {
     pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1;
-	
     timerOverridesMutable(timer2id(TIM3))->outputMode = OUTPUT_MODE_MOTORS;
     timerOverridesMutable(timer2id(TIM2))->outputMode = OUTPUT_MODE_MOTORS;
 }
diff --git a/src/main/target/FLYWOOF722PRO/target.h b/src/main/target/FLYWOOF722PRO/target.h
index ae12a324701..3f11b44e317 100644
--- a/src/main/target/FLYWOOF722PRO/target.h
+++ b/src/main/target/FLYWOOF722PRO/target.h
@@ -148,7 +148,7 @@
 
 /*** Timer/PWM output ***/
 #define USE_SERIAL_4WAY_BLHELI_INTERFACE
-#define MAX_PWM_OUTPUT_PORTS            6
+#define MAX_PWM_OUTPUT_PORTS            8
 #define USE_DSHOT
 #define USE_ESC_SENSOR
 

From 42a94cb7acb9e306f89992956e45fa80fab83346 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Tue, 4 Jun 2024 20:40:37 +0200
Subject: [PATCH 174/323] Few extra changes to cut down noise

---
 .gitignore | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/.gitignore b/.gitignore
index ab82cc0fcd4..39ef205b954 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,10 +1,12 @@
 *.o
 .DS_Store
 *~
+*.swp
 *.uvopt
 *.dep
 *.bak
 *.uvgui.*
+*.ubx
 .project
 .settings
 .cproject
@@ -16,6 +18,7 @@ startup_stm32f10x_md_gcc.s
 cov-int*
 /build/
 /build_SITL/
+/sitl/
 /obj/
 /patches/
 /tools/

From 4da66220a043fcbf2cd1db8ff101fd0db1d7506e Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Thu, 6 Jun 2024 00:31:31 +0200
Subject: [PATCH 175/323] Plumbing in place

Time to wire things up and test.
---
 src/main/build/debug.h           | 8 ++++++++
 src/main/drivers/gimbal_common.c | 7 +++----
 src/main/drivers/gimbal_common.h | 3 ++-
 src/main/fc/fc_init.c            | 5 ++++-
 src/main/fc/fc_tasks.c           | 6 +++++-
 src/main/scheduler/scheduler.h   | 4 ++--
 src/main/target/SITL/target.h    | 1 +
 7 files changed, 25 insertions(+), 9 deletions(-)

diff --git a/src/main/build/debug.h b/src/main/build/debug.h
index 3a09da50daf..8b739cf67c2 100644
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@@ -21,6 +21,8 @@
 #include <stdint.h>
 #include <stdbool.h>
 
+#include "platform.h"
+
 #define DEBUG32_VALUE_COUNT 8
 extern int32_t debug[DEBUG32_VALUE_COUNT];
 extern uint8_t debugMode;
@@ -74,3 +76,9 @@ typedef enum {
     DEBUG_POS_EST,
     DEBUG_COUNT
 } debugType_e;
+
+#ifdef SITL_BUILD
+#define SD(X) (X)
+#else
+#define SD(X)
+#endif
diff --git a/src/main/drivers/gimbal_common.c b/src/main/drivers/gimbal_common.c
index 2a2ba396333..340db5fbdce 100644
--- a/src/main/drivers/gimbal_common.c
+++ b/src/main/drivers/gimbal_common.c
@@ -19,6 +19,7 @@
 
 #ifdef USE_SERIAL_GIMBAL
 
+#include <stdio.h>
 #include <stdint.h>
 #include <stdlib.h>
 #include <config/parameter_group_ids.h>
@@ -51,7 +52,7 @@ gimbalDevice_t *gimbalCommonDevice(void)
 
 void gimbalCommonProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTimeUs)
 {
-    if (gimbalDevice && gimbalDevice->vTable->process) {
+    if (gimbalDevice && gimbalDevice->vTable->process && gimbalCommonIsReady(gimbalDevice)) {
         gimbalDevice->vTable->process(gimbalDevice, currentTimeUs);
     }
 }
@@ -65,7 +66,7 @@ gimbalDevType_e gimbalCommonGetDeviceType(gimbalDevice_t *gimbalDevice)
     return gimbalDevice->vTable->getDeviceType(gimbalDevice);
 }
 
-bool gimbalCommonDeviceIsReady(gimbalDevice_t *gimbalDevice)
+bool gimbalCommonIsReady(gimbalDevice_t *gimbalDevice)
 {
     if (gimbalDevice && gimbalDevice->vTable->isReady) {
         return gimbalDevice->vTable->isReady(gimbalDevice);
@@ -86,6 +87,4 @@ void taskUpdateGimbal(timeUs_t currentTimeUs)
     }
 }
 
-
-
 #endif
\ No newline at end of file
diff --git a/src/main/drivers/gimbal_common.h b/src/main/drivers/gimbal_common.h
index 15d4099dc4a..8814e1010dd 100644
--- a/src/main/drivers/gimbal_common.h
+++ b/src/main/drivers/gimbal_common.h
@@ -33,6 +33,7 @@ typedef enum {
     GIMBAL_DEV_UNKNOWN=0xFF
 } gimbalDevType_e;
 
+
 struct gimbalVTable_s;
 
 typedef struct gimbalDevice_s {
@@ -65,7 +66,7 @@ typedef enum {
     GIMBAL_MODE_FOLLOW = 2
 } gimbal_htk_mode_e;
 
-#define GIMBAL_MODE_DEFAULT = GIMBAL_MODE_FOLLOW;
+#define GIMBAL_MODE_DEFAULT GIMBAL_MODE_FOLLOW
 
 void gimbalCommonInit(void);
 void gimbalCommonSetDevice(gimbalDevice_t *gimbalDevice);
diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index 5feb7c739a4..124fe1dc67c 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -689,8 +689,11 @@ void init(void)
     initDShotCommands();
 #endif
 
+    fprintf(stderr, "HERE\n");
 #ifdef USE_SERIAL_GIMBAL
-
+    fprintf(stderr, "HERE2\n");
+    gimbalCommonInit();
+    gimbalSerialInit();
 #endif
 
     // Latch active features AGAIN since some may be modified by init().
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index 355d8824487..fb672a50766 100755
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -427,6 +427,10 @@ void fcTasksInit(void)
     setTaskEnabled(TASK_SMARTPORT_MASTER, true);
 #endif
 
+#ifdef USE_SERIAL_GIMBAL
+    setTaskEnabled(TASK_GIMBAL, true);
+#endif
+
 #if defined(SITL_BUILD)
     serialProxyStart();
 #endif
@@ -686,7 +690,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
         .taskName = "GIMBAL",
         .taskFunc = taskUpdateGimbal,
         .desiredPeriod = TASK_PERIOD_HZ(50),
-        .staticPriority = TASK_PRIORITY_LOW,
+        .staticPriority = TASK_PRIORITY_MEDIUM,
     }
 #endif
 };
diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h
index 88b1ccf0384..36fa524141f 100755
--- a/src/main/scheduler/scheduler.h
+++ b/src/main/scheduler/scheduler.h
@@ -122,9 +122,9 @@ typedef enum {
 #ifdef USE_IRLOCK
     TASK_IRLOCK,
 #endif
-#ifdef USE_SERIAL_GIMBAL
+//#ifdef USE_SERIAL_GIMBAL
     TASK_GIMBAL,
-#endif
+//#endif
     /* Count of real tasks */
     TASK_COUNT,
 
diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h
index 3fbe45a5cab..05405a51ede 100644
--- a/src/main/target/SITL/target.h
+++ b/src/main/target/SITL/target.h
@@ -74,6 +74,7 @@
 
 #define USE_MSP_OSD
 #define USE_OSD
+#define USE_SERIAL_GIMBAL
 
 #undef USE_DASHBOARD
 

From ae07fd72165f62b089f71ba858dc1d6bc161fc4d Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Thu, 6 Jun 2024 00:36:42 +0200
Subject: [PATCH 176/323] Some files that were missed

---
 src/main/io/gimbal_htk.c       | 62 ++++++++++++++++++++++++++++++----
 src/main/io/gimbal_htk.h       |  7 ++--
 src/main/scheduler/scheduler.h |  4 +--
 3 files changed, 62 insertions(+), 11 deletions(-)

diff --git a/src/main/io/gimbal_htk.c b/src/main/io/gimbal_htk.c
index e3c97c8a425..2c80113365e 100644
--- a/src/main/io/gimbal_htk.c
+++ b/src/main/io/gimbal_htk.c
@@ -15,12 +15,17 @@
  * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <stdio.h>
+#include <stdint.h>
+#include <unistd.h>
+
 #include "platform.h"
 
 #ifdef USE_SERIAL_GIMBAL
 
 #include <common/crc.h>
 #include <common/utils.h>
+#include <build/debug.h>
 
 #include <drivers/gimbal_common.h>
 #include <drivers/serial.h>
@@ -31,7 +36,6 @@
 #include <rx/rx.h>
 #include <fc/rc_modes.h>
 
-
 STATIC_ASSERT(sizeof(gimbalHtkAttitudePkt_t) == 10, gimbalHtkAttitudePkt_t_size_not_10);
 
 #define HTK_TX_BUFFER_SIZE 512
@@ -39,15 +43,56 @@ static volatile uint8_t txBuffer[HTK_TX_BUFFER_SIZE];
 
 static serialPort_t *htkPort = NULL;
 
-bool gimbal_htk_detect(void)
+gimbalVTable_t gimbalSerialVTable = {
+       //void (*process)(gimbalDevice_t *gimbalDevice, timeUs_t currentTimeUs);
+    .process = gimbalSerialProcess,
+
+    //gimbalDevType_e (*getDeviceType)(const gimbalDevice_t *gimablDevice);
+    .getDeviceType = gimbalSerialGetDeviceType,
+    //bool (*isReady)(const gimbalDevice_t *gimbalDevice); 
+    .isReady = gimbalSerialIsReady
+
+};
+
+gimbalDevice_t serialGimbalDevice = {
+    .vTable = &gimbalSerialVTable
+
+};
+
+gimbalDevType_e gimbalSerialGetDeviceType(const gimbalDevice_t *gimbalDevice)
+{
+    UNUSED(gimbalDevice);
+    return GIMBAL_DEV_SERIAL;
+}
+
+bool gimbalSerialIsReady(const gimbalDevice_t *gimbalDevice)
+{
+    return htkPort != NULL && gimbalDevice->vTable != NULL;
+}
+
+bool gimbalSerialInit(void)
 {
-    serialPortConfig_t *portConfig = findNextSerialPortConfig(FUNCTION_HTK_GIMBAL);
+    if(gimbalSerialDetect()) {
+        gimbalCommonSetDevice(&serialGimbalDevice);
+        return true;
+    }
+
+    return false;
+}
+
+bool gimbalSerialDetect(void)
+{
+
+    SD(fprintf(stderr, "[GIMBAL]: serial Detect...\n"));
+    serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_HTK_GIMBAL);
 
     if (portConfig) {
+        SD(fprintf(stderr, "[GIMBAL]: found port...\n"));
         htkPort = openSerialPort(portConfig->identifier, FUNCTION_HTK_GIMBAL, NULL, NULL,
                 115200, MODE_RXTX, SERIAL_NOT_INVERTED);
 
         if (htkPort) {
+            SD(fprintf(stderr, "[GIMBAL]: port open!\n"));
             htkPort->txBuffer = txBuffer;
             htkPort->txBufferSize = HTK_TX_BUFFER_SIZE;
             htkPort->txBufferTail = 0;
@@ -57,21 +102,24 @@ bool gimbal_htk_detect(void)
         }
     }
 
+    SD(fprintf(stderr, "[GIMBAL]: port not found :(...\n"));
     return false;
 }
 
-void gimbal_htk_update(void)
+void gimbalSerialProcess(gimbalDevice_t *gimablDevice, timeUs_t currentTime)
 {
-    if (!htkPort) {
+    UNUSED(currentTime);
+
+    if (!gimbalSerialIsReady(gimablDevice)) {
+        SD(fprintf(stderr, "[GIMBAL] gimabl not ready...\n"));
         return;
     }
 
     gimbalHtkAttitudePkt_t attittude = {
         .sync = {HTKATTITUDE_SYNC0, HTKATTITUDE_SYNC1},
-        .mode = GIMBAL_MODE_FOLLOW,
+        .mode = GIMBAL_MODE_DEFAULT
     };
 
-
     const gimbalConfig_t *cfg = gimbalConfig();
 
     int yaw = 1500;
diff --git a/src/main/io/gimbal_htk.h b/src/main/io/gimbal_htk.h
index b45b8f02651..bebcb794009 100644
--- a/src/main/io/gimbal_htk.h
+++ b/src/main/io/gimbal_htk.h
@@ -23,7 +23,6 @@
 
 #include <stdint.h>
 
-#define GIMBAL_HTK_MODE_DEFAULT GIMBAL_HTK_MODE_FOLLOW
 
 #define HTKATTITUDE_SYNC0  0xA5
 #define HTKATTITUDE_SYNC1  0x5A
@@ -43,6 +42,10 @@ typedef struct gimbalHtkAttitudePkt_s
 uint8_t gimbal_scale8(int8_t inputMin, int8_t inputMax, int8_t outputMin, int8_t outputMax, int8_t value);
 uint16_t gimbal_scale16(int16_t inputMin, int16_t inputMax, int16_t outputMin, int16_t outputMax, int16_t value);
 
-void gimbal_htk_update(void);
+bool gimbalSerialInit(void);
+bool gimbalSerialDetect(void);
+void gimbalSerialProcess(gimbalDevice_t *gimablDevice, timeUs_t currentTime);
+bool gimbalSerialIsReady(const gimbalDevice_t *gimbalDevice);
+gimbalDevType_e gimbalSerialGetDeviceType(const gimbalDevice_t *gimbalDevice);
 
 #endif
\ No newline at end of file
diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h
index 36fa524141f..88b1ccf0384 100755
--- a/src/main/scheduler/scheduler.h
+++ b/src/main/scheduler/scheduler.h
@@ -122,9 +122,9 @@ typedef enum {
 #ifdef USE_IRLOCK
     TASK_IRLOCK,
 #endif
-//#ifdef USE_SERIAL_GIMBAL
+#ifdef USE_SERIAL_GIMBAL
     TASK_GIMBAL,
-//#endif
+#endif
     /* Count of real tasks */
     TASK_COUNT,
 

From 5037401b32a951ea7ba9a4080cbee855cf12d6ec Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Thu, 6 Jun 2024 00:45:23 +0200
Subject: [PATCH 177/323] Fix typo in settings and update docs

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

diff --git a/docs/Settings.md b/docs/Settings.md
index fb93cfb69ce..ab5faf3ba1f 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -1462,7 +1462,7 @@ Yaw Iterm is frozen when bank angle is above this threshold [degrees]. This solv
 
 ---
 
-### gimabl_pan_channel
+### gimbal_pan_channel
 
 Gimbal pan rc channel index. 0 is no channel.
 
@@ -1472,7 +1472,7 @@ Gimbal pan rc channel index. 0 is no channel.
 
 ---
 
-### gimabl_roll_channel
+### gimbal_roll_channel
 
 Gimbal roll rc channel index. 0 is no channel.
 
@@ -1482,7 +1482,7 @@ Gimbal roll rc channel index. 0 is no channel.
 
 ---
 
-### gimabl_sensitivity
+### gimbal_sensitivity
 
 Gimbal sensitivity is similar to gain and will affect how quickly the gimbal will react.
 
@@ -1492,7 +1492,7 @@ Gimbal sensitivity is similar to gain and will affect how quickly the gimbal wil
 
 ---
 
-### gimabl_tilt_channel
+### gimbal_tilt_channel
 
 Gimbal tilt rc channel index. 0 is no channel.
 
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 2127c3505ae..8f50b3218ba 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -4151,25 +4151,25 @@ groups:
     headers: ["drivers/gimbal_common.h"]
     condition: USE_SERIAL_GIMBAL
     members:
-      - name: gimabl_pan_channel
+      - name: gimbal_pan_channel
         description: "Gimbal pan rc channel index. 0 is no channel."
         default_value: 0
         field: panChannel
         min: 0
         max: 32
-      - name: gimabl_roll_channel
+      - name: gimbal_roll_channel
         description: "Gimbal roll rc channel index. 0 is no channel."
         default_value: 0
         field: rollChannel
         min: 0
         max: 32
-      - name: gimabl_tilt_channel
+      - name: gimbal_tilt_channel
         description: "Gimbal tilt rc channel index. 0 is no channel."
         default_value: 0
         field: tiltChannel
         min: 0
         max: 32
-      - name: gimabl_sensitivity
+      - name: gimbal_sensitivity
         description: "Gimbal sensitivity is similar to gain and will affect how quickly the gimbal will react."
         default_value: 0
         field: sensitivity

From 033869b9da0c65828c518dba24be5ae468169e1f Mon Sep 17 00:00:00 2001
From: Darren Lines <darren@darrenlines.uk>
Date: Thu, 6 Jun 2024 12:04:29 +0100
Subject: [PATCH 178/323] Update settings.yaml

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

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 3c192495751..4a22ecb8711 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -2866,7 +2866,7 @@ groups:
         description: "Forward acceleration threshold for bungee launch or throw launch [cm/s/s], 1G = 981 cm/s/s"
         default_value: 1863
         field: fw.launch_accel_thresh
-        min: 1500
+        min: 1350
         max: 20000
       - name: nav_fw_launch_max_angle
         description: "Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg]"

From 4fb7204d378c86f7cfe164097ddfe104b73f0ef8 Mon Sep 17 00:00:00 2001
From: Darren Lines <darren@darrenlines.uk>
Date: Thu, 6 Jun 2024 12:06:26 +0100
Subject: [PATCH 179/323] Update Settings.md

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

diff --git a/docs/Settings.md b/docs/Settings.md
index a1c339c1d8c..3d0758ab2a2 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -2958,7 +2958,7 @@ Forward acceleration threshold for bungee launch or throw launch [cm/s/s], 1G =
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 1863 | 1500 | 20000 |
+| 1863 | 1350 | 20000 |
 
 ---
 

From 93d19f786bd647dafdb26013f10bfdc02abf94d3 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Thu, 6 Jun 2024 17:35:53 +0200
Subject: [PATCH 180/323] Let f722 have some fun

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

diff --git a/src/main/target/common.h b/src/main/target/common.h
index 676b1760409..cf23a7a4b9b 100644
--- a/src/main/target/common.h
+++ b/src/main/target/common.h
@@ -190,10 +190,10 @@
 #define ADSB_LIMIT_CM                   6400000
 #endif
 
+#define USE_SERIAL_GIMBAL
 
 //Designed to free space of F722 and F411 MCUs
 #if (MCU_FLASH_SIZE > 512)
-#define USE_SERIAL_GIMBAL
 #define USE_VTX_FFPV
 #define USE_SERIALRX_SUMD
 #define USE_TELEMETRY_HOTT

From 1f9c2857cbf80101f236c2ceb8ab4c75eb9f4e96 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Thu, 6 Jun 2024 17:44:31 +0200
Subject: [PATCH 181/323] More .gitignore

---
 .gitignore | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.gitignore b/.gitignore
index 39ef205b954..65804b74b0a 100644
--- a/.gitignore
+++ b/.gitignore
@@ -18,7 +18,7 @@ startup_stm32f10x_md_gcc.s
 cov-int*
 /build/
 /build_SITL/
-/sitl/
+/[hs]itl/
 /obj/
 /patches/
 /tools/

From c6da0b7540796cedb1602cdc2577040f117f521a Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Thu, 6 Jun 2024 21:12:05 +0200
Subject: [PATCH 182/323] Add unit tests

---
 src/main/CMakeLists.txt                       |  4 +-
 src/main/drivers/gimbal_common.c              |  6 +++
 src/main/drivers/gimbal_common.h              |  7 +++
 src/main/fc/fc_init.c                         |  2 +-
 src/main/io/{gimbal_htk.c => gimbal_serial.c} | 33 ++++++++----
 src/main/io/{gimbal_htk.h => gimbal_serial.h} | 14 +++++-
 src/main/io/serial.h                          |  2 +-
 src/test/unit/CMakeLists.txt                  |  3 ++
 src/test/unit/gimbal_serial_unittest.cc       | 50 +++++++++++++++++++
 src/test/unit/maths_unittest.cc               |  8 +--
 10 files changed, 109 insertions(+), 20 deletions(-)
 rename src/main/io/{gimbal_htk.c => gimbal_serial.c} (88%)
 rename src/main/io/{gimbal_htk.h => gimbal_serial.h} (93%)
 create mode 100644 src/test/unit/gimbal_serial_unittest.cc

diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt
index 31d09c716cf..6fb191c161e 100755
--- a/src/main/CMakeLists.txt
+++ b/src/main/CMakeLists.txt
@@ -356,8 +356,8 @@ main_sources(COMMON_SRC
     io/servo_sbus.h
     io/frsky_osd.c
     io/frsky_osd.h
-    io/gimbal_htk.c
-    io/gimbal_htk.h
+    io/gimbal_serial.c
+    io/gimbal_serial.h
     io/osd_dji_hd.c
     io/osd_dji_hd.h
     io/lights.c
diff --git a/src/main/drivers/gimbal_common.c b/src/main/drivers/gimbal_common.c
index 340db5fbdce..ba09b8597c8 100644
--- a/src/main/drivers/gimbal_common.c
+++ b/src/main/drivers/gimbal_common.c
@@ -74,6 +74,11 @@ bool gimbalCommonIsReady(gimbalDevice_t *gimbalDevice)
     return false;
 }
 
+#ifdef GIMBAL_UNIT_TEST
+void taskUpdateGimbal(timeUs_t currentTimeUs)
+{
+}
+#else
 void taskUpdateGimbal(timeUs_t currentTimeUs)
 {
     if (cliMode) {
@@ -86,5 +91,6 @@ void taskUpdateGimbal(timeUs_t currentTimeUs)
         gimbalCommonProcess(gimbalDevice, currentTimeUs);
     }
 }
+#endif
 
 #endif
\ No newline at end of file
diff --git a/src/main/drivers/gimbal_common.h b/src/main/drivers/gimbal_common.h
index 8814e1010dd..0a7113688ee 100644
--- a/src/main/drivers/gimbal_common.h
+++ b/src/main/drivers/gimbal_common.h
@@ -26,6 +26,9 @@
 #include "config/feature.h"
 #include "common/time.h"
 
+#ifdef __cplusplus
+extern "C" {
+#endif
 
 typedef enum {
     GIMBAL_DEV_UNSUPPORTED = 0,
@@ -80,4 +83,8 @@ bool gimbalCommonIsReady(gimbalDevice_t *gimbalDevice);
 
 void taskUpdateGimbal(timeUs_t currentTimeUs);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif
\ No newline at end of file
diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index 124fe1dc67c..76aa9d79511 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -108,7 +108,7 @@
 #include "io/displayport_msp_osd.h"
 #include "io/displayport_srxl.h"
 #include "io/flashfs.h"
-#include "io/gimbal_htk.h"
+#include "io/gimbal_serial.h"
 #include "io/gps.h"
 #include "io/ledstrip.h"
 #include "io/osd.h"
diff --git a/src/main/io/gimbal_htk.c b/src/main/io/gimbal_serial.c
similarity index 88%
rename from src/main/io/gimbal_htk.c
rename to src/main/io/gimbal_serial.c
index 2c80113365e..8bc0c328a51 100644
--- a/src/main/io/gimbal_htk.c
+++ b/src/main/io/gimbal_serial.c
@@ -30,7 +30,7 @@
 #include <drivers/gimbal_common.h>
 #include <drivers/serial.h>
 
-#include <io/gimbal_htk.h>
+#include <io/gimbal_serial.h>
 #include <io/serial.h>
 
 #include <rx/rx.h>
@@ -38,8 +38,8 @@
 
 STATIC_ASSERT(sizeof(gimbalHtkAttitudePkt_t) == 10, gimbalHtkAttitudePkt_t_size_not_10);
 
-#define HTK_TX_BUFFER_SIZE 512
-static volatile uint8_t txBuffer[HTK_TX_BUFFER_SIZE];
+#define GIMBAL_SERIAL_BUFFER_SIZE 512
+static volatile uint8_t txBuffer[GIMBAL_SERIAL_BUFFER_SIZE];
 
 static serialPort_t *htkPort = NULL;
 
@@ -80,21 +80,27 @@ bool gimbalSerialInit(void)
     return false;
 }
 
+#ifdef GIMBAL_UNIT_TEST
+bool gimbalSerialDetect(void)
+{
+    return false;
+}
+#else
 bool gimbalSerialDetect(void)
 {
 
     SD(fprintf(stderr, "[GIMBAL]: serial Detect...\n"));
-    serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_HTK_GIMBAL);
+    serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_GIMBAL);
 
     if (portConfig) {
         SD(fprintf(stderr, "[GIMBAL]: found port...\n"));
-        htkPort = openSerialPort(portConfig->identifier, FUNCTION_HTK_GIMBAL, NULL, NULL,
-                115200, MODE_RXTX, SERIAL_NOT_INVERTED);
+        htkPort = openSerialPort(portConfig->identifier, FUNCTION_GIMBAL, NULL, NULL,
+                baudRates[portConfig->peripheral_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
 
         if (htkPort) {
             SD(fprintf(stderr, "[GIMBAL]: port open!\n"));
             htkPort->txBuffer = txBuffer;
-            htkPort->txBufferSize = HTK_TX_BUFFER_SIZE;
+            htkPort->txBufferSize = GIMBAL_SERIAL_BUFFER_SIZE;
             htkPort->txBufferTail = 0;
             htkPort->txBufferHead = 0;
 
@@ -105,7 +111,13 @@ bool gimbalSerialDetect(void)
     SD(fprintf(stderr, "[GIMBAL]: port not found :(...\n"));
     return false;
 }
+#endif
 
+#ifdef GIMBAL_UNIT_TEST
+void gimbalSerialProcess(gimbalDevice_t *gimablDevice, timeUs_t currentTime)
+{
+}
+#else
 void gimbalSerialProcess(gimbalDevice_t *gimablDevice, timeUs_t currentTime)
 {
     UNUSED(currentTime);
@@ -171,9 +183,9 @@ void gimbalSerialProcess(gimbalDevice_t *gimablDevice, timeUs_t currentTime)
 
     attittude.sensibility = gimbal_scale8(-16, 15, 0, 31, cfg->sensitivity);
 
-    attittude.yaw = gimbal_scale16(1000, 2000, 0, 4095, yaw);
-    attittude.pitch = gimbal_scale16(1000, 2000, 0, 4095, pitch);
-    attittude.roll = gimbal_scale16(1000, 2000, 0, 4095, roll);
+    attittude.yaw = 3000;//gimbal_scale16(1000, 2000, 0, 4095, yaw);
+    attittude.pitch = 3000; //gimbal_scale16(1000, 2000, 0, 4095, pitch);
+    attittude.roll = 3000; // gimbal_scale16(1000, 2000, 0, 4095, roll);
 
     uint16_t crc16 = 0;
     uint8_t *b = (uint8_t *)&attittude;
@@ -188,6 +200,7 @@ void gimbalSerialProcess(gimbalDevice_t *gimablDevice, timeUs_t currentTime)
     serialEndWrite(htkPort);
     // Send new data
 }
+#endif
 
 uint8_t gimbal_scale8(int8_t inputMin, int8_t inputMax, int8_t outputMin, int8_t outputMax, int8_t value)
 {
diff --git a/src/main/io/gimbal_htk.h b/src/main/io/gimbal_serial.h
similarity index 93%
rename from src/main/io/gimbal_htk.h
rename to src/main/io/gimbal_serial.h
index bebcb794009..67b34a7458e 100644
--- a/src/main/io/gimbal_htk.h
+++ b/src/main/io/gimbal_serial.h
@@ -17,12 +17,18 @@
 
 #pragma once
 
+#include <stdint.h>
+
 #include "platform.h"
 
-#ifdef USE_SERIAL_GIMBAL
+#include "common/time.h"
+#include "drivers/gimbal_common.h"
 
-#include <stdint.h>
+#ifdef __cplusplus
+extern "C" {
+#endif
 
+#ifdef USE_SERIAL_GIMBAL
 
 #define HTKATTITUDE_SYNC0  0xA5
 #define HTKATTITUDE_SYNC1  0x5A
@@ -48,4 +54,8 @@ void gimbalSerialProcess(gimbalDevice_t *gimablDevice, timeUs_t currentTime);
 bool gimbalSerialIsReady(const gimbalDevice_t *gimbalDevice);
 gimbalDevType_e gimbalSerialGetDeviceType(const gimbalDevice_t *gimbalDevice);
 
+#endif
+
+#ifdef __cplusplus
+}
 #endif
\ No newline at end of file
diff --git a/src/main/io/serial.h b/src/main/io/serial.h
index dad56a9f4bc..c2429d9887a 100644
--- a/src/main/io/serial.h
+++ b/src/main/io/serial.h
@@ -57,7 +57,7 @@ typedef enum {
     FUNCTION_TELEMETRY_SMARTPORT_MASTER = (1 << 23), // 8388608
     FUNCTION_UNUSED_2                   = (1 << 24), // 16777216
     FUNCTION_MSP_OSD                    = (1 << 25), // 33554432
-    FUNCTION_HTK_GIMBAL                 = (1 << 26), // 67108864
+    FUNCTION_GIMBAL                     = (1 << 26), // 67108864
 } serialPortFunction_e;
 
 #define FUNCTION_VTX_MSP FUNCTION_MSP_OSD
diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt
index ebfd3b78d4f..8f165c0fe9e 100644
--- a/src/test/unit/CMakeLists.txt
+++ b/src/test/unit/CMakeLists.txt
@@ -40,6 +40,9 @@ set_property(SOURCE osd_unittest.cc PROPERTY definitions OSD_UNIT_TEST USE_MSP_D
 set_property(SOURCE gps_ublox_unittest.cc PROPERTY depends "io/gps_ublox_utils.c")
 set_property(SOURCE gps_ublox_unittest.cc PROPERTY definitions GPS_UBLOX_UNIT_TEST)
 
+set_property(SOURCE gimbal_serial_unittest.cc PROPERTY depends "io/gimbal_serial.c" "drivers/gimbal_common.c")
+set_property(SOURCE gimbal_serial_unittest.cc PROPERTY definitions USE_SERIAL_GIMBAL GIMBAL_UNIT_TEST)
+
 function(unit_test src)
     get_filename_component(basename ${src} NAME)
     string(REPLACE ".cc" "" name ${basename} )
diff --git a/src/test/unit/gimbal_serial_unittest.cc b/src/test/unit/gimbal_serial_unittest.cc
new file mode 100644
index 00000000000..0cafc2081bf
--- /dev/null
+++ b/src/test/unit/gimbal_serial_unittest.cc
@@ -0,0 +1,50 @@
+/*
+ * This file is part of INAV.
+ *
+ * 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 INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <iostream>
+#include <string>
+#include <stdio.h>
+#include <stdint.h>
+
+#include "gtest/gtest.h"
+#include "unittest_macros.h"
+
+#include "io/gimbal_serial.h"
+
+void dumpMemory(uint8_t *mem, int size)
+{
+    for(int i =0; i < size; ++i) {
+        printf("%02x ", mem[i]);
+    }
+    printf("\n");
+}
+
+TEST(GimbalSerialTest, TestGimbalSerialScale)
+{
+//uint8_t gimbal_scale8(int8_t inputMin, int8_t inputMax, int8_t outputMin, int8_t outputMax, int8_t value);
+    uint8_t res8 = gimbal_scale8(-16, 15, 0, 32, 0);
+    printf("res8: %i\n", res8);
+    EXPECT_TRUE(res8 == 15);
+//uint16_t gimbal_scale16(int16_t inputMin, int16_t inputMax, int16_t outputMin, int16_t outputMax, int16_t value);
+    uint16_t res16 = gimbal_scale16(1000, 2000, -2048, 2047, 0);
+    printf("res16: %i\n", res16);
+    EXPECT_TRUE(res16 == 1499);
+    //EXPECT_TRUE(valuesAdded == 12);
+    //EXPECT_TRUE(1 == valuesAdded);
+    //EXPECT_FALSE(memcmp((void *)expected, (void *)&cfg, 17));
+    //EXPECT_FALSE(strcmp(buf, " 123.45"));
+}
\ No newline at end of file
diff --git a/src/test/unit/maths_unittest.cc b/src/test/unit/maths_unittest.cc
index 538f28086af..77779d366e2 100644
--- a/src/test/unit/maths_unittest.cc
+++ b/src/test/unit/maths_unittest.cc
@@ -128,28 +128,28 @@ void expectVectorsAreEqual(fpVector3_t *a, fpVector3_t *b)
 
 TEST(MathsUnittest, TestRotateVectorWithNoAngle)
 {
-    fpVector3_t vector = { 1.0f, 0.0f, 0.0f};
+    fpVector3_t vector = { .x = 1.0f, .y = 0.0f, .z = 0.0f};
     fp_angles_t euler_angles = {.raw={0.0f, 0.0f, 0.0f}};
 
     fpMat3_t rmat;
     rotationMatrixFromAngles(&rmat, &euler_angles);
     rotationMatrixRotateVector(&vector, &vector, &rmat);
 
-    fpVector3_t expected_result = { 1.0f, 0.0f, 0.0f};
+    fpVector3_t expected_result = { .x = 1.0f, .y = 0.0f, .z = 0.0f};
     expectVectorsAreEqual(&vector, &expected_result);
 }
 
 TEST(MathsUnittest, TestRotateVectorAroundAxis)
 {
     // Rotate a vector <1, 0, 0> around an each axis x y and z.
-    fpVector3_t vector = { 1.0f, 0.0f, 0.0f};
+    fpVector3_t vector = { .x = 1.0f, .y = 0.0f, .z = 0.0f};
     fp_angles_t euler_angles = {.raw={90.0f, 0.0f, 0.0f}};
 
     fpMat3_t rmat;
     rotationMatrixFromAngles(&rmat, &euler_angles);
     rotationMatrixRotateVector(&vector, &vector, &rmat);
 
-    fpVector3_t expected_result = { 1.0f, 0.0f, 0.0f};
+    fpVector3_t expected_result = { .x = 1.0f, .y = 0.0f, .z = 0.0f};
     expectVectorsAreEqual(&vector, &expected_result);
 }
 

From 719a96e9c8527001915c6295867ba70ca6de1198 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Thu, 6 Jun 2024 22:28:19 +0200
Subject: [PATCH 183/323] Tested working. May need to clamp inputs to 1000-2000
 range

---
 src/main/io/gimbal_serial.c             | 49 ++++++++++++++++++++-----
 src/main/io/gimbal_serial.h             |  3 +-
 src/test/unit/CMakeLists.txt            |  2 +-
 src/test/unit/gimbal_serial_unittest.cc | 21 +++++------
 4 files changed, 51 insertions(+), 24 deletions(-)

diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index 8bc0c328a51..67c6f5263cb 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -25,6 +25,7 @@
 
 #include <common/crc.h>
 #include <common/utils.h>
+#include <common/maths.h>
 #include <build/debug.h>
 
 #include <drivers/gimbal_common.h>
@@ -181,11 +182,11 @@ void gimbalSerialProcess(gimbalDevice_t *gimablDevice, timeUs_t currentTime)
         }
     }
 
-    attittude.sensibility = gimbal_scale8(-16, 15, 0, 31, cfg->sensitivity);
+    attittude.sensibility = cfg->sensitivity; //gimbal_scale5(-16, 15, -16, 15, cfg->sensitivity);
 
-    attittude.yaw = 3000;//gimbal_scale16(1000, 2000, 0, 4095, yaw);
-    attittude.pitch = 3000; //gimbal_scale16(1000, 2000, 0, 4095, pitch);
-    attittude.roll = 3000; // gimbal_scale16(1000, 2000, 0, 4095, roll);
+    attittude.yaw = gimbal_scale12(1000, 2000, yaw);
+    attittude.pitch = gimbal_scale12(1000, 2000, pitch);
+    attittude.roll = gimbal_scale12(1000, 2000, roll);
 
     uint16_t crc16 = 0;
     uint8_t *b = (uint8_t *)&attittude;
@@ -202,16 +203,44 @@ void gimbalSerialProcess(gimbalDevice_t *gimablDevice, timeUs_t currentTime)
 }
 #endif
 
-uint8_t gimbal_scale8(int8_t inputMin, int8_t inputMax, int8_t outputMin, int8_t outputMax, int8_t value)
+int8_t gimbal_scale5(int8_t inputMin, int8_t inputMax, int8_t outputMin, int8_t outputMax, int8_t value)
 {
-    float m = (1.0f * outputMax - outputMin) / (inputMax - inputMin);
-    return (uint8_t)((outputMin + (m * (value - inputMin))) + 0.5f);
+    int8_t ret = 0;
+    //uint8_t *rp = (uint8_t *)&ret;
+    ret =  scaleRange(value, inputMin, inputMax, outputMin, outputMax);
+    return ret;
+    // bit magic for ensuring signed representation
+    //if(ret < 0) {
+    //    *rp = *rp >> 3;
+    //    *rp |= (1 << 5);
+    //}
+    //return *rp & 0b11111;
+    //return *rp;
 }
 
-uint16_t gimbal_scale16(int16_t inputMin, int16_t inputMax, int16_t outputMin, int16_t outputMax, int16_t value)
+int16_t gimbal_scale12(int16_t inputMin, int16_t inputMax, int16_t value)
 {
-    float m = (1.0f * outputMax - outputMin) / (inputMax - inputMin);
-    return (uint16_t)((outputMin + (m * (value - inputMin))) + 0.5f);
+    int16_t ret = 0;
+    //uint16_t *rp = (uint16_t *)&ret;
+    ret = scaleRange(value, inputMin, inputMax, -2048, 2047);
+    return ret;
+    // bit magic for signed representation
+    //if(ret < 0) {
+    //    *rp = *rp >> 4;
+    //    *rp |= (1 << 12);
+    //}
+    //return *rp & 0b111111111111;
+    //printf("bogus scale: %i(", ret);
+    //for (int i = 0; i < sizeof(ret) * 8; ++i) {
+    //    if (ret & (1 << ((sizeof(ret) * 8)- i))) {
+    //        printf("1");
+    //    } else {
+    //        printf("0");
+    //    }
+    //}
+    //printf(")\n");
+ 
+    //return *rp;
 }
 
 #endif
\ No newline at end of file
diff --git a/src/main/io/gimbal_serial.h b/src/main/io/gimbal_serial.h
index 67b34a7458e..f74f11bfe30 100644
--- a/src/main/io/gimbal_serial.h
+++ b/src/main/io/gimbal_serial.h
@@ -45,8 +45,7 @@ typedef struct gimbalHtkAttitudePkt_s
 	uint64_t crcl:8;    //Data validation L
 } __attribute__((packed)) gimbalHtkAttitudePkt_t;
 
-uint8_t gimbal_scale8(int8_t inputMin, int8_t inputMax, int8_t outputMin, int8_t outputMax, int8_t value);
-uint16_t gimbal_scale16(int16_t inputMin, int16_t inputMax, int16_t outputMin, int16_t outputMax, int16_t value);
+int16_t gimbal_scale12(int16_t inputMin, int16_t inputMax, int16_t value);
 
 bool gimbalSerialInit(void);
 bool gimbalSerialDetect(void);
diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt
index 8f165c0fe9e..300721b8f53 100644
--- a/src/test/unit/CMakeLists.txt
+++ b/src/test/unit/CMakeLists.txt
@@ -40,7 +40,7 @@ set_property(SOURCE osd_unittest.cc PROPERTY definitions OSD_UNIT_TEST USE_MSP_D
 set_property(SOURCE gps_ublox_unittest.cc PROPERTY depends "io/gps_ublox_utils.c")
 set_property(SOURCE gps_ublox_unittest.cc PROPERTY definitions GPS_UBLOX_UNIT_TEST)
 
-set_property(SOURCE gimbal_serial_unittest.cc PROPERTY depends "io/gimbal_serial.c" "drivers/gimbal_common.c")
+set_property(SOURCE gimbal_serial_unittest.cc PROPERTY depends "io/gimbal_serial.c" "drivers/gimbal_common.c" "common/maths.c")
 set_property(SOURCE gimbal_serial_unittest.cc PROPERTY definitions USE_SERIAL_GIMBAL GIMBAL_UNIT_TEST)
 
 function(unit_test src)
diff --git a/src/test/unit/gimbal_serial_unittest.cc b/src/test/unit/gimbal_serial_unittest.cc
index 0cafc2081bf..77ae4c93df8 100644
--- a/src/test/unit/gimbal_serial_unittest.cc
+++ b/src/test/unit/gimbal_serial_unittest.cc
@@ -35,16 +35,15 @@ void dumpMemory(uint8_t *mem, int size)
 
 TEST(GimbalSerialTest, TestGimbalSerialScale)
 {
-//uint8_t gimbal_scale8(int8_t inputMin, int8_t inputMax, int8_t outputMin, int8_t outputMax, int8_t value);
-    uint8_t res8 = gimbal_scale8(-16, 15, 0, 32, 0);
-    printf("res8: %i\n", res8);
-    EXPECT_TRUE(res8 == 15);
-//uint16_t gimbal_scale16(int16_t inputMin, int16_t inputMax, int16_t outputMin, int16_t outputMax, int16_t value);
-    uint16_t res16 = gimbal_scale16(1000, 2000, -2048, 2047, 0);
+    int16_t res16 = gimbal_scale12(1000, 2000, 2000);
+    EXPECT_TRUE(res16 == 2047);
+    res16 = gimbal_scale12(1000, 2000, 1000);
     printf("res16: %i\n", res16);
-    EXPECT_TRUE(res16 == 1499);
-    //EXPECT_TRUE(valuesAdded == 12);
-    //EXPECT_TRUE(1 == valuesAdded);
-    //EXPECT_FALSE(memcmp((void *)expected, (void *)&cfg, 17));
-    //EXPECT_FALSE(strcmp(buf, " 123.45"));
+    EXPECT_TRUE(res16 == -2048);
+    res16 = gimbal_scale12(1000, 2000, 1500);
+    printf("res16: %i\n", res16);
+    EXPECT_TRUE(res16 == -1);
+    res16 = gimbal_scale12(1000, 2000, 1501);
+    printf("res16: %i\n", res16);
+    EXPECT_TRUE(res16 == 3);
 }
\ No newline at end of file

From 4ba9a1b085b6dfd01ccee7abfa09b24569fe4e7b Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Thu, 6 Jun 2024 22:44:02 +0200
Subject: [PATCH 184/323] Clean up

---
 src/main/io/gimbal_serial.c | 34 ----------------------------------
 1 file changed, 34 deletions(-)

diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index 67c6f5263cb..223d9c7a408 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -199,48 +199,14 @@ void gimbalSerialProcess(gimbalDevice_t *gimablDevice, timeUs_t currentTime)
     serialBeginWrite(htkPort);
     serialWriteBuf(htkPort, (uint8_t *)&attittude, sizeof(gimbalHtkAttitudePkt_t));
     serialEndWrite(htkPort);
-    // Send new data
 }
 #endif
 
-int8_t gimbal_scale5(int8_t inputMin, int8_t inputMax, int8_t outputMin, int8_t outputMax, int8_t value)
-{
-    int8_t ret = 0;
-    //uint8_t *rp = (uint8_t *)&ret;
-    ret =  scaleRange(value, inputMin, inputMax, outputMin, outputMax);
-    return ret;
-    // bit magic for ensuring signed representation
-    //if(ret < 0) {
-    //    *rp = *rp >> 3;
-    //    *rp |= (1 << 5);
-    //}
-    //return *rp & 0b11111;
-    //return *rp;
-}
-
 int16_t gimbal_scale12(int16_t inputMin, int16_t inputMax, int16_t value)
 {
     int16_t ret = 0;
-    //uint16_t *rp = (uint16_t *)&ret;
     ret = scaleRange(value, inputMin, inputMax, -2048, 2047);
     return ret;
-    // bit magic for signed representation
-    //if(ret < 0) {
-    //    *rp = *rp >> 4;
-    //    *rp |= (1 << 12);
-    //}
-    //return *rp & 0b111111111111;
-    //printf("bogus scale: %i(", ret);
-    //for (int i = 0; i < sizeof(ret) * 8; ++i) {
-    //    if (ret & (1 << ((sizeof(ret) * 8)- i))) {
-    //        printf("1");
-    //    } else {
-    //        printf("0");
-    //    }
-    //}
-    //printf(")\n");
- 
-    //return *rp;
 }
 
 #endif
\ No newline at end of file

From 50754d09c9fea2ef115038f15b6740b4505aa900 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Thu, 6 Jun 2024 22:44:48 +0200
Subject: [PATCH 185/323] ninja folder for tests

---
 .gitignore | 1 +
 1 file changed, 1 insertion(+)

diff --git a/.gitignore b/.gitignore
index 65804b74b0a..fd9c771bea2 100644
--- a/.gitignore
+++ b/.gitignore
@@ -19,6 +19,7 @@ cov-int*
 /build/
 /build_SITL/
 /[hs]itl/
+/ninja/
 /obj/
 /patches/
 /tools/

From db0856b7cb7ef598ee2d0fab1829fdf48ba41aad Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Thu, 6 Jun 2024 22:48:26 +0200
Subject: [PATCH 186/323] Fix typos

---
 src/main/drivers/gimbal_common.h |  2 +-
 src/main/io/gimbal_serial.c      | 12 ++++--------
 src/main/io/gimbal_serial.h      |  2 +-
 3 files changed, 6 insertions(+), 10 deletions(-)

diff --git a/src/main/drivers/gimbal_common.h b/src/main/drivers/gimbal_common.h
index 0a7113688ee..7ffbb9376aa 100644
--- a/src/main/drivers/gimbal_common.h
+++ b/src/main/drivers/gimbal_common.h
@@ -49,7 +49,7 @@ typedef struct gimbalDevice_s {
 
 typedef struct gimbalVTable_s {
     void (*process)(gimbalDevice_t *gimbalDevice, timeUs_t currentTimeUs);
-    gimbalDevType_e (*getDeviceType)(const gimbalDevice_t *gimablDevice);
+    gimbalDevType_e (*getDeviceType)(const gimbalDevice_t *gimbalDevice);
     bool (*isReady)(const gimbalDevice_t *gimbalDevice);
 } gimbalVTable_t;
 
diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index 223d9c7a408..a499dffaaca 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -45,12 +45,8 @@ static volatile uint8_t txBuffer[GIMBAL_SERIAL_BUFFER_SIZE];
 static serialPort_t *htkPort = NULL;
 
 gimbalVTable_t gimbalSerialVTable = {
-       //void (*process)(gimbalDevice_t *gimbalDevice, timeUs_t currentTimeUs);
     .process = gimbalSerialProcess,
-
-    //gimbalDevType_e (*getDeviceType)(const gimbalDevice_t *gimablDevice);
     .getDeviceType = gimbalSerialGetDeviceType,
-    //bool (*isReady)(const gimbalDevice_t *gimbalDevice); 
     .isReady = gimbalSerialIsReady
 
 };
@@ -115,16 +111,16 @@ bool gimbalSerialDetect(void)
 #endif
 
 #ifdef GIMBAL_UNIT_TEST
-void gimbalSerialProcess(gimbalDevice_t *gimablDevice, timeUs_t currentTime)
+void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
 {
 }
 #else
-void gimbalSerialProcess(gimbalDevice_t *gimablDevice, timeUs_t currentTime)
+void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
 {
     UNUSED(currentTime);
 
-    if (!gimbalSerialIsReady(gimablDevice)) {
-        SD(fprintf(stderr, "[GIMBAL] gimabl not ready...\n"));
+    if (!gimbalSerialIsReady(gimbalDevice)) {
+        SD(fprintf(stderr, "[GIMBAL] gimbal not ready...\n"));
         return;
     }
 
diff --git a/src/main/io/gimbal_serial.h b/src/main/io/gimbal_serial.h
index f74f11bfe30..3d0188b6b1c 100644
--- a/src/main/io/gimbal_serial.h
+++ b/src/main/io/gimbal_serial.h
@@ -49,7 +49,7 @@ int16_t gimbal_scale12(int16_t inputMin, int16_t inputMax, int16_t value);
 
 bool gimbalSerialInit(void);
 bool gimbalSerialDetect(void);
-void gimbalSerialProcess(gimbalDevice_t *gimablDevice, timeUs_t currentTime);
+void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime);
 bool gimbalSerialIsReady(const gimbalDevice_t *gimbalDevice);
 gimbalDevType_e gimbalSerialGetDeviceType(const gimbalDevice_t *gimbalDevice);
 

From eabca5e0a5dfc5cb5e072c458fb39a4f980cb29b Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Fri, 7 Jun 2024 12:38:18 +0200
Subject: [PATCH 187/323] Disable adaptive filter for SITL

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

diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h
index 3fbe45a5cab..048626ad8dd 100644
--- a/src/main/target/SITL/target.h
+++ b/src/main/target/SITL/target.h
@@ -97,6 +97,7 @@
 #undef USE_BRUSHED_ESC_AUTODETECT
 #undef USE_SERIAL_4WAY_BLHELI_BOOTLOADER
 #undef USE_SERIAL_4WAY_SK_BOOTLOADER
+#undef USE_ADAPTIVE_FILTER
 
 #undef USE_I2C
 #undef USE_SPI

From f8e72b8afd20f7b914069012665169da7160cad2 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Fri, 7 Jun 2024 12:57:42 +0200
Subject: [PATCH 188/323] Enable task only when adaptive filter is enabled

---
 src/main/fc/fc_tasks.c  | 2 +-
 src/main/sensors/gyro.c | 2 +-
 src/main/sensors/gyro.h | 2 +-
 3 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index 8da10280a9d..0fd239127c2 100755
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -428,7 +428,7 @@ void fcTasksInit(void)
 #endif
 
 #ifdef USE_ADAPTIVE_FILTER
-    setTaskEnabled(TASK_ADAPTIVE_FILTER, true);
+    setTaskEnabled(TASK_ADAPTIVE_FILTER, (gyroConfig()->gyroFilterMode == GYRO_FILTER_MODE_ADAPTIVE));
 #endif
 
 #if defined(SITL_BUILD)
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 7f7d1d171d9..2b0b7d8c9bd 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -130,8 +130,8 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .adaptiveFilterHpfHz = SETTING_GYRO_ADAPTIVE_FILTER_HPF_HZ_DEFAULT,
     .adaptiveFilterIntegratorThresholdHigh = SETTING_GYRO_ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD_HIGH_DEFAULT,
     .adaptiveFilterIntegratorThresholdLow  = SETTING_GYRO_ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD_LOW_DEFAULT,
-    .gyroFilterMode = SETTING_GYRO_FILTER_MODE_DEFAULT,
 #endif
+    .gyroFilterMode = SETTING_GYRO_FILTER_MODE_DEFAULT,
 );
 
 STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware)
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index 7f767e370d1..910dac2ea00 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -100,8 +100,8 @@ typedef struct gyroConfig_s {
     float adaptiveFilterHpfHz;
     float adaptiveFilterIntegratorThresholdHigh;
     float adaptiveFilterIntegratorThresholdLow;
-    uint8_t gyroFilterMode;
 #endif
+    uint8_t gyroFilterMode;
 } gyroConfig_t;
 
 PG_DECLARE(gyroConfig_t, gyroConfig);

From 08d46dd4712a9f38eab43e256172a2dbf9a9f23c Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Fri, 7 Jun 2024 13:39:37 +0200
Subject: [PATCH 189/323] Fix compilation error in config.c

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

diff --git a/src/main/fc/config.c b/src/main/fc/config.c
index ac98eca02cb..dc249df059b 100755
--- a/src/main/fc/config.c
+++ b/src/main/fc/config.c
@@ -191,6 +191,7 @@ uint32_t getGyroLooptime(void)
 void validateAndFixConfig(void)
 {
 
+#ifdef USE_ADAPTIVE_FILTER
     // gyroConfig()->adaptiveFilterMinHz has to be at least 5 units lower than gyroConfig()->gyro_main_lpf_hz
     if (gyroConfig()->adaptiveFilterMinHz + 5 > gyroConfig()->gyro_main_lpf_hz) {
         gyroConfigMutable()->adaptiveFilterMinHz = gyroConfig()->gyro_main_lpf_hz - 5;
@@ -199,6 +200,7 @@ void validateAndFixConfig(void)
     if (gyroConfig()->adaptiveFilterMaxHz - 5 < gyroConfig()->gyro_main_lpf_hz) {
         gyroConfigMutable()->adaptiveFilterMaxHz = gyroConfig()->gyro_main_lpf_hz + 5;
     }
+#endif
 
     if (accelerometerConfig()->acc_notch_cutoff >= accelerometerConfig()->acc_notch_hz) {
         accelerometerConfigMutable()->acc_notch_hz = 0;

From a136afe0ff67d0c7570b9eb87efe49f64fc94936 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sat, 8 Jun 2024 12:40:49 +0200
Subject: [PATCH 190/323] Add modes to active modes.

---
 src/main/drivers/gimbal_common.c |  6 ++++++
 src/main/drivers/gimbal_common.h |  2 ++
 src/main/fc/fc_msp_box.c         | 10 ++++++++++
 3 files changed, 18 insertions(+)

diff --git a/src/main/drivers/gimbal_common.c b/src/main/drivers/gimbal_common.c
index ba09b8597c8..3c93b44d793 100644
--- a/src/main/drivers/gimbal_common.c
+++ b/src/main/drivers/gimbal_common.c
@@ -93,4 +93,10 @@ void taskUpdateGimbal(timeUs_t currentTimeUs)
 }
 #endif
 
+// TODO: check if any gimbal types are enabled
+bool gimbalCommonIsEnabled(void)
+{
+    return true;
+}
+
 #endif
\ No newline at end of file
diff --git a/src/main/drivers/gimbal_common.h b/src/main/drivers/gimbal_common.h
index 7ffbb9376aa..93501e1dc9c 100644
--- a/src/main/drivers/gimbal_common.h
+++ b/src/main/drivers/gimbal_common.h
@@ -83,6 +83,8 @@ bool gimbalCommonIsReady(gimbalDevice_t *gimbalDevice);
 
 void taskUpdateGimbal(timeUs_t currentTimeUs);
 
+bool gimbalCommonIsEnabled(void);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c
index ca384084866..600fd06a7fc 100644
--- a/src/main/fc/fc_msp_box.c
+++ b/src/main/fc/fc_msp_box.c
@@ -43,6 +43,8 @@
 
 #include "telemetry/telemetry.h"
 
+#include "drivers/gimbal_common.h"
+
 #define BOX_SUFFIX ';'
 #define BOX_SUFFIX_LEN 1
 
@@ -361,6 +363,14 @@ void initActiveBoxIds(void)
     ADD_ACTIVE_BOX(BOXMIXERPROFILE);
     ADD_ACTIVE_BOX(BOXMIXERTRANSITION);
 #endif
+
+#ifdef USE_SERIAL_GIMBAL
+    if(gimbalCommonIsEnabled()) {
+        ADD_ACTIVE_BOX(BOXGIMBALPLOCK);
+        ADD_ACTIVE_BOX(BOXGIMBALPRLOCK);
+        ADD_ACTIVE_BOX(BOXGIMBALCENTER);
+    }
+#endif
 }
 
 #define IS_ENABLED(mask) ((mask) == 0 ? 0 : 1)

From 1474e029ac985eb3be8b74690d4706a817af4664 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sat, 8 Jun 2024 13:03:39 +0200
Subject: [PATCH 191/323] Add active modes

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

diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c
index 600fd06a7fc..fec9cde9550 100644
--- a/src/main/fc/fc_msp_box.c
+++ b/src/main/fc/fc_msp_box.c
@@ -104,8 +104,8 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { .boxId = BOXMIXERPROFILE,     .boxName = "MIXER PROFILE 2",   .permanentId = 62 },
     { .boxId = BOXMIXERTRANSITION,  .boxName = "MIXER TRANSITION",  .permanentId = 63 },
     { .boxId = BOXANGLEHOLD,        .boxName = "ANGLE HOLD",        .permanentId = 64 },
-    { .boxId = BOXGIMBALPLOCK,      .boxName = "GIMBAL LEVEL P",    .permanentId = 66 },
-    { .boxId = BOXGIMBALPRLOCK,     .boxName = "GIMBAL LEVEL P+R",  .permanentId = 66 },
+    { .boxId = BOXGIMBALPLOCK,      .boxName = "GIMBAL LEVEL PITCH",    .permanentId = 66 },
+    { .boxId = BOXGIMBALPRLOCK,     .boxName = "GIMBAL LEVEL PITCH ROLL",  .permanentId = 66 },
     { .boxId = BOXGIMBALCENTER,     .boxName = "GIMBAL CENTER",     .permanentId = 67 },
     { .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL,                .permanentId = 0xFF }
 };

From bcc9fd27ff8ab7150913c9e3083d6c384367d1db Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sat, 8 Jun 2024 13:12:24 +0200
Subject: [PATCH 192/323] fix permanentBoxId

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

diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c
index fec9cde9550..2915c0c50cb 100644
--- a/src/main/fc/fc_msp_box.c
+++ b/src/main/fc/fc_msp_box.c
@@ -104,7 +104,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { .boxId = BOXMIXERPROFILE,     .boxName = "MIXER PROFILE 2",   .permanentId = 62 },
     { .boxId = BOXMIXERTRANSITION,  .boxName = "MIXER TRANSITION",  .permanentId = 63 },
     { .boxId = BOXANGLEHOLD,        .boxName = "ANGLE HOLD",        .permanentId = 64 },
-    { .boxId = BOXGIMBALPLOCK,      .boxName = "GIMBAL LEVEL PITCH",    .permanentId = 66 },
+    { .boxId = BOXGIMBALPLOCK,      .boxName = "GIMBAL LEVEL PITCH",    .permanentId = 65 },
     { .boxId = BOXGIMBALPRLOCK,     .boxName = "GIMBAL LEVEL PITCH ROLL",  .permanentId = 66 },
     { .boxId = BOXGIMBALCENTER,     .boxName = "GIMBAL CENTER",     .permanentId = 67 },
     { .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL,                .permanentId = 0xFF }

From bf530ecd6fcedeebb8f178d24588df3f59d0dd1d Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sat, 8 Jun 2024 13:55:07 +0200
Subject: [PATCH 193/323] Lower new rates slightly

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

diff --git a/docs/Settings.md b/docs/Settings.md
index 40490808c83..265500e6720 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -2758,7 +2758,7 @@ Speed in fully autonomous modes (RTH, WP) [cm/s]. Used for WP mode when no speci
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 600 | 10 | 2000 |
+| 500 | 10 | 2000 |
 
 ---
 
@@ -3418,7 +3418,7 @@ Maximum speed allowed when processing pilot input for POSHOLD/CRUISE control mod
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 1000 | 10 | 2000 |
+| 750 | 10 | 2000 |
 
 ---
 
@@ -3438,7 +3438,7 @@ Maximum speed allowed in fully autonomous modes (RTH, WP) [cm/s] [Multirotor onl
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 1200 | 10 | 2000 |
+| 1000 | 10 | 2000 |
 
 ---
 
@@ -3478,7 +3478,7 @@ Maximum banking angle (deg) that multicopter navigation is allowed to set. Machi
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 40 | 15 | 45 |
+| 35 | 15 | 45 |
 
 ---
 
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 71964891ce9..f5b52be72a8 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -2510,7 +2510,7 @@ groups:
         table: nav_fw_wp_turn_smoothing
       - name: nav_auto_speed
         description: "Speed in fully autonomous modes (RTH, WP) [cm/s]. Used for WP mode when no specific WP speed set. [Multirotor only]"
-        default_value: 600
+        default_value: 500
         field: general.auto_speed
         min: 10
         max: 2000
@@ -2522,13 +2522,13 @@ groups:
         max: 50
       - name: nav_max_auto_speed
         description: "Maximum speed allowed in fully autonomous modes (RTH, WP) [cm/s] [Multirotor only]"
-        default_value: 1200
+        default_value: 1000
         field: general.max_auto_speed
         min: 10
         max: 2000
       - name: nav_manual_speed
         description: "Maximum speed allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only]"
-        default_value: 1000
+        default_value: 750
         field: general.max_manual_speed
         min: 10
         max: 2000
@@ -2704,7 +2704,7 @@ groups:
         max: 120
       - name: nav_mc_bank_angle
         description: "Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude"
-        default_value: 40
+        default_value: 35
         field: mc.max_bank_angle
         min: 15
         max: 45

From bf651acaf91d06bac78ccf8c0dbc352714fdafeb Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sat, 8 Jun 2024 17:26:06 +0200
Subject: [PATCH 194/323] Update flight modes

---
 src/main/drivers/gimbal_common.h | 11 +++++++----
 src/main/fc/fc_msp_box.c         | 12 ++++++------
 src/main/fc/rc_modes.h           |  4 ++--
 src/main/io/gimbal_serial.c      | 21 +++++++++++++--------
 4 files changed, 28 insertions(+), 20 deletions(-)

diff --git a/src/main/drivers/gimbal_common.h b/src/main/drivers/gimbal_common.h
index 93501e1dc9c..0181c2cb2c0 100644
--- a/src/main/drivers/gimbal_common.h
+++ b/src/main/drivers/gimbal_common.h
@@ -64,12 +64,15 @@ typedef struct gimbalConfig_s {
 PG_DECLARE(gimbalConfig_t, gimbalConfig);
 
 typedef enum {
-    GIMBAL_MODE_PITCH_ROLL_LOCK = 0,
-    GIMBAL_MODE_PITCH_LOCK = 1,
-    GIMBAL_MODE_FOLLOW = 2
+    GIMBAL_MODE_FOLLOW = 0,
+    GIMBAL_MODE_TILT_LOCK = (1<<0),
+    GIMBAL_MODE_ROLL_LOCK = (1<<1),
+    GIMBAL_MODE_PAN_LOCK  = (1<<2),
 } gimbal_htk_mode_e;
 
-#define GIMBAL_MODE_DEFAULT GIMBAL_MODE_FOLLOW
+#define GIMBAL_MODE_DEFAULT             GIMBAL_MODE_FOLLOW
+#define GIMBAL_MODE_TILT_ROLL_LOCK      (GIMBAL_MODE_TILT_LOCK | GIMBAL_MODE_ROLL_LOCK)
+#define GIMBAL_MODE_PAN_TILT_ROLL_LOCK  (GIMBAL_MODE_TILT_LOCK | GIMBAL_MODE_ROLL_LOCK | GIMBAL_MODE_PAN_LOCK)
 
 void gimbalCommonInit(void);
 void gimbalCommonSetDevice(gimbalDevice_t *gimbalDevice);
diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c
index 2915c0c50cb..15c0d7136ef 100644
--- a/src/main/fc/fc_msp_box.c
+++ b/src/main/fc/fc_msp_box.c
@@ -104,8 +104,8 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { .boxId = BOXMIXERPROFILE,     .boxName = "MIXER PROFILE 2",   .permanentId = 62 },
     { .boxId = BOXMIXERTRANSITION,  .boxName = "MIXER TRANSITION",  .permanentId = 63 },
     { .boxId = BOXANGLEHOLD,        .boxName = "ANGLE HOLD",        .permanentId = 64 },
-    { .boxId = BOXGIMBALPLOCK,      .boxName = "GIMBAL LEVEL PITCH",    .permanentId = 65 },
-    { .boxId = BOXGIMBALPRLOCK,     .boxName = "GIMBAL LEVEL PITCH ROLL",  .permanentId = 66 },
+    { .boxId = BOXGIMBALTLOCK,      .boxName = "GIMBAL LEVEL TILT", .permanentId = 65 },
+    { .boxId = BOXGIMBALRLOCK,      .boxName = "GIMBAL LEVEL ROLL", .permanentId = 66 },
     { .boxId = BOXGIMBALCENTER,     .boxName = "GIMBAL CENTER",     .permanentId = 67 },
     { .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL,                .permanentId = 0xFF }
 };
@@ -366,8 +366,8 @@ void initActiveBoxIds(void)
 
 #ifdef USE_SERIAL_GIMBAL
     if(gimbalCommonIsEnabled()) {
-        ADD_ACTIVE_BOX(BOXGIMBALPLOCK);
-        ADD_ACTIVE_BOX(BOXGIMBALPRLOCK);
+        ADD_ACTIVE_BOX(BOXGIMBALTLOCK);
+        ADD_ACTIVE_BOX(BOXGIMBALRLOCK);
         ADD_ACTIVE_BOX(BOXGIMBALCENTER);
     }
 #endif
@@ -445,8 +445,8 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
 #endif
     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXANGLEHOLD)),       BOXANGLEHOLD);
 
-    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALPLOCK)),     BOXGIMBALPLOCK);
-    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALPRLOCK)),    BOXGIMBALPRLOCK);
+    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALTLOCK)),     BOXGIMBALTLOCK);
+    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALRLOCK)),     BOXGIMBALRLOCK);
     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)),    BOXGIMBALCENTER);
 
     memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t));
diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h
index fb517efa009..69ef0374cb0 100644
--- a/src/main/fc/rc_modes.h
+++ b/src/main/fc/rc_modes.h
@@ -81,8 +81,8 @@ typedef enum {
     BOXMIXERPROFILE  = 53,
     BOXMIXERTRANSITION = 54,
     BOXANGLEHOLD     = 55,
-    BOXGIMBALPLOCK   = 56,
-    BOXGIMBALPRLOCK  = 57,
+    BOXGIMBALTLOCK   = 56,
+    BOXGIMBALRLOCK  = 57,
     BOXGIMBALCENTER  = 58,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index a499dffaaca..184a34385cd 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -135,12 +135,17 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
     int pitch = 1500;
     int roll = 1500;
 
+    if (IS_RC_MODE_ACTIVE(BOXGIMBALTLOCK)) {
+        attittude.mode |= GIMBAL_MODE_TILT_LOCK;
+    }
+
+    if (IS_RC_MODE_ACTIVE(BOXGIMBALRLOCK)) {
+        attittude.mode |= GIMBAL_MODE_ROLL_LOCK;
+    }
+
+    // Follow center overrides all
     if (IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)) {
         attittude.mode = GIMBAL_MODE_FOLLOW;
-    } else if (IS_RC_MODE_ACTIVE(BOXGIMBALPLOCK)) {
-        attittude.mode = GIMBAL_MODE_PITCH_LOCK;
-    } else if (IS_RC_MODE_ACTIVE(BOXGIMBALPRLOCK)) {
-        attittude.mode = GIMBAL_MODE_PITCH_ROLL_LOCK;
     }
 
     if (rxAreFlightChannelsValid() && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)) {
@@ -148,10 +153,10 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
             yaw = rxGetChannelValue(cfg->panChannel - 1);
             // const rxChannelRangeConfig_t *channelRanges =
             // rxChannelRangeConfigs(cfg->pitchChannel - 1);
-            if (yaw < 1000) {
-                yaw = 1000;
-            } else if (yaw > 2000) {
-                yaw = 2000;
+            if (yaw < 1050) {
+                yaw = 1050;
+            } else if (yaw > 1950) {
+                yaw = 1950;
             }
         }
 

From 0253e86133d351d1fa3d63ef25a9d30612cb997f Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sat, 8 Jun 2024 17:33:30 +0200
Subject: [PATCH 195/323] Only activate TILT lock and ROLL lock if CENTER is
 not active

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

diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c
index 15c0d7136ef..f118b2a4daa 100644
--- a/src/main/fc/fc_msp_box.c
+++ b/src/main/fc/fc_msp_box.c
@@ -445,8 +445,8 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
 #endif
     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXANGLEHOLD)),       BOXANGLEHOLD);
 
-    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALTLOCK)),     BOXGIMBALTLOCK);
-    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALRLOCK)),     BOXGIMBALRLOCK);
+    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALTLOCK) && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)),     BOXGIMBALTLOCK);
+    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALRLOCK) && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)),     BOXGIMBALRLOCK);
     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)),    BOXGIMBALCENTER);
 
     memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t));

From ca494d1d72f74d3e0589ee4ebdd0a3968ff8e303 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sat, 8 Jun 2024 18:20:23 +0200
Subject: [PATCH 196/323] Cleanup

---
 src/main/io/gimbal_serial.c | 22 ++++++++--------------
 1 file changed, 8 insertions(+), 14 deletions(-)

diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index 184a34385cd..d14fffce1c4 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -151,8 +151,6 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
     if (rxAreFlightChannelsValid() && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)) {
         if (cfg->panChannel > 0) {
             yaw = rxGetChannelValue(cfg->panChannel - 1);
-            // const rxChannelRangeConfig_t *channelRanges =
-            // rxChannelRangeConfigs(cfg->pitchChannel - 1);
             if (yaw < 1050) {
                 yaw = 1050;
             } else if (yaw > 1950) {
@@ -162,23 +160,19 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
 
         if (cfg->tiltChannel > 0) {
             pitch = rxGetChannelValue(cfg->tiltChannel - 1);
-            // const rxChannelRangeConfig_t *channelRanges =
-            // rxChannelRangeConfigs(cfg->pitchChannel - 1);
-            if (pitch < 1000) {
-                pitch = 1000;
-            } else if (pitch > 2000) {
-                pitch = 2000;
+            if (pitch < 1050) {
+                pitch = 1050;
+            } else if (pitch > 1950) {
+                pitch = 1950;
             }
         }
 
         if (cfg->rollChannel > 0) {
             roll = rxGetChannelValue(cfg->rollChannel - 1);
-            // const rxChannelRangeConfig_t *channelRanges =
-            // rxChannelRangeConfigs(cfg->pitchChannel - 1);
-            if (roll < 1000) {
-                roll = 1000;
-            } else if (roll > 2000) {
-                roll = 2000;
+            if (roll < 1050) {
+                roll = 1050;
+            } else if (roll > 1950) {
+                roll = 1950;
             }
         }
     }

From 0f2eedaf7d30ea3fd8a3c18f0f02d06471cd3ac2 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sat, 8 Jun 2024 19:06:33 +0200
Subject: [PATCH 197/323] Use constrain instead of duplicating this code

---
 src/main/io/gimbal_serial.c | 18 +++---------------
 1 file changed, 3 insertions(+), 15 deletions(-)

diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index d14fffce1c4..364ed922fd6 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -151,29 +151,17 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
     if (rxAreFlightChannelsValid() && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)) {
         if (cfg->panChannel > 0) {
             yaw = rxGetChannelValue(cfg->panChannel - 1);
-            if (yaw < 1050) {
-                yaw = 1050;
-            } else if (yaw > 1950) {
-                yaw = 1950;
-            }
+            yaw = constrain(yaw, 1000, 2000);
         }
 
         if (cfg->tiltChannel > 0) {
             pitch = rxGetChannelValue(cfg->tiltChannel - 1);
-            if (pitch < 1050) {
-                pitch = 1050;
-            } else if (pitch > 1950) {
-                pitch = 1950;
-            }
+            pitch = constrain(pitch, 1000, 2000);
         }
 
         if (cfg->rollChannel > 0) {
             roll = rxGetChannelValue(cfg->rollChannel - 1);
-            if (roll < 1050) {
-                roll = 1050;
-            } else if (roll > 1950) {
-                roll = 1950;
-            }
+            roll = constrain(roll, 1000, 2000);
         }
     }
 

From 504dd5b3c65cd46642898ad5bc593a8deedf1a64 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sat, 8 Jun 2024 22:34:13 +0200
Subject: [PATCH 198/323] First steps to allow reading headtracker info from a
 side channel

---
 src/main/drivers/gimbal_common.c | 11 ++++++
 src/main/drivers/gimbal_common.h |  2 ++
 src/main/fc/fc_msp_box.c         | 18 +++++++---
 src/main/fc/rc_modes.h           |  3 +-
 src/main/io/gimbal_serial.c      | 58 +++++++++++++++++++++++---------
 src/main/io/gimbal_serial.h      |  1 +
 src/main/io/serial.h             |  1 +
 7 files changed, 73 insertions(+), 21 deletions(-)

diff --git a/src/main/drivers/gimbal_common.c b/src/main/drivers/gimbal_common.c
index 3c93b44d793..9fcfcc2b9bc 100644
--- a/src/main/drivers/gimbal_common.c
+++ b/src/main/drivers/gimbal_common.c
@@ -99,4 +99,15 @@ bool gimbalCommonIsEnabled(void)
     return true;
 }
 
+
+bool gimbalCommonHtrkIsEnabled(void)
+{
+    const gimbalDevice_t *dev = gimbalCommonDevice();
+    if(dev && dev->vTable->hasHeadTracker) {
+        return dev->vTable->hasHeadTracker(dev);
+    }
+
+    return false;
+}
+
 #endif
\ No newline at end of file
diff --git a/src/main/drivers/gimbal_common.h b/src/main/drivers/gimbal_common.h
index 0181c2cb2c0..2d914bbf77a 100644
--- a/src/main/drivers/gimbal_common.h
+++ b/src/main/drivers/gimbal_common.h
@@ -51,6 +51,7 @@ typedef struct gimbalVTable_s {
     void (*process)(gimbalDevice_t *gimbalDevice, timeUs_t currentTimeUs);
     gimbalDevType_e (*getDeviceType)(const gimbalDevice_t *gimbalDevice);
     bool (*isReady)(const gimbalDevice_t *gimbalDevice);
+    bool (*hasHeadTracker)(const gimbalDevice_t *gimbalDevice);
 } gimbalVTable_t;
 
 
@@ -87,6 +88,7 @@ bool gimbalCommonIsReady(gimbalDevice_t *gimbalDevice);
 void taskUpdateGimbal(timeUs_t currentTimeUs);
 
 bool gimbalCommonIsEnabled(void);
+bool gimbalCommonHtrkIsEnabled(void);
 
 #ifdef __cplusplus
 }
diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c
index f118b2a4daa..d295bb90e25 100644
--- a/src/main/fc/fc_msp_box.c
+++ b/src/main/fc/fc_msp_box.c
@@ -107,6 +107,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { .boxId = BOXGIMBALTLOCK,      .boxName = "GIMBAL LEVEL TILT", .permanentId = 65 },
     { .boxId = BOXGIMBALRLOCK,      .boxName = "GIMBAL LEVEL ROLL", .permanentId = 66 },
     { .boxId = BOXGIMBALCENTER,     .boxName = "GIMBAL CENTER",     .permanentId = 67 },
+    { .boxId = BOXGIMBALHTRK,       .boxName = "GIMBAL HEADTRACKER", .permanentId = 68 },
     { .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL,                .permanentId = 0xFF }
 };
 
@@ -365,10 +366,13 @@ void initActiveBoxIds(void)
 #endif
 
 #ifdef USE_SERIAL_GIMBAL
-    if(gimbalCommonIsEnabled()) {
+    if (gimbalCommonIsEnabled()) {
         ADD_ACTIVE_BOX(BOXGIMBALTLOCK);
         ADD_ACTIVE_BOX(BOXGIMBALRLOCK);
         ADD_ACTIVE_BOX(BOXGIMBALCENTER);
+        if (gimbalCommonHtrkIsEnabled()) {
+            ADD_ACTIVE_BOX(BOXGIMBALHTRK);
+        }
     }
 #endif
 }
@@ -445,9 +449,15 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
 #endif
     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXANGLEHOLD)),       BOXANGLEHOLD);
 
-    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALTLOCK) && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)),     BOXGIMBALTLOCK);
-    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALRLOCK) && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)),     BOXGIMBALRLOCK);
-    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)),    BOXGIMBALCENTER);
+    if(IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)) {
+        CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)), BOXGIMBALCENTER);
+    } else if (IS_RC_MODE_ACTIVE(BOXGIMBALHTRK)) {
+        CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALHTRK)), BOXGIMBALHTRK);
+    } else {
+        CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALTLOCK) && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)),     BOXGIMBALTLOCK);
+        CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALRLOCK) && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)),     BOXGIMBALRLOCK);
+        CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALHTRK) && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)),     BOXGIMBALRLOCK);
+    }
 
     memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t));
     for (uint32_t i = 0; i < activeBoxIdCount; i++) {
diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h
index 69ef0374cb0..c4b21a9005b 100644
--- a/src/main/fc/rc_modes.h
+++ b/src/main/fc/rc_modes.h
@@ -82,8 +82,9 @@ typedef enum {
     BOXMIXERTRANSITION = 54,
     BOXANGLEHOLD     = 55,
     BOXGIMBALTLOCK   = 56,
-    BOXGIMBALRLOCK  = 57,
+    BOXGIMBALRLOCK   = 57,
     BOXGIMBALCENTER  = 58,
+    BOXGIMBALHTRK    = 59,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 
diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index 364ed922fd6..a77bf2b54a3 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -42,12 +42,14 @@ STATIC_ASSERT(sizeof(gimbalHtkAttitudePkt_t) == 10, gimbalHtkAttitudePkt_t_size_
 #define GIMBAL_SERIAL_BUFFER_SIZE 512
 static volatile uint8_t txBuffer[GIMBAL_SERIAL_BUFFER_SIZE];
 
-static serialPort_t *htkPort = NULL;
+static serialPort_t *headTrackerPort = NULL;
+static serialPort_t *gimbalPort = NULL;
 
 gimbalVTable_t gimbalSerialVTable = {
     .process = gimbalSerialProcess,
     .getDeviceType = gimbalSerialGetDeviceType,
-    .isReady = gimbalSerialIsReady
+    .isReady = gimbalSerialIsReady,
+    .hasHeadTracker = gimbalSerialHasHeadTracker,
 
 };
 
@@ -64,7 +66,12 @@ gimbalDevType_e gimbalSerialGetDeviceType(const gimbalDevice_t *gimbalDevice)
 
 bool gimbalSerialIsReady(const gimbalDevice_t *gimbalDevice)
 {
-    return htkPort != NULL && gimbalDevice->vTable != NULL;
+    return gimbalPort != NULL && gimbalDevice->vTable != NULL;
+}
+
+bool gimbalSerialHasHeadTracker(const gimbalDevice_t *gimbalDevice)
+{
+    return headTrackerPort;
 }
 
 bool gimbalSerialInit(void)
@@ -85,28 +92,47 @@ bool gimbalSerialDetect(void)
 #else
 bool gimbalSerialDetect(void)
 {
-
     SD(fprintf(stderr, "[GIMBAL]: serial Detect...\n"));
     serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_GIMBAL);
 
     if (portConfig) {
         SD(fprintf(stderr, "[GIMBAL]: found port...\n"));
-        htkPort = openSerialPort(portConfig->identifier, FUNCTION_GIMBAL, NULL, NULL,
+        gimbalPort = openSerialPort(portConfig->identifier, FUNCTION_GIMBAL, NULL, NULL,
                 baudRates[portConfig->peripheral_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
 
-        if (htkPort) {
+        if (gimbalPort) {
             SD(fprintf(stderr, "[GIMBAL]: port open!\n"));
-            htkPort->txBuffer = txBuffer;
-            htkPort->txBufferSize = GIMBAL_SERIAL_BUFFER_SIZE;
-            htkPort->txBufferTail = 0;
-            htkPort->txBufferHead = 0;
+            gimbalPort->txBuffer = txBuffer;
+            gimbalPort->txBufferSize = GIMBAL_SERIAL_BUFFER_SIZE;
+            gimbalPort->txBufferTail = 0;
+            gimbalPort->txBufferHead = 0;
+        } else {
+            SD(fprintf(stderr, "[GIMBAL]: port NOT open!\n"));
+            return false;
+        }
+    }
 
-            return true;
+    SD(fprintf(stderr, "[GIMBAL_HTRK]: headtracker Detect...\n"));
+    portConfig = findSerialPortConfig(FUNCTION_GIMBAL_HEADTRACKER);
+
+    if (portConfig) {
+        SD(fprintf(stderr, "[GIMBAL_HTRK]: found port...\n"));
+        headTrackerPort = openSerialPort(portConfig->identifier, FUNCTION_GIMBAL_HEADTRACKER, NULL, NULL,
+                baudRates[portConfig->peripheral_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
+
+        if (headTrackerPort) {
+            SD(fprintf(stderr, "[GIMBAL_HTRK]: port open!\n"));
+            headTrackerPort->txBuffer = txBuffer;
+            headTrackerPort->txBufferSize = GIMBAL_SERIAL_BUFFER_SIZE;
+            headTrackerPort->txBufferTail = 0;
+            headTrackerPort->txBufferHead = 0;
+        } else {
+            SD(fprintf(stderr, "[GIMBAL_HTRK]: port NOT open!\n"));
+            return false;
         }
     }
 
-    SD(fprintf(stderr, "[GIMBAL]: port not found :(...\n"));
-    return false;
+    return gimbalPort || headTrackerPort;
 }
 #endif
 
@@ -179,9 +205,9 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
     attittude.crch = (crc16 >> 8) & 0xFF;
     attittude.crcl = crc16 & 0xFF;
 
-    serialBeginWrite(htkPort);
-    serialWriteBuf(htkPort, (uint8_t *)&attittude, sizeof(gimbalHtkAttitudePkt_t));
-    serialEndWrite(htkPort);
+    serialBeginWrite(gimbalPort);
+    serialWriteBuf(gimbalPort, (uint8_t *)&attittude, sizeof(gimbalHtkAttitudePkt_t));
+    serialEndWrite(gimbalPort);
 }
 #endif
 
diff --git a/src/main/io/gimbal_serial.h b/src/main/io/gimbal_serial.h
index 3d0188b6b1c..5173531a6fc 100644
--- a/src/main/io/gimbal_serial.h
+++ b/src/main/io/gimbal_serial.h
@@ -52,6 +52,7 @@ bool gimbalSerialDetect(void);
 void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime);
 bool gimbalSerialIsReady(const gimbalDevice_t *gimbalDevice);
 gimbalDevType_e gimbalSerialGetDeviceType(const gimbalDevice_t *gimbalDevice);
+bool gimbalSerialHasHeadTracker(const gimbalDevice_t *gimbalDevice);
 
 #endif
 
diff --git a/src/main/io/serial.h b/src/main/io/serial.h
index c2429d9887a..aa9d709472e 100644
--- a/src/main/io/serial.h
+++ b/src/main/io/serial.h
@@ -58,6 +58,7 @@ typedef enum {
     FUNCTION_UNUSED_2                   = (1 << 24), // 16777216
     FUNCTION_MSP_OSD                    = (1 << 25), // 33554432
     FUNCTION_GIMBAL                     = (1 << 26), // 67108864
+    FUNCTION_GIMBAL_HEADTRACKER         = (1 << 27), // 134217728
 } serialPortFunction_e;
 
 #define FUNCTION_VTX_MSP FUNCTION_MSP_OSD

From 1be5b7457b9862085110c598ba6d1fca0a678c01 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sat, 8 Jun 2024 22:37:55 +0200
Subject: [PATCH 199/323] Fix warning about unused variable

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

diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index a77bf2b54a3..dd9166d07c7 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -71,6 +71,7 @@ bool gimbalSerialIsReady(const gimbalDevice_t *gimbalDevice)
 
 bool gimbalSerialHasHeadTracker(const gimbalDevice_t *gimbalDevice)
 {
+    UNUSED(gimbalDevice);
     return headTrackerPort;
 }
 

From 9ad3f2408aebf8bc6c84a0027ea7a29006b7c3ff Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sat, 8 Jun 2024 23:47:46 +0200
Subject: [PATCH 200/323] Parse incomming headtracking info from sidechannel

---
 src/main/io/gimbal_serial.c | 95 ++++++++++++++++++++++++++++++++-----
 src/main/io/gimbal_serial.h | 42 ++++++++++++----
 2 files changed, 117 insertions(+), 20 deletions(-)

diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index dd9166d07c7..4cbde173c36 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -30,6 +30,7 @@
 
 #include <drivers/gimbal_common.h>
 #include <drivers/serial.h>
+#include <drivers/time.h>
 
 #include <io/gimbal_serial.h>
 #include <io/serial.h>
@@ -53,9 +54,15 @@ gimbalVTable_t gimbalSerialVTable = {
 
 };
 
-gimbalDevice_t serialGimbalDevice = {
-    .vTable = &gimbalSerialVTable
+static gimbalSerialHtrkState_t headTrackerState = { 
+    .lastUpdate = 0,
+    .payloadSize = 0,
+    .state = WAITING_HDR1,
+};
+
 
+static gimbalDevice_t serialGimbalDevice = {
+    .vTable = &gimbalSerialVTable
 };
 
 gimbalDevType_e gimbalSerialGetDeviceType(const gimbalDevice_t *gimbalDevice)
@@ -118,7 +125,7 @@ bool gimbalSerialDetect(void)
 
     if (portConfig) {
         SD(fprintf(stderr, "[GIMBAL_HTRK]: found port...\n"));
-        headTrackerPort = openSerialPort(portConfig->identifier, FUNCTION_GIMBAL_HEADTRACKER, NULL, NULL,
+        headTrackerPort = openSerialPort(portConfig->identifier, FUNCTION_GIMBAL_HEADTRACKER, gimbalSerialHeadTrackerReceive, &headTrackerState,
                 baudRates[portConfig->peripheral_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
 
         if (headTrackerPort) {
@@ -158,8 +165,8 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
 
     const gimbalConfig_t *cfg = gimbalConfig();
 
-    int yaw = 1500;
-    int pitch = 1500;
+    int pan = 1500;
+    int tilt = 1500;
     int roll = 1500;
 
     if (IS_RC_MODE_ACTIVE(BOXGIMBALTLOCK)) {
@@ -177,13 +184,13 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
 
     if (rxAreFlightChannelsValid() && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)) {
         if (cfg->panChannel > 0) {
-            yaw = rxGetChannelValue(cfg->panChannel - 1);
-            yaw = constrain(yaw, 1000, 2000);
+            pan = rxGetChannelValue(cfg->panChannel - 1);
+            pan = constrain(pan, 1000, 2000);
         }
 
         if (cfg->tiltChannel > 0) {
-            pitch = rxGetChannelValue(cfg->tiltChannel - 1);
-            pitch = constrain(pitch, 1000, 2000);
+            tilt = rxGetChannelValue(cfg->tiltChannel - 1);
+            tilt = constrain(tilt, 1000, 2000);
         }
 
         if (cfg->rollChannel > 0) {
@@ -194,8 +201,10 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
 
     attittude.sensibility = cfg->sensitivity; //gimbal_scale5(-16, 15, -16, 15, cfg->sensitivity);
 
-    attittude.yaw = gimbal_scale12(1000, 2000, yaw);
-    attittude.pitch = gimbal_scale12(1000, 2000, pitch);
+    // Radio endpoints may need to be adjusted, as it seems ot go a bit bananas
+    // at the extremes
+    attittude.pan = gimbal_scale12(1000, 2000, pan);
+    attittude.tilt = gimbal_scale12(1000, 2000, tilt);
     attittude.roll = gimbal_scale12(1000, 2000, roll);
 
     uint16_t crc16 = 0;
@@ -219,4 +228,68 @@ int16_t gimbal_scale12(int16_t inputMin, int16_t inputMax, int16_t value)
     return ret;
 }
 
+static void resetState(gimbalSerialHtrkState_t *state)
+{
+    state->state = WAITING_HDR1;
+    state->payloadSize = 0;
+}
+
+static bool checkCrc(gimbalHtkAttitudePkt_t *attitude)
+{
+    uint8_t *attitudePkt = (uint8_t *)attitude;
+    uint16_t crc = 0;
+
+    for(uint8_t i = 0; i < sizeof(gimbalHtkAttitudePkt_t) - 2; ++i) {
+        crc = crc16_ccitt(crc, attitudePkt[i]);
+    }
+
+    return (attitude->crch == ((crc >> 8) & 0xFF)) &&
+           (attitude->crcl == (crc & 0xFF));
+}
+
+
+void gimbalSerialHeadTrackerReceive(uint16_t c, void *data)
+{
+    gimbalSerialHtrkState_t *state = (gimbalSerialHtrkState_t *)data;
+    uint8_t *payload = (uint8_t *)&(state->attittude);
+    payload += 2;
+
+    switch(state->state) {
+        case WAITING_HDR1:
+            if(c == HTKATTITUDE_SYNC0) {
+                state->attittude.sync[0] = c;
+                state->state = WAITING_HDR2;
+            }
+            break;
+        case WAITING_HDR2:
+            if(c == HTKATTITUDE_SYNC1) {
+                state->attittude.sync[1] = c;
+                state->state = WAITING_PAYLOAD;
+            } else {
+                resetState(state);
+            }
+            break;
+        case WAITING_PAYLOAD:
+            payload[state->payloadSize++] = c;
+            if(state->payloadSize == HEADTRACKER_PAYLOAD_SIZE)
+            {
+                state->state = WAITING_CRCH;
+            }
+            break;
+        case WAITING_CRCH:
+            state->attittude.crch = c;
+            break;
+        case WAITING_CRCL:
+            state->attittude.crcl = c;
+            if(checkCrc(&(state->attittude))) {
+                state->lastUpdate = micros();
+                state->pan = state->attittude.pan;
+                state->tilt = state->attittude.tilt;
+                state->roll = state->attittude.roll;
+            }
+            resetState(state);
+            break;
+    }
+}
+
 #endif
\ No newline at end of file
diff --git a/src/main/io/gimbal_serial.h b/src/main/io/gimbal_serial.h
index 5173531a6fc..3677551e88e 100644
--- a/src/main/io/gimbal_serial.h
+++ b/src/main/io/gimbal_serial.h
@@ -34,17 +34,40 @@ extern "C" {
 #define HTKATTITUDE_SYNC1  0x5A
 typedef struct gimbalHtkAttitudePkt_s
 {
-	uint8_t  sync[2];   //data synchronization 0xA5, 0x5A
-	uint64_t mode:3;     //Gimbal Mode [0~7] [Only 0 1 2 modes are supported for the time being]
-	int64_t  sensibility:5;     //Cloud sensibility [-16~15]
-	uint64_t reserved:4;     //hold on to one's reserve
-	int64_t  roll:12;    //Roll angle [-2048~2047] => [-180~180]
-	int64_t  pitch:12;    //Pich angle [-2048~2047] => [-180~180]
-	int64_t  yaw:12;    //Yaw angle [-2048~2047] => [-180~180]
-	uint64_t crch:8;    //Data validation H
-	uint64_t crcl:8;    //Data validation L
+	uint8_t  sync[2];       //data synchronization 0xA5, 0x5A
+	uint64_t mode:3;        //Gimbal Mode [0~7] [Only 0 1 2 modes are supported for the time being]
+	int64_t  sensibility:5; // Stabilization sensibility [-16~15]
+	uint64_t reserved:4;    //hold on to one's reserve
+	int64_t  roll:12;       //Roll angle [-2048~2047] => [-180~180]
+	int64_t  tilt:12;       //Pich angle [-2048~2047] => [-180~180]
+	int64_t  pan:12;        //Yaw angle [-2048~2047] => [-180~180]
+	uint8_t  crch;          //Data validation H
+	uint8_t  crcl;          //Data validation L
 } __attribute__((packed)) gimbalHtkAttitudePkt_t;
 
+
+#define HEADTRACKER_PAYLOAD_SIZE (sizeof(gimbalHtkAttitudePkt_t) - 4)
+
+typedef enum {
+    WAITING_HDR1,
+    WAITING_HDR2,
+    WAITING_PAYLOAD,
+    WAITING_CRCH,
+    WAITING_CRCL,
+} gimbalHeadtrackerState_e;
+
+typedef struct gimbalSerialHtrkState_s {
+    timeUs_t lastUpdate;
+    uint8_t  payloadSize;
+	int16_t  roll;   //Roll angle [-2048~2047] => [-180~180]
+	int16_t  tilt;   //Pich angle [-2048~2047] => [-180~180]
+	int16_t  pan;    //Yaw angle [-2048~2047] => [-180~180]
+    gimbalHeadtrackerState_e state;
+    gimbalHtkAttitudePkt_t attittude;
+} gimbalSerialHtrkState_t;
+
+
+
 int16_t gimbal_scale12(int16_t inputMin, int16_t inputMax, int16_t value);
 
 bool gimbalSerialInit(void);
@@ -53,6 +76,7 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime);
 bool gimbalSerialIsReady(const gimbalDevice_t *gimbalDevice);
 gimbalDevType_e gimbalSerialGetDeviceType(const gimbalDevice_t *gimbalDevice);
 bool gimbalSerialHasHeadTracker(const gimbalDevice_t *gimbalDevice);
+void gimbalSerialHeadTrackerReceive(uint16_t c, void *data);
 
 #endif
 

From 03f1a8b700704330980d62e1b0a88716fd31eb0f Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sun, 9 Jun 2024 00:07:09 +0200
Subject: [PATCH 201/323] Plumbing for head tracking in place.

1 UART will control gimbal
1 UART will read headtracking info from external device

INAV will control gimbal and will either forward headtracking attitude info
or control gimbal directly.

Sensitivity from headtracker unit is currently ignored.
---
 src/main/io/gimbal_serial.c | 16 ++++++++++++++--
 1 file changed, 14 insertions(+), 2 deletions(-)

diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index 4cbde173c36..6e22cf710e6 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -178,10 +178,10 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
     }
 
     // Follow center overrides all
-    if (IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)) {
+    if (IS_RC_MODE_ACTIVE(BOXGIMBALCENTER) || IS_RC_MODE_ACTIVE(BOXGIMBALHTRK)) {
         attittude.mode = GIMBAL_MODE_FOLLOW;
     }
-
+    
     if (rxAreFlightChannelsValid() && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)) {
         if (cfg->panChannel > 0) {
             pan = rxGetChannelValue(cfg->panChannel - 1);
@@ -199,6 +199,18 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
         }
     }
 
+    if(IS_RC_MODE_ACTIVE(BOXGIMBALHTRK)) {
+        if (gimbalCommonHtrkIsEnabled() && (micros() - headTrackerState.lastUpdate) < MAX_INVALID_RX_PULSE_TIME) {
+            tilt = headTrackerState.tilt;
+            pan = headTrackerState.pan;
+            roll = headTrackerState.roll;
+        } else {
+            tilt = 0;
+            pan = 0;
+            roll = 0;
+        }
+    }
+
     attittude.sensibility = cfg->sensitivity; //gimbal_scale5(-16, 15, -16, 15, cfg->sensitivity);
 
     // Radio endpoints may need to be adjusted, as it seems ot go a bit bananas

From 99a351847ebb22abf45ddaa20abb6ed7bf8cfb88 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sun, 9 Jun 2024 00:40:22 +0200
Subject: [PATCH 202/323] Fix unit test build

---
 src/main/io/gimbal_serial.c | 20 +++++++++++++-------
 1 file changed, 13 insertions(+), 7 deletions(-)

diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index 6e22cf710e6..9bdee4e8705 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -41,8 +41,17 @@
 STATIC_ASSERT(sizeof(gimbalHtkAttitudePkt_t) == 10, gimbalHtkAttitudePkt_t_size_not_10);
 
 #define GIMBAL_SERIAL_BUFFER_SIZE 512
+
+#ifndef GIMBAL_UNIT_TEST
 static volatile uint8_t txBuffer[GIMBAL_SERIAL_BUFFER_SIZE];
 
+static gimbalSerialHtrkState_t headTrackerState = { 
+    .lastUpdate = 0,
+    .payloadSize = 0,
+    .state = WAITING_HDR1,
+};
+#endif
+
 static serialPort_t *headTrackerPort = NULL;
 static serialPort_t *gimbalPort = NULL;
 
@@ -54,13 +63,6 @@ gimbalVTable_t gimbalSerialVTable = {
 
 };
 
-static gimbalSerialHtrkState_t headTrackerState = { 
-    .lastUpdate = 0,
-    .payloadSize = 0,
-    .state = WAITING_HDR1,
-};
-
-
 static gimbalDevice_t serialGimbalDevice = {
     .vTable = &gimbalSerialVTable
 };
@@ -147,6 +149,8 @@ bool gimbalSerialDetect(void)
 #ifdef GIMBAL_UNIT_TEST
 void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
 {
+    UNUSED(gimbalDevice);
+    UNUSED(currentTime);
 }
 #else
 void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
@@ -240,6 +244,7 @@ int16_t gimbal_scale12(int16_t inputMin, int16_t inputMax, int16_t value)
     return ret;
 }
 
+#ifndef GIMBAL_UNIT_TEST
 static void resetState(gimbalSerialHtrkState_t *state)
 {
     state->state = WAITING_HDR1;
@@ -303,5 +308,6 @@ void gimbalSerialHeadTrackerReceive(uint16_t c, void *data)
             break;
     }
 }
+#endif
 
 #endif
\ No newline at end of file

From e30ab94c8503acc1d560581c30f5aae7c2a66e0e Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sun, 9 Jun 2024 11:23:21 +0200
Subject: [PATCH 203/323] Add uart sharing.

FC rx goes to headtracking source.
FC tx goes to gymbal.
---
 docs/Settings.md                      | 10 ++++++++++
 src/main/config/parameter_group_ids.h |  3 ++-
 src/main/fc/settings.yaml             | 11 ++++++++++-
 src/main/io/gimbal_serial.c           |  9 +++++++--
 src/main/io/gimbal_serial.h           |  5 +++++
 5 files changed, 34 insertions(+), 4 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index e328ba120cd..81e83a2ae04 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -1492,6 +1492,16 @@ Gimbal sensitivity is similar to gain and will affect how quickly the gimbal wil
 
 ---
 
+### gimbal_serial_single_uart
+
+Gimbal serial and headtracker device share same UART. FC RX goes to headtracker device, FC TX goes to gimbal.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| OFF | OFF | ON |
+
+---
+
 ### gimbal_tilt_channel
 
 Gimbal tilt rc channel index. 0 is no channel.
diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h
index 90085fae638..067d97bf0a1 100644
--- a/src/main/config/parameter_group_ids.h
+++ b/src/main/config/parameter_group_ids.h
@@ -127,7 +127,8 @@
 #define PG_FW_AUTOLAND_APPROACH_CONFIG 1037
 #define PG_OSD_CUSTOM_ELEMENTS_CONFIG 1038
 #define PG_GIMBAL_CONFIG 1039
-#define PG_INAV_END PG_GIMBAL_CONFIG
+#define PG_GIMBAL_SERIAL_CONFIG 1040
+#define PG_INAV_END PG_GIMBAL_SERIAL_CONFIG
 
 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index c9525703e88..5b6a9bb34f4 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -4226,4 +4226,13 @@ groups:
         default_value: 0
         field: sensitivity
         min: -16
-        max: 15
\ No newline at end of file
+        max: 15
+  - name: PG_GIMBAL_SERIAL_CONFIG
+    type: gimbalSerialConfig_t
+    headers: ["io/gimbal_serial.h"]
+    condition: USE_SERIAL_GIMBAL
+    members:
+      - name: gimbal_serial_single_uart
+        description: "Gimbal serial and headtracker device share same UART. FC RX goes to headtracker device, FC TX goes to gimbal."
+        default_value: false
+        field: singleUart
\ No newline at end of file
diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index 9bdee4e8705..49a1f69f543 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -38,6 +38,10 @@
 #include <rx/rx.h>
 #include <fc/rc_modes.h>
 
+#include <config/parameter_group_ids.h>
+
+PG_REGISTER(gimbalSerialConfig_t, gimbalSerialConfig, PG_GIMBAL_SERIAL_CONFIG, 0);
+
 STATIC_ASSERT(sizeof(gimbalHtkAttitudePkt_t) == 10, gimbalHtkAttitudePkt_t_size_not_10);
 
 #define GIMBAL_SERIAL_BUFFER_SIZE 512
@@ -104,10 +108,11 @@ bool gimbalSerialDetect(void)
 {
     SD(fprintf(stderr, "[GIMBAL]: serial Detect...\n"));
     serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_GIMBAL);
+    bool singleUart = gimbalSerialConfig()->singleUart;
 
     if (portConfig) {
         SD(fprintf(stderr, "[GIMBAL]: found port...\n"));
-        gimbalPort = openSerialPort(portConfig->identifier, FUNCTION_GIMBAL, NULL, NULL,
+        gimbalPort = openSerialPort(portConfig->identifier, FUNCTION_GIMBAL, singleUart ? gimbalSerialHeadTrackerReceive : NULL, singleUart ? &headTrackerState :  NULL,
                 baudRates[portConfig->peripheral_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
 
         if (gimbalPort) {
@@ -123,7 +128,7 @@ bool gimbalSerialDetect(void)
     }
 
     SD(fprintf(stderr, "[GIMBAL_HTRK]: headtracker Detect...\n"));
-    portConfig = findSerialPortConfig(FUNCTION_GIMBAL_HEADTRACKER);
+    portConfig = singleUart ? NULL : findSerialPortConfig(FUNCTION_GIMBAL_HEADTRACKER);
 
     if (portConfig) {
         SD(fprintf(stderr, "[GIMBAL_HTRK]: found port...\n"));
diff --git a/src/main/io/gimbal_serial.h b/src/main/io/gimbal_serial.h
index 3677551e88e..a7136b0e24c 100644
--- a/src/main/io/gimbal_serial.h
+++ b/src/main/io/gimbal_serial.h
@@ -66,6 +66,11 @@ typedef struct gimbalSerialHtrkState_s {
     gimbalHtkAttitudePkt_t attittude;
 } gimbalSerialHtrkState_t;
 
+typedef struct gimbalSerialConfig_s {
+    bool singleUart;
+} gimbalSerialConfig_t;
+
+PG_DECLARE(gimbalSerialConfig_t, gimbalSerialConfig);
 
 
 int16_t gimbal_scale12(int16_t inputMin, int16_t inputMax, int16_t value);

From 327d173c46f51cd3ed387b38b214de98bdaec88b Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sun, 9 Jun 2024 17:14:24 +0200
Subject: [PATCH 204/323] fix headtracker center position

---
 src/main/fc/fc_msp_box.c    |  4 +--
 src/main/io/gimbal_serial.c | 68 ++++++++++++++++++-------------------
 src/main/io/gimbal_serial.h |  4 +--
 3 files changed, 37 insertions(+), 39 deletions(-)

diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c
index d295bb90e25..44a3ada88e3 100644
--- a/src/main/fc/fc_msp_box.c
+++ b/src/main/fc/fc_msp_box.c
@@ -370,9 +370,7 @@ void initActiveBoxIds(void)
         ADD_ACTIVE_BOX(BOXGIMBALTLOCK);
         ADD_ACTIVE_BOX(BOXGIMBALRLOCK);
         ADD_ACTIVE_BOX(BOXGIMBALCENTER);
-        if (gimbalCommonHtrkIsEnabled()) {
-            ADD_ACTIVE_BOX(BOXGIMBALHTRK);
-        }
+        ADD_ACTIVE_BOX(BOXGIMBALHTRK);
     }
 #endif
 }
diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index 49a1f69f543..09c6c857a96 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -50,7 +50,7 @@ STATIC_ASSERT(sizeof(gimbalHtkAttitudePkt_t) == 10, gimbalHtkAttitudePkt_t_size_
 static volatile uint8_t txBuffer[GIMBAL_SERIAL_BUFFER_SIZE];
 
 static gimbalSerialHtrkState_t headTrackerState = { 
-    .lastUpdate = 0,
+    .expires = 0,
     .payloadSize = 0,
     .state = WAITING_HDR1,
 };
@@ -85,7 +85,7 @@ bool gimbalSerialIsReady(const gimbalDevice_t *gimbalDevice)
 bool gimbalSerialHasHeadTracker(const gimbalDevice_t *gimbalDevice)
 {
     UNUSED(gimbalDevice);
-    return headTrackerPort;
+    return headTrackerPort || (gimbalSerialConfig()->singleUart && gimbalPort);
 }
 
 bool gimbalSerialInit(void)
@@ -167,7 +167,7 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
         return;
     }
 
-    gimbalHtkAttitudePkt_t attittude = {
+    gimbalHtkAttitudePkt_t attitude = {
         .sync = {HTKATTITUDE_SYNC0, HTKATTITUDE_SYNC1},
         .mode = GIMBAL_MODE_DEFAULT
     };
@@ -179,16 +179,16 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
     int roll = 1500;
 
     if (IS_RC_MODE_ACTIVE(BOXGIMBALTLOCK)) {
-        attittude.mode |= GIMBAL_MODE_TILT_LOCK;
+        attitude.mode |= GIMBAL_MODE_TILT_LOCK;
     }
 
     if (IS_RC_MODE_ACTIVE(BOXGIMBALRLOCK)) {
-        attittude.mode |= GIMBAL_MODE_ROLL_LOCK;
+        attitude.mode |= GIMBAL_MODE_ROLL_LOCK;
     }
 
     // Follow center overrides all
     if (IS_RC_MODE_ACTIVE(BOXGIMBALCENTER) || IS_RC_MODE_ACTIVE(BOXGIMBALHTRK)) {
-        attittude.mode = GIMBAL_MODE_FOLLOW;
+        attitude.mode = GIMBAL_MODE_FOLLOW;
     }
     
     if (rxAreFlightChannelsValid() && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)) {
@@ -209,35 +209,35 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
     }
 
     if(IS_RC_MODE_ACTIVE(BOXGIMBALHTRK)) {
-        if (gimbalCommonHtrkIsEnabled() && (micros() - headTrackerState.lastUpdate) < MAX_INVALID_RX_PULSE_TIME) {
-            tilt = headTrackerState.tilt;
-            pan = headTrackerState.pan;
-            roll = headTrackerState.roll;
+        if (gimbalCommonHtrkIsEnabled() && (micros() < headTrackerState.expires)) {
+            attitude.tilt = headTrackerState.tilt;
+            attitude.pan = headTrackerState.pan;
+            attitude.roll = headTrackerState.roll;
         } else {
-            tilt = 0;
-            pan = 0;
-            roll = 0;
+            attitude.tilt = 0;
+            attitude.pan = 0;
+            attitude.roll = 0;
         }
+    } else {
+        // Radio endpoints may need to be adjusted, as it seems ot go a bit
+        // bananas at the extremes
+        attitude.pan = gimbal_scale12(1000, 2000, pan);
+        attitude.tilt = gimbal_scale12(1000, 2000, tilt);
+        attitude.roll = gimbal_scale12(1000, 2000, roll);
     }
 
-    attittude.sensibility = cfg->sensitivity; //gimbal_scale5(-16, 15, -16, 15, cfg->sensitivity);
-
-    // Radio endpoints may need to be adjusted, as it seems ot go a bit bananas
-    // at the extremes
-    attittude.pan = gimbal_scale12(1000, 2000, pan);
-    attittude.tilt = gimbal_scale12(1000, 2000, tilt);
-    attittude.roll = gimbal_scale12(1000, 2000, roll);
+    attitude.sensibility = cfg->sensitivity;
 
     uint16_t crc16 = 0;
-    uint8_t *b = (uint8_t *)&attittude;
+    uint8_t *b = (uint8_t *)&attitude;
     for (uint8_t i = 0; i < sizeof(gimbalHtkAttitudePkt_t) - 2; i++) {
         crc16 = crc16_ccitt(crc16, *(b + i));
     }
-    attittude.crch = (crc16 >> 8) & 0xFF;
-    attittude.crcl = crc16 & 0xFF;
+    attitude.crch = (crc16 >> 8) & 0xFF;
+    attitude.crcl = crc16 & 0xFF;
 
     serialBeginWrite(gimbalPort);
-    serialWriteBuf(gimbalPort, (uint8_t *)&attittude, sizeof(gimbalHtkAttitudePkt_t));
+    serialWriteBuf(gimbalPort, (uint8_t *)&attitude, sizeof(gimbalHtkAttitudePkt_t));
     serialEndWrite(gimbalPort);
 }
 #endif
@@ -273,19 +273,19 @@ static bool checkCrc(gimbalHtkAttitudePkt_t *attitude)
 void gimbalSerialHeadTrackerReceive(uint16_t c, void *data)
 {
     gimbalSerialHtrkState_t *state = (gimbalSerialHtrkState_t *)data;
-    uint8_t *payload = (uint8_t *)&(state->attittude);
+    uint8_t *payload = (uint8_t *)&(state->attitude);
     payload += 2;
 
     switch(state->state) {
         case WAITING_HDR1:
             if(c == HTKATTITUDE_SYNC0) {
-                state->attittude.sync[0] = c;
+                state->attitude.sync[0] = c;
                 state->state = WAITING_HDR2;
             }
             break;
         case WAITING_HDR2:
             if(c == HTKATTITUDE_SYNC1) {
-                state->attittude.sync[1] = c;
+                state->attitude.sync[1] = c;
                 state->state = WAITING_PAYLOAD;
             } else {
                 resetState(state);
@@ -299,15 +299,15 @@ void gimbalSerialHeadTrackerReceive(uint16_t c, void *data)
             }
             break;
         case WAITING_CRCH:
-            state->attittude.crch = c;
+            state->attitude.crch = c;
             break;
         case WAITING_CRCL:
-            state->attittude.crcl = c;
-            if(checkCrc(&(state->attittude))) {
-                state->lastUpdate = micros();
-                state->pan = state->attittude.pan;
-                state->tilt = state->attittude.tilt;
-                state->roll = state->attittude.roll;
+            state->attitude.crcl = c;
+            if(checkCrc(&(state->attitude))) {
+                state->expires = micros() + MAX_INVALID_RX_PULSE_TIME;
+                state->pan = state->attitude.pan;
+                state->tilt = state->attitude.tilt;
+                state->roll = state->attitude.roll;
             }
             resetState(state);
             break;
diff --git a/src/main/io/gimbal_serial.h b/src/main/io/gimbal_serial.h
index a7136b0e24c..2cae71f9dd6 100644
--- a/src/main/io/gimbal_serial.h
+++ b/src/main/io/gimbal_serial.h
@@ -57,13 +57,13 @@ typedef enum {
 } gimbalHeadtrackerState_e;
 
 typedef struct gimbalSerialHtrkState_s {
-    timeUs_t lastUpdate;
+    timeUs_t expires;
     uint8_t  payloadSize;
 	int16_t  roll;   //Roll angle [-2048~2047] => [-180~180]
 	int16_t  tilt;   //Pich angle [-2048~2047] => [-180~180]
 	int16_t  pan;    //Yaw angle [-2048~2047] => [-180~180]
     gimbalHeadtrackerState_e state;
-    gimbalHtkAttitudePkt_t attittude;
+    gimbalHtkAttitudePkt_t attitude;
 } gimbalSerialHtrkState_t;
 
 typedef struct gimbalSerialConfig_s {

From 21ccfa5233e96b28a09836b8409a78b05de51fed Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sun, 9 Jun 2024 18:36:25 +0200
Subject: [PATCH 205/323] Switch based changing from head tracker to center and
 sliders verified working

---
 src/main/build/debug.h           |  1 +
 src/main/drivers/gimbal_common.c |  3 +++
 src/main/fc/settings.yaml        |  2 +-
 src/main/io/gimbal_serial.c      | 19 ++++++++++++++++++-
 src/main/io/gimbal_serial.h      |  2 ++
 5 files changed, 25 insertions(+), 2 deletions(-)

diff --git a/src/main/build/debug.h b/src/main/build/debug.h
index 904e036c7f1..fea424b9005 100644
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@@ -75,6 +75,7 @@ typedef enum {
     DEBUG_LANDING,
     DEBUG_POS_EST,
     DEBUG_ADAPTIVE_FILTER,
+    DEBUG_HEADTRACKING,
     DEBUG_COUNT
 } debugType_e;
 
diff --git a/src/main/drivers/gimbal_common.c b/src/main/drivers/gimbal_common.c
index 9fcfcc2b9bc..bd066250ce5 100644
--- a/src/main/drivers/gimbal_common.c
+++ b/src/main/drivers/gimbal_common.c
@@ -102,12 +102,15 @@ bool gimbalCommonIsEnabled(void)
 
 bool gimbalCommonHtrkIsEnabled(void)
 {
+    return true;
+    /*
     const gimbalDevice_t *dev = gimbalCommonDevice();
     if(dev && dev->vTable->hasHeadTracker) {
         return dev->vTable->hasHeadTracker(dev);
     }
 
     return false;
+    */
 }
 
 #endif
\ No newline at end of file
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 5b6a9bb34f4..ab3b1de545d 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -83,7 +83,7 @@ tables:
     values: ["NONE", "AGL", "FLOW_RAW", "FLOW", "ALWAYS", "SAG_COMP_VOLTAGE",
       "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
       "NAV_YAW", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "ALTITUDE",
-      "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST", "ADAPTIVE_FILTER"]
+      "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST", "ADAPTIVE_FILTER", "HEADTRACKER" ]
   - name: aux_operator
     values: ["OR", "AND"]
     enum: modeActivationOperator_e
diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index 09c6c857a96..4ade9fdbdc8 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -213,12 +213,15 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
             attitude.tilt = headTrackerState.tilt;
             attitude.pan = headTrackerState.pan;
             attitude.roll = headTrackerState.roll;
+            DEBUG_SET(DEBUG_HEADTRACKING, 4, 1);
         } else {
             attitude.tilt = 0;
             attitude.pan = 0;
             attitude.roll = 0;
+            DEBUG_SET(DEBUG_HEADTRACKING, 4, -1);
         }
     } else {
+        DEBUG_SET(DEBUG_HEADTRACKING, 4, 0);
         // Radio endpoints may need to be adjusted, as it seems ot go a bit
         // bananas at the extremes
         attitude.pan = gimbal_scale12(1000, 2000, pan);
@@ -226,6 +229,10 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
         attitude.roll = gimbal_scale12(1000, 2000, roll);
     }
 
+    DEBUG_SET(DEBUG_HEADTRACKING, 5, attitude.pan);
+    DEBUG_SET(DEBUG_HEADTRACKING, 6, attitude.tilt);
+    DEBUG_SET(DEBUG_HEADTRACKING, 7, attitude.roll);
+
     attitude.sensibility = cfg->sensitivity;
 
     uint16_t crc16 = 0;
@@ -272,10 +279,16 @@ static bool checkCrc(gimbalHtkAttitudePkt_t *attitude)
 
 void gimbalSerialHeadTrackerReceive(uint16_t c, void *data)
 {
+    static int charCount = 0;
+    static int pktCount = 0;
+    static int errorCount = 0;
     gimbalSerialHtrkState_t *state = (gimbalSerialHtrkState_t *)data;
     uint8_t *payload = (uint8_t *)&(state->attitude);
     payload += 2;
 
+    DEBUG_SET(DEBUG_HEADTRACKING, 0, charCount++);
+    DEBUG_SET(DEBUG_HEADTRACKING, 1, state->state);
+
     switch(state->state) {
         case WAITING_HDR1:
             if(c == HTKATTITUDE_SYNC0) {
@@ -300,14 +313,18 @@ void gimbalSerialHeadTrackerReceive(uint16_t c, void *data)
             break;
         case WAITING_CRCH:
             state->attitude.crch = c;
+            state->state = WAITING_CRCL;
             break;
         case WAITING_CRCL:
             state->attitude.crcl = c;
             if(checkCrc(&(state->attitude))) {
-                state->expires = micros() + MAX_INVALID_RX_PULSE_TIME;
+                state->expires = micros() + MAX_HEADTRACKER_DATA_AGE_US;
                 state->pan = state->attitude.pan;
                 state->tilt = state->attitude.tilt;
                 state->roll = state->attitude.roll;
+                DEBUG_SET(DEBUG_HEADTRACKING, 2, pktCount++);
+            } else {
+                DEBUG_SET(DEBUG_HEADTRACKING, 3, errorCount++);
             }
             resetState(state);
             break;
diff --git a/src/main/io/gimbal_serial.h b/src/main/io/gimbal_serial.h
index 2cae71f9dd6..8ee8c28b58c 100644
--- a/src/main/io/gimbal_serial.h
+++ b/src/main/io/gimbal_serial.h
@@ -30,6 +30,8 @@ extern "C" {
 
 #ifdef USE_SERIAL_GIMBAL
 
+#define MAX_HEADTRACKER_DATA_AGE_US HZ2US(25)
+
 #define HTKATTITUDE_SYNC0  0xA5
 #define HTKATTITUDE_SYNC1  0x5A
 typedef struct gimbalHtkAttitudePkt_s

From 5afef318eeeda8347bed0a7dc93abce179e6044f Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sun, 9 Jun 2024 19:49:07 +0200
Subject: [PATCH 206/323] Small fixes

Some debugging info and fixes single uart setting
---
 src/main/drivers/gimbal_common.c | 9 +++++----
 src/main/fc/fc_msp_box.c         | 2 +-
 src/main/fc/settings.yaml        | 3 ++-
 src/main/io/gimbal_serial.c      | 2 ++
 4 files changed, 10 insertions(+), 6 deletions(-)

diff --git a/src/main/drivers/gimbal_common.c b/src/main/drivers/gimbal_common.c
index bd066250ce5..bf2db50a261 100644
--- a/src/main/drivers/gimbal_common.c
+++ b/src/main/drivers/gimbal_common.c
@@ -22,6 +22,8 @@
 #include <stdio.h>
 #include <stdint.h>
 #include <stdlib.h>
+
+#include <build/debug.h>
 #include <config/parameter_group_ids.h>
 
 #include "common/time.h"
@@ -42,6 +44,7 @@ void gimbalCommonInit(void)
 
 void gimbalCommonSetDevice(gimbalDevice_t *gimbalDevice)
 {
+    SD(fprintf(stderr, "[GIMBAL]: device added %p\n", gimbalDevice));
     commonGimbalDevice = gimbalDevice;
 }
 
@@ -102,15 +105,13 @@ bool gimbalCommonIsEnabled(void)
 
 bool gimbalCommonHtrkIsEnabled(void)
 {
-    return true;
-    /*
     const gimbalDevice_t *dev = gimbalCommonDevice();
     if(dev && dev->vTable->hasHeadTracker) {
-        return dev->vTable->hasHeadTracker(dev);
+        bool ret = dev->vTable->hasHeadTracker(dev);
+        return ret;
     }
 
     return false;
-    */
 }
 
 #endif
\ No newline at end of file
diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c
index 44a3ada88e3..92776a107f9 100644
--- a/src/main/fc/fc_msp_box.c
+++ b/src/main/fc/fc_msp_box.c
@@ -449,7 +449,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
 
     if(IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)) {
         CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)), BOXGIMBALCENTER);
-    } else if (IS_RC_MODE_ACTIVE(BOXGIMBALHTRK)) {
+    } else if (gimbalCommonHtrkIsEnabled() && IS_RC_MODE_ACTIVE(BOXGIMBALHTRK)) {
         CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALHTRK)), BOXGIMBALHTRK);
     } else {
         CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALTLOCK) && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)),     BOXGIMBALTLOCK);
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index ab3b1de545d..b6bf0cbf11e 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -4234,5 +4234,6 @@ groups:
     members:
       - name: gimbal_serial_single_uart
         description: "Gimbal serial and headtracker device share same UART. FC RX goes to headtracker device, FC TX goes to gimbal."
-        default_value: false
+        type: bool
+        default_value: OFF
         field: singleUart
\ No newline at end of file
diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index 4ade9fdbdc8..4382904fa23 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -91,6 +91,7 @@ bool gimbalSerialHasHeadTracker(const gimbalDevice_t *gimbalDevice)
 bool gimbalSerialInit(void)
 {
     if(gimbalSerialDetect()) {
+        SD(fprintf(stderr, "Setting gimbal device\n"));
         gimbalCommonSetDevice(&serialGimbalDevice);
         return true;
     }
@@ -147,6 +148,7 @@ bool gimbalSerialDetect(void)
         }
     }
 
+    SD(fprintf(stderr, "[GIMBAL]: gimbalPort: %p headTrackerPort: %p\n", gimbalPort, headTrackerPort));
     return gimbalPort || headTrackerPort;
 }
 #endif

From 3b42180117dd166019bb8f6e2f78024026c0f90e Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Mon, 10 Jun 2024 16:10:04 +0200
Subject: [PATCH 207/323] Placeholders for head tracker as servo input

---
 src/main/flight/servos.c | 6 ++++++
 src/main/flight/servos.h | 3 +++
 2 files changed, 9 insertions(+)

diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c
index f38a4ea108c..f8e51c008a2 100755
--- a/src/main/flight/servos.c
+++ b/src/main/flight/servos.c
@@ -347,6 +347,12 @@ void servoMixer(float dT)
     input[INPUT_RC_CH16]     = GET_RX_CHANNEL_INPUT(AUX12);
 #undef GET_RX_CHANNEL_INPUT
 
+#ifdef USE_SERIAL_GIMBAL
+    input[INPUT_HEADTRACKER_PAN] = 0;
+    input[INPUT_HEADTRACKER_TILT] = 0;
+    input[INPUT_HEADTRACKER_ROLL] = 0;
+#endif
+
 #ifdef USE_SIMULATOR
 	simulatorData.input[INPUT_STABILIZED_ROLL] = input[INPUT_STABILIZED_ROLL];
 	simulatorData.input[INPUT_STABILIZED_PITCH] = input[INPUT_STABILIZED_PITCH];
diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h
index 1dd65912218..d271142ece0 100644
--- a/src/main/flight/servos.h
+++ b/src/main/flight/servos.h
@@ -63,6 +63,9 @@ typedef enum {
     INPUT_GVAR_6                    = 36,
     INPUT_GVAR_7                    = 37,
     INPUT_MIXER_TRANSITION          = 38,
+    INPUT_HEADTRACKER_PAN           = 39,
+    INPUT_HEADTRACKER_TILT          = 40,
+    INPUT_HEADTRACKER_ROLL          = 41,
     INPUT_SOURCE_COUNT
 } inputSource_e;
 

From 498ad3b578aa811584b6776874a748219f0a968b Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Mon, 10 Jun 2024 17:29:52 +0200
Subject: [PATCH 208/323] Basic plumbing on servos.

Need to separate head tracker in headtracker common and headtracker
serial.

Potentially add headtracker msp
---
 src/main/flight/servos.c    | 13 ++++++++++---
 src/main/io/gimbal_serial.c | 18 +++++++++---------
 2 files changed, 19 insertions(+), 12 deletions(-)

diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c
index f8e51c008a2..a214765bb25 100755
--- a/src/main/flight/servos.c
+++ b/src/main/flight/servos.c
@@ -38,6 +38,7 @@
 #include "drivers/pwm_output.h"
 #include "drivers/pwm_mapping.h"
 #include "drivers/time.h"
+#include "drivers/gimbal_common.h"
 
 #include "fc/config.h"
 #include "fc/fc_core.h"
@@ -348,9 +349,15 @@ void servoMixer(float dT)
 #undef GET_RX_CHANNEL_INPUT
 
 #ifdef USE_SERIAL_GIMBAL
-    input[INPUT_HEADTRACKER_PAN] = 0;
-    input[INPUT_HEADTRACKER_TILT] = 0;
-    input[INPUT_HEADTRACKER_ROLL] = 0;
+    if(gimbalCommonHtrkIsEnabled() && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)) {
+        input[INPUT_HEADTRACKER_PAN] = 0;
+        input[INPUT_HEADTRACKER_TILT] = 0;
+        input[INPUT_HEADTRACKER_ROLL] = 0;
+    } else {
+        input[INPUT_HEADTRACKER_PAN] = 0;
+        input[INPUT_HEADTRACKER_TILT] = 0;
+        input[INPUT_HEADTRACKER_ROLL] = 0;
+    }
 #endif
 
 #ifdef USE_SIMULATOR
diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index 4382904fa23..d6135c1b0fe 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -176,9 +176,9 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
 
     const gimbalConfig_t *cfg = gimbalConfig();
 
-    int pan = 1500;
-    int tilt = 1500;
-    int roll = 1500;
+    int pan = PWM_RANGE_MIDDLE;
+    int tilt = PWM_RANGE_MIDDLE;
+    int roll = PWM_RANGE_MIDDLE;
 
     if (IS_RC_MODE_ACTIVE(BOXGIMBALTLOCK)) {
         attitude.mode |= GIMBAL_MODE_TILT_LOCK;
@@ -196,17 +196,17 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
     if (rxAreFlightChannelsValid() && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)) {
         if (cfg->panChannel > 0) {
             pan = rxGetChannelValue(cfg->panChannel - 1);
-            pan = constrain(pan, 1000, 2000);
+            pan = constrain(pan, PWM_RANGE_MIN, PWM_RANGE_MAX);
         }
 
         if (cfg->tiltChannel > 0) {
             tilt = rxGetChannelValue(cfg->tiltChannel - 1);
-            tilt = constrain(tilt, 1000, 2000);
+            tilt = constrain(tilt, PWM_RANGE_MIN, PWM_RANGE_MAX);
         }
 
         if (cfg->rollChannel > 0) {
             roll = rxGetChannelValue(cfg->rollChannel - 1);
-            roll = constrain(roll, 1000, 2000);
+            roll = constrain(roll, PWM_RANGE_MIN, PWM_RANGE_MAX);
         }
     }
 
@@ -226,9 +226,9 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
         DEBUG_SET(DEBUG_HEADTRACKING, 4, 0);
         // Radio endpoints may need to be adjusted, as it seems ot go a bit
         // bananas at the extremes
-        attitude.pan = gimbal_scale12(1000, 2000, pan);
-        attitude.tilt = gimbal_scale12(1000, 2000, tilt);
-        attitude.roll = gimbal_scale12(1000, 2000, roll);
+        attitude.pan = gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, pan);
+        attitude.tilt = gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, tilt);
+        attitude.roll = gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, roll);
     }
 
     DEBUG_SET(DEBUG_HEADTRACKING, 5, attitude.pan);

From 8f3f1469819239fb9f9cbb364988e5c4e329e6fc Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Mon, 10 Jun 2024 20:37:15 +0200
Subject: [PATCH 209/323] Make serial headtracker input available on the mixer
 tab

---
 docs/Settings.md                      | 10 +++
 src/main/CMakeLists.txt               |  2 +
 src/main/config/parameter_group_ids.h |  3 +-
 src/main/fc/fc_init.c                 | 12 +++-
 src/main/fc/fc_tasks.c                | 14 +++++
 src/main/fc/settings.yaml             | 24 ++++++--
 src/main/flight/servos.c              | 10 +--
 src/main/io/gimbal_serial.c           | 89 +++++++++++++++++++++++++++
 src/main/io/gimbal_serial.h           | 13 ++++
 src/main/scheduler/scheduler.h        |  4 ++
 src/main/target/common.h              |  1 +
 11 files changed, 170 insertions(+), 12 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index 81e83a2ae04..1328128fc32 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -1822,6 +1822,16 @@ This setting limits yaw rotation rate that HEADING_HOLD controller can request f
 
 ---
 
+### headtracker_type
+
+Type of headtrackr dervice
+
+| Default | Min | Max |
+| --- | --- | --- |
+| SERIAL |  |  |
+
+---
+
 ### hott_alarm_sound_interval
 
 Battery alarm delay in seconds for Hott telemetry
diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt
index 91aa45d42e3..7162bc4faa4 100755
--- a/src/main/CMakeLists.txt
+++ b/src/main/CMakeLists.txt
@@ -179,6 +179,8 @@ main_sources(COMMON_SRC
     drivers/flash_w25n01g.h
     drivers/gimbal_common.h
     drivers/gimbal_common.c
+    drivers/headtracker_common.h
+    drivers/headtracker_common.c
     drivers/io.c
     drivers/io.h
     drivers/io_pcf8574.c
diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h
index 067d97bf0a1..b4062201c72 100644
--- a/src/main/config/parameter_group_ids.h
+++ b/src/main/config/parameter_group_ids.h
@@ -128,7 +128,8 @@
 #define PG_OSD_CUSTOM_ELEMENTS_CONFIG 1038
 #define PG_GIMBAL_CONFIG 1039
 #define PG_GIMBAL_SERIAL_CONFIG 1040
-#define PG_INAV_END PG_GIMBAL_SERIAL_CONFIG
+#define PG_HEADTRACKER_CONFIG 1041
+#define PG_INAV_END PG_HEADTRACKER_CONFIG
 
 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047
diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index 76aa9d79511..b57a60a9f69 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -54,6 +54,7 @@
 #include "drivers/io.h"
 #include "drivers/flash.h"
 #include "drivers/gimbal_common.h"
+#include "drivers/headtracker_common.h"
 #include "drivers/light_led.h"
 #include "drivers/nvic.h"
 #include "drivers/osd.h"
@@ -689,13 +690,20 @@ void init(void)
     initDShotCommands();
 #endif
 
-    fprintf(stderr, "HERE\n");
 #ifdef USE_SERIAL_GIMBAL
-    fprintf(stderr, "HERE2\n");
     gimbalCommonInit();
+    // Needs to be called before gimbalSerialHeadTrackerInit
     gimbalSerialInit();
 #endif
 
+#ifdef USE_HEADTRACKER
+    headTrackerCommonInit();
+#ifdef USE_SERIAL_GIMBAL
+    // Needs to be called after gimbalSerialInit
+    gimbalSerialHeadTrackerInit();
+#endif
+#endif
+
     // Latch active features AGAIN since some may be modified by init().
     latchActiveFeatures();
     motorControlEnable = true;
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index 9dff7c70e6f..3dc126cf0c8 100755
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -432,6 +432,10 @@ void fcTasksInit(void)
     setTaskEnabled(TASK_GIMBAL, true);
 #endif
 
+#ifdef USE_HEADTRACKER
+    setTaskEnabled(TASK_HEADTRACKER, true);
+#endif
+
 #ifdef USE_ADAPTIVE_FILTER
     setTaskEnabled(TASK_ADAPTIVE_FILTER, (gyroConfig()->gyroFilterMode == GYRO_FILTER_MODE_ADAPTIVE));
 #endif
@@ -707,4 +711,14 @@ cfTask_t cfTasks[TASK_COUNT] = {
         .staticPriority = TASK_PRIORITY_MEDIUM,
     },
 #endif
+
+#ifdef USE_SERIAL_GIMBAL
+    [TASK_HEADTRACKER] = {
+        .taskName = "HEADTRACKER",
+        .taskFunc = taskUpdateGimbal,
+        .desiredPeriod = TASK_PERIOD_HZ(50),
+        .staticPriority = TASK_PRIORITY_MEDIUM,
+    },
+#endif
+
 };
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index b6bf0cbf11e..ee1ba4140f0 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -194,6 +194,9 @@ tables:
   - name: gyro_filter_mode
     values: ["STATIC", "DYNAMIC", "ADAPTIVE"]
     enum: gyroFilterType_e
+  - name: headtracker_dev_type
+    values: ["NONE", "SERIAL", "MSP"]
+    enum: headTrackerDevType_e
 
 constants:
   RPYL_PID_MIN: 0
@@ -4232,8 +4235,19 @@ groups:
     headers: ["io/gimbal_serial.h"]
     condition: USE_SERIAL_GIMBAL
     members:
-      - name: gimbal_serial_single_uart
-        description: "Gimbal serial and headtracker device share same UART. FC RX goes to headtracker device, FC TX goes to gimbal."
-        type: bool
-        default_value: OFF
-        field: singleUart
\ No newline at end of file
+    - name: gimbal_serial_single_uart
+      description: "Gimbal serial and headtracker device share same UART. FC RX goes to headtracker device, FC TX goes to gimbal."
+      type: bool
+      default_value: OFF
+      field: singleUart
+  - name: PG_HEADTRACKER_CONFIG
+    type: headTrackerConfig_t
+    headers: ["drivers/headtracker_common.h"]
+    condition: USE_HEADTRACKER
+    members:
+    - name: headtracker_type
+      description: "Type of headtrackr dervice"
+      default_value: "SERIAL"
+      field: devType
+      type: uint8_t
+      table: headtracker_dev_type
diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c
index a214765bb25..3e93b5f082b 100755
--- a/src/main/flight/servos.c
+++ b/src/main/flight/servos.c
@@ -39,6 +39,7 @@
 #include "drivers/pwm_mapping.h"
 #include "drivers/time.h"
 #include "drivers/gimbal_common.h"
+#include "drivers/headtracker_common.h"
 
 #include "fc/config.h"
 #include "fc/fc_core.h"
@@ -349,10 +350,11 @@ void servoMixer(float dT)
 #undef GET_RX_CHANNEL_INPUT
 
 #ifdef USE_SERIAL_GIMBAL
-    if(gimbalCommonHtrkIsEnabled() && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)) {
-        input[INPUT_HEADTRACKER_PAN] = 0;
-        input[INPUT_HEADTRACKER_TILT] = 0;
-        input[INPUT_HEADTRACKER_ROLL] = 0;
+    headTrackerDevice_t *dev = headTrackerCommonDevice();
+    if(dev && headTrackerCommonIsValid(dev) && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)) {
+        input[INPUT_HEADTRACKER_PAN] = headTrackerCommonGetPanPWM(dev) - PWM_RANGE_MIDDLE;
+        input[INPUT_HEADTRACKER_TILT] = headTrackerCommonGetTiltPWM(dev) - PWM_RANGE_MIDDLE;
+        input[INPUT_HEADTRACKER_ROLL] = headTrackerCommonGetRollPWM(dev) - PWM_RANGE_MIDDLE;
     } else {
         input[INPUT_HEADTRACKER_PAN] = 0;
         input[INPUT_HEADTRACKER_TILT] = 0;
diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index d6135c1b0fe..0dd28b9dd3e 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -29,6 +29,7 @@
 #include <build/debug.h>
 
 #include <drivers/gimbal_common.h>
+#include <drivers/headtracker_common.h>
 #include <drivers/serial.h>
 #include <drivers/time.h>
 
@@ -332,6 +333,94 @@ void gimbalSerialHeadTrackerReceive(uint16_t c, void *data)
             break;
     }
 }
+
+#ifdef USE_HEADTRACKER
+
+static headtrackerVTable_t headTrackerVTable = {
+    .process = headtrackerSerialProcess,
+    .getDeviceType = headtrackerSerialGetDeviceType,
+    .isReady = headTrackerSerialIsReady,
+    .isValid = headTrackerSerialIsValid,
+    .getPanPWM = headTrackerSerialGetPanPWM,
+    .getTiltPWM = headTrackerSerialGetTiltPWM,
+    .getRollPWM = headTrackerSerialGetRollPWM,
+};
+
+headTrackerDevice_t headTrackerDevice = {
+    .vTable = &headTrackerVTable,
+};
+
+bool gimbalSerialHeadTrackerInit(void)
+{
+    if(headTrackerConfig()->devType == HEADTRACKER_SERIAL) {
+        headTrackerCommonSetDevice(&headTrackerDevice);
+    }
+
+    return false;
+}
+
+void headtrackerSerialProcess(headTrackerDevice_t *headTrackerDevice, timeUs_t currentTimeUs)
+{
+    UNUSED(currentTimeUs);
+    headTrackerDevice->expires = headTrackerState.expires;
+    return;
+}
+
+headTrackerDevType_e headtrackerSerialGetDeviceType(const headTrackerDevice_t *headTrackerDevice)
+{
+    UNUSED(headTrackerDevice);
+    return HEADTRACKER_SERIAL;
+}
+
+bool headTrackerSerialIsReady(const headTrackerDevice_t *headTrackerDevice)
+{
+    UNUSED(headTrackerDevice);
+
+    if(headTrackerPort || (gimbalSerialConfig()->singleUart && gimbalPort)) {
+        return  headTrackerSerialIsValid(headTrackerDevice);
+    }
+
+    return false;
+}
+
+bool headTrackerSerialIsValid(const headTrackerDevice_t *headTrackerDevice)
+{
+    UNUSED(headTrackerDevice);
+    return micros() < headTrackerState.expires;
+}
+
+int headTrackerSerialGetPanPWM(const headTrackerDevice_t *headTrackerDevice)
+{
+    UNUSED(headTrackerDevice);
+    if(micros() < headTrackerState.expires) {
+        return scaleRange(headTrackerState.pan, -2048, 2047, PWM_RANGE_MIN, PWM_RANGE_MAX);
+    }
+
+    return PWM_RANGE_MIDDLE;
+}
+
+int headTrackerSerialGetTiltPWM(const headTrackerDevice_t *headTrackerDevice)
+{
+    UNUSED(headTrackerDevice);
+    if(micros() < headTrackerState.expires) {
+        return scaleRange(headTrackerState.tilt, -2048, 2047, PWM_RANGE_MIN, PWM_RANGE_MAX);
+    }
+
+    return PWM_RANGE_MIDDLE;
+}
+
+int headTrackerSerialGetRollPWM(const headTrackerDevice_t *headTrackerDevice)
+{
+    UNUSED(headTrackerDevice);
+    if(micros() < headTrackerState.expires) {
+        return scaleRange(headTrackerState.roll, -2048, 2047, PWM_RANGE_MIN, PWM_RANGE_MAX);
+    }
+
+    return PWM_RANGE_MIDDLE;
+}
+
+#endif
+
 #endif
 
 #endif
\ No newline at end of file
diff --git a/src/main/io/gimbal_serial.h b/src/main/io/gimbal_serial.h
index 8ee8c28b58c..c2f01aa1b90 100644
--- a/src/main/io/gimbal_serial.h
+++ b/src/main/io/gimbal_serial.h
@@ -23,6 +23,7 @@
 
 #include "common/time.h"
 #include "drivers/gimbal_common.h"
+#include "drivers/headtracker_common.h"
 
 #ifdef __cplusplus
 extern "C" {
@@ -85,6 +86,18 @@ gimbalDevType_e gimbalSerialGetDeviceType(const gimbalDevice_t *gimbalDevice);
 bool gimbalSerialHasHeadTracker(const gimbalDevice_t *gimbalDevice);
 void gimbalSerialHeadTrackerReceive(uint16_t c, void *data);
 
+
+#ifdef USE_HEADTRACKER
+bool gimbalSerialHeadTrackerInit(void);
+void headtrackerSerialProcess(headTrackerDevice_t *headTrackerDevice, timeUs_t currentTimeUs);
+headTrackerDevType_e headtrackerSerialGetDeviceType(const headTrackerDevice_t *headTrackerDevice);
+bool headTrackerSerialIsReady(const headTrackerDevice_t *headTrackerDevice);
+bool headTrackerSerialIsValid(const headTrackerDevice_t *headTrackerDevice);
+int headTrackerSerialGetPanPWM(const headTrackerDevice_t *headTrackerDevice);
+int headTrackerSerialGetTiltPWM(const headTrackerDevice_t *headTrackerDevice);
+int headTrackerSerialGetRollPWM(const headTrackerDevice_t *headTrackerDevice);
+#endif
+
 #endif
 
 #ifdef __cplusplus
diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h
index d8a6a34c8ef..a43206840fe 100755
--- a/src/main/scheduler/scheduler.h
+++ b/src/main/scheduler/scheduler.h
@@ -129,6 +129,10 @@ typedef enum {
     TASK_GIMBAL,
 #endif
 
+#ifdef USE_HEADTRACKER
+    TASK_HEADTRACKER,
+#endif
+
     /* Count of real tasks */
     TASK_COUNT,
 
diff --git a/src/main/target/common.h b/src/main/target/common.h
index 84dc7bf5a72..7f2a8b221f1 100644
--- a/src/main/target/common.h
+++ b/src/main/target/common.h
@@ -191,6 +191,7 @@
 #endif
 
 #define USE_SERIAL_GIMBAL
+#define USE_HEADTRACKER
 
 //Designed to free space of F722 and F411 MCUs
 #if (MCU_FLASH_SIZE > 512)

From 5865ab990dfe056a58b3cfa4ec80c4efda8d08af Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Mon, 10 Jun 2024 21:04:58 +0200
Subject: [PATCH 210/323] Build fixed for no gimbal or no headtracker

---
 src/main/fc/fc_msp_box.c    | 11 ++++++-
 src/main/fc/fc_tasks.c      |  5 +--
 src/main/flight/servos.c    |  6 +++-
 src/main/io/gimbal_serial.c | 66 ++++++++++++++++++++++++-------------
 4 files changed, 62 insertions(+), 26 deletions(-)

diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c
index 92776a107f9..a20a23e0b24 100644
--- a/src/main/fc/fc_msp_box.c
+++ b/src/main/fc/fc_msp_box.c
@@ -44,6 +44,7 @@
 #include "telemetry/telemetry.h"
 
 #include "drivers/gimbal_common.h"
+#include "drivers/headtracker_common.h"
 
 #define BOX_SUFFIX ';'
 #define BOX_SUFFIX_LEN 1
@@ -370,6 +371,10 @@ void initActiveBoxIds(void)
         ADD_ACTIVE_BOX(BOXGIMBALTLOCK);
         ADD_ACTIVE_BOX(BOXGIMBALRLOCK);
         ADD_ACTIVE_BOX(BOXGIMBALCENTER);
+    }
+#endif
+#ifdef USE_HEADTRACKER
+    if(headTrackerConfig()->devType != HEADTRACKER_NONE) {
         ADD_ACTIVE_BOX(BOXGIMBALHTRK);
     }
 #endif
@@ -447,15 +452,19 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
 #endif
     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXANGLEHOLD)),       BOXANGLEHOLD);
 
+#ifdef USE_SERIAL_GIMBAL
     if(IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)) {
         CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)), BOXGIMBALCENTER);
-    } else if (gimbalCommonHtrkIsEnabled() && IS_RC_MODE_ACTIVE(BOXGIMBALHTRK)) {
+#ifdef USE_HEADTRACKER
+    } else if (headTrackerCommonIsReady(headTrackerCommonDevice()) && IS_RC_MODE_ACTIVE(BOXGIMBALHTRK)) {
         CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALHTRK)), BOXGIMBALHTRK);
+#endif
     } else {
         CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALTLOCK) && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)),     BOXGIMBALTLOCK);
         CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALRLOCK) && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)),     BOXGIMBALRLOCK);
         CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALHTRK) && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)),     BOXGIMBALRLOCK);
     }
+#endif
 
     memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t));
     for (uint32_t i = 0; i < activeBoxIdCount; i++) {
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index 3dc126cf0c8..1179f057fa5 100755
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -35,6 +35,7 @@
 #include "drivers/stack_check.h"
 #include "drivers/pwm_mapping.h"
 #include "drivers/gimbal_common.h"
+#include "drivers/headtracker_common.h"
 
 #include "fc/cli.h"
 #include "fc/config.h"
@@ -712,10 +713,10 @@ cfTask_t cfTasks[TASK_COUNT] = {
     },
 #endif
 
-#ifdef USE_SERIAL_GIMBAL
+#ifdef USE_HEADTRACKER
     [TASK_HEADTRACKER] = {
         .taskName = "HEADTRACKER",
-        .taskFunc = taskUpdateGimbal,
+        .taskFunc = taskUpdateHeadTracker,
         .desiredPeriod = TASK_PERIOD_HZ(50),
         .staticPriority = TASK_PRIORITY_MEDIUM,
     },
diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c
index 3e93b5f082b..3830660660a 100755
--- a/src/main/flight/servos.c
+++ b/src/main/flight/servos.c
@@ -349,7 +349,7 @@ void servoMixer(float dT)
     input[INPUT_RC_CH16]     = GET_RX_CHANNEL_INPUT(AUX12);
 #undef GET_RX_CHANNEL_INPUT
 
-#ifdef USE_SERIAL_GIMBAL
+#ifdef USE_HEADTRACKER
     headTrackerDevice_t *dev = headTrackerCommonDevice();
     if(dev && headTrackerCommonIsValid(dev) && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)) {
         input[INPUT_HEADTRACKER_PAN] = headTrackerCommonGetPanPWM(dev) - PWM_RANGE_MIDDLE;
@@ -360,6 +360,10 @@ void servoMixer(float dT)
         input[INPUT_HEADTRACKER_TILT] = 0;
         input[INPUT_HEADTRACKER_ROLL] = 0;
     }
+#else
+        input[INPUT_HEADTRACKER_PAN] = 0;
+        input[INPUT_HEADTRACKER_TILT] = 0;
+        input[INPUT_HEADTRACKER_ROLL] = 0;
 #endif
 
 #ifdef USE_SIMULATOR
diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index 0dd28b9dd3e..b4b1ef3b1ca 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -112,10 +112,17 @@ bool gimbalSerialDetect(void)
     serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_GIMBAL);
     bool singleUart = gimbalSerialConfig()->singleUart;
 
+
     if (portConfig) {
         SD(fprintf(stderr, "[GIMBAL]: found port...\n"));
+#ifdef USE_HEADTRACKER
         gimbalPort = openSerialPort(portConfig->identifier, FUNCTION_GIMBAL, singleUart ? gimbalSerialHeadTrackerReceive : NULL, singleUart ? &headTrackerState :  NULL,
                 baudRates[portConfig->peripheral_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
+#else
+        UNUSED(singleUart);
+        gimbalPort = openSerialPort(portConfig->identifier, FUNCTION_GIMBAL, NULL, NULL,
+                baudRates[portConfig->peripheral_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
+#endif
 
         if (gimbalPort) {
             SD(fprintf(stderr, "[GIMBAL]: port open!\n"));
@@ -129,28 +136,8 @@ bool gimbalSerialDetect(void)
         }
     }
 
-    SD(fprintf(stderr, "[GIMBAL_HTRK]: headtracker Detect...\n"));
-    portConfig = singleUart ? NULL : findSerialPortConfig(FUNCTION_GIMBAL_HEADTRACKER);
-
-    if (portConfig) {
-        SD(fprintf(stderr, "[GIMBAL_HTRK]: found port...\n"));
-        headTrackerPort = openSerialPort(portConfig->identifier, FUNCTION_GIMBAL_HEADTRACKER, gimbalSerialHeadTrackerReceive, &headTrackerState,
-                baudRates[portConfig->peripheral_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
-
-        if (headTrackerPort) {
-            SD(fprintf(stderr, "[GIMBAL_HTRK]: port open!\n"));
-            headTrackerPort->txBuffer = txBuffer;
-            headTrackerPort->txBufferSize = GIMBAL_SERIAL_BUFFER_SIZE;
-            headTrackerPort->txBufferTail = 0;
-            headTrackerPort->txBufferHead = 0;
-        } else {
-            SD(fprintf(stderr, "[GIMBAL_HTRK]: port NOT open!\n"));
-            return false;
-        }
-    }
-
-    SD(fprintf(stderr, "[GIMBAL]: gimbalPort: %p headTrackerPort: %p\n", gimbalPort, headTrackerPort));
-    return gimbalPort || headTrackerPort;
+    SD(fprintf(stderr, "[GIMBAL]: gimbalPort\n", gimbalPort));
+    return gimbalPort;
 }
 #endif
 
@@ -350,8 +337,43 @@ headTrackerDevice_t headTrackerDevice = {
     .vTable = &headTrackerVTable,
 };
 
+bool gimbalSerialHeadTrackerDetect(void)
+{
+    bool singleUart = gimbalSerialConfig()->singleUart;
+
+    SD(fprintf(stderr, "[GIMBAL_HTRK]: headtracker Detect...\n"));
+    serialPortConfig_t *portConfig = singleUart ? NULL : findSerialPortConfig(FUNCTION_GIMBAL_HEADTRACKER);
+
+    if (portConfig) {
+        SD(fprintf(stderr, "[GIMBAL_HTRK]: found port...\n"));
+        headTrackerPort = openSerialPort(portConfig->identifier, FUNCTION_GIMBAL_HEADTRACKER, gimbalSerialHeadTrackerReceive, &headTrackerState,
+                baudRates[portConfig->peripheral_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
+
+        if (headTrackerPort) {
+            SD(fprintf(stderr, "[GIMBAL_HTRK]: port open!\n"));
+            headTrackerPort->txBuffer = txBuffer;
+            headTrackerPort->txBufferSize = GIMBAL_SERIAL_BUFFER_SIZE;
+            headTrackerPort->txBufferTail = 0;
+            headTrackerPort->txBufferHead = 0;
+        } else {
+            SD(fprintf(stderr, "[GIMBAL_HTRK]: port NOT open!\n"));
+            return false;
+        }
+    }
+
+    SD(fprintf(stderr, "[GIMBAL]: gimbalPort: %p headTrackerPort: %p\n", gimbalPort, headTrackerPort));
+    return (singleUart && gimbalPort) || headTrackerPort;
+}
+
 bool gimbalSerialHeadTrackerInit(void)
 {
+    if(gimbalSerialHeadTrackerDetect()) {
+        SD(fprintf(stderr, "Setting gimbal device\n"));
+        gimbalCommonSetDevice(&serialGimbalDevice);
+        return true;
+    }
+
+
     if(headTrackerConfig()->devType == HEADTRACKER_SERIAL) {
         headTrackerCommonSetDevice(&headTrackerDevice);
     }

From 5efba06ad5425de43fdbe5590a3d5c93edd81ed2 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Mon, 10 Jun 2024 21:10:34 +0200
Subject: [PATCH 211/323] Missing file

---
 src/main/io/gimbal_serial.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/io/gimbal_serial.h b/src/main/io/gimbal_serial.h
index c2f01aa1b90..7e342994a38 100644
--- a/src/main/io/gimbal_serial.h
+++ b/src/main/io/gimbal_serial.h
@@ -89,6 +89,7 @@ void gimbalSerialHeadTrackerReceive(uint16_t c, void *data);
 
 #ifdef USE_HEADTRACKER
 bool gimbalSerialHeadTrackerInit(void);
+bool gimbalSerialHeadTrackerDetect(void);
 void headtrackerSerialProcess(headTrackerDevice_t *headTrackerDevice, timeUs_t currentTimeUs);
 headTrackerDevType_e headtrackerSerialGetDeviceType(const headTrackerDevice_t *headTrackerDevice);
 bool headTrackerSerialIsReady(const headTrackerDevice_t *headTrackerDevice);

From 03698d688d0025585ea556e6ec26559809c76471 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Mon, 10 Jun 2024 21:11:10 +0200
Subject: [PATCH 212/323] New files

---
 src/main/drivers/headtracker_common.c | 152 ++++++++++++++++++++++++++
 src/main/drivers/headtracker_common.h |  95 ++++++++++++++++
 2 files changed, 247 insertions(+)
 create mode 100644 src/main/drivers/headtracker_common.c
 create mode 100644 src/main/drivers/headtracker_common.h

diff --git a/src/main/drivers/headtracker_common.c b/src/main/drivers/headtracker_common.c
new file mode 100644
index 00000000000..6f10eb1a5a9
--- /dev/null
+++ b/src/main/drivers/headtracker_common.c
@@ -0,0 +1,152 @@
+/*
+ * This file is part of INAV.
+ *
+ * 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 INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "platform.h"
+
+#ifdef USE_HEADTRACKER
+
+#include <stdio.h>
+#include <stdint.h>
+#include <stdlib.h>
+
+#include <build/debug.h>
+#include <config/parameter_group_ids.h>
+
+#include "common/time.h"
+#include "common/maths.h"
+
+#include "fc/cli.h"
+
+#include "rx/rx.h"
+
+#include "drivers/headtracker_common.h"
+#include "drivers/time.h"
+
+
+PG_REGISTER(headTrackerConfig_t, headTrackerConfig, PG_HEADTRACKER_CONFIG, 0);
+
+
+static headTrackerDevice_t *commonHeadTrackerDevice = NULL;
+
+void headTrackerCommonInit(void)
+{
+}
+
+void headTrackerCommonSetDevice(headTrackerDevice_t *headTrackerDevice)
+{
+    SD(fprintf(stderr, "[headTracker]: device added %p\n", headTrackerDevice));
+    commonHeadTrackerDevice = headTrackerDevice;
+}
+
+headTrackerDevice_t *headTrackerCommonDevice(void)
+{
+    return commonHeadTrackerDevice;
+}
+
+void headTrackerCommonProcess(headTrackerDevice_t *headTrackerDevice, timeUs_t currentTimeUs)
+{
+    if (headTrackerDevice && headTrackerDevice->vTable->process && headTrackerCommonIsReady(headTrackerDevice)) {
+        headTrackerDevice->vTable->process(headTrackerDevice, currentTimeUs);
+    }
+}
+
+headTrackerDevType_e headTrackerCommonGetDeviceType(headTrackerDevice_t *headTrackerDevice)
+{
+    if (!headTrackerDevice || !headTrackerDevice->vTable->getDeviceType) {
+        return HEADTRACKER_UNKNOWN;
+    }
+
+    return headTrackerDevice->vTable->getDeviceType(headTrackerDevice);
+}
+
+bool headTrackerCommonIsReady(headTrackerDevice_t *headTrackerDevice)
+{
+    if (headTrackerDevice && headTrackerDevice->vTable->isReady) {
+        return headTrackerDevice->vTable->isReady(headTrackerDevice);
+    }
+    return false;
+}
+
+int headTrackerCommonGetPanPWM(const headTrackerDevice_t *headTrackerDevice)
+{
+    if(headTrackerDevice && headTrackerDevice->vTable && headTrackerDevice->vTable->getPanPWM) {
+        return headTrackerDevice->vTable->getPanPWM(headTrackerDevice);
+    }
+
+    return constrain(headTrackerDevice->pan, PWM_RANGE_MIN, PWM_RANGE_MAX);
+}
+
+int headTrackerCommonGetTiltPWM(const headTrackerDevice_t *headTrackerDevice)
+{
+    if(headTrackerDevice && headTrackerDevice->vTable && headTrackerDevice->vTable->getPanPWM) {
+        return headTrackerDevice->vTable->getTiltPWM(headTrackerDevice);
+    }
+
+    return constrain(headTrackerDevice->tilt, PWM_RANGE_MIN, PWM_RANGE_MAX);
+}
+
+int headTrackerCommonGetRollPWM(const headTrackerDevice_t *headTrackerDevice)
+{
+    if(headTrackerDevice && headTrackerDevice->vTable && headTrackerDevice->vTable->getPanPWM) {
+        return headTrackerDevice->vTable->getTiltPWM(headTrackerDevice);
+    }
+
+    return constrain(headTrackerDevice->roll, PWM_RANGE_MIN, PWM_RANGE_MAX);
+}
+
+
+#ifdef headTracker_UNIT_TEST
+void taskUpdateHeadTracker(timeUs_t currentTimeUs)
+{
+}
+#else
+void taskUpdateHeadTracker(timeUs_t currentTimeUs)
+{
+    if (cliMode) {
+        return;
+    }
+
+    headTrackerDevice_t *headTrackerDevice = headTrackerCommonDevice();
+
+    if(headTrackerDevice) {
+        headTrackerCommonProcess(headTrackerDevice, currentTimeUs);
+    }
+}
+
+// TODO: check if any headTracker types are enabled
+bool headTrackerCommonIsEnabled(void)
+{
+    if (commonHeadTrackerDevice && headTrackerCommonIsReady(commonHeadTrackerDevice)) {
+        return true;
+    }
+
+    return false;
+}
+
+bool headTrackerCommonIsValid(headTrackerDevice_t *dev) {
+    if(dev && dev->vTable && dev->vTable->isValid) {
+        return dev->vTable->isValid(dev);
+    }
+
+    return micros() < dev->expires;
+}
+
+
+#endif
+
+
+#endif
\ No newline at end of file
diff --git a/src/main/drivers/headtracker_common.h b/src/main/drivers/headtracker_common.h
new file mode 100644
index 00000000000..27b62e0e568
--- /dev/null
+++ b/src/main/drivers/headtracker_common.h
@@ -0,0 +1,95 @@
+/*
+ * This file is part of INAV.
+ *
+ * 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 INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "platform.h"
+
+#ifdef USE_HEADTRACKER
+
+#include <stdint.h>
+
+#include "config/feature.h"
+#include "common/time.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef enum {
+    HEADTRACKER_NONE   = 0,
+    HEADTRACKER_SERIAL = 1,
+    HEADTRACKER_MSP    = 2,
+    HEADTRACKER_UNKNOWN = 0xFF
+} headTrackerDevType_e;
+
+
+struct headTrackerVTable_s;
+
+typedef struct headTrackerDevice_s {
+    const struct headTrackerVTable_s *vTable;
+    int pan;
+    int tilt;
+    int roll;
+    timeUs_t expires;
+} headTrackerDevice_t;
+
+// {set,get}BandAndChannel: band and channel are 1 origin
+// {set,get}PowerByIndex: 0 = Power OFF, 1 = device dependent
+// {set,get}PitMode: 0 = OFF, 1 = ON
+
+typedef struct headTrackerVTable_s {
+    void (*process)(headTrackerDevice_t *headTrackerDevice, timeUs_t currentTimeUs);
+    headTrackerDevType_e (*getDeviceType)(const headTrackerDevice_t *headTrackerDevice);
+    bool (*isReady)(const headTrackerDevice_t *headTrackerDevice);
+    bool (*isValid)(const headTrackerDevice_t *headTrackerDevice);
+    int (*getPanPWM)(const headTrackerDevice_t *headTrackerDevice);
+    int (*getTiltPWM)(const headTrackerDevice_t *headTrackerDevice);
+    int (*getRollPWM)(const headTrackerDevice_t *headTrackerDevice);
+} headtrackerVTable_t;
+
+
+typedef struct headTrackerConfig_s {
+    headTrackerDevType_e devType;
+} headTrackerConfig_t;
+
+PG_DECLARE(headTrackerConfig_t, headTrackerConfig);
+
+void headTrackerCommonInit(void);
+void headTrackerCommonSetDevice(headTrackerDevice_t *headTrackerDevice);
+headTrackerDevice_t *headTrackerCommonDevice(void);
+
+// VTable functions
+void headTrackerCommonProcess(headTrackerDevice_t *headTrackerDevice, timeUs_t currentTimeUs);
+headTrackerDevType_e headTrackerCommonGetDeviceType(headTrackerDevice_t *headTrackerDevice);
+bool headTrackerCommonIsReady(headTrackerDevice_t *headtrackerDevice);
+bool headTrackerCommonIsValid(headTrackerDevice_t *headtrackerDevice);
+int headTrackerCommonGetPanPWM(const headTrackerDevice_t *headTrackerDevice);
+int headTrackerCommonGetTiltPWM(const headTrackerDevice_t *headTrackerDevice);
+int headTrackerCommonGetRollPWM(const headTrackerDevice_t *headTrackerDevice);
+
+
+void taskUpdateHeadTracker(timeUs_t currentTimeUs);
+
+bool headtrackerCommonIsEnabled(void);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
\ No newline at end of file

From f09eef140175b092ba8e44171a5b9b513afd8acd Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Mon, 10 Jun 2024 21:54:36 +0200
Subject: [PATCH 213/323] sitl fix

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

diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index b4b1ef3b1ca..71719810895 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -136,7 +136,7 @@ bool gimbalSerialDetect(void)
         }
     }
 
-    SD(fprintf(stderr, "[GIMBAL]: gimbalPort\n", gimbalPort));
+    SD(fprintf(stderr, "[GIMBAL]: gimbalPort: %p\n", gimbalPort));
     return gimbalPort;
 }
 #endif

From 03829b730b85ae3ecc34a945f298cd55fcc09074 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Mon, 10 Jun 2024 22:08:28 +0200
Subject: [PATCH 214/323] fix detection

---
 src/main/io/gimbal_serial.c | 14 ++++++--------
 1 file changed, 6 insertions(+), 8 deletions(-)

diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index 71719810895..b49ec75ccf2 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -367,15 +367,13 @@ bool gimbalSerialHeadTrackerDetect(void)
 
 bool gimbalSerialHeadTrackerInit(void)
 {
-    if(gimbalSerialHeadTrackerDetect()) {
-        SD(fprintf(stderr, "Setting gimbal device\n"));
-        gimbalCommonSetDevice(&serialGimbalDevice);
-        return true;
-    }
-
-
     if(headTrackerConfig()->devType == HEADTRACKER_SERIAL) {
-        headTrackerCommonSetDevice(&headTrackerDevice);
+        if (gimbalSerialHeadTrackerDetect()) {
+            SD(fprintf(stderr, "Setting gimbal device\n"));
+            headTrackerCommonSetDevice(&headTrackerDevice);
+
+            return true;
+        }
     }
 
     return false;

From 155732d814a2833dc28331b3b0e00241ed5874da Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Tue, 11 Jun 2024 12:06:16 +0100
Subject: [PATCH 215/323] Change to using vertical position sanity check

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

diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c
index 96d44252715..169876d240a 100644
--- a/src/main/navigation/navigation_multicopter.c
+++ b/src/main/navigation/navigation_multicopter.c
@@ -799,19 +799,23 @@ bool isMulticopterCrashedInverted(void)
 {
     static timeMs_t startTime = 0;
 
-    if ((ABS(attitude.values.roll) > 1000 || ABS(attitude.values.pitch) > 700) && fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) < MC_LAND_CHECK_VEL_Z_MOVING) {
+    if (ABS(attitude.values.roll) > 1000 || ABS(attitude.values.pitch) > 700) {
+        static uint32_t initialAltitude;
+
         if (startTime == 0) {
             startTime = millis();
-        } else {
-            /* minimum 2s disarm delay + extra user set delay time. Min time of 3s given min user setting is 1s if enabled */
-            uint16_t disarmTimeDelay = 2000 + S2MS(navConfig()->mc.inverted_crash_detection);
+            initialAltitude = navGetCurrentActualPositionAndVelocity()->pos.z;
+            return false;
+        } else if (ABS(initialAltitude - navGetCurrentActualPositionAndVelocity()->pos.z) < 200) {
+            /* Check altitude change < 2m during disarm timeout period indicating MR not in flight.
+             * Minimum 3s disarm delay + extra user set delay time = Min time of 5s given min user setting is 1s if enabled */
 
+            uint16_t disarmTimeDelay = 3000 + S2MS(navConfig()->mc.inverted_crash_detection);
             return millis() - startTime > disarmTimeDelay;
         }
-    } else {
-        startTime = 0;
     }
 
+    startTime = 0;
     return false;
 }
 

From 4257ff3177a5f3e9f57a1b1451edecd78bd4fcc5 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Tue, 11 Jun 2024 17:56:55 +0200
Subject: [PATCH 216/323] Add MSP headtracker

---
 src/main/CMakeLists.txt               |  2 +
 src/main/drivers/headtracker_common.c |  9 +--
 src/main/drivers/headtracker_common.h | 15 +++--
 src/main/fc/fc_msp.c                  | 10 +++-
 src/main/io/gimbal_serial.c           |  2 +-
 src/main/io/headtracker_msp.c         | 84 +++++++++++++++++++++++++++
 src/main/io/headtracker_msp.h         | 38 ++++++++++++
 src/main/msp/msp_protocol_v2_sensor.h |  3 +-
 src/main/target/common.h              |  2 +
 9 files changed, 153 insertions(+), 12 deletions(-)
 create mode 100644 src/main/io/headtracker_msp.c
 create mode 100644 src/main/io/headtracker_msp.h

diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt
index 7162bc4faa4..2e2b6cb2b20 100755
--- a/src/main/CMakeLists.txt
+++ b/src/main/CMakeLists.txt
@@ -362,6 +362,8 @@ main_sources(COMMON_SRC
     io/frsky_osd.h
     io/gimbal_serial.c
     io/gimbal_serial.h
+    io/headtracker_msp.c
+    io/headtracker_msp.h
     io/osd_dji_hd.c
     io/osd_dji_hd.h
     io/lights.c
diff --git a/src/main/drivers/headtracker_common.c b/src/main/drivers/headtracker_common.c
index 6f10eb1a5a9..ed895e0bc47 100644
--- a/src/main/drivers/headtracker_common.c
+++ b/src/main/drivers/headtracker_common.c
@@ -29,12 +29,13 @@
 #include "common/time.h"
 #include "common/maths.h"
 
+#include "drivers/time.h"
+
 #include "fc/cli.h"
 
 #include "rx/rx.h"
 
 #include "drivers/headtracker_common.h"
-#include "drivers/time.h"
 
 
 PG_REGISTER(headTrackerConfig_t, headTrackerConfig, PG_HEADTRACKER_CONFIG, 0);
@@ -64,7 +65,7 @@ void headTrackerCommonProcess(headTrackerDevice_t *headTrackerDevice, timeUs_t c
     }
 }
 
-headTrackerDevType_e headTrackerCommonGetDeviceType(headTrackerDevice_t *headTrackerDevice)
+headTrackerDevType_e headTrackerCommonGetDeviceType(const headTrackerDevice_t *headTrackerDevice)
 {
     if (!headTrackerDevice || !headTrackerDevice->vTable->getDeviceType) {
         return HEADTRACKER_UNKNOWN;
@@ -73,7 +74,7 @@ headTrackerDevType_e headTrackerCommonGetDeviceType(headTrackerDevice_t *headTra
     return headTrackerDevice->vTable->getDeviceType(headTrackerDevice);
 }
 
-bool headTrackerCommonIsReady(headTrackerDevice_t *headTrackerDevice)
+bool headTrackerCommonIsReady(const headTrackerDevice_t *headTrackerDevice)
 {
     if (headTrackerDevice && headTrackerDevice->vTable->isReady) {
         return headTrackerDevice->vTable->isReady(headTrackerDevice);
@@ -137,7 +138,7 @@ bool headTrackerCommonIsEnabled(void)
     return false;
 }
 
-bool headTrackerCommonIsValid(headTrackerDevice_t *dev) {
+bool headTrackerCommonIsValid(const headTrackerDevice_t *dev) {
     if(dev && dev->vTable && dev->vTable->isValid) {
         return dev->vTable->isValid(dev);
     }
diff --git a/src/main/drivers/headtracker_common.h b/src/main/drivers/headtracker_common.h
index 27b62e0e568..092a00ab0a4 100644
--- a/src/main/drivers/headtracker_common.h
+++ b/src/main/drivers/headtracker_common.h
@@ -23,9 +23,14 @@
 
 #include <stdint.h>
 
-#include "config/feature.h"
 #include "common/time.h"
 
+#include "drivers/time.h"
+
+#include "config/feature.h"
+
+#define MAX_HEADTRACKER_DATA_AGE_US HZ2US(25)
+
 #ifdef __cplusplus
 extern "C" {
 #endif
@@ -60,7 +65,7 @@ typedef struct headTrackerVTable_s {
     int (*getPanPWM)(const headTrackerDevice_t *headTrackerDevice);
     int (*getTiltPWM)(const headTrackerDevice_t *headTrackerDevice);
     int (*getRollPWM)(const headTrackerDevice_t *headTrackerDevice);
-} headtrackerVTable_t;
+} headTrackerVTable_t;
 
 
 typedef struct headTrackerConfig_s {
@@ -75,9 +80,9 @@ headTrackerDevice_t *headTrackerCommonDevice(void);
 
 // VTable functions
 void headTrackerCommonProcess(headTrackerDevice_t *headTrackerDevice, timeUs_t currentTimeUs);
-headTrackerDevType_e headTrackerCommonGetDeviceType(headTrackerDevice_t *headTrackerDevice);
-bool headTrackerCommonIsReady(headTrackerDevice_t *headtrackerDevice);
-bool headTrackerCommonIsValid(headTrackerDevice_t *headtrackerDevice);
+headTrackerDevType_e headTrackerCommonGetDeviceType(const headTrackerDevice_t *headTrackerDevice);
+bool headTrackerCommonIsReady(const headTrackerDevice_t *headtrackerDevice);
+bool headTrackerCommonIsValid(const headTrackerDevice_t *headtrackerDevice);
 int headTrackerCommonGetPanPWM(const headTrackerDevice_t *headTrackerDevice);
 int headTrackerCommonGetTiltPWM(const headTrackerDevice_t *headTrackerDevice);
 int headTrackerCommonGetRollPWM(const headTrackerDevice_t *headTrackerDevice);
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 2109d3e08a9..6362901e45b 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -96,6 +96,7 @@
 #include "io/vtx.h"
 #include "io/vtx_string.h"
 #include "io/gps_private.h"  //for MSP_SIMULATOR
+#include "io/headtracker_msp.h"
 
 #include "io/osd/custom_elements.h"
 
@@ -4048,7 +4049,8 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
 
 static mspResult_e mspProcessSensorCommand(uint16_t cmdMSP, sbuf_t *src)
 {
-    UNUSED(src);
+    int dataSize = sbufBytesRemaining(src);
+    UNUSED(dataSize);
 
     switch (cmdMSP) {
 #if defined(USE_RANGEFINDER_MSP)
@@ -4086,6 +4088,12 @@ static mspResult_e mspProcessSensorCommand(uint16_t cmdMSP, sbuf_t *src)
             mspPitotmeterReceiveNewData(sbufPtr(src));
             break;
 #endif
+
+#if (defined(USE_HEADTRACKER) && defined(USE_HEADTRACKER_MSP))
+        case MSP2_SENSOR_HEADTRACKER:
+            mspHeadTrackerReceiverNewData(sbufPtr(src), dataSize);
+            break;
+#endif
     }
 
     return MSP_RESULT_NO_REPLY;
diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index b49ec75ccf2..77f992b2b8a 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -323,7 +323,7 @@ void gimbalSerialHeadTrackerReceive(uint16_t c, void *data)
 
 #ifdef USE_HEADTRACKER
 
-static headtrackerVTable_t headTrackerVTable = {
+static headTrackerVTable_t headTrackerVTable = {
     .process = headtrackerSerialProcess,
     .getDeviceType = headtrackerSerialGetDeviceType,
     .isReady = headTrackerSerialIsReady,
diff --git a/src/main/io/headtracker_msp.c b/src/main/io/headtracker_msp.c
new file mode 100644
index 00000000000..35757665d5e
--- /dev/null
+++ b/src/main/io/headtracker_msp.c
@@ -0,0 +1,84 @@
+/*
+ * This file is part of INAV.
+ *
+ * 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 INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <platform.h>
+
+#if (defined(USE_HEADTRACKER_MSP) && defined(USE_HEADTRACKER))
+
+#include "build/debug.h"
+
+#include "common/utils.h"
+#include "common/time.h"
+
+#include "drivers/headtracker_common.h"
+
+#include "io/headtracker_msp.h"
+
+/*
+typedef struct headTrackerVTable_s {
+    void (*process)(headTrackerDevice_t *headTrackerDevice, timeUs_t currentTimeUs);
+    headTrackerDevType_e (*getDeviceType)(const headTrackerDevice_t *headTrackerDevice);
+    bool (*isReady)(const headTrackerDevice_t *headTrackerDevice);
+    bool (*isValid)(const headTrackerDevice_t *headTrackerDevice);
+    int (*getPanPWM)(const headTrackerDevice_t *headTrackerDevice);
+    int (*getTiltPWM)(const headTrackerDevice_t *headTrackerDevice);
+    int (*getRollPWM)(const headTrackerDevice_t *headTrackerDevice);
+} headtrackerVTable_t;
+*/
+
+static headTrackerVTable_t headTrackerMspVTable = {
+    .process = NULL,
+    .getDeviceType = NULL,
+    .isReady = NULL,
+    .isValid = headTrackerCommonIsValid,
+    .getPanPWM = headTrackerCommonGetPanPWM,
+    .getTiltPWM = headTrackerCommonGetTiltPWM,
+    .getRollPWM = headTrackerCommonGetRollPWM,
+};
+
+static headTrackerDevice_t headTrackerMspDevice = {
+    .vTable = &headTrackerMspVTable,
+    .pan = 0,
+    .tilt = 0,
+    .roll = 0,
+    .expires = 0,
+};
+
+void mspHeadTrackerInit(void)
+{
+    if(headTrackerConfig()->devType == HEADTRACKER_MSP) {
+        headTrackerCommonSetDevice(&headTrackerMspDevice);
+    }
+}
+
+void mspHeadTrackerReceiverNewData(uint8_t *data, int dataSize)
+{
+    if(dataSize != sizeof(headtrackerMspMessage_t)) {
+        return;
+    }
+
+    headtrackerMspMessage_t *status = (headtrackerMspMessage_t *)data;
+
+    headTrackerMspDevice.pan = status->pan;
+    headTrackerMspDevice.tilt = status->tilt;
+    headTrackerMspDevice.roll = status->roll;
+    headTrackerMspDevice.expires = micros() + MAX_HEADTRACKER_DATA_AGE_US;
+
+    UNUSED(status);
+}
+
+#endif
\ No newline at end of file
diff --git a/src/main/io/headtracker_msp.h b/src/main/io/headtracker_msp.h
new file mode 100644
index 00000000000..8c437314a9e
--- /dev/null
+++ b/src/main/io/headtracker_msp.h
@@ -0,0 +1,38 @@
+/*
+ * This file is part of INAV.
+ *
+ * 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 INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+
+#pragma once
+
+#include <platform.h>
+
+#if (defined(USE_HEADTRACKER_MSP) && defined(USE_HEADTRACKER))
+
+#include <stdint.h>
+#include <unistd.h>
+
+#include "drivers/headtracker_common.h"
+
+typedef struct headtrackerMspMessage_s {
+    int16_t pan;            // -2048~2047. Scale is min/max angle for gimbal
+    int16_t tilt;           // -2048~2047. Scale is min/max angle for gimbal
+    int16_t roll;           // -2048~2047. Scale is min/max angle for gimbal
+    int16_t sensitivity;    // -16~15. Scale is min/max angle for gimbal
+} __attribute__((packed)) headtrackerMspMessage_t;
+
+void mspHeadTrackerReceiverNewData(uint8_t *data, int dataSize);
+#endif
\ No newline at end of file
diff --git a/src/main/msp/msp_protocol_v2_sensor.h b/src/main/msp/msp_protocol_v2_sensor.h
index d913723be7d..b243448e9f9 100644
--- a/src/main/msp/msp_protocol_v2_sensor.h
+++ b/src/main/msp/msp_protocol_v2_sensor.h
@@ -22,4 +22,5 @@
 #define MSP2_SENSOR_GPS             0x1F03
 #define MSP2_SENSOR_COMPASS         0x1F04
 #define MSP2_SENSOR_BAROMETER       0x1F05
-#define MSP2_SENSOR_AIRSPEED        0x1F06
\ No newline at end of file
+#define MSP2_SENSOR_AIRSPEED        0x1F06
+#define MSP2_SENSOR_HEADTRACKER     0x1F07
\ No newline at end of file
diff --git a/src/main/target/common.h b/src/main/target/common.h
index 7f2a8b221f1..8bb735fc437 100644
--- a/src/main/target/common.h
+++ b/src/main/target/common.h
@@ -192,6 +192,8 @@
 
 #define USE_SERIAL_GIMBAL
 #define USE_HEADTRACKER
+#define USE_HEADTRACKER_SERIAL
+#define USE_HEADTRACKER_MSP
 
 //Designed to free space of F722 and F411 MCUs
 #if (MCU_FLASH_SIZE > 512)

From ab7f14c2c1da7a83a8f965f46bed462c882133c2 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Tue, 11 Jun 2024 18:41:38 +0200
Subject: [PATCH 217/323] Add init() call for msp head tracker

---
 src/main/fc/fc_init.c         | 6 +++++-
 src/main/io/gimbal_serial.c   | 5 +++--
 src/main/io/gimbal_serial.h   | 3 +--
 src/main/io/headtracker_msp.h | 3 +++
 4 files changed, 12 insertions(+), 5 deletions(-)

diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index b57a60a9f69..e9ed6efe4c1 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -110,6 +110,7 @@
 #include "io/displayport_srxl.h"
 #include "io/flashfs.h"
 #include "io/gimbal_serial.h"
+#include "io/headtracker_msp.h"
 #include "io/gps.h"
 #include "io/ledstrip.h"
 #include "io/osd.h"
@@ -698,10 +699,13 @@ void init(void)
 
 #ifdef USE_HEADTRACKER
     headTrackerCommonInit();
-#ifdef USE_SERIAL_GIMBAL
+#ifdef USE_HEADTRACKER_SERIAL
     // Needs to be called after gimbalSerialInit
     gimbalSerialHeadTrackerInit();
 #endif
+#ifdef USE_HEADTRACKER_MSP
+    mspHeadTrackerInit();
+#endif
 #endif
 
     // Latch active features AGAIN since some may be modified by init().
diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index 77f992b2b8a..ee0721b81bf 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -91,7 +91,7 @@ bool gimbalSerialHasHeadTracker(const gimbalDevice_t *gimbalDevice)
 
 bool gimbalSerialInit(void)
 {
-    if(gimbalSerialDetect()) {
+    if (gimbalSerialDetect()) {
         SD(fprintf(stderr, "Setting gimbal device\n"));
         gimbalCommonSetDevice(&serialGimbalDevice);
         return true;
@@ -108,6 +108,7 @@ bool gimbalSerialDetect(void)
 #else
 bool gimbalSerialDetect(void)
 {
+    
     SD(fprintf(stderr, "[GIMBAL]: serial Detect...\n"));
     serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_GIMBAL);
     bool singleUart = gimbalSerialConfig()->singleUart;
@@ -321,7 +322,7 @@ void gimbalSerialHeadTrackerReceive(uint16_t c, void *data)
     }
 }
 
-#ifdef USE_HEADTRACKER
+#if (defined(USE_HEADTRACKER) && defined(USE_HEADTRACKER_SERIAL))
 
 static headTrackerVTable_t headTrackerVTable = {
     .process = headtrackerSerialProcess,
diff --git a/src/main/io/gimbal_serial.h b/src/main/io/gimbal_serial.h
index 7e342994a38..4f7de6753b7 100644
--- a/src/main/io/gimbal_serial.h
+++ b/src/main/io/gimbal_serial.h
@@ -31,7 +31,6 @@ extern "C" {
 
 #ifdef USE_SERIAL_GIMBAL
 
-#define MAX_HEADTRACKER_DATA_AGE_US HZ2US(25)
 
 #define HTKATTITUDE_SYNC0  0xA5
 #define HTKATTITUDE_SYNC1  0x5A
@@ -87,7 +86,7 @@ bool gimbalSerialHasHeadTracker(const gimbalDevice_t *gimbalDevice);
 void gimbalSerialHeadTrackerReceive(uint16_t c, void *data);
 
 
-#ifdef USE_HEADTRACKER
+#if (defined(USE_HEADTRACKER) && defined(USE_HEADTRACKER_SERIAL))
 bool gimbalSerialHeadTrackerInit(void);
 bool gimbalSerialHeadTrackerDetect(void);
 void headtrackerSerialProcess(headTrackerDevice_t *headTrackerDevice, timeUs_t currentTimeUs);
diff --git a/src/main/io/headtracker_msp.h b/src/main/io/headtracker_msp.h
index 8c437314a9e..4552bf1ea1b 100644
--- a/src/main/io/headtracker_msp.h
+++ b/src/main/io/headtracker_msp.h
@@ -34,5 +34,8 @@ typedef struct headtrackerMspMessage_s {
     int16_t sensitivity;    // -16~15. Scale is min/max angle for gimbal
 } __attribute__((packed)) headtrackerMspMessage_t;
 
+void mspHeadTrackerInit(void);
+
 void mspHeadTrackerReceiverNewData(uint8_t *data, int dataSize);
+
 #endif
\ No newline at end of file

From 5b16cdeb6ffe47bcaa6c7089ce4d34f523e8a4f4 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Tue, 11 Jun 2024 19:32:38 +0200
Subject: [PATCH 218/323] Add config reset functions

---
 src/main/drivers/headtracker_common.c | 13 ++++++++++---
 src/main/io/gimbal_serial.c           | 12 +++++++++++-
 2 files changed, 21 insertions(+), 4 deletions(-)

diff --git a/src/main/drivers/headtracker_common.c b/src/main/drivers/headtracker_common.c
index ed895e0bc47..d73aceaeae6 100644
--- a/src/main/drivers/headtracker_common.c
+++ b/src/main/drivers/headtracker_common.c
@@ -22,10 +22,13 @@
 #include <stdio.h>
 #include <stdint.h>
 #include <stdlib.h>
+#include <string.h>
 
 #include <build/debug.h>
 #include <config/parameter_group_ids.h>
 
+#include "settings_generated.h"
+
 #include "common/time.h"
 #include "common/maths.h"
 
@@ -37,12 +40,16 @@
 
 #include "drivers/headtracker_common.h"
 
-
-PG_REGISTER(headTrackerConfig_t, headTrackerConfig, PG_HEADTRACKER_CONFIG, 0);
-
+PG_REGISTER_WITH_RESET_FN(headTrackerConfig_t, headTrackerConfig, PG_HEADTRACKER_CONFIG, 0);
 
 static headTrackerDevice_t *commonHeadTrackerDevice = NULL;
 
+void pgResetFn_headTrackerConfig(headTrackerConfig_t *conf)
+{
+    memset(conf, 0, sizeof(headTrackerConfig_t));
+    conf->devType = SETTING_HEADTRACKER_TYPE_DEFAULT;
+}
+
 void headTrackerCommonInit(void)
 {
 }
diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index ee0721b81bf..da1f976fc9d 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -18,6 +18,7 @@
 #include <stdio.h>
 #include <stdint.h>
 #include <unistd.h>
+#include <string.h>
 
 #include "platform.h"
 
@@ -41,7 +42,9 @@
 
 #include <config/parameter_group_ids.h>
 
-PG_REGISTER(gimbalSerialConfig_t, gimbalSerialConfig, PG_GIMBAL_SERIAL_CONFIG, 0);
+#include "settings_generated.h"
+
+PG_REGISTER_WITH_RESET_FN(gimbalSerialConfig_t, gimbalSerialConfig, PG_GIMBAL_SERIAL_CONFIG, 0);
 
 STATIC_ASSERT(sizeof(gimbalHtkAttitudePkt_t) == 10, gimbalHtkAttitudePkt_t_size_not_10);
 
@@ -72,6 +75,13 @@ static gimbalDevice_t serialGimbalDevice = {
     .vTable = &gimbalSerialVTable
 };
 
+void pgResetFn_gimbalSerialConfig(gimbalSerialConfig_t *conf)
+{
+    memset(conf, 0, sizeof(gimbalSerialConfig));
+    conf->singleUart = SETTING_GIMBAL_SERIAL_SINGLE_UART_DEFAULT;
+}
+
+
 gimbalDevType_e gimbalSerialGetDeviceType(const gimbalDevice_t *gimbalDevice)
 {
     UNUSED(gimbalDevice);

From 7405383aed844aa1be86a14519d20c01a977218d Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Tue, 11 Jun 2024 19:51:59 +0100
Subject: [PATCH 219/323] fix time settings

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

diff --git a/docs/Settings.md b/docs/Settings.md
index a6cd597ef17..08a811debb3 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -3584,7 +3584,7 @@ Multicopter hover throttle hint for altitude controller. Should be set to approx
 
 ### nav_mc_inverted_crash_detection
 
-Setting a value > 0 enables inverted crash detection for multirotors. It is intended for situations where the multirotor has crashed inverted on the ground and can't be manually disarmed due to loss of control. When enabled the setting defines the additional number of seconds before disarm beyond a minimum fixed time delay such that the minimum possible time delay before disarm is 3s.
+Setting a value > 0 enables inverted crash detection for multirotors. It is intended for situations where the multirotor has crashed inverted on the ground and can't be manually disarmed due to loss of control or some other reason. When enabled the setting defines the additional number of seconds before disarm beyond a minimum fixed time delay such that the minimum possible time delay before disarm is 4s.
 
 | Default | Min | Max |
 | --- | --- | --- |
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 2dbda7569bb..068ddf63af4 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -2416,7 +2416,7 @@ groups:
         field: general.flags.landing_bump_detection
         type: bool
       - name: nav_mc_inverted_crash_detection
-        description: "Setting a value > 0 enables inverted crash detection for multirotors. It is intended for situations where the multirotor has crashed inverted on the ground and can't be manually disarmed due to loss of control. When enabled the setting defines the additional number of seconds before disarm beyond a minimum fixed time delay such that the minimum possible time delay before disarm is 3s."
+        description: "Setting a value > 0 enables inverted crash detection for multirotors. It is intended for situations where the multirotor has crashed inverted on the ground and can't be manually disarmed due to loss of control or some other reason. When enabled the setting defines the additional number of seconds before disarm beyond a minimum fixed time delay such that the minimum possible time delay before disarm is 4s."
         default_value: 0
         field: mc.inverted_crash_detection
         min: 0
diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c
index 169876d240a..41903ff5f3f 100644
--- a/src/main/navigation/navigation_multicopter.c
+++ b/src/main/navigation/navigation_multicopter.c
@@ -808,7 +808,7 @@ bool isMulticopterCrashedInverted(void)
             return false;
         } else if (ABS(initialAltitude - navGetCurrentActualPositionAndVelocity()->pos.z) < 200) {
             /* Check altitude change < 2m during disarm timeout period indicating MR not in flight.
-             * Minimum 3s disarm delay + extra user set delay time = Min time of 5s given min user setting is 1s if enabled */
+             * Minimum 3s disarm delay + extra user set delay time = Min time of 4s given min user setting is 1s if enabled */
 
             uint16_t disarmTimeDelay = 3000 + S2MS(navConfig()->mc.inverted_crash_detection);
             return millis() - startTime > disarmTimeDelay;

From 58faed455603d7c8bcc7c8744146ef0808d58df5 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Tue, 11 Jun 2024 22:07:56 +0200
Subject: [PATCH 220/323] Min and max values made defaults invalid

Old values failed checks for +/-5Hz from main gyro lpf frequency
---
 docs/Settings.md          | 4 ++--
 src/main/fc/settings.yaml | 6 +++---
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index 7177d4a8ed1..2367cc8f1ab 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -1618,7 +1618,7 @@ Maximum frequency for adaptive filter
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 150 | 100 | 500 |
+| 150 | 0 | 505 |
 
 ---
 
@@ -1628,7 +1628,7 @@ Minimum frequency for adaptive filter
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 50 | 30 | 250 |
+| 50 | 0 | 250 |
 
 ---
 
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 886fbfad5f4..cfe40bb400f 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -344,15 +344,15 @@ groups:
         description: "Minimum frequency for adaptive filter"
         default_value: 50
         field: adaptiveFilterMinHz
-        min: 30
+        min: 0
         max: 250
         condition: USE_ADAPTIVE_FILTER
       - name: gyro_adaptive_filter_max_hz
         description: "Maximum frequency for adaptive filter"
         default_value: 150
         field: adaptiveFilterMaxHz
-        min: 100
-        max: 500
+        min: 0
+        max: 505
         condition: USE_ADAPTIVE_FILTER
       - name: gyro_adaptive_filter_std_lpf_hz
         description: "Standard deviation low pass filter cutoff frequency"

From 2d2eab8b6cc8d8474c74362f4966a8d6358635f3 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Tue, 11 Jun 2024 23:30:25 +0200
Subject: [PATCH 221/323] Use reset template

---
 src/main/drivers/headtracker_common.c | 10 ++++------
 src/main/io/gimbal_serial.c           | 10 ++++------
 2 files changed, 8 insertions(+), 12 deletions(-)

diff --git a/src/main/drivers/headtracker_common.c b/src/main/drivers/headtracker_common.c
index d73aceaeae6..271bb25f8c2 100644
--- a/src/main/drivers/headtracker_common.c
+++ b/src/main/drivers/headtracker_common.c
@@ -40,15 +40,13 @@
 
 #include "drivers/headtracker_common.h"
 
-PG_REGISTER_WITH_RESET_FN(headTrackerConfig_t, headTrackerConfig, PG_HEADTRACKER_CONFIG, 0);
+PG_REGISTER_WITH_RESET_TEMPLATE(headTrackerConfig_t, headTrackerConfig, PG_HEADTRACKER_CONFIG, 0);
 
 static headTrackerDevice_t *commonHeadTrackerDevice = NULL;
 
-void pgResetFn_headTrackerConfig(headTrackerConfig_t *conf)
-{
-    memset(conf, 0, sizeof(headTrackerConfig_t));
-    conf->devType = SETTING_HEADTRACKER_TYPE_DEFAULT;
-}
+PG_RESET_TEMPLATE(headTrackerConfig_t, headTrackerConfig,
+    .devType = SETTING_HEADTRACKER_TYPE_DEFAULT
+);
 
 void headTrackerCommonInit(void)
 {
diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index da1f976fc9d..03a44c02879 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -44,7 +44,7 @@
 
 #include "settings_generated.h"
 
-PG_REGISTER_WITH_RESET_FN(gimbalSerialConfig_t, gimbalSerialConfig, PG_GIMBAL_SERIAL_CONFIG, 0);
+PG_REGISTER_WITH_RESET_TEMPLATE(gimbalSerialConfig_t, gimbalSerialConfig, PG_GIMBAL_SERIAL_CONFIG, 0);
 
 STATIC_ASSERT(sizeof(gimbalHtkAttitudePkt_t) == 10, gimbalHtkAttitudePkt_t_size_not_10);
 
@@ -75,11 +75,9 @@ static gimbalDevice_t serialGimbalDevice = {
     .vTable = &gimbalSerialVTable
 };
 
-void pgResetFn_gimbalSerialConfig(gimbalSerialConfig_t *conf)
-{
-    memset(conf, 0, sizeof(gimbalSerialConfig));
-    conf->singleUart = SETTING_GIMBAL_SERIAL_SINGLE_UART_DEFAULT;
-}
+PG_RESET_TEMPLATE(gimbalSerialConfig_t, gimbalSerialConfig,
+    .singleUart = SETTING_GIMBAL_SERIAL_SINGLE_UART_DEFAULT
+);
 
 
 gimbalDevType_e gimbalSerialGetDeviceType(const gimbalDevice_t *gimbalDevice)

From 6893844bb01595f08f4a770f358d06cd8f03e4c2 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Tue, 11 Jun 2024 23:31:56 +0200
Subject: [PATCH 222/323] group reset template

---
 src/main/drivers/headtracker_common.c | 4 ++--
 src/main/io/gimbal_serial.c           | 9 ++++-----
 2 files changed, 6 insertions(+), 7 deletions(-)

diff --git a/src/main/drivers/headtracker_common.c b/src/main/drivers/headtracker_common.c
index 271bb25f8c2..670a412c501 100644
--- a/src/main/drivers/headtracker_common.c
+++ b/src/main/drivers/headtracker_common.c
@@ -42,12 +42,12 @@
 
 PG_REGISTER_WITH_RESET_TEMPLATE(headTrackerConfig_t, headTrackerConfig, PG_HEADTRACKER_CONFIG, 0);
 
-static headTrackerDevice_t *commonHeadTrackerDevice = NULL;
-
 PG_RESET_TEMPLATE(headTrackerConfig_t, headTrackerConfig,
     .devType = SETTING_HEADTRACKER_TYPE_DEFAULT
 );
 
+static headTrackerDevice_t *commonHeadTrackerDevice = NULL;
+
 void headTrackerCommonInit(void)
 {
 }
diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index 03a44c02879..90216ea8c60 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -46,6 +46,10 @@
 
 PG_REGISTER_WITH_RESET_TEMPLATE(gimbalSerialConfig_t, gimbalSerialConfig, PG_GIMBAL_SERIAL_CONFIG, 0);
 
+PG_RESET_TEMPLATE(gimbalSerialConfig_t, gimbalSerialConfig,
+    .singleUart = SETTING_GIMBAL_SERIAL_SINGLE_UART_DEFAULT
+);
+
 STATIC_ASSERT(sizeof(gimbalHtkAttitudePkt_t) == 10, gimbalHtkAttitudePkt_t_size_not_10);
 
 #define GIMBAL_SERIAL_BUFFER_SIZE 512
@@ -75,11 +79,6 @@ static gimbalDevice_t serialGimbalDevice = {
     .vTable = &gimbalSerialVTable
 };
 
-PG_RESET_TEMPLATE(gimbalSerialConfig_t, gimbalSerialConfig,
-    .singleUart = SETTING_GIMBAL_SERIAL_SINGLE_UART_DEFAULT
-);
-
-
 gimbalDevType_e gimbalSerialGetDeviceType(const gimbalDevice_t *gimbalDevice)
 {
     UNUSED(gimbalDevice);

From 2366b0e418e0f10a4e2b1bcb80d367f418f10217 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Wed, 12 Jun 2024 00:15:24 +0200
Subject: [PATCH 223/323] Add headtracker to SITL

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

diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h
index 4afab35aa4d..f6d80f74971 100644
--- a/src/main/target/SITL/target.h
+++ b/src/main/target/SITL/target.h
@@ -75,6 +75,9 @@
 #define USE_MSP_OSD
 #define USE_OSD
 #define USE_SERIAL_GIMBAL
+#define USE_HEADTRACKER
+#define USE_HEADTRACKER_SERIAL
+#define USE_HEADTRACKER_MSP
 
 #undef USE_DASHBOARD
 

From 1123e59198b9af28c0aabebb10d476cca8f1a1fc Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Wed, 12 Jun 2024 09:30:48 +0200
Subject: [PATCH 224/323] Enable adaptive filter only when min and max are set

---
 src/main/fc/fc_tasks.c | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index 0fd239127c2..1230f6f01d7 100755
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -428,7 +428,11 @@ void fcTasksInit(void)
 #endif
 
 #ifdef USE_ADAPTIVE_FILTER
-    setTaskEnabled(TASK_ADAPTIVE_FILTER, (gyroConfig()->gyroFilterMode == GYRO_FILTER_MODE_ADAPTIVE));
+    setTaskEnabled(TASK_ADAPTIVE_FILTER, (
+        gyroConfig()->gyroFilterMode == GYRO_FILTER_MODE_ADAPTIVE && 
+        gyroConfig()->adaptiveFilterMinHz > 0 && 
+        gyroConfig()->adaptiveFilterMaxHz > 0
+    ));
 #endif
 
 #if defined(SITL_BUILD)

From 5a50f95f0822f8f7da67be77ee24b6468fef0b5e Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Wed, 12 Jun 2024 14:47:44 +0200
Subject: [PATCH 225/323] Increase max number of pwm outputs by 1, if a LED pin
 is defined

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

diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c
index e953c86b96e..5ed17d7bb9a 100644
--- a/src/main/drivers/pwm_mapping.c
+++ b/src/main/drivers/pwm_mapping.c
@@ -53,8 +53,8 @@ enum {
 typedef struct {
     int maxTimMotorCount;
     int maxTimServoCount;
-    const timerHardware_t * timMotors[MAX_PWM_OUTPUT_PORTS];
-    const timerHardware_t * timServos[MAX_PWM_OUTPUT_PORTS];
+    const timerHardware_t * timMotors[MAX_PWM_OUTPUTS];
+    const timerHardware_t * timServos[MAX_PWM_OUTPUTS];
 } timMotorServoHardware_t;
 
 static pwmInitError_e pwmInitError = PWM_INIT_ERROR_NONE;
diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c
index d48f38679e9..faa9cd373d9 100644
--- a/src/main/drivers/pwm_output.c
+++ b/src/main/drivers/pwm_output.c
@@ -98,7 +98,7 @@ typedef struct {
     bool                requestTelemetry;
 } pwmOutputMotor_t;
 
-static DMA_RAM pwmOutputPort_t pwmOutputPorts[MAX_PWM_OUTPUT_PORTS];
+static DMA_RAM pwmOutputPort_t pwmOutputPorts[MAX_PWM_OUTPUTS];
 
 static pwmOutputMotor_t        motors[MAX_MOTORS];
 static motorPwmProtocolTypes_e initMotorProtocol;
@@ -142,7 +142,7 @@ static void pwmOutConfigTimer(pwmOutputPort_t * p, TCH_t * tch, uint32_t hz, uin
 
 static pwmOutputPort_t *pwmOutAllocatePort(void)
 {
-    if (allocatedOutputPortCount >= MAX_PWM_OUTPUT_PORTS) {
+    if (allocatedOutputPortCount >= MAX_PWM_OUTPUTS) {
         LOG_ERROR(PWM, "Attempt to allocate PWM output beyond MAX_PWM_OUTPUT_PORTS");
         return NULL;
     }
diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h
index b3c0fa6be0e..1041ace04fa 100644
--- a/src/main/drivers/pwm_output.h
+++ b/src/main/drivers/pwm_output.h
@@ -20,6 +20,12 @@
 #include "drivers/io_types.h"
 #include "drivers/time.h"
 
+
+#if defined(WS2811_PIN)
+#define MAX_PWM_OUTPUTS (MAX_PWM_OUTPUT_PORTS + 1)
+#else
+#define MAX_PWM_OUTPUTS (MAX_PWM_OUTPUT_PORTS)
+#endif
 typedef enum {
     DSHOT_CMD_SPIN_DIRECTION_NORMAL = 20,
     DSHOT_CMD_SPIN_DIRECTION_REVERSED = 21,

From 1c6499354edc1cf18a5c58b62da4f09a329c740b Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Wed, 12 Jun 2024 16:48:11 +0200
Subject: [PATCH 226/323] We already support 4 page fonts

bump max number of chars for displayport_msp
---
 src/main/io/displayport_msp_osd.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/io/displayport_msp_osd.c b/src/main/io/displayport_msp_osd.c
index 2e425e6aecb..0b9be8c6d86 100644
--- a/src/main/io/displayport_msp_osd.c
+++ b/src/main/io/displayport_msp_osd.c
@@ -368,7 +368,7 @@ static uint32_t txBytesFree(const displayPort_t *displayPort)
 static bool getFontMetadata(displayFontMetadata_t *metadata, const displayPort_t *displayPort)
 {
     UNUSED(displayPort);
-    metadata->charCount = 512;
+    metadata->charCount = 1024;
     metadata->version = FONT_VERSION;
     return true;
 }

From ff137b73e87002f771b6c1f69eba5304bace6b06 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Wed, 12 Jun 2024 17:32:54 +0200
Subject: [PATCH 227/323] Add -j4 to allow parallel builds.

Based on info available at https://docs.github.com/en/actions/using-github-hosted-runners/about-github-hosted-runners/about-github-hosted-runners#standard-github-hosted-runners-for-public-repositories
---
 .github/workflows/ci.yml | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index 111a0cc35c0..6129d4b4da8 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -36,7 +36,7 @@ jobs:
           path: downloads
           key: ${{ runner.os }}-downloads-${{ hashFiles('CMakeLists.txt') }}-${{ hashFiles('**/cmake/*')}}
       - name: Build targets (${{ matrix.id }})
-        run: mkdir -p build && cd build && cmake -DWARNINGS_AS_ERRORS=ON -DCI_JOB_INDEX=${{ matrix.id }} -DCI_JOB_COUNT=${{ strategy.job-total }} -DBUILD_SUFFIX=${{ env.BUILD_SUFFIX }} -G Ninja .. && ninja ci
+        run: mkdir -p build && cd build && cmake -DWARNINGS_AS_ERRORS=ON -DCI_JOB_INDEX=${{ matrix.id }} -DCI_JOB_COUNT=${{ strategy.job-total }} -DBUILD_SUFFIX=${{ env.BUILD_SUFFIX }} -G Ninja .. && ninja -j4 ci
       - name: Upload artifacts
         uses: actions/upload-artifact@v3
         with:
@@ -66,7 +66,7 @@ jobs:
           echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
           echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
       - name: Build SITL
-        run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja
+        run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja -j4
       - name: Upload artifacts
         uses: actions/upload-artifact@v3
         with:
@@ -101,7 +101,7 @@ jobs:
         run: |
           mkdir -p build_SITL && cd build_SITL
           cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -DCMAKE_OSX_ARCHITECTURES="arm64;x86_64" -G Ninja ..
-          ninja
+          ninja -j3
 
       - name: Upload artifacts
         uses: actions/upload-artifact@v3
@@ -137,7 +137,7 @@ jobs:
           echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
           echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
       - name: Build SITL
-        run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja
+        run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja -j4
       - name: Upload artifacts
         uses: actions/upload-artifact@v3
         with:

From 2e6fa1593b4e03a0c66d3faf442f361cfe3f1876 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Wed, 12 Jun 2024 17:50:33 +0200
Subject: [PATCH 228/323] Switch to make, ninja seems ot be ignoring the -j4

---
 .github/workflows/ci.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index 6129d4b4da8..4cf4d7f68ea 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -36,7 +36,7 @@ jobs:
           path: downloads
           key: ${{ runner.os }}-downloads-${{ hashFiles('CMakeLists.txt') }}-${{ hashFiles('**/cmake/*')}}
       - name: Build targets (${{ matrix.id }})
-        run: mkdir -p build && cd build && cmake -DWARNINGS_AS_ERRORS=ON -DCI_JOB_INDEX=${{ matrix.id }} -DCI_JOB_COUNT=${{ strategy.job-total }} -DBUILD_SUFFIX=${{ env.BUILD_SUFFIX }} -G Ninja .. && ninja -j4 ci
+        run: mkdir -p build && cd build && cmake -DWARNINGS_AS_ERRORS=ON -DCI_JOB_INDEX=${{ matrix.id }} -DCI_JOB_COUNT=${{ strategy.job-total }} -DBUILD_SUFFIX=${{ env.BUILD_SUFFIX }} .. && make -j4 ci
       - name: Upload artifacts
         uses: actions/upload-artifact@v3
         with:

From dad4c304c746455c420630d102bae55b12850fb5 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Wed, 12 Jun 2024 18:12:03 +0200
Subject: [PATCH 229/323] Try -pipe

---
 .github/workflows/ci.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index 4cf4d7f68ea..8a4c1bb4748 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -36,7 +36,7 @@ jobs:
           path: downloads
           key: ${{ runner.os }}-downloads-${{ hashFiles('CMakeLists.txt') }}-${{ hashFiles('**/cmake/*')}}
       - name: Build targets (${{ matrix.id }})
-        run: mkdir -p build && cd build && cmake -DWARNINGS_AS_ERRORS=ON -DCI_JOB_INDEX=${{ matrix.id }} -DCI_JOB_COUNT=${{ strategy.job-total }} -DBUILD_SUFFIX=${{ env.BUILD_SUFFIX }} .. && make -j4 ci
+        run: mkdir -p build && cd build && cmake -DWARNINGS_AS_ERRORS=ON -DCI_JOB_INDEX=${{ matrix.id }} -DCI_JOB_COUNT=${{ strategy.job-total }} -DBUILD_SUFFIX=${{ env.BUILD_SUFFIX }} -DMAIN_COMPILE_OPTIONS=-pipe .. && make -j4 ci
       - name: Upload artifacts
         uses: actions/upload-artifact@v3
         with:

From 75d85eb49525c8f01a120b601f55aed7b46d74bb Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Wed, 12 Jun 2024 18:20:21 +0200
Subject: [PATCH 230/323] Back to ninja, bump number of runners

---
 .github/workflows/ci.yml | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index 8a4c1bb4748..272918902fd 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -9,7 +9,7 @@ jobs:
     runs-on: ubuntu-latest
     strategy:
         matrix:
-          id: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9]
+          id: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14]
 
     steps:
       - uses: actions/checkout@v3
@@ -36,7 +36,7 @@ jobs:
           path: downloads
           key: ${{ runner.os }}-downloads-${{ hashFiles('CMakeLists.txt') }}-${{ hashFiles('**/cmake/*')}}
       - name: Build targets (${{ matrix.id }})
-        run: mkdir -p build && cd build && cmake -DWARNINGS_AS_ERRORS=ON -DCI_JOB_INDEX=${{ matrix.id }} -DCI_JOB_COUNT=${{ strategy.job-total }} -DBUILD_SUFFIX=${{ env.BUILD_SUFFIX }} -DMAIN_COMPILE_OPTIONS=-pipe .. && make -j4 ci
+        run: mkdir -p build && cd build && cmake -DWARNINGS_AS_ERRORS=ON -DCI_JOB_INDEX=${{ matrix.id }} -DCI_JOB_COUNT=${{ strategy.job-total }} -DBUILD_SUFFIX=${{ env.BUILD_SUFFIX }} -DMAIN_COMPILE_OPTIONS=-pipe -G Ninja .. && ninja -j4 ci
       - name: Upload artifacts
         uses: actions/upload-artifact@v3
         with:

From 3da647d6dd4467d32e29937db74ea5d7f0821d90 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Wed, 12 Jun 2024 18:29:31 +0200
Subject: [PATCH 231/323] Tests don't really depend on build.

---
 .github/workflows/ci.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index 272918902fd..187150695ac 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -146,7 +146,7 @@ jobs:
 
 
   test:
-    needs: [build]
+    #needs: [build]
     runs-on: ubuntu-latest
     steps:
       - uses: actions/checkout@v3

From 0e5f7716acd7e495acec6ff8f33dbf99fb4e87e5 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Wed, 12 Jun 2024 18:38:49 +0200
Subject: [PATCH 232/323] Bump parallel task once more

---
 .github/workflows/ci.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index 187150695ac..b98e46ec570 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -9,7 +9,7 @@ jobs:
     runs-on: ubuntu-latest
     strategy:
         matrix:
-          id: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14]
+          id: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19]
 
     steps:
       - uses: actions/checkout@v3

From 556a0fadd9dd21b2ef7eb4027841b7199db8ac9d Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Wed, 12 Jun 2024 18:48:36 +0200
Subject: [PATCH 233/323] 18 jobs.

---
 .github/workflows/ci.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index b98e46ec570..019d6b87271 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -9,7 +9,7 @@ jobs:
     runs-on: ubuntu-latest
     strategy:
         matrix:
-          id: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19]
+          id: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17]
 
     steps:
       - uses: actions/checkout@v3

From bbad573eff936fdbad3a4d687d301fbeeaae1e48 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Wed, 12 Jun 2024 18:51:42 +0200
Subject: [PATCH 234/323] Strange things happening

---
 .github/workflows/ci.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index 019d6b87271..0330041607d 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -9,7 +9,7 @@ jobs:
     runs-on: ubuntu-latest
     strategy:
         matrix:
-          id: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17]
+          id: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16]
 
     steps:
       - uses: actions/checkout@v3

From 2e69f3746e591f10e31fffb53c69e4c6afb4c256 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Wed, 12 Jun 2024 19:41:04 +0200
Subject: [PATCH 235/323] Update actions

---
 .github/workflows/ci.yml | 20 ++++++++++----------
 1 file changed, 10 insertions(+), 10 deletions(-)

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index 111a0cc35c0..481e6ad2858 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -12,7 +12,7 @@ jobs:
           id: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9]
 
     steps:
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
       - name: Install dependencies
         run: sudo apt-get update && sudo apt-get -y install ninja-build
       - name: Setup environment
@@ -38,7 +38,7 @@ jobs:
       - name: Build targets (${{ matrix.id }})
         run: mkdir -p build && cd build && cmake -DWARNINGS_AS_ERRORS=ON -DCI_JOB_INDEX=${{ matrix.id }} -DCI_JOB_COUNT=${{ strategy.job-total }} -DBUILD_SUFFIX=${{ env.BUILD_SUFFIX }} -G Ninja .. && ninja ci
       - name: Upload artifacts
-        uses: actions/upload-artifact@v3
+        uses: actions/upload-artifact@v4
         with:
           name: ${{ env.BUILD_NAME }}
           path: ./build/*.hex
@@ -46,7 +46,7 @@ jobs:
   build-SITL-Linux:
     runs-on: ubuntu-latest
     steps:
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
       - name: Install dependencies
         run: sudo apt-get update && sudo apt-get -y install ninja-build
       - name: Setup environment
@@ -68,15 +68,15 @@ jobs:
       - name: Build SITL
         run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja
       - name: Upload artifacts
-        uses: actions/upload-artifact@v3
+        uses: actions/upload-artifact@v4
         with:
-          name: ${{ env.BUILD_NAME }}_SITL
+          name: ${{ env.BUILD_NAME }}_SITL-Linux
           path: ./build_SITL/*_SITL
 
   build-SITL-Mac:
     runs-on: macos-latest
     steps:
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
       - name: Install dependencies
         run: |
           brew install cmake ninja ruby
@@ -104,7 +104,7 @@ jobs:
           ninja
 
       - name: Upload artifacts
-        uses: actions/upload-artifact@v3
+        uses: actions/upload-artifact@v4
         with:
           name: ${{ env.BUILD_NAME }}_SITL-MacOS
           path: ./build_SITL/*_SITL
@@ -115,7 +115,7 @@ jobs:
       run:
         shell: C:\tools\cygwin\bin\bash.exe -o igncr '{0}'
     steps:
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
       - name: Setup Cygwin
         uses: egor-tensin/setup-cygwin@v4
         with:
@@ -139,7 +139,7 @@ jobs:
       - name: Build SITL
         run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja
       - name: Upload artifacts
-        uses: actions/upload-artifact@v3
+        uses: actions/upload-artifact@v4
         with:
           name: ${{ env.BUILD_NAME }}_SITL-WIN
           path: ./build_SITL/*.exe
@@ -149,7 +149,7 @@ jobs:
     needs: [build]
     runs-on: ubuntu-latest
     steps:
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
       - name: Install dependencies
         run: sudo apt-get update && sudo apt-get -y install ninja-build
       - name: Run Tests

From 6250e0bd94137e2fe4f1ef11eaa120d934f187a3 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Wed, 12 Jun 2024 19:43:15 +0200
Subject: [PATCH 236/323] Update cache action

---
 .github/workflows/ci.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index 481e6ad2858..9caa077b82e 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -31,7 +31,7 @@ jobs:
           VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }')
           echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
           echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
-      - uses: actions/cache@v1
+      - uses: actions/cache@v4
         with:
           path: downloads
           key: ${{ runner.os }}-downloads-${{ hashFiles('CMakeLists.txt') }}-${{ hashFiles('**/cmake/*')}}

From 31208fe2f7edeab43644a588e15f48df20f9d49b Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Wed, 12 Jun 2024 19:44:35 +0200
Subject: [PATCH 237/323] Update checkout action version

---
 .github/workflows/docs.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/docs.yml b/.github/workflows/docs.yml
index 624a129c35d..edaaecdf3bb 100644
--- a/.github/workflows/docs.yml
+++ b/.github/workflows/docs.yml
@@ -14,7 +14,7 @@ jobs:
     runs-on: ubuntu-latest
 
     steps:
-      - uses: actions/checkout@v3
+      - uses: actions/checkout@v4
       - name: Install dependencies
         run: sudo apt-get update && sudo apt-get -y install python3-yaml
       - name: Check that Settings.md is up to date

From df9805236886d68d889679712b491b3f50ed7222 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Wed, 12 Jun 2024 20:10:32 +0200
Subject: [PATCH 238/323] Break .zip file by matrix id, for now.

---
 .github/workflows/ci.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index 4c989894188..c389884008d 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -40,7 +40,7 @@ jobs:
       - name: Upload artifacts
         uses: actions/upload-artifact@v4
         with:
-          name: ${{ env.BUILD_NAME }}
+          name: ${{ env.BUILD_NAME }}.${{ matrix.id }}
           path: ./build/*.hex
 
   build-SITL-Linux:

From af6f1b798f5b1007d9e357a20c234161ea769f43 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Wed, 12 Jun 2024 20:55:34 +0200
Subject: [PATCH 239/323] Attempt at building master on sucessfull push

---
 .github/workflows/dev-builds.yml | 207 +++++++++++++++++++++++++++++++
 1 file changed, 207 insertions(+)
 create mode 100644 .github/workflows/dev-builds.yml

diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml
new file mode 100644
index 00000000000..81551a3a330
--- /dev/null
+++ b/.github/workflows/dev-builds.yml
@@ -0,0 +1,207 @@
+name: Build pre-release
+# Don't enable CI on push, just on PR. If you
+# are working on the main repo and want to trigger
+# a CI build submit a draft PR.
+on: 
+  push:
+    branches:
+    - main
+      master
+      mmosca-update-github-actions
+
+jobs:
+  build:
+    runs-on: ubuntu-latest
+    strategy:
+        matrix:
+          id: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16]
+
+    steps:
+      - uses: actions/checkout@v4
+      - name: Install dependencies
+        run: sudo apt-get update && sudo apt-get -y install ninja-build
+      - name: Setup environment
+        env:
+          ACTIONS_ALLOW_UNSECURE_COMMANDS: true
+        run: |
+          # This is the hash of the commit for the PR
+          # when the action is triggered by PR, empty otherwise
+          COMMIT_ID=${{ github.event.pull_request.head.sha }}
+          # This is the hash of the commit when triggered by push
+          # but the hash of refs/pull/<n>/merge, which is different
+          # from the hash of the latest commit in the PR, that's
+          # why we try github.event.pull_request.head.sha first
+          COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
+          BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
+          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }')
+          echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
+          echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
+      - uses: actions/cache@v4
+        with:
+          path: downloads
+          key: ${{ runner.os }}-downloads-${{ hashFiles('CMakeLists.txt') }}-${{ hashFiles('**/cmake/*')}}
+      - name: Build targets (${{ matrix.id }})
+        run: mkdir -p build && cd build && cmake -DWARNINGS_AS_ERRORS=ON -DCI_JOB_INDEX=${{ matrix.id }} -DCI_JOB_COUNT=${{ strategy.job-total }} -DBUILD_SUFFIX=${{ env.BUILD_SUFFIX }} -G Ninja .. && ninja ci
+      - name: Upload artifacts
+        uses: actions/upload-artifact@v4
+        with:
+          name: ${{ env.BUILD_NAME }}.${{ matrix.id }}
+          path: ./build/*.hex
+
+  build-SITL-Linux:
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v4
+      - name: Install dependencies
+        run: sudo apt-get update && sudo apt-get -y install ninja-build
+      - name: Setup environment
+        env:
+          ACTIONS_ALLOW_UNSECURE_COMMANDS: true
+        run: |
+          # This is the hash of the commit for the PR
+          # when the action is triggered by PR, empty otherwise
+          COMMIT_ID=${{ github.event.pull_request.head.sha }}
+          # This is the hash of the commit when triggered by push
+          # but the hash of refs/pull/<n>/merge, which is different
+          # from the hash of the latest commit in the PR, that's
+          # why we try github.event.pull_request.head.sha first
+          COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
+          BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
+          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/[ \t\)]+/, "", $2); print $2 }')
+          echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
+          echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
+      - name: Build SITL
+        run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja
+      - name: Upload artifacts
+        uses: actions/upload-artifact@v4
+        with:
+          name: sitl-${{ env.BUILD_NAME }}-Linux
+          path: ./build_SITL/*_SITL
+
+  build-SITL-Mac:
+    runs-on: macos-latest
+    steps:
+      - uses: actions/checkout@v4
+      - name: Install dependencies
+        run: |
+          brew install cmake ninja ruby
+
+      - name: Setup environment
+        env:
+          ACTIONS_ALLOW_UNSECURE_COMMANDS: true
+        run: |
+          # This is the hash of the commit for the PR
+          # when the action is triggered by PR, empty otherwise
+          COMMIT_ID=${{ github.event.pull_request.head.sha }}
+          # This is the hash of the commit when triggered by push
+          # but the hash of refs/pull/<n>/merge, which is different
+          # from the hash of the latest commit in the PR, that's
+          # why we try github.event.pull_request.head.sha first
+          COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
+          BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
+          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/[ \t\)]+/, "", $2); print $2 }')
+          echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
+          echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
+      - name: Build SITL
+        run: |
+          mkdir -p build_SITL && cd build_SITL
+          cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -DCMAKE_OSX_ARCHITECTURES="arm64;x86_64" -G Ninja ..
+          ninja
+
+      - name: Upload artifacts
+        uses: actions/upload-artifact@v4
+        with:
+          name: sitl-${{ env.BUILD_NAME }}-MacOS
+          path: ./build_SITL/*_SITL
+
+  build-SITL-Windows:
+    runs-on: windows-latest
+    defaults:
+      run:
+        shell: C:\tools\cygwin\bin\bash.exe -o igncr '{0}'
+    steps:
+      - uses: actions/checkout@v4
+      - name: Setup Cygwin
+        uses: egor-tensin/setup-cygwin@v4
+        with:
+          packages: cmake ruby ninja gcc-g++
+      - name: Setup environment
+        env:
+          ACTIONS_ALLOW_UNSECURE_COMMANDS: true
+        run: |
+          # This is the hash of the commit for the PR
+          # when the action is triggered by PR, empty otherwise
+          COMMIT_ID=${{ github.event.pull_request.head.sha }}
+          # This is the hash of the commit when triggered by push
+          # but the hash of refs/pull/<n>/merge, which is different
+          # from the hash of the latest commit in the PR, that's
+          # why we try github.event.pull_request.head.sha first
+          COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
+          BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
+          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/[ \t\)]+/, "", $2); print $2 }')
+          echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
+          echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
+      - name: Build SITL
+        run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja
+      - name: Upload artifacts
+        uses: actions/upload-artifact@v4
+        with:
+          name: sitl-${{ env.BUILD_NAME }}-WIN
+          path: ./build_SITL/*.exe
+
+
+  test:
+    needs: [build]
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v4
+      - name: Install dependencies
+        run: sudo apt-get update && sudo apt-get -y install ninja-build
+      - name: Run Tests
+        run: mkdir -p build && cd build && cmake -DTOOLCHAIN=none -G Ninja .. && ninja check
+
+  release:
+    needs: [build, build-SITL-Linux, build-SITL-Mac, build-SITL-Windows, test]
+    steps:
+      - name: download artifacts
+        uses: actions/download-artifact@v4
+        path: hexes
+        with:
+          name: inav-*
+          merge-multiple: true
+      - name: download sitl linux
+        uses: actions/download-artifact@v4
+        path: resources/sitl/linux
+        with:
+          name: sitl-*-Linux
+          merge-multiple: true
+      - name: download sitl windows
+        uses: actions/download-artifact@v4
+        path: resources/sitl/windows
+        with:
+          name: sitl-*-WIN
+          merge-multiple: true
+      - name: download sitl mac
+        uses: actions/download-artifact@v4
+        path: resources/sitl/macos
+        with:
+          name: sitl-*-MacOS
+          merge-multiple: true
+      - name: Consolidate sitl files
+        run: |
+          zip -a -9 sitl-resources.zip resources/
+
+      - name: Upload release artifacts
+        uses: softprops/action-gh-release@v2
+        with:
+          name: inav-nightly
+          prerelease: true
+          draft: true
+          generate_release_notes: true
+          make_latest: false
+          files: |
+            hexes/*.hex
+            sitl-linux/*.hex
+            sitl-windows/*.exe
+            sitl-mac/*.hex
+

From 897587500118d7e3ca6dbad0e36ebc376be7df58 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Wed, 12 Jun 2024 20:59:01 +0200
Subject: [PATCH 240/323] Trigger on pr

---
 .github/workflows/dev-builds.yml | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml
index 81551a3a330..7a07dd98457 100644
--- a/.github/workflows/dev-builds.yml
+++ b/.github/workflows/dev-builds.yml
@@ -8,6 +8,9 @@ on:
     - main
       master
       mmosca-update-github-actions
+  pull_request:
+    branches:
+    - mmosca-update-github-actions
 
 jobs:
   build:

From bb6b8c278b9b10afa18b0029753c76f061f0626c Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Wed, 12 Jun 2024 21:08:35 +0200
Subject: [PATCH 241/323] update triggers

---
 .github/workflows/dev-builds.yml | 14 +++++++-------
 1 file changed, 7 insertions(+), 7 deletions(-)

diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml
index 7a07dd98457..fef857f31aa 100644
--- a/.github/workflows/dev-builds.yml
+++ b/.github/workflows/dev-builds.yml
@@ -3,14 +3,14 @@ name: Build pre-release
 # are working on the main repo and want to trigger
 # a CI build submit a draft PR.
 on: 
-  push:
-    branches:
-    - main
-      master
-      mmosca-update-github-actions
+#  push:
+#    branches:
+#    - main
+#      master
+#      mmosca-update-github-actions
   pull_request:
-    branches:
-    - mmosca-update-github-actions
+#    branches:
+#    - mmosca-update-github-actions
 
 jobs:
   build:

From 6de946339dadb1c22b2499857907c1eb6f0f0926 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Wed, 12 Jun 2024 21:12:17 +0200
Subject: [PATCH 242/323] fix path

---
 .github/workflows/dev-builds.yml | 13 +++++--------
 1 file changed, 5 insertions(+), 8 deletions(-)

diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml
index fef857f31aa..24970628b8a 100644
--- a/.github/workflows/dev-builds.yml
+++ b/.github/workflows/dev-builds.yml
@@ -168,32 +168,31 @@ jobs:
     steps:
       - name: download artifacts
         uses: actions/download-artifact@v4
-        path: hexes
         with:
+          path: hexes
           name: inav-*
           merge-multiple: true
       - name: download sitl linux
         uses: actions/download-artifact@v4
-        path: resources/sitl/linux
         with:
+          path: resources/sitl/linux
           name: sitl-*-Linux
           merge-multiple: true
       - name: download sitl windows
         uses: actions/download-artifact@v4
-        path: resources/sitl/windows
         with:
+          path: resources/sitl/windows
           name: sitl-*-WIN
           merge-multiple: true
       - name: download sitl mac
         uses: actions/download-artifact@v4
-        path: resources/sitl/macos
         with:
+          path: resources/sitl/macos
           name: sitl-*-MacOS
           merge-multiple: true
       - name: Consolidate sitl files
         run: |
           zip -a -9 sitl-resources.zip resources/
-
       - name: Upload release artifacts
         uses: softprops/action-gh-release@v2
         with:
@@ -204,7 +203,5 @@ jobs:
           make_latest: false
           files: |
             hexes/*.hex
-            sitl-linux/*.hex
-            sitl-windows/*.exe
-            sitl-mac/*.hex
+            sitl-resources.zip
 

From 2c88008a2035b818f3e590219060ca3c479abf25 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Wed, 12 Jun 2024 21:13:21 +0200
Subject: [PATCH 243/323] add runs-on

---
 .github/workflows/dev-builds.yml | 1 +
 1 file changed, 1 insertion(+)

diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml
index 24970628b8a..2d4ace3d726 100644
--- a/.github/workflows/dev-builds.yml
+++ b/.github/workflows/dev-builds.yml
@@ -165,6 +165,7 @@ jobs:
 
   release:
     needs: [build, build-SITL-Linux, build-SITL-Mac, build-SITL-Windows, test]
+    runs-on: ubuntu-latest
     steps:
       - name: download artifacts
         uses: actions/download-artifact@v4

From 455bc7108dee293a2e19c396d4bf2c0411ae7ad4 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Wed, 12 Jun 2024 21:40:39 +0200
Subject: [PATCH 244/323] fix download artifact

---
 .github/workflows/dev-builds.yml | 20 ++++++++------------
 1 file changed, 8 insertions(+), 12 deletions(-)

diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml
index 2d4ace3d726..2b02fb396bd 100644
--- a/.github/workflows/dev-builds.yml
+++ b/.github/workflows/dev-builds.yml
@@ -3,14 +3,11 @@ name: Build pre-release
 # are working on the main repo and want to trigger
 # a CI build submit a draft PR.
 on: 
-#  push:
-#    branches:
-#    - main
-#      master
-#      mmosca-update-github-actions
+  push:
+    branches:
+    - master
+      mmosca-update-github-actions
   pull_request:
-#    branches:
-#    - mmosca-update-github-actions
 
 jobs:
   build:
@@ -154,7 +151,6 @@ jobs:
 
 
   test:
-    needs: [build]
     runs-on: ubuntu-latest
     steps:
       - uses: actions/checkout@v4
@@ -171,25 +167,25 @@ jobs:
         uses: actions/download-artifact@v4
         with:
           path: hexes
-          name: inav-*
+          pattern: inav-*
           merge-multiple: true
       - name: download sitl linux
         uses: actions/download-artifact@v4
         with:
           path: resources/sitl/linux
-          name: sitl-*-Linux
+          pattern: sitl-*-Linux
           merge-multiple: true
       - name: download sitl windows
         uses: actions/download-artifact@v4
         with:
           path: resources/sitl/windows
-          name: sitl-*-WIN
+          pattern: sitl-*-WIN
           merge-multiple: true
       - name: download sitl mac
         uses: actions/download-artifact@v4
         with:
           path: resources/sitl/macos
-          name: sitl-*-MacOS
+          pattern: sitl-*-MacOS
           merge-multiple: true
       - name: Consolidate sitl files
         run: |

From 3ee3c307229241f0e0957e4c9e0ce8ab745ca30e Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Wed, 12 Jun 2024 21:58:22 +0200
Subject: [PATCH 245/323] Fix zip

---
 .github/workflows/dev-builds.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml
index 2b02fb396bd..63ec7f7f665 100644
--- a/.github/workflows/dev-builds.yml
+++ b/.github/workflows/dev-builds.yml
@@ -189,7 +189,7 @@ jobs:
           merge-multiple: true
       - name: Consolidate sitl files
         run: |
-          zip -a -9 sitl-resources.zip resources/
+          zip -9 sitl-resources.zip resources/
       - name: Upload release artifacts
         uses: softprops/action-gh-release@v2
         with:

From b91831e26680b1e9355ea5ab1cde1aa6286dd38b Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Wed, 12 Jun 2024 22:20:41 +0200
Subject: [PATCH 246/323] attemp at fixing sitl files

---
 .github/workflows/dev-builds.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml
index 63ec7f7f665..e0b0b514292 100644
--- a/.github/workflows/dev-builds.yml
+++ b/.github/workflows/dev-builds.yml
@@ -189,7 +189,7 @@ jobs:
           merge-multiple: true
       - name: Consolidate sitl files
         run: |
-          zip -9 sitl-resources.zip resources/
+          zip -r -9 sitl-resources.zip resources/
       - name: Upload release artifacts
         uses: softprops/action-gh-release@v2
         with:

From 5a5c7f7172df6e8901f48ad051a7b90d9c4d85a1 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Wed, 12 Jun 2024 22:47:59 +0200
Subject: [PATCH 247/323] Test publishing to another repo

---
 .github/workflows/dev-builds.yml | 11 ++++++-----
 1 file changed, 6 insertions(+), 5 deletions(-)

diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml
index e0b0b514292..abd822585ad 100644
--- a/.github/workflows/dev-builds.yml
+++ b/.github/workflows/dev-builds.yml
@@ -32,7 +32,7 @@ jobs:
           # from the hash of the latest commit in the PR, that's
           # why we try github.event.pull_request.head.sha first
           COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
-          BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
+          BUILD_SUFFIX=dev-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
           VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }')
           echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
           echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
@@ -66,7 +66,7 @@ jobs:
           # from the hash of the latest commit in the PR, that's
           # why we try github.event.pull_request.head.sha first
           COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
-          BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
+          BUILD_SUFFIX=dev-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
           VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/[ \t\)]+/, "", $2); print $2 }')
           echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
           echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
@@ -98,7 +98,7 @@ jobs:
           # from the hash of the latest commit in the PR, that's
           # why we try github.event.pull_request.head.sha first
           COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
-          BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
+          BUILD_SUFFIX=dev-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
           VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/[ \t\)]+/, "", $2); print $2 }')
           echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
           echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
@@ -137,7 +137,7 @@ jobs:
           # from the hash of the latest commit in the PR, that's
           # why we try github.event.pull_request.head.sha first
           COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
-          BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
+          BUILD_SUFFIX=dev-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
           VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/[ \t\)]+/, "", $2); print $2 }')
           echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
           echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
@@ -193,7 +193,8 @@ jobs:
       - name: Upload release artifacts
         uses: softprops/action-gh-release@v2
         with:
-          name: inav-nightly
+          name: inav-nightly-${{ GITHUB_RUN_NUMBER }}
+          repository: iNavFlight/inav-configurator
           prerelease: true
           draft: true
           generate_release_notes: true

From fbbd4536d65115e3b93b7c7067cbb739d68e88ac Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Wed, 12 Jun 2024 22:53:06 +0200
Subject: [PATCH 248/323] fix github.run_number

---
 .github/workflows/dev-builds.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml
index abd822585ad..65b08d5a180 100644
--- a/.github/workflows/dev-builds.yml
+++ b/.github/workflows/dev-builds.yml
@@ -193,7 +193,7 @@ jobs:
       - name: Upload release artifacts
         uses: softprops/action-gh-release@v2
         with:
-          name: inav-nightly-${{ GITHUB_RUN_NUMBER }}
+          name: inav-nightly-${{ github.run_number }}
           repository: iNavFlight/inav-configurator
           prerelease: true
           draft: true

From cda0b8cb550c078042fd3fba056d333c7073afc3 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Wed, 12 Jun 2024 23:12:30 +0200
Subject: [PATCH 249/323] massage release name

---
 .github/workflows/dev-builds.yml | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml
index 65b08d5a180..8291fec12a9 100644
--- a/.github/workflows/dev-builds.yml
+++ b/.github/workflows/dev-builds.yml
@@ -163,6 +163,10 @@ jobs:
     needs: [build, build-SITL-Linux, build-SITL-Mac, build-SITL-Windows, test]
     runs-on: ubuntu-latest
     steps:
+      - name: Get current date
+        id: date
+        run: |
+          echo "{release_tag}={$(date +'%Y%m%d')}" >> $GITHUB_STATE
       - name: download artifacts
         uses: actions/download-artifact@v4
         with:
@@ -193,8 +197,8 @@ jobs:
       - name: Upload release artifacts
         uses: softprops/action-gh-release@v2
         with:
-          name: inav-nightly-${{ github.run_number }}
-          repository: iNavFlight/inav-configurator
+          name: inav-nightly-${{ env.release_tag }}-${{ github.sha}}
+          repository: iNavFlight/inav
           prerelease: true
           draft: true
           generate_release_notes: true

From 7095dc7ca146d743f24b1f22888d6bda1d702940 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Wed, 12 Jun 2024 23:38:19 +0200
Subject: [PATCH 250/323] just adding some place holders.

It will need an actual secret created in the future
---
 .github/workflows/dev-builds.yml | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml
index 8291fec12a9..4a474055470 100644
--- a/.github/workflows/dev-builds.yml
+++ b/.github/workflows/dev-builds.yml
@@ -198,6 +198,8 @@ jobs:
         uses: softprops/action-gh-release@v2
         with:
           name: inav-nightly-${{ env.release_tag }}-${{ github.sha}}
+          # To create release on a different repo, we need a token setup
+          #token: ${{ secrets.GITHUB_TOKEN }}
           repository: iNavFlight/inav
           prerelease: true
           draft: true

From 0e780408481a3877e75b68b046d8025ffca7051f Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Thu, 13 Jun 2024 00:08:49 +0200
Subject: [PATCH 251/323] At least create a different release

---
 .github/workflows/dev-builds.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml
index 4a474055470..46305c69fdd 100644
--- a/.github/workflows/dev-builds.yml
+++ b/.github/workflows/dev-builds.yml
@@ -197,7 +197,7 @@ jobs:
       - name: Upload release artifacts
         uses: softprops/action-gh-release@v2
         with:
-          name: inav-nightly-${{ env.release_tag }}-${{ github.sha}}
+          name: inav-nightly-${{ env.release_tag }}-${{ github.run_id }}-${{ github.sha}}
           # To create release on a different repo, we need a token setup
           #token: ${{ secrets.GITHUB_TOKEN }}
           repository: iNavFlight/inav

From 044678404693fca0d7a026b162b389277e1936f6 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Thu, 13 Jun 2024 00:12:31 +0200
Subject: [PATCH 252/323] fix variable name

---
 .github/workflows/dev-builds.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml
index 46305c69fdd..b32bb72a6fa 100644
--- a/.github/workflows/dev-builds.yml
+++ b/.github/workflows/dev-builds.yml
@@ -166,7 +166,7 @@ jobs:
       - name: Get current date
         id: date
         run: |
-          echo "{release_tag}={$(date +'%Y%m%d')}" >> $GITHUB_STATE
+          echo "release_tag={$(date +'%Y%m%d')}" >> $GITHUB_STATE
       - name: download artifacts
         uses: actions/download-artifact@v4
         with:

From af8a561ed016c247182d1aa3b229140c4a95d683 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Wed, 12 Jun 2024 23:12:48 +0100
Subject: [PATCH 253/323] inhibit failsafe on landing

---
 src/main/flight/failsafe.c       | 16 ++++++++--------
 src/main/navigation/navigation.c | 13 +++++++------
 2 files changed, 15 insertions(+), 14 deletions(-)

diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c
index 2a32dbbd12c..24896663dce 100644
--- a/src/main/flight/failsafe.c
+++ b/src/main/flight/failsafe.c
@@ -84,7 +84,7 @@ PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig,
     .failsafe_mission_delay = SETTING_FAILSAFE_MISSION_DELAY_DEFAULT,                   // Time delay before Failsafe activated during WP mission (s)
 #ifdef USE_GPS_FIX_ESTIMATION
     .failsafe_gps_fix_estimation_delay = SETTING_FAILSAFE_GPS_FIX_ESTIMATION_DELAY_DEFAULT, // Time delay before Failsafe activated when GPS Fix estimation is allied
-#endif    
+#endif
 );
 
 typedef enum {
@@ -350,16 +350,16 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void)
         }
     }
 
-    // Inhibit Failsafe if emergency landing triggered manually
-    if (posControl.flags.manualEmergLandActive) {
+    // Inhibit Failsafe if emergency landing triggered manually or if landing is detected
+    if (posControl.flags.manualEmergLandActive || STATE(LANDING_DETECTED)) {
         return FAILSAFE_PROCEDURE_NONE;
     }
 
     // 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)) 
-#ifdef USE_GPS_FIX_ESTIMATION    
+            ((sensors(SENSOR_GPS) && STATE(GPS_FIX))
+#ifdef USE_GPS_FIX_ESTIMATION
                 || STATE(GPS_ESTIMATED_FIX)
 #endif
                 ) && STATE(GPS_FIX_HOME)) {
@@ -429,8 +429,8 @@ void failsafeUpdateState(void)
 #ifdef USE_GPS_FIX_ESTIMATION
                     if ( checkGPSFixFailsafe() ) {
                         reprocessState = true;
-                    } else 
-#endif                    
+                    } else
+#endif
                     if (!receivingRxDataAndNotFailsafeMode) {
                         if ((failsafeConfig()->failsafe_throttle_low_delay && (millis() > failsafeState.throttleLowPeriod)) || STATE(NAV_MOTOR_STOP_OR_IDLE)) {
                             // JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds or waiting for launch
@@ -499,7 +499,7 @@ void failsafeUpdateState(void)
                 } else if (failsafeChooseFailsafeProcedure() != FAILSAFE_PROCEDURE_NONE) {  // trigger new failsafe procedure if changed
                     failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED;
                     reprocessState = true;
-                } 
+                }
 #ifdef USE_GPS_FIX_ESTIMATION
                 else {
                     if ( checkGPSFixFailsafe() ) {
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index cc4e269ef4b..3c87ced7a11 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -3321,10 +3321,6 @@ void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t d
  *-----------------------------------------------------------*/
 void updateLandingStatus(timeMs_t currentTimeMs)
 {
-    if (STATE(AIRPLANE) && !navConfig()->general.flags.disarm_on_landing) {
-        return;     // no point using this with a fixed wing if not set to disarm
-    }
-
     static timeMs_t lastUpdateTimeMs = 0;
     if ((currentTimeMs - lastUpdateTimeMs) <= HZ2MS(100)) {  // limit update to 100Hz
         return;
@@ -3354,8 +3350,13 @@ void updateLandingStatus(timeMs_t currentTimeMs)
             ENABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
             disarm(DISARM_LANDING);
         } else if (!navigationInAutomaticThrottleMode()) {
-            // for multirotor only - reactivate landing detector without disarm when throttle raised toward hover throttle
-            landingDetectorIsActive = rxGetChannelValue(THROTTLE) < (0.5 * (currentBatteryProfile->nav.mc.hover_throttle + getThrottleIdleValue()));
+            if (STATE(AIRPLANE) && isFlightDetected()) {
+                // Cancel landing detection flag if fixed wing redetected in flight
+                resetLandingDetector();
+            } else {
+                // For multirotor - reactivate landing detector without disarm when throttle raised toward hover throttle
+                landingDetectorIsActive = rxGetChannelValue(THROTTLE) < (0.5 * (currentBatteryProfile->nav.mc.hover_throttle + getThrottleIdleValue()));
+            }
         }
     } else if (isLandingDetected()) {
         ENABLE_STATE(LANDING_DETECTED);

From bb84bcd10797f174506071e6d04ee03df748aefb Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Thu, 13 Jun 2024 00:29:57 +0200
Subject: [PATCH 254/323] date

---
 .github/workflows/dev-builds.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml
index b32bb72a6fa..bc2490a033a 100644
--- a/.github/workflows/dev-builds.yml
+++ b/.github/workflows/dev-builds.yml
@@ -197,7 +197,7 @@ jobs:
       - name: Upload release artifacts
         uses: softprops/action-gh-release@v2
         with:
-          name: inav-nightly-${{ env.release_tag }}-${{ github.run_id }}-${{ github.sha}}
+          name: inav-nightly-$(date '+%Y%m%d')-${{ github.run_id }}-${{ github.sha}}
           # To create release on a different repo, we need a token setup
           #token: ${{ secrets.GITHUB_TOKEN }}
           repository: iNavFlight/inav

From 830ef4f3a2a70b4aebbfff664e81e983e5445962 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Thu, 13 Jun 2024 00:47:53 +0200
Subject: [PATCH 255/323] rename

---
 .github/workflows/dev-builds.yml | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml
index bc2490a033a..6b02719d24f 100644
--- a/.github/workflows/dev-builds.yml
+++ b/.github/workflows/dev-builds.yml
@@ -166,7 +166,7 @@ jobs:
       - name: Get current date
         id: date
         run: |
-          echo "release_tag={$(date +'%Y%m%d')}" >> $GITHUB_STATE
+          echo "RELEASE_DATE=$(date +'%Y%m%d')" >> $GITHUB_STATE
       - name: download artifacts
         uses: actions/download-artifact@v4
         with:
@@ -197,7 +197,7 @@ jobs:
       - name: Upload release artifacts
         uses: softprops/action-gh-release@v2
         with:
-          name: inav-nightly-$(date '+%Y%m%d')-${{ github.run_id }}-${{ github.sha}}
+          name: inav-dev-release-${{ env.RELEASE_DATE }}-${{ github.run_id }}-${{ github.sha}}
           # To create release on a different repo, we need a token setup
           #token: ${{ secrets.GITHUB_TOKEN }}
           repository: iNavFlight/inav

From 59dd2533191d0b8f82f88a3ed40925b949d20f0d Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Thu, 13 Jun 2024 15:47:23 +0100
Subject: [PATCH 256/323] Update navigation_multicopter.c

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

diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c
index b108ef6aa31..44264985cfb 100644
--- a/src/main/navigation/navigation_multicopter.c
+++ b/src/main/navigation/navigation_multicopter.c
@@ -802,8 +802,9 @@ bool isMulticopterLandingDetected(void)
     const timeMs_t currentTimeMs = millis();
 
 #if defined(USE_BARO)
-    if (sensors(SENSOR_BARO) && navConfig()->general.flags.landing_bump_detection && isLandingGbumpDetected(currentTimeMs)) {
-        return true;    // Landing flagged immediately if landing bump detected
+    /* Only allow landing G bump detection when xy velocity is low */
+    if (sensors(SENSOR_BARO) && navConfig()->general.flags.landing_bump_detection && posControl.actualState.velXY < MC_LAND_CHECK_VEL_XY_MOVING) {
+        return isLandingGbumpDetected(currentTimeMs);    // Landing flagged immediately if landing bump detected
     }
 #endif
 

From b289ba8356917b97c72e941514221c30534fd3ec Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Thu, 13 Jun 2024 19:41:41 +0200
Subject: [PATCH 257/323] try to fix date

---
 .github/workflows/dev-builds.yml | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml
index 6b02719d24f..75c2fe80a44 100644
--- a/.github/workflows/dev-builds.yml
+++ b/.github/workflows/dev-builds.yml
@@ -165,8 +165,7 @@ jobs:
     steps:
       - name: Get current date
         id: date
-        run: |
-          echo "RELEASE_DATE=$(date +'%Y%m%d')" >> $GITHUB_STATE
+        run: echo "today=$(date '+%Y%m%d')" >> $GITHUB_OUTPUT
       - name: download artifacts
         uses: actions/download-artifact@v4
         with:
@@ -197,7 +196,8 @@ jobs:
       - name: Upload release artifacts
         uses: softprops/action-gh-release@v2
         with:
-          name: inav-dev-release-${{ env.RELEASE_DATE }}-${{ github.run_id }}-${{ github.sha}}
+          name: inav-dev-release-${{ steps.date.outputs.today }}-${{ github.run_id }}-${{ github.sha}}
+          tag_name: v${{ steps.date.outputs.today }}.${{ github.run_number }}
           # To create release on a different repo, we need a token setup
           #token: ${{ secrets.GITHUB_TOKEN }}
           repository: iNavFlight/inav

From 01df8f7a1395418cc81bce0360c16092809588c1 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Thu, 13 Jun 2024 19:41:59 +0200
Subject: [PATCH 258/323] try to fix run number

---
 .github/workflows/dev-builds.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml
index 75c2fe80a44..4bc468d3d19 100644
--- a/.github/workflows/dev-builds.yml
+++ b/.github/workflows/dev-builds.yml
@@ -196,7 +196,7 @@ jobs:
       - name: Upload release artifacts
         uses: softprops/action-gh-release@v2
         with:
-          name: inav-dev-release-${{ steps.date.outputs.today }}-${{ github.run_id }}-${{ github.sha}}
+          name: inav-dev-release-${{ steps.date.outputs.today }}-${{ github.run_number }}-${{ github.sha}}
           tag_name: v${{ steps.date.outputs.today }}.${{ github.run_number }}
           # To create release on a different repo, we need a token setup
           #token: ${{ secrets.GITHUB_TOKEN }}

From f41dd600f91b82eabbd8ea119503cfad094f26c3 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Thu, 13 Jun 2024 20:22:24 +0200
Subject: [PATCH 259/323] Fix version type definition

---
 .github/workflows/dev-builds.yml | 16 ++++++++++++----
 CMakeLists.txt                   |  1 +
 src/main/build/version.h         |  4 ++++
 src/main/fc/cli.c                | 10 ++++++----
 4 files changed, 23 insertions(+), 8 deletions(-)

diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml
index 4bc468d3d19..64a14ee4d57 100644
--- a/.github/workflows/dev-builds.yml
+++ b/.github/workflows/dev-builds.yml
@@ -139,10 +139,12 @@ jobs:
           COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
           BUILD_SUFFIX=dev-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
           VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/[ \t\)]+/, "", $2); print $2 }')
-          echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
-          echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
+          #echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
+          #echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
+          #echo "VERSION_TAG=-$(date '+%Y%m%d')" >> $GITHUB_ENV
+          echo "version=${VERSION}" >> $GITHUB_OUTPUT
       - name: Build SITL
-        run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja
+        run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -DVERSION_TYPE="dev" -G Ninja .. && ninja
       - name: Upload artifacts
         uses: actions/upload-artifact@v4
         with:
@@ -163,6 +165,12 @@ jobs:
     needs: [build, build-SITL-Linux, build-SITL-Mac, build-SITL-Windows, test]
     runs-on: ubuntu-latest
     steps:
+      - uses: actions/checkout@v4
+      - name: Get version
+        id: version
+        run: |
+          VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/[ \t\)]+/, "", $2); print $2 }')
+          echo "version=${VERSION}" >> $GITHUB_OUTPUT
       - name: Get current date
         id: date
         run: echo "today=$(date '+%Y%m%d')" >> $GITHUB_OUTPUT
@@ -196,7 +204,7 @@ jobs:
       - name: Upload release artifacts
         uses: softprops/action-gh-release@v2
         with:
-          name: inav-dev-release-${{ steps.date.outputs.today }}-${{ github.run_number }}-${{ github.sha}}
+          name: inav-${{ steps.version.outputs.version }}-dev-${{ steps.date.outputs.today }}-${{ github.run_number }}-${{ github.sha }}
           tag_name: v${{ steps.date.outputs.today }}.${{ github.run_number }}
           # To create release on a different repo, we need a token setup
           #token: ${{ secrets.GITHUB_TOKEN }}
diff --git a/CMakeLists.txt b/CMakeLists.txt
index db8ec36897b..9b5f40f3337 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -79,6 +79,7 @@ set(COMMON_COMPILE_DEFINITIONS
     FC_VERSION_MAJOR=${CMAKE_PROJECT_VERSION_MAJOR}
     FC_VERSION_MINOR=${CMAKE_PROJECT_VERSION_MINOR}
     FC_VERSION_PATCH_LEVEL=${CMAKE_PROJECT_VERSION_PATCH}
+    FC_VERSION_TYPE="${VERSION_TYPE}"
 )
 
 if (NOT SITL)
diff --git a/src/main/build/version.h b/src/main/build/version.h
index 49ec81d1b11..583cdd82dd8 100644
--- a/src/main/build/version.h
+++ b/src/main/build/version.h
@@ -18,8 +18,12 @@
 #define STR_HELPER(x) #x
 #define STR(x) STR_HELPER(x)
 #define FC_VERSION_STRING STR(FC_VERSION_MAJOR) "." STR(FC_VERSION_MINOR) "." STR(FC_VERSION_PATCH_LEVEL)
+#ifndef FC_VERSION_TYPE
+#define FC_VERSION_TYPE ""
+#endif
 #define FC_FIRMWARE_NAME "INAV"
 
+
 #define MW_VERSION              231
 
 extern const char* const compilerVersion;
diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index 18243fdefce..7b9c1329884 100644
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -3662,13 +3662,14 @@ static void cliStatus(char *cmdline)
     char buf[MAX(FORMATTED_DATE_TIME_BUFSIZE, SETTING_MAX_NAME_LENGTH)];
     dateTime_t dt;
 
-    cliPrintLinef("%s/%s %s %s / %s (%s)",
+    cliPrintLinef("%s/%s %s %s / %s (%s) %s",
         FC_FIRMWARE_NAME,
         targetName,
         FC_VERSION_STRING,
         buildDate,
         buildTime,
-        shortGitRevision
+        shortGitRevision,
+        FC_VERSION_TYPE
     );
     cliPrintLinef("GCC-%s",
         compilerVersion
@@ -3906,13 +3907,14 @@ static void cliVersion(char *cmdline)
 {
     UNUSED(cmdline);
 
-    cliPrintLinef("# %s/%s %s %s / %s (%s)",
+    cliPrintLinef("# %s/%s %s %s / %s (%s) %s",
         FC_FIRMWARE_NAME,
         targetName,
         FC_VERSION_STRING,
         buildDate,
         buildTime,
-        shortGitRevision
+        shortGitRevision,
+        FC_VERSION_TYPE
     );
     cliPrintLinef("# GCC-%s",
         compilerVersion

From 1fa882bf869c619a995dcd9d5c3954f5d488dcac Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Thu, 13 Jun 2024 20:54:57 +0200
Subject: [PATCH 260/323] BUILD_TYPE on all builds

---
 .github/workflows/dev-builds.yml | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml
index 64a14ee4d57..426b3218ea2 100644
--- a/.github/workflows/dev-builds.yml
+++ b/.github/workflows/dev-builds.yml
@@ -41,7 +41,7 @@ jobs:
           path: downloads
           key: ${{ runner.os }}-downloads-${{ hashFiles('CMakeLists.txt') }}-${{ hashFiles('**/cmake/*')}}
       - name: Build targets (${{ matrix.id }})
-        run: mkdir -p build && cd build && cmake -DWARNINGS_AS_ERRORS=ON -DCI_JOB_INDEX=${{ matrix.id }} -DCI_JOB_COUNT=${{ strategy.job-total }} -DBUILD_SUFFIX=${{ env.BUILD_SUFFIX }} -G Ninja .. && ninja ci
+        run: mkdir -p build && cd build && cmake -DWARNINGS_AS_ERRORS=ON -DCI_JOB_INDEX=${{ matrix.id }} -DCI_JOB_COUNT=${{ strategy.job-total }} -DBUILD_SUFFIX=${{ env.BUILD_SUFFIX }} -DVERSION_TYPE=dev -G Ninja .. && ninja ci
       - name: Upload artifacts
         uses: actions/upload-artifact@v4
         with:
@@ -71,7 +71,7 @@ jobs:
           echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV
           echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV
       - name: Build SITL
-        run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja
+        run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja -DVERSION_TYPE=dev .. && ninja
       - name: Upload artifacts
         uses: actions/upload-artifact@v4
         with:
@@ -105,7 +105,7 @@ jobs:
       - name: Build SITL
         run: |
           mkdir -p build_SITL && cd build_SITL
-          cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -DCMAKE_OSX_ARCHITECTURES="arm64;x86_64" -G Ninja ..
+          cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -DCMAKE_OSX_ARCHITECTURES="arm64;x86_64" -DVERSION_TYPE=dev -G Ninja ..
           ninja
 
       - name: Upload artifacts
@@ -144,7 +144,7 @@ jobs:
           #echo "VERSION_TAG=-$(date '+%Y%m%d')" >> $GITHUB_ENV
           echo "version=${VERSION}" >> $GITHUB_OUTPUT
       - name: Build SITL
-        run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -DVERSION_TYPE="dev" -G Ninja .. && ninja
+        run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -DVERSION_TYPE=dev -G Ninja .. && ninja
       - name: Upload artifacts
         uses: actions/upload-artifact@v4
         with:

From 9f74b52732e86a56cdef916ac4f38205b8793edb Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Thu, 13 Jun 2024 21:27:32 +0200
Subject: [PATCH 261/323] Test non-daft

---
 .github/workflows/dev-builds.yml | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml
index 426b3218ea2..215b90c4aba 100644
--- a/.github/workflows/dev-builds.yml
+++ b/.github/workflows/dev-builds.yml
@@ -7,7 +7,7 @@ on:
     branches:
     - master
       mmosca-update-github-actions
-  pull_request:
+  #pull_request:
 
 jobs:
   build:
@@ -210,7 +210,7 @@ jobs:
           #token: ${{ secrets.GITHUB_TOKEN }}
           repository: iNavFlight/inav
           prerelease: true
-          draft: true
+          draft: false
           generate_release_notes: true
           make_latest: false
           files: |

From 0c68faec4c60185fe52694d914ecfff6823c0dc9 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Thu, 13 Jun 2024 21:30:17 +0200
Subject: [PATCH 262/323] re-enable on branch

---
 .github/workflows/dev-builds.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml
index 215b90c4aba..950d1c3dbfd 100644
--- a/.github/workflows/dev-builds.yml
+++ b/.github/workflows/dev-builds.yml
@@ -7,7 +7,7 @@ on:
     branches:
     - master
       mmosca-update-github-actions
-  #pull_request:
+  pull_request:
 
 jobs:
   build:

From 5d4e9f7ee92d338af3046f7d3ecd1af3cfb242c1 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Thu, 13 Jun 2024 21:46:32 +0200
Subject: [PATCH 263/323] re-enable, set as draft again

---
 .github/workflows/dev-builds.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml
index 950d1c3dbfd..426b3218ea2 100644
--- a/.github/workflows/dev-builds.yml
+++ b/.github/workflows/dev-builds.yml
@@ -210,7 +210,7 @@ jobs:
           #token: ${{ secrets.GITHUB_TOKEN }}
           repository: iNavFlight/inav
           prerelease: true
-          draft: false
+          draft: true
           generate_release_notes: true
           make_latest: false
           files: |

From 8449e0d3f6fe1e56e091aaec66ccd35568ea5ad8 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Thu, 13 Jun 2024 21:18:01 +0100
Subject: [PATCH 264/323] add posStatus check

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

diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c
index 44264985cfb..179e475b218 100644
--- a/src/main/navigation/navigation_multicopter.c
+++ b/src/main/navigation/navigation_multicopter.c
@@ -802,9 +802,12 @@ bool isMulticopterLandingDetected(void)
     const timeMs_t currentTimeMs = millis();
 
 #if defined(USE_BARO)
-    /* Only allow landing G bump detection when xy velocity is low */
-    if (sensors(SENSOR_BARO) && navConfig()->general.flags.landing_bump_detection && posControl.actualState.velXY < MC_LAND_CHECK_VEL_XY_MOVING) {
-        return isLandingGbumpDetected(currentTimeMs);    // Landing flagged immediately if landing bump detected
+    /* G bump landing detection only active when xy velocity is usable and low */
+    if (sensors(SENSOR_BARO) && navConfig()->general.flags.landing_bump_detection &&
+        posControl.flags.estPosStatus >= EST_USABLE && posControl.actualState.velXY < MC_LAND_CHECK_VEL_XY_MOVING &&
+        isLandingGbumpDetected(currentTimeMs)) { // CR129
+
+        return true;    // Landing flagged immediately if landing bump detected
     }
 #endif
 

From 456c49a7927a979a8e6d9a0b532ef17e6f278442 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Thu, 13 Jun 2024 21:54:17 +0100
Subject: [PATCH 265/323] add failsafe logic

---
 docs/Settings.md                             | 2 +-
 src/main/fc/settings.yaml                    | 2 +-
 src/main/navigation/navigation_multicopter.c | 9 +++++----
 3 files changed, 7 insertions(+), 6 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index 2367cc8f1ab..296d86931ee 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -3474,7 +3474,7 @@ Defines at what altitude the descent velocity should start to be `nav_land_minal
 
 ### nav_landing_bump_detection
 
-Allows immediate landing detection based on G bump at touchdown when set to ON. Requires a barometer and currently only works for multirotors.
+Allows immediate landing detection based on G bump at touchdown when set to ON. Requires a barometer and GPS and currently only works for multirotors (Note: will work during Failsafe without need for a GPS).
 
 | Default | Min | Max |
 | --- | --- | --- |
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index cfe40bb400f..0d358f58ffb 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -2463,7 +2463,7 @@ groups:
         min: 1
         max: 15
       - name: nav_landing_bump_detection
-        description: "Allows immediate landing detection based on G bump at touchdown when set to ON. Requires a barometer and currently only works for multirotors."
+        description: "Allows immediate landing detection based on G bump at touchdown when set to ON. Requires a barometer and GPS and currently only works for multirotors (Note: will work during Failsafe without need for a GPS)."
         default_value: OFF
         field: general.flags.landing_bump_detection
         type: bool
diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c
index 179e475b218..b58431211df 100644
--- a/src/main/navigation/navigation_multicopter.c
+++ b/src/main/navigation/navigation_multicopter.c
@@ -802,11 +802,12 @@ bool isMulticopterLandingDetected(void)
     const timeMs_t currentTimeMs = millis();
 
 #if defined(USE_BARO)
-    /* G bump landing detection only active when xy velocity is usable and low */
-    if (sensors(SENSOR_BARO) && navConfig()->general.flags.landing_bump_detection &&
-        posControl.flags.estPosStatus >= EST_USABLE && posControl.actualState.velXY < MC_LAND_CHECK_VEL_XY_MOVING &&
-        isLandingGbumpDetected(currentTimeMs)) { // CR129
+    /* G bump landing detection only used when xy velocity is usable and low or failsafe is active */
+    bool gBumpDetectionUsable = navConfig()->general.flags.landing_bump_detection && sensors(SENSOR_BARO) &&
+                                ((posControl.flags.estPosStatus >= EST_USABLE && posControl.actualState.velXY < MC_LAND_CHECK_VEL_XY_MOVING) ||
+                                FLIGHT_MODE(FAILSAFE_MODE));
 
+    if (gBumpDetectionUsable && isLandingGbumpDetected(currentTimeMs)) {
         return true;    // Landing flagged immediately if landing bump detected
     }
 #endif

From 29a2d6935bdd1f1910f1d2f3a6292f3697d54369 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Fri, 14 Jun 2024 11:13:28 +0200
Subject: [PATCH 266/323] Add version header, in case we want to expand it in
 the future

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

diff --git a/src/main/io/headtracker_msp.h b/src/main/io/headtracker_msp.h
index 4552bf1ea1b..d1d5bb68651 100644
--- a/src/main/io/headtracker_msp.h
+++ b/src/main/io/headtracker_msp.h
@@ -28,6 +28,7 @@
 #include "drivers/headtracker_common.h"
 
 typedef struct headtrackerMspMessage_s {
+    uint8_t version;	    // 0
     int16_t pan;            // -2048~2047. Scale is min/max angle for gimbal
     int16_t tilt;           // -2048~2047. Scale is min/max angle for gimbal
     int16_t roll;           // -2048~2047. Scale is min/max angle for gimbal
@@ -38,4 +39,4 @@ void mspHeadTrackerInit(void);
 
 void mspHeadTrackerReceiverNewData(uint8_t *data, int dataSize);
 
-#endif
\ No newline at end of file
+#endif

From d351af62ea1ba589f0ffc114ea36c259caf15acc Mon Sep 17 00:00:00 2001
From: Jonathan Hudson <jh+github@daria.co.uk>
Date: Sat, 15 Jun 2024 10:11:39 +0100
Subject: [PATCH 267/323] [crsf] move LAND flight mode into ARMED section

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

diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c
index 5ed25de73be..8b7f289db20 100755
--- a/src/main/telemetry/crsf.c
+++ b/src/main/telemetry/crsf.c
@@ -354,11 +354,11 @@ static void crsfFrameFlightMode(sbuf_t *dst)
             flightMode = "HOR";
         } else if (FLIGHT_MODE(ANGLEHOLD_MODE)) {
             flightMode = "ANGH";
-        }
 #ifdef USE_FW_AUTOLAND
-    } else if (FLIGHT_MODE(NAV_FW_AUTOLAND)) {
-        flightMode = "LAND";
+        } else if (FLIGHT_MODE(NAV_FW_AUTOLAND)) {
+            flightMode = "LAND";
 #endif
+        }
 #ifdef USE_GPS
     } else if (feature(FEATURE_GPS) && navConfig()->general.flags.extra_arming_safety && (!STATE(GPS_FIX) || !STATE(GPS_FIX_HOME))) {
         flightMode = "WAIT"; // Waiting for GPS lock

From 68e749831e25d1518d6dff171971b7e3b2f4dfa8 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Sun, 16 Jun 2024 08:24:45 +0100
Subject: [PATCH 268/323] Update navigation.c

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

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 3c87ced7a11..4c42ce900fd 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -3353,7 +3353,7 @@ void updateLandingStatus(timeMs_t currentTimeMs)
             if (STATE(AIRPLANE) && isFlightDetected()) {
                 // Cancel landing detection flag if fixed wing redetected in flight
                 resetLandingDetector();
-            } else {
+            } else if (STATE(MULTIROTOR)) {
                 // For multirotor - reactivate landing detector without disarm when throttle raised toward hover throttle
                 landingDetectorIsActive = rxGetChannelValue(THROTTLE) < (0.5 * (currentBatteryProfile->nav.mc.hover_throttle + getThrottleIdleValue()));
             }

From 58fc4ed8ec83af48fa38fde0ed0bbf5abfdb9543 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Mon, 17 Jun 2024 13:55:19 +0200
Subject: [PATCH 269/323] Update dev-builds.yml

Use newly created secret
---
 .github/workflows/dev-builds.yml | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml
index 426b3218ea2..d93f94dd1e8 100644
--- a/.github/workflows/dev-builds.yml
+++ b/.github/workflows/dev-builds.yml
@@ -207,10 +207,10 @@ jobs:
           name: inav-${{ steps.version.outputs.version }}-dev-${{ steps.date.outputs.today }}-${{ github.run_number }}-${{ github.sha }}
           tag_name: v${{ steps.date.outputs.today }}.${{ github.run_number }}
           # To create release on a different repo, we need a token setup
-          #token: ${{ secrets.GITHUB_TOKEN }}
-          repository: iNavFlight/inav
+          token: ${{ secrets.NIGHTLY_TOKEN }}
+          repository: iNavFlight/inav-nightly
           prerelease: true
-          draft: true
+          draft: false
           generate_release_notes: true
           make_latest: false
           files: |

From 91f4569c3a564804b77184e3c5cfe92b414310f2 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Mon, 17 Jun 2024 14:31:24 +0200
Subject: [PATCH 270/323] Update dev-builds.yml

---
 .github/workflows/dev-builds.yml | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml
index d93f94dd1e8..becbea70546 100644
--- a/.github/workflows/dev-builds.yml
+++ b/.github/workflows/dev-builds.yml
@@ -208,7 +208,8 @@ jobs:
           tag_name: v${{ steps.date.outputs.today }}.${{ github.run_number }}
           # To create release on a different repo, we need a token setup
           token: ${{ secrets.NIGHTLY_TOKEN }}
-          repository: iNavFlight/inav-nightly
+          #repository: iNavFlight/inav-nightly
+          reposiroty: mmosca-test-org/test-repo
           prerelease: true
           draft: false
           generate_release_notes: true

From 0116404b27e38fcfdff656bd7e25476dfe3875fc Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Mon, 17 Jun 2024 14:51:44 +0200
Subject: [PATCH 271/323] Update dev-builds.yml

---
 .github/workflows/dev-builds.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml
index becbea70546..ea767900b04 100644
--- a/.github/workflows/dev-builds.yml
+++ b/.github/workflows/dev-builds.yml
@@ -209,7 +209,7 @@ jobs:
           # To create release on a different repo, we need a token setup
           token: ${{ secrets.NIGHTLY_TOKEN }}
           #repository: iNavFlight/inav-nightly
-          reposiroty: mmosca-test-org/test-repo
+          repository: mmosca-test-org/test-repo
           prerelease: true
           draft: false
           generate_release_notes: true

From 4e8bd5c6b36ad8cfb76da86ea719deef2855cca3 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Mon, 17 Jun 2024 15:23:59 +0200
Subject: [PATCH 272/323] Update dev-builds.yml

---
 .github/workflows/dev-builds.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml
index ea767900b04..ed5540fcec8 100644
--- a/.github/workflows/dev-builds.yml
+++ b/.github/workflows/dev-builds.yml
@@ -205,7 +205,7 @@ jobs:
         uses: softprops/action-gh-release@v2
         with:
           name: inav-${{ steps.version.outputs.version }}-dev-${{ steps.date.outputs.today }}-${{ github.run_number }}-${{ github.sha }}
-          tag_name: v${{ steps.date.outputs.today }}.${{ github.run_number }}
+          # tag_name: v${{ steps.date.outputs.today }}.${{ github.run_number }}
           # To create release on a different repo, we need a token setup
           token: ${{ secrets.NIGHTLY_TOKEN }}
           #repository: iNavFlight/inav-nightly

From 6675361ff2d82e0bbdac5c0485c3b918cf200494 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Mon, 17 Jun 2024 15:42:09 +0200
Subject: [PATCH 273/323] Update dev-builds.yml

---
 .github/workflows/dev-builds.yml | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml
index ed5540fcec8..d93f94dd1e8 100644
--- a/.github/workflows/dev-builds.yml
+++ b/.github/workflows/dev-builds.yml
@@ -205,11 +205,10 @@ jobs:
         uses: softprops/action-gh-release@v2
         with:
           name: inav-${{ steps.version.outputs.version }}-dev-${{ steps.date.outputs.today }}-${{ github.run_number }}-${{ github.sha }}
-          # tag_name: v${{ steps.date.outputs.today }}.${{ github.run_number }}
+          tag_name: v${{ steps.date.outputs.today }}.${{ github.run_number }}
           # To create release on a different repo, we need a token setup
           token: ${{ secrets.NIGHTLY_TOKEN }}
-          #repository: iNavFlight/inav-nightly
-          repository: mmosca-test-org/test-repo
+          repository: iNavFlight/inav-nightly
           prerelease: true
           draft: false
           generate_release_notes: true

From 88fb1d0216679e23c8c85b36207334ea3221070b Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Mon, 17 Jun 2024 16:03:36 +0200
Subject: [PATCH 274/323] Update dev-builds.yml

---
 .github/workflows/dev-builds.yml | 16 +++++++++++++++-
 1 file changed, 15 insertions(+), 1 deletion(-)

diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml
index d93f94dd1e8..ee9494e1e11 100644
--- a/.github/workflows/dev-builds.yml
+++ b/.github/workflows/dev-builds.yml
@@ -211,9 +211,23 @@ jobs:
           repository: iNavFlight/inav-nightly
           prerelease: true
           draft: false
-          generate_release_notes: true
+          #generate_release_notes: true
           make_latest: false
           files: |
             hexes/*.hex
             sitl-resources.zip
+          body: |
+            ${{ steps.notes.outputs.notes }}
+
+            ### Repository:
+            ${{ github.repository }} ([link](${{ github.event.repository.html_url }}))
+
+            ### Branch:
+            ${{ github.ref_name }} ([link](${{ github.event.repository.html_url }}/tree/${{ github.ref_name }}))
+
+            ### Latest changeset:
+            ${{ github.event.head_commit.id }} ([link](${{ github.event.head_commit.url }}))
+
+            ### Changes:
+            ${{ github.event.head_commit.message }}
 

From e2e37c7113d52cccadf52e4a9ea10e7ef8383fc4 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Mon, 17 Jun 2024 16:19:20 +0200
Subject: [PATCH 275/323] Update dev-builds.yml

Don't trigger on prs
---
 .github/workflows/dev-builds.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml
index ee9494e1e11..c1dac23c067 100644
--- a/.github/workflows/dev-builds.yml
+++ b/.github/workflows/dev-builds.yml
@@ -7,7 +7,7 @@ on:
     branches:
     - master
       mmosca-update-github-actions
-  pull_request:
+  #pull_request:
 
 jobs:
   build:

From 6d6a4870d49e2aaafa146f70b32b86a0943a9226 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Mon, 17 Jun 2024 16:30:55 +0200
Subject: [PATCH 276/323] Update dev-builds.yml

---
 .github/workflows/dev-builds.yml | 2 --
 1 file changed, 2 deletions(-)

diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml
index c1dac23c067..8bfa1dbc5ef 100644
--- a/.github/workflows/dev-builds.yml
+++ b/.github/workflows/dev-builds.yml
@@ -6,8 +6,6 @@ on:
   push:
     branches:
     - master
-      mmosca-update-github-actions
-  #pull_request:
 
 jobs:
   build:

From 9117cfcccafab9dc0aadb216c412a218c619942d Mon Sep 17 00:00:00 2001
From: rmaia <9812730+rmaia3d@users.noreply.github.com>
Date: Mon, 17 Jun 2024 12:08:43 -0300
Subject: [PATCH 277/323] - Remove unused, commented out, legacy function

---
 src/main/io/osd.c | 31 -------------------------------
 1 file changed, 31 deletions(-)

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 43e74530337..7feba3d9272 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -477,37 +477,6 @@ static void osdFormatWindSpeedStr(char *buff, int32_t ws, bool isValid)
 }
 #endif
 
-/*
- * This is a simplified altitude conversion code that does not use any scaling
- * but is fully compatible with the DJI G2 MSP Displayport OSD implementation.
- */
-/* void osdSimpleAltitudeSymbol(char *buff, int32_t alt) {
-
-    int32_t convertedAltutude = 0;
-    char suffix = '\0';
-
-    switch ((osd_unit_e)osdConfig()->units) {
-        case OSD_UNIT_UK:
-            FALLTHROUGH;
-        case OSD_UNIT_GA:
-            FALLTHROUGH;
-        case OSD_UNIT_IMPERIAL:
-            convertedAltutude = CENTIMETERS_TO_FEET(alt);
-            suffix = SYM_ALT_FT;
-            break;
-        case OSD_UNIT_METRIC_MPH:
-            FALLTHROUGH;
-        case OSD_UNIT_METRIC:
-            convertedAltutude = CENTIMETERS_TO_METERS(alt);
-            suffix = SYM_ALT_M;
-            break;
-    }
-
-    tfp_sprintf(buff, "%4d", (int) convertedAltutude);
-    buff[4] = suffix;
-    buff[5] = '\0';
-} */
-
 /**
 * Converts altitude into a string based on the current unit system
 * prefixed by a a symbol to indicate the unit used.

From 0849408fa1ad5fd56e22e46b8b3a04724e3594bc Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Mon, 17 Jun 2024 19:51:41 +0200
Subject: [PATCH 278/323] Add headtracker axis ratios

---
 docs/Settings.md                      | 30 +++++++++++++++++++++++++++
 src/main/drivers/headtracker_common.c |  2 +-
 src/main/drivers/headtracker_common.h |  3 +++
 src/main/fc/settings.yaml             | 21 +++++++++++++++++++
 src/main/io/gimbal_serial.c           |  6 +++---
 src/main/io/headtracker_msp.c         |  6 +++---
 6 files changed, 61 insertions(+), 7 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index 102f520b80a..cd58bb0cfbf 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -1822,6 +1822,36 @@ This setting limits yaw rotation rate that HEADING_HOLD controller can request f
 
 ---
 
+### headtracker_pan_ratio
+
+Head pan movement vs camera movement ratio
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 1 | 0 | 3 |
+
+---
+
+### headtracker_roll_ratio
+
+Head roll movement vs camera movement ratio
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 1 | 0 | 3 |
+
+---
+
+### headtracker_tilt_ratio
+
+Head tilt movement vs camera movement ratio
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 1 | 0 | 3 |
+
+---
+
 ### headtracker_type
 
 Type of headtrackr dervice
diff --git a/src/main/drivers/headtracker_common.c b/src/main/drivers/headtracker_common.c
index 670a412c501..84484cfb00d 100644
--- a/src/main/drivers/headtracker_common.c
+++ b/src/main/drivers/headtracker_common.c
@@ -40,7 +40,7 @@
 
 #include "drivers/headtracker_common.h"
 
-PG_REGISTER_WITH_RESET_TEMPLATE(headTrackerConfig_t, headTrackerConfig, PG_HEADTRACKER_CONFIG, 0);
+PG_REGISTER_WITH_RESET_TEMPLATE(headTrackerConfig_t, headTrackerConfig, PG_HEADTRACKER_CONFIG, 1);
 
 PG_RESET_TEMPLATE(headTrackerConfig_t, headTrackerConfig,
     .devType = SETTING_HEADTRACKER_TYPE_DEFAULT
diff --git a/src/main/drivers/headtracker_common.h b/src/main/drivers/headtracker_common.h
index 092a00ab0a4..f8b894f9127 100644
--- a/src/main/drivers/headtracker_common.h
+++ b/src/main/drivers/headtracker_common.h
@@ -70,6 +70,9 @@ typedef struct headTrackerVTable_s {
 
 typedef struct headTrackerConfig_s {
     headTrackerDevType_e devType;
+    float pan_ratio;
+    float tilt_ratio;
+    float roll_ratio;
 } headTrackerConfig_t;
 
 PG_DECLARE(headTrackerConfig_t, headTrackerConfig);
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 66d2b098f4b..0cd859462ec 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -4251,3 +4251,24 @@ groups:
       field: devType
       type: uint8_t
       table: headtracker_dev_type
+    - name: headtracker_pan_ratio
+      description: "Head pan movement vs camera movement ratio"
+      type: float
+      default_value: 1
+      field: tilt_ratio
+      min: 0
+      max: 3
+    - name: headtracker_tilt_ratio
+      description: "Head tilt movement vs camera movement ratio"
+      type: float
+      default_value: 1
+      field: tilt_ratio
+      min: 0
+      max: 3
+    - name: headtracker_roll_ratio
+      description: "Head roll movement vs camera movement ratio"
+      type: float
+      default_value: 1
+      field: tilt_ratio
+      min: 0
+      max: 3
\ No newline at end of file
diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index 90216ea8c60..f73f6759bfa 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -317,9 +317,9 @@ void gimbalSerialHeadTrackerReceive(uint16_t c, void *data)
             state->attitude.crcl = c;
             if(checkCrc(&(state->attitude))) {
                 state->expires = micros() + MAX_HEADTRACKER_DATA_AGE_US;
-                state->pan = state->attitude.pan;
-                state->tilt = state->attitude.tilt;
-                state->roll = state->attitude.roll;
+                state->pan = constrain(state->attitude.pan * headTrackerConfig()->pan_ratio) + 0.5, -2048, 2047);
+                state->tilt = constrain(state->attitude.tilt * headTrackerConfig()->tilt_ratio) + 0.5, -2048, 2047);
+                state->roll = constrain(state->attitude.roll * headTrackerConfig()->roll_ratio) + 0.5, -2048, 2047);
                 DEBUG_SET(DEBUG_HEADTRACKING, 2, pktCount++);
             } else {
                 DEBUG_SET(DEBUG_HEADTRACKING, 3, errorCount++);
diff --git a/src/main/io/headtracker_msp.c b/src/main/io/headtracker_msp.c
index 35757665d5e..a1d85bc1bb1 100644
--- a/src/main/io/headtracker_msp.c
+++ b/src/main/io/headtracker_msp.c
@@ -73,9 +73,9 @@ void mspHeadTrackerReceiverNewData(uint8_t *data, int dataSize)
 
     headtrackerMspMessage_t *status = (headtrackerMspMessage_t *)data;
 
-    headTrackerMspDevice.pan = status->pan;
-    headTrackerMspDevice.tilt = status->tilt;
-    headTrackerMspDevice.roll = status->roll;
+    headTrackerMspDevice.pan = constrain((status->pan * headTrackerConfig()->pan_ratio) + 0.5, -2048, 2047);
+    headTrackerMspDevice.tilt = constrain((status->tilt * headTrackerConfig()->tilt_ratio) + 0.5, -2048, 2047);
+    headTrackerMspDevice.roll = constrain((status->roll * headTrackerConfig()->roll_ratio) + 0.5, -2048, 2047);
     headTrackerMspDevice.expires = micros() + MAX_HEADTRACKER_DATA_AGE_US;
 
     UNUSED(status);

From 2f268e9d5ba1a1aa7560b02f93fc74fd6f7d946a Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Mon, 17 Jun 2024 19:54:18 +0200
Subject: [PATCH 279/323] build fixes

---
 src/main/io/gimbal_serial.c   | 6 +++---
 src/main/io/headtracker_msp.c | 1 +
 2 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index f73f6759bfa..bce0f26f3ab 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -317,9 +317,9 @@ void gimbalSerialHeadTrackerReceive(uint16_t c, void *data)
             state->attitude.crcl = c;
             if(checkCrc(&(state->attitude))) {
                 state->expires = micros() + MAX_HEADTRACKER_DATA_AGE_US;
-                state->pan = constrain(state->attitude.pan * headTrackerConfig()->pan_ratio) + 0.5, -2048, 2047);
-                state->tilt = constrain(state->attitude.tilt * headTrackerConfig()->tilt_ratio) + 0.5, -2048, 2047);
-                state->roll = constrain(state->attitude.roll * headTrackerConfig()->roll_ratio) + 0.5, -2048, 2047);
+                state->pan = constrain((state->attitude.pan * headTrackerConfig()->pan_ratio) + 0.5, -2048, 2047);
+                state->tilt = constrain((state->attitude.tilt * headTrackerConfig()->tilt_ratio) + 0.5, -2048, 2047);
+                state->roll = constrain((state->attitude.roll * headTrackerConfig()->roll_ratio) + 0.5, -2048, 2047);
                 DEBUG_SET(DEBUG_HEADTRACKING, 2, pktCount++);
             } else {
                 DEBUG_SET(DEBUG_HEADTRACKING, 3, errorCount++);
diff --git a/src/main/io/headtracker_msp.c b/src/main/io/headtracker_msp.c
index a1d85bc1bb1..1f0e20b45d2 100644
--- a/src/main/io/headtracker_msp.c
+++ b/src/main/io/headtracker_msp.c
@@ -23,6 +23,7 @@
 
 #include "common/utils.h"
 #include "common/time.h"
+#include "common/maths.h"
 
 #include "drivers/headtracker_common.h"
 

From 528e7b4332918189f0c83d87ef7bdff5207c59bc Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Mon, 17 Jun 2024 20:00:00 +0200
Subject: [PATCH 280/323] Fix sitl warnings

---
 src/main/io/gimbal_serial.c   |  6 +++---
 src/main/io/gimbal_serial.h   | 12 ++++++------
 src/main/io/headtracker_msp.c |  6 +++---
 3 files changed, 12 insertions(+), 12 deletions(-)

diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index bce0f26f3ab..39b8f8a3f97 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -317,9 +317,9 @@ void gimbalSerialHeadTrackerReceive(uint16_t c, void *data)
             state->attitude.crcl = c;
             if(checkCrc(&(state->attitude))) {
                 state->expires = micros() + MAX_HEADTRACKER_DATA_AGE_US;
-                state->pan = constrain((state->attitude.pan * headTrackerConfig()->pan_ratio) + 0.5, -2048, 2047);
-                state->tilt = constrain((state->attitude.tilt * headTrackerConfig()->tilt_ratio) + 0.5, -2048, 2047);
-                state->roll = constrain((state->attitude.roll * headTrackerConfig()->roll_ratio) + 0.5, -2048, 2047);
+                state->pan = constrain((state->attitude.pan * headTrackerConfig()->pan_ratio) + 0.5f, -2048, 2047);
+                state->tilt = constrain((state->attitude.tilt * headTrackerConfig()->tilt_ratio) + 0.5f, -2048, 2047);
+                state->roll = constrain((state->attitude.roll * headTrackerConfig()->roll_ratio) + 0.5f, -2048, 2047);
                 DEBUG_SET(DEBUG_HEADTRACKING, 2, pktCount++);
             } else {
                 DEBUG_SET(DEBUG_HEADTRACKING, 3, errorCount++);
diff --git a/src/main/io/gimbal_serial.h b/src/main/io/gimbal_serial.h
index 4f7de6753b7..61d6599e0ed 100644
--- a/src/main/io/gimbal_serial.h
+++ b/src/main/io/gimbal_serial.h
@@ -37,12 +37,12 @@ extern "C" {
 typedef struct gimbalHtkAttitudePkt_s
 {
 	uint8_t  sync[2];       //data synchronization 0xA5, 0x5A
-	uint64_t mode:3;        //Gimbal Mode [0~7] [Only 0 1 2 modes are supported for the time being]
-	int64_t  sensibility:5; // Stabilization sensibility [-16~15]
-	uint64_t reserved:4;    //hold on to one's reserve
-	int64_t  roll:12;       //Roll angle [-2048~2047] => [-180~180]
-	int64_t  tilt:12;       //Pich angle [-2048~2047] => [-180~180]
-	int64_t  pan:12;        //Yaw angle [-2048~2047] => [-180~180]
+	uint8_t mode:3;        //Gimbal Mode [0~7] [Only 0 1 2 modes are supported for the time being]
+	int16_t  sensibility:5; // Stabilization sensibility [-16~15]
+	uint8_t reserved:4;    //hold on to one's reserve
+	int32_t  roll:12;       //Roll angle [-2048~2047] => [-180~180]
+	int32_t  tilt:12;       //Pich angle [-2048~2047] => [-180~180]
+	int32_t  pan:12;        //Yaw angle [-2048~2047] => [-180~180]
 	uint8_t  crch;          //Data validation H
 	uint8_t  crcl;          //Data validation L
 } __attribute__((packed)) gimbalHtkAttitudePkt_t;
diff --git a/src/main/io/headtracker_msp.c b/src/main/io/headtracker_msp.c
index 1f0e20b45d2..6f0fdf3b3f2 100644
--- a/src/main/io/headtracker_msp.c
+++ b/src/main/io/headtracker_msp.c
@@ -74,9 +74,9 @@ void mspHeadTrackerReceiverNewData(uint8_t *data, int dataSize)
 
     headtrackerMspMessage_t *status = (headtrackerMspMessage_t *)data;
 
-    headTrackerMspDevice.pan = constrain((status->pan * headTrackerConfig()->pan_ratio) + 0.5, -2048, 2047);
-    headTrackerMspDevice.tilt = constrain((status->tilt * headTrackerConfig()->tilt_ratio) + 0.5, -2048, 2047);
-    headTrackerMspDevice.roll = constrain((status->roll * headTrackerConfig()->roll_ratio) + 0.5, -2048, 2047);
+    headTrackerMspDevice.pan = constrain((status->pan * headTrackerConfig()->pan_ratio) + 0.5f, -2048, 2047);
+    headTrackerMspDevice.tilt = constrain((status->tilt * headTrackerConfig()->tilt_ratio) + 0.5f, -2048, 2047);
+    headTrackerMspDevice.roll = constrain((status->roll * headTrackerConfig()->roll_ratio) + 0.5f, -2048, 2047);
     headTrackerMspDevice.expires = micros() + MAX_HEADTRACKER_DATA_AGE_US;
 
     UNUSED(status);

From 74c309f2b4536322f8dbfeb928a0376e169ae209 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Mon, 17 Jun 2024 20:07:15 +0200
Subject: [PATCH 281/323] fix reset function

---
 src/main/drivers/headtracker_common.c | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/src/main/drivers/headtracker_common.c b/src/main/drivers/headtracker_common.c
index 84484cfb00d..d08c5d818bf 100644
--- a/src/main/drivers/headtracker_common.c
+++ b/src/main/drivers/headtracker_common.c
@@ -43,7 +43,10 @@
 PG_REGISTER_WITH_RESET_TEMPLATE(headTrackerConfig_t, headTrackerConfig, PG_HEADTRACKER_CONFIG, 1);
 
 PG_RESET_TEMPLATE(headTrackerConfig_t, headTrackerConfig,
-    .devType = SETTING_HEADTRACKER_TYPE_DEFAULT
+    .devType = SETTING_HEADTRACKER_TYPE_DEFAULT,
+    .pan_ratio = SETTING_HEADTRACKER_PAN_RATIO_DEFAULT,
+    .tilt_ratio = SETTING_HEADTRACKER_TILT_RATIO_DEFAULT,
+    .roll_ratio = SETTING_HEADTRACKER_ROLL_RATIO_DEFAULT,
 );
 
 static headTrackerDevice_t *commonHeadTrackerDevice = NULL;

From a8255fbceeff527b3263826f06a473d2cc5e3e7f Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Mon, 17 Jun 2024 16:39:04 -0400
Subject: [PATCH 282/323] Update dev-builds.yml

Make tag more readable.
---
 .github/workflows/dev-builds.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml
index 8bfa1dbc5ef..5510b258b2f 100644
--- a/.github/workflows/dev-builds.yml
+++ b/.github/workflows/dev-builds.yml
@@ -203,7 +203,7 @@ jobs:
         uses: softprops/action-gh-release@v2
         with:
           name: inav-${{ steps.version.outputs.version }}-dev-${{ steps.date.outputs.today }}-${{ github.run_number }}-${{ github.sha }}
-          tag_name: v${{ steps.date.outputs.today }}.${{ github.run_number }}
+          tag_name: v${{ steps.version.outputs.version }}-${{ steps.date.outputs.today }}.${{ github.run_number }}
           # To create release on a different repo, we need a token setup
           token: ${{ secrets.NIGHTLY_TOKEN }}
           repository: iNavFlight/inav-nightly

From b402d685c650bb33d9848ceaa9eaec1f8727c63b Mon Sep 17 00:00:00 2001
From: Sensei <deepmagicbeginshere@gmail.com>
Date: Tue, 18 Jun 2024 02:10:18 -0500
Subject: [PATCH 283/323] Programming framework doc - loiter radius

Clarified worsing
---
 docs/Programming Framework.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md
index e0199cfd714..4eb7744e54e 100644
--- a/docs/Programming Framework.md	
+++ b/docs/Programming Framework.md	
@@ -86,7 +86,7 @@ IPF can be edited using INAV Configurator user interface, or via CLI. To use COn
 | 38            | Override RC Channel           | Overrides channel set by `Operand A` to value of `Operand B`. Note operand A should normally be set as a "Value", NOT as "Get RC Channel"|
 | 39            | Set Heading Target            | Sets heading-hold target to `Operand A`, in centidegrees. Value wraps-around. |
 | 40            | Modulo                        | Modulo. Divide `Operand A` by `Operand B` and returns the remainder |
-| 41            | Override Loiter Radius        | 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. |
+| 41            | Override Loiter Radius        | Sets the loiter radius to `Operand A` [`0` : `100000`] in cm. Must be larger than the loiter radius set in the **Advanced Tuning**. |
 | 42            | Set Control 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            | 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` |

From 1e1962413061991495cf19a6999cee28327992f6 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Tue, 18 Jun 2024 12:59:37 +0200
Subject: [PATCH 284/323] Update release notes with recommendation fro full
 chip erase.

---
 .github/workflows/dev-builds.yml | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml
index 5510b258b2f..ca31566da19 100644
--- a/.github/workflows/dev-builds.yml
+++ b/.github/workflows/dev-builds.yml
@@ -217,6 +217,10 @@ jobs:
           body: |
             ${{ steps.notes.outputs.notes }}
 
+            ### Flashing
+            These are nightly builds and configuration settings can be added and removed often. Flashing with Full chip erase is strongly recommended to avoid issues.
+            Firmware related issues should be open in the iNavflight/inav repository, not in inav-nightly.
+
             ### Repository:
             ${{ github.repository }} ([link](${{ github.event.repository.html_url }}))
 

From 305f7231d72b9abc412cf771a31dd3cb45bac8b1 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Tue, 18 Jun 2024 13:38:04 +0100
Subject: [PATCH 285/323] change to using Baro rate

---
 docs/Settings.md                              |  2 +-
 src/main/fc/settings.yaml                     |  2 +-
 src/main/navigation/navigation.h              |  2 +-
 src/main/navigation/navigation_multicopter.c  | 62 ++++++++++---------
 .../navigation/navigation_pos_estimator.c     |  2 +-
 5 files changed, 36 insertions(+), 34 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index 08a811debb3..0009455db5a 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -3584,7 +3584,7 @@ Multicopter hover throttle hint for altitude controller. Should be set to approx
 
 ### nav_mc_inverted_crash_detection
 
-Setting a value > 0 enables inverted crash detection for multirotors. It is intended for situations where the multirotor has crashed inverted on the ground and can't be manually disarmed due to loss of control or some other reason. When enabled the setting defines the additional number of seconds before disarm beyond a minimum fixed time delay such that the minimum possible time delay before disarm is 4s.
+Setting a value > 0 enables inverted crash detection for multirotors. It is intended for situations where the multirotor has crashed inverted on the ground and can't be manually disarmed due to loss of control or for some other reason. When enabled this setting defines the additional number of seconds before disarm beyond a minimum fixed time delay of 3s. Requires a barometer to work.
 
 | Default | Min | Max |
 | --- | --- | --- |
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 068ddf63af4..40a65f3441f 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -2416,7 +2416,7 @@ groups:
         field: general.flags.landing_bump_detection
         type: bool
       - name: nav_mc_inverted_crash_detection
-        description: "Setting a value > 0 enables inverted crash detection for multirotors. It is intended for situations where the multirotor has crashed inverted on the ground and can't be manually disarmed due to loss of control or some other reason. When enabled the setting defines the additional number of seconds before disarm beyond a minimum fixed time delay such that the minimum possible time delay before disarm is 4s."
+        description: "Setting a value > 0 enables inverted crash detection for multirotors. It is intended for situations where the multirotor has crashed inverted on the ground and can't be manually disarmed due to loss of control or for some other reason. When enabled this setting defines the additional number of seconds before disarm beyond a minimum fixed time delay of 3s. Requires a barometer to work."
         default_value: 0
         field: mc.inverted_crash_detection
         min: 0
diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
index 1a19de7b34d..101b41cd637 100644
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@@ -689,7 +689,7 @@ float getEstimatedAglPosition(void);
 bool isEstimatedAglTrusted(void);
 
 void checkManualEmergencyLandingControl(bool forcedActivation);
-float updateBaroAltitudeRate(float newBaroAltRate, bool updateValue);
+void updateBaroAltitudeRate(float newBaroAltRate);
 bool rthAltControlStickOverrideCheck(uint8_t axis);
 
 int8_t navCheckActiveAngleHoldAxis(void);
diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c
index 41903ff5f3f..7206ac2472d 100644
--- a/src/main/navigation/navigation_multicopter.c
+++ b/src/main/navigation/navigation_multicopter.c
@@ -757,15 +757,12 @@ bool isMulticopterFlying(void)
 /*-----------------------------------------------------------
  * Multicopter land detector
  *-----------------------------------------------------------*/
-  #if defined(USE_BARO)
-float updateBaroAltitudeRate(float newBaroAltRate, bool updateValue)
-{
-    static float baroAltRate;
-    if (updateValue) {
-        baroAltRate = newBaroAltRate;
-    }
+#if defined(USE_BARO)
+static float baroAltRate;
 
-    return baroAltRate;
+void updateBaroAltitudeRate(float newBaroAltRate)
+{
+    baroAltRate = newBaroAltRate;
 }
 
 static bool isLandingGbumpDetected(timeMs_t currentTimeMs)
@@ -776,7 +773,6 @@ static bool isLandingGbumpDetected(timeMs_t currentTimeMs)
      * Throttle trigger: must be below hover throttle with lower threshold for manual throttle control */
 
     static timeMs_t gSpikeDetectTimeMs = 0;
-    float baroAltRate = updateBaroAltitudeRate(0, false);
 
     if (!gSpikeDetectTimeMs && acc.accADCf[Z] > 2.0f && baroAltRate < 0.0f) {
         gSpikeDetectTimeMs = currentTimeMs;
@@ -794,46 +790,52 @@ static bool isLandingGbumpDetected(timeMs_t currentTimeMs)
 
     return false;
 }
-#endif
-bool isMulticopterCrashedInverted(void)
+
+bool isMulticopterCrashedInverted(timeMs_t currentTimeMs)
 {
-    static timeMs_t startTime = 0;
+    /* Disarms MR if inverted on the ground. Checks vertical velocity is low based on Baro rate below 2 m/s */
 
-    if (ABS(attitude.values.roll) > 1000 || ABS(attitude.values.pitch) > 700) {
-        static uint32_t initialAltitude;
+    static timeMs_t startTime = 0;
 
+    if ((ABS(attitude.values.roll) > 1000 || ABS(attitude.values.pitch) > 700) && fabsf(baroAltRate) < 200.0f) {
         if (startTime == 0) {
-            startTime = millis();
-            initialAltitude = navGetCurrentActualPositionAndVelocity()->pos.z;
-            return false;
-        } else if (ABS(initialAltitude - navGetCurrentActualPositionAndVelocity()->pos.z) < 200) {
-            /* Check altitude change < 2m during disarm timeout period indicating MR not in flight.
-             * Minimum 3s disarm delay + extra user set delay time = Min time of 4s given min user setting is 1s if enabled */
-
-            uint16_t disarmTimeDelay = 3000 + S2MS(navConfig()->mc.inverted_crash_detection);
-            return millis() - startTime > disarmTimeDelay;
+            startTime = currentTimeMs;
         }
+
+        /* Minimum 3s disarm delay + extra user set delay time (min overall delay of 4s) */
+        uint16_t disarmTimeDelay = 3000 + S2MS(navConfig()->mc.inverted_crash_detection);
+        return currentTimeMs - startTime > disarmTimeDelay;
     }
 
     startTime = 0;
     return false;
 }
+#endif
 
 bool isMulticopterLandingDetected(void)
 {
     DEBUG_SET(DEBUG_LANDING, 4, 0);
     DEBUG_SET(DEBUG_LANDING, 3, averageAbsGyroRates() * 100);
 
-    if (navConfig()->mc.inverted_crash_detection && isMulticopterCrashedInverted()) {
-        ENABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
-        disarm(DISARM_LANDING);
-    }
-
     const timeMs_t currentTimeMs = millis();
 
 #if defined(USE_BARO)
-    if (sensors(SENSOR_BARO) && navConfig()->general.flags.landing_bump_detection && isLandingGbumpDetected(currentTimeMs)) {
-        return true;    // Landing flagged immediately if landing bump detected
+    if (sensors(SENSOR_BARO)) {
+        /* Inverted crash landing detection - immediate disarm */
+        if (navConfig()->mc.inverted_crash_detection && isMulticopterCrashedInverted(currentTimeMs)) {
+            ENABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
+            disarm(DISARM_LANDING);
+        }
+
+        /* G bump landing detection *
+         * Only used when xy velocity is low or failsafe is active */
+        bool gBumpDetectionUsable = navConfig()->general.flags.landing_bump_detection &&
+                                    ((posControl.flags.estPosStatus >= EST_USABLE && posControl.actualState.velXY < MC_LAND_CHECK_VEL_XY_MOVING) ||
+                                    FLIGHT_MODE(FAILSAFE_MODE));
+
+        if (gBumpDetectionUsable && isLandingGbumpDetected(currentTimeMs)) {
+            return true;    // Landing flagged immediately if landing bump detected
+        }
     }
 #endif
 
diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
index 30fa4012e46..d5a342173d3 100755
--- a/src/main/navigation/navigation_pos_estimator.c
+++ b/src/main/navigation/navigation_pos_estimator.c
@@ -299,7 +299,7 @@ void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs)
             static float baroAltPrevious = 0;
             posEstimator.baro.baroAltRate = (posEstimator.baro.alt - baroAltPrevious) / US2S(baroDtUs);
             baroAltPrevious = posEstimator.baro.alt;
-            updateBaroAltitudeRate(posEstimator.baro.baroAltRate, true);
+            updateBaroAltitudeRate(posEstimator.baro.baroAltRate);
         }
     }
     else {

From 500bedb5bf9544c7598a7fa240f654d683155787 Mon Sep 17 00:00:00 2001
From: 0crap <31951195+0crap@users.noreply.github.com>
Date: Tue, 18 Jun 2024 19:12:29 +0200
Subject: [PATCH 286/323] Update dev-builds.yml

Grammar
---
 .github/workflows/dev-builds.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml
index ca31566da19..e0f648e003e 100644
--- a/.github/workflows/dev-builds.yml
+++ b/.github/workflows/dev-builds.yml
@@ -219,7 +219,7 @@ jobs:
 
             ### Flashing
             These are nightly builds and configuration settings can be added and removed often. Flashing with Full chip erase is strongly recommended to avoid issues.
-            Firmware related issues should be open in the iNavflight/inav repository, not in inav-nightly.
+            Firmware related issues should be opened in the iNavflight/inav repository, not in inav-nightly.
 
             ### Repository:
             ${{ github.repository }} ([link](${{ github.event.repository.html_url }}))

From ee4dd48a042060d79b11f31c49f77a02def6d399 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Tue, 18 Jun 2024 19:53:49 +0100
Subject: [PATCH 287/323] fix landed state message priority

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

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 207b3cb73ec..8cfde49c768 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -5644,9 +5644,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
         const char *invertedInfoMessage = NULL;
 
         if (ARMING_FLAG(ARMED)) {
-            if (STATE(LANDING_DETECTED)) {
-                messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_LANDED);
-            } else if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
+            if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
                 /* ADDS MAXIMUM OF 5 MESSAGES TO TOTAL */
                 if (navGetCurrentStateFlags() & NAV_AUTO_WP_DONE) {
                     messages[messageCount++] = STATE(LANDING_DETECTED) ? OSD_MESSAGE_STR(OSD_MSG_WP_LANDED) : OSD_MESSAGE_STR(OSD_MSG_WP_FINISHED);
@@ -5703,7 +5701,10 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                     // if RTH activated whilst WP mode selected, remind pilot to cancel WP mode to exit RTH
                     messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_WP_RTH_CANCEL);
                 }
-            } else {    /* Messages shown only when Failsafe, WP, RTH or Emergency Landing not active */
+            } else if (STATE(LANDING_DETECTED)) {
+                messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_LANDED);
+            } else {
+                /* Messages shown only when Failsafe, WP, RTH or Emergency Landing not active and landed state inactive */
                 /* ADDS MAXIMUM OF 4 MESSAGES TO TOTAL */
                 if (STATE(AIRPLANE)) {      /* ADDS MAXIMUM OF 3 MESSAGES TO TOTAL */
 #ifdef USE_FW_AUTOLAND

From f6c034886d0f69177247f7bf9d2a18a443f798f2 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Tue, 18 Jun 2024 21:05:05 +0100
Subject: [PATCH 288/323] fix mission planner message

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

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 8cfde49c768..01bdc1764fb 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -5645,7 +5645,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
 
         if (ARMING_FLAG(ARMED)) {
             if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
-                /* ADDS MAXIMUM OF 5 MESSAGES TO TOTAL */
+                /* ADDS MAXIMUM OF 3 MESSAGES TO TOTAL NORMALLY, 5 MESSAGES DURING FAILSAFE */
                 if (navGetCurrentStateFlags() & NAV_AUTO_WP_DONE) {
                     messages[messageCount++] = STATE(LANDING_DETECTED) ? OSD_MESSAGE_STR(OSD_MSG_WP_LANDED) : OSD_MESSAGE_STR(OSD_MSG_WP_FINISHED);
                 } else if (NAV_Status.state == MW_NAV_STATE_WP_ENROUTE) {
@@ -5705,7 +5705,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                 messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_LANDED);
             } else {
                 /* Messages shown only when Failsafe, WP, RTH or Emergency Landing not active and landed state inactive */
-                /* ADDS MAXIMUM OF 4 MESSAGES TO TOTAL */
+                /* ADDS MAXIMUM OF 3 MESSAGES TO TOTAL */
                 if (STATE(AIRPLANE)) {      /* ADDS MAXIMUM OF 3 MESSAGES TO TOTAL */
 #ifdef USE_FW_AUTOLAND
                     if (canFwLandingBeCancelled()) {
@@ -5763,10 +5763,6 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                         messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALTITUDE_HOLD);
                     }
                 }
-
-                if (posControl.flags.wpMissionPlannerActive) {
-                    messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MISSION_PLANNER);
-                }
             }
         } else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) {    /* ADDS MAXIMUM OF 2 MESSAGES TO TOTAL */
             unsigned invalidIndex;
@@ -5795,7 +5791,11 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
             }
         }
 
-        /* Messages that are shown regardless of Arming state - ADDS MAXIMUM OF 1 MESSAGES TO TOTAL */
+        /* Messages that are shown regardless of Arming state */
+        /* ADDS MAXIMUM OF 2 MESSAGES TO TOTAL NORMALLY, 1 MESSAGE DURING FAILSAFE */
+        if (posControl.flags.wpMissionPlannerActive && !FLIGHT_MODE(FAILSAFE_MODE)) {
+            messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MISSION_PLANNER);
+        }
 
         // The following has been commented out as it will be added in #9688
         // uint16_t rearmMs = (emergInflightRearmEnabled()) ? emergencyInFlightRearmTimeMS() : 0;

From 0843ff7c7d1509a5920a1859643348addbf9fa42 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Wed, 19 Jun 2024 09:50:47 +0200
Subject: [PATCH 289/323] Fix copy and paste error on settings.yaml

---
 src/main/fc/settings.yaml | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 169d52024fb..6430ae9ceee 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -4255,7 +4255,7 @@ groups:
       description: "Head pan movement vs camera movement ratio"
       type: float
       default_value: 1
-      field: tilt_ratio
+      field: pan_ratio
       min: 0
       max: 3
     - name: headtracker_tilt_ratio
@@ -4269,6 +4269,6 @@ groups:
       description: "Head roll movement vs camera movement ratio"
       type: float
       default_value: 1
-      field: tilt_ratio
+      field: roll_ratio
       min: 0
       max: 3
\ No newline at end of file

From 1dbe0b781179464af14bd28974cb9d8fa34ac503 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Wed, 19 Jun 2024 10:44:25 +0100
Subject: [PATCH 290/323] inhibit during turtle mode

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

diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c
index 7206ac2472d..cd23edd6b32 100644
--- a/src/main/navigation/navigation_multicopter.c
+++ b/src/main/navigation/navigation_multicopter.c
@@ -822,7 +822,7 @@ bool isMulticopterLandingDetected(void)
 #if defined(USE_BARO)
     if (sensors(SENSOR_BARO)) {
         /* Inverted crash landing detection - immediate disarm */
-        if (navConfig()->mc.inverted_crash_detection && isMulticopterCrashedInverted(currentTimeMs)) {
+        if (navConfig()->mc.inverted_crash_detection && !FLIGHT_MODE(TURTLE_MODE) && isMulticopterCrashedInverted(currentTimeMs)) {
             ENABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
             disarm(DISARM_LANDING);
         }

From 70717526b8b2c2f7c6326188f6263c0f63b3f31c Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Wed, 19 Jun 2024 11:51:04 +0100
Subject: [PATCH 291/323] improve setting description

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

diff --git a/docs/Settings.md b/docs/Settings.md
index 4c32a2b60e3..36b6abb54eb 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -3654,7 +3654,7 @@ Multicopter hover throttle hint for altitude controller. Should be set to approx
 
 ### nav_mc_inverted_crash_detection
 
-Setting a value > 0 enables inverted crash detection for multirotors. It is intended for situations where the multirotor has crashed inverted on the ground and can't be manually disarmed due to loss of control or for some other reason. When enabled this setting defines the additional number of seconds before disarm beyond a minimum fixed time delay of 3s. Requires a barometer to work.
+Setting a value > 0 enables inverted crash detection for multirotors. It will auto disarm in situations where the multirotor has crashed inverted on the ground and can't be manually disarmed due to loss of control or for some other reason. When enabled this setting defines the additional number of seconds before disarm beyond a minimum fixed time delay of 3s. Requires a barometer to work.
 
 | Default | Min | Max |
 | --- | --- | --- |
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 97f37276508..44d77f18019 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -2468,7 +2468,7 @@ groups:
         field: general.flags.landing_bump_detection
         type: bool
       - name: nav_mc_inverted_crash_detection
-        description: "Setting a value > 0 enables inverted crash detection for multirotors. It is intended for situations where the multirotor has crashed inverted on the ground and can't be manually disarmed due to loss of control or for some other reason. When enabled this setting defines the additional number of seconds before disarm beyond a minimum fixed time delay of 3s. Requires a barometer to work."
+        description: "Setting a value > 0 enables inverted crash detection for multirotors. It will auto disarm in situations where the multirotor has crashed inverted on the ground and can't be manually disarmed due to loss of control or for some other reason. When enabled this setting defines the additional number of seconds before disarm beyond a minimum fixed time delay of 3s. Requires a barometer to work."
         default_value: 0
         field: mc.inverted_crash_detection
         min: 0

From ac3834f2934a199da25b69447f6f7a7d2680b1ae Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Wed, 19 Jun 2024 21:47:22 +0200
Subject: [PATCH 292/323] Cleanup

---
 src/main/drivers/headtracker_common.h |  1 -
 src/main/io/gimbal_serial.c           | 77 ++++++++++++++-------------
 src/main/io/gimbal_serial.h           |  4 --
 3 files changed, 41 insertions(+), 41 deletions(-)

diff --git a/src/main/drivers/headtracker_common.h b/src/main/drivers/headtracker_common.h
index f8b894f9127..769bfb206eb 100644
--- a/src/main/drivers/headtracker_common.h
+++ b/src/main/drivers/headtracker_common.h
@@ -90,7 +90,6 @@ int headTrackerCommonGetPanPWM(const headTrackerDevice_t *headTrackerDevice);
 int headTrackerCommonGetTiltPWM(const headTrackerDevice_t *headTrackerDevice);
 int headTrackerCommonGetRollPWM(const headTrackerDevice_t *headTrackerDevice);
 
-
 void taskUpdateHeadTracker(timeUs_t currentTimeUs);
 
 bool headtrackerCommonIsEnabled(void);
diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index 39b8f8a3f97..dbb30d08a47 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -58,7 +58,6 @@ STATIC_ASSERT(sizeof(gimbalHtkAttitudePkt_t) == 10, gimbalHtkAttitudePkt_t_size_
 static volatile uint8_t txBuffer[GIMBAL_SERIAL_BUFFER_SIZE];
 
 static gimbalSerialHtrkState_t headTrackerState = { 
-    .expires = 0,
     .payloadSize = 0,
     .state = WAITING_HDR1,
 };
@@ -79,6 +78,30 @@ static gimbalDevice_t serialGimbalDevice = {
     .vTable = &gimbalSerialVTable
 };
 
+#if (defined(USE_HEADTRACKER) && defined(USE_HEADTRACKER_SERIAL))
+
+static headTrackerVTable_t headTrackerVTable = {
+    .process = headtrackerSerialProcess,
+    .getDeviceType = headtrackerSerialGetDeviceType,
+    .isReady = headTrackerSerialIsReady,
+    .isValid = headTrackerSerialIsValid,
+    .getPanPWM = headTrackerSerialGetPanPWM,
+    .getTiltPWM = headTrackerSerialGetTiltPWM,
+    .getRollPWM = headTrackerSerialGetRollPWM,
+};
+
+
+headTrackerDevice_t headTrackerDevice = {
+    .vTable = &headTrackerVTable,
+    .pan = 0,
+    .tilt = 0,
+    .roll = 0,
+    .expires = 0
+};
+
+#endif
+
+
 gimbalDevType_e gimbalSerialGetDeviceType(const gimbalDevice_t *gimbalDevice)
 {
     UNUSED(gimbalDevice);
@@ -207,14 +230,14 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
     }
 
     if(IS_RC_MODE_ACTIVE(BOXGIMBALHTRK)) {
-        if (gimbalCommonHtrkIsEnabled() && (micros() < headTrackerState.expires)) {
-            attitude.tilt = headTrackerState.tilt;
-            attitude.pan = headTrackerState.pan;
-            attitude.roll = headTrackerState.roll;
+        if (gimbalCommonHtrkIsEnabled() && (micros() < headTrackerDevice.expires)) {
+            attitude.pan = headTrackerDevice.pan;
+            attitude.tilt = headTrackerDevice.tilt;
+            attitude.roll = headTrackerDevice.roll;
             DEBUG_SET(DEBUG_HEADTRACKING, 4, 1);
         } else {
-            attitude.tilt = 0;
             attitude.pan = 0;
+            attitude.tilt = 0;
             attitude.roll = 0;
             DEBUG_SET(DEBUG_HEADTRACKING, 4, -1);
         }
@@ -316,10 +339,10 @@ void gimbalSerialHeadTrackerReceive(uint16_t c, void *data)
         case WAITING_CRCL:
             state->attitude.crcl = c;
             if(checkCrc(&(state->attitude))) {
-                state->expires = micros() + MAX_HEADTRACKER_DATA_AGE_US;
-                state->pan = constrain((state->attitude.pan * headTrackerConfig()->pan_ratio) + 0.5f, -2048, 2047);
-                state->tilt = constrain((state->attitude.tilt * headTrackerConfig()->tilt_ratio) + 0.5f, -2048, 2047);
-                state->roll = constrain((state->attitude.roll * headTrackerConfig()->roll_ratio) + 0.5f, -2048, 2047);
+                headTrackerDevice.expires = micros() + MAX_HEADTRACKER_DATA_AGE_US;
+                headTrackerDevice.pan = constrain((state->attitude.pan * headTrackerConfig()->pan_ratio) + 0.5f, -2048, 2047);
+                headTrackerDevice.tilt = constrain((state->attitude.tilt * headTrackerConfig()->tilt_ratio) + 0.5f, -2048, 2047);
+                headTrackerDevice.roll = constrain((state->attitude.roll * headTrackerConfig()->roll_ratio) + 0.5f, -2048, 2047);
                 DEBUG_SET(DEBUG_HEADTRACKING, 2, pktCount++);
             } else {
                 DEBUG_SET(DEBUG_HEADTRACKING, 3, errorCount++);
@@ -331,20 +354,6 @@ void gimbalSerialHeadTrackerReceive(uint16_t c, void *data)
 
 #if (defined(USE_HEADTRACKER) && defined(USE_HEADTRACKER_SERIAL))
 
-static headTrackerVTable_t headTrackerVTable = {
-    .process = headtrackerSerialProcess,
-    .getDeviceType = headtrackerSerialGetDeviceType,
-    .isReady = headTrackerSerialIsReady,
-    .isValid = headTrackerSerialIsValid,
-    .getPanPWM = headTrackerSerialGetPanPWM,
-    .getTiltPWM = headTrackerSerialGetTiltPWM,
-    .getRollPWM = headTrackerSerialGetRollPWM,
-};
-
-headTrackerDevice_t headTrackerDevice = {
-    .vTable = &headTrackerVTable,
-};
-
 bool gimbalSerialHeadTrackerDetect(void)
 {
     bool singleUart = gimbalSerialConfig()->singleUart;
@@ -389,8 +398,8 @@ bool gimbalSerialHeadTrackerInit(void)
 
 void headtrackerSerialProcess(headTrackerDevice_t *headTrackerDevice, timeUs_t currentTimeUs)
 {
+    UNUSED(headTrackerDevice);
     UNUSED(currentTimeUs);
-    headTrackerDevice->expires = headTrackerState.expires;
     return;
 }
 
@@ -413,15 +422,13 @@ bool headTrackerSerialIsReady(const headTrackerDevice_t *headTrackerDevice)
 
 bool headTrackerSerialIsValid(const headTrackerDevice_t *headTrackerDevice)
 {
-    UNUSED(headTrackerDevice);
-    return micros() < headTrackerState.expires;
+    return micros() < headTrackerDevice->expires;
 }
 
 int headTrackerSerialGetPanPWM(const headTrackerDevice_t *headTrackerDevice)
 {
-    UNUSED(headTrackerDevice);
-    if(micros() < headTrackerState.expires) {
-        return scaleRange(headTrackerState.pan, -2048, 2047, PWM_RANGE_MIN, PWM_RANGE_MAX);
+    if(micros() < headTrackerDevice->expires) {
+        return scaleRange(headTrackerDevice->pan, -2048, 2047, PWM_RANGE_MIN, PWM_RANGE_MAX);
     }
 
     return PWM_RANGE_MIDDLE;
@@ -429,9 +436,8 @@ int headTrackerSerialGetPanPWM(const headTrackerDevice_t *headTrackerDevice)
 
 int headTrackerSerialGetTiltPWM(const headTrackerDevice_t *headTrackerDevice)
 {
-    UNUSED(headTrackerDevice);
-    if(micros() < headTrackerState.expires) {
-        return scaleRange(headTrackerState.tilt, -2048, 2047, PWM_RANGE_MIN, PWM_RANGE_MAX);
+    if(micros() < headTrackerDevice->expires) {
+        return scaleRange(headTrackerDevice->tilt, -2048, 2047, PWM_RANGE_MIN, PWM_RANGE_MAX);
     }
 
     return PWM_RANGE_MIDDLE;
@@ -439,9 +445,8 @@ int headTrackerSerialGetTiltPWM(const headTrackerDevice_t *headTrackerDevice)
 
 int headTrackerSerialGetRollPWM(const headTrackerDevice_t *headTrackerDevice)
 {
-    UNUSED(headTrackerDevice);
-    if(micros() < headTrackerState.expires) {
-        return scaleRange(headTrackerState.roll, -2048, 2047, PWM_RANGE_MIN, PWM_RANGE_MAX);
+    if(micros() < headTrackerDevice->expires) {
+        return scaleRange(headTrackerDevice->roll, -2048, 2047, PWM_RANGE_MIN, PWM_RANGE_MAX);
     }
 
     return PWM_RANGE_MIDDLE;
diff --git a/src/main/io/gimbal_serial.h b/src/main/io/gimbal_serial.h
index 61d6599e0ed..6c6bf427672 100644
--- a/src/main/io/gimbal_serial.h
+++ b/src/main/io/gimbal_serial.h
@@ -59,11 +59,7 @@ typedef enum {
 } gimbalHeadtrackerState_e;
 
 typedef struct gimbalSerialHtrkState_s {
-    timeUs_t expires;
     uint8_t  payloadSize;
-	int16_t  roll;   //Roll angle [-2048~2047] => [-180~180]
-	int16_t  tilt;   //Pich angle [-2048~2047] => [-180~180]
-	int16_t  pan;    //Yaw angle [-2048~2047] => [-180~180]
     gimbalHeadtrackerState_e state;
     gimbalHtkAttitudePkt_t attitude;
 } gimbalSerialHtrkState_t;

From 4e99f80c62455702597edbb2d7dfd3d6fac4bde1 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Wed, 19 Jun 2024 22:59:11 +0200
Subject: [PATCH 293/323] Increase max for heatracker ratio

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

diff --git a/docs/Settings.md b/docs/Settings.md
index e2968169e1c..bd4f06b0cb4 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -1828,7 +1828,7 @@ Head pan movement vs camera movement ratio
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 1 | 0 | 3 |
+| 1 | 0 | 5 |
 
 ---
 
@@ -1838,7 +1838,7 @@ Head roll movement vs camera movement ratio
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 1 | 0 | 3 |
+| 1 | 0 | 5 |
 
 ---
 
@@ -1848,7 +1848,7 @@ Head tilt movement vs camera movement ratio
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 1 | 0 | 3 |
+| 1 | 0 | 5 |
 
 ---
 
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 6430ae9ceee..a0ae4c402bc 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -4257,18 +4257,18 @@ groups:
       default_value: 1
       field: pan_ratio
       min: 0
-      max: 3
+      max: 5
     - name: headtracker_tilt_ratio
       description: "Head tilt movement vs camera movement ratio"
       type: float
       default_value: 1
       field: tilt_ratio
       min: 0
-      max: 3
+      max: 5
     - name: headtracker_roll_ratio
       description: "Head roll movement vs camera movement ratio"
       type: float
       default_value: 1
       field: roll_ratio
       min: 0
-      max: 3
\ No newline at end of file
+      max: 5
\ No newline at end of file

From 439c561b30f391cc3fb2411d5c40692c56492ede Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Wed, 19 Jun 2024 17:39:45 -0400
Subject: [PATCH 294/323] Add link to nightly builds

---
 readme.md | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/readme.md b/readme.md
index fac247ef9ab..7b4b14cbc69 100644
--- a/readme.md
+++ b/readme.md
@@ -124,3 +124,10 @@ Please refer to the development section in the [docs/development](https://github
 
 ## INAV Releases
 https://github.com/iNavFlight/inav/releases
+
+## Nightly builds
+
+https://github.com/iNavFlight/inav-nightly/releases
+
+https://github.com/iNavFlight/inav-configurator-nightly/releases
+

From 8f5a9b86e2443b97444035a52e10bb9480a836d5 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Wed, 19 Jun 2024 17:42:28 -0400
Subject: [PATCH 295/323] Downgrade links into developer section

---
 readme.md | 9 +++++----
 1 file changed, 5 insertions(+), 4 deletions(-)

diff --git a/readme.md b/readme.md
index 7b4b14cbc69..6376efc7726 100644
--- a/readme.md
+++ b/readme.md
@@ -122,12 +122,13 @@ Before creating new issues please check to see if there is an existing one, sear
 
 Please refer to the development section in the [docs/development](https://github.com/iNavFlight/inav/tree/master/docs/development) folder.
 
-## INAV Releases
-https://github.com/iNavFlight/inav/releases
-
-## Nightly builds
+Nightly builds are avaialble for testing on the following links:
 
 https://github.com/iNavFlight/inav-nightly/releases
 
 https://github.com/iNavFlight/inav-configurator-nightly/releases
 
+## INAV Releases
+https://github.com/iNavFlight/inav/releases
+
+

From 0550aac4f63b1248a6f61301a2843995e4337ab5 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Thu, 20 Jun 2024 17:35:49 +0200
Subject: [PATCH 296/323] Change default headtracker type to NONE

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

diff --git a/docs/Settings.md b/docs/Settings.md
index bd4f06b0cb4..a23ff66a4d2 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -1858,7 +1858,7 @@ Type of headtrackr dervice
 
 | Default | Min | Max |
 | --- | --- | --- |
-| SERIAL |  |  |
+| NONE |  |  |
 
 ---
 
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index a0ae4c402bc..df5be7efca6 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -4247,7 +4247,7 @@ groups:
     members:
     - name: headtracker_type
       description: "Type of headtrackr dervice"
-      default_value: "SERIAL"
+      default_value: "NONE"
       field: devType
       type: uint8_t
       table: headtracker_dev_type

From 005a8188b7466297cfd6b9899a50e4218af77f3d Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Thu, 20 Jun 2024 20:05:08 +0200
Subject: [PATCH 297/323] Cleanup and simplification.

---
 src/main/drivers/headtracker_common.c | 44 +++++++++++++++++++++++----
 src/main/drivers/headtracker_common.h | 10 ++++++
 src/main/io/gimbal_serial.c           | 21 ++++++++-----
 src/main/io/headtracker_msp.c         | 20 +++++++++---
 src/main/io/headtracker_msp.h         |  2 ++
 5 files changed, 78 insertions(+), 19 deletions(-)

diff --git a/src/main/drivers/headtracker_common.c b/src/main/drivers/headtracker_common.c
index d08c5d818bf..5bd7977cfe2 100644
--- a/src/main/drivers/headtracker_common.c
+++ b/src/main/drivers/headtracker_common.c
@@ -90,31 +90,63 @@ bool headTrackerCommonIsReady(const headTrackerDevice_t *headTrackerDevice)
     return false;
 }
 
+int headTrackerCommonGetPan(const headTrackerDevice_t *headTrackerDevice)
+{
+    if(headTrackerDevice && headTrackerDevice->vTable && headTrackerDevice->vTable->getPan) {
+        return headTrackerDevice->vTable->getPan(headTrackerDevice);
+    }
+
+    return constrain(headTrackerDevice->pan * headTrackerConfig()->pan_ratio, -2048, 2047);
+}
+
+int headTrackerCommonGetTilt(const headTrackerDevice_t *headTrackerDevice)
+{
+    if(headTrackerDevice && headTrackerDevice->vTable && headTrackerDevice->vTable->getTilt) {
+        return headTrackerDevice->vTable->getTilt(headTrackerDevice);
+    }
+
+    return constrain(headTrackerDevice->tilt * headTrackerConfig()->tilt_ratio, -2048, 2047);
+}
+
+int headTrackerCommonGetRoll(const headTrackerDevice_t *headTrackerDevice)
+{
+    if(headTrackerDevice && headTrackerDevice->vTable && headTrackerDevice->vTable->getRoll) {
+        return headTrackerDevice->vTable->getRollPWM(headTrackerDevice);
+    }
+
+    return constrain(headTrackerDevice->roll * headTrackerConfig()->roll_ratio, -2048, 2047);
+}
+
+int headTracker2PWM(int value)
+{
+    return constrain(scaleRange(value, -2048, 2047, PWM_RANGE_MIN, PWM_RANGE_MAX), PWM_RANGE_MIN, PWM_RANGE_MAX);
+}
+
 int headTrackerCommonGetPanPWM(const headTrackerDevice_t *headTrackerDevice)
 {
     if(headTrackerDevice && headTrackerDevice->vTable && headTrackerDevice->vTable->getPanPWM) {
         return headTrackerDevice->vTable->getPanPWM(headTrackerDevice);
     }
 
-    return constrain(headTrackerDevice->pan, PWM_RANGE_MIN, PWM_RANGE_MAX);
+    return headTracker2PWM(headTrackerCommonGetPan(headTrackerDevice));
 }
 
 int headTrackerCommonGetTiltPWM(const headTrackerDevice_t *headTrackerDevice)
 {
-    if(headTrackerDevice && headTrackerDevice->vTable && headTrackerDevice->vTable->getPanPWM) {
+    if(headTrackerDevice && headTrackerDevice->vTable && headTrackerDevice->vTable->getTiltPWM) {
         return headTrackerDevice->vTable->getTiltPWM(headTrackerDevice);
     }
 
-    return constrain(headTrackerDevice->tilt, PWM_RANGE_MIN, PWM_RANGE_MAX);
+    return headTracker2PWM(headTrackerCommonGetTilt(headTrackerDevice));
 }
 
 int headTrackerCommonGetRollPWM(const headTrackerDevice_t *headTrackerDevice)
 {
-    if(headTrackerDevice && headTrackerDevice->vTable && headTrackerDevice->vTable->getPanPWM) {
-        return headTrackerDevice->vTable->getTiltPWM(headTrackerDevice);
+    if(headTrackerDevice && headTrackerDevice->vTable && headTrackerDevice->vTable->getRollPWM) {
+        return headTrackerDevice->vTable->getRollPWM(headTrackerDevice);
     }
 
-    return constrain(headTrackerDevice->roll, PWM_RANGE_MIN, PWM_RANGE_MAX);
+    return headTracker2PWM(headTrackerCommonGetRoll(headTrackerDevice));
 }
 
 
diff --git a/src/main/drivers/headtracker_common.h b/src/main/drivers/headtracker_common.h
index 769bfb206eb..1f6a67a5a50 100644
--- a/src/main/drivers/headtracker_common.h
+++ b/src/main/drivers/headtracker_common.h
@@ -65,6 +65,9 @@ typedef struct headTrackerVTable_s {
     int (*getPanPWM)(const headTrackerDevice_t *headTrackerDevice);
     int (*getTiltPWM)(const headTrackerDevice_t *headTrackerDevice);
     int (*getRollPWM)(const headTrackerDevice_t *headTrackerDevice);
+    int (*getPan)(const headTrackerDevice_t *headTrackerDevice);
+    int (*getTilt)(const headTrackerDevice_t *headTrackerDevice);
+    int (*getRoll)(const headTrackerDevice_t *headTrackerDevice);
 } headTrackerVTable_t;
 
 
@@ -86,10 +89,17 @@ void headTrackerCommonProcess(headTrackerDevice_t *headTrackerDevice, timeUs_t c
 headTrackerDevType_e headTrackerCommonGetDeviceType(const headTrackerDevice_t *headTrackerDevice);
 bool headTrackerCommonIsReady(const headTrackerDevice_t *headtrackerDevice);
 bool headTrackerCommonIsValid(const headTrackerDevice_t *headtrackerDevice);
+
+// Scaled value, constrained to PWM_RANGE_MIN~PWM_RANGE_MAX
 int headTrackerCommonGetPanPWM(const headTrackerDevice_t *headTrackerDevice);
 int headTrackerCommonGetTiltPWM(const headTrackerDevice_t *headTrackerDevice);
 int headTrackerCommonGetRollPWM(const headTrackerDevice_t *headTrackerDevice);
 
+// Scaled value, constrained to -2048~2047
+int headTrackerCommonGetPan(const headTrackerDevice_t *headTrackerDevice);
+int headTrackerCommonGetTilt(const headTrackerDevice_t *headTrackerDevice);
+int headTrackerCommonGetRoll(const headTrackerDevice_t *headTrackerDevice);
+
 void taskUpdateHeadTracker(timeUs_t currentTimeUs);
 
 bool headtrackerCommonIsEnabled(void);
diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index dbb30d08a47..e20ed2931e7 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -83,11 +83,11 @@ static gimbalDevice_t serialGimbalDevice = {
 static headTrackerVTable_t headTrackerVTable = {
     .process = headtrackerSerialProcess,
     .getDeviceType = headtrackerSerialGetDeviceType,
-    .isReady = headTrackerSerialIsReady,
+    //.isReady = headTrackerSerialIsReady,
     .isValid = headTrackerSerialIsValid,
-    .getPanPWM = headTrackerSerialGetPanPWM,
-    .getTiltPWM = headTrackerSerialGetTiltPWM,
-    .getRollPWM = headTrackerSerialGetRollPWM,
+    //.getPanPWM = headTrackerSerialGetPanPWM,
+    //.getTiltPWM = headTrackerSerialGetTiltPWM,
+    //.getRollPWM = headTrackerSerialGetRollPWM,
 };
 
 
@@ -229,11 +229,13 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
         }
     }
 
+#ifdef USE_HEADTRACKER
     if(IS_RC_MODE_ACTIVE(BOXGIMBALHTRK)) {
-        if (gimbalCommonHtrkIsEnabled() && (micros() < headTrackerDevice.expires)) {
-            attitude.pan = headTrackerDevice.pan;
-            attitude.tilt = headTrackerDevice.tilt;
-            attitude.roll = headTrackerDevice.roll;
+        headTrackerDevice_t *dev = headTrackerCommonDevice();
+        if (gimbalCommonHtrkIsEnabled() && dev && headTrackerCommonIsValid(dev)) {
+            attitude.pan = headTrackerCommonGetPan(dev);
+            attitude.tilt = headTrackerCommonGetTilt(dev);
+            attitude.roll = headTrackerCommonGetRoll(dev);
             DEBUG_SET(DEBUG_HEADTRACKING, 4, 1);
         } else {
             attitude.pan = 0;
@@ -242,6 +244,9 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
             DEBUG_SET(DEBUG_HEADTRACKING, 4, -1);
         }
     } else {
+#else
+    {
+#endif
         DEBUG_SET(DEBUG_HEADTRACKING, 4, 0);
         // Radio endpoints may need to be adjusted, as it seems ot go a bit
         // bananas at the extremes
diff --git a/src/main/io/headtracker_msp.c b/src/main/io/headtracker_msp.c
index 6f0fdf3b3f2..7f0c4a5d795 100644
--- a/src/main/io/headtracker_msp.c
+++ b/src/main/io/headtracker_msp.c
@@ -16,6 +16,7 @@
  */
 
 #include <platform.h>
+#include <stdio.h>
 
 #if (defined(USE_HEADTRACKER_MSP) && defined(USE_HEADTRACKER))
 
@@ -43,12 +44,12 @@ typedef struct headTrackerVTable_s {
 
 static headTrackerVTable_t headTrackerMspVTable = {
     .process = NULL,
-    .getDeviceType = NULL,
+    .getDeviceType = heatTrackerMspGetDeviceType,
     .isReady = NULL,
-    .isValid = headTrackerCommonIsValid,
-    .getPanPWM = headTrackerCommonGetPanPWM,
-    .getTiltPWM = headTrackerCommonGetTiltPWM,
-    .getRollPWM = headTrackerCommonGetRollPWM,
+    .isValid = NULL,
+    //.getPanPWM = headTrackerCommonGetPanPWM,
+    //.getTiltPWM = headTrackerCommonGetTiltPWM,
+    //.getRollPWM = headTrackerCommonGetRollPWM,
 };
 
 static headTrackerDevice_t headTrackerMspDevice = {
@@ -69,6 +70,7 @@ void mspHeadTrackerInit(void)
 void mspHeadTrackerReceiverNewData(uint8_t *data, int dataSize)
 {
     if(dataSize != sizeof(headtrackerMspMessage_t)) {
+        SD(fprintf(stderr, "[headTracker]: invalid data size %d\n", dataSize));
         return;
     }
 
@@ -79,7 +81,15 @@ void mspHeadTrackerReceiverNewData(uint8_t *data, int dataSize)
     headTrackerMspDevice.roll = constrain((status->roll * headTrackerConfig()->roll_ratio) + 0.5f, -2048, 2047);
     headTrackerMspDevice.expires = micros() + MAX_HEADTRACKER_DATA_AGE_US;
 
+    SD(fprintf(stderr, "[headTracker]: pan: %d tilt: %d roll: %d\n", status->pan, status->tilt, status->roll));
+    SD(fprintf(stderr, "[headTracker]: scaled pan: %d tilt: %d roll: %d\n", headTrackerMspDevice.pan, headTrackerMspDevice.tilt, headTrackerMspDevice.roll));
+
     UNUSED(status);
 }
 
+headTrackerDevType_e heatTrackerMspGetDeviceType(const headTrackerDevice_t *headTrackerDevice) {
+    UNUSED(headTrackerDevice);
+    return HEADTRACKER_MSP;
+}
+
 #endif
\ No newline at end of file
diff --git a/src/main/io/headtracker_msp.h b/src/main/io/headtracker_msp.h
index d1d5bb68651..edcfb465874 100644
--- a/src/main/io/headtracker_msp.h
+++ b/src/main/io/headtracker_msp.h
@@ -39,4 +39,6 @@ void mspHeadTrackerInit(void);
 
 void mspHeadTrackerReceiverNewData(uint8_t *data, int dataSize);
 
+headTrackerDevType_e heatTrackerMspGetDeviceType(const headTrackerDevice_t *headTrackerDevice);
+
 #endif

From 69123b17a7b277c9944e99fee3c258c5bb8ccd6e Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Thu, 20 Jun 2024 20:34:53 +0200
Subject: [PATCH 298/323] Clean up

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

diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index e20ed2931e7..95d74eb7b88 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -84,7 +84,7 @@ static headTrackerVTable_t headTrackerVTable = {
     .process = headtrackerSerialProcess,
     .getDeviceType = headtrackerSerialGetDeviceType,
     //.isReady = headTrackerSerialIsReady,
-    .isValid = headTrackerSerialIsValid,
+    //.isValid = headTrackerSerialIsValid,
     //.getPanPWM = headTrackerSerialGetPanPWM,
     //.getTiltPWM = headTrackerSerialGetTiltPWM,
     //.getRollPWM = headTrackerSerialGetRollPWM,
@@ -414,12 +414,13 @@ headTrackerDevType_e headtrackerSerialGetDeviceType(const headTrackerDevice_t *h
     return HEADTRACKER_SERIAL;
 }
 
+/*
 bool headTrackerSerialIsReady(const headTrackerDevice_t *headTrackerDevice)
 {
     UNUSED(headTrackerDevice);
 
     if(headTrackerPort || (gimbalSerialConfig()->singleUart && gimbalPort)) {
-        return  headTrackerSerialIsValid(headTrackerDevice);
+        return  headTrackerCommonIsReady(headTrackerDevice);
     }
 
     return false;
@@ -456,6 +457,7 @@ int headTrackerSerialGetRollPWM(const headTrackerDevice_t *headTrackerDevice)
 
     return PWM_RANGE_MIDDLE;
 }
+*/
 
 #endif
 

From 9a4a74186ffa33a92a7d16871953466e1a5838bd Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Thu, 20 Jun 2024 20:50:10 +0200
Subject: [PATCH 299/323] More cleanup. Most of the functionality is on
 headtracker_common.

---
 src/main/io/gimbal_serial.c   | 50 -----------------------------------
 src/main/io/headtracker_msp.c |  3 ---
 2 files changed, 53 deletions(-)

diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index 95d74eb7b88..37b0583a8a9 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -83,11 +83,6 @@ static gimbalDevice_t serialGimbalDevice = {
 static headTrackerVTable_t headTrackerVTable = {
     .process = headtrackerSerialProcess,
     .getDeviceType = headtrackerSerialGetDeviceType,
-    //.isReady = headTrackerSerialIsReady,
-    //.isValid = headTrackerSerialIsValid,
-    //.getPanPWM = headTrackerSerialGetPanPWM,
-    //.getTiltPWM = headTrackerSerialGetTiltPWM,
-    //.getRollPWM = headTrackerSerialGetRollPWM,
 };
 
 
@@ -414,51 +409,6 @@ headTrackerDevType_e headtrackerSerialGetDeviceType(const headTrackerDevice_t *h
     return HEADTRACKER_SERIAL;
 }
 
-/*
-bool headTrackerSerialIsReady(const headTrackerDevice_t *headTrackerDevice)
-{
-    UNUSED(headTrackerDevice);
-
-    if(headTrackerPort || (gimbalSerialConfig()->singleUart && gimbalPort)) {
-        return  headTrackerCommonIsReady(headTrackerDevice);
-    }
-
-    return false;
-}
-
-bool headTrackerSerialIsValid(const headTrackerDevice_t *headTrackerDevice)
-{
-    return micros() < headTrackerDevice->expires;
-}
-
-int headTrackerSerialGetPanPWM(const headTrackerDevice_t *headTrackerDevice)
-{
-    if(micros() < headTrackerDevice->expires) {
-        return scaleRange(headTrackerDevice->pan, -2048, 2047, PWM_RANGE_MIN, PWM_RANGE_MAX);
-    }
-
-    return PWM_RANGE_MIDDLE;
-}
-
-int headTrackerSerialGetTiltPWM(const headTrackerDevice_t *headTrackerDevice)
-{
-    if(micros() < headTrackerDevice->expires) {
-        return scaleRange(headTrackerDevice->tilt, -2048, 2047, PWM_RANGE_MIN, PWM_RANGE_MAX);
-    }
-
-    return PWM_RANGE_MIDDLE;
-}
-
-int headTrackerSerialGetRollPWM(const headTrackerDevice_t *headTrackerDevice)
-{
-    if(micros() < headTrackerDevice->expires) {
-        return scaleRange(headTrackerDevice->roll, -2048, 2047, PWM_RANGE_MIN, PWM_RANGE_MAX);
-    }
-
-    return PWM_RANGE_MIDDLE;
-}
-*/
-
 #endif
 
 #endif
diff --git a/src/main/io/headtracker_msp.c b/src/main/io/headtracker_msp.c
index 7f0c4a5d795..c3da186aaa1 100644
--- a/src/main/io/headtracker_msp.c
+++ b/src/main/io/headtracker_msp.c
@@ -47,9 +47,6 @@ static headTrackerVTable_t headTrackerMspVTable = {
     .getDeviceType = heatTrackerMspGetDeviceType,
     .isReady = NULL,
     .isValid = NULL,
-    //.getPanPWM = headTrackerCommonGetPanPWM,
-    //.getTiltPWM = headTrackerCommonGetTiltPWM,
-    //.getRollPWM = headTrackerCommonGetRollPWM,
 };
 
 static headTrackerDevice_t headTrackerMspDevice = {

From 188cc0038bdf9f5e9432b278fb297ed5476908a2 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Thu, 20 Jun 2024 21:37:28 +0200
Subject: [PATCH 300/323] Build fixes for when different parts of heatracker /
 gimbal are disabled

---
 src/main/io/gimbal_serial.c | 17 ++++++++++++++---
 1 file changed, 14 insertions(+), 3 deletions(-)

diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index 37b0583a8a9..7c26a0c17af 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -57,12 +57,15 @@ STATIC_ASSERT(sizeof(gimbalHtkAttitudePkt_t) == 10, gimbalHtkAttitudePkt_t_size_
 #ifndef GIMBAL_UNIT_TEST
 static volatile uint8_t txBuffer[GIMBAL_SERIAL_BUFFER_SIZE];
 
+#if defined(USE_HEADTRACKER) && defined(USE_HEADTRACKER_SERIAL)
 static gimbalSerialHtrkState_t headTrackerState = { 
     .payloadSize = 0,
     .state = WAITING_HDR1,
 };
 #endif
 
+#endif
+
 static serialPort_t *headTrackerPort = NULL;
 static serialPort_t *gimbalPort = NULL;
 
@@ -141,7 +144,7 @@ bool gimbalSerialDetect(void)
 
     if (portConfig) {
         SD(fprintf(stderr, "[GIMBAL]: found port...\n"));
-#ifdef USE_HEADTRACKER
+#if defined(USE_HEADTRACKER) && defined(USE_HEADTRACKER_SERIAL)
         gimbalPort = openSerialPort(portConfig->identifier, FUNCTION_GIMBAL, singleUart ? gimbalSerialHeadTrackerReceive : NULL, singleUart ? &headTrackerState :  NULL,
                 baudRates[portConfig->peripheral_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
 #else
@@ -278,6 +281,8 @@ int16_t gimbal_scale12(int16_t inputMin, int16_t inputMax, int16_t value)
 }
 
 #ifndef GIMBAL_UNIT_TEST
+
+#if (defined(USE_HEADTRACKER) && defined(USE_HEADTRACKER_SERIAL))
 static void resetState(gimbalSerialHtrkState_t *state)
 {
     state->state = WAITING_HDR1;
@@ -297,7 +302,6 @@ static bool checkCrc(gimbalHtkAttitudePkt_t *attitude)
            (attitude->crcl == (crc & 0xFF));
 }
 
-
 void gimbalSerialHeadTrackerReceive(uint16_t c, void *data)
 {
     static int charCount = 0;
@@ -352,7 +356,6 @@ void gimbalSerialHeadTrackerReceive(uint16_t c, void *data)
     }
 }
 
-#if (defined(USE_HEADTRACKER) && defined(USE_HEADTRACKER_SERIAL))
 
 bool gimbalSerialHeadTrackerDetect(void)
 {
@@ -409,6 +412,14 @@ headTrackerDevType_e headtrackerSerialGetDeviceType(const headTrackerDevice_t *h
     return HEADTRACKER_SERIAL;
 }
 
+#else
+
+void gimbalSerialHeadTrackerReceive(uint16_t c, void *data)
+{
+    UNUSED(c);
+    UNUSED(data);
+}
+
 #endif
 
 #endif

From 677734d3ff3b1df0b5232d9201f2c1673aa7f731 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Thu, 20 Jun 2024 22:15:28 +0200
Subject: [PATCH 301/323] Move scaling to headtracker_common

---
 src/main/drivers/headtracker_common.c | 8 ++++----
 src/main/drivers/headtracker_common.h | 2 ++
 src/main/io/gimbal_serial.c           | 6 +++---
 src/main/io/headtracker_msp.c         | 6 +++---
 4 files changed, 12 insertions(+), 10 deletions(-)

diff --git a/src/main/drivers/headtracker_common.c b/src/main/drivers/headtracker_common.c
index 5bd7977cfe2..feec581da8a 100644
--- a/src/main/drivers/headtracker_common.c
+++ b/src/main/drivers/headtracker_common.c
@@ -96,7 +96,7 @@ int headTrackerCommonGetPan(const headTrackerDevice_t *headTrackerDevice)
         return headTrackerDevice->vTable->getPan(headTrackerDevice);
     }
 
-    return constrain(headTrackerDevice->pan * headTrackerConfig()->pan_ratio, -2048, 2047);
+    return constrain((headTrackerDevice->pan * headTrackerConfig()->pan_ratio) + 0.5f, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
 }
 
 int headTrackerCommonGetTilt(const headTrackerDevice_t *headTrackerDevice)
@@ -105,7 +105,7 @@ int headTrackerCommonGetTilt(const headTrackerDevice_t *headTrackerDevice)
         return headTrackerDevice->vTable->getTilt(headTrackerDevice);
     }
 
-    return constrain(headTrackerDevice->tilt * headTrackerConfig()->tilt_ratio, -2048, 2047);
+    return constrain((headTrackerDevice->tilt * headTrackerConfig()->tilt_ratio) + 0.5f, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
 }
 
 int headTrackerCommonGetRoll(const headTrackerDevice_t *headTrackerDevice)
@@ -114,12 +114,12 @@ int headTrackerCommonGetRoll(const headTrackerDevice_t *headTrackerDevice)
         return headTrackerDevice->vTable->getRollPWM(headTrackerDevice);
     }
 
-    return constrain(headTrackerDevice->roll * headTrackerConfig()->roll_ratio, -2048, 2047);
+    return constrain((headTrackerDevice->roll * headTrackerConfig()->roll_ratio) + 0.5f, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
 }
 
 int headTracker2PWM(int value)
 {
-    return constrain(scaleRange(value, -2048, 2047, PWM_RANGE_MIN, PWM_RANGE_MAX), PWM_RANGE_MIN, PWM_RANGE_MAX);
+    return constrain(scaleRange(value, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX, PWM_RANGE_MIN, PWM_RANGE_MAX), PWM_RANGE_MIN, PWM_RANGE_MAX);
 }
 
 int headTrackerCommonGetPanPWM(const headTrackerDevice_t *headTrackerDevice)
diff --git a/src/main/drivers/headtracker_common.h b/src/main/drivers/headtracker_common.h
index 1f6a67a5a50..7b46b5390b8 100644
--- a/src/main/drivers/headtracker_common.h
+++ b/src/main/drivers/headtracker_common.h
@@ -30,6 +30,8 @@
 #include "config/feature.h"
 
 #define MAX_HEADTRACKER_DATA_AGE_US HZ2US(25)
+#define HEADTRACKER_RANGE_MIN   -2048
+#define HEADTRACKER_RANGE_MAX   2047
 
 #ifdef __cplusplus
 extern "C" {
diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index 7c26a0c17af..323bc99e564 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -344,9 +344,9 @@ void gimbalSerialHeadTrackerReceive(uint16_t c, void *data)
             state->attitude.crcl = c;
             if(checkCrc(&(state->attitude))) {
                 headTrackerDevice.expires = micros() + MAX_HEADTRACKER_DATA_AGE_US;
-                headTrackerDevice.pan = constrain((state->attitude.pan * headTrackerConfig()->pan_ratio) + 0.5f, -2048, 2047);
-                headTrackerDevice.tilt = constrain((state->attitude.tilt * headTrackerConfig()->tilt_ratio) + 0.5f, -2048, 2047);
-                headTrackerDevice.roll = constrain((state->attitude.roll * headTrackerConfig()->roll_ratio) + 0.5f, -2048, 2047);
+                headTrackerDevice.pan = constrain(state->attitude.pan, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
+                headTrackerDevice.tilt = constrain(state->attitude.tilt, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
+                headTrackerDevice.roll = constrain(state->attitude.roll, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
                 DEBUG_SET(DEBUG_HEADTRACKING, 2, pktCount++);
             } else {
                 DEBUG_SET(DEBUG_HEADTRACKING, 3, errorCount++);
diff --git a/src/main/io/headtracker_msp.c b/src/main/io/headtracker_msp.c
index c3da186aaa1..6a4b73dbbfb 100644
--- a/src/main/io/headtracker_msp.c
+++ b/src/main/io/headtracker_msp.c
@@ -73,9 +73,9 @@ void mspHeadTrackerReceiverNewData(uint8_t *data, int dataSize)
 
     headtrackerMspMessage_t *status = (headtrackerMspMessage_t *)data;
 
-    headTrackerMspDevice.pan = constrain((status->pan * headTrackerConfig()->pan_ratio) + 0.5f, -2048, 2047);
-    headTrackerMspDevice.tilt = constrain((status->tilt * headTrackerConfig()->tilt_ratio) + 0.5f, -2048, 2047);
-    headTrackerMspDevice.roll = constrain((status->roll * headTrackerConfig()->roll_ratio) + 0.5f, -2048, 2047);
+    headTrackerMspDevice.pan = constrain(status->pan, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
+    headTrackerMspDevice.tilt = constrain(status->tilt, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
+    headTrackerMspDevice.roll = constrain(status->roll, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
     headTrackerMspDevice.expires = micros() + MAX_HEADTRACKER_DATA_AGE_US;
 
     SD(fprintf(stderr, "[headTracker]: pan: %d tilt: %d roll: %d\n", status->pan, status->tilt, status->roll));

From 0aecc4fcd3ca538113b0cf74b7a6968cad880d39 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Fri, 21 Jun 2024 01:22:40 +0200
Subject: [PATCH 302/323] Add axis trim

---
 docs/Settings.md                 | 30 ++++++++++++++++++++++++++++++
 src/main/drivers/gimbal_common.c | 13 ++++++++++++-
 src/main/drivers/gimbal_common.h |  3 +++
 src/main/fc/settings.yaml        | 18 ++++++++++++++++++
 src/main/io/gimbal_serial.c      |  6 +++---
 5 files changed, 66 insertions(+), 4 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index a23ff66a4d2..c29b6adc6f8 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -1472,6 +1472,16 @@ Gimbal pan rc channel index. 0 is no channel.
 
 ---
 
+### gimbal_pan_trim
+
+Trim gimbal pan center position.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | -500 | 500 |
+
+---
+
 ### gimbal_roll_channel
 
 Gimbal roll rc channel index. 0 is no channel.
@@ -1482,6 +1492,16 @@ Gimbal roll rc channel index. 0 is no channel.
 
 ---
 
+### gimbal_roll_trim
+
+Trim gimbal roll center position.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | -500 | 500 |
+
+---
+
 ### gimbal_sensitivity
 
 Gimbal sensitivity is similar to gain and will affect how quickly the gimbal will react.
@@ -1512,6 +1532,16 @@ Gimbal tilt rc channel index. 0 is no channel.
 
 ---
 
+### gimbal_tilt_trim
+
+Trim gimbal tilt center position.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | -500 | 500 |
+
+---
+
 ### gps_auto_baud
 
 Automatic configuration of GPS baudrate(The specified baudrate in configured in ports will be used) when used with UBLOX GPS
diff --git a/src/main/drivers/gimbal_common.c b/src/main/drivers/gimbal_common.c
index bf2db50a261..a7a83b98e65 100644
--- a/src/main/drivers/gimbal_common.c
+++ b/src/main/drivers/gimbal_common.c
@@ -32,9 +32,20 @@
 
 #include "drivers/gimbal_common.h"
 
+#include "settings_generated.h"
 
-PG_REGISTER(gimbalConfig_t, gimbalConfig, PG_GIMBAL_CONFIG, 0);
 
+PG_REGISTER_WITH_RESET_TEMPLATE(gimbalConfig_t, gimbalConfig, PG_GIMBAL_CONFIG, 1);
+
+PG_RESET_TEMPLATE(gimbalConfig_t, gimbalConfig, 
+    .panChannel = SETTING_GIMBAL_PAN_CHANNEL_DEFAULT,
+    .tiltChannel = SETTING_GIMBAL_TILT_CHANNEL_DEFAULT,
+    .rollChannel = SETTING_GIMBAL_ROLL_CHANNEL_DEFAULT,
+    .sensitivity = SETTING_GIMBAL_SENSITIVITY_DEFAULT,
+    .panTrim = SETTING_GIMBAL_PAN_TRIM_DEFAULT,
+    .tiltTrim = SETTING_GIMBAL_TILT_TRIM_DEFAULT,
+    .rollTrim = SETTING_GIMBAL_ROLL_TRIM_DEFAULT
+);
 
 static gimbalDevice_t *commonGimbalDevice = NULL;
 
diff --git a/src/main/drivers/gimbal_common.h b/src/main/drivers/gimbal_common.h
index 2d914bbf77a..96614fb6ea9 100644
--- a/src/main/drivers/gimbal_common.h
+++ b/src/main/drivers/gimbal_common.h
@@ -60,6 +60,9 @@ typedef struct gimbalConfig_s {
     uint8_t tiltChannel;
     uint8_t rollChannel;
     uint8_t sensitivity;
+    uint16_t panTrim;
+    uint16_t tiltTrim;
+    uint16_t rollTrim;
 } gimbalConfig_t;
 
 PG_DECLARE(gimbalConfig_t, gimbalConfig);
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index df5be7efca6..fe0cd358b94 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -4230,6 +4230,24 @@ groups:
         field: sensitivity
         min: -16
         max: 15
+      - name: gimbal_pan_trim
+        field: panTrim
+        description: "Trim gimbal pan center position."
+        default_value: 0
+        min: -500
+        max: 500
+      - name: gimbal_tilt_trim
+        field: tiltTrim
+        description: "Trim gimbal tilt center position."
+        default_value: 0
+        min: -500
+        max: 500
+      - name: gimbal_roll_trim
+        field: rollTrim
+        description: "Trim gimbal roll center position."
+        default_value: 0
+        min: -500
+        max: 500
   - name: PG_GIMBAL_SERIAL_CONFIG
     type: gimbalSerialConfig_t
     headers: ["io/gimbal_serial.h"]
diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index 323bc99e564..af3dd01115a 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -193,9 +193,9 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
 
     const gimbalConfig_t *cfg = gimbalConfig();
 
-    int pan = PWM_RANGE_MIDDLE;
-    int tilt = PWM_RANGE_MIDDLE;
-    int roll = PWM_RANGE_MIDDLE;
+    int pan = PWM_RANGE_MIDDLE + cfg->panTrim;
+    int tilt = PWM_RANGE_MIDDLE + cfg->tiltTrim;
+    int roll = PWM_RANGE_MIDDLE + cfg->rollTrim;
 
     if (IS_RC_MODE_ACTIVE(BOXGIMBALTLOCK)) {
         attitude.mode |= GIMBAL_MODE_TILT_LOCK;

From aa9451432a027ac93a80ab2caa8c6fd42434199a Mon Sep 17 00:00:00 2001
From: 0crap <31951195+0crap@users.noreply.github.com>
Date: Fri, 21 Jun 2024 09:06:41 +0200
Subject: [PATCH 303/323] Update readme.md

Typo
---
 readme.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/readme.md b/readme.md
index 6376efc7726..53710d519aa 100644
--- a/readme.md
+++ b/readme.md
@@ -122,7 +122,7 @@ Before creating new issues please check to see if there is an existing one, sear
 
 Please refer to the development section in the [docs/development](https://github.com/iNavFlight/inav/tree/master/docs/development) folder.
 
-Nightly builds are avaialble for testing on the following links:
+Nightly builds are available for testing on the following links:
 
 https://github.com/iNavFlight/inav-nightly/releases
 

From b9be14c70143ebd98118fd1551d71ffa0e0f5893 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Fri, 21 Jun 2024 10:52:02 +0200
Subject: [PATCH 304/323] Add trim when headtracker info is stale.

Stale headtracker info results in center position of gimbal.
---
 src/main/io/gimbal_serial.c | 32 ++++++++++++++++----------------
 1 file changed, 16 insertions(+), 16 deletions(-)

diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index af3dd01115a..58ba5212a30 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -193,9 +193,9 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
 
     const gimbalConfig_t *cfg = gimbalConfig();
 
-    int pan = PWM_RANGE_MIDDLE + cfg->panTrim;
-    int tilt = PWM_RANGE_MIDDLE + cfg->tiltTrim;
-    int roll = PWM_RANGE_MIDDLE + cfg->rollTrim;
+    int panPWM = PWM_RANGE_MIDDLE + cfg->panTrim;
+    int tiltPWM = PWM_RANGE_MIDDLE + cfg->tiltTrim;
+    int rollPWM = PWM_RANGE_MIDDLE + cfg->rollTrim;
 
     if (IS_RC_MODE_ACTIVE(BOXGIMBALTLOCK)) {
         attitude.mode |= GIMBAL_MODE_TILT_LOCK;
@@ -212,18 +212,18 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
     
     if (rxAreFlightChannelsValid() && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)) {
         if (cfg->panChannel > 0) {
-            pan = rxGetChannelValue(cfg->panChannel - 1);
-            pan = constrain(pan, PWM_RANGE_MIN, PWM_RANGE_MAX);
+            panPWM = rxGetChannelValue(cfg->panChannel - 1);
+            panPWM = constrain(panPWM, PWM_RANGE_MIN, PWM_RANGE_MAX);
         }
 
         if (cfg->tiltChannel > 0) {
-            tilt = rxGetChannelValue(cfg->tiltChannel - 1);
-            tilt = constrain(tilt, PWM_RANGE_MIN, PWM_RANGE_MAX);
+            tiltPWM = rxGetChannelValue(cfg->tiltChannel - 1);
+            tiltPWM = constrain(tiltPWM, PWM_RANGE_MIN, PWM_RANGE_MAX);
         }
 
         if (cfg->rollChannel > 0) {
-            roll = rxGetChannelValue(cfg->rollChannel - 1);
-            roll = constrain(roll, PWM_RANGE_MIN, PWM_RANGE_MAX);
+            rollPWM = rxGetChannelValue(cfg->rollChannel - 1);
+            rollPWM = constrain(rollPWM, PWM_RANGE_MIN, PWM_RANGE_MAX);
         }
     }
 
@@ -236,9 +236,9 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
             attitude.roll = headTrackerCommonGetRoll(dev);
             DEBUG_SET(DEBUG_HEADTRACKING, 4, 1);
         } else {
-            attitude.pan = 0;
-            attitude.tilt = 0;
-            attitude.roll = 0;
+            attitude.pan = constrain(gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, PWM_RANGE_MIDDLE + cfg->panTrim), PWM_RANGE_MIN, PWM_RANGE_MAX);
+            attitude.tilt = constrain(gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, PWM_RANGE_MIDDLE + cfg->tiltTrim), PWM_RANGE_MIN, PWM_RANGE_MAX);
+            attitude.roll = constrain(gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, PWM_RANGE_MIDDLE + cfg->rollTrim), PWM_RANGE_MIN, PWM_RANGE_MAX);
             DEBUG_SET(DEBUG_HEADTRACKING, 4, -1);
         }
     } else {
@@ -248,9 +248,9 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
         DEBUG_SET(DEBUG_HEADTRACKING, 4, 0);
         // Radio endpoints may need to be adjusted, as it seems ot go a bit
         // bananas at the extremes
-        attitude.pan = gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, pan);
-        attitude.tilt = gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, tilt);
-        attitude.roll = gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, roll);
+        attitude.pan = gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, panPWM);
+        attitude.tilt = gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, tiltPWM);
+        attitude.roll = gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, rollPWM);
     }
 
     DEBUG_SET(DEBUG_HEADTRACKING, 5, attitude.pan);
@@ -276,7 +276,7 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
 int16_t gimbal_scale12(int16_t inputMin, int16_t inputMax, int16_t value)
 {
     int16_t ret = 0;
-    ret = scaleRange(value, inputMin, inputMax, -2048, 2047);
+    ret = scaleRange(value, inputMin, inputMax, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
     return ret;
 }
 

From 91b20689edb739ad12b689277a751451edd62d62 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Fri, 21 Jun 2024 11:10:51 +0200
Subject: [PATCH 305/323] fix test build

---
 src/main/drivers/headtracker_common.h   | 7 ++++---
 src/test/unit/gimbal_serial_unittest.cc | 1 +
 2 files changed, 5 insertions(+), 3 deletions(-)

diff --git a/src/main/drivers/headtracker_common.h b/src/main/drivers/headtracker_common.h
index 7b46b5390b8..8c177e3e0f0 100644
--- a/src/main/drivers/headtracker_common.h
+++ b/src/main/drivers/headtracker_common.h
@@ -19,6 +19,10 @@
 
 #include "platform.h"
 
+#define MAX_HEADTRACKER_DATA_AGE_US HZ2US(25)
+#define HEADTRACKER_RANGE_MIN   -2048
+#define HEADTRACKER_RANGE_MAX   2047
+
 #ifdef USE_HEADTRACKER
 
 #include <stdint.h>
@@ -29,9 +33,6 @@
 
 #include "config/feature.h"
 
-#define MAX_HEADTRACKER_DATA_AGE_US HZ2US(25)
-#define HEADTRACKER_RANGE_MIN   -2048
-#define HEADTRACKER_RANGE_MAX   2047
 
 #ifdef __cplusplus
 extern "C" {
diff --git a/src/test/unit/gimbal_serial_unittest.cc b/src/test/unit/gimbal_serial_unittest.cc
index 77ae4c93df8..1f0c47231c6 100644
--- a/src/test/unit/gimbal_serial_unittest.cc
+++ b/src/test/unit/gimbal_serial_unittest.cc
@@ -24,6 +24,7 @@
 #include "unittest_macros.h"
 
 #include "io/gimbal_serial.h"
+#include "drivers/headtracker_common.h"
 
 void dumpMemory(uint8_t *mem, int size)
 {

From ea7564cfb6216f9aa2ffbbae24f2a52071ee95bc Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Fri, 21 Jun 2024 12:07:45 +0200
Subject: [PATCH 306/323] Add ICM42688-G to IFLIGHTF4_SUCCEXD

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

diff --git a/src/main/target/IFLIGHTF4_SUCCEXD/target.h b/src/main/target/IFLIGHTF4_SUCCEXD/target.h
index 6ba059bb63c..3cc4f030721 100644
--- a/src/main/target/IFLIGHTF4_SUCCEXD/target.h
+++ b/src/main/target/IFLIGHTF4_SUCCEXD/target.h
@@ -39,6 +39,11 @@
 #define MPU6000_CS_PIN          PA4
 #define MPU6000_SPI_BUS         BUS_SPI1
 
+#define USE_IMU_ICM42605
+#define IMU_ICM42605_ALIGN      CW180_DEG
+#define ICM42605_SPI_BUS        BUS_SPI1
+#define ICM42605_CS_PIN         PA4
+
 // *************** SPI2 OSD *****************************
 #define USE_SPI_DEVICE_2
 #define SPI2_SCK_PIN            PB13

From 813d3d37be4253c9d4c72de7a8402cae2eee8769 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Fri, 21 Jun 2024 12:41:06 +0100
Subject: [PATCH 307/323] Update navigation.c

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

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 36c320561b9..6eed742e804 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -119,7 +119,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang
 PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 2);
 #endif
 
-PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 6);
+PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 7);
 
 PG_RESET_TEMPLATE(navConfig_t, navConfig,
     .general = {

From 9c6dbbb08fa0673ec924768f4b7028dabf35fb6e Mon Sep 17 00:00:00 2001
From: Darren Lines <darren@darrenlines.uk>
Date: Fri, 21 Jun 2024 12:49:02 +0100
Subject: [PATCH 308/323] Update Controls.md

Updated `Profile` to `Control Profile` to match recent changes.
---
 docs/Controls.md | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/docs/Controls.md b/docs/Controls.md
index 3cc62b4e740..6dbc26df5fc 100644
--- a/docs/Controls.md
+++ b/docs/Controls.md
@@ -23,9 +23,9 @@ The stick positions are combined to activate different functions:
 
 | Function                      | Throttle | Yaw     | Pitch  | Roll   |
 | ----------------------------- | -------- | ------- | ------ | ------ |
-| Profile 1                     | LOW      | LOW     | CENTER | LOW    |
-| Profile 2                     | LOW      | LOW     | HIGH   | CENTER |
-| Profile 3                     | LOW      | LOW     | CENTER | HIGH   |
+| Control Profile 1             | LOW      | LOW     | CENTER | LOW    |
+| Control Profile 2             | LOW      | LOW     | HIGH   | CENTER |
+| Control Profile 3             | LOW      | LOW     | CENTER | HIGH   |
 | Battery profile 1             | HIGH     | LOW     | CENTER | LOW    |
 | Battery profile 2             | HIGH     | LOW     | HIGH   | CENTER |
 | Battery profile 3             | HIGH     | LOW     | CENTER | HIGH   |

From 34e949ae0815c898bab21e6361631ff78507cf00 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Fri, 21 Jun 2024 14:41:55 +0200
Subject: [PATCH 309/323] Don't run biulds for documentation only changes

---
 .github/workflows/ci.yml | 11 ++++++++++-
 1 file changed, 10 insertions(+), 1 deletion(-)

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index c389884008d..8ab3b5ae8d1 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -2,7 +2,16 @@ name: Build firmware
 # Don't enable CI on push, just on PR. If you
 # are working on the main repo and want to trigger
 # a CI build submit a draft PR.
-on: pull_request
+on: 
+  pull_request:
+    paths:
+      - src/**
+      - .github/**
+      - cmake/**
+      - lib/**
+      - docs/Settings.md
+      - CMakeLists.txt
+      - *.sh
 
 jobs:
   build:

From 27a630fd4e37b349a023ff73d821563d9ca5acc8 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Fri, 21 Jun 2024 14:44:27 +0200
Subject: [PATCH 310/323] synthax fix

---
 .github/workflows/ci.yml | 14 +++++++-------
 1 file changed, 7 insertions(+), 7 deletions(-)

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index 8ab3b5ae8d1..92862bb4300 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -5,13 +5,13 @@ name: Build firmware
 on: 
   pull_request:
     paths:
-      - src/**
-      - .github/**
-      - cmake/**
-      - lib/**
-      - docs/Settings.md
-      - CMakeLists.txt
-      - *.sh
+      - 'src/**'
+      - '.github/**'
+      - 'cmake/**'
+      - 'lib/**'
+      - 'docs/Settings.md'
+      - 'CMakeLists.txt'
+      - '*.sh'
 
 jobs:
   build:

From 6b574df70b91d36e84a1c5833e2c7e4c5b759b16 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Fri, 21 Jun 2024 17:37:43 +0200
Subject: [PATCH 311/323] Take gimbal input into account for pan servo
 compensation and center indication

---
 src/main/drivers/gimbal_common.c | 11 +++++++++++
 src/main/drivers/gimbal_common.h |  4 ++++
 src/main/io/gimbal_serial.c      | 14 +++++++++++++-
 src/main/io/gimbal_serial.h      |  3 ++-
 src/main/io/osd.c                | 10 +++++++++-
 5 files changed, 39 insertions(+), 3 deletions(-)

diff --git a/src/main/drivers/gimbal_common.c b/src/main/drivers/gimbal_common.c
index a7a83b98e65..a81b1b51c10 100644
--- a/src/main/drivers/gimbal_common.c
+++ b/src/main/drivers/gimbal_common.c
@@ -31,6 +31,7 @@
 #include "fc/cli.h"
 
 #include "drivers/gimbal_common.h"
+#include "rx/rx.h"
 
 #include "settings_generated.h"
 
@@ -125,4 +126,14 @@ bool gimbalCommonHtrkIsEnabled(void)
     return false;
 }
 
+
+int16_t gimbalCommonGetPanPwm(const gimbalDevice_t *gimbalDevice)
+{
+    if (gimbalDevice && gimbalDevice->vTable->getGimbalPanPWM) {
+        return gimbalDevice->vTable->getGimbalPanPWM(gimbalDevice);
+    }
+
+    return gimbalDevice ? gimbalDevice->currentPanPWM : PWM_RANGE_MIDDLE;
+}
+
 #endif
\ No newline at end of file
diff --git a/src/main/drivers/gimbal_common.h b/src/main/drivers/gimbal_common.h
index 96614fb6ea9..879e81116a0 100644
--- a/src/main/drivers/gimbal_common.h
+++ b/src/main/drivers/gimbal_common.h
@@ -41,6 +41,7 @@ struct gimbalVTable_s;
 
 typedef struct gimbalDevice_s {
     const struct gimbalVTable_s *vTable;
+    int16_t currentPanPWM;
 } gimbalDevice_t;
 
 // {set,get}BandAndChannel: band and channel are 1 origin
@@ -52,6 +53,7 @@ typedef struct gimbalVTable_s {
     gimbalDevType_e (*getDeviceType)(const gimbalDevice_t *gimbalDevice);
     bool (*isReady)(const gimbalDevice_t *gimbalDevice);
     bool (*hasHeadTracker)(const gimbalDevice_t *gimbalDevice);
+    int16_t (*getGimbalPanPWM)(const gimbalDevice_t *gimbalDevice);
 } gimbalVTable_t;
 
 
@@ -93,6 +95,8 @@ void taskUpdateGimbal(timeUs_t currentTimeUs);
 bool gimbalCommonIsEnabled(void);
 bool gimbalCommonHtrkIsEnabled(void);
 
+int16_t gimbalCommonGetPanPwm(const gimbalDevice_t *gimbalDevice);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index 58ba5212a30..6e6b7e93240 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -78,7 +78,8 @@ gimbalVTable_t gimbalSerialVTable = {
 };
 
 static gimbalDevice_t serialGimbalDevice = {
-    .vTable = &gimbalSerialVTable
+    .vTable = &gimbalSerialVTable,
+    .currentPanPWM = PWM_RANGE_MIDDLE
 };
 
 #if (defined(USE_HEADTRACKER) && defined(USE_HEADTRACKER_SERIAL))
@@ -234,6 +235,7 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
             attitude.pan = headTrackerCommonGetPan(dev);
             attitude.tilt = headTrackerCommonGetTilt(dev);
             attitude.roll = headTrackerCommonGetRoll(dev);
+
             DEBUG_SET(DEBUG_HEADTRACKING, 4, 1);
         } else {
             attitude.pan = constrain(gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, PWM_RANGE_MIDDLE + cfg->panTrim), PWM_RANGE_MIN, PWM_RANGE_MAX);
@@ -267,12 +269,22 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
     attitude.crch = (crc16 >> 8) & 0xFF;
     attitude.crcl = crc16 & 0xFF;
 
+    serialGimbalDevice.currentPanPWM = gimbal2pwm(attitude.pan);
+
     serialBeginWrite(gimbalPort);
     serialWriteBuf(gimbalPort, (uint8_t *)&attitude, sizeof(gimbalHtkAttitudePkt_t));
     serialEndWrite(gimbalPort);
 }
 #endif
 
+int16_t gimbal2pwm(int16_t value)
+{
+    int16_t ret = 0;
+    ret = scaleRange(value, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX, PWM_RANGE_MIN, PWM_RANGE_MAX);
+    return ret;
+}
+
+
 int16_t gimbal_scale12(int16_t inputMin, int16_t inputMax, int16_t value)
 {
     int16_t ret = 0;
diff --git a/src/main/io/gimbal_serial.h b/src/main/io/gimbal_serial.h
index 6c6bf427672..6c9a3251f89 100644
--- a/src/main/io/gimbal_serial.h
+++ b/src/main/io/gimbal_serial.h
@@ -70,9 +70,10 @@ typedef struct gimbalSerialConfig_s {
 
 PG_DECLARE(gimbalSerialConfig_t, gimbalSerialConfig);
 
-
 int16_t gimbal_scale12(int16_t inputMin, int16_t inputMax, int16_t value);
 
+int16_t gimbal2pwm(int16_t value);
+
 bool gimbalSerialInit(void);
 bool gimbalSerialDetect(void);
 void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime);
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 3b6ca7ce0de..d5ffcd9589e 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -62,6 +62,7 @@
 #include "drivers/osd_symbols.h"
 #include "drivers/time.h"
 #include "drivers/vtx_common.h"
+#include "drivers/gimbal_common.h"
 
 #include "io/adsb.h"
 #include "io/flashfs.h"
@@ -1203,8 +1204,15 @@ int16_t osdGetHeading(void)
 int16_t osdGetPanServoOffset(void)
 {
     int8_t servoIndex = osdConfig()->pan_servo_index;
-    int16_t servoPosition = servo[servoIndex];
     int16_t servoMiddle = servoParams(servoIndex)->middle;
+    int16_t servoPosition = servo[servoIndex];
+
+    gimbalDevice_t *dev = gimbalCommonDevice();
+    if (dev && gimbalCommonIsReady(dev)) {
+        servoPosition = gimbalCommonGetPanPwm(dev);
+        servoMiddle = PWM_RANGE_MIDDLE;
+    }
+
     return (int16_t)CENTIDEGREES_TO_DEGREES((servoPosition - servoMiddle) * osdConfig()->pan_servo_pwm2centideg);
 }
 

From ffb536db5d2ea60153c0b28b8abbb83016e7d49b Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Fri, 21 Jun 2024 18:03:58 +0200
Subject: [PATCH 312/323] Cleanup

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

diff --git a/src/main/io/headtracker_msp.c b/src/main/io/headtracker_msp.c
index 6a4b73dbbfb..8a800b3f46e 100644
--- a/src/main/io/headtracker_msp.c
+++ b/src/main/io/headtracker_msp.c
@@ -30,18 +30,6 @@
 
 #include "io/headtracker_msp.h"
 
-/*
-typedef struct headTrackerVTable_s {
-    void (*process)(headTrackerDevice_t *headTrackerDevice, timeUs_t currentTimeUs);
-    headTrackerDevType_e (*getDeviceType)(const headTrackerDevice_t *headTrackerDevice);
-    bool (*isReady)(const headTrackerDevice_t *headTrackerDevice);
-    bool (*isValid)(const headTrackerDevice_t *headTrackerDevice);
-    int (*getPanPWM)(const headTrackerDevice_t *headTrackerDevice);
-    int (*getTiltPWM)(const headTrackerDevice_t *headTrackerDevice);
-    int (*getRollPWM)(const headTrackerDevice_t *headTrackerDevice);
-} headtrackerVTable_t;
-*/
-
 static headTrackerVTable_t headTrackerMspVTable = {
     .process = NULL,
     .getDeviceType = heatTrackerMspGetDeviceType,
@@ -78,9 +66,6 @@ void mspHeadTrackerReceiverNewData(uint8_t *data, int dataSize)
     headTrackerMspDevice.roll = constrain(status->roll, HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
     headTrackerMspDevice.expires = micros() + MAX_HEADTRACKER_DATA_AGE_US;
 
-    SD(fprintf(stderr, "[headTracker]: pan: %d tilt: %d roll: %d\n", status->pan, status->tilt, status->roll));
-    SD(fprintf(stderr, "[headTracker]: scaled pan: %d tilt: %d roll: %d\n", headTrackerMspDevice.pan, headTrackerMspDevice.tilt, headTrackerMspDevice.roll));
-
     UNUSED(status);
 }
 

From 74b638d6a9b9a448b929379074edf0b1fcb9b96a Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Fri, 21 Jun 2024 20:26:18 +0200
Subject: [PATCH 313/323] Use printf for LOG_DEBUG in sitl.

---
 src/main/common/log.c | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/main/common/log.c b/src/main/common/log.c
index 20faa4a53db..49eb5a8b985 100644
--- a/src/main/common/log.c
+++ b/src/main/common/log.c
@@ -20,11 +20,12 @@
 #include <stdarg.h>
 #include <ctype.h>
 
-#if defined(SEMIHOSTING)
+#if defined(SEMIHOSTING) || defined(SITL_BUILD)
 #include <stdio.h>
 #endif
 
 #include "build/version.h"
+#include "build/debug.h"
 
 #include "drivers/serial.h"
 #include "drivers/time.h"
@@ -125,6 +126,7 @@ static void logPrint(const char *buf, size_t size)
         fputc(buf[ii], stdout);
     }
 #endif
+    SD(printf("%s\n", buf));
     if (logPort) {
         // Send data via UART (if configured & connected - a safeguard against zombie VCP)
         if (serialIsConnected(logPort)) {

From 3e285b0c057e58818377150d51affe0523bc002e Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Fri, 21 Jun 2024 23:57:48 +0200
Subject: [PATCH 314/323] Add CRSF bind command.

Also add MSP message to trigger bind.

Bind commands should work for srxl2 and crsf
---
 src/main/fc/cli.c                     |  7 +++++++
 src/main/fc/fc_msp.c                  | 25 ++++++++++++++++++++++++-
 src/main/msp/msp_protocol_v2_common.h |  1 +
 src/main/rx/crsf.c                    |  7 +++++++
 src/main/rx/crsf.h                    | 11 +++++++++++
 5 files changed, 50 insertions(+), 1 deletion(-)

diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index 7b9c1329884..0d54cab4398 100644
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -104,6 +104,7 @@ bool cliMode = false;
 #include "rx/rx.h"
 #include "rx/spektrum.h"
 #include "rx/srxl2.h"
+#include "rx/crsf.h"
 
 #include "scheduler/scheduler.h"
 
@@ -3224,6 +3225,12 @@ void cliRxBind(char *cmdline){
             srxl2Bind();
             cliPrint("Binding SRXL2 receiver...");
             break;
+#endif
+#if defined(USE_SERIALRX_CRSF)
+        case SERIALRX_CRSF:
+            crsfBind();
+            cliPrint("Binding CRSF receiver...");
+            break;
 #endif
         }
     }
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 6362901e45b..acbf0c2dcb4 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -110,6 +110,8 @@
 
 #include "rx/rx.h"
 #include "rx/msp.h"
+#include "rx/srxl2.h"
+#include "rx/crsf.h"
 
 #include "scheduler/scheduler.h"
 
@@ -1030,6 +1032,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
     case MSP_MIXER:
         sbufWriteU8(dst, 3); // mixerMode no longer supported, send 3 (QuadX) as fallback
         break;
+    
 
     case MSP_RX_CONFIG:
         sbufWriteU8(dst, rxConfig()->serialrx_provider);
@@ -2887,7 +2890,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
         } else
             return MSP_RESULT_ERROR;
         break;
-
+    
     case MSP_SET_FAILSAFE_CONFIG:
         if (dataSize == 20) {
             failsafeConfigMutable()->failsafe_delay = sbufReadU8(src);
@@ -3352,6 +3355,26 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
 
         break;
 
+    case MSP2_BETAFLIGHT_BIND:
+        if (rxConfig()->receiverType == RX_TYPE_SERIAL) {
+            switch (rxConfig()->serialrx_provider) {
+            default:
+                return MSP_RESULT_ERROR;
+    #if defined(USE_SERIALRX_SRXL2)
+            case SERIALRX_SRXL2:
+                srxl2Bind();
+                break;
+    #endif
+    #if defined(USE_SERIALRX_CRSF)
+            case SERIALRX_CRSF:
+                crsfBind();
+                break;
+    #endif
+            }
+        } else {
+            return MSP_RESULT_ERROR;
+        }
+        break;
 
     default:
         return MSP_RESULT_ERROR;
diff --git a/src/main/msp/msp_protocol_v2_common.h b/src/main/msp/msp_protocol_v2_common.h
index 82b7f754079..e778a1808c9 100644
--- a/src/main/msp/msp_protocol_v2_common.h
+++ b/src/main/msp/msp_protocol_v2_common.h
@@ -33,3 +33,4 @@
 #define MSP2_COMMON_SET_RADAR_POS       0x100B //SET radar position information
 #define MSP2_COMMON_SET_RADAR_ITD       0x100C //SET radar information to display
 
+#define MSP2_BETAFLIGHT_BIND            0x3000
diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c
index 85820f7fa20..f325859a8a0 100755
--- a/src/main/rx/crsf.c
+++ b/src/main/rx/crsf.c
@@ -331,4 +331,11 @@ bool crsfRxIsActive(void)
 {
     return serialPort != NULL;
 }
+
+
+void crsfBind(void)
+{
+
+}
+
 #endif
diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h
index f3bc7933494..69777a0390c 100755
--- a/src/main/rx/crsf.h
+++ b/src/main/rx/crsf.h
@@ -104,6 +104,15 @@ typedef enum {
     CRSF_FRAMETYPE_DISPLAYPORT_CMD = 0x7D, // displayport control command
 } crsfFrameType_e;
 
+enum {
+    CRSF_COMMAND_SUBCMD_RX = 0x10,    // receiver command
+    CRSF_COMMAND_SUBCMD_GENERAL = 0x0A,    // general command
+};
+
+enum {
+    CRSF_COMMAND_SUBCMD_RX_BIND = 0x01,    // bind command
+};
+
 typedef struct crsfFrameDef_s {
     uint8_t deviceAddress;
     uint8_t frameLength;
@@ -124,3 +133,5 @@ struct rxConfig_s;
 struct rxRuntimeConfig_s;
 bool crsfRxInit(const struct rxConfig_s *initialRxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig);
 bool crsfRxIsActive(void);
+
+void crsfBind(void);

From a33e13aff9268462beba563f9b1c541ed840d8b4 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sat, 22 Jun 2024 00:08:39 +0200
Subject: [PATCH 315/323] Almost forgot to commit the important bits :)

---
 src/main/rx/crsf.c | 15 ++++++++++++++-
 1 file changed, 14 insertions(+), 1 deletion(-)

diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c
index f325859a8a0..a791b2e9dd8 100755
--- a/src/main/rx/crsf.c
+++ b/src/main/rx/crsf.c
@@ -335,7 +335,20 @@ bool crsfRxIsActive(void)
 
 void crsfBind(void)
 {
-
+    if (serialPort != NULL) {
+        uint8_t bindFrame[] = {
+            CRSF_SYNC_BYTE,
+            0x07,  // frame length
+            CRSF_FRAMETYPE_COMMAND,
+            CRSF_ADDRESS_CRSF_RECEIVER,
+            CRSF_ADDRESS_FLIGHT_CONTROLLER,
+            CRSF_COMMAND_SUBCMD_RX,
+            CRSF_COMMAND_SUBCMD_RX_BIND,
+            0x9E,  // Command CRC8
+            0xE8,  // Packet CRC8
+        };
+        serialWriteBuf(serialPort, bindFrame, 9);
+    }
 }
 
 #endif

From f4ce36f6cb311c4a703ee5a10afb355b57bbd100 Mon Sep 17 00:00:00 2001
From: error414 <error414@error414.com>
Date: Mon, 1 Apr 2024 14:39:10 +0200
Subject: [PATCH 316/323] Fixed position for formation flight / inav radar

---
 docs/Settings.md          |  10 ++++
 src/main/common/maths.c   |   9 +++
 src/main/common/maths.h   |   1 +
 src/main/fc/settings.yaml |   6 ++
 src/main/io/osd.c         | 119 +++++++++++++++++++++++++++++++++++++-
 src/main/io/osd.h         |   8 ++-
 6 files changed, 149 insertions(+), 4 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index 38f37649b39..98b0eecaa3d 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -4942,6 +4942,16 @@ Number of leading digits removed from plus code. Removing 2, 4 and 6 digits requ
 
 ---
 
+### osd_radar_peers_display_time
+
+Time in seconds to display next peer 
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 3 | 1 | 10 |
+
+---
+
 ### osd_right_sidebar_scroll
 
 Scroll type for the right sidebar
diff --git a/src/main/common/maths.c b/src/main/common/maths.c
index b8ec59c33a2..993634d902d 100644
--- a/src/main/common/maths.c
+++ b/src/main/common/maths.c
@@ -123,6 +123,15 @@ int32_t wrap_18000(int32_t angle)
     return angle;
 }
 
+int16_t wrap_180(int16_t angle)
+{
+    if (angle > 180)
+        angle -= 360;
+    if (angle < -180)
+        angle += 360;
+    return angle;
+}
+
 int32_t wrap_36000(int32_t angle)
 {
     if (angle >= 36000)
diff --git a/src/main/common/maths.h b/src/main/common/maths.h
index ce96b7064fd..b8d3803613f 100644
--- a/src/main/common/maths.h
+++ b/src/main/common/maths.h
@@ -166,6 +166,7 @@ int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax);
 float scaleRangef(float x, float srcMin, float srcMax, float destMin, float destMax);
 
 int32_t wrap_18000(int32_t angle);
+int16_t wrap_180(int16_t angle);
 int32_t wrap_36000(int32_t angle);
 
 int32_t quickMedianFilter3(int32_t * v);
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index b60c1546694..9e501c7078c 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -3486,6 +3486,12 @@ groups:
         min: 1
         max: 10
         default_value: 3
+      - name: osd_radar_peers_display_time
+        description: "Time in seconds to display next peer "
+        field: radar_peers_display_time
+        min: 1
+        max: 10
+        default_value: 3
       - name: osd_hud_wp_disp
         description: "How many navigation waypoints are displayed, set to 0 (zero) to disable. As sample, if set to 2, and you just passed the 3rd waypoint of the mission, you'll see markers for the 4th waypoint (marked 1) and the 5th waypoint (marked 2)"
         default_value: 0
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 3b6ca7ce0de..9ab45f2736d 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -2505,6 +2505,121 @@ static bool osdDrawSingleElement(uint8_t item)
         }
 #endif
 
+    case OSD_FORMATION_FLIGHT:
+    {
+        static uint8_t currentPeerIndex = 0;
+        static timeMs_t lastPeerSwitch;
+
+        if ((STATE(GPS_FIX) && isImuHeadingValid())) {
+            if ((radar_pois[currentPeerIndex].gps.lat == 0 || radar_pois[currentPeerIndex].gps.lon == 0 || radar_pois[currentPeerIndex].state >= 2) || (millis() > (osdConfig()->radar_peers_display_time * 1000) + lastPeerSwitch)) {
+                lastPeerSwitch = millis();
+
+                for(uint8_t i = 1; i < RADAR_MAX_POIS - 1; i++) {
+                    uint8_t nextPeerIndex = (currentPeerIndex + i) % (RADAR_MAX_POIS - 1);
+                    if (radar_pois[nextPeerIndex].gps.lat != 0 && radar_pois[nextPeerIndex].gps.lon != 0 && radar_pois[nextPeerIndex].state < 2) {
+                        currentPeerIndex = nextPeerIndex;
+                        break;
+                    }
+                }
+            }
+
+            radar_pois_t *currentPeer = &(radar_pois[currentPeerIndex]);
+            if (currentPeer->gps.lat != 0 && currentPeer->gps.lon != 0 && currentPeer->state < 2) {
+                fpVector3_t poi;
+                geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &currentPeer->gps, GEO_ALT_RELATIVE);
+
+                currentPeer->distance = calculateDistanceToDestination(&poi) / 100; // In m
+                currentPeer->altitude = (int16_t )((currentPeer->gps.alt - osdGetAltitudeMsl()) / 100);
+                currentPeer->direction = (int16_t )(calculateBearingToDestination(&poi) / 100); // In °
+
+                int16_t panServoDirOffset = 0;
+                if (osdConfig()->pan_servo_pwm2centideg != 0){
+                    panServoDirOffset = osdGetPanServoOffset();
+                }
+
+                //line 1
+                //[peer heading][peer ID][LQ][direction to peer]
+
+                //[peer heading]
+                int relativePeerHeading = osdGetHeadingAngle(currentPeer->heading - (int)DECIDEGREES_TO_DEGREES(osdGetHeading()));
+                displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_DECORATION + ((relativePeerHeading + 22) / 45) % 8);
+
+                //[peer ID]
+                displayWriteChar(osdDisplayPort, elemPosX + 1, elemPosY, 65 + currentPeerIndex);
+
+                //[LQ]
+                displayWriteChar(osdDisplayPort, elemPosX + 2, elemPosY, SYM_HUD_SIGNAL_0 + currentPeer->lq);
+
+                //[direction to peer]
+                int directionToPeerError = wrap_180(osdGetHeadingAngle(currentPeer->direction) + panServoDirOffset - (int)DECIDEGREES_TO_DEGREES(osdGetHeading()));
+                uint16_t iconIndexOffset = constrain(((directionToPeerError + 180) / 30), 0, 12);
+                if (iconIndexOffset == 12) {
+                    iconIndexOffset = 0; // Directly behind
+                }
+                displayWriteChar(osdDisplayPort, elemPosX + 3, elemPosY, SYM_HUD_CARDINAL + iconIndexOffset);
+
+
+                //line 2
+                switch ((osd_unit_e)osdConfig()->units) {
+                    case OSD_UNIT_UK:
+                                FALLTHROUGH;
+                    case OSD_UNIT_IMPERIAL:
+                        osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(currentPeer->distance * 100), FEET_PER_MILE, 0, 4, 4, false);
+                        break;
+                    case OSD_UNIT_GA:
+                        osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(currentPeer->distance * 100), (uint32_t)FEET_PER_NAUTICALMILE, 0, 4, 4, false);
+                        break;
+                    default:
+                                FALLTHROUGH;
+                    case OSD_UNIT_METRIC_MPH:
+                                FALLTHROUGH;
+                    case OSD_UNIT_METRIC:
+                        osdFormatCentiNumber(buff, currentPeer->distance * 100, METERS_PER_KILOMETER, 0, 4, 4, false);
+                        break;
+                }
+                displayWrite(osdDisplayPort, elemPosX, elemPosY + 1, buff);
+
+
+                //line 3
+                displayWriteChar(osdDisplayPort, elemPosX, elemPosY + 2, (currentPeer->altitude >= 0) ? SYM_AH_DECORATION_UP : SYM_AH_DECORATION_DOWN);
+
+                int altc = currentPeer->altitude;
+                switch ((osd_unit_e)osdConfig()->units) {
+                    case OSD_UNIT_UK:
+                                FALLTHROUGH;
+                    case OSD_UNIT_GA:
+                                FALLTHROUGH;
+                    case OSD_UNIT_IMPERIAL:
+                        // Convert to feet
+                        altc = CENTIMETERS_TO_FEET(altc * 100);
+                        break;
+                    default:
+                                FALLTHROUGH;
+                    case OSD_UNIT_METRIC_MPH:
+                                FALLTHROUGH;
+                    case OSD_UNIT_METRIC:
+                        // Already in metres
+                        break;
+                }
+
+                altc = ABS(constrain(altc, -999, 999));
+                tfp_sprintf(buff, "%3d", altc);
+                displayWrite(osdDisplayPort, elemPosX + 1, elemPosY + 2, buff);
+
+                return true;
+            }
+        }
+
+        //clear screen
+        for(uint8_t i = 0; i < 4; i++){
+            displayWriteChar(osdDisplayPort, elemPosX + i, elemPosY, SYM_BLANK);
+            displayWriteChar(osdDisplayPort, elemPosX + i, elemPosY + 1, SYM_BLANK);
+            displayWriteChar(osdDisplayPort, elemPosX + i, elemPosY + 2, SYM_BLANK);
+        }
+
+        return true;
+    }
+
     case OSD_CROSSHAIRS: // Hud is a sub-element of the crosshair
 
         osdCrosshairPosition(&elemPosX, &elemPosY);
@@ -3922,7 +4037,9 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
 
     .stats_energy_unit = SETTING_OSD_STATS_ENERGY_UNIT_DEFAULT,
     .stats_page_auto_swap_time = SETTING_OSD_STATS_PAGE_AUTO_SWAP_TIME_DEFAULT,
-    .stats_show_metric_efficiency = SETTING_OSD_STATS_SHOW_METRIC_EFFICIENCY_DEFAULT
+    .stats_show_metric_efficiency = SETTING_OSD_STATS_SHOW_METRIC_EFFICIENCY_DEFAULT,
+
+    .radar_peers_display_time = SETTING_OSD_RADAR_PEERS_DISPLAY_TIME_DEFAULT
 );
 
 void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index 7ac14054994..a73db015793 100644
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -283,9 +283,10 @@ typedef enum {
     OSD_CUSTOM_ELEMENT_1,
     OSD_CUSTOM_ELEMENT_2,
     OSD_CUSTOM_ELEMENT_3,
-    OSD_ADSB_WARNING,
+    OSD_ADSB_WARNING, //150
     OSD_ADSB_INFO,
     OSD_BLACKBOX,
+    OSD_FORMATION_FLIGHT, //153
     OSD_ITEM_COUNT // MUST BE LAST
 } osd_items_e;
 
@@ -460,11 +461,12 @@ typedef struct osdConfig_s {
 #ifndef DISABLE_MSP_DJI_COMPAT
     bool            highlight_djis_missing_characters;  // If enabled, show question marks where there is no character in DJI's font to represent an OSD element symbol
 #endif
-#ifdef USE_ADSB
+ #ifdef USE_ADSB
     uint16_t adsb_distance_warning;                     // in metres
     uint16_t adsb_distance_alert;                       // in metres
     uint16_t adsb_ignore_plane_above_me_limit;          // in metres
-#endif
+ #endif
+    uint8_t  radar_peers_display_time;                  // in seconds
 } osdConfig_t;
 
 PG_DECLARE(osdConfig_t, osdConfig);

From 055642ac67211923516a2ec3bea3657fcc78a7b1 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sat, 22 Jun 2024 12:42:31 +0200
Subject: [PATCH 317/323] Add pan trim to stick inputs and center checks

Add pan trim to rc inputs so it can match center flight mode.

Also add pan trim to osd center checks.

I did not add pan trim to headtracker output, as that would probably
confuse the user more than not.

And not adding it to the headtracker input, the osd will still match the expected position.

You probably shouldn't be mounting your gimbal sideways, anyway. ;)
---
 src/main/drivers/gimbal_common.c | 2 +-
 src/main/io/gimbal_serial.c      | 6 +++---
 src/main/io/osd.c                | 2 +-
 3 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/main/drivers/gimbal_common.c b/src/main/drivers/gimbal_common.c
index a81b1b51c10..e03e7fd9ddd 100644
--- a/src/main/drivers/gimbal_common.c
+++ b/src/main/drivers/gimbal_common.c
@@ -133,7 +133,7 @@ int16_t gimbalCommonGetPanPwm(const gimbalDevice_t *gimbalDevice)
         return gimbalDevice->vTable->getGimbalPanPWM(gimbalDevice);
     }
 
-    return gimbalDevice ? gimbalDevice->currentPanPWM : PWM_RANGE_MIDDLE;
+    return gimbalDevice ? gimbalDevice->currentPanPWM : PWM_RANGE_MIDDLE + gimbalConfig()->panTrim;
 }
 
 #endif
\ No newline at end of file
diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index 6e6b7e93240..311d790a9cd 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -213,17 +213,17 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
     
     if (rxAreFlightChannelsValid() && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)) {
         if (cfg->panChannel > 0) {
-            panPWM = rxGetChannelValue(cfg->panChannel - 1);
+            panPWM = rxGetChannelValue(cfg->panChannel - 1) + cfg->panTrim;
             panPWM = constrain(panPWM, PWM_RANGE_MIN, PWM_RANGE_MAX);
         }
 
         if (cfg->tiltChannel > 0) {
-            tiltPWM = rxGetChannelValue(cfg->tiltChannel - 1);
+            tiltPWM = rxGetChannelValue(cfg->tiltChannel - 1) + cfg->tiltTrim;
             tiltPWM = constrain(tiltPWM, PWM_RANGE_MIN, PWM_RANGE_MAX);
         }
 
         if (cfg->rollChannel > 0) {
-            rollPWM = rxGetChannelValue(cfg->rollChannel - 1);
+            rollPWM = rxGetChannelValue(cfg->rollChannel - 1) + cfg->rollTrim;
             rollPWM = constrain(rollPWM, PWM_RANGE_MIN, PWM_RANGE_MAX);
         }
     }
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index d5ffcd9589e..1b183ff58f7 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -1210,7 +1210,7 @@ int16_t osdGetPanServoOffset(void)
     gimbalDevice_t *dev = gimbalCommonDevice();
     if (dev && gimbalCommonIsReady(dev)) {
         servoPosition = gimbalCommonGetPanPwm(dev);
-        servoMiddle = PWM_RANGE_MIDDLE;
+        servoMiddle = PWM_RANGE_MIDDLE + gimbalConfig()->panTrim;
     }
 
     return (int16_t)CENTIDEGREES_TO_DEGREES((servoPosition - servoMiddle) * osdConfig()->pan_servo_pwm2centideg);

From 7ffb0bc5008ebf75ec2b9fe932f2a5ee43e8acb0 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sat, 22 Jun 2024 14:03:06 +0200
Subject: [PATCH 318/323] Fix center when no valid headtracker data is
 available

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

diff --git a/src/main/io/gimbal_serial.c b/src/main/io/gimbal_serial.c
index 311d790a9cd..4eec2903947 100644
--- a/src/main/io/gimbal_serial.c
+++ b/src/main/io/gimbal_serial.c
@@ -60,6 +60,7 @@ static volatile uint8_t txBuffer[GIMBAL_SERIAL_BUFFER_SIZE];
 #if defined(USE_HEADTRACKER) && defined(USE_HEADTRACKER_SERIAL)
 static gimbalSerialHtrkState_t headTrackerState = { 
     .payloadSize = 0,
+    .attitude = {},
     .state = WAITING_HDR1,
 };
 #endif
@@ -189,7 +190,10 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
 
     gimbalHtkAttitudePkt_t attitude = {
         .sync = {HTKATTITUDE_SYNC0, HTKATTITUDE_SYNC1},
-        .mode = GIMBAL_MODE_DEFAULT
+        .mode = GIMBAL_MODE_DEFAULT,
+        .pan = 0,
+        .tilt = 0,
+        .roll = 0
     };
 
     const gimbalConfig_t *cfg = gimbalConfig();
@@ -238,9 +242,9 @@ void gimbalSerialProcess(gimbalDevice_t *gimbalDevice, timeUs_t currentTime)
 
             DEBUG_SET(DEBUG_HEADTRACKING, 4, 1);
         } else {
-            attitude.pan = constrain(gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, PWM_RANGE_MIDDLE + cfg->panTrim), PWM_RANGE_MIN, PWM_RANGE_MAX);
-            attitude.tilt = constrain(gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, PWM_RANGE_MIDDLE + cfg->tiltTrim), PWM_RANGE_MIN, PWM_RANGE_MAX);
-            attitude.roll = constrain(gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, PWM_RANGE_MIDDLE + cfg->rollTrim), PWM_RANGE_MIN, PWM_RANGE_MAX);
+            attitude.pan = constrain(gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, PWM_RANGE_MIDDLE + cfg->panTrim), HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
+            attitude.tilt = constrain(gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, PWM_RANGE_MIDDLE + cfg->tiltTrim), HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
+            attitude.roll = constrain(gimbal_scale12(PWM_RANGE_MIN, PWM_RANGE_MAX, PWM_RANGE_MIDDLE + cfg->rollTrim), HEADTRACKER_RANGE_MIN, HEADTRACKER_RANGE_MAX);
             DEBUG_SET(DEBUG_HEADTRACKING, 4, -1);
         }
     } else {

From 7b495664afe7308d0a595391239fc16dac0aaecc Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sat, 22 Jun 2024 14:36:37 +0200
Subject: [PATCH 319/323] Update bind_rx cli documentation

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

diff --git a/docs/Cli.md b/docs/Cli.md
index b0cf7d20bfc..a3e18229fc0 100644
--- a/docs/Cli.md
+++ b/docs/Cli.md
@@ -71,7 +71,7 @@ While connected to the CLI, all Logical Switches are temporarily disabled (5.1.0
 | `batch` | Start or end a batch of commands |
 | `battery_profile` | Change battery profile |
 | `beeper` | Show/set beeper (buzzer) [usage](Buzzer.md) |
-| `bind_rx` | Initiate binding for RX SPI or SRXL2 |
+| `bind_rx` | Initiate binding for SRXL2 or CRSF receivers |
 | `blackbox` | Configure blackbox fields |
 | `bootlog` | Show boot events |
 | `color` | Configure colors |

From a8b0ff612cf0de184bc25287ce6a40433607b433 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sat, 22 Jun 2024 14:44:31 +0200
Subject: [PATCH 320/323] Attempt at passing branch protection for non-code
 commits

---
 .github/workflows/non-code-change.yaml | 25 +++++++++++++++++++++++++
 1 file changed, 25 insertions(+)
 create mode 100644 .github/workflows/non-code-change.yaml

diff --git a/.github/workflows/non-code-change.yaml b/.github/workflows/non-code-change.yaml
new file mode 100644
index 00000000000..5c30c6e6ead
--- /dev/null
+++ b/.github/workflows/non-code-change.yaml
@@ -0,0 +1,25 @@
+name: Build firmware
+# Don't enable CI on push, just on PR. If you
+# are working on the main repo and want to trigger
+# a CI build submit a draft PR.
+on: 
+  pull_request:
+    paths-ignore:
+      - 'src/**'
+      - '.github/**'
+      - 'cmake/**'
+      - 'lib/**'
+      - 'docs/Settings.md'
+      - 'CMakeLists.txt'
+      - '*.sh'
+
+jobs:
+  test:
+    #needs: [build]
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v4
+      - name: Install dependencies
+        run: sudo apt-get update && sudo apt-get -y install ninja-build
+      - name: Run Tests
+        run: mkdir -p build && cd build && cmake -DTOOLCHAIN=none -G Ninja .. && ninja check

From bc9975b02a79b730106685e266d5a0241faa6132 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sat, 22 Jun 2024 09:41:20 -0400
Subject: [PATCH 321/323] Update Cli.md

Update list of serial functions.

Another test for not building the firmware on doc only changes.
---
 docs/Cli.md | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/docs/Cli.md b/docs/Cli.md
index a3e18229fc0..6a48244a13a 100644
--- a/docs/Cli.md
+++ b/docs/Cli.md
@@ -157,6 +157,8 @@ A shorter form is also supported to enable and disable a single function using `
 | TELEMETRY_SMARTPORT_MASTER | 23       | 8388608 |
 | UNUSED                | 24            | 16777216 |
 | MSP_DISPLAYPORT       | 25            | 33554432 |
+| GIMBAL_SERIAL         | 26            | 67108864 |
+| HEADTRACKER_SERIAL    | 27            | 134217728 |
 
 Thus, to enable MSP and LTM on a port, one would use the function **value** of 17 (1 << 0)+(1<<4), aka 1+16, aka 17.
 

From ba199c57364746421721d813b56fe96fcaab3376 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sat, 22 Jun 2024 09:43:45 -0400
Subject: [PATCH 322/323] Only generate nightly build release if the change
 includes code.

---
 .github/workflows/dev-builds.yml | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/.github/workflows/dev-builds.yml b/.github/workflows/dev-builds.yml
index e0f648e003e..94492966f79 100644
--- a/.github/workflows/dev-builds.yml
+++ b/.github/workflows/dev-builds.yml
@@ -6,6 +6,14 @@ on:
   push:
     branches:
     - master
+    paths:
+      - 'src/**'
+      - '.github/**'
+      - 'cmake/**'
+      - 'lib/**'
+      - 'docs/Settings.md'
+      - 'CMakeLists.txt'
+      - '*.sh'
 
 jobs:
   build:

From c5d6a4ffb31b298e0e19477dbc32436312e25402 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sat, 22 Jun 2024 10:29:51 -0400
Subject: [PATCH 323/323] Update osd.c

Looks like it was bumped to 11 on master already, going +1
---
 src/main/io/osd.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 9ab45f2736d..757ecc7c909 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -223,7 +223,7 @@ static bool osdDisplayHasCanvas;
 
 #define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees)
 
-PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 11);
+PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 12);
 PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 1);
 
 void osdStartedSaveProcess(void) {