From 5a2a0b87a5a496e87b432bbe04ca8ae3a1156db5 Mon Sep 17 00:00:00 2001
From: gismo2004 <reumuellermarkus@gmail.com>
Date: Sun, 14 Sep 2025 16:57:22 +0200
Subject: [PATCH 001/102] [crsf] add temperature and RPM telemetry

---
 src/main/rx/crsf.h        |  2 +
 src/main/telemetry/crsf.c | 97 +++++++++++++++++++++++++++++++++++++++
 2 files changed, 99 insertions(+)

diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h
index 8d84d52b8e5..02ca7ddac42 100755
--- a/src/main/rx/crsf.h
+++ b/src/main/rx/crsf.h
@@ -88,6 +88,8 @@ typedef enum {
     CRSF_FRAMETYPE_VARIO_SENSOR = 0x07,
     CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08,
     CRSF_FRAMETYPE_BAROMETER_ALTITUDE = 0x09,
+    CRSF_FRAMETYPE_RPM = 0x0C,
+    CRSF_FRAMETYPE_TEMP = 0x0D,
     CRSF_FRAMETYPE_LINK_STATISTICS = 0x14,
     CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16,
     CRSF_FRAMETYPE_ATTITUDE = 0x1E,
diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c
index d86822ada7a..c6ef8a53a35 100755
--- a/src/main/telemetry/crsf.c
+++ b/src/main/telemetry/crsf.c
@@ -46,6 +46,7 @@
 #include "fc/runtime_config.h"
 
 #include "flight/imu.h"
+#include "flight/mixer.h"
 
 #include "io/gps.h"
 #include "io/serial.h"
@@ -55,8 +56,11 @@
 #include "rx/crsf.h"
 #include "rx/rx.h"
 
+
 #include "sensors/battery.h"
+#include "sensors/esc_sensor.h"
 #include "sensors/sensors.h"
+#include "sensors/temperature.h"
 
 #include "telemetry/crsf.h"
 #include "telemetry/telemetry.h"
@@ -162,6 +166,14 @@ static void crsfSerialize16(sbuf_t *dst, uint16_t v)
     crsfSerialize8(dst, (uint8_t)v);
 }
 
+static void crsfSerialize24(sbuf_t *dst, uint32_t v)
+{
+    // Use BigEndian format
+    crsfSerialize8(dst, (v >> 16));
+    crsfSerialize8(dst, (v >> 8));
+    crsfSerialize8(dst, (uint8_t)v);
+}
+
 static void crsfSerialize32(sbuf_t *dst, uint32_t v)
 {
     // Use BigEndian format
@@ -296,6 +308,69 @@ static void crsfBarometerAltitude(sbuf_t *dst)
     crsfSerialize16(dst, altitude_packed);
 }
 
+/*
+0x0C RPM
+Payload:
+uint8_t    rpm_source_id;  // Identifies the source of the RPM data (e.g., 0 = Motor 1, 1 = Motor 2, etc.)
+int24_t    rpm_value[];     // 1 - 19 RPM values with negative ones representing the motor spinning in reverse
+*/
+static void crsfRpm(sbuf_t *dst)
+{
+    uint8_t motorCount = getMotorCount();
+
+    if (STATE(ESC_SENSOR_ENABLED) && motorCount > 0) {
+        sbufWriteU8(dst, 1 + (motorCount * 3) + CRSF_FRAME_LENGTH_TYPE_CRC);
+        crsfSerialize8(dst, CRSF_FRAMETYPE_RPM);
+        // 0 = FC including all ESCs
+        crsfSerialize8(dst, 0);
+
+        for (uint8_t i = 0; i < motorCount; i++) {
+            const escSensorData_t *escState = getEscTelemetry(i);
+            crsfSerialize24(dst, (escState) ? escState->rpm : 0);
+        }
+    }
+}
+
+/*
+0x0D TEMP
+Payload:
+uint8_t temp_source_id; // Identifies the source of the temperature data (e.g., 0 = FC including all ESCs, 1 = Ambient, etc.)
+int16_t temperature[]; // up to 20 temperature values in deci-degree (tenths of a degree) Celsius (e.g., 250 = 25.0°C, -50 = -5.0°C)
+*/
+static void crsfTemperature(sbuf_t *dst)
+{
+
+    uint8_t tempCount = 0;
+    int16_t temperatures[20];
+
+#ifdef USE_ESC_SENSOR
+    uint8_t motorCount = getMotorCount();
+    if (STATE(ESC_SENSOR_ENABLED) && motorCount > 0) {
+        for (uint8_t i = 0; i < motorCount; i++) {
+            const escSensorData_t *escState = getEscTelemetry(i);
+            temperatures[tempCount++] = (escState) ? escState->temperature * 10 : TEMPERATURE_INVALID_VALUE;
+        }
+    }
+#endif
+
+#ifdef USE_TEMPERATURE_SENSOR
+    for (uint8_t i = 0; i < MAX_TEMP_SENSORS; i++) {
+        int16_t value;
+        if (getSensorTemperature(i, &value))
+            temperatures[tempCount++] = value;
+    }
+#endif
+
+    if (tempCount > 0) {
+        sbufWriteU8(dst, 1 + (tempCount * 2) + CRSF_FRAME_LENGTH_TYPE_CRC);
+        crsfSerialize8(dst, CRSF_FRAMETYPE_TEMP);
+        // 0 = FC including all ESCs
+        crsfSerialize8(dst, 0);
+        for (uint8_t i = 0; i < tempCount; i++)
+            crsfSerialize16(dst, temperatures[i]);
+    }
+}
+
 typedef enum {
     CRSF_ACTIVE_ANTENNA1 = 0,
     CRSF_ACTIVE_ANTENNA2 = 1
@@ -465,6 +540,8 @@ typedef enum {
     CRSF_FRAME_GPS_INDEX,
     CRSF_FRAME_VARIO_SENSOR_INDEX,
     CRSF_FRAME_BAROMETER_ALTITUDE_INDEX,
+    CRSF_FRAME_TEMP_INDEX,
+    CRSF_FRAME_RPM_INDEX,
     CRSF_SCHEDULE_COUNT_MAX
 } crsfFrameTypeIndex_e;
 
@@ -526,6 +603,20 @@ static void processCrsf(void)
         crsfFrameFlightMode(dst);
         crsfFinalize(dst);
     }
+#ifdef USE_ESC_SENSOR
+    if (currentSchedule & BV(CRSF_FRAME_RPM_INDEX)) {
+        crsfInitializeFrame(dst);
+        crsfRpm(dst);
+        crsfFinalize(dst);
+    }
+#endif
+#if defined(USE_ESC_SENSOR) || defined(USE_TEMPERATURE_SENSOR)
+    if (currentSchedule & BV(CRSF_FRAME_TEMP_INDEX)) {
+        crsfInitializeFrame(dst);
+        crsfTemperature(dst);
+        crsfFinalize(dst);
+    }
+#endif
 #ifdef USE_GPS
     if (currentSchedule & BV(CRSF_FRAME_GPS_INDEX)) {
         crsfInitializeFrame(dst);
@@ -582,6 +673,12 @@ void initCrsfTelemetry(void)
     if (sensors(SENSOR_BARO)) {
         crsfSchedule[index++] = BV(CRSF_FRAME_BAROMETER_ALTITUDE_INDEX);
     }
+#endif
+#ifdef USE_ESC_SENSOR
+    crsfSchedule[index++] = BV(CRSF_FRAME_RPM_INDEX);
+#endif
+#if defined(USE_ESC_SENSOR) || defined(USE_TEMPERATURE_SENSOR)
+    crsfSchedule[index++] = BV(CRSF_FRAME_TEMP_INDEX);
 #endif
     crsfScheduleCount = (uint8_t)index;
 }

From 710b36e46d56b827456ac80caa5a658d60e4ad55 Mon Sep 17 00:00:00 2001
From: gismo2004 <reumuellermarkus@gmail.com>
Date: Sun, 14 Sep 2025 17:29:00 +0200
Subject: [PATCH 002/102] make the compiler happy

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

diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c
index c6ef8a53a35..afbd1438cb6 100755
--- a/src/main/telemetry/crsf.c
+++ b/src/main/telemetry/crsf.c
@@ -308,6 +308,7 @@ static void crsfBarometerAltitude(sbuf_t *dst)
     crsfSerialize16(dst, altitude_packed);
 }
 
+#ifdef USE_ESC_SENSOR
 /*
 0x0C RPM
 Payload:
@@ -330,6 +331,7 @@ static void crsfRpm(sbuf_t *dst)
         }
     }
 }
+#endif
 
 /*
 0x0D TEMP

From b39cc96f274fb3e554c2cbe197cd64a3d7f6c1cb Mon Sep 17 00:00:00 2001
From: gismo2004 <reumuellermarkus@gmail.com>
Date: Mon, 15 Sep 2025 06:44:52 +0200
Subject: [PATCH 003/102] more compiler warnings

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

diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c
index afbd1438cb6..33cd8253f27 100755
--- a/src/main/telemetry/crsf.c
+++ b/src/main/telemetry/crsf.c
@@ -56,7 +56,6 @@
 #include "rx/crsf.h"
 #include "rx/rx.h"
 
-
 #include "sensors/battery.h"
 #include "sensors/esc_sensor.h"
 #include "sensors/sensors.h"
@@ -166,6 +165,7 @@ static void crsfSerialize16(sbuf_t *dst, uint16_t v)
     crsfSerialize8(dst, (uint8_t)v);
 }
 
+#ifdef USE_ESC_SENSOR
 static void crsfSerialize24(sbuf_t *dst, uint32_t v)
 {
     // Use BigEndian format
@@ -173,6 +173,7 @@ static void crsfSerialize24(sbuf_t *dst, uint32_t v)
     crsfSerialize8(dst, (v >> 8));
     crsfSerialize8(dst, (uint8_t)v);
 }
+#endif
 
 static void crsfSerialize32(sbuf_t *dst, uint32_t v)
 {

From f8dc0bce93c8f4bebf92656942d1354ddef96331 Mon Sep 17 00:00:00 2001
From: gismo2004 <reumuellermarkus@gmail.com>
Date: Tue, 23 Sep 2025 12:06:05 +0200
Subject: [PATCH 004/102] add AirSpeed

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

diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h
index 02ca7ddac42..d5c0219f106 100755
--- a/src/main/rx/crsf.h
+++ b/src/main/rx/crsf.h
@@ -46,6 +46,7 @@ enum {
     CRSF_FRAME_VARIO_SENSOR_PAYLOAD_SIZE = 2,
     CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE = 8,
     CRSF_FRAME_BAROMETER_ALTITUDE_PAYLOAD_SIZE = 2,
+    CRSF_FRAME_AIRSPEED_PAYLOAD_SIZE = 2,
     CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE = 10,
     CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE = 22, // 11 bits per channel * 16 channels = 22 bytes.
     CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE = 6,
@@ -88,6 +89,7 @@ typedef enum {
     CRSF_FRAMETYPE_VARIO_SENSOR = 0x07,
     CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08,
     CRSF_FRAMETYPE_BAROMETER_ALTITUDE = 0x09,
+    CRSF_FRAMETYPE_AIRSPEED_SENSOR = 0x0A,
     CRSF_FRAMETYPE_RPM = 0x0C,
     CRSF_FRAMETYPE_TEMP = 0x0D,
     CRSF_FRAMETYPE_LINK_STATISTICS = 0x14,
diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c
index 33cd8253f27..3007742df5c 100755
--- a/src/main/telemetry/crsf.c
+++ b/src/main/telemetry/crsf.c
@@ -58,6 +58,7 @@
 
 #include "sensors/battery.h"
 #include "sensors/esc_sensor.h"
+#include "sensors/pitotmeter.h"
 #include "sensors/sensors.h"
 #include "sensors/temperature.h"
 
@@ -309,6 +310,21 @@ static void crsfBarometerAltitude(sbuf_t *dst)
     crsfSerialize16(dst, altitude_packed);
 }
 
+#ifdef USE_PITOT
+/*
+0x0A Airspeed sensor
+Payload:
+int16      Air speed ( dm/s )
+*/
+static void crsfFrameAirSpeedSensor(sbuf_t *dst)
+{
+    // use sbufWrite since CRC does not include frame length
+    sbufWriteU8(dst, CRSF_FRAME_AIRSPEED_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
+    crsfSerialize8(dst, CRSF_FRAMETYPE_AIRSPEED_SENSOR);
+    crsfSerialize16(dst, (uint16_t)(getAirspeedEstimate() * 36 / 100));
+}
+#endif
+
 #ifdef USE_ESC_SENSOR
 /*
 0x0C RPM
@@ -545,11 +561,12 @@ typedef enum {
     CRSF_FRAME_BAROMETER_ALTITUDE_INDEX,
     CRSF_FRAME_TEMP_INDEX,
     CRSF_FRAME_RPM_INDEX,
+    CRSF_FRAME_AIRSPEED_INDEX,
     CRSF_SCHEDULE_COUNT_MAX
 } crsfFrameTypeIndex_e;
 
 static uint8_t crsfScheduleCount;
-static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX];
+static uint16_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX];
 
 #if defined(USE_MSP_OVER_TELEMETRY)
 
@@ -586,7 +603,7 @@ static void processCrsf(void)
     }
 
     static uint8_t crsfScheduleIndex = 0;
-    const uint8_t currentSchedule = crsfSchedule[crsfScheduleIndex];
+    const uint16_t currentSchedule = crsfSchedule[crsfScheduleIndex];
 
     sbuf_t crsfPayloadBuf;
     sbuf_t *dst = &crsfPayloadBuf;
@@ -638,6 +655,13 @@ static void processCrsf(void)
         crsfBarometerAltitude(dst);
         crsfFinalize(dst);
     }
+#endif
+#ifdef USE_PITOT
+    if (currentSchedule & BV(CRSF_FRAME_AIRSPEED_INDEX)) {
+        crsfInitializeFrame(dst);
+        crsfFrameAirSpeedSensor(dst);
+        crsfFinalize(dst);
+    }
 #endif
     crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount;
 }
@@ -682,6 +706,11 @@ void initCrsfTelemetry(void)
 #endif
 #if defined(USE_ESC_SENSOR) || defined(USE_TEMPERATURE_SENSOR)
     crsfSchedule[index++] = BV(CRSF_FRAME_TEMP_INDEX);
+#endif
+#ifdef USE_PITOT
+    if (sensors(SENSOR_PITOT)) {
+        crsfSchedule[index++] = BV(CRSF_FRAME_AIRSPEED_INDEX);
+    }
 #endif
     crsfScheduleCount = (uint8_t)index;
 }

From e5bfe799c3d56fc95a7573b27bfec77ca8044249 Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Thu, 18 Dec 2025 12:14:41 -0600
Subject: [PATCH 005/102] Fix CRSF telemetry corruption from PR #11025
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

This commit fixes the critical bugs that caused PR #11025 to be reverted
(PR #11139). The original implementation caused telemetry stream corruption
by emitting malformed frames when sensors were unavailable.

Root Cause:
The original PR scheduled telemetry frames unconditionally (if feature
compiled in) but frame functions only wrote data when sensors were available.
This resulted in frames containing only [SYNC][CRC] instead of the proper
[SYNC][LENGTH][TYPE][PAYLOAD][CRC] structure, corrupting the CRSF protocol
stream and breaking ALL telemetry (not just new frames).

Fixes Implemented:

1. RPM Frame (Bug #1 - CRITICAL):
   - Added conditional scheduling: only schedule if ESC_SENSOR_ENABLED
     and motorCount > 0
   - Added protocol limit enforcement: max 19 RPM values per CRSF spec
   - Location: src/main/telemetry/crsf.c:705-707, 337-343

2. Temperature Frame (Bug #2 - CRITICAL):
   - Added conditional scheduling: only schedule if temperature sources
     are actually available (ESC sensors OR temperature sensors)
   - Added bounds checking: prevent buffer overflow beyond 20 temperatures
   - Location: src/main/telemetry/crsf.c:709-732, 361-381

3. Buffer Overflow Protection (Bug #4):
   - Added MAX_CRSF_TEMPS constant and bounds checks in loops
   - Prevents array overflow if >20 temperature sources configured
   - Location: src/main/telemetry/crsf.c:361, 368, 376

4. Protocol Limit Enforcement (Bug #5):
   - Added MAX_CRSF_RPM_VALUES constant (19 per CRSF spec)
   - Clamp motorCount to protocol limit before sending
   - Location: src/main/telemetry/crsf.c:337, 342-344

Implementation Pattern:
Follows the GPS frame pattern:
  ✓ Conditional scheduling - only schedule if sensor available
  ✓ Unconditional writing - always write complete frame data when called

Testing:
- Code compiles successfully (verified with SITL build)
- No syntax errors or warnings
- All fixes follow existing code patterns in crsf.c

Related Issues:
- Original PR: #11025 (merged Nov 28, 2025, reverted same day)
- Revert PR: #11139
- Investigation: claude/developer/sent/pr11025-root-cause-analysis.md

Co-Authored-By: Claude Sonnet 4.5 <noreply@anthropic.com>
---
 src/main/telemetry/crsf.c | 39 ++++++++++++++++++++++++++++++++++-----
 1 file changed, 34 insertions(+), 5 deletions(-)

diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c
index 3007742df5c..159a8c16ce2 100755
--- a/src/main/telemetry/crsf.c
+++ b/src/main/telemetry/crsf.c
@@ -334,9 +334,15 @@ int24_t    rpm_value[];     // 1 - 19 RPM values with negative ones representing
 */
 static void crsfRpm(sbuf_t *dst)
 {
+    const uint8_t MAX_CRSF_RPM_VALUES = 19;  // CRSF protocol limit: 1-19 RPM values
     uint8_t motorCount = getMotorCount();
 
     if (STATE(ESC_SENSOR_ENABLED) && motorCount > 0) {
+        // Enforce protocol limit
+        if (motorCount > MAX_CRSF_RPM_VALUES) {
+            motorCount = MAX_CRSF_RPM_VALUES;
+        }
+
         sbufWriteU8(dst, 1 + (motorCount * 3) + CRSF_FRAME_LENGTH_TYPE_CRC);
         crsfSerialize8(dst, CRSF_FRAMETYPE_RPM);
         // 0 = FC including all ESCs
@@ -358,14 +364,14 @@ int16_t temperature[]; // up to 20 temperature values in deci-degree (tenths of
 */
 static void crsfTemperature(sbuf_t *dst)
 {
-
+    const uint8_t MAX_CRSF_TEMPS = 20;  // Maximum temperatures per CRSF frame
     uint8_t tempCount = 0;
     int16_t temperatures[20];
 
 #ifdef USE_ESC_SENSOR
     uint8_t motorCount = getMotorCount();
     if (STATE(ESC_SENSOR_ENABLED) && motorCount > 0) {
-        for (uint8_t i = 0; i < motorCount; i++) {
+        for (uint8_t i = 0; i < motorCount && tempCount < MAX_CRSF_TEMPS; i++) {
             const escSensorData_t *escState = getEscTelemetry(i);
             temperatures[tempCount++] = (escState) ? escState->temperature * 10 : TEMPERATURE_INVALID_VALUE;
         }
@@ -373,7 +379,7 @@ static void crsfTemperature(sbuf_t *dst)
 #endif
 
 #ifdef USE_TEMPERATURE_SENSOR
-    for (uint8_t i = 0; i < MAX_TEMP_SENSORS; i++) {
+    for (uint8_t i = 0; i < MAX_TEMP_SENSORS && tempCount < MAX_CRSF_TEMPS; i++) {
         int16_t value;
         if (getSensorTemperature(i, &value))
             temperatures[tempCount++] = value;
@@ -702,10 +708,33 @@ void initCrsfTelemetry(void)
     }
 #endif
 #ifdef USE_ESC_SENSOR
-    crsfSchedule[index++] = BV(CRSF_FRAME_RPM_INDEX);
+    if (STATE(ESC_SENSOR_ENABLED) && getMotorCount() > 0) {
+        crsfSchedule[index++] = BV(CRSF_FRAME_RPM_INDEX);
+    }
 #endif
 #if defined(USE_ESC_SENSOR) || defined(USE_TEMPERATURE_SENSOR)
-    crsfSchedule[index++] = BV(CRSF_FRAME_TEMP_INDEX);
+    // Only schedule temperature frame if we have temperature sources available
+    bool hasTemperatureSources = false;
+#ifdef USE_ESC_SENSOR
+    if (STATE(ESC_SENSOR_ENABLED) && getMotorCount() > 0) {
+        hasTemperatureSources = true;
+    }
+#endif
+#ifdef USE_TEMPERATURE_SENSOR
+    if (!hasTemperatureSources) {
+        // Check if any temperature sensors are configured
+        for (uint8_t i = 0; i < MAX_TEMP_SENSORS; i++) {
+            int16_t value;
+            if (getSensorTemperature(i, &value)) {
+                hasTemperatureSources = true;
+                break;
+            }
+        }
+    }
+#endif
+    if (hasTemperatureSources) {
+        crsfSchedule[index++] = BV(CRSF_FRAME_TEMP_INDEX);
+    }
 #endif
 #ifdef USE_PITOT
     if (sensors(SENSOR_PITOT)) {

From 7b391b3ba1e910f2d54d74dac108d1c331def56b Mon Sep 17 00:00:00 2001
From: meng9633 <3330224355@qq.com>
Date: Mon, 5 Jan 2026 14:34:41 +0800
Subject: [PATCH 006/102] FIX AT32 LED  blink BUG

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

diff --git a/src/main/drivers/io.c b/src/main/drivers/io.c
index b13b73cbfd2..c602a2df492 100644
--- a/src/main/drivers/io.c
+++ b/src/main/drivers/io.c
@@ -244,10 +244,11 @@ void IOToggle(IO_t io)
         IO_GPIO(io)->BSRRL = mask;
     }
 #elif defined(AT32F43x)
- if (IO_GPIO(io)->odt & mask)
-        mask <<= 16;   // bit is set, shift mask to reset half 
-    
-    IO_GPIO(io)->scr = IO_Pin(io);
+    if (IO_GPIO(io)->odt & mask) {
+        IO_GPIO(io)->clr = mask;
+    } else {
+        IO_GPIO(io)->scr = mask;
+    }
 #else
     if (IO_GPIO(io)->ODR & mask)
         mask <<= 16;   // bit is set, shift mask to reset half

From 380fccff2a34919e5c5ac6cd902c3ddadab10b3a Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Sun, 11 Jan 2026 14:36:14 -0600
Subject: [PATCH 007/102] Fix SPI register masking in busWriteBuf

Correct the SPI register address masking in busWriteBuf() to clear the
MSB instead of setting it. For SPI protocol, the MSB indicates read (1)
or write (0) operations. Write operations should use (reg & 0x7F) to
clear the MSB, not (reg | 0x80) which sets it.

This matches the correct implementation in busWrite() at line 318 which
uses (reg & 0x7F) for write operations.

Note: This bug affects theoretical future SPI devices using buffer writes.
Current devices using busWriteBuf() are all I2C-based (VL53L0X, VL53L1X,
MLX90393, TERARANGER_EVO, US42 rangefinders) and are unaffected.

Fixes #10674
---
 src/main/drivers/bus.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/drivers/bus.c b/src/main/drivers/bus.c
index 0da3f76d307..71410fb2748 100644
--- a/src/main/drivers/bus.c
+++ b/src/main/drivers/bus.c
@@ -283,7 +283,7 @@ bool busWriteBuf(const busDevice_t * dev, uint8_t reg, const uint8_t * data, uin
                 return spiBusWriteBuffer(dev, reg, data, length);
             }
             else {
-                return spiBusWriteBuffer(dev, reg | 0x80, data, length);
+                return spiBusWriteBuffer(dev, reg & 0x7F, data, length);
             }
 #else
             return false;

From bd45290a09a729d98bc0f32916e62dcecac585f9 Mon Sep 17 00:00:00 2001
From: Radek Domin <radek.domin@gmail.com>
Date: Fri, 16 Jan 2026 12:35:57 +0100
Subject: [PATCH 008/102] Adding support for switchable 9v BEC for FLYWOOF405HD
 and FLYWOOF405PRO targets.

---
 src/main/target/FLYWOOF405PRO/config.c | 4 ++++
 src/main/target/FLYWOOF405PRO/target.h | 6 ++++++
 2 files changed, 10 insertions(+)

diff --git a/src/main/target/FLYWOOF405PRO/config.c b/src/main/target/FLYWOOF405PRO/config.c
index dfe6c0ce100..6366994c25c 100644
--- a/src/main/target/FLYWOOF405PRO/config.c
+++ b/src/main/target/FLYWOOF405PRO/config.c
@@ -31,4 +31,8 @@ void targetConfiguration(void)
 
     timerOverridesMutable(timer2id(TIM3))->outputMode = OUTPUT_MODE_MOTORS;
     timerOverridesMutable(timer2id(TIM2))->outputMode = OUTPUT_MODE_MOTORS;
+
+    // Pinio Box params as per manufacturer specification
+    pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1;
+    pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2;
 }
diff --git a/src/main/target/FLYWOOF405PRO/target.h b/src/main/target/FLYWOOF405PRO/target.h
index a5a02c5fe84..22ddd0a6fd2 100644
--- a/src/main/target/FLYWOOF405PRO/target.h
+++ b/src/main/target/FLYWOOF405PRO/target.h
@@ -152,6 +152,12 @@
 #define CURRENT_METER_ADC_CHANNEL   ADC_CHN_2
 #define RSSI_ADC_CHANNEL            ADC_CHN_3
 
+// *************** PINIO ***********************
+#define USE_PINIO
+#define USE_PINIOBOX
+#define PINIO1_PIN              PD6
+#define PINIO2_PIN              PC8
+
 // *************** LED2812 ************************
 #define USE_LED_STRIP
 #define WS2811_PIN                      PA9

From a6ba1168fa693d176ba1324982d05ff5351a7841 Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Sat, 17 Jan 2026 21:32:53 -0600
Subject: [PATCH 009/102] WIP: Attempt circular DShot DMA during settings save

This approach doesn't prevent DShot interruption during EEPROM writes.
Committing for potential future refinement.

Changes:
- Added impl_timerPWMSetDMACircular() to switch DMA mode at runtime
- Modified processDelayedSave() to use circular mode during writeEEPROM()
- Called pwmCompleteMotorUpdate() 3x to latch DShot 0 packets

Issue: DShot still shows gaps during settings save on oscilloscope.

Next approach: Test with simple GPIO high instead of DShot.
---
 src/main/drivers/pwm_output.c           | 14 ++++++++++++++
 src/main/drivers/pwm_output.h           |  1 +
 src/main/drivers/timer_impl.h           |  1 +
 src/main/drivers/timer_impl_hal.c       | 23 +++++++++++++++++++++++
 src/main/drivers/timer_impl_stdperiph.c | 20 ++++++++++++++++++++
 src/main/fc/config.c                    | 14 ++++++++++++++
 6 files changed, 73 insertions(+)

diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c
index 619f4b95db5..42debf6cb48 100644
--- a/src/main/drivers/pwm_output.c
+++ b/src/main/drivers/pwm_output.c
@@ -226,6 +226,20 @@ void pwmEnableMotors(void)
     pwmMotorsEnabled = true;
 }
 
+void pwmSetMotorDMACircular(bool circular)
+{
+#ifdef USE_DSHOT
+    // Set DMA circular mode for all motor outputs
+    for (int i = 0; i < getMotorCount(); i++) {
+        if (motors[i].pwmPort && motors[i].pwmPort->tch) {
+            impl_timerPWMSetDMACircular(motors[i].pwmPort->tch, circular);
+        }
+    }
+#else
+    UNUSED(circular);
+#endif
+}
+
 bool isMotorBrushed(uint16_t motorPwmRateHz)
 {
     return (motorPwmRateHz > 500);
diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h
index 1041ace04fa..1dc644f7f4e 100644
--- a/src/main/drivers/pwm_output.h
+++ b/src/main/drivers/pwm_output.h
@@ -50,6 +50,7 @@ void pwmWriteServo(uint8_t index, uint16_t value);
 
 void pwmDisableMotors(void);
 void pwmEnableMotors(void);
+void pwmSetMotorDMACircular(bool circular);
 struct timerHardware_s;
 
 void pwmMotorPreconfigure(void);
diff --git a/src/main/drivers/timer_impl.h b/src/main/drivers/timer_impl.h
index 4d0a87f9aa5..83a7a93f2b1 100644
--- a/src/main/drivers/timer_impl.h
+++ b/src/main/drivers/timer_impl.h
@@ -84,6 +84,7 @@ bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBuf
 void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferElementCount);
 void impl_timerPWMStartDMA(TCH_t * tch);
 void impl_timerPWMStopDMA(TCH_t * tch);
+void impl_timerPWMSetDMACircular(TCH_t * tch, bool circular);
 
 #ifdef USE_DSHOT_DMAR
 bool impl_timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount);
diff --git a/src/main/drivers/timer_impl_hal.c b/src/main/drivers/timer_impl_hal.c
index 8df0f7024d3..6390b4beef5 100644
--- a/src/main/drivers/timer_impl_hal.c
+++ b/src/main/drivers/timer_impl_hal.c
@@ -580,3 +580,26 @@ void impl_timerPWMStopDMA(TCH_t * tch)
 
     HAL_TIM_Base_Start(tch->timCtx->timHandle);
 }
+
+void impl_timerPWMSetDMACircular(TCH_t * tch, bool circular)
+{
+    if (!tch->dma || !tch->dma->dma) {
+        return;
+    }
+
+    const uint32_t streamLL = lookupDMALLStreamTable[DMATAG_GET_STREAM(tch->timHw->dmaTag)];
+    DMA_TypeDef *dmaBase = tch->dma->dma;
+
+    // Temporarily disable DMA while modifying configuration
+    LL_DMA_DisableStream(dmaBase, streamLL);
+
+    // Modify the DMA mode
+    if (circular) {
+        LL_DMA_SetMode(dmaBase, streamLL, LL_DMA_MODE_CIRCULAR);
+    } else {
+        LL_DMA_SetMode(dmaBase, streamLL, LL_DMA_MODE_NORMAL);
+    }
+
+    // Re-enable DMA
+    LL_DMA_EnableStream(dmaBase, streamLL);
+}
diff --git a/src/main/drivers/timer_impl_stdperiph.c b/src/main/drivers/timer_impl_stdperiph.c
index d2bd35dd521..2c82b052890 100644
--- a/src/main/drivers/timer_impl_stdperiph.c
+++ b/src/main/drivers/timer_impl_stdperiph.c
@@ -519,3 +519,23 @@ void impl_timerPWMStopDMA(TCH_t * tch)
     tch->dmaState = TCH_DMA_IDLE;
     TIM_Cmd(tch->timHw->tim, ENABLE);
 }
+
+void impl_timerPWMSetDMACircular(TCH_t * tch, bool circular)
+{
+    if (!tch->dma || !tch->dma->ref) {
+        return;
+    }
+
+    // Temporarily disable DMA while modifying configuration
+    DMA_Cmd(tch->dma->ref, DISABLE);
+
+    // Modify the DMA mode
+    if (circular) {
+        tch->dma->ref->CR |= DMA_SxCR_CIRC;  // Set circular bit
+    } else {
+        tch->dma->ref->CR &= ~DMA_SxCR_CIRC; // Clear circular bit
+    }
+
+    // Re-enable DMA
+    DMA_Cmd(tch->dma->ref, ENABLE);
+}
diff --git a/src/main/fc/config.c b/src/main/fc/config.c
index d3021317ae5..b2e29caf4e0 100755
--- a/src/main/fc/config.c
+++ b/src/main/fc/config.c
@@ -420,7 +420,21 @@ void processDelayedSave(void)
         saveState = SAVESTATE_NONE;
     } else if (saveState == SAVESTATE_SAVEONLY) {
         suspendRxSignal();
+
+        // Prevent ESC spinup during settings save
+        // Switch to circular mode first
+        pwmSetMotorDMACircular(true);
+        // Force motor updates to latch current (zero) throttle into circular DMA buffer
+        pwmCompleteMotorUpdate();
+        delayMicroseconds(200);
+        pwmCompleteMotorUpdate();
+        delayMicroseconds(200);
+        pwmCompleteMotorUpdate();
+
         writeEEPROM();
+
+        pwmSetMotorDMACircular(false);
+
         resumeRxSignal();
         saveState = SAVESTATE_NONE;
     }

From d32cf254e68c718e61680acaa6f6917e8cdccc9a Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Sun, 18 Jan 2026 00:04:47 -0600
Subject: [PATCH 010/102] Fix ESC spinup during MSP settings save (#10913)
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Move circular DShot DMA code from processDelayedSave() to writeConfigToEEPROM().
This ensures the fix works for MSP_EEPROM_WRITE commands, not just delayed saves.

The MSP call path is:
MSP_EEPROM_WRITE → writeEEPROM() → writeConfigToEEPROM() → writeSettingsToEEPROM()

The previous commit (a6ba1168f) had circular DMA in processDelayedSave(),
which is only called for delayed saves (on disarm), not MSP commands.

Changes:
- Move circular DMA setup to writeConfigToEEPROM() in config_eeprom.c
- Remove unused pwmSetMotorPinsHigh() function
- Add pwm_output.h include to config_eeprom.c

Test method:
- MSP_EEPROM_WRITE command sent once per second
- DShot signal monitored on oscilloscope
- Confirmed: DShot no longer interrupted during settings save

Issue: #10913
Related: #9441
---
 src/main/config/config_eeprom.c | 14 ++++++++++++++
 1 file changed, 14 insertions(+)

diff --git a/src/main/config/config_eeprom.c b/src/main/config/config_eeprom.c
index 6c1dfc3dc7e..38b35937019 100755
--- a/src/main/config/config_eeprom.c
+++ b/src/main/config/config_eeprom.c
@@ -33,6 +33,7 @@
 
 #include "drivers/system.h"
 #include "drivers/flash.h"
+#include "drivers/pwm_output.h"
 
 #include "fc/config.h"
 
@@ -321,6 +322,16 @@ static bool writeSettingsToEEPROM(void)
 
 void writeConfigToEEPROM(void)
 {
+    // Prevent ESC spinup during settings save using circular DMA
+    pwmSetMotorDMACircular(true);
+
+    // Force motor updates to latch current (zero) throttle into circular DMA buffer
+    pwmCompleteMotorUpdate();
+    delayMicroseconds(200);
+    pwmCompleteMotorUpdate();
+    delayMicroseconds(200);
+    pwmCompleteMotorUpdate();
+
     bool success = false;
     // write it
     for (int attempt = 0; attempt < 3 && !success; attempt++) {
@@ -333,6 +344,9 @@ void writeConfigToEEPROM(void)
         }
     }
 
+    // Restore normal DMA mode
+    pwmSetMotorDMACircular(false);
+
     if (success && isEEPROMContentValid()) {
         return;
     }

From 4249b5576587211f32fd06ce85aa7b00c34a04f6 Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Sun, 18 Jan 2026 15:35:17 -0600
Subject: [PATCH 011/102] Address code review feedback for ESC spinup fix

Based on code-reviewer agent feedback:

1. Add missing AT32 platform implementation
   - Implement impl_timerPWMSetDMACircular() for AT32F43x targets
   - Uses AT32 loop_mode (ctrl_bit.lm) instead of DMA_SxCR_CIRC

2. Remove duplicate circular DMA code from config.c
   - processDelayedSave() calls writeEEPROM() which calls writeConfigToEEPROM()
   - writeConfigToEEPROM() already has circular DMA protection
   - Removed redundant nested enable/disable from config.c

3. Add ATOMIC_BLOCK protection to DMA mode switch
   - Consistent with existing impl_timerPWMStopDMA() pattern
   - Prevents interrupt interference during DMA reconfiguration
   - Applied to HAL, StdPeriph, and AT32 implementations

Issue: #10913
---
 src/main/drivers/timer_impl_hal.c            | 23 +++++++++++---------
 src/main/drivers/timer_impl_stdperiph.c      | 23 +++++++++++---------
 src/main/drivers/timer_impl_stdperiph_at32.c | 23 ++++++++++++++++++++
 src/main/fc/config.c                         | 16 +-------------
 4 files changed, 50 insertions(+), 35 deletions(-)

diff --git a/src/main/drivers/timer_impl_hal.c b/src/main/drivers/timer_impl_hal.c
index 6390b4beef5..0036a6d50fb 100644
--- a/src/main/drivers/timer_impl_hal.c
+++ b/src/main/drivers/timer_impl_hal.c
@@ -590,16 +590,19 @@ void impl_timerPWMSetDMACircular(TCH_t * tch, bool circular)
     const uint32_t streamLL = lookupDMALLStreamTable[DMATAG_GET_STREAM(tch->timHw->dmaTag)];
     DMA_TypeDef *dmaBase = tch->dma->dma;
 
-    // Temporarily disable DMA while modifying configuration
-    LL_DMA_DisableStream(dmaBase, streamLL);
+    // Protect DMA reconfiguration from interrupt interference
+    ATOMIC_BLOCK(NVIC_PRIO_MAX) {
+        // Temporarily disable DMA while modifying configuration
+        LL_DMA_DisableStream(dmaBase, streamLL);
 
-    // Modify the DMA mode
-    if (circular) {
-        LL_DMA_SetMode(dmaBase, streamLL, LL_DMA_MODE_CIRCULAR);
-    } else {
-        LL_DMA_SetMode(dmaBase, streamLL, LL_DMA_MODE_NORMAL);
-    }
+        // Modify the DMA mode
+        if (circular) {
+            LL_DMA_SetMode(dmaBase, streamLL, LL_DMA_MODE_CIRCULAR);
+        } else {
+            LL_DMA_SetMode(dmaBase, streamLL, LL_DMA_MODE_NORMAL);
+        }
 
-    // Re-enable DMA
-    LL_DMA_EnableStream(dmaBase, streamLL);
+        // Re-enable DMA
+        LL_DMA_EnableStream(dmaBase, streamLL);
+    }
 }
diff --git a/src/main/drivers/timer_impl_stdperiph.c b/src/main/drivers/timer_impl_stdperiph.c
index 2c82b052890..d3bb0555004 100644
--- a/src/main/drivers/timer_impl_stdperiph.c
+++ b/src/main/drivers/timer_impl_stdperiph.c
@@ -526,16 +526,19 @@ void impl_timerPWMSetDMACircular(TCH_t * tch, bool circular)
         return;
     }
 
-    // Temporarily disable DMA while modifying configuration
-    DMA_Cmd(tch->dma->ref, DISABLE);
+    // Protect DMA reconfiguration from interrupt interference
+    ATOMIC_BLOCK(NVIC_PRIO_MAX) {
+        // Temporarily disable DMA while modifying configuration
+        DMA_Cmd(tch->dma->ref, DISABLE);
 
-    // Modify the DMA mode
-    if (circular) {
-        tch->dma->ref->CR |= DMA_SxCR_CIRC;  // Set circular bit
-    } else {
-        tch->dma->ref->CR &= ~DMA_SxCR_CIRC; // Clear circular bit
-    }
+        // Modify the DMA mode
+        if (circular) {
+            tch->dma->ref->CR |= DMA_SxCR_CIRC;  // Set circular bit
+        } else {
+            tch->dma->ref->CR &= ~DMA_SxCR_CIRC; // Clear circular bit
+        }
 
-    // Re-enable DMA
-    DMA_Cmd(tch->dma->ref, ENABLE);
+        // Re-enable DMA
+        DMA_Cmd(tch->dma->ref, ENABLE);
+    }
 }
diff --git a/src/main/drivers/timer_impl_stdperiph_at32.c b/src/main/drivers/timer_impl_stdperiph_at32.c
index 0cc194897d9..b74fd4f18a4 100644
--- a/src/main/drivers/timer_impl_stdperiph_at32.c
+++ b/src/main/drivers/timer_impl_stdperiph_at32.c
@@ -407,3 +407,26 @@ void impl_timerPWMStopDMA(TCH_t * tch)
     tch->dmaState = TCH_DMA_IDLE;
     tmr_counter_enable(tch->timHw->tim, TRUE);
 }
+
+void impl_timerPWMSetDMACircular(TCH_t * tch, bool circular)
+{
+    if (!tch->dma || !tch->dma->ref) {
+        return;
+    }
+
+    // Protect DMA reconfiguration from interrupt interference
+    ATOMIC_BLOCK(NVIC_PRIO_MAX) {
+        // Temporarily disable DMA while modifying configuration
+        dma_channel_enable(tch->dma->ref, FALSE);
+
+        // Modify the DMA loop mode (AT32's equivalent of circular mode)
+        if (circular) {
+            tch->dma->ref->ctrl_bit.lm = TRUE;  // Enable loop mode
+        } else {
+            tch->dma->ref->ctrl_bit.lm = FALSE; // Disable loop mode
+        }
+
+        // Re-enable DMA
+        dma_channel_enable(tch->dma->ref, TRUE);
+    }
+}
diff --git a/src/main/fc/config.c b/src/main/fc/config.c
index b2e29caf4e0..fcc40d530e0 100755
--- a/src/main/fc/config.c
+++ b/src/main/fc/config.c
@@ -420,21 +420,7 @@ void processDelayedSave(void)
         saveState = SAVESTATE_NONE;
     } else if (saveState == SAVESTATE_SAVEONLY) {
         suspendRxSignal();
-
-        // Prevent ESC spinup during settings save
-        // Switch to circular mode first
-        pwmSetMotorDMACircular(true);
-        // Force motor updates to latch current (zero) throttle into circular DMA buffer
-        pwmCompleteMotorUpdate();
-        delayMicroseconds(200);
-        pwmCompleteMotorUpdate();
-        delayMicroseconds(200);
-        pwmCompleteMotorUpdate();
-
-        writeEEPROM();
-
-        pwmSetMotorDMACircular(false);
-
+        writeEEPROM();  // Circular DMA protection is inside writeConfigToEEPROM()
         resumeRxSignal();
         saveState = SAVESTATE_NONE;
     }

From ebcd802ff1724af418065d613e05edf90d113361 Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Sun, 18 Jan 2026 23:59:18 -0600
Subject: [PATCH 012/102] Fix H7 circular DMA implementation - wait for stream
 disable

Critical fixes for STM32H7 DMA circular mode:
- Wait for EN bit to actually clear before changing mode (was the primary bug)
- Disable/re-enable timer DMA requests during reconfiguration
- Reload DMA transfer count after mode change
- Clear pending DMA flags

Without these changes, the mode change was being ignored because the DMA
stream was still active when we tried to modify the configuration.
---
 src/main/drivers/timer_impl_hal.c | 29 +++++++++++++++++++++++++++--
 1 file changed, 27 insertions(+), 2 deletions(-)

diff --git a/src/main/drivers/timer_impl_hal.c b/src/main/drivers/timer_impl_hal.c
index 0036a6d50fb..5d62fc42128 100644
--- a/src/main/drivers/timer_impl_hal.c
+++ b/src/main/drivers/timer_impl_hal.c
@@ -590,11 +590,27 @@ void impl_timerPWMSetDMACircular(TCH_t * tch, bool circular)
     const uint32_t streamLL = lookupDMALLStreamTable[DMATAG_GET_STREAM(tch->timHw->dmaTag)];
     DMA_TypeDef *dmaBase = tch->dma->dma;
 
+    // Save the current transfer count before disabling
+    uint32_t dataLength = LL_DMA_GetDataLength(dmaBase, streamLL);
+
     // Protect DMA reconfiguration from interrupt interference
     ATOMIC_BLOCK(NVIC_PRIO_MAX) {
-        // Temporarily disable DMA while modifying configuration
+        // Disable timer DMA request first to stop new transfer triggers
+        LL_TIM_DisableDMAReq_CCx(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex]);
+
+        // Disable the DMA stream
         LL_DMA_DisableStream(dmaBase, streamLL);
 
+        // CRITICAL: Wait for stream to actually become disabled
+        // The EN bit doesn't clear immediately, especially if transfer is in progress
+        uint32_t timeout = 10000;
+        while (LL_DMA_IsEnabledStream(dmaBase, streamLL) && timeout--) {
+            __NOP();
+        }
+
+        // Clear any pending transfer complete flags
+        DMA_CLEAR_FLAG(tch->dma, DMA_IT_TCIF);
+
         // Modify the DMA mode
         if (circular) {
             LL_DMA_SetMode(dmaBase, streamLL, LL_DMA_MODE_CIRCULAR);
@@ -602,7 +618,16 @@ void impl_timerPWMSetDMACircular(TCH_t * tch, bool circular)
             LL_DMA_SetMode(dmaBase, streamLL, LL_DMA_MODE_NORMAL);
         }
 
-        // Re-enable DMA
+        // Reload the transfer count (required after mode change)
+        // If dataLength was 0 (transfer completed), keep it at 0 - the next motor update will reload it
+        if (dataLength > 0) {
+            LL_DMA_SetDataLength(dmaBase, streamLL, dataLength);
+        }
+
+        // Re-enable DMA stream
         LL_DMA_EnableStream(dmaBase, streamLL);
+
+        // Re-enable timer DMA requests
+        LL_TIM_EnableDMAReq_CCx(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex]);
     }
 }

From 9c2c1f4483edf7c7beaadcff68ec3725647c8252 Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Mon, 19 Jan 2026 01:13:42 -0600
Subject: [PATCH 013/102] Fix SITL build: Guard DShot circular DMA calls with
 SITL_BUILD check

SITL doesn't have real PWM/motor hardware, so pwmSetMotorDMACircular()
and pwmCompleteMotorUpdate() don't exist in SITL builds.

Wrap these calls with #if \!defined(SITL_BUILD) to allow SITL builds to
compile while preserving the ESC spinup fix for hardware builds.
---
 src/main/config/config_eeprom.c | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/src/main/config/config_eeprom.c b/src/main/config/config_eeprom.c
index 38b35937019..1efadd2a3fd 100755
--- a/src/main/config/config_eeprom.c
+++ b/src/main/config/config_eeprom.c
@@ -322,6 +322,7 @@ static bool writeSettingsToEEPROM(void)
 
 void writeConfigToEEPROM(void)
 {
+#if !defined(SITL_BUILD)
     // Prevent ESC spinup during settings save using circular DMA
     pwmSetMotorDMACircular(true);
 
@@ -331,6 +332,7 @@ void writeConfigToEEPROM(void)
     pwmCompleteMotorUpdate();
     delayMicroseconds(200);
     pwmCompleteMotorUpdate();
+#endif
 
     bool success = false;
     // write it
@@ -344,8 +346,10 @@ void writeConfigToEEPROM(void)
         }
     }
 
+#if !defined(SITL_BUILD)
     // Restore normal DMA mode
     pwmSetMotorDMACircular(false);
+#endif
 
     if (success && isEEPROMContentValid()) {
         return;

From eda9f2207fc9014419f2f754b007c23e9dece925 Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Mon, 19 Jan 2026 15:21:05 -0600
Subject: [PATCH 014/102] Add DMA disable timeout handling to all platforms

Address qodo-code-review feedback: Add defensive timeout checks when
waiting for DMA streams/channels to disable before reconfiguring.

Changes:
- H7 (timer_impl_hal.c): Check if timeout expired and abort if DMA
  still enabled
- F4/F7 (timer_impl_stdperiph.c): Add wait loop for EN bit to clear
  with timeout check
- AT32 (timer_impl_stdperiph_at32.c): Add wait loop for chen bit to
  clear with timeout check

This prevents potential race conditions where DMA configuration could
be modified while the stream is still active, which could cause
unstable behavior.
---
 src/main/drivers/timer_impl_hal.c            |  7 +++++++
 src/main/drivers/timer_impl_stdperiph.c      | 13 +++++++++++++
 src/main/drivers/timer_impl_stdperiph_at32.c | 13 +++++++++++++
 3 files changed, 33 insertions(+)

diff --git a/src/main/drivers/timer_impl_hal.c b/src/main/drivers/timer_impl_hal.c
index 5d62fc42128..85a8429f8f9 100644
--- a/src/main/drivers/timer_impl_hal.c
+++ b/src/main/drivers/timer_impl_hal.c
@@ -608,6 +608,13 @@ void impl_timerPWMSetDMACircular(TCH_t * tch, bool circular)
             __NOP();
         }
 
+        // If timeout occurred, DMA stream is still enabled - abort reconfiguration
+        if (timeout == 0 && LL_DMA_IsEnabledStream(dmaBase, streamLL)) {
+            // Re-enable timer DMA request and return to avoid unstable state
+            LL_TIM_EnableDMAReq_CCx(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex]);
+            return;
+        }
+
         // Clear any pending transfer complete flags
         DMA_CLEAR_FLAG(tch->dma, DMA_IT_TCIF);
 
diff --git a/src/main/drivers/timer_impl_stdperiph.c b/src/main/drivers/timer_impl_stdperiph.c
index d3bb0555004..af685d0a5ba 100644
--- a/src/main/drivers/timer_impl_stdperiph.c
+++ b/src/main/drivers/timer_impl_stdperiph.c
@@ -531,6 +531,19 @@ void impl_timerPWMSetDMACircular(TCH_t * tch, bool circular)
         // Temporarily disable DMA while modifying configuration
         DMA_Cmd(tch->dma->ref, DISABLE);
 
+        // Wait for DMA stream to actually be disabled
+        // The EN bit doesn't clear immediately, especially if transfer is in progress
+        uint32_t timeout = 10000;
+        while ((tch->dma->ref->CR & DMA_SxCR_EN) && timeout--) {
+            __NOP();
+        }
+
+        // If timeout occurred, DMA stream is still enabled - abort reconfiguration
+        if (timeout == 0 && (tch->dma->ref->CR & DMA_SxCR_EN)) {
+            DMA_Cmd(tch->dma->ref, ENABLE); // Re-enable and return
+            return;
+        }
+
         // Modify the DMA mode
         if (circular) {
             tch->dma->ref->CR |= DMA_SxCR_CIRC;  // Set circular bit
diff --git a/src/main/drivers/timer_impl_stdperiph_at32.c b/src/main/drivers/timer_impl_stdperiph_at32.c
index b74fd4f18a4..e39cef7b9ca 100644
--- a/src/main/drivers/timer_impl_stdperiph_at32.c
+++ b/src/main/drivers/timer_impl_stdperiph_at32.c
@@ -419,6 +419,19 @@ void impl_timerPWMSetDMACircular(TCH_t * tch, bool circular)
         // Temporarily disable DMA while modifying configuration
         dma_channel_enable(tch->dma->ref, FALSE);
 
+        // Wait for DMA channel to actually be disabled
+        // The enable bit doesn't clear immediately, especially if transfer is in progress
+        uint32_t timeout = 10000;
+        while (tch->dma->ref->ctrl_bit.chen && timeout--) {
+            __NOP();
+        }
+
+        // If timeout occurred, DMA channel is still enabled - abort reconfiguration
+        if (timeout == 0 && tch->dma->ref->ctrl_bit.chen) {
+            dma_channel_enable(tch->dma->ref, TRUE); // Re-enable and return
+            return;
+        }
+
         // Modify the DMA loop mode (AT32's equivalent of circular mode)
         if (circular) {
             tch->dma->ref->ctrl_bit.lm = TRUE;  // Enable loop mode

From c7b342df488428a43a513ecdb779a123aa2348fc Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Mon, 19 Jan 2026 19:39:55 -0600
Subject: [PATCH 015/102] Fix build for targets without DShot: Add USE_DSHOT
 guard

Some hardware targets don't compile DShot support, causing linker
errors when trying to call pwmSetMotorDMACircular() and
pwmCompleteMotorUpdate().

Change guard from:
  #if \!defined(SITL_BUILD)
To:
  #if \!defined(SITL_BUILD) && defined(USE_DSHOT)

This ensures the functions are only called on targets that actually
have DShot compiled in, fixing build failures on targets like
BEEROTORF4.
---
 src/main/config/config_eeprom.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/config/config_eeprom.c b/src/main/config/config_eeprom.c
index 1efadd2a3fd..e4a22aaa581 100755
--- a/src/main/config/config_eeprom.c
+++ b/src/main/config/config_eeprom.c
@@ -322,7 +322,7 @@ static bool writeSettingsToEEPROM(void)
 
 void writeConfigToEEPROM(void)
 {
-#if !defined(SITL_BUILD)
+#if !defined(SITL_BUILD) && defined(USE_DSHOT)
     // Prevent ESC spinup during settings save using circular DMA
     pwmSetMotorDMACircular(true);
 
@@ -346,7 +346,7 @@ void writeConfigToEEPROM(void)
         }
     }
 
-#if !defined(SITL_BUILD)
+#if !defined(SITL_BUILD) && defined(USE_DSHOT)
     // Restore normal DMA mode
     pwmSetMotorDMACircular(false);
 #endif

From e7075c5b5044f81eb1799d7ffb0659eb3a3e3deb Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Mon, 19 Jan 2026 21:05:30 -0600
Subject: [PATCH 016/102] Remove unnecessary comments

- config.c: Remove comment about circular DMA protection location
  (obvious from context)
- timer_impl_stdperiph_at32.c: Remove redundant comment about loop mode
  (already clear from 'Enable loop mode' / 'Disable loop mode' comments)
---
 src/main/drivers/timer_impl_stdperiph_at32.c | 1 -
 src/main/fc/config.c                         | 2 +-
 2 files changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/main/drivers/timer_impl_stdperiph_at32.c b/src/main/drivers/timer_impl_stdperiph_at32.c
index e39cef7b9ca..d9433ba1f66 100644
--- a/src/main/drivers/timer_impl_stdperiph_at32.c
+++ b/src/main/drivers/timer_impl_stdperiph_at32.c
@@ -432,7 +432,6 @@ void impl_timerPWMSetDMACircular(TCH_t * tch, bool circular)
             return;
         }
 
-        // Modify the DMA loop mode (AT32's equivalent of circular mode)
         if (circular) {
             tch->dma->ref->ctrl_bit.lm = TRUE;  // Enable loop mode
         } else {
diff --git a/src/main/fc/config.c b/src/main/fc/config.c
index fcc40d530e0..d3021317ae5 100755
--- a/src/main/fc/config.c
+++ b/src/main/fc/config.c
@@ -420,7 +420,7 @@ void processDelayedSave(void)
         saveState = SAVESTATE_NONE;
     } else if (saveState == SAVESTATE_SAVEONLY) {
         suspendRxSignal();
-        writeEEPROM();  // Circular DMA protection is inside writeConfigToEEPROM()
+        writeEEPROM();
         resumeRxSignal();
         saveState = SAVESTATE_NONE;
     }

From 0f45b6b2f061d4e352d46c45d1ab5eb936e9453b Mon Sep 17 00:00:00 2001
From: meng9633 <3330224355@qq.com>
Date: Fri, 23 Jan 2026 17:21:39 +0800
Subject: [PATCH 017/102] Add target BLUEBERRYH743

---
 src/main/target/BLUEBERRYH743/CMakeLists.txt |   2 +
 src/main/target/BLUEBERRYH743/config.c       |  34 +++
 src/main/target/BLUEBERRYH743/target.c       |  60 ++++++
 src/main/target/BLUEBERRYH743/target.h       | 213 +++++++++++++++++++
 4 files changed, 309 insertions(+)
 create mode 100644 src/main/target/BLUEBERRYH743/CMakeLists.txt
 create mode 100644 src/main/target/BLUEBERRYH743/config.c
 create mode 100644 src/main/target/BLUEBERRYH743/target.c
 create mode 100644 src/main/target/BLUEBERRYH743/target.h

diff --git a/src/main/target/BLUEBERRYH743/CMakeLists.txt b/src/main/target/BLUEBERRYH743/CMakeLists.txt
new file mode 100644
index 00000000000..99d8aeeda11
--- /dev/null
+++ b/src/main/target/BLUEBERRYH743/CMakeLists.txt
@@ -0,0 +1,2 @@
+target_stm32h743xi(BLUEBERRYH743)
+target_stm32h743xi(BLUEBERRYH743HD)
diff --git a/src/main/target/BLUEBERRYH743/config.c b/src/main/target/BLUEBERRYH743/config.c
new file mode 100644
index 00000000000..bcc1e5c91fd
--- /dev/null
+++ b/src/main/target/BLUEBERRYH743/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 "fc/config.h"
+#include "io/serial.h"
+#include "io/piniobox.h"
+
+void targetConfiguration(void)
+{
+    pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1;
+    pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2;
+    beeperConfigMutable()->pwmMode = true;
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].functionMask = FUNCTION_MSP;
+    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].msp_baudrateIndex = BAUD_115200;
+}
diff --git a/src/main/target/BLUEBERRYH743/target.c b/src/main/target/BLUEBERRYH743/target.c
new file mode 100644
index 00000000000..bc2fb01558a
--- /dev/null
+++ b/src/main/target/BLUEBERRYH743/target.c
@@ -0,0 +1,60 @@
+/*
+ * 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_icm42688, DEVHW_ICM42605, MPU6000_SPI_BUS,   MPU6000_CS_PIN,   NONE,   0,  DEVFLAGS_NONE,  IMU_MPU6000_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_icm20602, DEVHW_MPU6500,  MPU6500_SPI_BUS,   MPU6500_CS_PIN,   NONE,   1,  DEVFLAGS_NONE,  IMU_MPU6500_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_icm42605, DEVHW_ICM42605, ICM42605_SPI_BUS,  ICM42605_CS_PIN,  NONE,  2,  DEVFLAGS_NONE,  IMU_ICM42605_ALIGN);
+
+timerHardware_t timerHardware[] = {
+    DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0),   // S1
+    DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 1),   // S2
+
+    DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 2),   // S3  
+    DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 3),   // S4
+    DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 4),   // S5
+    DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 5),   // S6
+
+    DEF_TIM(TIM4, CH1, PD12, TIM_USE_OUTPUT_AUTO, 0, 6),   // S7
+    DEF_TIM(TIM4, CH2, PD13, TIM_USE_OUTPUT_AUTO, 0, 7),   // S8
+    DEF_TIM(TIM4, CH3, PD14, TIM_USE_OUTPUT_AUTO, 0, 0),   // S9
+    DEF_TIM(TIM4, CH4, PD15, TIM_USE_OUTPUT_AUTO, 0, 0),   // S10 DMA_NONE
+
+    DEF_TIM(TIM15, CH1, PE5, TIM_USE_OUTPUT_AUTO, 0, 0),   // S11
+    DEF_TIM(TIM15, CH2, PE6, TIM_USE_OUTPUT_AUTO, 0, 0),   // S12 DMA_NONE
+
+    DEF_TIM(TIM1,  CH1, PA8,  TIM_USE_LED, 0, 9),    // LED_2812
+    DEF_TIM(TIM2,  CH1, PA15, TIM_USE_BEEPER, 0, 0),  // BEEPER PWM
+
+    // DEF_TIM(TIM8,  CH2, PC7,  TIM_USE_PPM, 0, 0),   // RX6 PPM
+    DEF_TIM(TIM8,  CH1, PC6,  TIM_USE_ANY, 0, 0),   // TX6 SoftwareSerial   
+    // DEF_TIM(TIM16, CH1, PB8,  TIM_USE_ANY, 0, 0),   // RX4
+    // DEF_TIM(TIM17, CH1, PB9,  TIM_USE_ANY, 0, 0),   // TX4
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
diff --git a/src/main/target/BLUEBERRYH743/target.h b/src/main/target/BLUEBERRYH743/target.h
new file mode 100644
index 00000000000..f11e822d633
--- /dev/null
+++ b/src/main/target/BLUEBERRYH743/target.h
@@ -0,0 +1,213 @@
+/*
+ * 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 "H743"
+
+#if defined(BLUEBERRYH743HD)
+  #define USBD_PRODUCT_STRING     "BLUEBERRYH743HD"
+#else
+  #define USBD_PRODUCT_STRING     "BLUEBERRYH743"
+#endif
+
+#define USE_TARGET_CONFIG
+
+#define LED0                    PE3
+#define LED1                    PE4
+
+#define BEEPER                  PA15
+#define BEEPER_INVERTED
+#define BEEPER_PWM_FREQUENCY    2500
+
+// *************** 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           PD7
+
+#define USE_IMU_MPU6000
+
+#define IMU_MPU6000_ALIGN       CW0_DEG_FLIP
+#define MPU6000_SPI_BUS          BUS_SPI1
+#define MPU6000_CS_PIN          PC15
+
+// *************** SPI4 IMU1  ICM20602 **************
+#define USE_SPI_DEVICE_4
+#define SPI4_SCK_PIN            PE12
+#define SPI4_MISO_PIN           PE13
+#define SPI4_MOSI_PIN           PE14
+
+#define USE_IMU_MPU6500
+
+#define IMU_MPU6500_ALIGN       CW0_DEG_FLIP
+#define MPU6500_SPI_BUS         BUS_SPI4
+#define MPU6500_CS_PIN          PE11
+
+// *************** SPI4 IMU2 ICM42605 **************
+#define USE_IMU_ICM42605
+
+#define IMU_ICM42605_ALIGN      CW90_DEG_FLIP
+#define ICM42605_SPI_BUS        BUS_SPI4
+#define ICM42605_CS_PIN         PC13
+
+// *************** SPI2 OSD ***********************
+  #define USE_SPI_DEVICE_2
+  #define SPI2_SCK_PIN            PB13
+  #define SPI2_MISO_PIN           PB14
+  #define SPI2_MOSI_PIN           PB15
+
+#if defined(BLUEBERRYH743)
+  #define USE_MAX7456
+  #define MAX7456_SPI_BUS         BUS_SPI2
+  #define MAX7456_CS_PIN          PB12
+#endif
+
+// *************** SPI3 SPARE for external RM3100 ***********
+#define USE_SPI_DEVICE_3
+#define SPI3_SCK_PIN            PB3
+#define SPI3_MISO_PIN           PB4
+#define SPI3_MOSI_PIN           PB5
+
+#define USE_MAG_RM3100
+#define RM3100_CS_PIN           PE2   //CS2 pad
+//                              PD4   //CS1 pad
+#define RM3100_SPI_BUS          BUS_SPI3
+
+// *************** 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_I2C2
+#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
+
+#define TEMPERATURE_I2C_BUS     BUS_I2C2
+#define PITOT_I2C_BUS           BUS_I2C2
+
+#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            PB9
+#define UART4_RX_PIN            PB8
+
+#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 USE_SOFTSERIAL1
+#define SOFTSERIAL_1_TX_PIN      PC6  //TX6 pad
+#define SOFTSERIAL_1_RX_PIN      PC6  //TX6 pad
+
+#define SERIAL_PORT_COUNT       9
+
+#define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER       SERIALRX_CRSF
+#define SERIALRX_UART           SERIAL_PORT_USART6
+
+// *************** SDIO SD BLACKBOX*******************
+#define USE_SDCARD
+#define USE_SDCARD_SDIO
+#define SDCARD_SDIO_DEVICE      SDIODEV_1
+#define SDCARD_SDIO_4BIT
+
+#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
+
+// *************** ADC *****************************
+#define USE_ADC
+#define ADC_INSTANCE                ADC1
+
+#define ADC_CHANNEL_1_PIN           PC0  //ADC123 VBAT1
+#define ADC_CHANNEL_2_PIN           PC1  //ADC123 CURR1
+#define ADC_CHANNEL_3_PIN           PC5  //ADC12  RSSI
+#define ADC_CHANNEL_4_PIN           PC4  //ADC12  AirS
+#define ADC_CHANNEL_5_PIN           PA4  //ADC12  VB2
+#define ADC_CHANNEL_6_PIN           PA7  //ADC12  CU2
+
+#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
+
+// *************** PINIO ***************************
+#define USE_PINIO
+#define USE_PINIOBOX
+#define PINIO1_PIN                  PD10  // VTX power switcher
+#define PINIO2_PIN                  PD11  // 2xCamera switcher
+
+// *************** 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 | FEATURE_BLACKBOX)
+#define CURRENT_METER_SCALE         250
+
+#define USE_SERIAL_4WAY_BLHELI_INTERFACE
+
+#define TARGET_IO_PORTA (0xffff & ~(BIT(14) | BIT(13)))
+#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        15
+#define USE_DSHOT
+#define USE_ESC_SENSOR
+

From cdb29763c2c2e8b8394d05a4a13bb2c623fab773 Mon Sep 17 00:00:00 2001
From: Ada <ada@signi.local>
Date: Wed, 11 Feb 2026 13:22:27 -0700
Subject: [PATCH 018/102] Add NEXUS target for RadioMaster Nexus (Original)
 flight controller

STM32F722-based target for the original (discontinued) Nexus,
distinct from the Nexus-X/XR (NEXUSX target). Pin mapping derived
from Rotorflight NEXUS_F7 dump and verified on hardware.

Key hardware support:
- ICM-42688-P IMU on SPI1 (CW90 alignment, EXTI PA15)
- SPL06 barometer on I2C1 (PB8/PB9, separate from external I2C2)
- W25N01G 128MB blackbox flash on SPI2
- 5 servo/motor outputs (S1-S4 + M1), expandable to 7
- UART4 with TX/RX swap for CRSF receiver on Port A
- UART1 on PA9/PA10 (not PB6/PB7 as on Nexus-X/XR)

Also adds USE_UART4_SWAP support in serial_uart_hal.c for the
STM32 HAL advanced UART pin swap feature.
---
 src/main/drivers/serial_uart_hal.c   |   7 ++
 src/main/target/NEXUS/CMakeLists.txt |   1 +
 src/main/target/NEXUS/README.md      | 144 ++++++++++++++++++++++
 src/main/target/NEXUS/config.c       |  19 +++
 src/main/target/NEXUS/target.c       |  29 +++++
 src/main/target/NEXUS/target.h       | 173 +++++++++++++++++++++++++++
 6 files changed, 373 insertions(+)
 create mode 100644 src/main/target/NEXUS/CMakeLists.txt
 create mode 100644 src/main/target/NEXUS/README.md
 create mode 100644 src/main/target/NEXUS/config.c
 create mode 100644 src/main/target/NEXUS/target.c
 create mode 100644 src/main/target/NEXUS/target.h

diff --git a/src/main/drivers/serial_uart_hal.c b/src/main/drivers/serial_uart_hal.c
index cce38422848..f25b2cbafb6 100644
--- a/src/main/drivers/serial_uart_hal.c
+++ b/src/main/drivers/serial_uart_hal.c
@@ -52,6 +52,13 @@ static void usartConfigurePinInversion(uartPort_t *uartPort) {
             uartPort->Handle.AdvancedInit.TxPinLevelInvert = UART_ADVFEATURE_TXINV_ENABLE;
         }
     }
+
+#ifdef USE_UART4_SWAP
+    if (uartPort->Handle.Instance == UART4) {
+        uartPort->Handle.AdvancedInit.AdvFeatureInit |= UART_ADVFEATURE_SWAP_INIT;
+        uartPort->Handle.AdvancedInit.Swap = UART_ADVFEATURE_SWAP_ENABLE;
+    }
+#endif
 }
 
 static void uartReconfigure(uartPort_t *uartPort)
diff --git a/src/main/target/NEXUS/CMakeLists.txt b/src/main/target/NEXUS/CMakeLists.txt
new file mode 100644
index 00000000000..a9afac49839
--- /dev/null
+++ b/src/main/target/NEXUS/CMakeLists.txt
@@ -0,0 +1 @@
+target_stm32f722xe(NEXUS)
diff --git a/src/main/target/NEXUS/README.md b/src/main/target/NEXUS/README.md
new file mode 100644
index 00000000000..a6f74074303
--- /dev/null
+++ b/src/main/target/NEXUS/README.md
@@ -0,0 +1,144 @@
+# RadioMaster Nexus (Original)
+
+Flight controller originally designed for helicopters using Rotorflight.
+Based on STM32F722RET6. This is the **original** (discontinued) Nexus,
+not the Nexus-X or Nexus-XR.
+
+For the Nexus-X and Nexus-XR, use the `NEXUSX` target instead.
+
+## Hardware
+
+| Component | Spec |
+|-----------|------|
+| MCU | STM32F722RET6 (216MHz, 256KB RAM, 512KB flash) |
+| IMU | ICM-42688-P (SPI1), CW90 alignment |
+| Barometer | SPL06-001 (I2C1, internal) |
+| Blackbox | W25N01G 128MB (SPI2) |
+| Input voltage | 5 - 12.6V |
+| BEC output | 5V / 2A on Port A, B, C |
+| DSM power | 3.3V / 0.5A |
+| Dimensions | 41.3 x 25.4 x 13.1mm |
+| Weight | 16.8g |
+
+## Differences from Nexus-X/XR
+
+| | OG Nexus | Nexus-X/XR |
+|---|----------|------------|
+| IMU EXTI | PA15 | PB8 |
+| IMU alignment | CW90 | CW180 |
+| Baro I2C | I2C1 (PB8/PB9) | I2C2 (PB10/PB11) |
+| Flash | W25N01G (128MB) | W25N02K (256MB) |
+| Internal ELRS RX | None | RP4TD-M on UART5 |
+| PINIO1 (RX power) | None | PC8 |
+| UART1 pins | PA9/PA10 | PB6/PB7 |
+| Voltage input | 5-12.6V | 3.6-70V |
+| Servo outputs | 5 | 7 default (9 max) |
+| Rotorflight target | NEXUS_F7 | NEXUSX |
+
+## Pin Functions
+
+### Default Output Assignment
+
+| Output | Pin | Timer | Connector |
+|--------|-----|-------|-----------|
+| S1 | PB4 | TIM3_CH1 | Servo header |
+| S2 | PB5 | TIM3_CH2 | Servo header |
+| S3 | PB0 | TIM3_CH3 | Servo header |
+| S4 | PB3 | TIM2_CH2 | Servo header (Tail) |
+| M1 | PB6 | TIM4_CH1 | ESC header |
+
+### UART Ports
+
+| UART | Label | TX | RX | Notes |
+|------|-------|----|----|-------|
+| UART1 | DSM | PA9 | PA10 | |
+| UART2 | SBUS | PA2 | PA3 | Shared with FREQ/PPM |
+| UART3 | Port C | PB11 | PB10 | Shared with I2C2 |
+| UART4 | Port A | PA1 | PA0 | Primary receiver (CRSF) |
+| UART6 | Port B | PC7 | PC6 | |
+
+### I2C Buses
+
+| Bus | SCL | SDA | Usage |
+|-----|-----|-----|-------|
+| I2C1 | PB8 | PB9 | Internal barometer (SPL06) |
+| I2C2 | PB10 | PB11 | External sensors via Port C |
+
+### ADC Channels
+
+| Channel | Pin | Divider | Usage |
+|---------|-----|---------|-------|
+| BUS | PC2 | 320 | External battery voltage (mapped as VBAT) |
+| BEC | PC1 | 160 | 5V BEC rail monitoring |
+
+### Connector Pinouts
+
+**Port A (UART4 - CRSF receiver):**
+1. GND
+2. 5V
+3. RX (PA0, connect to receiver TX)
+4. TX (PA1, connect to receiver RX / telemetry)
+
+**Port B (UART6):**
+1. GND
+2. 5V
+3. TX (PC7)
+4. RX (PC6)
+
+**Port C (UART3 / I2C2):**
+1. GND
+2. 5V
+3. SCL/TX (PB10)
+4. SDA/RX (PB11)
+
+**ESC Header:**
+- Signal: PB6 (TIM4_CH1 PWM)
+
+## Typical Glider Setup (Elevon / Flying Wing)
+
+For a 5-channel elevon glider like the Kunai:
+
+1. Flash NEXUS target via DFU
+2. Set aircraft type: **Flying Wing**
+3. Connect RP3-H receiver to **Port A** (CRSF on UART4)
+4. Connect ESC to **ESC header** (M1)
+5. Connect left elevon servo to **S1**
+6. Connect right elevon servo to **S2**
+7. Configure elevon mixing in the Mixer tab
+8. Vario: SPL06 baro provides altitude-based vario out of the box
+9. GPS (Phase 2): Connect to **Port B** (UART6)
+
+## Verified on Hardware
+
+- [x] MCU boots, LEDs active (PC14, PC15)
+- [x] USB CDC enumeration and iNAV CLI
+- [x] IMU (gyro + accel) detected and responding
+- [x] iNAV Configurator 9.0 connects, 3D model tracks board movement
+- [ ] Barometer (SPL06 on I2C1) — fixed bus assignment, needs retest
+- [ ] VBAT ADC (PC2) — fixed pin assignment, needs retest
+- [ ] BEC ADC (PC1) — needs test
+- [ ] Gyro alignment (CW90) — fixed, needs retest
+- [ ] All servo outputs — need test
+- [ ] CRSF receiver on UART4 — needs test
+- [ ] Blackbox logging — needs test
+- [ ] VBAT scale calibration with multimeter
+
+## Building
+
+Requires NixOS flake (included in repo root) or standard iNAV build deps.
+
+```sh
+nix develop --impure   # or set up arm-none-eabi-gcc manually
+mkdir -p build && cd build
+cmake -GNinja -DCOMPILER_VERSION_CHECK=OFF ..
+ninja NEXUS
+```
+
+## Flashing via DFU
+
+Hold button while connecting USB, then:
+
+```sh
+arm-none-eabi-objcopy -O binary build/bin/NEXUS.elf build/NEXUS.bin
+dfu-util -a 0 -s 0x08000000:leave -D build/NEXUS.bin
+```
diff --git a/src/main/target/NEXUS/config.c b/src/main/target/NEXUS/config.c
new file mode 100644
index 00000000000..ebbdf786f1c
--- /dev/null
+++ b/src/main/target/NEXUS/config.c
@@ -0,0 +1,19 @@
+/*
+ * RadioMaster Nexus (Original) - Target configuration defaults
+ *
+ * Unlike the Nexus-X/XR, the OG Nexus has no internal ELRS receiver,
+ * so there is no PINIO / USER1 box configuration needed here.
+ */
+
+#include <stdint.h>
+#include "platform.h"
+#include "drivers/pwm_mapping.h"
+
+void targetConfiguration(void)
+{
+    // Force M1/ESC (PB6/TIM4) to motor mode so the PWM auto-allocator
+    // picks it as the motor output first. Without this, it walks the
+    // timer table in order, converts TIM3 (S1/S2/S3) to motors via
+    // pwmClaimTimer(), and leaves no timers available for servos.
+    timerOverridesMutable(timer2id(TIM4))->outputMode = OUTPUT_MODE_MOTORS;
+}
diff --git a/src/main/target/NEXUS/target.c b/src/main/target/NEXUS/target.c
new file mode 100644
index 00000000000..a903264269e
--- /dev/null
+++ b/src/main/target/NEXUS/target.c
@@ -0,0 +1,29 @@
+/*
+ * RadioMaster Nexus (Original) - Timer/PWM hardware configuration
+ *
+ * Timer allocation (from Rotorflight NEXUS_F7 dump):
+ *   TIM3: S1 (PB4/CH1), S2 (PB5/CH2), S3 (PB0/CH3)
+ *   TIM2: S4 (PB3/CH2)
+ *   TIM4: M1 (PB6/CH1) - ESC output
+ *
+ * Note: PA2/PA3 are FREQ/PPM inputs in Rotorflight. In iNAV they
+ * can be repurposed as servo outputs when UART2 is not assigned.
+ */
+
+#include <stdint.h>
+#include "platform.h"
+#include "drivers/io.h"
+#include "drivers/timer.h"
+#include "drivers/pwm_mapping.h"
+
+timerHardware_t timerHardware[] = {
+    DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0),  // S1
+    DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0),  // S2
+    DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0),  // S3
+    DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0),  // S4
+    DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0),  // M1/ESC
+    DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0),  // shared w/ UART2 TX
+    DEF_TIM(TIM9, CH2, PA3, TIM_USE_OUTPUT_AUTO, 0, 0),  // shared w/ UART2 RX
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
diff --git a/src/main/target/NEXUS/target.h b/src/main/target/NEXUS/target.h
new file mode 100644
index 00000000000..8f0510e5981
--- /dev/null
+++ b/src/main/target/NEXUS/target.h
@@ -0,0 +1,173 @@
+/*
+ * RadioMaster Nexus (Original) - iNAV target
+ *
+ * Based on the NEXUSX target by functionpointer, adapted for the
+ * original (discontinued) Nexus hardware.
+ *
+ * Key differences from Nexus-X/XR:
+ *   - IMU EXTI on PA15 (X/XR uses PB8)
+ *   - IMU alignment CW90 (X/XR uses CW180)
+ *   - Flash is W25N01G 128MB (X/XR uses W25N02K 256MB)
+ *   - No internal ELRS receiver (X/XR has RP4TD-M on UART5)
+ *   - No PINIO1 receiver power gate
+ *   - Voltage input 5-12.6V (X/XR supports 3.6-70V)
+ *   - 5 servo/motor outputs vs 7-9 on X/XR
+ *   - Baro on I2C1 (PB8/PB9), not I2C2
+ *   - UART1 on PA9/PA10, not PB6/PB7
+ *
+ * Pin mapping derived from:
+ *   - Rotorflight NEXUS_F7 target (authoritative)
+ *   - groundflight project (joshperry/groundflight)
+ *   - RadioMaster documentation
+ */
+
+#pragma once
+
+#define TARGET_BOARD_IDENTIFIER "NEXS"
+#define USBD_PRODUCT_STRING     "RadioMaster Nexus"
+
+/* ---- LEDs ---- */
+#define LED0                    PC14  // active low
+#define LED1                    PC15  // active low
+
+/* ---- Beeper ---- */
+// No dedicated beeper pin on Nexus hardware
+
+/* ---- SPI ---- */
+#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
+
+/* ---- IMU: ICM-42688-P ---- */
+// iNAV uses ICM42605 driver which is register-compatible with ICM42688P
+#define USE_IMU_ICM42605
+#define IMU_ICM42605_ALIGN      CW90_DEG
+#define ICM42605_CS_PIN         PA4
+#define ICM42605_SPI_BUS        BUS_SPI1
+#define ICM42605_EXTI_PIN       PA15
+
+/* ---- I2C ---- */
+// I2C1: PB8/PB9 - internal, used for barometer
+#define USE_I2C
+#define USE_I2C_DEVICE_1
+#define I2C1_SCL                PB8
+#define I2C1_SDA                PB9
+
+// I2C2: PB10/PB11 - external via Port C connector (shared with UART3)
+// Available for external sensors (magnetometer, rangefinder, etc.)
+#define USE_I2C_DEVICE_2
+#define I2C2_SCL                PB10
+#define I2C2_SDA                PB11
+#define I2C_DEVICE_2_SHARES_UART3
+
+/* ---- Barometer: SPL06-001 ---- */
+// Confirmed on I2C1 per Rotorflight NEXUS_F7 dump
+#define USE_BARO
+#define USE_BARO_SPL06
+#define BARO_I2C_BUS            BUS_I2C1
+
+/* ---- Magnetometer (external, optional via Port C / I2C2) ---- */
+#define USE_MAG
+#define USE_MAG_ALL
+#define MAG_I2C_BUS             BUS_I2C2
+
+/* ---- Flash: W25N01G (128MB) ---- */
+#define USE_FLASHFS
+#define USE_FLASH_W25N01G
+#define W25N01G_SPI_BUS         BUS_SPI2
+#define W25N01G_CS_PIN          PB12
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+
+/* ---- UARTs ---- */
+// OG Nexus UART layout (confirmed from Rotorflight NEXUS_F7 dump):
+//   UART1 [DSM port]  : PA9 (TX) / PA10 (RX)
+//   UART2 [SBUS/FREQ] : PA2 (TX) / PA3 (RX) - shared with RPM/TLM pins
+//   UART3 [Port C]    : PB11 (TX) / PB10 (RX) - shared with I2C2
+//   UART4 [Port A]    : PA1 (TX) / PA0 (RX) - primary CRSF receiver
+//   UART6 [Port B]    : PC7 (TX) / PC6 (RX)
+//
+// NOTE: No UART5 on OG Nexus (X/XR uses UART5 for internal ELRS)
+
+#define USE_VCP
+#define USE_USB_DETECT
+#define USB_DETECT_PIN          NONE
+
+#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            PB11
+#define UART3_RX_PIN            PB10
+
+#define USE_UART4
+#define USE_UART4_SWAP
+#define UART4_TX_PIN            PA1
+#define UART4_RX_PIN            PA0
+
+#define USE_UART6
+#define UART6_TX_PIN            PC7
+#define UART6_RX_PIN            PC6
+
+#define SERIAL_PORT_COUNT       6  // VCP + UART1-4 + UART6
+
+/* ---- Default serial receiver ---- */
+#define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER       SERIALRX_CRSF
+#define SERIALRX_UART           SERIAL_PORT_USART4
+
+
+/* ---- ADC ---- */
+// From Rotorflight dump:
+//   ADC_BEC = PC1, divider 160 (BEC 5V rail monitoring)
+//   ADC_BUS = PC2, divider 320 (external battery voltage)
+// Map BUS voltage as VBAT for iNAV since that's the flight battery
+#define USE_ADC
+#define ADC_INSTANCE            ADC1
+#define ADC_CHANNEL_1_PIN       PC2   // BUS voltage (external battery)
+#define VBAT_ADC_CHANNEL        ADC_CHN_1
+#define ADC_CHANNEL_2_PIN       PC1   // BEC voltage (5V rail)
+// VBAT scale: Rotorflight vbus_divider = 320
+// iNAV scale = divider * ~3.44 (ADC ref / resolution factor)
+// Start with 1100, calibrate with multimeter
+#define VBAT_SCALE_DEFAULT      1100
+
+/* ---- Sensors ---- */
+#define SENSORS_SET             (SENSOR_ACC | SENSOR_BARO)
+
+/* ---- PWM / Servo / Motor outputs ---- */
+// OG Nexus outputs (from Rotorflight dump):
+//   S1:   PB4  (TIM3_CH1)  - Servo header
+//   S2:   PB5  (TIM3_CH2)  - Servo header
+//   S3:   PB0  (TIM3_CH3)  - Servo header
+//   S4:   PB3  (TIM2_CH2)  - Servo header (Tail)
+//   M1:   PB6  (TIM4_CH1)  - ESC header (motor only, NOT UART1)
+//
+// Pin multiplexing when UARTs freed:
+//   PA2 (TIM5_CH3) - shared with UART2 TX / FREQ input
+//   PA3 (TIM9_CH2) - shared with UART2 RX / PPM input
+
+#define MAX_PWM_OUTPUT_PORTS    7
+
+/* ---- No internal receiver ---- */
+// OG Nexus has no internal ELRS receiver.
+// No UART5, no PINIO1 power gate.
+
+/* ---- Platform ---- */
+#define USE_SERIAL_4WAY_BLHELI_INTERFACE
+#define TARGET_IO_PORTA         0xffff
+#define TARGET_IO_PORTB         0xffff
+#define TARGET_IO_PORTC         0xffff
+
+#define DEFAULT_FEATURES        (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX)

From 66c87bf6be663de02242b2c60a3b8612280ef597 Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Wed, 18 Feb 2026 13:57:39 -0600
Subject: [PATCH 019/102] imu: compute GPS 3D speed only on new GPS data

GPS3DspeedFiltered was computed every PID cycle (1 kHz) from
gpsSol.velNED[], which only changes when the GPS task delivers a new
fix (~10 Hz). Move the sqrt and pt1 filter update into
imuCalculateTurnRateacceleration(), gate them on gpsHeartbeat change,
and make the filter state a static local. The variable is only
consumed in that function (fixed-wing + GPS-trustworthy path), so the
module-level FASTRAM variables and the imuInit() reset are also removed.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
---
 src/main/flight/imu.c | 15 ++++++++-------
 1 file changed, 8 insertions(+), 7 deletions(-)

diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index b012c040d27..d4ac5cf1a85 100644
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -113,8 +113,6 @@ STATIC_FASTRAM pt1Filter_t HeadVecEFFilterY;
 STATIC_FASTRAM pt1Filter_t HeadVecEFFilterZ;
 FASTRAM fpVector3_t HeadVecEFFiltered = {.v = {0.0f, 0.0f, 0.0f}};
 
-STATIC_FASTRAM float GPS3DspeedFiltered=0.0f;
-STATIC_FASTRAM pt1Filter_t GPS3DspeedFilter;
 
 FASTRAM bool gpsHeadingInitialized;
 
@@ -210,8 +208,6 @@ void imuInit(void)
     pt1FilterReset(&HeadVecEFFilterX, 0);
     pt1FilterReset(&HeadVecEFFilterY, 0);
     pt1FilterReset(&HeadVecEFFilterZ, 0);
-    // Initialize 3d speed filter
-    pt1FilterReset(&GPS3DspeedFilter, 0);
 }
 
 void imuSetMagneticDeclination(float declinationDeg)
@@ -667,9 +663,6 @@ static void imuCalculateFilters(float dT)
     HeadVecEFFiltered.y = pt1FilterApply4(&HeadVecEFFilterY, rMat[1][0], IMU_ROTATION_LPF, dT);
     HeadVecEFFiltered.z = pt1FilterApply4(&HeadVecEFFilterZ, rMat[2][0], IMU_ROTATION_LPF, dT);
 
-    //anti aliasing
-    float GPS3Dspeed = calc_length_pythagorean_3D(gpsSol.velNED[X],gpsSol.velNED[Y],gpsSol.velNED[Z]);
-    GPS3DspeedFiltered = pt1FilterApply4(&GPS3DspeedFilter, GPS3Dspeed, IMU_ROTATION_LPF, dT);
 }
 
 static void imuCalculateGPSacceleration(fpVector3_t *vEstAccelEF,fpVector3_t *vEstcentrifugalAccelBF, float *acc_ignore_slope_multipiler)
@@ -705,6 +698,14 @@ static void imuCalculateTurnRateacceleration(fpVector3_t *vEstcentrifugalAccelBF
     float currentspeed = 0;
     if (isGPSTrustworthy()){
         //first speed choice is gps
+        static bool lastGPSHeartbeat;
+        static pt1Filter_t GPS3DspeedFilter;
+        static float GPS3DspeedFiltered = 0.0f;
+        if (gpsSol.flags.gpsHeartbeat != lastGPSHeartbeat) {
+            lastGPSHeartbeat = gpsSol.flags.gpsHeartbeat;
+            float GPS3Dspeed = calc_length_pythagorean_3D(gpsSol.velNED[X], gpsSol.velNED[Y], gpsSol.velNED[Z]);
+            GPS3DspeedFiltered = pt1FilterApply4(&GPS3DspeedFilter, GPS3Dspeed, IMU_ROTATION_LPF, dT);
+        }
         currentspeed = GPS3DspeedFiltered;
         *acc_ignore_slope_multipiler = 4.0f;
     }

From 31a5b38825e36f2c1ccf6c2876a16c3d78243266 Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Wed, 18 Feb 2026 14:58:57 -0600
Subject: [PATCH 020/102] pid/filter: add FAST_CODE to pidController callees

pidController is FAST_CODE. Its callees that lacked FAST_CODE created
SRAM veneers (trampolines from ITCM to flash) on every call. Annotate
each callee that was audited as genuinely hot:

  pid.c:
    pTermProcess, dTermProcess, applyItermLimiting,
    pidApplySetpointRateLimiting, checkItermLimitingActive,
    checkItermFreezingActive, pidRcCommandToAngle, pidRcCommandToRate

  smith_predictor.c: applySmithPredictor

  filter.c: pt1ComputeRC (static; called from FAST_CODE pt1FilterApply4)

  maths.c: constrain, constrainf, scaleRangef
    (called from the pid.c functions above; candidates for static inline
    in a future refactor, but FAST_CODE resolves the veneer for now)

  fc_core.c: getAxisRcCommand

Functions removed from FAST_CODE where no FAST_CODE caller existed
(sin_approx, cos_approx, fast_fsqrtf, failsafeShouldApplyControlInput)
were never added in this tree; no net change for those.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
---
 src/main/common/filter.c          |  2 +-
 src/main/common/maths.c           |  6 +++---
 src/main/fc/fc_core.c             |  2 +-
 src/main/flight/pid.c             | 16 ++++++++--------
 src/main/flight/smith_predictor.c |  2 +-
 5 files changed, 14 insertions(+), 14 deletions(-)

diff --git a/src/main/common/filter.c b/src/main/common/filter.c
index 5fd72965d85..2e9e4574e71 100644
--- a/src/main/common/filter.c
+++ b/src/main/common/filter.c
@@ -44,7 +44,7 @@ float nullFilterApply4(void *filter, float input, float f_cut, float dt)
 
 // PT1 Low Pass filter
 
-static float pt1ComputeRC(const float f_cut)
+static float FAST_CODE pt1ComputeRC(const float f_cut)
 {
     return 1.0f / (2.0f * M_PIf * f_cut);
 }
diff --git a/src/main/common/maths.c b/src/main/common/maths.c
index 993634d902d..3dfa69cf4f5 100644
--- a/src/main/common/maths.c
+++ b/src/main/common/maths.c
@@ -165,7 +165,7 @@ int32_t applyDeadbandRescaled(int32_t value, int32_t deadband, int32_t min, int3
     return value;
 }
 
-int32_t constrain(int32_t amt, int32_t low, int32_t high)
+int32_t FAST_CODE constrain(int32_t amt, int32_t low, int32_t high)
 {
     if (amt < low)
         return low;
@@ -175,7 +175,7 @@ int32_t constrain(int32_t amt, int32_t low, int32_t high)
         return amt;
 }
 
-float constrainf(float amt, float low, float high)
+float FAST_CODE constrainf(float amt, float low, float high)
 {
     if (amt < low)
         return low;
@@ -225,7 +225,7 @@ int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax) {
     return ((a / b) + destMin);
 }
 
-float scaleRangef(float x, float srcMin, float srcMax, float destMin, float destMax) {
+float FAST_CODE scaleRangef(float x, float srcMin, float srcMax, float destMin, float destMax) {
     float a = (destMax - destMin) * (x - srcMin);
     float b = srcMax - srcMin;
     return ((a / b) + destMin);
diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index 11bbd6cf3cc..2aa806406db 100644
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -177,7 +177,7 @@ bool areSensorsCalibrating(void)
     return false;
 }
 
-int16_t getAxisRcCommand(int16_t rawData, int16_t rate, int16_t deadband)
+int16_t FAST_CODE getAxisRcCommand(int16_t rawData, int16_t rate, int16_t deadband)
 {
     int16_t stickDeflection = 0;
 
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 0a2faa648ca..5d7ab228cfd 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -413,7 +413,7 @@ float getAxisIterm(uint8_t axis)
     return pidState[axis].errorGyroIf;
 }
 
-static float pidRcCommandToAngle(int16_t stick, int16_t maxInclination)
+static float FAST_CODE pidRcCommandToAngle(int16_t stick, int16_t maxInclination)
 {
     stick = constrain(stick, -500, 500);
     return scaleRangef((float) stick, -500.0f, 500.0f, (float) -maxInclination, (float) maxInclination);
@@ -436,7 +436,7 @@ float pidRateToRcCommand(float rateDPS, uint8_t rate)
     return scaleRangef(rateDPS, -maxRateDPS, maxRateDPS, -500.0f, 500.0f);
 }
 
-float pidRcCommandToRate(int16_t stick, uint8_t rate)
+float FAST_CODE pidRcCommandToRate(int16_t stick, uint8_t rate)
 {
     const float maxRateDPS = rate * 10.0f;
     return scaleRangef((float) stick, -500.0f, 500.0f, -maxRateDPS, maxRateDPS);
@@ -725,7 +725,7 @@ static void pidLevel(const float angleTarget, pidState_t *pidState, flight_dynam
 }
 
 /* Apply angular acceleration limit to rate target to limit extreme stick inputs to respect physical capabilities of the machine */
-static void pidApplySetpointRateLimiting(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
+static void FAST_CODE pidApplySetpointRateLimiting(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
 {
     const uint32_t axisAccelLimit = (axis == FD_YAW) ? pidProfile()->axisAccelerationLimitYaw : pidProfile()->axisAccelerationLimitRollPitch;
 
@@ -734,7 +734,7 @@ static void pidApplySetpointRateLimiting(pidState_t *pidState, flight_dynamics_i
     }
 }
 
-static float pTermProcess(pidState_t *pidState, float rateError, float dT) {
+static float FAST_CODE pTermProcess(pidState_t *pidState, float rateError, float dT) {
     float newPTerm = rateError * pidState->kP;
 
     return pidState->ptermFilterApplyFn(&pidState->ptermLpfState, newPTerm, yawLpfHz, dT);
@@ -770,7 +770,7 @@ static float applyDBoost(pidState_t *pidState, float dT) {
 }
 #endif
 
-static float dTermProcess(pidState_t *pidState, float currentRateTarget, float dT, float dT_inv) {
+static float FAST_CODE dTermProcess(pidState_t *pidState, float currentRateTarget, float dT, float dT_inv) {
     // Calculate new D-term
     float newDTerm = 0;
     if (pidState->kD == 0) {
@@ -787,7 +787,7 @@ static float dTermProcess(pidState_t *pidState, float currentRateTarget, float d
     return(newDTerm);
 }
 
-static void applyItermLimiting(pidState_t *pidState) {
+static void FAST_CODE applyItermLimiting(pidState_t *pidState) {
     if (pidState->itermLimitActive) {
         pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit);
     } else
@@ -1136,7 +1136,7 @@ static void pidApplyFpvCameraAngleMix(pidState_t *pidState, uint8_t fpvCameraAng
     pidState[YAW].rateTarget = constrainf(yawRate * cosCameraAngle + rollRate * sinCameraAngle, -GYRO_SATURATION_LIMIT, GYRO_SATURATION_LIMIT);
 }
 
-void checkItermLimitingActive(pidState_t *pidState)
+void FAST_CODE checkItermLimitingActive(pidState_t *pidState)
 {
     bool shouldActivate = false;
 
@@ -1147,7 +1147,7 @@ void checkItermLimitingActive(pidState_t *pidState)
     pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate;
 }
 
-void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis)
+void FAST_CODE 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
diff --git a/src/main/flight/smith_predictor.c b/src/main/flight/smith_predictor.c
index 6a363c4f231..78151215ff5 100644
--- a/src/main/flight/smith_predictor.c
+++ b/src/main/flight/smith_predictor.c
@@ -34,7 +34,7 @@
 #include "flight/smith_predictor.h"
 #include "build/debug.h"
 
-float applySmithPredictor(uint8_t axis, smithPredictor_t *predictor, float sample) {
+float FAST_CODE applySmithPredictor(uint8_t axis, smithPredictor_t *predictor, float sample) {
     UNUSED(axis);
     if (predictor->enabled) {
         predictor->data[predictor->idx] = sample;

From 81d596af5d7d5fd306a0a0be2ecc5da260bb2b01 Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Wed, 18 Feb 2026 15:04:14 -0600
Subject: [PATCH 021/102] fc/pid: avoid recomputing values that change at lower
 rates
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Two independent optimisations for the main PID loop:

rcCommand caching (fc_core.c)
  getAxisRcCommand() and failsafeUpdateRcCommandValues() are now gated
  on isRXDataNew so they only run when the RX task delivers a fresh
  frame (~50 Hz) instead of every PID cycle (1 kHz / multirotor rate).
  rcCommand[] is updated from a small cache on every cycle so the rest
  of the code is unaffected.

pidSumLimit precomputation (pid.c)
  getPidSumLimit() returns 400 or 500 based on vehicle type and axis —
  values that are constant for the lifetime of a flight. Add a
  pidSumLimit field to pidState_t, initialise it once in pidInit(), and
  replace the two hot-path calls in pidApplyFixedWingRateController and
  pidApplyMulticopterRateController with a struct field read.
  getPidSumLimit() is retained for pid_autotune.c (not hot).

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
---
 src/main/fc/fc_core.c | 29 +++++++++++++++++++++++------
 src/main/flight/pid.c |  6 ++++--
 2 files changed, 27 insertions(+), 8 deletions(-)

diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index 2aa806406db..3223aca497e 100644
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -390,10 +390,25 @@ static void processPilotAndFailSafeActions(float dT)
         failsafeApplyControlInput();
     }
     else {
-        // Compute ROLL PITCH and YAW command
-        rcCommand[ROLL] = getAxisRcCommand(rxGetChannelValue(ROLL), FLIGHT_MODE(MANUAL_MODE) ? currentControlProfile->manual.rcExpo8 : currentControlProfile->stabilized.rcExpo8, rcControlsConfig()->deadband);
-        rcCommand[PITCH] = getAxisRcCommand(rxGetChannelValue(PITCH), FLIGHT_MODE(MANUAL_MODE) ? currentControlProfile->manual.rcExpo8 : currentControlProfile->stabilized.rcExpo8, rcControlsConfig()->deadband);
-        rcCommand[YAW] = -getAxisRcCommand(rxGetChannelValue(YAW), FLIGHT_MODE(MANUAL_MODE) ? currentControlProfile->manual.rcYawExpo8 : currentControlProfile->stabilized.rcYawExpo8, rcControlsConfig()->yaw_deadband);
+        // Compute ROLL PITCH and YAW command.
+        // Only recompute when the RX task has delivered new data (~50 Hz).
+        {
+            static int16_t cachedCmd[3] = {0, 0, 0};
+            if (isRXDataNew) {
+                cachedCmd[ROLL]  = getAxisRcCommand(rxGetChannelValue(ROLL),
+                    FLIGHT_MODE(MANUAL_MODE) ? currentControlProfile->manual.rcExpo8 : currentControlProfile->stabilized.rcExpo8,
+                    rcControlsConfig()->deadband);
+                cachedCmd[PITCH] = getAxisRcCommand(rxGetChannelValue(PITCH),
+                    FLIGHT_MODE(MANUAL_MODE) ? currentControlProfile->manual.rcExpo8 : currentControlProfile->stabilized.rcExpo8,
+                    rcControlsConfig()->deadband);
+                cachedCmd[YAW]   = -getAxisRcCommand(rxGetChannelValue(YAW),
+                    FLIGHT_MODE(MANUAL_MODE) ? currentControlProfile->manual.rcYawExpo8 : currentControlProfile->stabilized.rcYawExpo8,
+                    rcControlsConfig()->yaw_deadband);
+            }
+            rcCommand[ROLL]  = cachedCmd[ROLL];
+            rcCommand[PITCH] = cachedCmd[PITCH];
+            rcCommand[YAW]   = cachedCmd[YAW];
+        }
 
         // Apply manual control rates
         if (FLIGHT_MODE(MANUAL_MODE)) {
@@ -418,8 +433,10 @@ static void processPilotAndFailSafeActions(float dT)
         //Compute THROTTLE command
         rcCommand[THROTTLE] = throttleStickMixedValue();
 
-        // Signal updated rcCommand values to Failsafe system
-        failsafeUpdateRcCommandValues();
+        // Signal updated rcCommand values to Failsafe system when new RC data arrived
+        if (isRXDataNew) {
+            failsafeUpdateRcCommandValues();
+        }
 
         if (FLIGHT_MODE(HEADFREE_MODE)) {
             const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold);
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 5d7ab228cfd..354ffeff371 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -117,6 +117,7 @@ typedef struct {
     smithPredictor_t smithPredictor;
 
     fwPidAttenuation_t attenuation;
+    uint16_t pidSumLimit;
 } pidState_t;
 
 STATIC_FASTRAM bool pidFiltersConfigured = false;
@@ -861,7 +862,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, float
 
     applyItermLimiting(pidState);
 
-    const uint16_t limit = getPidSumLimit(pidState->axis);
+    const uint16_t limit = pidState->pidSumLimit;
 
     if (pidProfile()->pidItermLimitPercent != 0){
         float itermLimit = limit * pidProfile()->pidItermLimitPercent * 0.01f;
@@ -927,7 +928,7 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid
      */
     const float newCDTerm = rateTargetDeltaFiltered * pidState->kCD;
 
-    const uint16_t limit = getPidSumLimit(pidState->axis);
+    const uint16_t limit = pidState->pidSumLimit;
 
     // TODO: Get feedback from mixer on available correction range for each axis
     const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf + newCDTerm;
@@ -1376,6 +1377,7 @@ void pidInit(void)
     #endif
 
         pidState[axis].axis = axis;
+        pidState[axis].pidSumLimit = getPidSumLimit(axis);
         if (axis == FD_YAW) {
             if (yawLpfHz) {
                 pidState[axis].ptermFilterApplyFn = (filterApply4FnPtr) pt1FilterApply4;

From 2ce3f0ae00e41a55a827938dfb7cc1927d44fc9f Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Wed, 18 Feb 2026 17:35:33 -0600
Subject: [PATCH 022/102] imu: optimize imuMahonyAHRSupdate() hot path (5
 micro-opts)
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Five cycle-saving changes to imuMahonyAHRSupdate(), which accounts for
~205 µs (30%) of the PID loop on RP2350.  Each change is safe on all
targets; the gains are proportionally smaller on F7/H7 where the function
already lives in ITCM RAM.

1. Replace quaternionRotateVector({0,0,1}) with rMat[2][*] reads.
   imuComputeRotationMatrix() is called at the end of every invocation and
   keeps rMat in sync with orientation.  Rotating the constant gravity
   vector EF→BF yields exactly the third row of rMat, so three float loads
   replace ~56 floating-point multiply/add operations.

2. Eliminate sqrt() from the Taylor-series threshold.
   Original: thetaMagnitudeSq < sqrt(24e-6).
   Squaring both sides (both non-negative) gives the equivalent condition
   thetaMagnitudeSq² < 24e-6 with no sqrt call.

3. First-order Newton quaternion renormalization replaces quaternionNormalize()
   (sqrt + 4 divides, ~35 cycles) with scale = (3 - normSq) * 0.5 (~14 cycles).
   At 1 kHz the per-step drift |ε| < 1e-6, making the O(ε²) error < 1e-12.
   imuCheckAndResetOrientationQuaternion() remains as the catastrophic-failure
   safety net.

4. Precompute the anti-windup i_limit in imuConfigure() (called only on
   settings save).  The hot path now reads a single float instead of
   performing an add, a multiply, and a divide every PID cycle.
   Adds dcm_i_limit to imuRuntimeConfig_t and imuConfigure().

5. Reduce prevOrientation snapshot frequency from every PID cycle to
   every 100 cycles (~100 ms at 1 kHz).  The snapshot is only used by
   the fault-recovery path which should never fire in normal flight;
   16 bytes of unnecessary SRAM write every ms is not justified.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
---
 src/main/flight/imu.c | 56 +++++++++++++++++++++++++++++++++++++------
 src/main/flight/imu.h |  5 ++++
 2 files changed, 54 insertions(+), 7 deletions(-)

diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index b012c040d27..2c129ac27d0 100644
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -171,6 +171,12 @@ void imuConfigure(void)
     imuRuntimeConfig.dcm_kp_mag = imuConfig()->dcm_kp_mag / 10000.0f;
     imuRuntimeConfig.dcm_ki_mag = imuConfig()->dcm_ki_mag / 10000.0f;
     imuRuntimeConfig.small_angle = imuConfig()->small_angle;
+    /* Precompute the Mahony anti-windup clamp.  The PID loop reads this value
+     * 1000× per second; the kP gains only change when the user saves settings,
+     * so computing it here (called once per save) avoids one add, one multiply,
+     * and one divide on every PID cycle. */
+    imuRuntimeConfig.dcm_i_limit = DEGREES_TO_RADIANS(2.0f)
+        * (imuRuntimeConfig.dcm_kp_acc + imuRuntimeConfig.dcm_kp_mag) * 0.5f;
 }
 
 void imuInit(void)
@@ -368,7 +374,19 @@ static float imuCalculateMcCogAccWeight(void)
 static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVector3_t * accBF, const fpVector3_t * magBF, const fpVector3_t * vCOG, const fpVector3_t * vCOGAcc, float accWScaler, float magWScaler)
 {
     STATIC_FASTRAM fpVector3_t vGyroDriftEstimate = { 0 };
-    fpQuaternion_t prevOrientation = orientation;
+
+    /* Opt 5: snapshot prevOrientation every 100 PID cycles instead of every cycle.
+     * The snapshot is only used by the fault-recovery path in
+     * imuCheckAndResetOrientationQuaternion(), which should never fire in normal
+     * flight.  Copying 4 floats 1000×/s just to support a near-zero-probability
+     * reset path is wasteful; 100 ms staleness is a safe recovery point. */
+    static uint8_t prevOrientationSnapshotCount = 0;
+    static fpQuaternion_t prevOrientation = { .q0 = 1.0f };  // identity quaternion safe default
+    if (++prevOrientationSnapshotCount >= 100) {
+        prevOrientationSnapshotCount = 0;
+        prevOrientation = orientation;
+    }
+
     fpVector3_t vRotation = *gyroBF;
 
     /* Calculate general spin rate (rad/s) */
@@ -501,11 +519,17 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
 
     /* Step 2: Roll and pitch correction -  use measured acceleration vector */
     if (accBF) {
-        static const fpVector3_t vGravity = { .v = { 0.0f, 0.0f, 1.0f } };
         fpVector3_t vEstGravity, vAcc, vErr;
 
-        // Calculate estimated gravity vector in body frame
-        quaternionRotateVector(&vEstGravity, &vGravity, &orientation);    // EF -> BF
+        /* Opt 1: imuComputeRotationMatrix() is called at the END of every
+         * imuMahonyAHRSupdate() and keeps rMat in sync with orientation.
+         * Rotating the constant unit-gravity vector {0,0,1} from EF to BF by
+         * the current orientation quaternion yields exactly the third row of rMat
+         * (rMat[2][0..2]).  Reading those three floats replaces a quaternionRotateVector()
+         * call that costs 2× quaternionMultiply = ~32 multiplies + 24 adds. */
+        vEstGravity.x = rMat[2][0];
+        vEstGravity.y = rMat[2][1];
+        vEstGravity.z = rMat[2][2];
 
         // Error is sum of cross product between estimated direction and measured direction of gravity
         vectorNormalize(&vAcc, accBF);
@@ -528,7 +552,9 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
         vectorAdd(&vRotation, &vRotation, &vErr);
     }
     // Anti wind-up
-    float i_limit = DEGREES_TO_RADIANS(2.0f) * (imuRuntimeConfig.dcm_kp_acc + imuRuntimeConfig.dcm_kp_mag) / 2.0f;
+    /* Opt 4: dcm_i_limit is computed once in imuConfigure() (called on settings save),
+     * not recomputed each PID cycle.  The kP gains do not change at runtime. */
+    const float i_limit = imuRuntimeConfig.dcm_i_limit;
     vGyroDriftEstimate.x = constrainf(vGyroDriftEstimate.x, -i_limit, i_limit);
     vGyroDriftEstimate.y = constrainf(vGyroDriftEstimate.y, -i_limit, i_limit);
     vGyroDriftEstimate.z = constrainf(vGyroDriftEstimate.z, -i_limit, i_limit);
@@ -551,7 +577,10 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
         // Proper quaternion from axis/angle involves computing sin/cos, but the formula becomes numerically unstable as Theta approaches zero.
         // For near-zero cases we use the first 3 terms of the Taylor series expansion for sin/cos. We check if fourth term is less than machine precision -
         // then we can safely use the "low angle" approximated version without loss of accuracy.
-        if (thetaMagnitudeSq < fast_fsqrtf(24.0f * 1e-6f)) {
+        /* Opt 2: original condition was "thetaMagnitudeSq < sqrt(24e-6)".
+         * Squaring both sides (both are non-negative) gives an equivalent
+         * condition without a sqrt() call: thetaMagnitudeSq² < 24e-6. */
+        if (thetaMagnitudeSq * thetaMagnitudeSq < 24.0e-6f) {
             quaternionScale(&deltaQ, &deltaQ, 1.0f - thetaMagnitudeSq / 6.0f);
             deltaQ.q0 = 1.0f - thetaMagnitudeSq / 2.0f;
         }
@@ -563,7 +592,20 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
 
         // Calculate final orientation and renormalize
         quaternionMultiply(&orientation, &orientation, &deltaQ);
-        quaternionNormalize(&orientation, &orientation);
+        /* Opt 3: first-order Newton renormalization avoids sqrt() and 4 divides.
+         * At 1 kHz the quaternion norm drifts by < 1e-6 per step, so normSq = 1 + ε
+         * with |ε| ≪ 1.  The identity 1/sqrt(x) ≈ (3-x)/2 is accurate to O(ε²) ≈ 1e-12
+         * — well within float precision.  imuCheckAndResetOrientationQuaternion() below
+         * catches any catastrophic norm deviation that this approximation cannot correct. */
+        {
+            const float normSq = orientation.q0 * orientation.q0 + orientation.q1 * orientation.q1
+                               + orientation.q2 * orientation.q2 + orientation.q3 * orientation.q3;
+            const float scale = (3.0f - normSq) * 0.5f;
+            orientation.q0 *= scale;
+            orientation.q1 *= scale;
+            orientation.q2 *= scale;
+            orientation.q3 *= scale;
+        }
     }
 
     // Check for invalid quaternion and reset to previous known good one
diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h
index 6d285d0f794..14244c9cefc 100644
--- a/src/main/flight/imu.h
+++ b/src/main/flight/imu.h
@@ -65,6 +65,11 @@ typedef struct imuRuntimeConfig_s {
     float dcm_ki_acc;
     float dcm_kp_mag;
     float dcm_ki_mag;
+    /* Precomputed anti-windup limit for imuMahonyAHRSupdate(): equals
+     * DEGREES_TO_RADIANS(2) * (dcm_kp_acc + dcm_kp_mag) / 2.
+     * Updated once by imuConfigure() whenever settings are saved, so the
+     * hot PID path reads a single float instead of doing arithmetic. */
+    float dcm_i_limit;
     uint8_t small_angle;
 } imuRuntimeConfig_t;
 

From 2b8d57141d01c152985145e3a740fcd07b03da19 Mon Sep 17 00:00:00 2001
From: Ada <ada@6bit.com>
Date: Tue, 24 Feb 2026 09:56:03 -0700
Subject: [PATCH 023/102] NEXUS: fix VBAT scale, Port C pinout, and ADC docs
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Address hardware testing feedback from daijoubu:
- VBAT_SCALE_DEFAULT 1100 → 320 (hardware-verified)
- Port C connector pinout: pin 3 is SDA/TX (PB11), pin 4 is SCL/RX (PB10)
- Clarify OG Nexus has no EXT-V input (Vin ADC only, unlike X/XR)
- Update verification checklist with confirmed test results

Co-Authored-By: Joshua Perry <josh@6bit.com>
---
 src/main/target/NEXUS/README.md | 31 ++++++++++++++++++-------------
 src/main/target/NEXUS/target.h  | 19 +++++++++----------
 2 files changed, 27 insertions(+), 23 deletions(-)

diff --git a/src/main/target/NEXUS/README.md b/src/main/target/NEXUS/README.md
index a6f74074303..0edf84a7ac3 100644
--- a/src/main/target/NEXUS/README.md
+++ b/src/main/target/NEXUS/README.md
@@ -31,7 +31,7 @@ For the Nexus-X and Nexus-XR, use the `NEXUSX` target instead.
 | Internal ELRS RX | None | RP4TD-M on UART5 |
 | PINIO1 (RX power) | None | PC8 |
 | UART1 pins | PA9/PA10 | PB6/PB7 |
-| Voltage input | 5-12.6V | 3.6-70V |
+| Voltage sense | Vin ADC only (5-12.6V) | EXT-V input (3.6-70V) |
 | Servo outputs | 5 | 7 default (9 max) |
 | Rotorflight target | NEXUS_F7 | NEXUSX |
 
@@ -68,9 +68,12 @@ For the Nexus-X and Nexus-XR, use the `NEXUSX` target instead.
 
 | Channel | Pin | Divider | Usage |
 |---------|-----|---------|-------|
-| BUS | PC2 | 320 | External battery voltage (mapped as VBAT) |
+| BUS | PC2 | 320 | Vin rail, 5-12.6V (mapped as VBAT) |
 | BEC | PC1 | 160 | 5V BEC rail monitoring |
 
+Note: Unlike the Nexus-X/XR, the OG Nexus has no dedicated EXT-V
+high-voltage battery sense input. VBAT monitors the board input power.
+
 ### Connector Pinouts
 
 **Port A (UART4 - CRSF receiver):**
@@ -88,8 +91,8 @@ For the Nexus-X and Nexus-XR, use the `NEXUSX` target instead.
 **Port C (UART3 / I2C2):**
 1. GND
 2. 5V
-3. SCL/TX (PB10)
-4. SDA/RX (PB11)
+3. SDA/TX (PB11)
+4. SCL/RX (PB10)
 
 **ESC Header:**
 - Signal: PB6 (TIM4_CH1 PWM)
@@ -113,15 +116,17 @@ For a 5-channel elevon glider like the Kunai:
 - [x] MCU boots, LEDs active (PC14, PC15)
 - [x] USB CDC enumeration and iNAV CLI
 - [x] IMU (gyro + accel) detected and responding
-- [x] iNAV Configurator 9.0 connects, 3D model tracks board movement
-- [ ] Barometer (SPL06 on I2C1) — fixed bus assignment, needs retest
-- [ ] VBAT ADC (PC2) — fixed pin assignment, needs retest
-- [ ] BEC ADC (PC1) — needs test
-- [ ] Gyro alignment (CW90) — fixed, needs retest
-- [ ] All servo outputs — need test
-- [ ] CRSF receiver on UART4 — needs test
-- [ ] Blackbox logging — needs test
-- [ ] VBAT scale calibration with multimeter
+- [x] Gyro alignment (CW90) confirmed
+- [x] Accelerometer working
+- [x] Barometer (SPL06 on I2C1) working
+- [x] VBAT ADC (PC2) working (scale 320)
+- [x] All UART ports (1-4, 6) verified
+- [x] UART4 CRSF receiver working (TX/RX swap confirmed)
+- [x] DShot on all motor outputs
+- [x] Servo outputs working
+- [x] Blackbox logging working
+- [x] LEDs working
+- [x] I2C2 bus working
 
 ## Building
 
diff --git a/src/main/target/NEXUS/target.h b/src/main/target/NEXUS/target.h
index 8f0510e5981..84fde23a95e 100644
--- a/src/main/target/NEXUS/target.h
+++ b/src/main/target/NEXUS/target.h
@@ -129,19 +129,18 @@
 
 
 /* ---- ADC ---- */
-// From Rotorflight dump:
-//   ADC_BEC = PC1, divider 160 (BEC 5V rail monitoring)
-//   ADC_BUS = PC2, divider 320 (external battery voltage)
-// Map BUS voltage as VBAT for iNAV since that's the flight battery
+// OG Nexus has no EXT-V input (unlike X/XR which has a dedicated
+// high-voltage sense on PC0). Only two ADC channels:
+//   ADC_BUS = PC2, divider 320 (Vin rail, 5-12.6V)
+//   ADC_BEC = PC1, divider 160 (BEC 5V rail)
+// Map Vin as VBAT since it's the primary power input
 #define USE_ADC
 #define ADC_INSTANCE            ADC1
-#define ADC_CHANNEL_1_PIN       PC2   // BUS voltage (external battery)
+#define ADC_CHANNEL_1_PIN       PC2   // Vin (input power rail)
 #define VBAT_ADC_CHANNEL        ADC_CHN_1
-#define ADC_CHANNEL_2_PIN       PC1   // BEC voltage (5V rail)
-// VBAT scale: Rotorflight vbus_divider = 320
-// iNAV scale = divider * ~3.44 (ADC ref / resolution factor)
-// Start with 1100, calibrate with multimeter
-#define VBAT_SCALE_DEFAULT      1100
+#define ADC_CHANNEL_2_PIN       PC1   // BEC 5V rail
+// VBAT scale: hardware-verified value (divider ratio ~320)
+#define VBAT_SCALE_DEFAULT      320
 
 /* ---- Sensors ---- */
 #define SENSORS_SET             (SENSOR_ACC | SENSOR_BARO)

From 38da5a7202ec6824d091fb8971b37517856033fd Mon Sep 17 00:00:00 2001
From: Ada <ada@6bit.com>
Date: Wed, 25 Feb 2026 08:55:33 -0700
Subject: [PATCH 024/102] NEXUS: fix IMU alignment to match board orientation
 arrow

The CW90 value was taken directly from the Rotorflight NEXUS_F7 target,
but Rotorflight does not enforce arrow-forward as the board reference
orientation. The Rotorflight CW90 was calibrated to an arbitrary
mounting, not the case arrow. iNAV convention is arrow = forward,
which requires CW180 on this hardware.

Same root cause as the NEXUSX fix in #11338 / #11325.

Co-Authored-By: Joshua Perry <josh@6bit.com>
---
 src/main/target/NEXUS/target.h | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/main/target/NEXUS/target.h b/src/main/target/NEXUS/target.h
index 84fde23a95e..63c08049931 100644
--- a/src/main/target/NEXUS/target.h
+++ b/src/main/target/NEXUS/target.h
@@ -6,7 +6,8 @@
  *
  * Key differences from Nexus-X/XR:
  *   - IMU EXTI on PA15 (X/XR uses PB8)
- *   - IMU alignment CW90 (X/XR uses CW180)
+ *   - IMU alignment CW180 (Rotorflight uses CW90 with a different
+ *     board orientation reference; iNAV uses the arrow on the case)
  *   - Flash is W25N01G 128MB (X/XR uses W25N02K 256MB)
  *   - No internal ELRS receiver (X/XR has RP4TD-M on UART5)
  *   - No PINIO1 receiver power gate
@@ -48,7 +49,7 @@
 /* ---- IMU: ICM-42688-P ---- */
 // iNAV uses ICM42605 driver which is register-compatible with ICM42688P
 #define USE_IMU_ICM42605
-#define IMU_ICM42605_ALIGN      CW90_DEG
+#define IMU_ICM42605_ALIGN      CW180_DEG
 #define ICM42605_CS_PIN         PA4
 #define ICM42605_SPI_BUS        BUS_SPI1
 #define ICM42605_EXTI_PIN       PA15

From 7b0bd005adc7d2afaccbae5b630cb7ebcfd1b725 Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Wed, 25 Feb 2026 21:18:11 -0600
Subject: [PATCH 025/102] New target: FRSKYF405

FrSky F405 flight controller based on STM32F405RGT6.

Hardware: IIM-42688P gyro (SPI1), AT7456E OSD (SPI2), SD card (SPI3),
SPL06 barometer (I2C1), 9 motor outputs, hardware SBUS inverter on UART2,
LED strip on PA15. S7/S8 use TIM12 (PWM only, no DShot).
---
 src/main/target/FRSKYF405/CMakeLists.txt |   1 +
 src/main/target/FRSKYF405/config.c       |  32 ++++
 src/main/target/FRSKYF405/target.c       |  70 ++++++++
 src/main/target/FRSKYF405/target.h       | 210 +++++++++++++++++++++++
 4 files changed, 313 insertions(+)
 create mode 100644 src/main/target/FRSKYF405/CMakeLists.txt
 create mode 100644 src/main/target/FRSKYF405/config.c
 create mode 100644 src/main/target/FRSKYF405/target.c
 create mode 100644 src/main/target/FRSKYF405/target.h

diff --git a/src/main/target/FRSKYF405/CMakeLists.txt b/src/main/target/FRSKYF405/CMakeLists.txt
new file mode 100644
index 00000000000..fc8eb478bcc
--- /dev/null
+++ b/src/main/target/FRSKYF405/CMakeLists.txt
@@ -0,0 +1 @@
+target_stm32f405xg(FRSKYF405)
diff --git a/src/main/target/FRSKYF405/config.c b/src/main/target/FRSKYF405/config.c
new file mode 100644
index 00000000000..0aed59803e0
--- /dev/null
+++ b/src/main/target/FRSKYF405/config.c
@@ -0,0 +1,32 @@
+/*
+ * 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 "rx/rx.h"
+#include "fc/config.h"
+
+void targetConfiguration(void)
+{
+    // SBUS is received via a hardware inverter (2N7002E MOSFET, Q1).
+    // The signal arrives at UART2 RX already de-inverted (active high),
+    // so the UART must NOT apply additional software inversion.
+    rxConfigMutable()->serialrx_inverted = 1;
+}
diff --git a/src/main/target/FRSKYF405/target.c b/src/main/target/FRSKYF405/target.c
new file mode 100644
index 00000000000..89f3ee73a0e
--- /dev/null
+++ b/src/main/target/FRSKYF405/target.c
@@ -0,0 +1,70 @@
+/*
+ * 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 "platform.h"
+#include "drivers/io.h"
+#include "drivers/pwm_mapping.h"
+#include "drivers/timer.h"
+
+/*
+ * Timer Allocation for FrSky F405
+ *
+ * Connector mapping from schematic (CONN.SchDoc):
+ * - CON1 (S1): T4_2 = TIM4_CH2 = PB7
+ * - CON2 (S2): T4_1 = TIM4_CH1 = PB6
+ * - CON3 (S3): T3_3 = TIM3_CH3 = PB0
+ * - CON4 (S4): T3_4 = TIM3_CH4 = PB1
+ * - CON5 (S5): T8_3 = TIM8_CH3 = PC8
+ * - CON6 (S6): T8_4 = TIM8_CH4 = PC9
+ * - CON7 (S7): T12_1 = TIM12_CH1 = PB14  (no DMA - no DShot)
+ * - CON8 (S8): T12_2 = TIM12_CH2 = PB15  (no DMA - no DShot)
+ * - CON9 (S9): T1_1  = TIM1_CH1  = PA8
+ * - CON23 (LED): T2_1 = TIM2_CH1 = PA15
+ */
+
+timerHardware_t timerHardware[] = {
+    // Motor outputs S1-S9 (in physical connector order from schematic)
+    DEF_TIM(TIM4,  CH2,  PB7,  TIM_USE_OUTPUT_AUTO, 0, 0),  // S1 (CON1) - Motor 1
+    DEF_TIM(TIM4,  CH1,  PB6,  TIM_USE_OUTPUT_AUTO, 0, 0),  // S2 (CON2) - Motor 2
+    DEF_TIM(TIM3,  CH3,  PB0,  TIM_USE_OUTPUT_AUTO, 0, 0),  // S3 (CON3) - Motor 3
+    DEF_TIM(TIM3,  CH4,  PB1,  TIM_USE_OUTPUT_AUTO, 0, 0),  // S4 (CON4) - Motor 4
+    DEF_TIM(TIM8,  CH3,  PC8,  TIM_USE_OUTPUT_AUTO, 0, 1),  // S5 (CON5) - Motor 5  UP(2,1)
+    DEF_TIM(TIM8,  CH4,  PC9,  TIM_USE_OUTPUT_AUTO, 0, 0),  // S6 (CON6) - Motor 6
+    DEF_TIM(TIM12, CH1,  PB14, TIM_USE_OUTPUT_AUTO, 0, 0),  // S7 (CON7) - Motor 7  NOTE: TIM12 has no DMA - DShot not supported
+    DEF_TIM(TIM12, CH2,  PB15, TIM_USE_OUTPUT_AUTO, 0, 0),  // S8 (CON8) - Motor 8  NOTE: TIM12 has no DMA - DShot not supported
+    DEF_TIM(TIM1,  CH1,  PA8,  TIM_USE_OUTPUT_AUTO, 0, 0),  // S9 (CON9) - Motor 9  UP(2,5)
+
+    // LED Strip on CON23 - PA15 (T2_1 signal on schematic)
+    DEF_TIM(TIM2,  CH1,  PA15, TIM_USE_LED, 0, 0),          // LED strip (CON23)
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
+
+/*
+ * IMPLEMENTATION NOTES:
+ *
+ * 1. S1/S2 order matches physical board labels (CON1=S1, CON2=S2 from schematic)
+ *
+ * 2. TIM12 (S7, S8): No DMA support on STM32F405.
+ *    Works: Standard PWM, OneShot125, OneShot42, MultiShot
+ *    Does NOT work: DShot (all variants), ProShot
+ *    Use S7/S8 for non-DShot ESCs or servos
+ *
+ * 3. LED strip output: PA15 (T2_1) on CON23 with TIM2_CH1
+ */
diff --git a/src/main/target/FRSKYF405/target.h b/src/main/target/FRSKYF405/target.h
new file mode 100644
index 00000000000..e3eafdb51da
--- /dev/null
+++ b/src/main/target/FRSKYF405/target.h
@@ -0,0 +1,210 @@
+/*
+ * 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 "FRF4"
+
+#define USBD_PRODUCT_STRING  "FrSkyF405"
+
+// *************** LED & BEEPER **********************
+// Status LEDs share SWD debug pins - only available in release builds
+// In debug builds, these pins must remain in SWD mode for debugging
+// Red LED is likely a power indicator (always-on, no GPIO control)
+#ifndef DEBUG_BUILD
+    #define LED0                PA14    // Blue LED (shares SWCLK) via R34 1K
+    #define LED1                PA13    // Green LED (shares SWDIO) via R35 1K
+#endif
+// Red LED appears to be power indicator (connected to VCC, not GPIO)
+
+#define BEEPER                  PC15
+#define BEEPER_INVERTED         // INAV beeper driver writes !onoff; BEEPER_INVERTED makes it write HIGH to activate the N-channel MOSFET (Q2)
+
+// *************** 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
+
+// IIM-42688P Gyro on SPI1
+#define USE_IMU_ICM42605       // IIM-42688P is compatible with ICM42605 driver
+#define ICM42605_CS_PIN        PA4
+#define ICM42605_SPI_BUS       BUS_SPI1
+#define IMU_ICM42605_ALIGN     CW0_DEG  // TODO: Verify orientation from board layout
+
+
+// *************** OSD *****************************
+#define USE_SPI_DEVICE_2
+#define SPI2_SCK_PIN            PB13
+#define SPI2_MISO_PIN           PC2
+#define SPI2_MOSI_PIN           PC3
+
+// AT7456E OSD on SPI2
+#define USE_MAX7456
+#define MAX7456_SPI_BUS         BUS_SPI2
+#define MAX7456_CS_PIN          PB12
+
+// *************** SD Card *************************
+#define USE_SPI_DEVICE_3
+#define SPI3_SCK_PIN            PC10
+#define SPI3_MISO_PIN           PC11
+#define SPI3_MOSI_PIN           PC12
+
+// SD Card on SPI3
+// NOTE: PC14 used for CS (Chip Select) - OSC32_IN pin used as GPIO for slow CS signal
+#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
+
+// *************** UART *****************************
+#define USE_VCP
+// Note: USB VBUS sensing not clearly defined in schematic
+
+#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
+
+// USART2 has SBUS inverter circuit
+#define SERIALRX_UART           SERIAL_PORT_USART2
+#define SERIALRX_PROVIDER       SERIALRX_SBUS
+#define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
+
+// UART3 conflicts with I2C2 (shares PB10/PB11)
+// User must choose: enable UART3 OR I2C2, not both
+#define USE_UART3
+#define UART3_TX_PIN            PB10    // Conflicts with I2C2_SCL
+#define UART3_RX_PIN            PB11    // Conflicts with I2C2_SDA
+
+#define USE_UART4
+#define UART4_TX_PIN            PA0
+#define UART4_RX_PIN            PC5     // Shared with RSSI_ADC
+
+// UART5 TX conflicts with SPI3_MOSI (PC12)
+// User must choose: enable UART5 OR SD card, not both
+#define USE_UART5
+#define UART5_TX_PIN            PC12    // Conflicts with SPI3_MOSI
+#define UART5_RX_PIN            PD2
+
+#define USE_UART6
+#define UART6_TX_PIN            PC6
+#define UART6_RX_PIN            PC7
+
+#define SERIAL_PORT_COUNT       7       // VCP + UART1-6
+
+// *************** I2C ****************************
+#define USE_I2C
+
+// I2C1 for barometer (SPL06)
+#define USE_I2C_DEVICE_1
+#define I2C1_SCL                PB8
+#define I2C1_SDA                PB9
+
+// I2C2 conflicts with UART3 (shares PB10/PB11)
+// Enable one or the other, not both
+// #define USE_I2C_DEVICE_2
+// #define I2C2_SCL                PB10    // Conflicts with UART3_TX
+// #define I2C2_SDA                PB11    // Conflicts with UART3_RX
+
+#define DEFAULT_I2C_BUS         BUS_I2C1
+
+// SPL06 Barometer on I2C1 @ address 0x76
+#define USE_BARO
+#define BARO_I2C_BUS            BUS_I2C1
+#define USE_BARO_SPL06
+
+// Magnetometer on I2C (external via connector)
+#define USE_MAG
+#define MAG_I2C_BUS             DEFAULT_I2C_BUS
+#define USE_MAG_ALL
+
+#define PITOT_I2C_BUS           DEFAULT_I2C_BUS
+#define TEMPERATURE_I2C_BUS     DEFAULT_I2C_BUS
+#define USE_RANGEFINDER
+#define RANGEFINDER_I2C_BUS     DEFAULT_I2C_BUS
+
+// *************** ADC *****************************
+#define USE_ADC
+#define ADC_INSTANCE            ADC1
+#define ADC1_DMA_STREAM         DMA2_Stream0
+
+#define ADC_CHANNEL_1_PIN       PC0     // VBAT_ADC
+#define ADC_CHANNEL_2_PIN       PC1     // CURR_ADC
+#define ADC_CHANNEL_3_PIN       PC5     // RSSI_IN (shared with UART4_RX)
+
+#define VBAT_ADC_CHANNEL        ADC_CHN_1
+#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2
+#define RSSI_ADC_CHANNEL        ADC_CHN_3
+
+// Current sensor: INA139 with 0.25mΩ shunt
+#define CURRENT_METER_SCALE     250     // TODO: Calibrate based on INA139 + shunt values
+
+#define DEFAULT_FEATURES        (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY | FEATURE_BLACKBOX)
+
+// *************** LED STRIP ***********************
+// CON23 LED output - PA15 (T2_1 signal on schematic)
+#define USE_LED_STRIP
+#define WS2811_PIN              PA15    // TIM2_CH1, labeled as T2_1 on schematic
+
+// *************** PWM OUTPUTS *********************
+// 9 motor outputs (S1-S9)
+// Note: S7/S8 on TIM12 do not support DShot (no DMA on TIM12 for STM32F405)
+#define MAX_PWM_OUTPUT_PORTS    9
+
+// *************** Other ***************************
+#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))  // PD2 used for UART5_RX
+
+// *************** NOTES & ISSUES *******************
+//
+// DESIGN NOTES:
+// 1. PC14 used for SPI3_NSS (SD card CS) - RTC clock domain, should be fine for slow CS signal
+//
+// 2. Status LEDs share SWD debug pins (PA13/PA14) - HANDLED WITH #ifndef DEBUG_BUILD
+//    Release builds: Pins configured as GPIO outputs for status LEDs
+//    Debug builds: Pins remain in SWD mode for debugging (LEDs disabled)
+//    Red LED appears to be power indicator (always-on, no GPIO control needed)
+//
+// PIN CONFLICTS (target or user must choose):
+// 3. UART3 vs I2C2: PB10/PB11 shared
+//    - Enable UART3 for telemetry/GPS, OR
+//    - Enable I2C2 for additional sensors
+//
+// 4. UART5 TX vs SD Card: PC12 shared (SPI3_MOSI)
+//    - Enable UART5 for telemetry, OR
+//    - Enable SD card for blackbox logging
+//
+// 5. UART4_RX vs RSSI ADC: PC5 shared
+//    - May be intentional if RSSI comes via UART protocol
+//
+// MISSING INFORMATION:
+// 6. Gyro orientation (IMU_ALIGN) unknown - needs physical board inspection
+// 7. USB VBUS sensing pin not clearly defined in schematic (PC13 may be used)

From a30a4bdf13f561abb92875d91ec2e6f92d18b56c Mon Sep 17 00:00:00 2001
From: ncerzzk <huangcmzzk@gmail.com>
Date: Mon, 23 Feb 2026 22:13:23 +0800
Subject: [PATCH 026/102] fix lsm6dxx driver lack gyroAlign setting issue.

Signed-off-by: ncerzzk <huangcmzzk@gmail.com>
---
 src/main/drivers/accgyro/accgyro_lsm6dxx.c | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/drivers/accgyro/accgyro_lsm6dxx.c b/src/main/drivers/accgyro/accgyro_lsm6dxx.c
index 5e7d24f44e4..874f32eacb3 100644
--- a/src/main/drivers/accgyro/accgyro_lsm6dxx.c
+++ b/src/main/drivers/accgyro/accgyro_lsm6dxx.c
@@ -222,6 +222,7 @@ bool lsm6dGyroDetect(gyroDev_t *gyro)
     gyro->initFn = lsm6dxxSpiGyroInit;
     gyro->readFn = lsm6dxxGyroRead;
     gyro->intStatusFn = gyroCheckDataReady;
+    gyro->gyroAlign = gyro->busDev->param;
     gyro->scale = 1.0f / 16.4f; // 2000 dps
     return true;
 

From 623fe7ae38a7b1c7ee7723f12a0e1af0a1996eb6 Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Sun, 1 Mar 2026 12:08:06 -0600
Subject: [PATCH 027/102] FRSKYF405: fix pin assignments and clean up target
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

- Fix UART5_RX: PD12 → PD2 (only valid pin for UART5_RX on STM32F405)
- Move SPI3 to PB3/PB4/PB5, freeing PC10/PC11/PC12 for UART3 and UART5_TX
- Move UART3 to PC10/PC11 (was conflicting with I2C2 on PB10/PB11)
- Fix UART4_RX: PC5 → PA1 (PC5 is shared with RSSI ADC)
- Re-enable I2C2 on PB10/PB11 (now free after UART3 move)
- Add TARGET_IO_PORTD for PD2 (UART5_RX)
- Update SERIAL_PORT_COUNT to 7 (includes UART5)
- Remove config.c: UART2 has both standard and hardware-inverted SBUS pads
---
 src/main/target/FRSKYF405/config.c | 32 ----------
 src/main/target/FRSKYF405/target.c | 51 +++------------
 src/main/target/FRSKYF405/target.h | 99 ++++++++----------------------
 3 files changed, 36 insertions(+), 146 deletions(-)
 delete mode 100644 src/main/target/FRSKYF405/config.c

diff --git a/src/main/target/FRSKYF405/config.c b/src/main/target/FRSKYF405/config.c
deleted file mode 100644
index 0aed59803e0..00000000000
--- a/src/main/target/FRSKYF405/config.c
+++ /dev/null
@@ -1,32 +0,0 @@
-/*
- * 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 "rx/rx.h"
-#include "fc/config.h"
-
-void targetConfiguration(void)
-{
-    // SBUS is received via a hardware inverter (2N7002E MOSFET, Q1).
-    // The signal arrives at UART2 RX already de-inverted (active high),
-    // so the UART must NOT apply additional software inversion.
-    rxConfigMutable()->serialrx_inverted = 1;
-}
diff --git a/src/main/target/FRSKYF405/target.c b/src/main/target/FRSKYF405/target.c
index 89f3ee73a0e..0708a3981e4 100644
--- a/src/main/target/FRSKYF405/target.c
+++ b/src/main/target/FRSKYF405/target.c
@@ -22,49 +22,18 @@
 #include "drivers/pwm_mapping.h"
 #include "drivers/timer.h"
 
-/*
- * Timer Allocation for FrSky F405
- *
- * Connector mapping from schematic (CONN.SchDoc):
- * - CON1 (S1): T4_2 = TIM4_CH2 = PB7
- * - CON2 (S2): T4_1 = TIM4_CH1 = PB6
- * - CON3 (S3): T3_3 = TIM3_CH3 = PB0
- * - CON4 (S4): T3_4 = TIM3_CH4 = PB1
- * - CON5 (S5): T8_3 = TIM8_CH3 = PC8
- * - CON6 (S6): T8_4 = TIM8_CH4 = PC9
- * - CON7 (S7): T12_1 = TIM12_CH1 = PB14  (no DMA - no DShot)
- * - CON8 (S8): T12_2 = TIM12_CH2 = PB15  (no DMA - no DShot)
- * - CON9 (S9): T1_1  = TIM1_CH1  = PA8
- * - CON23 (LED): T2_1 = TIM2_CH1 = PA15
- */
-
 timerHardware_t timerHardware[] = {
-    // Motor outputs S1-S9 (in physical connector order from schematic)
-    DEF_TIM(TIM4,  CH2,  PB7,  TIM_USE_OUTPUT_AUTO, 0, 0),  // S1 (CON1) - Motor 1
-    DEF_TIM(TIM4,  CH1,  PB6,  TIM_USE_OUTPUT_AUTO, 0, 0),  // S2 (CON2) - Motor 2
-    DEF_TIM(TIM3,  CH3,  PB0,  TIM_USE_OUTPUT_AUTO, 0, 0),  // S3 (CON3) - Motor 3
-    DEF_TIM(TIM3,  CH4,  PB1,  TIM_USE_OUTPUT_AUTO, 0, 0),  // S4 (CON4) - Motor 4
-    DEF_TIM(TIM8,  CH3,  PC8,  TIM_USE_OUTPUT_AUTO, 0, 1),  // S5 (CON5) - Motor 5  UP(2,1)
-    DEF_TIM(TIM8,  CH4,  PC9,  TIM_USE_OUTPUT_AUTO, 0, 0),  // S6 (CON6) - Motor 6
-    DEF_TIM(TIM12, CH1,  PB14, TIM_USE_OUTPUT_AUTO, 0, 0),  // S7 (CON7) - Motor 7  NOTE: TIM12 has no DMA - DShot not supported
-    DEF_TIM(TIM12, CH2,  PB15, TIM_USE_OUTPUT_AUTO, 0, 0),  // S8 (CON8) - Motor 8  NOTE: TIM12 has no DMA - DShot not supported
-    DEF_TIM(TIM1,  CH1,  PA8,  TIM_USE_OUTPUT_AUTO, 0, 0),  // S9 (CON9) - Motor 9  UP(2,5)
+    DEF_TIM(TIM4,  CH2,  PB7,  TIM_USE_OUTPUT_AUTO, 0, 0),  // S1
+    DEF_TIM(TIM4,  CH1,  PB6,  TIM_USE_OUTPUT_AUTO, 0, 0),  // S2
+    DEF_TIM(TIM3,  CH3,  PB0,  TIM_USE_OUTPUT_AUTO, 0, 0),  // S3
+    DEF_TIM(TIM3,  CH4,  PB1,  TIM_USE_OUTPUT_AUTO, 0, 0),  // S4
+    DEF_TIM(TIM8,  CH3,  PC8,  TIM_USE_OUTPUT_AUTO, 0, 1),  // S5
+    DEF_TIM(TIM8,  CH4,  PC9,  TIM_USE_OUTPUT_AUTO, 0, 0),  // S6
+    DEF_TIM(TIM12, CH1,  PB14, TIM_USE_OUTPUT_AUTO, 0, 0),  // S7 - no DShot (TIM12 has no DMA)
+    DEF_TIM(TIM12, CH2,  PB15, TIM_USE_OUTPUT_AUTO, 0, 0),  // S8 - no DShot (TIM12 has no DMA)
+    DEF_TIM(TIM1,  CH1,  PA8,  TIM_USE_OUTPUT_AUTO, 0, 0),  // S9
 
-    // LED Strip on CON23 - PA15 (T2_1 signal on schematic)
-    DEF_TIM(TIM2,  CH1,  PA15, TIM_USE_LED, 0, 0),          // LED strip (CON23)
+    DEF_TIM(TIM2,  CH1,  PA15, TIM_USE_LED, 0, 0),          // LED strip
 };
 
 const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
-
-/*
- * IMPLEMENTATION NOTES:
- *
- * 1. S1/S2 order matches physical board labels (CON1=S1, CON2=S2 from schematic)
- *
- * 2. TIM12 (S7, S8): No DMA support on STM32F405.
- *    Works: Standard PWM, OneShot125, OneShot42, MultiShot
- *    Does NOT work: DShot (all variants), ProShot
- *    Use S7/S8 for non-DShot ESCs or servos
- *
- * 3. LED strip output: PA15 (T2_1) on CON23 with TIM2_CH1
- */
diff --git a/src/main/target/FRSKYF405/target.h b/src/main/target/FRSKYF405/target.h
index e3eafdb51da..146519a99b8 100644
--- a/src/main/target/FRSKYF405/target.h
+++ b/src/main/target/FRSKYF405/target.h
@@ -22,17 +22,11 @@
 #define USBD_PRODUCT_STRING  "FrSkyF405"
 
 // *************** LED & BEEPER **********************
-// Status LEDs share SWD debug pins - only available in release builds
-// In debug builds, these pins must remain in SWD mode for debugging
-// Red LED is likely a power indicator (always-on, no GPIO control)
-#ifndef DEBUG_BUILD
-    #define LED0                PA14    // Blue LED (shares SWCLK) via R34 1K
-    #define LED1                PA13    // Green LED (shares SWDIO) via R35 1K
-#endif
-// Red LED appears to be power indicator (connected to VCC, not GPIO)
+#define LED0                PA14    // shares SWCLK
+#define LED1                PA13    // shares SWDIO
 
 #define BEEPER                  PC15
-#define BEEPER_INVERTED         // INAV beeper driver writes !onoff; BEEPER_INVERTED makes it write HIGH to activate the N-channel MOSFET (Q2)
+#define BEEPER_INVERTED
 
 // *************** Gyro & ACC **********************
 #define USE_SPI
@@ -42,11 +36,11 @@
 #define SPI1_MISO_PIN           PA6
 #define SPI1_MOSI_PIN           PA7
 
-// IIM-42688P Gyro on SPI1
-#define USE_IMU_ICM42605       // IIM-42688P is compatible with ICM42605 driver
+// IIM-42688P is compatible with ICM42605 driver
+#define USE_IMU_ICM42605
 #define ICM42605_CS_PIN        PA4
 #define ICM42605_SPI_BUS       BUS_SPI1
-#define IMU_ICM42605_ALIGN     CW0_DEG  // TODO: Verify orientation from board layout
+#define IMU_ICM42605_ALIGN     CW0_DEG
 
 
 // *************** OSD *****************************
@@ -55,19 +49,16 @@
 #define SPI2_MISO_PIN           PC2
 #define SPI2_MOSI_PIN           PC3
 
-// AT7456E OSD on SPI2
 #define USE_MAX7456
 #define MAX7456_SPI_BUS         BUS_SPI2
 #define MAX7456_CS_PIN          PB12
 
 // *************** SD Card *************************
 #define USE_SPI_DEVICE_3
-#define SPI3_SCK_PIN            PC10
-#define SPI3_MISO_PIN           PC11
-#define SPI3_MOSI_PIN           PC12
+#define SPI3_SCK_PIN            PB3
+#define SPI3_MISO_PIN           PB4
+#define SPI3_MOSI_PIN           PB5
 
-// SD Card on SPI3
-// NOTE: PC14 used for CS (Chip Select) - OSC32_IN pin used as GPIO for slow CS signal
 #define USE_SDCARD
 #define USE_SDCARD_SPI
 #define SDCARD_SPI_BUS          BUS_SPI3
@@ -77,7 +68,6 @@
 
 // *************** UART *****************************
 #define USE_VCP
-// Note: USB VBUS sensing not clearly defined in schematic
 
 #define USE_UART1
 #define UART1_TX_PIN            PA9
@@ -87,55 +77,47 @@
 #define UART2_TX_PIN            PA2
 #define UART2_RX_PIN            PA3
 
-// USART2 has SBUS inverter circuit
+// UART2 has two pads: standard (for CRSF etc) and hardware-inverted SBUS
 #define SERIALRX_UART           SERIAL_PORT_USART2
 #define SERIALRX_PROVIDER       SERIALRX_SBUS
 #define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
 
-// UART3 conflicts with I2C2 (shares PB10/PB11)
-// User must choose: enable UART3 OR I2C2, not both
 #define USE_UART3
-#define UART3_TX_PIN            PB10    // Conflicts with I2C2_SCL
-#define UART3_RX_PIN            PB11    // Conflicts with I2C2_SDA
+#define UART3_TX_PIN            PC10
+#define UART3_RX_PIN            PC11
 
 #define USE_UART4
 #define UART4_TX_PIN            PA0
-#define UART4_RX_PIN            PC5     // Shared with RSSI_ADC
+#define UART4_RX_PIN            PA1
+
 
-// UART5 TX conflicts with SPI3_MOSI (PC12)
-// User must choose: enable UART5 OR SD card, not both
 #define USE_UART5
-#define UART5_TX_PIN            PC12    // Conflicts with SPI3_MOSI
+#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       // VCP + UART1-6
+#define SERIAL_PORT_COUNT       7       // VCP + UART1-5, UART6
 
 // *************** I2C ****************************
 #define USE_I2C
 
-// I2C1 for barometer (SPL06)
 #define USE_I2C_DEVICE_1
 #define I2C1_SCL                PB8
 #define I2C1_SDA                PB9
 
-// I2C2 conflicts with UART3 (shares PB10/PB11)
-// Enable one or the other, not both
-// #define USE_I2C_DEVICE_2
-// #define I2C2_SCL                PB10    // Conflicts with UART3_TX
-// #define I2C2_SDA                PB11    // Conflicts with UART3_RX
+#define USE_I2C_DEVICE_2
+#define I2C2_SCL                PB10
+#define I2C2_SDA                PB11
 
 #define DEFAULT_I2C_BUS         BUS_I2C1
 
-// SPL06 Barometer on I2C1 @ address 0x76
 #define USE_BARO
 #define BARO_I2C_BUS            BUS_I2C1
 #define USE_BARO_SPL06
 
-// Magnetometer on I2C (external via connector)
 #define USE_MAG
 #define MAG_I2C_BUS             DEFAULT_I2C_BUS
 #define USE_MAG_ALL
@@ -150,27 +132,24 @@
 #define ADC_INSTANCE            ADC1
 #define ADC1_DMA_STREAM         DMA2_Stream0
 
-#define ADC_CHANNEL_1_PIN       PC0     // VBAT_ADC
-#define ADC_CHANNEL_2_PIN       PC1     // CURR_ADC
-#define ADC_CHANNEL_3_PIN       PC5     // RSSI_IN (shared with UART4_RX)
+#define ADC_CHANNEL_1_PIN       PC0
+#define ADC_CHANNEL_2_PIN       PC1
+#define ADC_CHANNEL_3_PIN       PC5
 
 #define VBAT_ADC_CHANNEL        ADC_CHN_1
 #define CURRENT_METER_ADC_CHANNEL ADC_CHN_2
 #define RSSI_ADC_CHANNEL        ADC_CHN_3
 
-// Current sensor: INA139 with 0.25mΩ shunt
-#define CURRENT_METER_SCALE     250     // TODO: Calibrate based on INA139 + shunt values
+#define CURRENT_METER_SCALE     250
 
 #define DEFAULT_FEATURES        (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY | FEATURE_BLACKBOX)
 
 // *************** LED STRIP ***********************
-// CON23 LED output - PA15 (T2_1 signal on schematic)
 #define USE_LED_STRIP
-#define WS2811_PIN              PA15    // TIM2_CH1, labeled as T2_1 on schematic
+#define WS2811_PIN              PA15
 
 // *************** PWM OUTPUTS *********************
-// 9 motor outputs (S1-S9)
-// Note: S7/S8 on TIM12 do not support DShot (no DMA on TIM12 for STM32F405)
+// S7/S8 on TIM12 do not support DShot
 #define MAX_PWM_OUTPUT_PORTS    9
 
 // *************** Other ***************************
@@ -181,30 +160,4 @@
 #define TARGET_IO_PORTA         0xffff
 #define TARGET_IO_PORTB         0xffff
 #define TARGET_IO_PORTC         0xffff
-#define TARGET_IO_PORTD         (BIT(2))  // PD2 used for UART5_RX
-
-// *************** NOTES & ISSUES *******************
-//
-// DESIGN NOTES:
-// 1. PC14 used for SPI3_NSS (SD card CS) - RTC clock domain, should be fine for slow CS signal
-//
-// 2. Status LEDs share SWD debug pins (PA13/PA14) - HANDLED WITH #ifndef DEBUG_BUILD
-//    Release builds: Pins configured as GPIO outputs for status LEDs
-//    Debug builds: Pins remain in SWD mode for debugging (LEDs disabled)
-//    Red LED appears to be power indicator (always-on, no GPIO control needed)
-//
-// PIN CONFLICTS (target or user must choose):
-// 3. UART3 vs I2C2: PB10/PB11 shared
-//    - Enable UART3 for telemetry/GPS, OR
-//    - Enable I2C2 for additional sensors
-//
-// 4. UART5 TX vs SD Card: PC12 shared (SPI3_MOSI)
-//    - Enable UART5 for telemetry, OR
-//    - Enable SD card for blackbox logging
-//
-// 5. UART4_RX vs RSSI ADC: PC5 shared
-//    - May be intentional if RSSI comes via UART protocol
-//
-// MISSING INFORMATION:
-// 6. Gyro orientation (IMU_ALIGN) unknown - needs physical board inspection
-// 7. USB VBUS sensing pin not clearly defined in schematic (PC13 may be used)
+#define TARGET_IO_PORTD         (BIT(2))    // PD2 - UART5_RX

From 06e7ef2c2d81e3ec77998e5baf7039659618f648 Mon Sep 17 00:00:00 2001
From: jp39 <jp39@gmx.com>
Date: Wed, 8 Oct 2025 12:50:28 +0200
Subject: [PATCH 028/102] AIKONF4V2 support.

---
 src/main/target/AIKONF4V2/CMakeLists.txt |   1 +
 src/main/target/AIKONF4V2/config.c       |  27 +++++
 src/main/target/AIKONF4V2/target.c       |  39 +++++++
 src/main/target/AIKONF4V2/target.h       | 134 +++++++++++++++++++++++
 4 files changed, 201 insertions(+)
 create mode 100644 src/main/target/AIKONF4V2/CMakeLists.txt
 create mode 100644 src/main/target/AIKONF4V2/config.c
 create mode 100644 src/main/target/AIKONF4V2/target.c
 create mode 100644 src/main/target/AIKONF4V2/target.h

diff --git a/src/main/target/AIKONF4V2/CMakeLists.txt b/src/main/target/AIKONF4V2/CMakeLists.txt
new file mode 100644
index 00000000000..a9393354aea
--- /dev/null
+++ b/src/main/target/AIKONF4V2/CMakeLists.txt
@@ -0,0 +1 @@
+target_stm32f405xg(AIKONF4V2)
diff --git a/src/main/target/AIKONF4V2/config.c b/src/main/target/AIKONF4V2/config.c
new file mode 100644
index 00000000000..fcb850b3f53
--- /dev/null
+++ b/src/main/target/AIKONF4V2/config.c
@@ -0,0 +1,27 @@
+/*
+ * 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)
+{
+}
diff --git a/src/main/target/AIKONF4V2/target.c b/src/main/target/AIKONF4V2/target.c
new file mode 100644
index 00000000000..54777bc509c
--- /dev/null
+++ b/src/main/target/AIKONF4V2/target.c
@@ -0,0 +1,39 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * 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/>.
+ */
+
+#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(TIM2,  CH2,  PB3,  TIM_USE_CAMERA_CONTROL,      0, 0), // There is not camera control in INAV
+    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(TIM4,  CH1,  PB6,  TIM_USE_LED, 0, 0), // LED
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
diff --git a/src/main/target/AIKONF4V2/target.h b/src/main/target/AIKONF4V2/target.h
new file mode 100644
index 00000000000..a93415371bd
--- /dev/null
+++ b/src/main/target/AIKONF4V2/target.h
@@ -0,0 +1,134 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * 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 DEFAULT_FEATURES        (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_BLACKBOX | FEATURE_SOFTSERIAL)
+
+#define TARGET_BOARD_IDENTIFIER "AIK4"
+#define USBD_PRODUCT_STRING     "AIKONF4V2"
+
+// Beeper
+#define USE_BEEPER
+#define BEEPER                  PB5
+#define BEEPER_INVERTED
+// Leds
+#define USE_LED_STRIP
+#define WS2811_PIN              PB6
+#define LED0                    PB4
+// UARTs
+#define USB_IO
+#define USE_VCP
+#define USE_UART1
+#define UART1_RX_PIN            PA10
+#define UART1_TX_PIN            PA9
+#define USE_UART2
+#define UART2_TX_PIN            PA2
+#define UART2_RX_PIN            PA3
+#define USE_UART3
+#define UART3_RX_PIN            PB11
+#define UART3_TX_PIN            PB10
+#define USE_UART4
+#define UART4_TX_PIN            PA0
+#define UART4_RX_PIN            PA1
+#define SERIAL_PORT_COUNT       5   //VCP, UART1, UART2, UART3, UART4
+#define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER       SERIALRX_SBUS
+#define SERIALRX_UART           SERIAL_PORT_USART1
+// 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            PB12
+#define SPI2_SCK_PIN            PB13
+#define SPI2_MISO_PIN           PB14
+#define SPI2_MOSI_PIN           PB15
+#define USE_SPI_DEVICE_3
+#define SPI3_NSS_PIN            PA15
+#define SPI3_SCK_PIN            PC10
+#define SPI3_MISO_PIN           PC11
+#define SPI3_MOSI_PIN           PC12
+// I2C
+#define USE_I2C
+#define USE_I2C_DEVICE_1
+#define I2C1_SCL                PB8
+#define I2C1_SDA                PB9
+#define DEFAULT_I2C_BUS         BUS_I2C1
+#define EXTERNAL_I2C_BUS        DEFAULT_I2C_BUS
+// MAG
+#define USE_MAG
+#define MAG_I2C_BUS             DEFAULT_I2C_BUS
+#define USE_MAG_ALL
+// ADC
+#define USE_ADC
+#define ADC_CHANNEL_1_PIN               PC1
+#define ADC_CHANNEL_2_PIN               PC2
+#define ADC_CHANNEL_3_PIN               PC3
+#define CURRENT_METER_ADC_CHANNEL       ADC_CHN_1
+#define VBAT_ADC_CHANNEL                ADC_CHN_2
+#define RSSI_ADC_CHANNEL                ADC_CHN_3
+#define ADC_INSTANCE ADC1
+// Gyro & ACC
+#define USE_IMU_MPU6000
+#define MPU6000_CS_PIN          SPI1_NSS_PIN
+#define MPU6000_SPI_BUS         BUS_SPI1
+#define IMU_MPU6000_ALIGN       CW0_DEG
+#define USE_IMU_ICM42605
+#define ICM42605_CS_PIN         SPI1_NSS_PIN
+#define ICM42605_SPI_BUS        BUS_SPI1
+#define IMU_ICM42605_ALIGN      CW90_DEG
+// OSD
+#define USE_MAX7456
+#define MAX7456_CS_PIN          SPI3_NSS_PIN
+#define MAX7456_SPI_BUS         BUS_SPI3
+// Baro
+#define USE_BARO
+#define BARO_I2C_BUS            DEFAULT_I2C_BUS
+#define USE_BARO_ALL
+// Blackbox
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+#define M25P16_CS_PIN           SPI2_NSS_PIN
+#define M25P16_SPI_BUS          BUS_SPI2
+#define USE_FLASHFS
+#define USE_FLASH_M25P16
+// Others
+#define TEMPERATURE_I2C_BUS     DEFAULT_I2C_BUS
+#define PITOT_I2C_BUS           DEFAULT_I2C_BUS
+#define RANGEFINDER_I2C_BUS     DEFAULT_I2C_BUS
+
+#define MAX_PWM_OUTPUT_PORTS    4
+#define USE_SERIAL_4WAY_BLHELI_INTERFACE
+#define USE_SERIALSHOT
+#define USE_DSHOT
+#define USE_ESC_SENSOR
+#define VOLTAGE_METER_SCALE 110
+#define CURRENT_METER_SCALE 400
+
+#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 TARGET_IO_PORTF         0xffff
+

From 2be65e05eb63792c7c244105904e2f1a6ebc9282 Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Mon, 2 Mar 2026 16:08:17 -0600
Subject: [PATCH 029/102] drivers: wait for DMA EN bit to clear before
 reconfiguring DShot DMA

Per STM32F7 Reference Manual (RM0410 Section 8.3.5), writes to DMA_SxNDTR
and DMA_SxM0AR are silently ignored while the EN bit is asserted. After
calling LL_DMA_DisableStream(), EN does not clear synchronously -- the
hardware may still be completing an in-progress burst.

impl_timerPWMPrepareDMA() previously called LL_DMA_SetDataLength() and
LL_DMA_ConfigAddresses() immediately after LL_DMA_DisableStream() with no
wait. In the race window where EN is still 1, both writes are discarded,
leaving stale count and address in the DMA registers. The subsequent
LL_DMA_EnableStream() then fires DMA with the old (now incorrect) transfer
count and/or buffer address, producing garbled DShot frames.

Fix: add a bounded wait loop for EN to clear before reconfiguring. If EN
has not cleared after the timeout (indicating a hardware fault), skip the
reconfiguration entirely rather than proceeding with stale register values.

This race is most visible on STM32F765 targets, where the larger 16 KB
I-Cache keeps the interrupt handler hot path in cache, resulting in faster
execution that consistently lands within the ~5-18 ns EN clear window.
On STM32F745 (4 KB I-Cache), cache misses add stall cycles that naturally
extend the window enough for EN to clear before reconfiguration begins.
---
 src/main/drivers/timer_impl_hal.c | 13 +++++++++++++
 1 file changed, 13 insertions(+)

diff --git a/src/main/drivers/timer_impl_hal.c b/src/main/drivers/timer_impl_hal.c
index 8df0f7024d3..090a383c4d1 100644
--- a/src/main/drivers/timer_impl_hal.c
+++ b/src/main/drivers/timer_impl_hal.c
@@ -528,6 +528,19 @@ void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferElementCount)
         DMA_CLEAR_FLAG(tch->dma, DMA_IT_TCIF);
     }
 
+    // Wait for EN bit to actually clear before reconfiguring DMA registers.
+    // Per STM32F7 RM: writes to DMA_SxNDTR and DMA_SxM0AR are ignored while EN=1.
+    // The EN bit does not clear synchronously - hardware may still be completing an
+    // in-progress burst when software writes 0 to EN.
+    uint32_t timeout = 10000;
+    while (LL_DMA_IsEnabledStream(dmaBase, streamLL) && timeout--) {
+        __NOP();
+    }
+    if (LL_DMA_IsEnabledStream(dmaBase, streamLL)) {
+        // EN did not clear - skip reconfiguration to avoid silent data corruption.
+        return;
+    }
+
     LL_DMA_SetDataLength(dmaBase, streamLL, dmaBufferElementCount);
     LL_DMA_ConfigAddresses(dmaBase, streamLL, (uint32_t)tch->dmaBuffer, (uint32_t)impl_timerCCR(tch), LL_DMA_DIRECTION_MEMORY_TO_PERIPH);
     LL_DMA_EnableIT_TC(dmaBase, streamLL);

From 2d60aa101426dffadfd7e2bc82a7aedbbecf9869 Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Mon, 2 Mar 2026 16:09:08 -0600
Subject: [PATCH 030/102] =?UTF-8?q?target:=20fix=20STM32F765xG=20linker=20?=
 =?UTF-8?q?scripts=20=E2=80=94=20correct=20DTCM=20size=20and=20SRAM1=20add?=
 =?UTF-8?q?ress?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

The three stm32_flash_f765xg*.ld linker scripts were copy-pasted from the
STM32F745 linker without updating the memory map for the F765's larger DTCM.

STM32F745: DTCM = 64 KB (0x20000000–0x2000FFFF), SRAM1 starts at 0x20010000
STM32F765: DTCM = 128 KB (0x20000000–0x2001FFFF), SRAM1 starts at 0x20020000

The F765xG linker scripts kept the F745 values (TCM = 64 KB, RAM at
0x20010000), which places the RAM region inside the upper half of DTCM on
F765 hardware rather than in SRAM1.

Fix all three xG variants (normal, bl, for_bl):
- Expand DTCM_RAM from 64 KB to 128 KB (the full F765 DTCM)
- Move SRAM1 origin from 0x20010000 to 0x20020000 (correct F765 address)
- Add SRAM2 region (16 KB at 0x2007C000) matching the xi scripts
- Update REGION_ALIAS("RAM") to point to SRAM1
- Update stale file headers that still referenced STM32F745VGTx / 320 KB RAM
- Update flash sector comment from "32K on F74x" to "32K on F7xx"

The xi variant linker scripts (used by MATEKF765, MATEKF765SE) already had
the correct addresses and are not changed.

Affected targets: FRSKYPILOT, FRSKYPILOT_LED.
---
 src/main/target/link/stm32_flash_f765xg.ld    | 22 +++++++++++--------
 src/main/target/link/stm32_flash_f765xg_bl.ld | 22 +++++++++++--------
 .../target/link/stm32_flash_f765xg_for_bl.ld  | 22 +++++++++++--------
 3 files changed, 39 insertions(+), 27 deletions(-)

diff --git a/src/main/target/link/stm32_flash_f765xg.ld b/src/main/target/link/stm32_flash_f765xg.ld
index a83374e4e41..ea548ed4ff3 100644
--- a/src/main/target/link/stm32_flash_f765xg.ld
+++ b/src/main/target/link/stm32_flash_f765xg.ld
@@ -1,10 +1,10 @@
 /*
 *****************************************************************************
 **
-**  File        : stm32_flash_f745.ld
+**  File        : stm32_flash_f765xg.ld
 **
-**  Abstract    : Linker script for STM32F745VGTx Device with
-**                1024KByte FLASH, 320KByte RAM
+**  Abstract    : Linker script for STM32F765xGTx Device with
+**                1024KByte FLASH, 512KByte RAM
 **
 *****************************************************************************
 */
@@ -29,7 +29,7 @@ MEMORY
 {
     ITCM_RAM (rx)			: ORIGIN = 0x00000000, LENGTH = 16K
     ITCM_FLASH (rx)        	: ORIGIN = 0x00200000, LENGTH = 32K
-    /* config occupies the entire flash sector 1 for the ease of erasure, 32K on F74x */
+    /* config occupies the entire flash sector 1 for the ease of erasure, 32K on F7xx */
     ITCM_FLASH_CONFIG (r)  	: ORIGIN = 0x00208000, LENGTH = 32K
     ITCM_FLASH1 (rx)       	: ORIGIN = 0x00210000, LENGTH = 960K
 
@@ -37,12 +37,16 @@ MEMORY
     FLASH_CONFIG (r)  : ORIGIN = 0x08008000, LENGTH = 32K
     FLASH1 (rx)       : ORIGIN = 0x08010000, LENGTH = 960K
 
-    TCM (rwx)         : ORIGIN = 0x20000000, LENGTH = 64K
-    RAM (rwx)         : ORIGIN = 0x20010000, LENGTH = 256K
+    DTCM_RAM (rwx)    : ORIGIN = 0x20000000, LENGTH = 128K
+    SRAM1 (rwx)       : ORIGIN = 0x20020000, LENGTH = 368K
+    SRAM2 (rwx)       : ORIGIN = 0x2007C000, LENGTH = 16K
     MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
 }
-/* note CCM could be used for stack */
-REGION_ALIAS("STACKRAM", TCM)
-REGION_ALIAS("FASTRAM", TCM)
+/* STM32F765 DTCM is 128KB (0x20000000-0x2001FFFF); SRAM1 starts at 0x20020000.
+ * DMA cannot access DTCM on STM32F7 - all DMA buffers must be in SRAM1 or SRAM2.
+ * NOTE: F745 DTCM is only 64KB, so F745 SRAM1 starts at 0x20010000 (different!). */
+REGION_ALIAS("STACKRAM", DTCM_RAM)
+REGION_ALIAS("FASTRAM",  DTCM_RAM)
+REGION_ALIAS("RAM",      SRAM1)
 
 INCLUDE "stm32_flash_f7_split.ld"
diff --git a/src/main/target/link/stm32_flash_f765xg_bl.ld b/src/main/target/link/stm32_flash_f765xg_bl.ld
index d357aad98d8..92f3d36754b 100644
--- a/src/main/target/link/stm32_flash_f765xg_bl.ld
+++ b/src/main/target/link/stm32_flash_f765xg_bl.ld
@@ -1,10 +1,10 @@
 /*
 *****************************************************************************
 **
-**  File        : stm32_flash_f745.ld
+**  File        : stm32_flash_f765xg_bl.ld
 **
-**  Abstract    : Linker script for STM32F745VGTx Device with
-**                1024KByte FLASH, 320KByte RAM
+**  Abstract    : Linker script for STM32F765xGTx Device with
+**                1024KByte FLASH, 512KByte RAM (bootloader)
 **
 *****************************************************************************
 */
@@ -29,7 +29,7 @@ MEMORY
 {
     ITCM_RAM (rx)               : ORIGIN = 0x00000000, LENGTH = 16K
     ITCM_FLASH (rx)             : ORIGIN = 0x00200000, LENGTH = 32K
-    /* config occupies the entire flash sector 1 for the ease of erasure, 32K on F74x */
+    /* config occupies the entire flash sector 1 for the ease of erasure, 32K on F7xx */
     ITCM_FLASH_CONFIG (r)       : ORIGIN = 0x00208000, LENGTH = 32K
     ITCM_FLASH1 (rx)            : ORIGIN = 0x00210000, LENGTH = 928K
 
@@ -37,13 +37,17 @@ MEMORY
     FIRMWARE (rx)               : ORIGIN = 0x08008000, LENGTH = 32K
     FLASH_CONFIG (r)            : ORIGIN = 0x08010000, LENGTH = 32K
 
-    TCM (rwx)                   : ORIGIN = 0x20000000, LENGTH = 64K
-    RAM (rwx)                   : ORIGIN = 0x20010000, LENGTH = 256K
+    DTCM_RAM (rwx)              : ORIGIN = 0x20000000, LENGTH = 128K
+    SRAM1 (rwx)                 : ORIGIN = 0x20020000, LENGTH = 368K
+    SRAM2 (rwx)                 : ORIGIN = 0x2007C000, LENGTH = 16K
     MEMORY_B1 (rx)              : ORIGIN = 0x60000000, LENGTH = 0K
 }
-/* note CCM could be used for stack */
-REGION_ALIAS("STACKRAM", TCM)
-REGION_ALIAS("FASTRAM", TCM)
+/* STM32F765 DTCM is 128KB (0x20000000-0x2001FFFF); SRAM1 starts at 0x20020000.
+ * DMA cannot access DTCM on STM32F7 - all DMA buffers must be in SRAM1 or SRAM2.
+ * NOTE: F745 DTCM is only 64KB, so F745 SRAM1 starts at 0x20010000 (different!). */
+REGION_ALIAS("STACKRAM", DTCM_RAM)
+REGION_ALIAS("FASTRAM",  DTCM_RAM)
+REGION_ALIAS("RAM",      SRAM1)
 
 __firmware_start = ORIGIN(FIRMWARE);
 
diff --git a/src/main/target/link/stm32_flash_f765xg_for_bl.ld b/src/main/target/link/stm32_flash_f765xg_for_bl.ld
index c7667d1dc26..b50247b015d 100644
--- a/src/main/target/link/stm32_flash_f765xg_for_bl.ld
+++ b/src/main/target/link/stm32_flash_f765xg_for_bl.ld
@@ -1,10 +1,10 @@
 /*
 *****************************************************************************
 **
-**  File        : stm32_flash_f745.ld
+**  File        : stm32_flash_f765xg_for_bl.ld
 **
-**  Abstract    : Linker script for STM32F745VGTx Device with
-**                1024KByte FLASH, 320KByte RAM
+**  Abstract    : Linker script for STM32F765xGTx Device with
+**                1024KByte FLASH, 512KByte RAM (bootloader-aware)
 **
 *****************************************************************************
 */
@@ -29,7 +29,7 @@ MEMORY
 {
     ITCM_RAM (rx)               : ORIGIN = 0x00000000, LENGTH = 16K
     ITCM_FLASH (rx)             : ORIGIN = 0x00200000, LENGTH = 32K
-    /* config occupies the entire flash sector 1 for the ease of erasure, 32K on F74x */
+    /* config occupies the entire flash sector 1 for the ease of erasure, 32K on F7xx */
     ITCM_FLASH_CONFIG (r)       : ORIGIN = 0x00208000, LENGTH = 32K
     ITCM_FLASH1 (rx)            : ORIGIN = 0x00210000, LENGTH = 960K
 
@@ -37,13 +37,17 @@ MEMORY
     FLASH_CONFIG (r)            : ORIGIN = 0x08010000, LENGTH = 32K
     FLASH1 (rx)                 : ORIGIN = 0x08018000, LENGTH = 928K
 
-    TCM (rwx)                   : ORIGIN = 0x20000000, LENGTH = 64K
-    RAM (rwx)                   : ORIGIN = 0x20010000, LENGTH = 256K
+    DTCM_RAM (rwx)              : ORIGIN = 0x20000000, LENGTH = 128K
+    SRAM1 (rwx)                 : ORIGIN = 0x20020000, LENGTH = 368K
+    SRAM2 (rwx)                 : ORIGIN = 0x2007C000, LENGTH = 16K
     MEMORY_B1 (rx)              : ORIGIN = 0x60000000, LENGTH = 0K
 }
-/* note CCM could be used for stack */
-REGION_ALIAS("STACKRAM", TCM)
-REGION_ALIAS("FASTRAM", TCM)
+/* STM32F765 DTCM is 128KB (0x20000000-0x2001FFFF); SRAM1 starts at 0x20020000.
+ * DMA cannot access DTCM on STM32F7 - all DMA buffers must be in SRAM1 or SRAM2.
+ * NOTE: F745 DTCM is only 64KB, so F745 SRAM1 starts at 0x20010000 (different!). */
+REGION_ALIAS("STACKRAM", DTCM_RAM)
+REGION_ALIAS("FASTRAM",  DTCM_RAM)
+REGION_ALIAS("RAM",      SRAM1)
 
 __firmware_start = ORIGIN(FLASH);
 

From 52ba52bc6f6d93cf82405461d46af6a15d796d16 Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Mon, 2 Mar 2026 16:43:33 -0600
Subject: [PATCH 031/102] =?UTF-8?q?drivers:=20fix=20DMA=20EN-bit=20timeout?=
 =?UTF-8?q?=20path=20=E2=80=94=20set=20dmaState=20IDLE=20and=20avoid=20cou?=
 =?UTF-8?q?nter=20underflow?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Two issues in the EN-bit wait loop added to impl_timerPWMPrepareDMA():

1. On timeout, the early return skipped setting tch->dmaState, leaving it in
   whatever state it had on entry (could be TCH_DMA_ACTIVE if the DMA TC
   interrupt was suppressed by the preceding ATOMIC_BLOCK). Subsequent calls
   to impl_timerPWMStartDMA() check for TCH_DMA_READY, so an ACTIVE or stale
   state would not cause a spurious fire, but timer.c timerIsMotorBusy() uses
   dmaState != TCH_DMA_IDLE as a busy check, which would return true forever
   on a stuck channel. Fix: explicitly set TCH_DMA_IDLE on timeout.

2. The while-loop condition used post-decrement (timeout--), causing the
   counter to wrap from 0 to UINT32_MAX on the final iteration. The counter
   is not used after the loop so this was harmless, but the pattern is fragile
   and easy to misread. Fix: use a for-loop where timeout decrements cleanly
   to 0 and the loop exit condition is unambiguous.
---
 src/main/drivers/timer_impl_hal.c | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/src/main/drivers/timer_impl_hal.c b/src/main/drivers/timer_impl_hal.c
index 090a383c4d1..a2cd24ae69e 100644
--- a/src/main/drivers/timer_impl_hal.c
+++ b/src/main/drivers/timer_impl_hal.c
@@ -532,12 +532,13 @@ void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferElementCount)
     // Per STM32F7 RM: writes to DMA_SxNDTR and DMA_SxM0AR are ignored while EN=1.
     // The EN bit does not clear synchronously - hardware may still be completing an
     // in-progress burst when software writes 0 to EN.
-    uint32_t timeout = 10000;
-    while (LL_DMA_IsEnabledStream(dmaBase, streamLL) && timeout--) {
+    for (uint32_t timeout = 10000; timeout && LL_DMA_IsEnabledStream(dmaBase, streamLL); timeout--) {
         __NOP();
     }
     if (LL_DMA_IsEnabledStream(dmaBase, streamLL)) {
-        // EN did not clear - skip reconfiguration to avoid silent data corruption.
+        // EN did not clear - hardware fault. Set channel idle so it is skipped
+        // on the next StartDMA call rather than being left in an ambiguous state.
+        tch->dmaState = TCH_DMA_IDLE;
         return;
     }
 

From 0735c57fe79ed8e2560e8edba72c7a3b8c57fade Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Mon, 2 Mar 2026 16:53:25 -0600
Subject: [PATCH 032/102] =?UTF-8?q?drivers:=20clarify=20EN-bit=20timeout?=
 =?UTF-8?q?=20comment=20=E2=80=94=20skipped=20frame,=20not=20hardware=20fa?=
 =?UTF-8?q?ult?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

The timeout (~140-230 us at 216 MHz) is long enough that any in-progress
DMA transfer has certainly completed or been aborted by the time it expires.
The stuck EN bit means we cannot reconfigure DMA registers this cycle, not
that the hardware is permanently broken. Update the comment to reflect that:
the ESC holds its last command for one missed frame, and EN will almost
certainly have cleared before the next PrepareDMA call (~1-2 ms later).
---
 src/main/drivers/timer_impl_hal.c | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/src/main/drivers/timer_impl_hal.c b/src/main/drivers/timer_impl_hal.c
index a2cd24ae69e..b61ec6813fa 100644
--- a/src/main/drivers/timer_impl_hal.c
+++ b/src/main/drivers/timer_impl_hal.c
@@ -536,8 +536,12 @@ void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferElementCount)
         __NOP();
     }
     if (LL_DMA_IsEnabledStream(dmaBase, streamLL)) {
-        // EN did not clear - hardware fault. Set channel idle so it is skipped
-        // on the next StartDMA call rather than being left in an ambiguous state.
+        // EN did not clear within the timeout (~140-230 us at 216 MHz). Any in-progress
+        // transfer has long since completed or been aborted, so no data corruption risk
+        // remains - but we cannot safely reconfigure the DMA registers this cycle.
+        // Skip this frame (ESC holds its last command) and mark the channel IDLE so
+        // StartDMA passes over it. EN will almost certainly have cleared by the next
+        // PrepareDMA call (~1-2 ms later), allowing normal operation to resume.
         tch->dmaState = TCH_DMA_IDLE;
         return;
     }

From 76cc064fc0e417f0da8116628aad387d92405322 Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Mon, 2 Mar 2026 16:55:00 -0600
Subject: [PATCH 033/102] =?UTF-8?q?drivers:=20tighten=20EN-bit=20timeout?=
 =?UTF-8?q?=20comment=20=E2=80=94=20concise=20and=20accurate?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/main/drivers/timer_impl_hal.c | 8 ++------
 1 file changed, 2 insertions(+), 6 deletions(-)

diff --git a/src/main/drivers/timer_impl_hal.c b/src/main/drivers/timer_impl_hal.c
index b61ec6813fa..93ebec5de86 100644
--- a/src/main/drivers/timer_impl_hal.c
+++ b/src/main/drivers/timer_impl_hal.c
@@ -536,12 +536,8 @@ void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferElementCount)
         __NOP();
     }
     if (LL_DMA_IsEnabledStream(dmaBase, streamLL)) {
-        // EN did not clear within the timeout (~140-230 us at 216 MHz). Any in-progress
-        // transfer has long since completed or been aborted, so no data corruption risk
-        // remains - but we cannot safely reconfigure the DMA registers this cycle.
-        // Skip this frame (ESC holds its last command) and mark the channel IDLE so
-        // StartDMA passes over it. EN will almost certainly have cleared by the next
-        // PrepareDMA call (~1-2 ms later), allowing normal operation to resume.
+        // EN did not clear - cannot reconfigure this cycle. Skip frame (ESC holds
+        // last command); EN should clear before the next call.
         tch->dmaState = TCH_DMA_IDLE;
         return;
     }

From 3b183b3555eb44ca0a0a8bf04628869db928168b Mon Sep 17 00:00:00 2001
From: Daria Cacic <daria.cacic@orqafpv.com>
Date: Wed, 4 Mar 2026 09:38:04 +0100
Subject: [PATCH 034/102] added changes to max7456.c to add setting speed and
 removing spi_mode_0 flag from target.h file

---
 src/main/drivers/max7456.c | 29 ++++++++++++++++++++++++++++-
 1 file changed, 28 insertions(+), 1 deletion(-)

diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c
index 4d541d32027..3d75ffcd8e6 100644
--- a/src/main/drivers/max7456.c
+++ b/src/main/drivers/max7456.c
@@ -293,6 +293,31 @@ static int max7456PrepareBuffer(uint8_t * buf, size_t bufsize, int bufPtr, uint8
     return bufPtr;
 }
 
+void max7456ApplyBusSpeed(void)
+{
+#if defined(MAX7456_SPI_SPEED)
+    busSetSpeed(state.dev, MAX7456_SPI_SPEED);
+#else
+    // Default safe speed for MAX7456
+    busSetSpeed(state.dev, BUS_SPEED_STANDARD);
+#endif
+}
+
+// Used when standard DEVFLAGS_SPI_MODE_0 is omitted in common_hardware 
+void max7456SpiModeOverride(void)
+{
+#if defined(STM32H7) && defined(MAX7456_MANUAL_SPI_CONFIG)
+            SPI_TypeDef *maxSpiInstance = spiInstanceByDevice(state.dev->busdev.spi.spiBus);
+            if (!maxSpiInstance) return;
+            maxSpiInstance->CR1 &= ~SPI_CR1_SPE;
+            maxSpiInstance->CFG2 &= ~(SPI_CFG2_CPHA | SPI_CFG2_CPOL);
+
+            maxSpiInstance->CFG2 |= (SPI_CFG2_CPHA | SPI_CFG2_CPOL);
+            maxSpiInstance->CR1 |= SPI_CR1_SPE;
+#endif
+}
+
+
 uint16_t max7456GetScreenSize(void)
 {
     // Default to PAL while the display is not yet initialized. This
@@ -386,7 +411,9 @@ void max7456Init(const videoSystem_e videoSystem)
         return;
     }
 
-    busSetSpeed(state.dev, BUS_SPEED_STANDARD);
+    max7456ApplyBusSpeed();
+
+    max7456SpiModeOverride(); 
 
     // force soft reset on Max7456
     busWrite(state.dev, MAX7456ADD_VM0, MAX7456_RESET);

From ada5eb9bd334735a8a7e4d8c0bb15e7941784b55 Mon Sep 17 00:00:00 2001
From: Daria Cacic <daria.cacic@orqafpv.com>
Date: Wed, 4 Mar 2026 10:05:48 +0100
Subject: [PATCH 035/102] changed spacing and added static to function

---
 src/main/drivers/max7456.c | 22 +++++++++++-----------
 1 file changed, 11 insertions(+), 11 deletions(-)

diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c
index 3d75ffcd8e6..6914a3ea2bb 100644
--- a/src/main/drivers/max7456.c
+++ b/src/main/drivers/max7456.c
@@ -293,7 +293,7 @@ static int max7456PrepareBuffer(uint8_t * buf, size_t bufsize, int bufPtr, uint8
     return bufPtr;
 }
 
-void max7456ApplyBusSpeed(void)
+static void max7456ApplyBusSpeed(void)
 {
 #if defined(MAX7456_SPI_SPEED)
     busSetSpeed(state.dev, MAX7456_SPI_SPEED);
@@ -304,20 +304,21 @@ void max7456ApplyBusSpeed(void)
 }
 
 // Used when standard DEVFLAGS_SPI_MODE_0 is omitted in common_hardware 
-void max7456SpiModeOverride(void)
+static void max7456SpiModeOverride(void)
 {
 #if defined(STM32H7) && defined(MAX7456_MANUAL_SPI_CONFIG)
-            SPI_TypeDef *maxSpiInstance = spiInstanceByDevice(state.dev->busdev.spi.spiBus);
-            if (!maxSpiInstance) return;
-            maxSpiInstance->CR1 &= ~SPI_CR1_SPE;
-            maxSpiInstance->CFG2 &= ~(SPI_CFG2_CPHA | SPI_CFG2_CPOL);
-
-            maxSpiInstance->CFG2 |= (SPI_CFG2_CPHA | SPI_CFG2_CPOL);
-            maxSpiInstance->CR1 |= SPI_CR1_SPE;
+    SPI_TypeDef *maxSpiInstance = spiInstanceByDevice(state.dev->busdev.spi.spiBus);
+    if (!maxSpiInstance){
+        return;
+    }
+    
+    maxSpiInstance->CR1 &= ~SPI_CR1_SPE;
+    maxSpiInstance->CFG2 &= ~(SPI_CFG2_CPHA | SPI_CFG2_CPOL);
+    maxSpiInstance->CFG2 |= (SPI_CFG2_CPHA | SPI_CFG2_CPOL);
+    maxSpiInstance->CR1 |= SPI_CR1_SPE;
 #endif
 }
 
-
 uint16_t max7456GetScreenSize(void)
 {
     // Default to PAL while the display is not yet initialized. This
@@ -412,7 +413,6 @@ void max7456Init(const videoSystem_e videoSystem)
     }
 
     max7456ApplyBusSpeed();
-
     max7456SpiModeOverride(); 
 
     // force soft reset on Max7456

From 8ba58ccc5f4b5f820760baa0a666ce2b17f43524 Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Wed, 4 Mar 2026 12:05:51 -0600
Subject: [PATCH 036/102] frskyf405: Fix crystal frequency and accel alignment

---
 src/main/target/FRSKYF405/CMakeLists.txt | 2 +-
 src/main/target/FRSKYF405/target.h       | 4 ++--
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/target/FRSKYF405/CMakeLists.txt b/src/main/target/FRSKYF405/CMakeLists.txt
index fc8eb478bcc..0e382d8acb7 100644
--- a/src/main/target/FRSKYF405/CMakeLists.txt
+++ b/src/main/target/FRSKYF405/CMakeLists.txt
@@ -1 +1 @@
-target_stm32f405xg(FRSKYF405)
+target_stm32f405xg(FRSKYF405 HSE_MHZ 24)
diff --git a/src/main/target/FRSKYF405/target.h b/src/main/target/FRSKYF405/target.h
index 146519a99b8..12e61c5f29a 100644
--- a/src/main/target/FRSKYF405/target.h
+++ b/src/main/target/FRSKYF405/target.h
@@ -17,7 +17,7 @@
 
 #pragma once
 
-#define TARGET_BOARD_IDENTIFIER "FRF4"
+#define TARGET_BOARD_IDENTIFIER "FR45"
 
 #define USBD_PRODUCT_STRING  "FrSkyF405"
 
@@ -40,7 +40,7 @@
 #define USE_IMU_ICM42605
 #define ICM42605_CS_PIN        PA4
 #define ICM42605_SPI_BUS       BUS_SPI1
-#define IMU_ICM42605_ALIGN     CW0_DEG
+#define IMU_ICM42605_ALIGN     CW180_DEG_FLIP
 
 
 // *************** OSD *****************************

From a294302305add795ebde5888d7fea85161c7a81c Mon Sep 17 00:00:00 2001
From: Daria Cacic <daria.cacic@orqafpv.com>
Date: Thu, 5 Mar 2026 13:10:44 +0100
Subject: [PATCH 037/102] added bounds check for speed and fixed spi re-enable
 issue

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

diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c
index 6914a3ea2bb..9cb2750de4d 100644
--- a/src/main/drivers/max7456.c
+++ b/src/main/drivers/max7456.c
@@ -296,7 +296,8 @@ static int max7456PrepareBuffer(uint8_t * buf, size_t bufsize, int bufPtr, uint8
 static void max7456ApplyBusSpeed(void)
 {
 #if defined(MAX7456_SPI_SPEED)
-    busSetSpeed(state.dev, MAX7456_SPI_SPEED);
+    busSpeed_e speed = (MAX7456_SPI_SPEED <= BUS_SPEED_ULTRAFAST) ? MAX7456_SPI_SPEED : BUS_SPEED_STANDARD;
+    busSetSpeed(state.dev, speed);
 #else
     // Default safe speed for MAX7456
     busSetSpeed(state.dev, BUS_SPEED_STANDARD);
@@ -311,11 +312,9 @@ static void max7456SpiModeOverride(void)
     if (!maxSpiInstance){
         return;
     }
-    
+   
     maxSpiInstance->CR1 &= ~SPI_CR1_SPE;
-    maxSpiInstance->CFG2 &= ~(SPI_CFG2_CPHA | SPI_CFG2_CPOL);
     maxSpiInstance->CFG2 |= (SPI_CFG2_CPHA | SPI_CFG2_CPOL);
-    maxSpiInstance->CR1 |= SPI_CR1_SPE;
 #endif
 }
 

From a9fb044a395500a453d3be14afbbc3ce89130a77 Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Fri, 6 Mar 2026 10:13:23 -0600
Subject: [PATCH 038/102] osd: replace common tfp_sprintf patterns with direct
 formatting helpers

Replace 81 tfp_sprintf calls in osd.c with lightweight helpers that
skip format-string parsing overhead:

- osdFormatIntUnit: replaces "%Nd" and "%Nd%c" patterns (47 calls)
- osdWriteChar/osdWriteChar2: replaces "%c" and "%c%c" (33 calls)
- osdFormatTime_MMSS: replaces "%02d:%02d" pattern (2 calls)

Removes one dead null-terminator write and collapses a redundant
RSSI_DBM branch that became identical after conversion.
---
 src/main/io/osd.c | 212 +++++++++++++++++++++++++++-------------------
 1 file changed, 127 insertions(+), 85 deletions(-)

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index f4fe19a4040..2b91a83220f 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -228,6 +228,53 @@ static bool osdDisplayHasCanvas;
 PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 15);
 PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 3);
 
+/* OSD formatting helpers replacing common tfp_sprintf patterns
+ * for reduced code size and CPU overhead. */
+
+static int i2a_len(int num, char *bf)
+{
+    i2a(num, bf);
+    return (int)strlen(bf);
+}
+
+static void osdFormatIntUnit(char *buff, int width, int value, char unit)
+{
+    char tmp[12];
+    int len = i2a_len(value, tmp);
+    int pad = width - len;
+    int i = 0;
+    while (pad-- > 0) buff[i++] = ' ';
+    const char *src = tmp;
+    while (*src) buff[i++] = *src++;
+    if (unit) buff[i++] = unit;
+    buff[i] = '\0';
+}
+
+static inline void osdWriteChar(char *buff, char c)
+{
+    buff[0] = c;
+    buff[1] = '\0';
+}
+
+static inline void osdWriteChar2(char *buff, char c1, char c2)
+{
+    buff[0] = c1;
+    buff[1] = c2;
+    buff[2] = '\0';
+}
+
+static void osdFormatTime_MMSS(char *buff, int m, int s)
+{
+    m %= 100; // clamp to two digits to prevent writing non-ASCII to display
+    s %= 100;
+    buff[0] = '0' + m / 10;
+    buff[1] = '0' + m % 10;
+    buff[2] = ':';
+    buff[3] = '0' + s / 10;
+    buff[4] = '0' + s % 10;
+    buff[5] = '\0';
+}
+
 void osdStartedSaveProcess(void) {
     savingSettings = true;
 }
@@ -660,7 +707,7 @@ static void osdFormatTime(char *buff, uint32_t seconds, char sym_m, char sym_h)
         value = seconds / 60;
     }
     buff[0] = sym;
-    tfp_sprintf(buff + 1, "%02d:%02d", (int)(value / 60), (int)(value % 60));
+    osdFormatTime_MMSS(buff + 1, (int)(value / 60), (int)(value % 60));
 }
 
 static inline void osdFormatOnTime(char *buff)
@@ -771,7 +818,7 @@ static void osdDisplayTemperature(uint8_t elemPosX, uint8_t elemPosY, uint16_t s
 
         if ((temperature <= alarm_min) || (temperature >= alarm_max)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
         if (osdConfig()->units == OSD_UNIT_IMPERIAL) temperature = temperature * 9 / 5.0f + 320;
-        tfp_sprintf(buff, "%3d", temperature / 10);
+        osdFormatIntUnit(buff, 3, temperature / 10, 0);
 
     } else
         strcpy(buff, "---");
@@ -1205,7 +1252,7 @@ static void osdFormatThrottlePosition(char *buff, bool useScaled, textAttributes
         strcpy(buff + 1, message);
         return;
     }
-    tfp_sprintf(buff + 2, "%3d", throttlePercent);
+    osdFormatIntUnit(buff + 2, 3, throttlePercent, 0);
 }
 
 /**
@@ -1653,25 +1700,25 @@ static void osdDisplayFlightPIDValues(uint8_t elemPosX, uint8_t elemPosY, const
     }
 
     elemAttr = TEXT_ATTRIBUTES_NONE;
-    tfp_sprintf(buff, "%3d", pid->P);
+    osdFormatIntUnit(buff, 3, pid->P, 0);
     if ((isAdjustmentFunctionSelected(adjFuncP)) || (((adjFuncP == ADJUSTMENT_ROLL_P) || (adjFuncP == ADJUSTMENT_PITCH_P)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_P))))
         TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
     displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr);
 
     elemAttr = TEXT_ATTRIBUTES_NONE;
-    tfp_sprintf(buff, "%3d", pid->I);
+    osdFormatIntUnit(buff, 3, pid->I, 0);
     if ((isAdjustmentFunctionSelected(adjFuncI)) || (((adjFuncI == ADJUSTMENT_ROLL_I) || (adjFuncI == ADJUSTMENT_PITCH_I)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_I))))
         TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
     displayWriteWithAttr(osdDisplayPort, elemPosX + 8, elemPosY, buff, elemAttr);
 
     elemAttr = TEXT_ATTRIBUTES_NONE;
-    tfp_sprintf(buff, "%3d", pid->D);
+    osdFormatIntUnit(buff, 3, pid->D, 0);
     if ((isAdjustmentFunctionSelected(adjFuncD)) || (((adjFuncD == ADJUSTMENT_ROLL_D) || (adjFuncD == ADJUSTMENT_PITCH_D)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_D))))
         TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
     displayWriteWithAttr(osdDisplayPort, elemPosX + 12, elemPosY, buff, elemAttr);
 
     elemAttr = TEXT_ATTRIBUTES_NONE;
-    tfp_sprintf(buff, "%3d", pid->FF);
+    osdFormatIntUnit(buff, 3, pid->FF, 0);
     if ((isAdjustmentFunctionSelected(adjFuncFF)) || (((adjFuncFF == ADJUSTMENT_ROLL_FF) || (adjFuncFF == ADJUSTMENT_PITCH_FF)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_FF))))
         TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
     displayWriteWithAttr(osdDisplayPort, elemPosX + 16, elemPosY, buff, elemAttr);
@@ -1695,19 +1742,19 @@ static void osdDisplayNavPIDValues(uint8_t elemPosX, uint8_t elemPosY, const cha
     }
 
     elemAttr = TEXT_ATTRIBUTES_NONE;
-    tfp_sprintf(buff, "%3d", pid->P);
+    osdFormatIntUnit(buff, 3, pid->P, 0);
     if ((isAdjustmentFunctionSelected(adjFuncP)) || (((adjFuncP == ADJUSTMENT_ROLL_P) || (adjFuncP == ADJUSTMENT_PITCH_P)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_P))))
         TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
     displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr);
 
     elemAttr = TEXT_ATTRIBUTES_NONE;
-    tfp_sprintf(buff, "%3d", pid->I);
+    osdFormatIntUnit(buff, 3, pid->I, 0);
     if ((isAdjustmentFunctionSelected(adjFuncI)) || (((adjFuncI == ADJUSTMENT_ROLL_I) || (adjFuncI == ADJUSTMENT_PITCH_I)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_I))))
         TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
     displayWriteWithAttr(osdDisplayPort, elemPosX + 8, elemPosY, buff, elemAttr);
 
     elemAttr = TEXT_ATTRIBUTES_NONE;
-    tfp_sprintf(buff, "%3d", pidType == PID_TYPE_PIFF ? pid->FF : pid->D);
+    osdFormatIntUnit(buff, 3, pidType == PID_TYPE_PIFF ? pid->FF : pid->D, 0);
     if ((isAdjustmentFunctionSelected(adjFuncD)) || (((adjFuncD == ADJUSTMENT_ROLL_D) || (adjFuncD == ADJUSTMENT_PITCH_D)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_D))))
         TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
     displayWriteWithAttr(osdDisplayPort, elemPosX + 12, elemPosY, buff, elemAttr);
@@ -1849,7 +1896,7 @@ static bool osdDrawSingleElement(uint8_t item)
             uint8_t osdRssi = osdConvertRSSI();
             buff[0] = SYM_RSSI;
             if (osdRssi < 100)
-                tfp_sprintf(buff + 1, "%2d", osdRssi);
+                osdFormatIntUnit(buff + 1, 2, osdRssi, 0);
             else
                 tfp_sprintf(buff + 1, "%c ", SYM_MAX);
 
@@ -1899,7 +1946,7 @@ static bool osdDrawSingleElement(uint8_t item)
 #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
+            osdFormatIntUnit(buff, 5, (int)getMAhDrawn(), 0);   // Use 5 digits to allow packs below 100Ah
             buff[5] = SYM_MAH;
             buff[6] = '\0';
         } else
@@ -1940,7 +1987,7 @@ static bool osdDrawSingleElement(uint8_t item)
 #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
+                osdFormatIntUnit(buff, 5, (int)getBatteryRemainingCapacity(), 0);   // Use 5 digits to allow packs below 100Ah
                 buff[5] = SYM_MAH;
                 buff[6] = '\0';
                 unitsDrawn = true;
@@ -1979,7 +2026,7 @@ static bool osdDrawSingleElement(uint8_t item)
 
     case OSD_POWER_SUPPLY_IMPEDANCE:
         if (isPowerSupplyImpedanceValid())
-            tfp_sprintf(buff, "%3d", getPowerSupplyImpedance());
+            osdFormatIntUnit(buff, 3, getPowerSupplyImpedance(), 0);
         else
             strcpy(buff, "---");
         buff[3] = SYM_MILLIOHM;
@@ -1990,7 +2037,7 @@ static bool osdDrawSingleElement(uint8_t item)
     case OSD_GPS_SATS:
         buff[0] = SYM_SAT_L;
         buff[1] = SYM_SAT_R;
-        tfp_sprintf(buff + 2, "%2d", gpsSol.numSat);
+        osdFormatIntUnit(buff + 2, 2, gpsSol.numSat, 0);
 #ifdef USE_GPS_FIX_ESTIMATION
         if (STATE(GPS_ESTIMATED_FIX)) {
             strcpy(buff + 2, "ES");
@@ -2093,7 +2140,7 @@ static bool osdDrawSingleElement(uint8_t item)
 
             if (isImuHeadingValid() && navigationPositionEstimateIsHealthy()) {
                 int16_t h = lrintf(CENTIDEGREES_TO_DEGREES((float)wrap_18000(DEGREES_TO_CENTIDEGREES((int32_t)GPS_directionToHome) - (STATE(AIRPLANE) ? posControl.actualState.cog : DECIDEGREES_TO_CENTIDEGREES((int32_t)osdGetHeading())))));
-                tfp_sprintf(buff + 2, "%4d", h);
+                osdFormatIntUnit(buff + 2, 4, h, 0);
             } else {
                 strcpy(buff + 2, "----");
             }
@@ -2126,7 +2173,7 @@ static bool osdDrawSingleElement(uint8_t item)
 #ifdef USE_BLACKBOX
             if (IS_RC_MODE_ACTIVE(BOXBLACKBOX)) {
                 if (!isBlackboxDeviceWorking()) {
-                    tfp_sprintf(buff, "%c%c", SYM_BLACKBOX, SYM_ALERT);
+                    osdWriteChar2(buff, SYM_BLACKBOX, SYM_ALERT);
                 } else if (isBlackboxDeviceFull()) {
                     tfp_sprintf(buff, "%cFULL", SYM_BLACKBOX);
                 } else {
@@ -2134,7 +2181,7 @@ static bool osdDrawSingleElement(uint8_t item)
                     if (logNumber >= 0) {
                         tfp_sprintf(buff, "%c%05" PRId32, SYM_BLACKBOX, logNumber);
                     } else {
-                        tfp_sprintf(buff, "%c", SYM_BLACKBOX);
+                        osdWriteChar(buff, SYM_BLACKBOX);
                     }
                 }
             }
@@ -2178,7 +2225,7 @@ static bool osdDrawSingleElement(uint8_t item)
         {
             buff[0] = SYM_GROUND_COURSE;
             if (osdIsHeadingValid()) {
-                tfp_sprintf(&buff[1], "%3d", (int16_t)CENTIDEGREES_TO_DEGREES(posControl.actualState.cog));
+                osdFormatIntUnit(&buff[1], 3, (int16_t)CENTIDEGREES_TO_DEGREES(posControl.actualState.cog), 0);
             } else {
                 buff[1] = buff[2] = buff[3] = '-';
             }
@@ -2203,7 +2250,7 @@ static bool osdDrawSingleElement(uint8_t item)
                 if (ABS(herr) > 99)
                     strcpy(buff + 1, ">99");
                 else
-                    tfp_sprintf(buff + 1, "%3d", herr);
+                    osdFormatIntUnit(buff + 1, 3, herr, 0);
             }
 
             buff[4] = SYM_DEGREES;
@@ -2225,7 +2272,7 @@ static bool osdDrawSingleElement(uint8_t item)
             if (!ARMING_FLAG(ARMED)) {
                 buff[1] = buff[2] = buff[3] = buff[4] = '-';
             } else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
-                tfp_sprintf(buff + 1, "%4d", heading_adjust);
+                osdFormatIntUnit(buff + 1, 4, heading_adjust, 0);
             }
 
             buff[5] = SYM_DEGREES;
@@ -2372,7 +2419,7 @@ static bool osdDrawSingleElement(uint8_t item)
             } else if (!isImuHeadingValid()) {
                 buff[1] = 'H';
             } else {
-                tfp_sprintf(buff + 1, "%1d", getActiveVehiclesCount());
+                osdFormatIntUnit(buff + 1, 1, getActiveVehiclesCount(), 0);
             }
             break;
         }
@@ -2639,7 +2686,7 @@ static bool osdDrawSingleElement(uint8_t item)
             tfp_sprintf(buff, "CH:%c%s:", osdInfo.bandLetter, osdInfo.channelName);
             displayWrite(osdDisplayPort, elemPosX, elemPosY, buff);
 
-            tfp_sprintf(buff, "%c", osdInfo.powerIndexLetter);
+            osdWriteChar(buff, osdInfo.powerIndexLetter);
             if (isAdjustmentFunctionSelected(ADJUSTMENT_VTX_POWER_LEVEL)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
             displayWriteWithAttr(osdDisplayPort, elemPosX + 6, elemPosY, buff, elemAttr);
             return true;
@@ -2651,10 +2698,10 @@ static bool osdDrawSingleElement(uint8_t item)
             vtxDeviceOsdInfo_t osdInfo;
             vtxCommonGetOsdInfo(vtxCommonDevice(), &osdInfo);
 
-            tfp_sprintf(buff, "%c", SYM_VTX_POWER);
+            osdWriteChar(buff, SYM_VTX_POWER);
             displayWrite(osdDisplayPort, elemPosX, elemPosY, buff);
 
-            tfp_sprintf(buff, "%c", osdInfo.powerIndexLetter);
+            osdWriteChar(buff, osdInfo.powerIndexLetter);
             if (isAdjustmentFunctionSelected(ADJUSTMENT_VTX_POWER_LEVEL)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
             displayWriteWithAttr(osdDisplayPort, elemPosX+1, elemPosY, buff, elemAttr);
             return true;
@@ -2665,11 +2712,7 @@ static bool osdDrawSingleElement(uint8_t item)
         {
             int16_t rssi = rxLinkStatistics.uplinkRSSI;
             buff[0] = (rxLinkStatistics.activeAntenna == 0) ? SYM_RSSI : SYM_2RSS; // Separate symbols for each antenna
-            if (rssi <= -100) {
-                tfp_sprintf(buff + 1, "%4d%c", rssi, SYM_DBM);
-            } else {
-                tfp_sprintf(buff + 1, " %3d%c", rssi, SYM_DBM);
-            }
+            osdFormatIntUnit(buff + 1, 4, rssi, SYM_DBM);
             if (!failsafeIsReceivingRxData()){
                 TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
             } else if (osdConfig()->rssi_dbm_alarm && rssi < osdConfig()->rssi_dbm_alarm) {
@@ -2695,18 +2738,18 @@ static bool osdDrawSingleElement(uint8_t item)
                     break;
                 case OSD_CRSF_LQ_TYPE3:
                     if (!failsafeIsReceivingRxData()) {
-                        tfp_sprintf(buff+1, "%3d", 0);
+                        osdFormatIntUnit(buff+1, 3, 0, 0);
                     } else {
                         int16_t scaledLQ = scaleRange(constrain(rxLinkStatistics.uplinkLQ, 0, 100), 0, 100, 170, 300);
-                        tfp_sprintf(buff+1, "%3d", rxLinkStatistics.rfMode >= 2 ? scaledLQ : rxLinkStatistics.uplinkLQ);
+                        osdFormatIntUnit(buff+1, 3, rxLinkStatistics.rfMode >= 2 ? scaledLQ : rxLinkStatistics.uplinkLQ, 0);
                     }
                     break;
                 case OSD_CRSF_LQ_TYPE1:
                 default:
                     if (!failsafeIsReceivingRxData()) {
-                        tfp_sprintf(buff+1, "%3d", 0);
+                        osdFormatIntUnit(buff+1, 3, 0, 0);
                     } else {
-                        tfp_sprintf(buff+1, "%3d", rxLinkStatistics.uplinkLQ);
+                        osdFormatIntUnit(buff+1, 3, rxLinkStatistics.uplinkLQ, 0);
                     }
                     break;
             }
@@ -2722,9 +2765,9 @@ static bool osdDrawSingleElement(uint8_t item)
         {
             buff[0] = SYM_LQ;
             if (!failsafeIsReceivingRxData()) {
-                tfp_sprintf(buff+1, "%3d%c", 0, SYM_AH_DECORATION_DOWN);
+                osdFormatIntUnit(buff+1, 3, 0, SYM_AH_DECORATION_DOWN);
             } else {
-                tfp_sprintf(buff+1, "%3d%c", rxLinkStatistics.downlinkLQ, SYM_AH_DECORATION_DOWN);
+                osdFormatIntUnit(buff+1, 3, rxLinkStatistics.downlinkLQ, SYM_AH_DECORATION_DOWN);
             }
 
             if (!failsafeIsReceivingRxData()) {
@@ -2752,7 +2795,7 @@ static bool osdDrawSingleElement(uint8_t item)
             } else if (snrVal <= osdConfig()->snr_alarm) {
                 buff[0] = SYM_SNR;
                 if (snrVal <= -10) {
-                    tfp_sprintf(buff + 1, "%3d%c", snrVal, SYM_DB);
+                    osdFormatIntUnit(buff + 1, 3, snrVal, SYM_DB);
                 } else {
                     tfp_sprintf(buff + 1, "%2d%c%c", snrVal, SYM_DB, ' ');
                 }
@@ -2765,7 +2808,7 @@ static bool osdDrawSingleElement(uint8_t item)
             if (!failsafeIsReceivingRxData())
                 tfp_sprintf(buff, "%s%c", "    ", SYM_MW);
             else
-                tfp_sprintf(buff, "%4d%c", rxLinkStatistics.uplinkTXPower, SYM_MW);
+                osdFormatIntUnit(buff, 4, rxLinkStatistics.uplinkTXPower, SYM_MW);
             break;
         }
 
@@ -2889,7 +2932,7 @@ static bool osdDrawSingleElement(uint8_t item)
                 }
 
                 altc = ABS(constrain(altc, -999, 999));
-                tfp_sprintf(buff, "%3d", altc);
+                osdFormatIntUnit(buff, 3, altc, 0);
                 displayWrite(osdDisplayPort, elemPosX + 1, elemPosY + 2, buff);
 
                 return true;
@@ -3098,7 +3141,7 @@ static bool osdDrawSingleElement(uint8_t item)
                     // mAh/foot
                     if (efficiencyValid) {
                         osdFormatCentiNumber(buff, (value * METERS_PER_FOOT), 1, 2, 2, 3, false);
-                        tfp_sprintf(buff + strlen(buff), "%c%c", SYM_AH_V_FT_0, SYM_AH_V_FT_1);
+                        osdWriteChar2(buff + strlen(buff), SYM_AH_V_FT_0, SYM_AH_V_FT_1);
                     } else {
                         buff[0] = buff[1] = buff[2] = '-';
                         buff[3] = SYM_AH_V_FT_0;
@@ -3112,7 +3155,7 @@ static bool osdDrawSingleElement(uint8_t item)
                     // mAh/metre
                     if (efficiencyValid) {
                         osdFormatCentiNumber(buff, value, 1, 2, 2, 3, false);
-                        tfp_sprintf(buff + strlen(buff), "%c%c", SYM_AH_V_M_0, SYM_AH_V_M_1);
+                        osdWriteChar2(buff + strlen(buff), SYM_AH_V_M_0, SYM_AH_V_M_1);
                     } else {
                         buff[0] = buff[1] = buff[2] = '-';
                         buff[3] = SYM_AH_V_M_0;
@@ -3135,7 +3178,7 @@ static bool osdDrawSingleElement(uint8_t item)
                     buff[4] = SYM_DECORATION;
                     buff[5] = SYM_DECORATION;
                 } else {
-                    tfp_sprintf(buff + 1, "%02d:%02d", (int)(glideTime / 60), (int)(glideTime % 60));
+                    osdFormatTime_MMSS(buff + 1, (int)(glideTime / 60), (int)(glideTime % 60));
                 }
             } else {
                tfp_sprintf(buff + 1, "%s", "--:--");
@@ -3191,7 +3234,7 @@ static bool osdDrawSingleElement(uint8_t item)
                 }
 
                 if (osdConfig()->pan_servo_indicator_show_degrees) {
-                    tfp_sprintf(buff, "%3d%c", -panOffset, SYM_DEGREES);
+                    osdFormatIntUnit(buff, 3, -panOffset, SYM_DEGREES);
                     displayWriteWithAttr(osdDisplayPort, elemPosX+1, elemPosY, buff, elemAttr);
                 }
                 displayWriteCharWithAttr(osdDisplayPort, elemPosX, elemPosY, SYM_SERVO_PAN_IS_OFFSET_R, elemAttr);
@@ -3207,7 +3250,7 @@ static bool osdDrawSingleElement(uint8_t item)
                 }
 
                 if (osdConfig()->pan_servo_indicator_show_degrees) {
-                    tfp_sprintf(buff, "%3d%c", panOffset, SYM_DEGREES);
+                    osdFormatIntUnit(buff, 3, panOffset, SYM_DEGREES);
                     displayWriteWithAttr(osdDisplayPort, elemPosX+1, elemPosY, buff, elemAttr);
                 }
                 displayWriteCharWithAttr(osdDisplayPort, elemPosX, elemPosY, SYM_SERVO_PAN_IS_OFFSET_L, elemAttr);
@@ -3215,7 +3258,7 @@ static bool osdDrawSingleElement(uint8_t item)
                 panServoTimeOffCentre = 0;
 
                 if (osdConfig()->pan_servo_indicator_show_degrees) {
-                    tfp_sprintf(buff, "%3d%c", panOffset, SYM_DEGREES);
+                    osdFormatIntUnit(buff, 3, panOffset, SYM_DEGREES);
                     displayWriteWithAttr(osdDisplayPort, elemPosX+1, elemPosY, buff, elemAttr);
                 }
                 displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_SERVO_PAN_IS_CENTRED);
@@ -3294,7 +3337,7 @@ static bool osdDrawSingleElement(uint8_t item)
         displayWrite(osdDisplayPort, elemPosX, elemPosY, "SPR");
 
         elemAttr = TEXT_ATTRIBUTES_NONE;
-        tfp_sprintf(buff, "%3d", currentControlProfile->stabilized.rates[FD_PITCH]);
+        osdFormatIntUnit(buff, 3, currentControlProfile->stabilized.rates[FD_PITCH], 0);
         if (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_RATE))
             TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
         displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr);
@@ -3304,7 +3347,7 @@ static bool osdDrawSingleElement(uint8_t item)
         displayWrite(osdDisplayPort, elemPosX, elemPosY, "SRR");
 
         elemAttr = TEXT_ATTRIBUTES_NONE;
-        tfp_sprintf(buff, "%3d", currentControlProfile->stabilized.rates[FD_ROLL]);
+        osdFormatIntUnit(buff, 3, currentControlProfile->stabilized.rates[FD_ROLL], 0);
         if (isAdjustmentFunctionSelected(ADJUSTMENT_ROLL_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_RATE))
             TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
         displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr);
@@ -3326,7 +3369,7 @@ static bool osdDrawSingleElement(uint8_t item)
         displayWrite(osdDisplayPort, elemPosX, elemPosY, "MPR");
 
         elemAttr = TEXT_ATTRIBUTES_NONE;
-        tfp_sprintf(buff, "%3d", currentControlProfile->manual.rates[FD_PITCH]);
+        osdFormatIntUnit(buff, 3, currentControlProfile->manual.rates[FD_PITCH], 0);
         if (isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_ROLL_RATE))
             TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
         displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr);
@@ -3336,7 +3379,7 @@ static bool osdDrawSingleElement(uint8_t item)
         displayWrite(osdDisplayPort, elemPosX, elemPosY, "MRR");
 
         elemAttr = TEXT_ATTRIBUTES_NONE;
-        tfp_sprintf(buff, "%3d", currentControlProfile->manual.rates[FD_ROLL]);
+        osdFormatIntUnit(buff, 3, currentControlProfile->manual.rates[FD_ROLL], 0);
         if (isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_ROLL_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_ROLL_RATE))
             TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
         displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr);
@@ -3398,11 +3441,11 @@ static bool osdDrawSingleElement(uint8_t item)
             const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers();
             strcpy(buff, "POSO ");
             // display requested velocity cm/s
-            tfp_sprintf(buff + 5, "%4d", (int)lrintf(nav_pids->pos[X].output_constrained * 100));
+            osdFormatIntUnit(buff + 5, 4, (int)lrintf(nav_pids->pos[X].output_constrained * 100), 0);
             buff[9] = ' ';
-            tfp_sprintf(buff + 10, "%4d", (int)lrintf(nav_pids->pos[Y].output_constrained * 100));
+            osdFormatIntUnit(buff + 10, 4, (int)lrintf(nav_pids->pos[Y].output_constrained * 100), 0);
             buff[14] = ' ';
-            tfp_sprintf(buff + 15, "%4d", (int)lrintf(nav_pids->pos[Z].output_constrained * 100));
+            osdFormatIntUnit(buff + 15, 4, (int)lrintf(nav_pids->pos[Z].output_constrained * 100), 0);
             buff[19] = '\0';
             break;
         }
@@ -3515,7 +3558,7 @@ static bool osdDrawSingleElement(uint8_t item)
                 if (h < 0) {
                     h += 360;
                 }
-                tfp_sprintf(&buff[1], "%3d", h);
+                osdFormatIntUnit(&buff[1], 3, h, 0);
             } else {
                 buff[1] = buff[2] = buff[3] = '-';
             }
@@ -3575,9 +3618,9 @@ static bool osdDrawSingleElement(uint8_t item)
                 case OSD_UNIT_IMPERIAL:
                     moreThanAh = osdFormatCentiNumber(buff, value * METERS_PER_MILE / 10, 1000, 0, 2, digits, false);
                     if (!moreThanAh) {
-                        tfp_sprintf(buff + strlen(buff), "%c%c", SYM_MAH_MI_0, SYM_MAH_MI_1);
+                        osdWriteChar2(buff + strlen(buff), SYM_MAH_MI_0, SYM_MAH_MI_1);
                     } else {
-                        tfp_sprintf(buff + strlen(buff), "%c", SYM_AH_MI);
+                        osdWriteChar(buff + strlen(buff), SYM_AH_MI);
                     }
                     if (!efficiencyValid) {
                         buff[0] = buff[1] = buff[2] = buff[3] = '-';
@@ -3589,9 +3632,9 @@ static bool osdDrawSingleElement(uint8_t item)
                 case OSD_UNIT_GA:
                      moreThanAh = osdFormatCentiNumber(buff, value * METERS_PER_NAUTICALMILE / 10, 1000, 0, 2, digits, false);
                     if (!moreThanAh) {
-                        tfp_sprintf(buff + strlen(buff), "%c%c", SYM_MAH_NM_0, SYM_MAH_NM_1);
+                        osdWriteChar2(buff + strlen(buff), SYM_MAH_NM_0, SYM_MAH_NM_1);
                     } else {
-                        tfp_sprintf(buff + strlen(buff), "%c", SYM_AH_NM);
+                        osdWriteChar(buff + strlen(buff), SYM_AH_NM);
                     }
                     if (!efficiencyValid) {
                         buff[0] = buff[1] = buff[2] = buff[3] = '-';
@@ -3605,9 +3648,9 @@ static bool osdDrawSingleElement(uint8_t item)
                 case OSD_UNIT_METRIC:
                     moreThanAh = osdFormatCentiNumber(buff, value * 100, 1000, 0, 2, digits, false);
                     if (!moreThanAh) {
-                        tfp_sprintf(buff + strlen(buff), "%c%c", SYM_MAH_KM_0, SYM_MAH_KM_1);
+                        osdWriteChar2(buff + strlen(buff), SYM_MAH_KM_0, SYM_MAH_KM_1);
                     } else {
-                        tfp_sprintf(buff + strlen(buff), "%c", SYM_AH_KM);
+                        osdWriteChar(buff + strlen(buff), SYM_AH_KM);
                     }
                     if (!efficiencyValid) {
                         buff[0] = buff[1] = buff[2] = buff[3] = '-';
@@ -3818,7 +3861,7 @@ static bool osdDrawSingleElement(uint8_t item)
                 else
                     h = h + 180;
 
-                tfp_sprintf(&buff[1], "%3d", h);
+                osdFormatIntUnit(&buff[1], 3, h, 0);
             } else {
                 buff[1] = buff[2] = buff[3] = '-';
             }
@@ -3948,7 +3991,7 @@ static bool osdDrawSingleElement(uint8_t item)
 
             displayWrite(osdDisplayPort, elemPosX, elemPosY, "TPA");
             attr = TEXT_ATTRIBUTES_NONE;
-            tfp_sprintf(buff, "%3d", currentControlProfile->throttle.dynPID);
+            osdFormatIntUnit(buff, 3, currentControlProfile->throttle.dynPID, 0);
             if (isAdjustmentFunctionSelected(ADJUSTMENT_TPA)) {
                 TEXT_ATTRIBUTES_ADD_BLINK(attr);
             }
@@ -3956,7 +3999,7 @@ static bool osdDrawSingleElement(uint8_t item)
 
             displayWrite(osdDisplayPort, elemPosX, elemPosY + 1, "BP");
             attr = TEXT_ATTRIBUTES_NONE;
-            tfp_sprintf(buff, "%4d", currentControlProfile->throttle.pa_breakpoint);
+            osdFormatIntUnit(buff, 4, currentControlProfile->throttle.pa_breakpoint, 0);
             if (isAdjustmentFunctionSelected(ADJUSTMENT_TPA_BREAKPOINT)) {
                 TEXT_ATTRIBUTES_ADD_BLINK(attr);
             }
@@ -4646,7 +4689,7 @@ uint8_t drawStat_Stats(uint8_t statNameX, uint8_t row, uint8_t statValueX, bool
                 FALLTHROUGH;
             case OSD_UNIT_IMPERIAL:
                 if (isBootStats) {
-                    tfp_sprintf(string_buffer, "%5d", (uint16_t)(statsConfig()->stats_total_dist / METERS_PER_MILE));
+                    osdFormatIntUnit(string_buffer, 5, (uint16_t)(statsConfig()->stats_total_dist / METERS_PER_MILE), 0);
                     buffLen = 5;
                 } else {
                     uint16_t statTotalDist = (uint16_t)(statsConfig()->stats_total_dist / METERS_PER_MILE);
@@ -4659,7 +4702,7 @@ uint8_t drawStat_Stats(uint8_t statNameX, uint8_t row, uint8_t statValueX, bool
             default:
             case OSD_UNIT_GA:
                 if (isBootStats) {
-                    tfp_sprintf(string_buffer, "%5d", (uint16_t)(statsConfig()->stats_total_dist / METERS_PER_NAUTICALMILE));
+                    osdFormatIntUnit(string_buffer, 5, (uint16_t)(statsConfig()->stats_total_dist / METERS_PER_NAUTICALMILE), 0);
                     buffLen = 5;
                 } else {
                     uint16_t statTotalDist = (uint16_t)(statsConfig()->stats_total_dist / METERS_PER_NAUTICALMILE);
@@ -4673,7 +4716,7 @@ uint8_t drawStat_Stats(uint8_t statNameX, uint8_t row, uint8_t statValueX, bool
                 FALLTHROUGH;
             case OSD_UNIT_METRIC:
                 if (isBootStats) {
-                    tfp_sprintf(string_buffer, "%5d", (uint16_t)(statsConfig()->stats_total_dist / METERS_PER_KILOMETER));
+                    osdFormatIntUnit(string_buffer, 5, (uint16_t)(statsConfig()->stats_total_dist / METERS_PER_KILOMETER), 0);
                     buffLen = 5;
                 } else {
                     uint16_t statTotalDist = (uint16_t)(statsConfig()->stats_total_dist / METERS_PER_KILOMETER);
@@ -5051,7 +5094,7 @@ uint8_t drawStat_BatteryVoltage(uint8_t col, uint8_t row, uint8_t statValX)
     multiValueXOffset = strlen(buff);
     // AverageCell
     osdFormatCentiNumber(buff + multiValueXOffset, stats.min_voltage / getBatteryCellCount(), 0, 2, 0, 3, false);
-    tfp_sprintf(buff + strlen(buff), "%c", SYM_VOLT);
+    osdWriteChar(buff + strlen(buff), SYM_VOLT);
 
     displayWrite(osdDisplayPort, statValX, row++, buff);
 
@@ -5096,7 +5139,7 @@ uint8_t drawStat_UsedEnergy(uint8_t col, uint8_t row, uint8_t statValX)
         strcat(buff, "/");
         osdFormatCentiNumber(preBuff, getMWhDrawn() / 10, 0, 2, 0, 3, false);
         strcat(buff, osdFormatTrimWhiteSpace(preBuff));
-        tfp_sprintf(buff + strlen(buff), "%c", SYM_WH);
+        osdWriteChar(buff + strlen(buff), SYM_WH);
     }
     displayWrite(osdDisplayPort, statValX, row++, buff);
 
@@ -5135,9 +5178,9 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b
                         strcat(outBuff, osdFormatTrimWhiteSpace(buff));
                         if (osdDisplayIsHD()) {
                             if (!moreThanAh)
-                                tfp_sprintf(outBuff + strlen(outBuff), "%c%c", SYM_MAH_MI_0, SYM_MAH_MI_1);
+                                osdWriteChar2(outBuff + strlen(outBuff), SYM_MAH_MI_0, SYM_MAH_MI_1);
                             else
-                                tfp_sprintf(outBuff + strlen(outBuff), "%c", SYM_AH_MI);
+                                osdWriteChar(outBuff + strlen(outBuff), SYM_AH_MI);
 
                             moreThanAh = false;
                         }
@@ -5147,9 +5190,9 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b
                         strcat(outBuff, osdFormatTrimWhiteSpace(buff));
 
                         if (!moreThanAh)
-                            tfp_sprintf(outBuff + strlen(outBuff), "%c%c", SYM_MAH_MI_0, SYM_MAH_MI_1);
+                            osdWriteChar2(outBuff + strlen(outBuff), SYM_MAH_MI_0, SYM_MAH_MI_1);
                         else
-                            tfp_sprintf(outBuff + strlen(outBuff), "%c", SYM_AH_MI);
+                            osdWriteChar(outBuff + strlen(outBuff), SYM_AH_MI);
                     } else {
                         tfp_sprintf(outBuff + strlen(outBuff), "---/---%c%c", SYM_MAH_MI_0, SYM_MAH_MI_1);
                     }
@@ -5163,7 +5206,7 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b
                     } else {
                         strcat(outBuff, "---/---");
                     }
-                    tfp_sprintf(outBuff + strlen(outBuff), "%c", SYM_WH_MI);
+                    osdWriteChar(outBuff + strlen(outBuff), SYM_WH_MI);
                 }
                 break;
             case OSD_UNIT_GA:
@@ -5173,9 +5216,9 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b
                         strcat(outBuff, osdFormatTrimWhiteSpace(buff));
                          if (osdDisplayIsHD()) {
                             if (!moreThanAh)
-                                tfp_sprintf(outBuff + strlen(outBuff), "%c%c", SYM_MAH_NM_0, SYM_MAH_NM_1);
+                                osdWriteChar2(outBuff + strlen(outBuff), SYM_MAH_NM_0, SYM_MAH_NM_1);
                             else
-                                tfp_sprintf(outBuff + strlen(outBuff), "%c", SYM_AH_NM);
+                                osdWriteChar(outBuff + strlen(outBuff), SYM_AH_NM);
 
                             moreThanAh = false;
                         }
@@ -5184,9 +5227,9 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b
                         moreThanAh = moreThanAh || osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_NAUTICALMILE / totalDistance), 1000, 0, 2, digits, false);
                         strcat(outBuff, osdFormatTrimWhiteSpace(buff));
                         if (!moreThanAh) {
-                            tfp_sprintf(outBuff + strlen(outBuff), "%c%c", SYM_MAH_NM_0, SYM_MAH_NM_1);
+                            osdWriteChar2(outBuff + strlen(outBuff), SYM_MAH_NM_0, SYM_MAH_NM_1);
                         } else {
-                            tfp_sprintf(outBuff + strlen(outBuff), "%c", SYM_AH_NM);
+                            osdWriteChar(outBuff + strlen(outBuff), SYM_AH_NM);
                         }
                     } else {
                         tfp_sprintf(outBuff + strlen(outBuff), "---/---%c%c", SYM_MAH_NM_0, SYM_MAH_NM_1);
@@ -5201,7 +5244,7 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b
                     } else {
                         strcat(outBuff, "---/---");
                     }
-                    tfp_sprintf(outBuff + strlen(outBuff), "%c", SYM_WH_NM);
+                    osdWriteChar(outBuff + strlen(outBuff), SYM_WH_NM);
                 }
                 break;
             case OSD_UNIT_METRIC_MPH:
@@ -5218,9 +5261,9 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b
                 strcat(outBuff, osdFormatTrimWhiteSpace(buff));
                 if (osdDisplayIsHD()) {
                     if (!moreThanAh)
-                        tfp_sprintf(outBuff + strlen(outBuff), "%c%c", SYM_MAH_KM_0, SYM_MAH_KM_1);
+                        osdWriteChar2(outBuff + strlen(outBuff), SYM_MAH_KM_0, SYM_MAH_KM_1);
                     else
-                        tfp_sprintf(outBuff + strlen(outBuff), "%c", SYM_AH_KM);
+                        osdWriteChar(outBuff + strlen(outBuff), SYM_AH_KM);
 
                     moreThanAh = false;
                 }
@@ -5229,9 +5272,9 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b
                 moreThanAh = moreThanAh || osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000000.0f / totalDistance), 1000, 0, 2, digits, false);
                 strcat(outBuff, osdFormatTrimWhiteSpace(buff));
                 if (!moreThanAh) {
-                    tfp_sprintf(outBuff + strlen(outBuff), "%c%c", SYM_MAH_KM_0, SYM_MAH_KM_1);
+                    osdWriteChar2(outBuff + strlen(outBuff), SYM_MAH_KM_0, SYM_MAH_KM_1);
                 } else {
-                    tfp_sprintf(outBuff + strlen(outBuff), "%c", SYM_AH_KM);
+                    osdWriteChar(outBuff + strlen(outBuff), SYM_AH_KM);
                 }
             } else {
                 tfp_sprintf(outBuff + strlen(outBuff), "---/---%c%c", SYM_MAH_KM_0, SYM_MAH_KM_1);
@@ -5246,11 +5289,10 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b
             } else {
                 strcat(outBuff, "---/---");
             }
-            tfp_sprintf(outBuff + strlen(outBuff), "%c", SYM_WH_KM);
+            osdWriteChar(outBuff + strlen(outBuff), SYM_WH_KM);
         }
     }
 
-    tfp_sprintf(outBuff + strlen(outBuff), "%c", '\0');
     displayWrite(osdDisplayPort, statValX, row++, outBuff);
 
     return row;
@@ -5284,7 +5326,7 @@ uint8_t drawStat_RXStats(uint8_t col, uint8_t row, uint8_t statValX)
         if (osdDisplayIsHD()) {
             strcat(osdFormatTrimWhiteSpace(buff), "/");
             itoa(stats.min_rssi_dbm, buff + 2, 10);
-            tfp_sprintf(buff + strlen(buff), "%c", SYM_DBM);
+            osdWriteChar(buff + strlen(buff), SYM_DBM);
             displayWrite(osdDisplayPort, statValX, row++, buff);
         }
     }
@@ -5296,7 +5338,7 @@ uint8_t drawStat_RXStats(uint8_t col, uint8_t row, uint8_t statValX)
         memset(buff, '\0', strlen(buff));
         tfp_sprintf(buff, ": ");
         itoa(stats.min_rssi_dbm, buff + 2, 10);
-        tfp_sprintf(buff + strlen(buff), "%c", SYM_DBM);
+        osdWriteChar(buff + strlen(buff), SYM_DBM);
         displayWrite(osdDisplayPort, statValX, row++, buff);
     }
 

From 96a0ec3d37e68cf118d0634846c849611347ef18 Mon Sep 17 00:00:00 2001
From: Yuri-Sharapov <yuri.v.sharapov@gmail.com>
Date: Tue, 10 Mar 2026 12:17:34 +0300
Subject: [PATCH 039/102] added support of icm45686 sensor

---
 src/main/CMakeLists.txt                     |   2 +
 src/main/drivers/accgyro/accgyro_icm45686.c | 500 ++++++++++++++++++++
 src/main/drivers/accgyro/accgyro_icm45686.h |  22 +
 src/main/drivers/accgyro/accgyro_mpu.h      |   3 +-
 src/main/drivers/bus.h                      |   3 +-
 src/main/fc/cli.c                           |   2 +-
 src/main/fc/settings.yaml                   |   2 +-
 src/main/sensors/acceleration.c             |  15 +-
 src/main/sensors/acceleration.h             |   3 +-
 src/main/sensors/gyro.c                     |  12 +-
 src/main/sensors/gyro.h                     |   4 +-
 src/main/target/common_hardware.c           |   6 +-
 12 files changed, 564 insertions(+), 10 deletions(-)
 create mode 100644 src/main/drivers/accgyro/accgyro_icm45686.c
 create mode 100644 src/main/drivers/accgyro/accgyro_icm45686.h

diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt
index 101c7223372..531d43a92fd 100755
--- a/src/main/CMakeLists.txt
+++ b/src/main/CMakeLists.txt
@@ -87,6 +87,8 @@ main_sources(COMMON_SRC
     drivers/accgyro/accgyro_icm20689.h
     drivers/accgyro/accgyro_icm42605.c
     drivers/accgyro/accgyro_icm42605.h
+    drivers/accgyro/accgyro_icm45686.c
+    drivers/accgyro/accgyro_icm45686.h
     drivers/accgyro/accgyro_mpu.c
     drivers/accgyro/accgyro_mpu.h
     drivers/accgyro/accgyro_mpu6000.c
diff --git a/src/main/drivers/accgyro/accgyro_icm45686.c b/src/main/drivers/accgyro/accgyro_icm45686.c
new file mode 100644
index 00000000000..a9e0ee0b8a5
--- /dev/null
+++ b/src/main/drivers/accgyro/accgyro_icm45686.c
@@ -0,0 +1,500 @@
+/*
+ * 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 <stdlib.h>
+
+#include "platform.h"
+
+#include "build/debug.h"
+
+#include "common/axis.h"
+#include "common/maths.h"
+#include "common/utils.h"
+#include "common/log.h"
+
+#include "drivers/system.h"
+#include "drivers/time.h"
+
+#include "drivers/sensor.h"
+#include "drivers/accgyro/accgyro_mpu.h"
+#include "drivers/accgyro/accgyro.h"
+#include "accgyro_icm45686.h"
+
+#if defined(USE_IMU_ICM45686)
+/*
+reference: https://github.com/tdk-invn-oss/motion.mcu.icm45686.driver
+Datasheet: https://invensense.tdk.com/wp-content/uploads/documentation/DS-000577_ICM-45686.pdf
+Datasheet: https://invensense.tdk.com/wp-content/uploads/documentation/DS-000576_ICM-45605.pdf
+
+Note: ICM456xx has two modes of operation: Low-Power Mode Low-Noise Mode
+Note: Now implemented only UI Interface with Low-Noise Mode
+
+ The following diagram shows the signal path for each mode:
+ The cut-off frequency of the filters is determined by the ODR setting.
+
+                   Low-Noise Mode
+     +------+     +--------------+    +-------------+    +--------------+    +------------------+
+     | ADC  |---->| Anti-Alias   |--->| Interpolator|--->|     LPF      |--->| Sensor Registers |---> UI Interface
+     |      |     | Filter (AAF) |    |             | +->| & ODR Select |    |                  |
+     +--|---+     +--------------+    +-------------+ |  +--------------+    +------------------+
+        |                                             |
+        |           Low-Power Mode                    |
+        |         +--------------+                    |
+        |-------->| Notch Filter |--------------------|
+        |         |              |
+        |         +--------------+
+        |
+        |
+     +--|---+           +--------------+       +------+       +------+      +------------------+
+     | ADC  | --------> | Notch Filter | --->  | HPF  | --->  | LPF  | ---> | Sensor Registers | ---> AUX1 Interface
+     |      |           |              |       |      |       |      |      |                  |
+     +------+           +--------------+       +------+       +------+      +------------------+
+
+ The AUX1 interface default configuration can be checked by read only register IOC_PAD_SCENARIO through host interface.
+ By default, AUX1 interface is enabled, and default interface for AUX1 is SPI3W or I3CSM.
+
+ In Low-Noise Mode, the ADC output is sent through an Anti-Alias Filter (AAF). The AAF is an FIR filter with fixed
+ coefficients (not user configurable). The AAF can be enabled or disabled by the user using GYRO_SRC_CTRL and
+ ACCEL_SRC_CTRL.
+
+ The AUX1 signal path includes a Notch Filter. The notch filter is not user programmable. The usage of the notch
+ filter in the auxiliary path is recommended for sharper roll-off and for the cases where user is asynchronously
+ sampling the auxiliary interface data output at integer multiples of 1 kHz rate. The notch filter may be bypassed
+ using GYRO_OIS_M6_BYP.
+
+ The notch filter is followed by an HPF on the AUX1 signal path. HPF cut-off frequency can be selected using
+ GYRO_OIS_HPFBW_SEL and ACCEL_OIS_HPFBW_SEL. HPF can be bypassed using GYRO_OIS_HPF1_BYP and
+ ACCEL_OIS_HPF1_BYP.
+
+ The HPF is followed by LPF on the AUX1 signal path. The AUX1 LPF BW is set by register bit field
+ GYRO_OIS_LPF1BW_SEL and ACCEL_OIS_LPF1BW_SEL for gyroscope and accelerometer respectively. This is
+ followed by Full Scale Range (FSR) selection based on user configurable settings for register fields
+ GYRO_AUX1_FS_SEL and ACCEL_AUX1_FS_SEL. AUX1 output is fixed at 6.4kHz ODR.
+*/
+
+// NOTE: ICM-45686 does NOT have a bank select register like ICM-426xx
+// The ICM-45686 uses Indirect Register (IREG) access for internal registers
+// Register 0x75 is RESERVED/UNDEFINED in the ICM-45686 datasheet
+// DO NOT use bank switching on this device
+
+// Register map User Bank 0 (UI Interface)
+#define ICM456XX_WHO_AM_REGISTER                0x72
+#define ICM456XX_REG_MISC2                      0x7F
+#define ICM456XX_INT1_CONFIG0                   0x16
+#define ICM456XX_INT1_CONFIG2                   0x18
+#define ICM456XX_INT1_STATUS0                   0x19
+#define ICM456XX_INT1_STATUS1                   0x1A
+#define ICM456XX_GYRO_CONFIG0                   0x1C
+#define ICM456XX_ACCEL_CONFIG0                  0x1B
+#define ICM456XX_PWR_MGMT0                      0x10
+#define ICM456XX_TEMP_DATA1                     0x0C
+// Register map IPREG_TOP1 (for future use)
+// Note: SREG_CTRL register provides endian selection capability. 
+// Currently not utilized as UI interface reads are in device's native endianness.
+// Kept as reference for potential future optimization.
+#define ICM456XX_RA_SREG_CTRL                   0xA267 // To access: 0xA200 + 0x67
+
+// SREG_CTRL - 0x67 (bits 1:0 select data endianness)
+#define ICM456XX_SREG_DATA_ENDIAN_SEL_LITTLE    (0 << 1) // Little endian (native)
+#define ICM456XX_SREG_DATA_ENDIAN_SEL_BIG       (1 << 1) // Big endian (requires IREG write)
+
+// MGMT0 - 0x10 - Gyro
+#define ICM456XX_GYRO_MODE_OFF                  (0x00 << 2)
+#define ICM456XX_GYRO_MODE_STANDBY              (0x01 << 2)
+#define ICM456XX_GYRO_MODE_LP                   (0x02 << 2)  // Low Power Mode
+#define ICM456XX_GYRO_MODE_LN                   (0x03 << 2)  // Low Noise Mode
+
+// MGMT0 - 0x10 - Accel
+#define ICM456XX_ACCEL_MODE_OFF                 (0x00)
+#define ICM456XX_ACCEL_MODE_OFF2                (0x01)
+#define ICM456XX_ACCEL_MODE_LP                  (0x02) // Low Power Mode
+#define ICM456XX_ACCEL_MODE_LN                  (0x03) // Low Noise Mode
+
+// INT1_CONFIG0 - 0x16
+#define ICM456XX_INT1_STATUS_EN_RESET_DONE      (1 << 7)
+#define ICM456XX_INT1_STATUS_EN_AUX1_AGC_RDY    (1 << 6)
+#define ICM456XX_INT1_STATUS_EN_AP_AGC_RDY      (1 << 5)
+#define ICM456XX_INT1_STATUS_EN_AP_FSYNC        (1 << 4)
+#define ICM456XX_INT1_STATUS_EN_AUX1_DRDY       (1 << 3)
+#define ICM456XX_INT1_STATUS_EN_DRDY            (1 << 2)
+#define ICM456XX_INT1_STATUS_EN_FIFO_THS        (1 << 1)
+#define ICM456XX_INT1_STATUS_EN_FIFO_FULL       (1 << 0)
+
+// INT1_CONFIG2 - 0x18
+#define ICM456XX_INT1_DRIVE_CIRCUIT_PP          (0 << 2)
+#define ICM456XX_INT1_DRIVE_CIRCUIT_OD          (1 << 2)
+#define ICM456XX_INT1_MODE_PULSED               (0 << 1)
+#define ICM456XX_INT1_MODE_LATCHED              (1 << 1)
+#define ICM456XX_INT1_POLARITY_ACTIVE_LOW       (0 << 0)
+#define ICM456XX_INT1_POLARITY_ACTIVE_HIGH      (1 << 0)
+
+// INT1_STATUS0 - 0x19
+#define ICM456XX_INT1_STATUS_RESET_DONE         (1 << 7)
+#define ICM456XX_INT1_STATUS_AUX1_AGC_RDY       (1 << 6)
+#define ICM456XX_INT1_STATUS_AP_AGC_RDY         (1 << 5)
+#define ICM456XX_INT1_STATUS_AP_FSYNC           (1 << 4)
+#define ICM456XX_INT1_STATUS_AUX1_DRDY          (1 << 3)
+#define ICM456XX_INT1_STATUS_DRDY               (1 << 2)
+#define ICM456XX_INT1_STATUS_FIFO_THS           (1 << 1)
+#define ICM456XX_INT1_STATUS_FIFO_FULL          (1 << 0)
+
+// REG_MISC2 - 0x7F
+#define ICM456XX_SOFT_RESET                     (1 << 1)
+#define ICM456XX_RESET_TIMEOUT_US               20000  // 20 ms
+
+#define ICM456XX_ACCEL_DATA_X1_UI               0x00
+#define ICM456XX_GYRO_DATA_X1_UI                0x06
+
+// ACCEL_CONFIG0 - 0x1B
+#define ICM456XX_ACCEL_FS_SEL_32G               (0x00 << 4)
+#define ICM456XX_ACCEL_FS_SEL_16G               (0x01 << 4)
+#define ICM456XX_ACCEL_FS_SEL_8G                (0x02 << 4)
+#define ICM456XX_ACCEL_FS_SEL_4G                (0x03 << 4)
+#define ICM456XX_ACCEL_FS_SEL_2G                (0x04 << 4)
+
+// ACCEL_CONFIG0 - 0x1B
+#define ICM456XX_ACCEL_ODR_6K4_LN               0x03
+#define ICM456XX_ACCEL_ODR_3K2_LN               0x04
+#define ICM456XX_ACCEL_ODR_1K6_LN               0x05
+#define ICM456XX_ACCEL_ODR_800_LN               0x06
+#define ICM456XX_ACCEL_ODR_400_LP_LN            0x07
+#define ICM456XX_ACCEL_ODR_200_LP_LN            0x08
+#define ICM456XX_ACCEL_ODR_100_LP_LN            0x09
+#define ICM456XX_ACCEL_ODR_50_LP_LN             0x0A
+#define ICM456XX_ACCEL_ODR_25_LP_LN             0x0B
+#define ICM456XX_ACCEL_ODR_12_5_LP_LN           0x0C
+#define ICM456XX_ACCEL_ODR_6_25_LP              0x0D
+#define ICM456XX_ACCEL_ODR_3_125_LP             0x0E
+#define ICM456XX_ACCEL_ODR_1_5625_LP            0x0F
+
+// GYRO_CONFIG0 - 0x1C
+#define ICM456XX_GYRO_FS_SEL_4000DPS            (0x00 << 4)
+#define ICM456XX_GYRO_FS_SEL_2000DPS            (0x01 << 4)
+#define ICM456XX_GYRO_FS_SEL_1000DPS            (0x02 << 4)
+#define ICM456XX_GYRO_FS_SEL_500DPS             (0x03 << 4)
+#define ICM456XX_GYRO_FS_SEL_250DPS             (0x04 << 4)
+#define ICM456XX_GYRO_FS_SEL_125DPS             (0x05 << 4)
+#define ICM456XX_GYRO_FS_SEL_62_5DPS            (0x06 << 4)
+#define ICM456XX_GYRO_FS_SEL_31_25DPS           (0x07 << 4)
+#define ICM456XX_GYRO_FS_SEL_15_625DPS          (0x08 << 4)
+
+// GYRO_CONFIG0 - 0x1C
+#define ICM456XX_GYRO_ODR_6K4_LN                0x03
+#define ICM456XX_GYRO_ODR_3K2_LN                0x04
+#define ICM456XX_GYRO_ODR_1K6_LN                0x05
+#define ICM456XX_GYRO_ODR_800_LN                0x06
+#define ICM456XX_GYRO_ODR_400_LP_LN             0x07
+#define ICM456XX_GYRO_ODR_200_LP_LN             0x08
+#define ICM456XX_GYRO_ODR_100_LP_LN             0x09
+#define ICM456XX_GYRO_ODR_50_LP_LN              0x0A
+#define ICM456XX_GYRO_ODR_25_LP_LN              0x0B
+#define ICM456XX_GYRO_ODR_12_5_LP_LN            0x0C
+#define ICM456XX_GYRO_ODR_6_25_LP               0x0D
+#define ICM456XX_GYRO_ODR_3_125_LP              0x0E
+#define ICM456XX_GYRO_ODR_1_5625_LP             0x0F
+
+// Accel IPREG_SYS2_REG_123 - 0x7B
+#define ICM456XX_SRC_CTRL_AAF_ENABLE_BIT        (1 << 0) // Anti-Alias Filter - AAF
+#define ICM456XX_SRC_CTRL_INTERP_ENABLE_BIT     (1 << 1) // Interpolator
+
+// IPREG_SYS2_REG_123 - 0x7B
+#define ICM456XX_ACCEL_SRC_CTRL_IREG_ADDR       0xA57B // To access register in IPREG_SYS2, add base address 0xA500 + offset
+
+// IPREG_SYS1_REG_166 - 0xA6
+#define ICM456XX_GYRO_SRC_CTRL_IREG_ADDR        0xA4A6 // To access register in IPREG_SYS1, add base address 0xA400 + offset
+
+// HOST INDIRECT ACCESS REGISTER (IREG)
+#define ICM456XX_REG_IREG_ADDR_15_8             0x7C
+#define ICM456XX_REG_IREG_ADDR_7_0              0x7D
+#define ICM456XX_REG_IREG_DATA                  0x7E
+
+
+// IPREG_SYS1_REG_172 - 0xAC
+#define ICM456XX_GYRO_UI_LPF_CFG_IREG_ADDR       0xA4AC // To access register in IPREG_SYS1, add base address 0xA400 + offset
+
+// LPF UI - 0xAC PREG_SYS1_REG_172 (bits 2:0)
+#define ICM456XX_GYRO_UI_LPFBW_BYPASS            0x00
+#define ICM456XX_GYRO_UI_LPFBW_ODR_DIV_4         0x01 // 1600 Hz ODR = 6400 Hz:
+#define ICM456XX_GYRO_UI_LPFBW_ODR_DIV_8         0x02 // 800 Hz ODR = 6400 Hz:
+#define ICM456XX_GYRO_UI_LPFBW_ODR_DIV_16        0x03 // 400 Hz ODR = 6400 Hz:
+#define ICM456XX_GYRO_UI_LPFBW_ODR_DIV_32        0x04 // 200 Hz ODR = 6400 Hz
+#define ICM456XX_GYRO_UI_LPFBW_ODR_DIV_64        0x05 // 100 Hz ODR = 6400 Hz
+#define ICM456XX_GYRO_UI_LPFBW_ODR_DIV_128       0x06 // 50 Hz ODR = 6400 Hz
+
+// IPREG_SYS2_REG_131 - 0x83
+#define ICM456XX_ACCEL_UI_LPF_CFG_IREG_ADDR      0xA583 // To access register in IPREG_SYS2, add base address 0xA500 + offset
+
+// Accel UI path LPF - 0x83 IPREG_SYS2_REG_131 (bits 2:0)
+#define ICM456XX_ACCEL_UI_LPFBW_BYPASS           0x00
+#define ICM456XX_ACCEL_UI_LPFBW_ODR_DIV_4        0x01 // 400 Hz ODR = 1600 Hz:
+#define ICM456XX_ACCEL_UI_LPFBW_ODR_DIV_8        0x02 // 200 Hz ODR = 1600 Hz:
+#define ICM456XX_ACCEL_UI_LPFBW_ODR_DIV_16       0x03 // 100 Hz ODR = 1600 Hz:
+#define ICM456XX_ACCEL_UI_LPFBW_ODR_DIV_32       0x04 // 50 Hz ODR = 1600 Hz
+#define ICM456XX_ACCEL_UI_LPFBW_ODR_DIV_64       0x05 // 25 Hz ODR = 1600 Hz
+#define ICM456XX_ACCEL_UI_LPFBW_ODR_DIV_128      0x06 // 12.5 Hz ODR = 1600 Hz
+
+#ifndef ICM456XX_CLOCK
+// Default: 24 MHz max SPI frequency
+#define ICM456XX_MAX_SPI_CLK_HZ                 24000000
+#else
+#define ICM456XX_MAX_SPI_CLK_HZ                 ICM456XX_CLOCK
+#endif
+
+#define HZ_TO_US(hz)                            ((int32_t)((1000 * 1000) / (hz)))
+
+#define ICM456XX_BIT_IREG_DONE                  (1 << 0)
+
+// Startup timing constants (DS-000577 Table 9-6)
+#define ICM456XX_ACCEL_STARTUP_TIME_MS          10  // Min accel startup from OFF/STANDBY/LP
+#define ICM456XX_GYRO_STARTUP_TIME_MS           35  // Min gyro startup from OFF/STANDBY/LP
+#define ICM456XX_SENSOR_ENABLE_DELAY_MS         1   // Allow sensors to power on and stabilize
+#define ICM456XX_IREG_TIMEOUT_US                5000 // IREG operation timeout (5ms max)
+
+#define ICM456XX_DATA_LENGTH                    6  // 3 axes * 2 bytes per axis
+#define ICM456XX_SPI_BUFFER_SIZE                (1 + ICM456XX_DATA_LENGTH) // 1 byte register + 6 bytes data
+
+static const gyroFilterAndRateConfig_t icm45xxGyroConfigs[] = {
+    /*   LPF          ODR   { lpfBits,                            odrReg } */
+    { GYRO_LPF_NONE,  6000, { ICM456XX_GYRO_UI_LPFBW_BYPASS,      0x03 } },
+    { GYRO_LPF_256HZ, 6000, { ICM456XX_GYRO_UI_LPFBW_ODR_DIV_16,  0x03 } }, // ≈400 Hz
+    { GYRO_LPF_188HZ, 6000, { ICM456XX_GYRO_UI_LPFBW_ODR_DIV_32,  0x03 } }, // ≈200 Hz
+    { GYRO_LPF_98HZ,  6000, { ICM456XX_GYRO_UI_LPFBW_ODR_DIV_64,  0x03 } }, // ≈100 Hz
+    { GYRO_LPF_42HZ,  6000, { ICM456XX_GYRO_UI_LPFBW_ODR_DIV_128, 0x03 } }, // ≈50 Hz
+};
+
+/**
+ * @brief This function follows the IREG WRITE procedure (Section 14.1-14.4 of the datasheet)
+ * using indirect addressing via IREG_ADDR_15_8, IREG_ADDR_7_0, and IREG_DATA registers.
+ * After writing, an internal operation transfers the data to the target IREG address.
+ * Ensures compliance with the required minimum time gap and checks the IREG_DONE bit.
+ *
+ * @param dev   Pointer to the SPI device structure.
+ * @param reg   16-bit internal IREG register address.
+ * @param value Value to be written to the register.
+ * @return true if the write was successful
+ */
+static bool icm45686WriteIREG(const busDevice_t *dev, uint16_t reg, uint8_t value)
+{
+    const uint8_t msb = (reg >> 8) & 0xFF;
+    const uint8_t lsb = reg & 0xFF;
+
+    busWrite(dev, ICM456XX_REG_IREG_ADDR_15_8, msb);
+    busWrite(dev, ICM456XX_REG_IREG_ADDR_7_0, lsb);
+    busWrite(dev, ICM456XX_REG_IREG_DATA, value);
+
+    // Check IREG_DONE (bit 0 of REG_MISC2 = 0x7F) with elapsed-time tracking
+    for (uint32_t waited_us = 0; waited_us < ICM456XX_IREG_TIMEOUT_US; waited_us += 10) {
+        uint8_t misc2 = 0;
+        busRead(dev, ICM456XX_REG_MISC2, &misc2);
+        if (misc2 & ICM456XX_BIT_IREG_DONE) {
+            return true;
+        }
+        delay(1);
+    }
+
+    return false; // timeout
+}
+
+static void icm45686AccInit(accDev_t *acc)
+{
+    acc->acc_1G = 512 * 4; // 16g scale
+}
+
+static bool icm45686AccRead(accDev_t *acc)
+{
+    uint8_t data[6];
+
+    const bool ack = busReadBuf(acc->busDev, ICM456XX_ACCEL_DATA_X1_UI, data, 6);
+    if (!ack) {
+        return false;
+    }
+
+    acc->ADCRaw[X] = (float) int16_val_little_endian(data, 0);
+    acc->ADCRaw[Y] = (float) int16_val_little_endian(data, 1);
+    acc->ADCRaw[Z] = (float) int16_val_little_endian(data, 2);
+
+    return true;
+}
+
+static bool icm45686GyroRead(gyroDev_t *gyro)
+{
+    uint8_t data[6];
+
+    const bool ack = busReadBuf(gyro->busDev, ICM456XX_GYRO_DATA_X1_UI, data, 6);
+    if (!ack) {
+        return false;
+    }
+
+    gyro->gyroADCRaw[X] = (float) int16_val_little_endian(data, 0);
+    gyro->gyroADCRaw[Y] = (float) int16_val_little_endian(data, 1);
+    gyro->gyroADCRaw[Z] = (float) int16_val_little_endian(data, 2);
+
+    return true;
+}
+
+static bool icm45686ReadTemperature(gyroDev_t *gyro, int16_t * temp)
+{
+    uint8_t data[2];
+
+    const bool ack = busReadBuf(gyro->busDev, ICM456XX_TEMP_DATA1, data, 2);
+    if (!ack) {
+        return false;
+    }
+    // From datasheet: Temperature in Degrees Centigrade = (TEMP_DATA / 128) + 25 
+    *temp = ( int16_val_little_endian(data, 0) / 12.8 ) + 250; // Temperature stored as degC*10
+
+    return true;
+}
+
+static void icm45686AccAndGyroInit(gyroDev_t *gyro)
+{
+    busDevice_t * dev = gyro->busDev;
+    const gyroFilterAndRateConfig_t * config = chooseGyroConfig(gyro->lpf, 1000000 / gyro->requestedSampleIntervalUs,
+                                                                &icm45xxGyroConfigs[0], ARRAYLEN(icm45xxGyroConfigs));
+    gyro->sampleRateIntervalUs = 1000000 / config->gyroRateHz;
+        
+    busSetSpeed(dev, BUS_SPEED_INITIALIZATION);
+
+    // enable sensors
+    busWrite(dev, ICM456XX_PWR_MGMT0, (ICM456XX_GYRO_MODE_LN | ICM456XX_ACCEL_MODE_LN));
+    // Allow sensors to power on and stabilize
+    delay(ICM456XX_SENSOR_ENABLE_DELAY_MS);
+    // Configure accelerometer full-scale range (16g mode)
+    busWrite(dev, ICM456XX_ACCEL_CONFIG0, ICM456XX_ACCEL_FS_SEL_16G | ICM456XX_ACCEL_ODR_1K6_LN);
+    // Per datasheet Table 9-6: 10ms minimum startup time
+    delay(ICM456XX_ACCEL_STARTUP_TIME_MS);
+    // gyro filters
+    // Enable Anti-Alias (AAF) Filter and Interpolator for Gyro (Section 7.2 of datasheet)
+    if (!icm45686WriteIREG(dev, ICM456XX_GYRO_SRC_CTRL_IREG_ADDR, ICM456XX_SRC_CTRL_AAF_ENABLE_BIT | ICM456XX_SRC_CTRL_INTERP_ENABLE_BIT)) {
+        // AAF/Interpolator initialization failed, fallback to disabled state
+        icm45686WriteIREG(dev, ICM456XX_GYRO_SRC_CTRL_IREG_ADDR, 0);
+    }
+    // Set the Gyro UI LPF bandwidth cut-off (Section 7.3 of datasheet)
+    if (!icm45686WriteIREG(dev, ICM456XX_GYRO_UI_LPF_CFG_IREG_ADDR, config->gyroConfigValues[0])) {
+        // If LPF configuration fails, fallback to BYPASS
+        icm45686WriteIREG(dev, ICM456XX_GYRO_UI_LPF_CFG_IREG_ADDR, ICM456XX_GYRO_UI_LPFBW_BYPASS);
+    }
+    // accel filters
+    // Enable Anti-Alias Filter and Interpolator for Accel (Section 7.2 of datasheet)
+    if (!icm45686WriteIREG(dev, ICM456XX_ACCEL_SRC_CTRL_IREG_ADDR, ICM456XX_SRC_CTRL_AAF_ENABLE_BIT | ICM456XX_SRC_CTRL_INTERP_ENABLE_BIT)) {
+        icm45686WriteIREG(dev, ICM456XX_ACCEL_SRC_CTRL_IREG_ADDR, 0);
+    }
+    // Set the Accel UI LPF bandwidth cut-off to ODR/8 (Section 7.3 of datasheet)
+    if (!icm45686WriteIREG(dev, ICM456XX_ACCEL_UI_LPF_CFG_IREG_ADDR, ICM456XX_ACCEL_UI_LPFBW_ODR_DIV_8)) {
+        // If LPF configuration fails, fallback to BYPASS
+        icm45686WriteIREG(dev, ICM456XX_ACCEL_UI_LPF_CFG_IREG_ADDR, ICM456XX_ACCEL_UI_LPFBW_ODR_DIV_8);
+    }
+    // Setup scale and odr values for gyro
+    busWrite(dev, ICM456XX_GYRO_CONFIG0, ICM456XX_GYRO_FS_SEL_2000DPS | config->gyroConfigValues[1]);
+    // Per datasheet Table 9-6: 35ms minimum startup time
+    delay(ICM456XX_GYRO_STARTUP_TIME_MS);
+    
+    busWrite(dev, ICM456XX_INT1_CONFIG2, ICM456XX_INT1_MODE_PULSED | ICM456XX_INT1_DRIVE_CIRCUIT_PP |
+                                            ICM456XX_INT1_POLARITY_ACTIVE_HIGH);
+    busWrite(dev, ICM456XX_INT1_CONFIG0, ICM456XX_INT1_STATUS_EN_DRDY);
+
+    delay(15);
+    busSetSpeed(dev, BUS_SPEED_FAST);
+}
+
+static bool icm45686DeviceDetect(busDevice_t * dev)
+{
+    uint8_t tmp = 0xFF;
+    uint8_t attemptsRemaining = 5;
+    uint32_t waitedMs = 0;
+
+    busSetSpeed(dev, BUS_SPEED_INITIALIZATION);
+    // ICM-45686 does not use bank switching (register 0x75 is reserved)
+    // Perform soft reset directly
+    // Soft reset
+    busWrite(dev, ICM456XX_REG_MISC2, ICM456XX_SOFT_RESET);
+    // Wait for reset to complete (bit 1 of REG_MISC2 becomes 0)
+    while (1) {
+        busRead(dev, ICM456XX_REG_MISC2, &tmp);
+        if (!(tmp & ICM456XX_SOFT_RESET)) {
+            break;
+        }
+        delay(1);
+        waitedMs++;
+        if (waitedMs >= 20) {
+            return false;
+        }
+    }
+    // Initialize power management to a known state after reset
+    // This ensures sensors are off and ready for proper initialization
+    busWrite(dev, ICM456XX_PWR_MGMT0, 0x00);
+
+    do {
+        delay(150);
+        busRead(dev, ICM456XX_WHO_AM_REGISTER, &tmp);
+        if (tmp == ICM45686_WHO_AM_I_CONST) {
+            return true;
+        }
+    } while (attemptsRemaining--);
+
+    return false;
+}
+
+bool icm45686AccDetect(accDev_t *acc)
+{
+    acc->busDev = busDeviceOpen(BUSTYPE_ANY, DEVHW_ICM45686, acc->imuSensorToUse);
+    if (acc->busDev == NULL) {
+        return false;
+    }
+
+    mpuContextData_t * ctx = busDeviceGetScratchpadMemory(acc->busDev);
+    if (ctx->chipMagicNumber != 0x4265) {
+        return false;
+    }
+
+    acc->initFn = icm45686AccInit;
+    acc->readFn = icm45686AccRead;
+    acc->accAlign = acc->busDev->param;
+
+    return true;
+}
+
+bool icm45686GyroDetect(gyroDev_t *gyro)
+{
+    gyro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_ICM45686, gyro->imuSensorToUse, OWNER_MPU);
+    if (gyro->busDev == NULL) {
+        return false;
+    }
+
+    if (!icm45686DeviceDetect(gyro->busDev)) {
+        busDeviceDeInit(gyro->busDev);
+        return false;
+    }
+
+    // Magic number for ACC detection to indicate that we have detected icm45686 gyro
+    mpuContextData_t * ctx = busDeviceGetScratchpadMemory(gyro->busDev);
+    ctx->chipMagicNumber = 0x4265;
+
+    gyro->initFn = icm45686AccAndGyroInit;
+    gyro->readFn = icm45686GyroRead;
+    gyro->intStatusFn = gyroCheckDataReady;
+    gyro->temperatureFn = icm45686ReadTemperature;
+    gyro->scale = 1.0f / 16.4f;     // 16.4 dps/lsb scalefactor
+    gyro->gyroAlign = gyro->busDev->param;
+
+    return true;
+}
+
+
+#endif
\ No newline at end of file
diff --git a/src/main/drivers/accgyro/accgyro_icm45686.h b/src/main/drivers/accgyro/accgyro_icm45686.h
new file mode 100644
index 00000000000..83c630e13bc
--- /dev/null
+++ b/src/main/drivers/accgyro/accgyro_icm45686.h
@@ -0,0 +1,22 @@
+/*
+ * 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
+
+
+bool icm45686AccDetect(accDev_t *acc);
+bool icm45686GyroDetect(gyroDev_t *gyro);
\ No newline at end of file
diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h
index 62c2778b45b..16089f5f542 100644
--- a/src/main/drivers/accgyro/accgyro_mpu.h
+++ b/src/main/drivers/accgyro/accgyro_mpu.h
@@ -30,6 +30,7 @@
 #define ICM20689_WHO_AM_I_CONST             (0x98)
 #define ICM42605_WHO_AM_I_CONST             (0x42)
 #define ICM42688P_WHO_AM_I_CONST            (0x47)
+#define ICM45686_WHO_AM_I_CONST             (0xE9)
 
 
 // RA = Register Address
@@ -181,4 +182,4 @@ const gyroFilterAndRateConfig_t * mpuChooseGyroConfig(uint8_t desiredLpf, uint16
 bool mpuGyroRead(struct gyroDev_s *gyro);
 bool mpuGyroReadScratchpad(struct gyroDev_s *gyro);
 bool mpuAccReadScratchpad(struct accDev_s *acc);
-bool mpuTemperatureReadScratchpad(struct gyroDev_s *gyro, int16_t * data);
+bool mpuTemperatureReadScratchpad(struct gyroDev_s *gyro, int16_t * data);
\ No newline at end of file
diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h
index 5a3e6dba453..3c26067a557 100644
--- a/src/main/drivers/bus.h
+++ b/src/main/drivers/bus.h
@@ -85,6 +85,7 @@ typedef enum {
     DEVHW_ICM42605,
     DEVHW_BMI270,
     DEVHW_LSM6D,
+    DEVHW_ICM45686,
     /* Combined ACC/GYRO/MAG chips */
     DEVHW_MPU9250,
 
@@ -330,4 +331,4 @@ bool busWrite(const busDevice_t * busdev, uint8_t reg, uint8_t data);
 bool busTransfer(const busDevice_t * dev, uint8_t * rxBuf, const uint8_t * txBuf, int length);
 bool busTransferMultiple(const busDevice_t * dev, busTransferDescriptor_t * buffers, int count);
 
-bool busIsBusy(const busDevice_t * dev);
+bool busIsBusy(const busDevice_t * dev);
\ No newline at end of file
diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index 6c60f08c6ed..092855e5f99 100644
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -227,7 +227,7 @@ static const char *debugModeNames[DEBUG_COUNT] = {
 // sync with gyroSensor_e
 static const char *const gyroNames[] = {
     "NONE",     "AUTO",   "MPU6000",  "MPU6500", "MPU9250", "BMI160",
-    "ICM20689", "BMI088", "ICM42605", "BMI270",  "LSM6DXX", "FAKE"};
+    "ICM20689", "BMI088", "ICM42605", "BMI270",  "LSM6DXX", "ICM45686", "FAKE"};
 
 // sync this with sensors_e
 static const char * const sensorTypeNames[] = {
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 476dfe09ff7..cab48e08a15 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -2,7 +2,7 @@ tables:
   - name: alignment
     values: ["DEFAULT", "CW0", "CW90", "CW180", "CW270", "CW0FLIP", "CW90FLIP", "CW180FLIP", "CW270FLIP"]
   - name: acc_hardware
-    values: ["NONE", "AUTO", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "BMI270","LSM6DXX",  "FAKE"]
+    values: ["NONE", "AUTO", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "BMI270","LSM6DXX", ICM45686, "FAKE"]
     enum: accelerationSensor_e
   - name: rangefinder_hardware
     values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C", "FAKE", "TERARANGER_EVO", "USD1_V0", "NRA"]
diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c
index 16d10624c69..d0c913623ed 100644
--- a/src/main/sensors/acceleration.c
+++ b/src/main/sensors/acceleration.c
@@ -45,6 +45,7 @@
 #include "drivers/accgyro/accgyro_bmi270.h"
 #include "drivers/accgyro/accgyro_icm20689.h"
 #include "drivers/accgyro/accgyro_icm42605.h"
+#include "drivers/accgyro/accgyro_icm45686.h"
 #include "drivers/accgyro/accgyro_lsm6dxx.h"
 #include "drivers/accgyro/accgyro_fake.h"
 #include "drivers/sensor.h"
@@ -247,6 +248,18 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
         }
         FALLTHROUGH;
 #endif
+#ifdef USE_IMU_ICM45686
+    case ACC_ICM45686:
+        if (icm45686AccDetect(dev)) {
+            accHardware = ACC_ICM45686;
+            break;
+        }
+        /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
+        if (accHardwareToUse != ACC_AUTODETECT) {
+            break;
+        }
+        FALLTHROUGH;
+#endif
 #ifdef USE_IMU_FAKE
     case ACC_FAKE:
         if (fakeAccDetect(dev)) {
@@ -696,4 +709,4 @@ void accInitFilters(void)
 bool accIsHealthy(void)
 {
     return true;
-}
+}
\ No newline at end of file
diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h
index 2e2bfa461e0..1037f85ca37 100644
--- a/src/main/sensors/acceleration.h
+++ b/src/main/sensors/acceleration.h
@@ -44,6 +44,7 @@ typedef enum {
     ACC_ICM42605,
     ACC_BMI270,
     ACC_LSM6DXX,
+    ACC_ICM45686,
     ACC_FAKE,
     ACC_MAX = ACC_FAKE
 } accelerationSensor_e;
@@ -97,4 +98,4 @@ void accSetCalibrationValues(void);
 void accInitFilters(void);
 bool accIsHealthy(void);
 bool accGetCalibrationAxisStatus(int axis);
-uint8_t accGetCalibrationAxisFlags(void);
+uint8_t accGetCalibrationAxisFlags(void);
\ No newline at end of file
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 9fa0a7c0e4d..5abc4980f97 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -47,6 +47,7 @@
 #include "drivers/accgyro/accgyro_bmi270.h"
 #include "drivers/accgyro/accgyro_icm20689.h"
 #include "drivers/accgyro/accgyro_icm42605.h"
+#include "drivers/accgyro/accgyro_icm45686.h"
 #include "drivers/accgyro/accgyro_lsm6dxx.h"
 #include "drivers/accgyro/accgyro_fake.h"
 #include "drivers/io.h"
@@ -228,6 +229,15 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
         FALLTHROUGH;
 #endif
 
+#ifdef USE_IMU_ICM45686
+    case GYRO_ICM45686:
+        if (icm45686GyroDetect(dev)) {
+            gyroHardware = GYRO_ICM45686;
+            break;
+        }
+        FALLTHROUGH;
+#endif
+
 #ifdef USE_IMU_FAKE
     case GYRO_FAKE:
         if (fakeGyroDetect(dev)) {
@@ -609,4 +619,4 @@ void gyroUpdateDynamicLpf(float cutoffFreq) {
 float averageAbsGyroRates(void)
 {
     return (fabsf(gyro.gyroADCf[ROLL]) + fabsf(gyro.gyroADCf[PITCH]) + fabsf(gyro.gyroADCf[YAW])) / 3.0f;
-}
+}
\ No newline at end of file
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index de78b2f81c2..d04eb2f705d 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -43,8 +43,8 @@ typedef enum {
     GYRO_ICM42605,
     GYRO_BMI270,
     GYRO_LSM6DXX,
+    GYRO_ICM45686,
     GYRO_FAKE
-   
 } gyroSensor_e;
 
 typedef enum {
@@ -120,4 +120,4 @@ bool gyroReadTemperature(void);
 int16_t gyroGetTemperature(void);
 int16_t gyroRateDps(int axis);
 void gyroUpdateDynamicLpf(float cutoffFreq);
-float averageAbsGyroRates(void);
+float averageAbsGyroRates(void);
\ No newline at end of file
diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c
index aac3db564cc..020e3f7b35c 100755
--- a/src/main/target/common_hardware.c
+++ b/src/main/target/common_hardware.c
@@ -82,6 +82,10 @@
         BUSDEV_REGISTER_SPI(busdev_lsm6dxx,      DEVHW_LSM6D,       LSM6DXX_SPI_BUS,     LSM6DXX_CS_PIN,      NONE,  DEVFLAGS_NONE,  IMU_LSM6DXX_ALIGN);
     #endif
 
+    #if defined(USE_IMU_ICM45686)
+        BUSDEV_REGISTER_SPI(busdev_icm45686,    DEVHW_ICM45686,     ICM45686_SPI_BUS,   ICM45686_CS_PIN,    NONE,  DEVFLAGS_NONE,  IMU_ICM45686_ALIGN);
+    #endif
+
 #endif
 
 
@@ -473,4 +477,4 @@
     BUSDEV_REGISTER_I2C(busdev_pcf8574,      DEVHW_PCF8574,       PCF8574_I2C_BUS,     0x20,               NONE,           DEVFLAGS_NONE, 0);
 #endif
 
-#endif  // USE_TARGET_HARDWARE_DESCRIPTORS
+#endif  // USE_TARGET_HARDWARE_DESCRIPTORS
\ No newline at end of file

From 8a475617c479a0fad7b3369ccea6c930f571eab4 Mon Sep 17 00:00:00 2001
From: Scavanger <andreas.kanzler@outlook.com>
Date: Fri, 13 Mar 2026 21:30:03 -0300
Subject: [PATCH 040/102] SITL Tweaks

---
 cmake/main.cmake                              |   6 +-
 cmake/sitl.cmake                              |  10 +-
 src/main/config/config_streamer_file.c        |   1 -
 src/main/scheduler/scheduler.c                |  50 ++
 src/main/target/SITL/sim/realFlight.c         | 217 ++++----
 src/main/target/SITL/sim/realFlight.h         |   1 +
 src/main/target/SITL/sim/simple_soap_client.c | 172 ------
 src/main/target/SITL/sim/soap_client.c        | 516 ++++++++++++++++++
 .../{simple_soap_client.h => soap_client.h}   |  34 +-
 src/main/target/SITL/sim/xplane.c             | 118 ++--
 src/main/target/SITL/sim/xplane.h             |   1 +
 src/main/target/SITL/target.c                 |  29 +-
 12 files changed, 822 insertions(+), 333 deletions(-)
 delete mode 100644 src/main/target/SITL/sim/simple_soap_client.c
 create mode 100644 src/main/target/SITL/sim/soap_client.c
 rename src/main/target/SITL/sim/{simple_soap_client.h => soap_client.h} (60%)

diff --git a/cmake/main.cmake b/cmake/main.cmake
index d8e2dddf3ec..1b987204b25 100644
--- a/cmake/main.cmake
+++ b/cmake/main.cmake
@@ -108,10 +108,10 @@ function(setup_firmware_target exe name)
 endfunction()
 
 function(exclude_from_all target)
-    set_property(TARGET ${target} PROPERTY
+    set_target_properties(${target} PROPERTIES
         TARGET_MESSAGES OFF
-        EXCLUDE_FROM_ALL 1
-        EXCLUDE_FROM_DEFAULT_BUILD 1)
+        EXCLUDE_FROM_ALL ON
+        EXCLUDE_FROM_DEFAULT_BUILD ON)
 endfunction()
 
 function(collect_targets)
diff --git a/cmake/sitl.cmake b/cmake/sitl.cmake
index 39e6456830a..d93f0b89d7d 100644
--- a/cmake/sitl.cmake
+++ b/cmake/sitl.cmake
@@ -19,8 +19,8 @@ main_sources(SITL_SRC
     target/SITL/sim/realFlight.h
     target/SITL/sim/simHelper.c
     target/SITL/sim/simHelper.h
-    target/SITL/sim/simple_soap_client.c
-    target/SITL/sim/simple_soap_client.h
+    target/SITL/sim/soap_client.c
+    target/SITL/sim/soap_client.h
     target/SITL/sim/xplane.c
     target/SITL/sim/xplane.h
 )
@@ -159,8 +159,8 @@ function (target_sitl name)
             WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
             COMMAND ${generator_cmd} clean
             COMMENT "Removing intermediate files for ${name}")
-        set_property(TARGET ${clean_target} PROPERTY
-            EXCLUDE_FROM_ALL 1
-            EXCLUDE_FROM_DEFAULT_BUILD 1)
+        set_target_properties(${clean_target} PROPERTIES
+            EXCLUDE_FROM_ALL ON
+            EXCLUDE_FROM_DEFAULT_BUILD ON)
     endif()
 endfunction()
diff --git a/src/main/config/config_streamer_file.c b/src/main/config/config_streamer_file.c
index 379155c22e1..86d677498c1 100644
--- a/src/main/config/config_streamer_file.c
+++ b/src/main/config/config_streamer_file.c
@@ -103,7 +103,6 @@ int config_streamer_impl_write_word(config_streamer_t *c, config_streamer_buffer
 
     if ((c->address >= (uintptr_t)eepromData) && (c->address < (uintptr_t)ARRAYEND(eepromData))) {
         *((uint32_t*)c->address) = *buffer;
-        fprintf(stderr, "[EEPROM] Program word  %p = %08x\n", (void*)c->address, *((uint32_t*)c->address));
     } else {
         fprintf(stderr, "[EEPROM] Program word %p out of range!\n", (void*)c->address);
     }
diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c
index e04a92a8993..ad21c32359e 100755
--- a/src/main/scheduler/scheduler.c
+++ b/src/main/scheduler/scheduler.c
@@ -19,6 +19,10 @@
 #include <stdint.h>
 #include <string.h>
 
+#if defined(SITL_BUILD)
+#include <unistd.h>
+#endif
+
 #include "platform.h"
 
 #include "scheduler.h"
@@ -215,11 +219,22 @@ void FAST_CODE NOINLINE scheduler(void)
     uint16_t selectedTaskDynamicPriority = 0;
     bool forcedRealTimeTask = false;
 
+#if defined(SITL_BUILD)
+    // Track the earliest time at which the next task will become due so we can
+    // sleep until then instead of busy-waiting.  Cap at 1 ms so event-driven
+    // tasks (checkFunc) are still polled frequently enough.
+    timeUs_t sitlEarliestNextTaskAt = currentTimeUs + 1000;
+    bool sitlHasCheckFuncTask = false;
+#endif
+
     // Update task dynamic priorities
     uint16_t waitingTasks = 0;
     for (cfTask_t *task = queueFirst(); task != NULL; task = queueNext()) {
         // Task has checkFunc - event driven
         if (task->checkFunc) {
+#if defined(SITL_BUILD)
+            sitlHasCheckFuncTask = true;
+#endif
             const timeUs_t currentTimeBeforeCheckFuncCallUs = micros();
 
             // Increase priority for event driven tasks
@@ -248,7 +263,19 @@ void FAST_CODE NOINLINE scheduler(void)
                 waitingTasks++;
                 forcedRealTimeTask = true;
             }
+#if defined(SITL_BUILD)
+            const timeUs_t taskNextAt = task->lastExecutedAt + (timeUs_t)task->desiredPeriod;
+            if (taskNextAt < sitlEarliestNextTaskAt) {
+                sitlEarliestNextTaskAt = taskNextAt;
+            }            
+#endif
         } else {
+#if defined(SITL_BUILD)
+            const timeUs_t taskNextAt = task->lastExecutedAt + (timeUs_t)task->desiredPeriod;
+            if (taskNextAt < sitlEarliestNextTaskAt) {
+                sitlEarliestNextTaskAt = taskNextAt;
+            }            
+#endif
             // Task is time-driven, dynamicPriority is last execution age (measured in desiredPeriods)
             // Task age is calculated from last execution
             task->taskAgeCycles = ((timeDelta_t)(currentTimeUs - task->lastExecutedAt)) / task->desiredPeriod;
@@ -294,4 +321,27 @@ void FAST_CODE NOINLINE scheduler(void)
         selectedTask->totalExecutionTime += taskExecutionTime;   // time consumed by scheduler + task
         selectedTask->maxExecutionTime = MAX(selectedTask->maxExecutionTime, taskExecutionTime);
     }
+
+#if defined(SITL_BUILD)
+    {
+        // Avoid busy-waiting and burning 100% CPU in SITL.  After executing the
+        // current task (or finding nothing to do), sleep until just before the
+        // next task is due.  For event-driven tasks (checkFunc) we limit the
+        // sleep so the check function is still called frequently.
+        if (sitlHasCheckFuncTask) {
+            // Poll event-driven tasks at least every 500 µs
+            const timeUs_t eventCap = micros() + 500;
+            if (eventCap < sitlEarliestNextTaskAt) {
+                sitlEarliestNextTaskAt = eventCap;
+            }
+        }
+        const timeUs_t nowUs = micros();
+        if (sitlEarliestNextTaskAt > nowUs + 50) {
+            const timeDelta_t sleepUs = (timeDelta_t)(sitlEarliestNextTaskAt - nowUs) - 50;
+            if (sleepUs > 0) {
+                usleep((useconds_t)sleepUs);
+            }
+        }
+    }
+#endif
 }
diff --git a/src/main/target/SITL/sim/realFlight.c b/src/main/target/SITL/sim/realFlight.c
index a8d241425d1..6477de23f42 100644
--- a/src/main/target/SITL/sim/realFlight.c
+++ b/src/main/target/SITL/sim/realFlight.c
@@ -28,14 +28,14 @@
 #include <string.h>
 #include <stdarg.h>
 #include <pthread.h>
+#include <stdatomic.h>
 #include <math.h>
 
 #include "platform.h"
 
 #include "target.h"
 #include "target/SITL/sim/realFlight.h"
-#include "target/SITL/sim/simple_soap_client.h"
-#include "target/SITL/sim/xplane.h"
+#include "target/SITL/sim/soap_client.h"
 #include "target/SITL/sim/simHelper.h"
 #include "fc/runtime_config.h"
 #include "drivers/time.h"
@@ -55,8 +55,9 @@
 #include "flight/imu.h"
 #include "io/gps.h"
 #include "rx/sim.h"
+#include "realFlight.h"
 
-#define RF_PORT 18083
+#define RF_PORT "18083"
 #define RF_MAX_CHANNEL_COUNT 12
 
 // "RealFlight Ranch" is located in Sierra Nevada, southern Spain
@@ -67,17 +68,14 @@
 static uint8_t pwmMapping[RF_MAX_PWM_OUTS];
 static uint8_t mappingCount;
 
-static pthread_cond_t sockcond1 = PTHREAD_COND_INITIALIZER;
-static pthread_cond_t sockcond2 = PTHREAD_COND_INITIALIZER;
-static pthread_mutex_t sockmtx = PTHREAD_MUTEX_INITIALIZER;
+static pthread_mutex_t initMutex = PTHREAD_MUTEX_INITIALIZER;
+static pthread_cond_t initCond = PTHREAD_COND_INITIALIZER;
+static atomic_bool shouldStopSoapThread = false;
 
-static soap_client_t *client = NULL;
-static soap_client_t *clientNext = NULL;
+static soap_client_t client;
+static pthread_t soapThread = 0;
 
-static pthread_t soapThread;
-static pthread_t creationThread;
-
-static bool isInitalised = false;
+static atomic_bool isInitalised = false;
 static bool useImu = false;
 
 typedef struct 
@@ -133,38 +131,6 @@ typedef struct
 
 rfValues_t rfValues; 
 
-static void deleteClient(soap_client_t *client)
-{
-    soapClientClose(client);
-    free(client);
-    client = NULL;
-}
-
-static void startRequest(char* action, const char* fmt, ...)
-{
-    pthread_mutex_lock(&sockmtx);
-    while (clientNext == NULL) {
-        pthread_cond_wait(&sockcond1, &sockmtx);
-    }
-
-    client = clientNext;
-    clientNext = NULL;
-
-    pthread_cond_broadcast(&sockcond2);
-    pthread_mutex_unlock(&sockmtx);
-
-    va_list va;
-    va_start(va, fmt);
-    soapClientSendRequestVa(client, action, fmt, va);
-    va_end(va);
-}
-
-static char* endRequest(void)
-{
-   char* ret = soapClientReceive(client); 
-   deleteClient(client);
-   return ret;   
-}
 
 // Simple, but fast ;)
 static double getDoubleFromResponse(const char* response, const char* elementName)
@@ -261,6 +227,7 @@ static float convertAzimuth(float azimuth)
     return 360 - fmodf(azimuth + 90, 360.0f);
 }
 
+
 static void exchangeData(void)
 {
     double servoValues[RF_MAX_PWM_OUTS] = {  };    
@@ -272,14 +239,32 @@ static void exchangeData(void)
         }
     }
 
-    startRequest("ExchangeData", "<ExchangeData><pControlInputs><m-selectedChannels>%u</m-selectedChannels>"
-        "<m-channelValues-0to1><item>%.4f</item><item>%.4f</item><item>%.4f</item><item>%.4f</item><item>%.4f</item><item>%.4f</item><item>%.4f</item><item>%.4f</item>"
-        "<item>%.4f</item><item>%.4f</item><item>%.4f</item><item>%.4f</item></m-channelValues-0to1></pControlInputs></ExchangeData>",
-        0xFFF, 
-        servoValues[0], servoValues[1], servoValues[2], servoValues[3], servoValues[4], servoValues[5], servoValues[6], servoValues[7],
-        servoValues[8], servoValues[9], servoValues[10], servoValues[11]);
-    char* response = endRequest();
+    char requestBody[1024] = "<ExchangeData><pControlInputs><m-selectedChannels>12</m-selectedChannels><m-channelValues-0to1>";
+    
+    for (int i = 0; i < RF_MAX_CHANNEL_COUNT; i++) {
+        char value[32];
+        snprintf(value, sizeof(value), "<item>%.4f</item>", servoValues[i]);
+        strncat(requestBody, value, sizeof(requestBody) - strlen(requestBody) - 1);
+    }
+    strncat(requestBody, "</m-channelValues-0to1></pControlInputs></ExchangeData>", sizeof(requestBody) - strlen(requestBody) - 1);
+
+    char *response = NULL;
+    int http_status = 0; 
 
+    int ret = soap_client_call_raw_body(
+        &client,
+        "ExchangeData",
+        requestBody,
+        &response,
+        &http_status
+    );
+    
+    if (ret < 0 || http_status != 200 || !response) {
+        fprintf(stderr, "[SIM] Data exchange with RealFlight failed.\n");
+        free(response);
+        return;
+    }
+  
     //rfValues.m_currentPhysicsTime_SEC = getDoubleFromResponse(response, "m-currentPhysicsTime-SEC");
     //rfValues.m_currentPhysicsSpeedMultiplier = getDoubleFromResponse(response, "m-currentPhysicsSpeedMultiplier");
     rfValues.m_airspeed_MPS = getDoubleFromResponse(response, "m-airspeed-MPS");
@@ -410,50 +395,71 @@ static void exchangeData(void)
     free(response);
 }
 
-static void* soapWorker(void* arg)
+static bool restoreOriginalControllerDevice(void)
 {
-    UNUSED(arg);
-    while(true)
-    {     
-        if (!isInitalised) {
-            startRequest("RestoreOriginalControllerDevice", "<RestoreOriginalControllerDevice><a>1</a><b>2</b></RestoreOriginalControllerDevice>");
-            free(endRequest());
-            startRequest("InjectUAVControllerInterface", "<InjectUAVControllerInterface><a>1</a><b>2</b></InjectUAVControllerInterface>");
-            free(endRequest());  
-            exchangeData();
-            ENABLE_ARMING_FLAG(SIMULATOR_MODE_SITL);
-            
-            isInitalised = true;  
-        }
+    char* response = NULL;
+    int http_status = 0;
+    const int ret = soap_client_call_raw_body(
+        &client,
+        "RestoreOriginalControllerDevice",
+        "<RestoreOriginalControllerDevice><a>1</a><b>2</b></RestoreOriginalControllerDevice>",
+        &response,
+        &http_status
+    );
 
-        exchangeData();
-        unlockMainPID();
+    if (ret < 0 || (http_status != 200 && http_status != 500) || !response) {
+        free(response);
+        return false;
     }
 
-    return NULL;
+    free(response);
+    return true;
 }
 
-
-static void* creationWorker(void* arg)
+static void* soapWorker(void* arg)
 {
-    char* ip = (char*)arg;
-    
-    while (true) {
-        pthread_mutex_lock(&sockmtx);
-        while (clientNext != NULL) {
-            pthread_cond_wait(&sockcond2, &sockmtx);
-        }
-        pthread_mutex_unlock(&sockmtx);
-        
-        soap_client_t *cli = malloc(sizeof(soap_client_t));
-        if (!soapClientConnect(cli, ip, RF_PORT)) {
-            continue;
+    UNUSED(arg);
+    while(!atomic_load(&shouldStopSoapThread)) {
+
+        if (!atomic_load(&isInitalised)) {
+            // Initialize RealFlight
+
+            // Alway try to restore the original controller device first to avoid problems with broken connections and the interface being stuck in a half-initialised state. 
+            // RealFlight seems to not properly close the connection on its side if the connection is interrupted, but only after a timeout of about 30 seconds. 
+            // During this time the interface is not usable, but without this step it would be stuck in an unusable state until the next restart of the SITL.
+            if (!restoreOriginalControllerDevice()) {
+                fprintf(stderr, "[SIM] Failed to restore original controller device in RealFlight. RealFlight might be in a broken state. Retrying...\n");
+                delay(1000);
+                continue;
+            }
+
+            char* response = NULL;
+            int http_status = 0;
+            const int ret = soap_client_call_raw_body(
+                &client,
+                "InjectUAVControllerInterface",
+                "<InjectUAVControllerInterface><a>1</a><b>2</b></InjectUAVControllerInterface>",
+                &response,
+                &http_status
+            );
+
+            if (ret < 0 || http_status != 200 || !response) {
+                free(response);
+                delay(1000);
+                continue; 
+            }
+            
+            exchangeData(); // Get initial data and set initial state in RealFlight
+            
+            ENABLE_ARMING_FLAG(SIMULATOR_MODE_SITL);
+            atomic_store(&isInitalised, true);
+            pthread_mutex_lock(&initMutex);
+            pthread_cond_signal(&initCond);
+            pthread_mutex_unlock(&initMutex);
         }
-        
-        clientNext = cli;
-        pthread_mutex_lock(&sockmtx);
-        pthread_cond_broadcast(&sockcond1);
-        pthread_mutex_unlock(&sockmtx);
+
+        exchangeData();
+        unlockMainPID();
     }
 
     return NULL;
@@ -465,19 +471,46 @@ bool simRealFlightInit(char* ip, uint8_t* mapping, uint8_t mapCount, bool imu)
     mappingCount = mapCount;
     useImu = imu;
 
-    if (pthread_create(&soapThread, NULL, soapWorker, NULL) < 0) {
+    if (soap_client_init(&client, ip, RF_PORT, "/", 1000) < 0) {
         return false;
     }
 
-    if (pthread_create(&creationThread, NULL, creationWorker, (void*)ip) < 0) {
+    atomic_store(&isInitalised, false);
+    atomic_store(&shouldStopSoapThread, false);
+    if (pthread_create(&soapThread, NULL, soapWorker, &client) < 0) {
         return false;
     }
 
     // Wait until the connection is established, the interface has been initialised 
-    // and the first valid packet has been received to avoid problems with the startup calibration.   
-    while (!isInitalised) {
-        delay(250);
+    // and the first valid packet has been received to avoid problems with the startup calibration.
+    pthread_mutex_lock(&initMutex);
+    while (!atomic_load(&isInitalised)) {
+        pthread_cond_wait(&initCond, &initMutex);
     }
+    pthread_mutex_unlock(&initMutex);
 
     return true;
 }
+
+void simRealFlightClose(void)
+{   
+    atomic_store(&shouldStopSoapThread, true);
+    if (soapThread) {
+        pthread_join(soapThread, NULL);
+    }
+
+    if (atomic_load(&isInitalised)) {
+
+        if (!restoreOriginalControllerDevice( )) {
+            fprintf(stderr, "[SIM] Failed to restore original controller device in RealFlight.\n");
+        } else {
+            fprintf(stderr, "[SIM] Restored original controller device in RealFlight.\n");
+        }
+    }
+
+    DISABLE_ARMING_FLAG(SIMULATOR_MODE_SITL);
+    atomic_store(&isInitalised, false);
+    pthread_mutex_destroy(&initMutex);
+    pthread_cond_destroy(&initCond);
+    soap_client_destroy(&client);
+}
diff --git a/src/main/target/SITL/sim/realFlight.h b/src/main/target/SITL/sim/realFlight.h
index af34c2c524e..80e27c9087c 100644
--- a/src/main/target/SITL/sim/realFlight.h
+++ b/src/main/target/SITL/sim/realFlight.h
@@ -27,3 +27,4 @@
 #define RF_MAX_PWM_OUTS 12
 
 bool simRealFlightInit(char* ip, uint8_t* mapping, uint8_t mapCount, bool imu);
+void simRealFlightClose(void);
diff --git a/src/main/target/SITL/sim/simple_soap_client.c b/src/main/target/SITL/sim/simple_soap_client.c
deleted file mode 100644
index e6400ae3b14..00000000000
--- a/src/main/target/SITL/sim/simple_soap_client.c
+++ /dev/null
@@ -1,172 +0,0 @@
-/*
- * 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/.
- */
-
-#define _GNU_SOURCE
-
-#include <stdbool.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <stdarg.h>
-#include <unistd.h>
-#include <string.h>
-#include <sys/socket.h>
-# include <netinet/in.h>
-# include <netdb.h>
-#include <fcntl.h>
-#include <sys/select.h>
-
-#include "simple_soap_client.h"
-
-#define REC_BUF_SIZE 6000
-char recBuffer[REC_BUF_SIZE];
-
-bool soapClientConnect(soap_client_t *client, const char *address, int port)
-{
-    client->sockedFd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
-    if (client->sockedFd < 0) {
-        return false;
-    }
-
-    int one = 1;
-    if (setsockopt(client->sockedFd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)) < 0) {
-        return false;
-    }
-
-    client->socketAddr.sin_family = AF_INET;
-    client->socketAddr.sin_port = htons(port);
-    client->socketAddr.sin_addr.s_addr = inet_addr(address);
-
-    if (connect(client->sockedFd, (struct sockaddr*)&client->socketAddr, sizeof(client->socketAddr)) < 0) {
-        return false;
-    }
-
-    client->isConnected = true;
-    client->isInitalised = true;
-
-    return true;
-}
-
-void soapClientClose(soap_client_t *client)
-{
-    close(client->sockedFd);
-    memset(client, 0, sizeof(soap_client_t));
-    client->isConnected = false;
-    client->isInitalised = false;
-}
-
-void soapClientSendRequestVa(soap_client_t *client, const char* action, const char *fmt, va_list va)
-{
-     if (!client->isConnected) {
-        return;
-    }
-
-    char* requestBody;
-    if (vasprintf(&requestBody, fmt, va) < 0) {
-        return;
-    }
-
-    char* request;
-    if (asprintf(&request, "POST / HTTP/1.1\r\nsoapaction: %s\r\ncontent-length: %u\r\ncontent-type: text/xml;charset='UTF-8'\r\nConnection: Keep-Alive\r\n\r\n<soap:Envelope xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\" xmlns:xsd=\"http://www.w3.org/2001/XMLSchema\" xmlns:soap=\"http://schemas.xmlsoap.org/soap/envelope/\"><soap:Body>%s</soap:Body></soap:Envelope>",
-         action, (unsigned)strlen(requestBody), requestBody) < 0) {
-            return;
-         }
-
-    send(client->sockedFd, request, strlen(request), 0);
-
-    free(requestBody);
-    free(request);
-}
-
-void soapClientSendRequest(soap_client_t *client, const char* action, const char *fmt, ...)
-{
-    va_list va;
-
-    va_start(va, fmt);
-    soapClientSendRequestVa(client, action, fmt, va);
-    va_end(va);
-}
-
-static bool soapClientPoll(soap_client_t *client, uint32_t timeout_ms)
-{
-    fd_set fds;
-    struct timeval tv;
-
-    FD_ZERO(&fds);
-    FD_SET(client->sockedFd, &fds);
-
-    tv.tv_sec = timeout_ms / 1000;
-    tv.tv_usec = (timeout_ms % 1000) * 1000UL;
-
-    if (select(client->sockedFd + 1, &fds, NULL, NULL, &tv) != 1) {
-        return false;
-    }
-    return true;
-}
-
-
-char* soapClientReceive(soap_client_t *client)
-{
-     if (!client->isInitalised){
-        return false;
-    }
-
-    if (!soapClientPoll(client, 1000)) {
-        return false;
-    }
-
-    ssize_t size = recv(client->sockedFd, recBuffer, REC_BUF_SIZE, 0);
-
-    if (size <= 0) {
-        return NULL;
-    }
-
-    char* pos = strstr(recBuffer, "Content-Length: ");
-    if (!pos) {
-       return NULL;
-    }
-
-    uint32_t contentLength = strtoul(pos + 16, NULL, 10);
-    char *body = strstr(pos, "\r\n\r\n");
-    if (!body) {
-        return NULL;
-    }
-
-    body += 4;
-
-    ssize_t expectedLength = contentLength + body - recBuffer;
-    if ((unsigned)expectedLength >= sizeof(recBuffer)) {
-        return NULL;
-    }
-
-    while (size < expectedLength){
-        ssize_t size2 = recv(client->sockedFd, &recBuffer[size], sizeof(recBuffer - size + 1), 0);
-        if (size2 <= 0) {
-            return NULL;
-        }
-        size += size2;
-    }
-
-    recBuffer[size] = '\0';
-    return strdup(body);
-}
diff --git a/src/main/target/SITL/sim/soap_client.c b/src/main/target/SITL/sim/soap_client.c
new file mode 100644
index 00000000000..6ac5f65826a
--- /dev/null
+++ b/src/main/target/SITL/sim/soap_client.c
@@ -0,0 +1,516 @@
+/*
+ * 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/.
+ */
+
+#define _POSIX_C_SOURCE 200809L
+
+#include "soap_client.h"
+
+#include <arpa/inet.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <netdb.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/socket.h>
+#include <sys/time.h>
+#include <unistd.h>
+
+static int set_socket_timeout(int fd, int timeout_ms) {
+    struct timeval tv;
+    tv.tv_sec = timeout_ms / 1000;
+    tv.tv_usec = (timeout_ms % 1000) * 1000;
+
+    if (setsockopt(fd, SOL_SOCKET, SO_SNDTIMEO, &tv, sizeof(tv)) != 0) {
+        return -1;
+    }
+    if (setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)) != 0) {
+        return -1;
+    }
+    return 0;
+}
+
+static int connect_with_timeout(const struct addrinfo* ai, int timeout_ms) {
+    int fd = socket(ai->ai_family, ai->ai_socktype, ai->ai_protocol);
+    if (fd < 0) {
+        return -1;
+    }
+
+    int flags = fcntl(fd, F_GETFL, 0);
+    if (flags < 0) {
+        close(fd);
+        return -1;
+    }
+    if (fcntl(fd, F_SETFL, flags | O_NONBLOCK) < 0) {
+        close(fd);
+        return -1;
+    }
+
+    int one = 1;
+    if (setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)) != 0) {
+        close(fd);
+        return -1;
+    }
+
+    int rc = connect(fd, ai->ai_addr, ai->ai_addrlen);
+    if (rc == 0) {
+        (void)fcntl(fd, F_SETFL, flags);
+        return fd;
+    }
+    if (errno != EINPROGRESS) {
+        close(fd);
+        return -1;
+    }
+
+    fd_set wfds;
+    FD_ZERO(&wfds);
+    FD_SET(fd, &wfds);
+    struct timeval tv;
+    tv.tv_sec = timeout_ms / 1000;
+    tv.tv_usec = (timeout_ms % 1000) * 1000;
+
+    rc = select(fd + 1, NULL, &wfds, NULL, &tv);
+    if (rc <= 0) {
+        close(fd);
+        errno = (rc == 0) ? ETIMEDOUT : errno;
+        return -1;
+    }
+
+    int so_error = 0;
+    socklen_t so_error_len = sizeof(so_error);
+    if (getsockopt(fd, SOL_SOCKET, SO_ERROR, &so_error, &so_error_len) != 0 || so_error != 0) {
+        close(fd);
+        errno = (so_error != 0) ? so_error : errno;
+        return -1;
+    }
+
+    if (fcntl(fd, F_SETFL, flags) < 0) {
+        close(fd);
+        return -1;
+    }
+
+    return fd;
+}
+
+static int open_tcp_connection(const char* host, const char* port, int timeout_ms) {
+    struct addrinfo hints;
+    memset(&hints, 0, sizeof(hints));
+    hints.ai_family = AF_UNSPEC;
+    hints.ai_socktype = SOCK_STREAM;
+
+    struct addrinfo* result = NULL;
+    if (getaddrinfo(host, port, &hints, &result) != 0) {
+        return -1;
+    }
+
+    int fd = -1;
+    for (const struct addrinfo* ai = result; ai != NULL; ai = ai->ai_next) {
+        fd = connect_with_timeout(ai, timeout_ms);
+        if (fd >= 0) {
+            if (set_socket_timeout(fd, timeout_ms) == 0) {
+                break;
+            }
+            close(fd);
+            fd = -1;
+        }
+    }
+
+    freeaddrinfo(result);
+    return fd;
+}
+
+static int send_all(int fd, const char* data, size_t len) {
+    size_t total = 0;
+    while (total < len) {
+        ssize_t n = send(fd, data + total, len - total, 0);
+        if (n < 0) {
+            if (errno == EINTR) {
+                continue;
+            }
+            return -1;
+        }
+        if (n == 0) {
+            return -1;
+        }
+        total += (size_t)n;
+    }
+    return 0;
+}
+
+static char* recv_all(int fd, size_t* out_len) {
+    size_t cap = 8192;
+    size_t len = 0;
+    char* buf = (char*)malloc(cap);
+    if (!buf) {
+        return NULL;
+    }
+
+    while (1) {
+        if (len == cap) {
+            size_t new_cap = cap * 2;
+            char* next = (char*)realloc(buf, new_cap);
+            if (!next) {
+                free(buf);
+                return NULL;
+            }
+            buf = next;
+            cap = new_cap;
+        }
+
+        ssize_t n = recv(fd, buf + len, cap - len, 0);
+        if (n < 0) {
+            if (errno == EINTR) {
+                continue;
+            }
+            free(buf);
+            return NULL;
+        }
+        if (n == 0) {
+            break;
+        }
+        len += (size_t)n;
+    }
+
+    char* out = (char*)realloc(buf, len + 1);
+    if (!out) {
+        free(buf);
+        return NULL;
+    }
+    out[len] = '\0';
+    *out_len = len;
+    return out;
+}
+
+static int parse_http_status(const char* response) {
+    int status = 0;
+    if (sscanf(response, "HTTP/%*d.%*d %d", &status) == 1) {
+        return status;
+    }
+    return 0;
+}
+
+static char* dechunk_http_body(const char* body, size_t body_len, size_t* out_len) {
+    size_t cap = body_len + 1;
+    char* out = (char*)malloc(cap);
+    if (!out) {
+        return NULL;
+    }
+    size_t out_pos = 0;
+    size_t pos = 0;
+
+    while (pos < body_len) {
+        const char* line_end = strstr(body + pos, "\r\n");
+        if (!line_end) {
+            free(out);
+            return NULL;
+        }
+
+        char size_buf[32];
+        size_t size_len = (size_t)(line_end - (body + pos));
+        if (size_len == 0 || size_len >= sizeof(size_buf)) {
+            free(out);
+            return NULL;
+        }
+        memcpy(size_buf, body + pos, size_len);
+        size_buf[size_len] = '\0';
+
+        char* endptr = NULL;
+        unsigned long chunk_size = strtoul(size_buf, &endptr, 16);
+        if (endptr == size_buf) {
+            free(out);
+            return NULL;
+        }
+
+        pos = (size_t)(line_end - body) + 2;
+        if (chunk_size == 0) {
+            break;
+        }
+        if (pos + chunk_size + 2 > body_len) {
+            free(out);
+            return NULL;
+        }
+
+        if (out_pos + chunk_size + 1 > cap) {
+            size_t new_cap = cap;
+            while (out_pos + chunk_size + 1 > new_cap) {
+                new_cap *= 2;
+            }
+            char* next = (char*)realloc(out, new_cap);
+            if (!next) {
+                free(out);
+                return NULL;
+            }
+            out = next;
+            cap = new_cap;
+        }
+
+        memcpy(out + out_pos, body + pos, chunk_size);
+        out_pos += chunk_size;
+        pos += chunk_size;
+
+        if (!(body[pos] == '\r' && body[pos + 1] == '\n')) {
+            free(out);
+            return NULL;
+        }
+        pos += 2;
+    }
+
+    out[out_pos] = '\0';
+    *out_len = out_pos;
+    return out;
+}
+
+static char* extract_soap_body_raw(const char* xml, size_t xml_len) {
+    const char* p = xml;
+    const char* end = xml + xml_len;
+
+    const char* body_open = NULL;
+    const char* body_name_start = NULL;
+    const char* body_name_end = NULL;
+
+    while (p < end) {
+        const char* lt = strchr(p, '<');
+        if (!lt || lt >= end) {
+            break;
+        }
+        if (lt + 1 < end && (lt[1] == '/' || lt[1] == '?' || lt[1] == '!')) {
+            p = lt + 1;
+            continue;
+        }
+        const char* gt = strchr(lt, '>');
+        if (!gt || gt >= end) {
+            break;
+        }
+
+        const char* name_start = lt + 1;
+        const char* name_end = name_start;
+        while (name_end < gt && *name_end != ' ' && *name_end != '\t' && *name_end != '\r' && *name_end != '\n' && *name_end != '/') {
+            name_end++;
+        }
+
+        size_t name_len = (size_t)(name_end - name_start);
+        if (name_len >= 4 && strncmp(name_end - 4, "Body", 4) == 0) {
+            body_open = gt + 1;
+            body_name_start = name_start;
+            body_name_end = name_end;
+            break;
+        }
+        p = gt + 1;
+    }
+
+    if (!body_open || !body_name_start || !body_name_end) {
+        return NULL;
+    }
+
+    char close_tag[128];
+    size_t tag_len = (size_t)(body_name_end - body_name_start);
+    if (tag_len + 4 >= sizeof(close_tag)) {
+        return NULL;
+    }
+    close_tag[0] = '<';
+    close_tag[1] = '/';
+    memcpy(close_tag + 2, body_name_start, tag_len);
+    close_tag[tag_len + 2] = '>';
+    close_tag[tag_len + 3] = '\0';
+
+    const char* close = strstr(body_open, close_tag);
+    if (!close) {
+        return NULL;
+    }
+
+    size_t inner_len = (size_t)(close - body_open);
+    char* inner = (char*)malloc(inner_len + 1);
+    if (!inner) {
+        return NULL;
+    }
+    memcpy(inner, body_open, inner_len);
+    inner[inner_len] = '\0';
+    return inner;
+}
+
+int soap_client_init(soap_client_t* c, const char* host, const char* port, const char* path, int timeout_ms) {
+    if (!c || !host || !port || !path) {
+        return -1;
+    }
+    memset(c, 0, sizeof(*c));
+    if (snprintf(c->host, sizeof(c->host), "%s", host) >= (int)sizeof(c->host)) {
+        return -1;
+    }
+    if (snprintf(c->port, sizeof(c->port), "%s", port) >= (int)sizeof(c->port)) {
+        return -1;
+    }
+    if (snprintf(c->path, sizeof(c->path), "%s", path) >= (int)sizeof(c->path)) {
+        return -1;
+    }
+    c->timeout_ms = timeout_ms;
+    return pthread_mutex_init(&c->lock, NULL);
+}
+
+void soap_client_destroy(soap_client_t* c) {
+    if (c) {
+        pthread_mutex_destroy(&c->lock);
+    }
+}
+
+int soap_client_call_raw_body(soap_client_t* c,
+                              const char* soap_action,
+                              const char* request_body_xml,
+                              char** response_body_xml,
+                              int* http_status) {
+    if (!c || !request_body_xml || !response_body_xml || !http_status) {
+        return -1;
+    }
+
+    char host[256];
+    char port[16];
+    char path[256];
+    int timeout_ms = 0;
+
+    pthread_mutex_lock(&c->lock);
+    snprintf(host, sizeof(host), "%s", c->host);
+    snprintf(port, sizeof(port), "%s", c->port);
+    snprintf(path, sizeof(path), "%s", c->path);
+    timeout_ms = c->timeout_ms;
+    pthread_mutex_unlock(&c->lock);
+
+    const char* envelope_prefix =
+        "<?xml version=\"1.0\" encoding=\"utf-8\"?>"
+        "<soap:Envelope xmlns:soap=\"http://schemas.xmlsoap.org/soap/envelope/\">"
+        "<soap:Body>";
+    const char* envelope_suffix = "</soap:Body></soap:Envelope>";
+
+    size_t body_len = strlen(request_body_xml);
+    size_t envelope_len = strlen(envelope_prefix) + body_len + strlen(envelope_suffix);
+    char* envelope = (char*)malloc(envelope_len + 1);
+    if (!envelope) {
+        return -1;
+    }
+    snprintf(envelope, envelope_len + 1, "%s%s%s", envelope_prefix, request_body_xml, envelope_suffix);
+
+    const char* action_header = (soap_action && soap_action[0] != '\0') ? soap_action : "";
+    int req_len = snprintf(NULL,
+                           0,
+                           "POST %s HTTP/1.1\r\n"
+                           "Host: %s:%s\r\n"
+                           "Content-Type: text/xml; charset=utf-8\r\n"
+                           "SOAPAction: \"%s\"\r\n"
+                           "Content-Length: %zu\r\n"
+                           "Connection: close\r\n\r\n%s",
+                           path,
+                           host,
+                           port,
+                           action_header,
+                           envelope_len,
+                           envelope);
+    if (req_len < 0) {
+        free(envelope);
+        return -1;
+    }
+
+    char* request = (char*)malloc((size_t)req_len + 1);
+    if (!request) {
+        free(envelope);
+        return -1;
+    }
+    snprintf(request,
+             (size_t)req_len + 1,
+             "POST %s HTTP/1.1\r\n"
+             "Host: %s:%s\r\n"
+             "Content-Type: text/xml; charset=utf-8\r\n"
+             "SOAPAction: \"%s\"\r\n"
+             "Content-Length: %zu\r\n"
+             "Connection: close\r\n\r\n%s",
+             path,
+             host,
+             port,
+             action_header,
+             envelope_len,
+             envelope);
+
+    int fd = open_tcp_connection(host, port, timeout_ms);
+    if (fd < 0) {
+        free(request);
+        free(envelope);
+        return -1;
+    }
+
+    int rc = 0;
+    size_t resp_len = 0;
+    char* response = NULL;
+    char* body = NULL;
+    char* soap_body = NULL;
+
+    if (send_all(fd, request, (size_t)req_len) != 0) {
+        rc = -1;
+        goto cleanup;
+    }
+
+    response = recv_all(fd, &resp_len);
+    if (!response || resp_len == 0) {
+        rc = -1;
+        goto cleanup;
+    }
+
+    *http_status = parse_http_status(response);
+
+    char* headers_end = strstr(response, "\r\n\r\n");
+    if (!headers_end) {
+        rc = -1;
+        goto cleanup;
+    }
+    char* raw_body = headers_end + 4;
+    size_t raw_body_len = resp_len - (size_t)(raw_body - response);
+
+    if (strstr(response, "Transfer-Encoding: chunked") || strstr(response, "transfer-encoding: chunked")) {
+        size_t decoded_len = 0;
+        body = dechunk_http_body(raw_body, raw_body_len, &decoded_len);
+        if (!body) {
+            rc = -1;
+            goto cleanup;
+        }
+        soap_body = extract_soap_body_raw(body, decoded_len);
+    } else {
+        soap_body = extract_soap_body_raw(raw_body, raw_body_len);
+    }
+
+    if (!soap_body) {
+        rc = -1;
+        goto cleanup;
+    }
+
+    *response_body_xml = soap_body;
+    soap_body = NULL;
+
+cleanup:
+    if (fd >= 0) {
+        close(fd);
+    }
+    free(request);
+    free(envelope);
+    free(response);
+    free(body);
+    free(soap_body);
+    return rc;
+}
diff --git a/src/main/target/SITL/sim/simple_soap_client.h b/src/main/target/SITL/sim/soap_client.h
similarity index 60%
rename from src/main/target/SITL/sim/simple_soap_client.h
rename to src/main/target/SITL/sim/soap_client.h
index 8522b28859a..ae8cd8cedcf 100644
--- a/src/main/target/SITL/sim/simple_soap_client.h
+++ b/src/main/target/SITL/sim/soap_client.h
@@ -22,27 +22,25 @@
  * along with this program. If not, see http://www.gnu.org/licenses/.
  */
 
-#include <arpa/inet.h>
-#include <netinet/in.h>
-#include <netdb.h>
+#pragma once
 
-#define SOAP_REC_BUF_SIZE 256 * 1024
+#include <pthread.h>
 
 typedef struct {
-    int sockedFd;
-    struct sockaddr_in socketAddr;
-    bool isInitalised;
-    bool isConnected;
+    char host[256];
+    char port[16];
+    char path[256];
+    int timeout_ms;
+    pthread_mutex_t lock;
 } soap_client_t;
 
-typedef struct {
-    soap_client_t client;
-    char* content;
-} send_info_t;
-
+int soap_client_init(soap_client_t* c, const char* host, const char* port, const char* path, int timeout_ms);
+void soap_client_destroy(soap_client_t* c);
 
-bool soapClientConnect(soap_client_t *client, const char *address, int port);
-void soapClientClose(soap_client_t *client);
-void soapClientSendRequestVa(soap_client_t *client, const char* action, const char *fmt, va_list va);
-void soapClientSendRequest(soap_client_t *client, const char* action, const char *fmt, ...);
-char* soapClientReceive(soap_client_t *client);
+// Sends raw XML as content of <soap:Body> and returns raw XML content of response <soap:Body>.
+// Caller must free(*response_body_xml) on success.
+int soap_client_call_raw_body(soap_client_t* c,
+                              const char* soap_action,
+                              const char* request_body_xml,
+                              char** response_body_xml,
+                              int* http_status);
\ No newline at end of file
diff --git a/src/main/target/SITL/sim/xplane.c b/src/main/target/SITL/sim/xplane.c
index d629c2130e3..bf0349c47d1 100644
--- a/src/main/target/SITL/sim/xplane.c
+++ b/src/main/target/SITL/sim/xplane.c
@@ -33,8 +33,10 @@
 #include <sys/types.h>
 #include <netinet/in.h>
 #include <pthread.h>
+#include <stdatomic.h>
 #include <errno.h>
 #include <math.h>
+#include <unistd.h>
 
 #include "platform.h"
 
@@ -58,6 +60,7 @@
 #include "flight/imu.h"
 #include "io/gps.h"
 #include "rx/sim.h"
+#include "xplane.h"
 
 #define XP_PORT 49000
 #define XPLANE_JOYSTICK_AXIS_COUNT 8
@@ -66,11 +69,15 @@
 static uint8_t pwmMapping[XP_MAX_PWM_OUTS];
 static uint8_t mappingCount;
 
+static pthread_mutex_t initMutex = PTHREAD_MUTEX_INITIALIZER;
+static pthread_cond_t initCond = PTHREAD_COND_INITIALIZER;
+static atomic_bool shouldStopListenThread = false;
+
 static struct sockaddr_storage serverAddr;
 static socklen_t serverAddrLen;
 static int sockFd;
-static pthread_t listenThread;
-static bool initalized = false;
+static pthread_t listenThread = 0;
+static atomic_bool initalized = false;
 static bool useImu = false;
 
 static float lattitude = 0;
@@ -170,6 +177,40 @@ static void sendDref(char* dref, float value)
     sendto(sockFd, (void*)buf, sizeof(buf), 0, (struct sockaddr*)&serverAddr, serverAddrLen);
 }
 
+static void tryRegisterDrefs(void)
+{
+    registerDref(DREF_LATITUDE, "sim/flightmodel/position/latitude", 100);
+    registerDref(DREF_LONGITUDE, "sim/flightmodel/position/longitude", 100);
+    registerDref(DREF_ELEVATION, "sim/flightmodel/position/elevation", 100);
+    registerDref(DREF_AGL, "sim/flightmodel/position/y_agl", 100);
+    registerDref(DREF_LOCAL_VX, "sim/flightmodel/position/local_vx", 100);
+    registerDref(DREF_LOCAL_VY, "sim/flightmodel/position/local_vy", 100);
+    registerDref(DREF_LOCAL_VZ, "sim/flightmodel/position/local_vz", 100);
+    registerDref(DREF_GROUNDSPEED, "sim/flightmodel/position/groundspeed", 100);
+    registerDref(DREF_TRUE_AIRSPEED, "sim/flightmodel/position/true_airspeed", 100);
+    registerDref(DREF_POS_PHI, "sim/flightmodel/position/phi", 100);
+    registerDref(DREF_POS_THETA, "sim/flightmodel/position/theta", 100);
+    registerDref(DREF_POS_PSI, "sim/flightmodel/position/psi", 100);
+    registerDref(DREF_POS_HPATH, "sim/flightmodel/position/hpath", 100);
+    registerDref(DREF_FORCE_G_AXI1, "sim/flightmodel/forces/g_axil", 100);
+    registerDref(DREF_FORCE_G_SIDE, "sim/flightmodel/forces/g_side", 100);
+    registerDref(DREF_FORCE_G_NRML, "sim/flightmodel/forces/g_nrml", 100);
+    registerDref(DREF_POS_P, "sim/flightmodel/position/P", 100);
+    registerDref(DREF_POS_Q, "sim/flightmodel/position/Q", 100);
+    registerDref(DREF_POS_R, "sim/flightmodel/position/R", 100);
+    registerDref(DREF_POS_BARO_CURRENT_INHG, "sim/weather/barometer_current_inhg", 100);
+    registerDref(DREF_HAS_JOYSTICK, "sim/joystick/has_joystick", 100);
+    registerDref(DREF_JOYSTICK_VALUES_PITCH, "sim/joystick/joy_mapped_axis_value[1]", 100);
+    registerDref(DREF_JOYSTICK_VALUES_ROll, "sim/joystick/joy_mapped_axis_value[2]", 100);
+    registerDref(DREF_JOYSTICK_VALUES_YAW, "sim/joystick/joy_mapped_axis_value[3]", 100);
+    // Abusing cowl flaps for other channels
+    registerDref(DREF_JOYSTICK_VALUES_THROTTLE, "sim/joystick/joy_mapped_axis_value[57]", 100);
+    registerDref(DREF_JOYSTICK_VALUES_CH5, "sim/joystick/joy_mapped_axis_value[58]", 100);
+    registerDref(DREF_JOYSTICK_VALUES_CH6, "sim/joystick/joy_mapped_axis_value[59]", 100);
+    registerDref(DREF_JOYSTICK_VALUES_CH7, "sim/joystick/joy_mapped_axis_value[60]", 100);
+    registerDref(DREF_JOYSTICK_VALUES_CH8, "sim/joystick/joy_mapped_axis_value[61]", 100);
+}
+
 static void* listenWorker(void* arg)
 {
     UNUSED(arg);
@@ -179,8 +220,11 @@ static void* listenWorker(void* arg)
     socklen_t slen = sizeof(remoteAddr);
     int recvLen;
 
-    while (true)
+    while (!atomic_load(&shouldStopListenThread))
     {
+        if (!atomic_load(&initalized)) {
+            tryRegisterDrefs();
+        }
 
         float motorValue = 0;
         float yokeValues[3] = { 0 };
@@ -210,10 +254,12 @@ static void* listenWorker(void* arg)
 
         recvLen = recvfrom(sockFd, buf, sizeof(buf), 0, (struct sockaddr*)&remoteAddr, &slen);
         if (recvLen < 0 && errno != EWOULDBLOCK) {
+            delay(250);
             continue;
         }
 
         if (strncmp((char*)buf, "RREF", 4) != 0) {
+            delay(250);
             continue;
         }
 
@@ -426,11 +472,14 @@ static void* listenWorker(void* arg)
             constrainToInt16(north.z * 1024.0f)
         );
 
-        if (!initalized) {
+        if (!atomic_load(&initalized)) {
             ENABLE_ARMING_FLAG(SIMULATOR_MODE_SITL);
             // Aircraft can wobble on the runway and prevents calibration of the accelerometer
             ENABLE_STATE(ACCELEROMETER_CALIBRATED);
-            initalized = true;
+            atomic_store(&initalized, true);
+            pthread_mutex_lock(&initMutex);
+            pthread_cond_signal(&initCond);
+            pthread_mutex_unlock(&initMutex);
         }
 
         unlockMainPID();
@@ -458,7 +507,7 @@ bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, bool
     if (sockFd < 0) {
         return false;
     } else {
-	char addrbuf[IPADDRESS_PRINT_BUFLEN];
+        char addrbuf[IPADDRESS_PRINT_BUFLEN];
         char *nptr = prettyPrintAddress((struct sockaddr *)&serverAddr, addrbuf, IPADDRESS_PRINT_BUFLEN );
         if (nptr != NULL) {
             fprintf(stderr, "[SOCKET] xplane address = %s, fd=%d\n", nptr, sockFd);
@@ -466,9 +515,9 @@ bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, bool
     }
 
     struct timeval tv;
-        tv.tv_sec = 1;
-        tv.tv_usec = 0;
-        if (setsockopt(sockFd, SOL_SOCKET, SO_RCVTIMEO, (struct timeval *) &tv,sizeof(struct timeval))) {
+    tv.tv_sec = 1;
+    tv.tv_usec = 0;
+    if (setsockopt(sockFd, SOL_SOCKET, SO_RCVTIMEO, (struct timeval *) &tv,sizeof(struct timeval))) {
         return false;
     }
 
@@ -476,43 +525,30 @@ bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, bool
         return false;
     }
 
+    atomic_store(&initalized, false);
+    atomic_store(&shouldStopListenThread, false);
     if (pthread_create(&listenThread, NULL, listenWorker, NULL) < 0) {
         return false;
     }
 
-    while (!initalized) {
-        registerDref(DREF_LATITUDE, "sim/flightmodel/position/latitude", 100);
-        registerDref(DREF_LONGITUDE, "sim/flightmodel/position/longitude", 100);
-        registerDref(DREF_ELEVATION, "sim/flightmodel/position/elevation", 100);
-        registerDref(DREF_AGL, "sim/flightmodel/position/y_agl", 100);
-        registerDref(DREF_LOCAL_VX, "sim/flightmodel/position/local_vx", 100);
-        registerDref(DREF_LOCAL_VY, "sim/flightmodel/position/local_vy", 100);
-        registerDref(DREF_LOCAL_VZ, "sim/flightmodel/position/local_vz", 100);
-        registerDref(DREF_GROUNDSPEED, "sim/flightmodel/position/groundspeed", 100);
-        registerDref(DREF_TRUE_AIRSPEED, "sim/flightmodel/position/true_airspeed", 100);
-        registerDref(DREF_POS_PHI, "sim/flightmodel/position/phi", 100);
-        registerDref(DREF_POS_THETA, "sim/flightmodel/position/theta", 100);
-        registerDref(DREF_POS_PSI, "sim/flightmodel/position/psi", 100);
-        registerDref(DREF_POS_HPATH, "sim/flightmodel/position/hpath", 100);
-        registerDref(DREF_FORCE_G_AXI1, "sim/flightmodel/forces/g_axil", 100);
-        registerDref(DREF_FORCE_G_SIDE, "sim/flightmodel/forces/g_side", 100);
-        registerDref(DREF_FORCE_G_NRML, "sim/flightmodel/forces/g_nrml", 100);
-        registerDref(DREF_POS_P, "sim/flightmodel/position/P", 100);
-        registerDref(DREF_POS_Q, "sim/flightmodel/position/Q", 100);
-        registerDref(DREF_POS_R, "sim/flightmodel/position/R", 100);
-        registerDref(DREF_POS_BARO_CURRENT_INHG, "sim/weather/barometer_current_inhg", 100);
-        registerDref(DREF_HAS_JOYSTICK, "sim/joystick/has_joystick", 100);
-        registerDref(DREF_JOYSTICK_VALUES_PITCH, "sim/joystick/joy_mapped_axis_value[1]", 100);
-        registerDref(DREF_JOYSTICK_VALUES_ROll, "sim/joystick/joy_mapped_axis_value[2]", 100);
-        registerDref(DREF_JOYSTICK_VALUES_YAW, "sim/joystick/joy_mapped_axis_value[3]", 100);
-        // Abusing cowl flaps for other channels
-        registerDref(DREF_JOYSTICK_VALUES_THROTTLE, "sim/joystick/joy_mapped_axis_value[57]", 100);
-        registerDref(DREF_JOYSTICK_VALUES_CH5, "sim/joystick/joy_mapped_axis_value[58]", 100);
-        registerDref(DREF_JOYSTICK_VALUES_CH6, "sim/joystick/joy_mapped_axis_value[59]", 100);
-        registerDref(DREF_JOYSTICK_VALUES_CH7, "sim/joystick/joy_mapped_axis_value[60]", 100);
-        registerDref(DREF_JOYSTICK_VALUES_CH8, "sim/joystick/joy_mapped_axis_value[61]", 100);
-        delay(250);
+    pthread_mutex_lock(&initMutex);
+    while (!atomic_load(&initalized)) {
+        pthread_cond_wait(&initCond, &initMutex);        
     }
+    pthread_mutex_unlock(&initMutex);
 
     return true;
 }
+
+void simXPlaneClose(void)
+{
+        atomic_store(&shouldStopListenThread, true);
+        if (listenThread) {
+            pthread_join(listenThread, NULL);
+        }
+        DISABLE_ARMING_FLAG(SIMULATOR_MODE_SITL);
+        atomic_store(&initalized, false);
+        pthread_mutex_destroy(&initMutex);
+        pthread_cond_destroy(&initCond);
+        close(sockFd);
+}
diff --git a/src/main/target/SITL/sim/xplane.h b/src/main/target/SITL/sim/xplane.h
index 1777a30af2b..619bef615e0 100644
--- a/src/main/target/SITL/sim/xplane.h
+++ b/src/main/target/SITL/sim/xplane.h
@@ -27,3 +27,4 @@
 #define XP_MAX_PWM_OUTS 4
 
 bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, bool imu);
+void simXPlaneClose(void);
diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c
index e473c636981..67394a815e1 100644
--- a/src/main/target/SITL/target.c
+++ b/src/main/target/SITL/target.c
@@ -39,6 +39,7 @@
 #include <arpa/inet.h>
 #include <netdb.h>
 #include <netinet/in.h>
+#include <signal.h>
 
 #include <platform.h>
 #include "target.h"
@@ -80,6 +81,25 @@ static void printVersion(void) {
     fprintf(stderr, "INAV %d.%d.%d SITL (%s)\n", FC_VERSION_MAJOR, FC_VERSION_MINOR, FC_VERSION_PATCH_LEVEL, shortGitRevision);
 }
 
+static void cleanupAndExit(int code, bool shouldExit) {
+    if (sitlSim == SITL_SIM_XPLANE) {
+        simXPlaneClose();
+    } else if (sitlSim == SITL_SIM_REALFLIGHT) {
+        simRealFlightClose();
+    }
+    pthread_mutex_destroy(&mainLoopLock);
+
+    if (shouldExit) {
+        exit(code);
+    }
+}
+
+static void on_sigint(int sig) {
+    UNUSED(sig);
+    fprintf(stderr, "\n[SYSTEM] Caught SIGINT, exiting...\n");
+    cleanupAndExit(0, true);
+}
+
 void systemInit(void) {
     printVersion();
     clock_gettime(CLOCK_MONOTONIC, &start_time);
@@ -100,6 +120,12 @@ void systemInit(void) {
         exit(1);
     }
 
+    struct sigaction sa;
+    sa.sa_handler = on_sigint;
+    sigemptyset(&sa.sa_mask);
+    sa.sa_flags = 0;
+    sigaction(SIGINT, &sa, NULL);
+
     if (sitlSim != SITL_SIM_NONE) {
         fprintf(stderr, "[SIM] Waiting for connection...\n");
     }
@@ -377,6 +403,7 @@ void delay(timeMs_t ms)
 void systemReset(void)
 {
     fprintf(stderr, "[SYSTEM] Reset\n");
+    cleanupAndExit(0, false);
 #if defined(__CYGWIN__) || defined(__APPLE__) || GCC_MAJOR < 12
     for(int j = 3; j < 1024; j++) {
         close(j);
@@ -391,7 +418,7 @@ void systemReset(void)
 void systemResetToBootloader(void)
 {
     fprintf(stderr, "[SYSTEM] Reset to bootloader\n");
-    exit(0);
+    cleanupAndExit(0, true);
 }
 
 void failureMode(failureMode_e mode) {

From bdf10e141ca5976b4d440841bf65e8b5f3e3ff15 Mon Sep 17 00:00:00 2001
From: Scavanger <andreas.kanzler@outlook.com>
Date: Mon, 16 Mar 2026 11:00:43 -0300
Subject: [PATCH 041/102] Remove unnecessary error messages and update control
 input channel value in RealFlight simulation

---
 src/main/config/config_streamer_file.c | 1 -
 src/main/target/SITL/sim/realFlight.c  | 3 +--
 2 files changed, 1 insertion(+), 3 deletions(-)

diff --git a/src/main/config/config_streamer_file.c b/src/main/config/config_streamer_file.c
index 86d677498c1..bce0f5be522 100644
--- a/src/main/config/config_streamer_file.c
+++ b/src/main/config/config_streamer_file.c
@@ -47,7 +47,6 @@ bool configFileSetPath(char* path)
 void config_streamer_impl_unlock(void)
 {
     if (eepromFd != NULL) {
-        fprintf(stderr, "[EEPROM] Unable to load %s\n", eepromPath);
         return;
     }
 
diff --git a/src/main/target/SITL/sim/realFlight.c b/src/main/target/SITL/sim/realFlight.c
index f5903f75546..6ff0ff413af 100644
--- a/src/main/target/SITL/sim/realFlight.c
+++ b/src/main/target/SITL/sim/realFlight.c
@@ -239,7 +239,7 @@ static void exchangeData(void)
         }
     }
 
-    char requestBody[1024] = "<ExchangeData><pControlInputs><m-selectedChannels>12</m-selectedChannels><m-channelValues-0to1>";
+    char requestBody[1024] = "<ExchangeData><pControlInputs><m-selectedChannels>4095</m-selectedChannels><m-channelValues-0to1>";
     
     for (int i = 0; i < RF_MAX_CHANNEL_COUNT; i++) {
         char value[32];
@@ -428,7 +428,6 @@ static void* soapWorker(void* arg)
             // RealFlight seems to not properly close the connection on its side if the connection is interrupted, but only after a timeout of about 30 seconds. 
             // During this time the interface is not usable, but without this step it would be stuck in an unusable state until the next restart of the SITL.
             if (!restoreOriginalControllerDevice()) {
-                fprintf(stderr, "[SIM] Failed to restore original controller device in RealFlight. RealFlight might be in a broken state. Retrying...\n");
                 delay(1000);
                 continue;
             }

From c756251439ab3eb13ac87cdbb8e9d50496d2fd69 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Wed, 18 Mar 2026 23:57:20 +0000
Subject: [PATCH 042/102] virtual pitot fixes

---
 src/main/fc/cli.c             |  3 ++-
 src/main/sensors/pitotmeter.c | 48 ++++++++++++++++++-----------------
 2 files changed, 27 insertions(+), 24 deletions(-)

diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index 3e8165dcf32..e356e29fe20 100644
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -4142,13 +4142,14 @@ static void cliStatus(char *cmdline)
 #endif // for if at32
 #endif // for SITL
 
-    cliPrintLinef("Sensor status: GYRO=%s, ACC=%s, MAG=%s, BARO=%s, RANGEFINDER=%s, OPFLOW=%s, GPS=%s",
+    cliPrintLinef("Sensor status: GYRO=%s, ACC=%s, MAG=%s, BARO=%s, RANGEFINDER=%s, OPFLOW=%s, PITOT=%s, GPS=%s",
         hardwareSensorStatusNames[getHwGyroStatus()],
         hardwareSensorStatusNames[getHwAccelerometerStatus()],
         hardwareSensorStatusNames[getHwCompassStatus()],
         hardwareSensorStatusNames[getHwBarometerStatus()],
         hardwareSensorStatusNames[getHwRangefinderStatus()],
         hardwareSensorStatusNames[getHwOpticalFlowStatus()],
+        hardwareSensorStatusNames[getHwPitotmeterStatus()],
         hardwareSensorStatusNames[getHwGPSStatus()]
     );
 
diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c
index b4b61f57669..2b8a3873e6a 100755
--- a/src/main/sensors/pitotmeter.c
+++ b/src/main/sensors/pitotmeter.c
@@ -134,14 +134,15 @@ bool pitotDetect(pitotDev_t *dev, uint8_t pitotHardwareToUse)
             FALLTHROUGH;
 
         case PITOT_VIRTUAL:
-#if defined(USE_WIND_ESTIMATOR) && defined(USE_PITOT_VIRTUAL) 
-            if ((pitotHardwareToUse != PITOT_AUTODETECT) && virtualPitotDetect(dev)) {
-                pitotHardware = PITOT_VIRTUAL;
-                break;
-            }
-#endif
-            /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
             if (pitotHardwareToUse != PITOT_AUTODETECT) {
+#if defined(USE_WIND_ESTIMATOR) && defined(USE_PITOT_VIRTUAL)
+                 if (virtualPitotDetect(dev)) {
+                    pitotHardware = PITOT_VIRTUAL;
+                    break;
+                }
+#endif
+                // set requested to None to prevent hardware failure if GPS not enabled
+                requestedSensors[SENSOR_INDEX_PITOT] = PITOT_NONE;
                 break;
             }
             FALLTHROUGH;
@@ -226,9 +227,10 @@ STATIC_PROTOTHREAD(pitotThread)
 
     // Init filter
     pitot.lastMeasurementUs = micros();
-    if(pitotmeterConfig()->pitot_lpf_milli_hz >0){
+    if (pitotmeterConfig()->pitot_lpf_milli_hz > 0) {
         pt1FilterInit(&pitot.lpfState, pitotmeterConfig()->pitot_lpf_milli_hz / 1000.0f, 0.0f);
     }
+
     while(1) {
 #ifdef USE_SIMULATOR
     	while (SIMULATOR_HAS_OPTION(HITL_AIRSPEED) && SIMULATOR_HAS_OPTION(HITL_PITOT_FAILURE))
@@ -236,33 +238,33 @@ STATIC_PROTOTHREAD(pitotThread)
             ptDelayUs(10000);
     	}
 #endif
-
-        if ( pitot.lastSeenHealthyMs == 0 ) {
+        if (pitot.lastSeenHealthyMs == 0) {
             if (pitot.dev.start(&pitot.dev)) {
                 pitot.lastSeenHealthyMs = millis();
-            }        
+            }
         }
 
-        if ( (millis() - pitot.lastSeenHealthyMs) >= US2MS(pitot.dev.delay)) {
-            if (pitot.dev.get(&pitot.dev))          // read current data
+        if ((millis() - pitot.lastSeenHealthyMs) >= US2MS(pitot.dev.delay)) {
+            if (pitot.dev.get(&pitot.dev)) {    // read current data
                 pitot.lastSeenHealthyMs = millis();
+            }
 
-            if (pitot.dev.start(&pitot.dev))        // init for next read
-                pitot.lastSeenHealthyMs = millis();        
+            if (pitot.dev.start(&pitot.dev)) {  // init for next read
+                pitot.lastSeenHealthyMs = millis();
+            }
         }
 
-
         pitot.dev.calculate(&pitot.dev, &pitotPressureTmp, &pitotTemperatureTmp);
 
 #ifdef USE_SIMULATOR
         if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) {
-            pitotPressureTmp = sq(simulatorData.airSpeed) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE;     
+            pitotPressureTmp = sq(simulatorData.airSpeed) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE;
         }
 #endif
 #if defined(USE_PITOT_FAKE)
-        if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) { 
-            pitotPressureTmp = sq(fakePitotGetAirspeed()) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE;     
-        } 
+        if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) {
+            pitotPressureTmp = sq(fakePitotGetAirspeed()) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE;
+        }
 #endif
         ptYield();
 
@@ -280,9 +282,9 @@ STATIC_PROTOTHREAD(pitotThread)
 
             // NOTE ::filter pressure - apply filter when NOT calibrating for zero !!!
             currentTimeUs = micros();
-            if(pitotmeterConfig()->pitot_lpf_milli_hz >0){
+            if (pitotmeterConfig()->pitot_lpf_milli_hz > 0) {
                 pitot.pressure = pt1FilterApply3(&pitot.lpfState, pitotPressureTmp, US2S(currentTimeUs - pitot.lastMeasurementUs));
-            }else{
+            } else {
                 pitot.pressure = pitotPressureTmp;
             }
             pitot.lastMeasurementUs = currentTimeUs;
@@ -297,7 +299,7 @@ STATIC_PROTOTHREAD(pitotThread)
         }
 
 #if defined(USE_PITOT_FAKE)
-        if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) { 
+        if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) {
             pitot.airSpeed = fakePitotGetAirspeed();
         }
 #endif

From ac8522629e15683ff96e480d7b0c9f05de0dc435 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Thu, 19 Mar 2026 00:03:50 +0000
Subject: [PATCH 043/102] Update pitotmeter.c

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

diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c
index 2b8a3873e6a..8183e2f4811 100755
--- a/src/main/sensors/pitotmeter.c
+++ b/src/main/sensors/pitotmeter.c
@@ -136,7 +136,7 @@ bool pitotDetect(pitotDev_t *dev, uint8_t pitotHardwareToUse)
         case PITOT_VIRTUAL:
             if (pitotHardwareToUse != PITOT_AUTODETECT) {
 #if defined(USE_WIND_ESTIMATOR) && defined(USE_PITOT_VIRTUAL)
-                 if (virtualPitotDetect(dev)) {
+                if (virtualPitotDetect(dev)) {
                     pitotHardware = PITOT_VIRTUAL;
                     break;
                 }

From 7cdd387fb7eeec9e2d9a54fbde824572e7b43510 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Thu, 19 Mar 2026 11:26:52 +0000
Subject: [PATCH 044/102] Add settings 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 4e577bbedd5..561802c817d 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -5624,7 +5624,7 @@ Defines rotation rate on PITCH axis that UAV will try to archive on max. stick d
 
 ### pitot_hardware
 
-Selection of pitot hardware.
+Selection of pitot hardware. VIRTUAL only works if a GPS is enabled.
 
 | Default | Min | Max |
 | --- | --- | --- |
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 476dfe09ff7..1c57f8363b2 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -667,7 +667,7 @@ groups:
     condition: USE_PITOT
     members:
       - name: pitot_hardware
-        description: "Selection of pitot hardware."
+        description: "Selection of pitot hardware. VIRTUAL only works if a GPS is enabled."
         default_value: "NONE"
         table: pitot_hardware
       - name: pitot_lpf_milli_hz

From 7801a8e16569c32b3c929dbe9b1af84dab6a7c53 Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Thu, 19 Mar 2026 13:34:12 -0500
Subject: [PATCH 045/102] Fix circular DMA reliability: IRQ handler guard, data
 length reload, DMAR support

The circular DMA protection was unreliable because:
- DMA TC interrupt handler immediately disabled the stream on every
  circular cycle completion, defeating the circular mode entirely
- H7 could enable circular mode with zero data length (undefined per RM)
- pwmCompleteMotorUpdate() rate limiter silently swallowed the
  zero-throttle latching calls
- Burst DMA (USE_DSHOT_DMAR) targets were not handled

Fixes:
- Add TCH_DMA_CIRCULAR state so IRQ handlers skip stream-disable
- Disable TC interrupt during circular mode as belt-and-suspenders
- Always reload transfer count to buffer size when enabling circular
- Load zero-throttle directly into DMA buffers, bypassing rate limiter
- Add burst DMA circular support for DMAR targets (F4/F7/H7)
- Disable timer DMA request during reconfiguration on all platforms
- Add __DSB() barrier before re-enabling DMA streams
- Add configured check consistent with pwmCompleteMotorUpdate()
---
 src/main/config/config_eeprom.c              | 12 +--
 src/main/drivers/pwm_output.c                | 56 +++++++++++--
 src/main/drivers/timer.h                     |  1 +
 src/main/drivers/timer_impl.h                |  3 +-
 src/main/drivers/timer_impl_hal.c            | 83 ++++++++++++++------
 src/main/drivers/timer_impl_stdperiph.c      | 80 ++++++++++++++++---
 src/main/drivers/timer_impl_stdperiph_at32.c | 39 ++++++---
 7 files changed, 211 insertions(+), 63 deletions(-)

diff --git a/src/main/config/config_eeprom.c b/src/main/config/config_eeprom.c
index e4a22aaa581..608a08a0a84 100755
--- a/src/main/config/config_eeprom.c
+++ b/src/main/config/config_eeprom.c
@@ -323,15 +323,10 @@ static bool writeSettingsToEEPROM(void)
 void writeConfigToEEPROM(void)
 {
 #if !defined(SITL_BUILD) && defined(USE_DSHOT)
-    // Prevent ESC spinup during settings save using circular DMA
+    // Enable circular DMA so hardware keeps repeating zero-throttle DShot
+    // packets during flash writes (which block the CPU for 20-200ms).
+    // Without this, ESCs lose signal and may spin up or reboot.
     pwmSetMotorDMACircular(true);
-
-    // Force motor updates to latch current (zero) throttle into circular DMA buffer
-    pwmCompleteMotorUpdate();
-    delayMicroseconds(200);
-    pwmCompleteMotorUpdate();
-    delayMicroseconds(200);
-    pwmCompleteMotorUpdate();
 #endif
 
     bool success = false;
@@ -347,7 +342,6 @@ void writeConfigToEEPROM(void)
     }
 
 #if !defined(SITL_BUILD) && defined(USE_DSHOT)
-    // Restore normal DMA mode
     pwmSetMotorDMACircular(false);
 #endif
 
diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c
index 42debf6cb48..ca1838f197b 100644
--- a/src/main/drivers/pwm_output.c
+++ b/src/main/drivers/pwm_output.c
@@ -124,6 +124,15 @@ static timeUs_t commandPostDelay = 0;
 static circularBuffer_t commandsCircularBuffer;
 static uint8_t commandsBuff[DHSOT_COMMAND_QUEUE_SIZE];
 static currentExecutingCommand_t currentExecutingCommand;
+
+static uint16_t prepareDshotPacket(const uint16_t value, bool requestTelemetry);
+static void loadDmaBufferDshot(timerDMASafeType_t *dmaBuffer, uint16_t packet);
+static void loadDmaBufferDshotStride(timerDMASafeType_t *dmaBuffer, int stride, uint16_t packet);
+
+#ifdef USE_DSHOT_DMAR
+burstDmaTimer_t burstDmaTimers[MAX_DMA_TIMERS];
+uint8_t burstDmaTimersCount = 0;
+#endif
 #endif
 
 static void pwmOutConfigTimer(pwmOutputPort_t * p, TCH_t * tch, uint32_t hz, uint16_t period, uint16_t value)
@@ -229,12 +238,48 @@ void pwmEnableMotors(void)
 void pwmSetMotorDMACircular(bool circular)
 {
 #ifdef USE_DSHOT
-    // Set DMA circular mode for all motor outputs
-    for (int i = 0; i < getMotorCount(); i++) {
-        if (motors[i].pwmPort && motors[i].pwmPort->tch) {
-            impl_timerPWMSetDMACircular(motors[i].pwmPort->tch, circular);
+    if (!isMotorProtocolDshot()) {
+        return;
+    }
+
+    int motorCount = getMotorCount();
+
+    if (circular) {
+        // Load zero-throttle packets directly into DMA buffers,
+        // bypassing the rate limiter in pwmCompleteMotorUpdate()
+        uint16_t packet = prepareDshotPacket(0, false);
+        for (int i = 0; i < motorCount; i++) {
+            if (motors[i].pwmPort && motors[i].pwmPort->configured) {
+#ifdef USE_DSHOT_DMAR
+                loadDmaBufferDshotStride(&motors[i].pwmPort->dmaBurstBuffer[motors[i].pwmPort->tch->timHw->channelIndex], 4, packet);
+#else
+                loadDmaBufferDshot(motors[i].pwmPort->dmaBuffer, packet);
+#endif
+            }
+        }
+    }
+
+#ifdef USE_DSHOT_DMAR
+    // Burst DMA: one DMA stream per timer, shared across channels
+    for (int i = 0; i < burstDmaTimersCount; i++) {
+        burstDmaTimer_t *burstDmaTimer = &burstDmaTimers[i];
+        // Find the first motor using this timer to get the TCH for DMA state
+        for (int m = 0; m < motorCount; m++) {
+            if (motors[m].pwmPort && motors[m].pwmPort->configured && motors[m].pwmPort->tch
+                && motors[m].pwmPort->tch->timHw->tim == burstDmaTimer->timer) {
+                impl_pwmBurstDMASetCircular(burstDmaTimer, motors[m].pwmPort->tch, circular, DSHOT_DMA_BUFFER_SIZE * 4);
+                break;
+            }
         }
     }
+#else
+    // Per-channel DMA: one DMA stream per motor
+    for (int i = 0; i < motorCount; i++) {
+        if (motors[i].pwmPort && motors[i].pwmPort->configured && motors[i].pwmPort->tch) {
+            impl_timerPWMSetDMACircular(motors[i].pwmPort->tch, circular, DSHOT_DMA_BUFFER_SIZE);
+        }
+    }
+#endif
 #else
     UNUSED(circular);
 #endif
@@ -278,9 +323,6 @@ uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType)
 }
 
 #ifdef USE_DSHOT_DMAR
-burstDmaTimer_t burstDmaTimers[MAX_DMA_TIMERS];
-uint8_t burstDmaTimersCount = 0;
-
 static uint8_t getBurstDmaTimerIndex(TIM_TypeDef *timer)
 {
     for (int i = 0; i < burstDmaTimersCount; i++) {
diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h
index d87e0400d52..512b0d7c106 100644
--- a/src/main/drivers/timer.h
+++ b/src/main/drivers/timer.h
@@ -138,6 +138,7 @@ typedef enum {
     TCH_DMA_IDLE = 0,
     TCH_DMA_READY,
     TCH_DMA_ACTIVE,
+    TCH_DMA_CIRCULAR,
 } tchDmaState_e;
 
 // Some forward declarations for types
diff --git a/src/main/drivers/timer_impl.h b/src/main/drivers/timer_impl.h
index 83a7a93f2b1..6a302f57cb4 100644
--- a/src/main/drivers/timer_impl.h
+++ b/src/main/drivers/timer_impl.h
@@ -84,9 +84,10 @@ bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBuf
 void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferElementCount);
 void impl_timerPWMStartDMA(TCH_t * tch);
 void impl_timerPWMStopDMA(TCH_t * tch);
-void impl_timerPWMSetDMACircular(TCH_t * tch, bool circular);
+void impl_timerPWMSetDMACircular(TCH_t * tch, bool circular, uint32_t dmaBufferSize);
 
 #ifdef USE_DSHOT_DMAR
 bool impl_timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount);
 void impl_pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstLength);
+void impl_pwmBurstDMASetCircular(burstDmaTimer_t * burstDmaTimer, TCH_t * tch, bool circular, uint32_t dmaBufferSize);
 #endif
\ No newline at end of file
diff --git a/src/main/drivers/timer_impl_hal.c b/src/main/drivers/timer_impl_hal.c
index 85a8429f8f9..111fa6ffb19 100644
--- a/src/main/drivers/timer_impl_hal.c
+++ b/src/main/drivers/timer_impl_hal.c
@@ -319,6 +319,12 @@ static void impl_timerDMA_IRQHandler(DMA_t descriptor)
     if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) {
         TCH_t * tch = (TCH_t *)descriptor->userParam;
 
+        // In circular mode, let DMA keep running - don't disable the stream
+        if (tch->dmaState == TCH_DMA_CIRCULAR) {
+            DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF);
+            return;
+        }
+
         // If it was ACTIVE - switch to IDLE
         if (tch->dmaState == TCH_DMA_ACTIVE) {
             tch->dmaState = TCH_DMA_IDLE;
@@ -512,6 +518,46 @@ void impl_pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstLength
     //LL_TIM_EnableDMAReq_UPDATE(burstDmaTimer->timer);
     LL_TIM_EnableDMAReq_CCx(burstDmaTimer->timer, burstDmaTimer->burstRequestSource);
 }
+
+void impl_pwmBurstDMASetCircular(burstDmaTimer_t * burstDmaTimer, TCH_t * tch, bool circular, uint32_t dmaBufferSize)
+{
+    if (!tch->dma || !tch->dma->dma) {
+        return;
+    }
+
+    ATOMIC_BLOCK(NVIC_PRIO_MAX) {
+        LL_TIM_DisableDMAReq_CCx(burstDmaTimer->timer, burstDmaTimer->burstRequestSource);
+        LL_DMA_DisableStream(burstDmaTimer->dma, burstDmaTimer->streamLL);
+
+        uint32_t timeout = 10000;
+        while (LL_DMA_IsEnabledStream(burstDmaTimer->dma, burstDmaTimer->streamLL) && timeout--) {
+            __NOP();
+        }
+
+        if (timeout == 0 && LL_DMA_IsEnabledStream(burstDmaTimer->dma, burstDmaTimer->streamLL)) {
+            LL_TIM_EnableDMAReq_CCx(burstDmaTimer->timer, burstDmaTimer->burstRequestSource);
+            return;
+        }
+
+        DMA_CLEAR_FLAG(tch->dma, DMA_IT_TCIF);
+
+        if (circular) {
+            LL_DMA_SetMode(burstDmaTimer->dma, burstDmaTimer->streamLL, LL_DMA_MODE_CIRCULAR);
+            LL_DMA_SetDataLength(burstDmaTimer->dma, burstDmaTimer->streamLL, dmaBufferSize);
+            LL_DMA_DisableIT_TC(burstDmaTimer->dma, burstDmaTimer->streamLL);
+            tch->dmaState = TCH_DMA_CIRCULAR;
+        } else {
+            LL_DMA_SetMode(burstDmaTimer->dma, burstDmaTimer->streamLL, LL_DMA_MODE_NORMAL);
+            LL_DMA_EnableIT_TC(burstDmaTimer->dma, burstDmaTimer->streamLL);
+            tch->dmaState = TCH_DMA_IDLE;
+        }
+
+        __DSB();
+
+        LL_DMA_EnableStream(burstDmaTimer->dma, burstDmaTimer->streamLL);
+        LL_TIM_EnableDMAReq_CCx(burstDmaTimer->timer, burstDmaTimer->burstRequestSource);
+    }
+}
 #endif
 
 void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferElementCount)
@@ -581,7 +627,7 @@ void impl_timerPWMStopDMA(TCH_t * tch)
     HAL_TIM_Base_Start(tch->timCtx->timHandle);
 }
 
-void impl_timerPWMSetDMACircular(TCH_t * tch, bool circular)
+void impl_timerPWMSetDMACircular(TCH_t * tch, bool circular, uint32_t dmaBufferSize)
 {
     if (!tch->dma || !tch->dma->dma) {
         return;
@@ -590,51 +636,42 @@ void impl_timerPWMSetDMACircular(TCH_t * tch, bool circular)
     const uint32_t streamLL = lookupDMALLStreamTable[DMATAG_GET_STREAM(tch->timHw->dmaTag)];
     DMA_TypeDef *dmaBase = tch->dma->dma;
 
-    // Save the current transfer count before disabling
-    uint32_t dataLength = LL_DMA_GetDataLength(dmaBase, streamLL);
-
-    // Protect DMA reconfiguration from interrupt interference
     ATOMIC_BLOCK(NVIC_PRIO_MAX) {
-        // Disable timer DMA request first to stop new transfer triggers
+        // Stop new transfer triggers before reconfiguring
         LL_TIM_DisableDMAReq_CCx(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex]);
-
-        // Disable the DMA stream
         LL_DMA_DisableStream(dmaBase, streamLL);
 
-        // CRITICAL: Wait for stream to actually become disabled
-        // The EN bit doesn't clear immediately, especially if transfer is in progress
-        uint32_t timeout = 10000;
+        // STM32H7 RM: poll EN bit until stream is actually disabled
+        uint32_t timeout = 10000; // ~20us at 480MHz, well above worst-case disable latency
         while (LL_DMA_IsEnabledStream(dmaBase, streamLL) && timeout--) {
             __NOP();
         }
 
-        // If timeout occurred, DMA stream is still enabled - abort reconfiguration
         if (timeout == 0 && LL_DMA_IsEnabledStream(dmaBase, streamLL)) {
-            // Re-enable timer DMA request and return to avoid unstable state
             LL_TIM_EnableDMAReq_CCx(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex]);
             return;
         }
 
-        // Clear any pending transfer complete flags
         DMA_CLEAR_FLAG(tch->dma, DMA_IT_TCIF);
 
-        // Modify the DMA mode
         if (circular) {
             LL_DMA_SetMode(dmaBase, streamLL, LL_DMA_MODE_CIRCULAR);
+            // Circular mode requires non-zero NDTR (STM32H7 RM constraint)
+            LL_DMA_SetDataLength(dmaBase, streamLL, dmaBufferSize);
+            // Disable TC interrupt — in circular mode, TC fires every cycle
+            // and the IRQ handler would otherwise disable the stream
+            LL_DMA_DisableIT_TC(dmaBase, streamLL);
+            tch->dmaState = TCH_DMA_CIRCULAR;
         } else {
             LL_DMA_SetMode(dmaBase, streamLL, LL_DMA_MODE_NORMAL);
+            LL_DMA_EnableIT_TC(dmaBase, streamLL);
+            tch->dmaState = TCH_DMA_IDLE;
         }
 
-        // Reload the transfer count (required after mode change)
-        // If dataLength was 0 (transfer completed), keep it at 0 - the next motor update will reload it
-        if (dataLength > 0) {
-            LL_DMA_SetDataLength(dmaBase, streamLL, dataLength);
-        }
+        // Ensure register writes are visible to DMA before re-enabling
+        __DSB();
 
-        // Re-enable DMA stream
         LL_DMA_EnableStream(dmaBase, streamLL);
-
-        // Re-enable timer DMA requests
         LL_TIM_EnableDMAReq_CCx(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex]);
     }
 }
diff --git a/src/main/drivers/timer_impl_stdperiph.c b/src/main/drivers/timer_impl_stdperiph.c
index af685d0a5ba..5f939b1ac34 100644
--- a/src/main/drivers/timer_impl_stdperiph.c
+++ b/src/main/drivers/timer_impl_stdperiph.c
@@ -270,6 +270,13 @@ static void impl_timerDMA_IRQHandler(DMA_t descriptor)
 {
     if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) {
         TCH_t * tch = (TCH_t *)descriptor->userParam;
+
+        // In circular mode, let DMA keep running - don't disable the stream
+        if (tch->dmaState == TCH_DMA_CIRCULAR) {
+            DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF);
+            return;
+        }
+
         tch->dmaState = TCH_DMA_IDLE;
 
         DMA_Cmd(tch->dma->ref, DISABLE);
@@ -463,6 +470,46 @@ void impl_pwmBurstDMAStart(burstDmaTimer_t * burstDmaTimer, uint32_t BurstLength
     TIM_DMAConfig(burstDmaTimer->timer, TIM_DMABase_CCR1, TIM_DMABurstLength_4Transfers);
     TIM_DMACmd(burstDmaTimer->timer, burstDmaTimer->burstRequestSource, ENABLE);
 }
+
+void impl_pwmBurstDMASetCircular(burstDmaTimer_t * burstDmaTimer, TCH_t * tch, bool circular, uint32_t dmaBufferSize)
+{
+    if (!tch->dma || !tch->dma->ref) {
+        return;
+    }
+
+    ATOMIC_BLOCK(NVIC_PRIO_MAX) {
+        TIM_DMACmd(burstDmaTimer->timer, burstDmaTimer->burstRequestSource, DISABLE);
+        DMA_Cmd(burstDmaTimer->dmaBurstStream, DISABLE);
+
+        uint32_t timeout = 10000;
+        while ((burstDmaTimer->dmaBurstStream->CR & DMA_SxCR_EN) && timeout--) {
+            __NOP();
+        }
+
+        if (timeout == 0 && (burstDmaTimer->dmaBurstStream->CR & DMA_SxCR_EN)) {
+            TIM_DMACmd(burstDmaTimer->timer, burstDmaTimer->burstRequestSource, ENABLE);
+            return;
+        }
+
+        DMA_CLEAR_FLAG(tch->dma, DMA_IT_TCIF);
+
+        if (circular) {
+            burstDmaTimer->dmaBurstStream->CR |= DMA_SxCR_CIRC;
+            DMA_SetCurrDataCounter(burstDmaTimer->dmaBurstStream, dmaBufferSize);
+            DMA_ITConfig(burstDmaTimer->dmaBurstStream, DMA_IT_TC, DISABLE);
+            tch->dmaState = TCH_DMA_CIRCULAR;
+        } else {
+            burstDmaTimer->dmaBurstStream->CR &= ~DMA_SxCR_CIRC;
+            DMA_ITConfig(burstDmaTimer->dmaBurstStream, DMA_IT_TC, ENABLE);
+            tch->dmaState = TCH_DMA_IDLE;
+        }
+
+        __DSB();
+
+        DMA_Cmd(burstDmaTimer->dmaBurstStream, ENABLE);
+        TIM_DMACmd(burstDmaTimer->timer, burstDmaTimer->burstRequestSource, ENABLE);
+    }
+}
 #endif
 
 void impl_timerPWMPrepareDMA(TCH_t * tch, uint32_t dmaBufferElementCount)
@@ -520,38 +567,47 @@ void impl_timerPWMStopDMA(TCH_t * tch)
     TIM_Cmd(tch->timHw->tim, ENABLE);
 }
 
-void impl_timerPWMSetDMACircular(TCH_t * tch, bool circular)
+void impl_timerPWMSetDMACircular(TCH_t * tch, bool circular, uint32_t dmaBufferSize)
 {
     if (!tch->dma || !tch->dma->ref) {
         return;
     }
 
-    // Protect DMA reconfiguration from interrupt interference
     ATOMIC_BLOCK(NVIC_PRIO_MAX) {
-        // Temporarily disable DMA while modifying configuration
+        // Stop new transfer triggers before reconfiguring
+        TIM_DMACmd(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex], DISABLE);
         DMA_Cmd(tch->dma->ref, DISABLE);
 
-        // Wait for DMA stream to actually be disabled
-        // The EN bit doesn't clear immediately, especially if transfer is in progress
-        uint32_t timeout = 10000;
+        // STM32F4/F7 RM: poll EN bit until stream is actually disabled
+        uint32_t timeout = 10000; // ~60us at 168MHz, well above worst-case disable latency
         while ((tch->dma->ref->CR & DMA_SxCR_EN) && timeout--) {
             __NOP();
         }
 
-        // If timeout occurred, DMA stream is still enabled - abort reconfiguration
         if (timeout == 0 && (tch->dma->ref->CR & DMA_SxCR_EN)) {
-            DMA_Cmd(tch->dma->ref, ENABLE); // Re-enable and return
+            TIM_DMACmd(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex], ENABLE);
             return;
         }
 
-        // Modify the DMA mode
+        DMA_CLEAR_FLAG(tch->dma, DMA_IT_TCIF);
+
         if (circular) {
-            tch->dma->ref->CR |= DMA_SxCR_CIRC;  // Set circular bit
+            tch->dma->ref->CR |= DMA_SxCR_CIRC;
+            DMA_SetCurrDataCounter(tch->dma->ref, dmaBufferSize);
+            // Disable TC interrupt — in circular mode, TC fires every cycle
+            // and the IRQ handler would otherwise disable the stream
+            DMA_ITConfig(tch->dma->ref, DMA_IT_TC, DISABLE);
+            tch->dmaState = TCH_DMA_CIRCULAR;
         } else {
-            tch->dma->ref->CR &= ~DMA_SxCR_CIRC; // Clear circular bit
+            tch->dma->ref->CR &= ~DMA_SxCR_CIRC;
+            DMA_ITConfig(tch->dma->ref, DMA_IT_TC, ENABLE);
+            tch->dmaState = TCH_DMA_IDLE;
         }
 
-        // Re-enable DMA
+        // Ensure register writes are visible to DMA before re-enabling
+        __DSB();
+
         DMA_Cmd(tch->dma->ref, ENABLE);
+        TIM_DMACmd(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex], ENABLE);
     }
 }
diff --git a/src/main/drivers/timer_impl_stdperiph_at32.c b/src/main/drivers/timer_impl_stdperiph_at32.c
index d9433ba1f66..8317f1f5795 100644
--- a/src/main/drivers/timer_impl_stdperiph_at32.c
+++ b/src/main/drivers/timer_impl_stdperiph_at32.c
@@ -269,6 +269,13 @@ static void impl_timerDMA_IRQHandler(DMA_t descriptor)
 {
     if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) {
         TCH_t * tch = (TCH_t *)descriptor->userParam;
+
+        // In circular mode, let DMA keep running - don't disable the channel
+        if (tch->dmaState == TCH_DMA_CIRCULAR) {
+            DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF);
+            return;
+        }
+
         tch->dmaState = TCH_DMA_IDLE;
         dma_channel_enable(tch->dma->ref,FALSE);
         tmr_dma_request_enable(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex], FALSE);
@@ -408,37 +415,47 @@ void impl_timerPWMStopDMA(TCH_t * tch)
     tmr_counter_enable(tch->timHw->tim, TRUE);
 }
 
-void impl_timerPWMSetDMACircular(TCH_t * tch, bool circular)
+void impl_timerPWMSetDMACircular(TCH_t * tch, bool circular, uint32_t dmaBufferSize)
 {
     if (!tch->dma || !tch->dma->ref) {
         return;
     }
 
-    // Protect DMA reconfiguration from interrupt interference
     ATOMIC_BLOCK(NVIC_PRIO_MAX) {
-        // Temporarily disable DMA while modifying configuration
+        // Stop new transfer triggers before reconfiguring
+        tmr_dma_request_enable(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex], FALSE);
         dma_channel_enable(tch->dma->ref, FALSE);
 
-        // Wait for DMA channel to actually be disabled
-        // The enable bit doesn't clear immediately, especially if transfer is in progress
-        uint32_t timeout = 10000;
+        // AT32: poll enable bit until channel is actually disabled
+        uint32_t timeout = 10000; // ~40us at 288MHz, well above worst-case disable latency
         while (tch->dma->ref->ctrl_bit.chen && timeout--) {
             __NOP();
         }
 
-        // If timeout occurred, DMA channel is still enabled - abort reconfiguration
         if (timeout == 0 && tch->dma->ref->ctrl_bit.chen) {
-            dma_channel_enable(tch->dma->ref, TRUE); // Re-enable and return
+            tmr_dma_request_enable(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex], TRUE);
             return;
         }
 
+        DMA_CLEAR_FLAG(tch->dma, DMA_IT_TCIF);
+
         if (circular) {
-            tch->dma->ref->ctrl_bit.lm = TRUE;  // Enable loop mode
+            tch->dma->ref->ctrl_bit.lm = TRUE;
+            dma_data_number_set(tch->dma->ref, dmaBufferSize);
+            // Disable TC interrupt — in circular mode, TC fires every cycle
+            // and the IRQ handler would otherwise disable the channel
+            dma_interrupt_enable(tch->dma->ref, DMA_IT_TCIF, FALSE);
+            tch->dmaState = TCH_DMA_CIRCULAR;
         } else {
-            tch->dma->ref->ctrl_bit.lm = FALSE; // Disable loop mode
+            tch->dma->ref->ctrl_bit.lm = FALSE;
+            dma_interrupt_enable(tch->dma->ref, DMA_IT_TCIF, TRUE);
+            tch->dmaState = TCH_DMA_IDLE;
         }
 
-        // Re-enable DMA
+        // Ensure register writes are visible to DMA before re-enabling
+        __DSB();
+
         dma_channel_enable(tch->dma->ref, TRUE);
+        tmr_dma_request_enable(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex], TRUE);
     }
 }

From f6281a600c5635bcd00a5c9f0ef3a4f7f86ef8bc Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Thu, 19 Mar 2026 15:15:06 -0500
Subject: [PATCH 046/102] use explicitly assign motor and servo timers before
 auto timers

---
 src/main/drivers/pwm_mapping.c | 134 ++++++++++++++-------------------
 1 file changed, 57 insertions(+), 77 deletions(-)

diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c
index 2d01127a504..120b3d4fc91 100644
--- a/src/main/drivers/pwm_mapping.c
+++ b/src/main/drivers/pwm_mapping.c
@@ -270,43 +270,23 @@ uint8_t pwmClaimTimer(HAL_Timer_t *tim, uint32_t usageFlags) {
     return changed;
 }
 
-void pwmEnsureEnoughtMotors(uint8_t motorCount)
+static void pwmAssignOutput(timMotorServoHardware_t *timOutputs, timerHardware_t *timHw, int type)
 {
-    uint8_t motorOnlyOutputs = 0;
-
-    for (int idx = 0; idx < timerHardwareCount; idx++) {
-        timerHardware_t *timHw = &timerHardware[idx];
-
-        timerHardwareOverride(timHw);
-
-        if (checkPwmTimerConflicts(timHw)) {
-            continue;
-        }
-
-        if (TIM_IS_MOTOR_ONLY(timHw->usageFlags)) {
-            motorOnlyOutputs++;
-            motorOnlyOutputs += pwmClaimTimer(timHw->tim, timHw->usageFlags);
-        }
-    }
-
-    for (int idx = 0; idx < timerHardwareCount; idx++) {
-        timerHardware_t *timHw = &timerHardware[idx];
-
-        if (checkPwmTimerConflicts(timHw)) {
-            continue;
-        }
-
-        if (TIM_IS_MOTOR(timHw->usageFlags) && !TIM_IS_MOTOR_ONLY(timHw->usageFlags)) {
-            if (motorOnlyOutputs < motorCount) {
-                timHw->usageFlags &= ~TIM_USE_SERVO;
-                timHw->usageFlags |= TIM_USE_MOTOR;
-                motorOnlyOutputs++;
-                motorOnlyOutputs += pwmClaimTimer(timHw->tim, timHw->usageFlags);
-            } else {
-                timHw->usageFlags &= ~TIM_USE_MOTOR;
-                pwmClaimTimer(timHw->tim, timHw->usageFlags);
-            }
-        }
+    switch (type) {
+        case MAP_TO_MOTOR_OUTPUT:
+            timHw->usageFlags &= TIM_USE_MOTOR;
+            timOutputs->timMotors[timOutputs->maxTimMotorCount++] = timHw;
+            pwmClaimTimer(timHw->tim, timHw->usageFlags);
+            break;
+        case MAP_TO_SERVO_OUTPUT:
+            timHw->usageFlags &= TIM_USE_SERVO;
+            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;
     }
 }
 
@@ -316,54 +296,54 @@ void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerU
     timOutputs->maxTimMotorCount = 0;
     timOutputs->maxTimServoCount = 0;
 
-    uint8_t motorCount = getMotorCount();
-    uint8_t motorIdx = 0;
-
-    pwmEnsureEnoughtMotors(motorCount);
+    const uint8_t motorCount = getMotorCount();
 
+    // Apply configurator overrides to all timer outputs
     for (int idx = 0; idx < timerHardwareCount; idx++) {
-        timerHardware_t *timHw = &timerHardware[idx];
+        timerHardwareOverride(&timerHardware[idx]);
+    }
 
-        int type = MAP_TO_NONE;
+    // Assign outputs in priority order: dedicated first, then auto.
+    // Within each pass, array order (S1, S2, ...) is preserved.
+    // Motors and servos cannot share a timer, enforced by pwmHasMotorOnTimer/pwmHasServoOnTimer.
+    for (int priority = 0; priority < 2; priority++) {
+        uint8_t motorIdx = timOutputs->maxTimMotorCount;
 
-        // Check for known conflicts (i.e. UART, LEDSTRIP, Rangefinder and ADC)
-        if (checkPwmTimerConflicts(timHw)) {
-            LOG_WARNING(PWM, "Timer output %d skipped", idx);
-            continue;
-        }
+        for (int idx = 0; idx < timerHardwareCount; idx++) {
+            timerHardware_t *timHw = &timerHardware[idx];
+            outputMode_e mode = timerOverrides(timer2id(timHw->tim))->outputMode;
+            bool isDedicated = (priority == 0);
 
-        // Make sure first motorCount motor outputs get assigned to motor
-        if (TIM_IS_MOTOR(timHw->usageFlags) && (motorIdx < motorCount)) {
-            timHw->usageFlags &= ~TIM_USE_SERVO;
-            pwmClaimTimer(timHw->tim, timHw->usageFlags);
-            motorIdx += 1;
-        }
+            if (checkPwmTimerConflicts(timHw)) {
+                if (priority == 0) {
+                    LOG_WARNING(PWM, "Timer output %d skipped", idx);
+                }
+                continue;
+            }
 
-        if (TIM_IS_SERVO(timHw->usageFlags) && !pwmHasMotorOnTimer(timOutputs, timHw->tim)) {
-            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;
-        }
+            // Motors: dedicated (OUTPUT_MODE_MOTORS) first, then auto
+            if (TIM_IS_MOTOR(timHw->usageFlags) && motorIdx < motorCount
+                    && !pwmHasServoOnTimer(timOutputs, timHw->tim)
+                    && (isDedicated ? mode == OUTPUT_MODE_MOTORS : mode != OUTPUT_MODE_MOTORS)) {
+                pwmAssignOutput(timOutputs, timHw, MAP_TO_MOTOR_OUTPUT);
+                motorIdx++;
+                continue;
+            }
+
+            // Servos: dedicated (OUTPUT_MODE_SERVOS) first, then auto
+            if (TIM_IS_SERVO(timHw->usageFlags)
+                    && !pwmHasMotorOnTimer(timOutputs, timHw->tim)
+                    && (isDedicated ? mode == OUTPUT_MODE_SERVOS : mode != OUTPUT_MODE_SERVOS)) {
+                pwmAssignOutput(timOutputs, timHw, MAP_TO_SERVO_OUTPUT);
+                continue;
+            }
 
-        switch(type) {
-            case MAP_TO_MOTOR_OUTPUT:
-                timHw->usageFlags &= TIM_USE_MOTOR;
-                timOutputs->timMotors[timOutputs->maxTimMotorCount++] = timHw;
-                pwmClaimTimer(timHw->tim, timHw->usageFlags);
-                break;
-            case MAP_TO_SERVO_OUTPUT:
-                timHw->usageFlags &= TIM_USE_SERVO;
-                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;
+            // LEDs: only on the auto pass, and only if timer is uncontested
+            if (!isDedicated && TIM_IS_LED(timHw->usageFlags)
+                    && !pwmHasMotorOnTimer(timOutputs, timHw->tim)
+                    && !pwmHasServoOnTimer(timOutputs, timHw->tim)) {
+                pwmAssignOutput(timOutputs, timHw, MAP_TO_LED_OUTPUT);
+            }
         }
     }
 }

From 6af160dd6e9966e1a980ec812f261929dc7cdd33 Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Fri, 20 Mar 2026 01:14:10 -0500
Subject: [PATCH 047/102] Fix forward declaration guards for
 loadDmaBufferDshot/Stride

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

diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c
index ca1838f197b..a4efb9e3008 100644
--- a/src/main/drivers/pwm_output.c
+++ b/src/main/drivers/pwm_output.c
@@ -126,8 +126,11 @@ static uint8_t commandsBuff[DHSOT_COMMAND_QUEUE_SIZE];
 static currentExecutingCommand_t currentExecutingCommand;
 
 static uint16_t prepareDshotPacket(const uint16_t value, bool requestTelemetry);
+#ifndef USE_DSHOT_DMAR
 static void loadDmaBufferDshot(timerDMASafeType_t *dmaBuffer, uint16_t packet);
+#else
 static void loadDmaBufferDshotStride(timerDMASafeType_t *dmaBuffer, int stride, uint16_t packet);
+#endif
 
 #ifdef USE_DSHOT_DMAR
 burstDmaTimer_t burstDmaTimers[MAX_DMA_TIMERS];

From d287397de2a122e423c6ba1e4f46e725e3960751 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Fri, 20 Mar 2026 23:14:42 +0000
Subject: [PATCH 048/102] BB acc fix

---
 src/main/blackbox/blackbox.c    | 7 ++++---
 src/main/sensors/acceleration.c | 2 +-
 2 files changed, 5 insertions(+), 4 deletions(-)

diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index e4c2afb3eec..f5225ca3e1e 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -504,7 +504,7 @@ typedef struct blackboxMainState_s {
     int16_t gyroPeaksYaw[DYN_NOTCH_PEAK_COUNT];
 
     int16_t accADC[XYZ_AXIS_COUNT];
-    int16_t accVib;
+    uint16_t accVib;
     int16_t attitude[XYZ_AXIS_COUNT];
     int32_t debug[DEBUG32_VALUE_COUNT];
     int16_t motor[MAX_SUPPORTED_MOTORS];
@@ -1642,7 +1642,7 @@ static void loadMainState(timeUs_t currentTimeUs)
         blackboxCurrent->axisPID_D[i] = axisPID_D[i];
         blackboxCurrent->axisPID_F[i] = axisPID_F[i];
         blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]);
-        blackboxCurrent->accADC[i] = lrintf(acc.accADCf[i] * acc.dev.acc_1G);
+        blackboxCurrent->accADC[i] = constrain(lrintf(acc.accADCf[i] * acc.dev.acc_1G), -32678, 32767);
         blackboxCurrent->gyroRaw[i] = lrintf(gyro.gyroRaw[i]);
 
 #ifdef USE_DYNAMIC_FILTERS
@@ -1668,7 +1668,8 @@ static void loadMainState(timeUs_t currentTimeUs)
             blackboxCurrent->mcVelAxisOutput[i] = lrintf(nav_pids->vel[i].output_constrained);
         }
     }
-    blackboxCurrent->accVib = lrintf(accGetVibrationLevel() * acc.dev.acc_1G);
+
+    blackboxCurrent->accVib = constrain(lrintf(accGetVibrationLevel() * acc.dev.acc_1G), 0, 65535);
 
     if (STATE(FIXED_WING_LEGACY)) {
 
diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c
index 16d10624c69..1a384e8645f 100644
--- a/src/main/sensors/acceleration.c
+++ b/src/main/sensors/acceleration.c
@@ -586,8 +586,8 @@ void accUpdate(void)
         // calc difference from this sample and 5hz filtered value, square and filter at 2hz
         const float accDiff = acc.accADCf[axis] - accFloorFilt;
         acc.accVibeSq[axis] = pt1FilterApply(&accVibeFilter[axis], accDiff * accDiff);
-        acc.accVibe = fast_fsqrtf(acc.accVibeSq[X] + acc.accVibeSq[Y] + acc.accVibeSq[Z]);
     }
+    acc.accVibe = fast_fsqrtf(acc.accVibeSq[X] + acc.accVibeSq[Y] + acc.accVibeSq[Z]);
 
     // Filter acceleration
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {

From 501fc54a46e31d62a257cd57bfcd6beac2e0d4dd Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Sun, 22 Mar 2026 14:00:11 -0500
Subject: [PATCH 049/102] Fix BLUEBERRYH743 target configuration issues

- Change TARGET_BOARD_IDENTIFIER from "H743" to "BB43" (unique 4-char ID)
- Remove BEEPER_PWM_FREQUENCY (no matching DEF_TIM entry)
- Remove USE_SOFTSERIAL1 (unnecessary with 8 hardware UARTs + VCP)
- Update SERIAL_PORT_COUNT from 9 to 8
- Remove beeper PWM and softserial timer entries from target.c
- Fix trailing whitespace on DEF_TIM S3 line
---
 src/main/target/BLUEBERRYH743/config.c | 3 ---
 src/main/target/BLUEBERRYH743/target.c | 8 +-------
 src/main/target/BLUEBERRYH743/target.h | 9 +++------
 3 files changed, 4 insertions(+), 16 deletions(-)

diff --git a/src/main/target/BLUEBERRYH743/config.c b/src/main/target/BLUEBERRYH743/config.c
index bcc1e5c91fd..0673117c182 100644
--- a/src/main/target/BLUEBERRYH743/config.c
+++ b/src/main/target/BLUEBERRYH743/config.c
@@ -28,7 +28,4 @@ void targetConfiguration(void)
 {
     pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1;
     pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2;
-    beeperConfigMutable()->pwmMode = true;
-    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].functionMask = FUNCTION_MSP;
-    serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].msp_baudrateIndex = BAUD_115200;
 }
diff --git a/src/main/target/BLUEBERRYH743/target.c b/src/main/target/BLUEBERRYH743/target.c
index bc2fb01558a..5245d05e257 100644
--- a/src/main/target/BLUEBERRYH743/target.c
+++ b/src/main/target/BLUEBERRYH743/target.c
@@ -35,7 +35,7 @@ timerHardware_t timerHardware[] = {
     DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0),   // S1
     DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 1),   // S2
 
-    DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 2),   // S3  
+    DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 2),   // S3
     DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 3),   // S4
     DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 4),   // S5
     DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 5),   // S6
@@ -49,12 +49,6 @@ timerHardware_t timerHardware[] = {
     DEF_TIM(TIM15, CH2, PE6, TIM_USE_OUTPUT_AUTO, 0, 0),   // S12 DMA_NONE
 
     DEF_TIM(TIM1,  CH1, PA8,  TIM_USE_LED, 0, 9),    // LED_2812
-    DEF_TIM(TIM2,  CH1, PA15, TIM_USE_BEEPER, 0, 0),  // BEEPER PWM
-
-    // DEF_TIM(TIM8,  CH2, PC7,  TIM_USE_PPM, 0, 0),   // RX6 PPM
-    DEF_TIM(TIM8,  CH1, PC6,  TIM_USE_ANY, 0, 0),   // TX6 SoftwareSerial   
-    // DEF_TIM(TIM16, CH1, PB8,  TIM_USE_ANY, 0, 0),   // RX4
-    // DEF_TIM(TIM17, CH1, PB9,  TIM_USE_ANY, 0, 0),   // TX4
 };
 
 const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
diff --git a/src/main/target/BLUEBERRYH743/target.h b/src/main/target/BLUEBERRYH743/target.h
index f11e822d633..b69400d0674 100644
--- a/src/main/target/BLUEBERRYH743/target.h
+++ b/src/main/target/BLUEBERRYH743/target.h
@@ -18,7 +18,7 @@
 
 #pragma once
 
-#define TARGET_BOARD_IDENTIFIER "H743"
+#define TARGET_BOARD_IDENTIFIER "BB43"
 
 #if defined(BLUEBERRYH743HD)
   #define USBD_PRODUCT_STRING     "BLUEBERRYH743HD"
@@ -33,7 +33,6 @@
 
 #define BEEPER                  PA15
 #define BEEPER_INVERTED
-#define BEEPER_PWM_FREQUENCY    2500
 
 // *************** IMU generic ***********************
 #define USE_DUAL_GYRO
@@ -152,11 +151,8 @@
 #define UART8_TX_PIN            PE1
 #define UART8_RX_PIN            PE0
 
-#define USE_SOFTSERIAL1
-#define SOFTSERIAL_1_TX_PIN      PC6  //TX6 pad
-#define SOFTSERIAL_1_RX_PIN      PC6  //TX6 pad
 
-#define SERIAL_PORT_COUNT       9
+#define SERIAL_PORT_COUNT       8
 
 #define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
 #define SERIALRX_PROVIDER       SERIALRX_CRSF
@@ -198,6 +194,7 @@
 
 #define DEFAULT_FEATURES            (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX)
 #define CURRENT_METER_SCALE         250
+#define CURRENT_METER_OFFSET    400
 
 #define USE_SERIAL_4WAY_BLHELI_INTERFACE
 

From 1e66912f1ad8d244117f781d53df01ac9ed47c7d Mon Sep 17 00:00:00 2001
From: liyang <coder.liyang@gmail.com>
Date: Tue, 24 Mar 2026 16:32:51 +0800
Subject: [PATCH 050/102] Fix the issue where the default configuration
 parameters for the log do not take effect

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

diff --git a/src/main/common/log.c b/src/main/common/log.c
index 050422561d7..c2cc30a3a48 100644
--- a/src/main/common/log.c
+++ b/src/main/common/log.c
@@ -54,7 +54,7 @@
 static serialPort_t * logPort = NULL;
 static mspPort_t * mspLogPort = NULL;
 
-PG_REGISTER(logConfig_t, logConfig, PG_LOG_CONFIG, 0);
+PG_REGISTER_WITH_RESET_TEMPLATE(logConfig_t, logConfig, PG_LOG_CONFIG, 0);
 
 PG_RESET_TEMPLATE(logConfig_t, logConfig,
     .level = SETTING_LOG_LEVEL_DEFAULT,

From 216b76c212b5c04929fd306d0853c27b20d99c4e Mon Sep 17 00:00:00 2001
From: Daniel Ribeiro <danarrib@gmail.com>
Date: Sun, 29 Mar 2026 13:28:25 -0300
Subject: [PATCH 051/102] Add three new MSP2 commands for GCS-initiated flight
 control
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Adds three new write-only MSP2 commands that allow a ground control
station to interact with an active INAV navigation session without
requiring direct RC transmitter input:

  MSP2_INAV_SET_WP_INDEX (0x2221)
    Jump to a specific waypoint during an active WP mission.
    Payload: U8 wp_index (0-based, relative to mission start waypoint).
    Preconditions: aircraft must be armed and NAV_WP_MODE must be active.
    Transitions any active waypoint FSM state back to PRE_ACTION so
    INAV re-initialises navigation for the new target waypoint.

  MSP2_INAV_SET_ALT_TARGET (0x2222)
    Set a new target altitude while altitude-controlled navigation is active.
    Payload: I32 altitude_cm (centimetres, relative to home).
    Preconditions: aircraft must be armed and NAV_CTL_ALT must be active.
    Calls updateClimbRateToAltitudeController() which works for both
    multicopter and fixed-wing platforms.

  MSP2_INAV_SET_CRUISE_HEADING (0x2223)
    Set the heading target while Cruise or Course Hold mode is active.
    Payload: I32 heading_centidegrees (0–35999).
    Preconditions: aircraft must be armed and NAV_COURSE_HOLD_MODE must
    be active. Sets both posControl.cruise.course and previousCourse to
    prevent spurious heading adjustments on the next control cycle.

Implementation details:
  - New FSM event NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP aliases
    NAV_FSM_EVENT_STATE_SPECIFIC_4 (safe: WP and RTH states never
    overlap so the same underlying integer can be reused in both groups).
  - FSM transitions added for PRE_ACTION, IN_PROGRESS, REACHED,
    HOLD_TIME and FINISHED states, all routing to PRE_ACTION.
  - Three new public functions declared in navigation.h and implemented
    in navigation.c: navSetActiveWaypointIndex(), navSetDesiredAltitude(),
    navSetCruiseHeading().
  - MSP handlers added in fc_msp.c; all return MSP_RESULT_ERROR if the
    aircraft is not in a compatible state.
---
 src/main/fc/fc_msp.c                     | 33 +++++++++
 src/main/msp/msp_protocol_v2_inav.h      |  4 +
 src/main/navigation/navigation.c         | 93 ++++++++++++++++++++++++
 src/main/navigation/navigation.h         |  3 +
 src/main/navigation/navigation_private.h |  1 +
 5 files changed, 134 insertions(+)

diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index cf5308067e9..0e33178664d 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -3684,6 +3684,39 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
         }
         break;
 
+    case MSP2_INAV_SET_CRUISE_HEADING:
+        // Set heading while Cruise / Course Hold is active.
+        // Payload: I32  heading_centidegrees  (0–35999)
+        if (dataSize == 4) {
+            int32_t headingCd;
+            if (sbufReadI32Safe(&headingCd, src) && navSetCruiseHeading(headingCd)) {
+                break;
+            }
+        }
+        return MSP_RESULT_ERROR;
+
+    case MSP2_INAV_SET_WP_INDEX:
+        // Jump to waypoint N during an active WP mission.
+        // Payload: U8  wp_index  (0-based, relative to mission start waypoint)
+        if (dataSize == 1) {
+            uint8_t wpIndex;
+            if (sbufReadU8Safe(&wpIndex, src) && navSetActiveWaypointIndex(wpIndex)) {
+                break;
+            }
+        }
+        return MSP_RESULT_ERROR;
+
+    case MSP2_INAV_SET_ALT_TARGET:
+        // Set a new target altitude while altitude-controlled navigation is active.
+        // Payload: I32  altitude_cm  (centimetres, relative to home)
+        if (dataSize == 4) {
+            int32_t altCm;
+            if (sbufReadI32Safe(&altCm, src) && navSetDesiredAltitude(altCm)) {
+                break;
+            }
+        }
+        return MSP_RESULT_ERROR;
+
     default:
         return MSP_RESULT_ERROR;
     }
diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h
index 0b893916895..3f20df8e61e 100755
--- a/src/main/msp/msp_protocol_v2_inav.h
+++ b/src/main/msp/msp_protocol_v2_inav.h
@@ -126,3 +126,7 @@
 #define MSP2_INAV_SET_GVAR                      0x2214
 
 #define MSP2_INAV_FULL_LOCAL_POSE               0x2220
+
+#define MSP2_INAV_SET_WP_INDEX                  0x2221  //in message  jump to waypoint N during active WP mission; payload: U8 wp_index (0-based, relative to mission start)
+#define MSP2_INAV_SET_ALT_TARGET                0x2222  //in message  set target altitude for active altitude-hold / cruise / WP mode; payload: I32 altitude_cm (relative to home)
+#define MSP2_INAV_SET_CRUISE_HEADING            0x2223  //in message  set heading while in Cruise/Course Hold mode; payload: I32 heading_centidegrees (0-35999)
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index e7f0774925e..1fd7edc0d30 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -785,6 +785,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
             [NAV_FSM_EVENT_SWITCH_TO_IDLE]              = NAV_STATE_IDLE,
             [NAV_FSM_EVENT_SWITCH_TO_RTH]               = NAV_STATE_RTH_INITIALIZE,
             [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED,
+            [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP]     = NAV_STATE_WAYPOINT_PRE_ACTION,   // MSP2_INAV_SET_WP_INDEX
         }
     },
 
@@ -806,6 +807,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_WAYPOINT_JUMP]        = NAV_STATE_WAYPOINT_PRE_ACTION,    // MSP2_INAV_SET_WP_INDEX
         }
     },
 
@@ -830,6 +832,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_WAYPOINT_JUMP]     = NAV_STATE_WAYPOINT_PRE_ACTION,    // MSP2_INAV_SET_WP_INDEX
         }
     },
 
@@ -851,6 +854,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_WAYPOINT_JUMP]        = NAV_STATE_WAYPOINT_PRE_ACTION,    // MSP2_INAV_SET_WP_INDEX
         }
     },
 
@@ -909,6 +913,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_WAYPOINT_JUMP]        = NAV_STATE_WAYPOINT_PRE_ACTION,    // MSP2_INAV_SET_WP_INDEX
         }
     },
 
@@ -3876,6 +3881,94 @@ int isGCSValid(void)
             (posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS));
 }
 
+/*
+ * navSetActiveWaypointIndex - MSP2_INAV_SET_WP_INDEX handler
+ *
+ * Jumps to a specific waypoint during an active WP mission without interrupting
+ * navigation mode.  'index' is 0-based and relative to the mission start
+ * (i.e. the first waypoint in the loaded mission is index 0, regardless of
+ * startWpIndex).
+ *
+ * Returns true on success, false when the preconditions are not met (not armed,
+ * not in WP mode, or index out of range).
+ */
+bool navSetActiveWaypointIndex(uint8_t index)
+{
+    // Must be armed and actively executing a WP mission
+    if (!ARMING_FLAG(ARMED) || !FLIGHT_MODE(NAV_WP_MODE)) {
+        return false;
+    }
+
+    // Translate user-visible 0-based index to the internal absolute index
+    int8_t absoluteIndex = (int8_t)index + posControl.startWpIndex;
+    if (absoluteIndex < posControl.startWpIndex ||
+        absoluteIndex >= posControl.startWpIndex + posControl.waypointCount) {
+        return false;
+    }
+
+    posControl.activeWaypointIndex = absoluteIndex;
+    posControl.wpMissionRestart = false;
+
+    // Transition immediately to WAYPOINT_PRE_ACTION so the new WP is set up
+    // on this navigation tick.  navProcessFSMEvents is safe to call here as
+    // everything runs in the same main-loop task context.
+    navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP);
+    return true;
+}
+
+/*
+ * navSetDesiredAltitude - MSP2_INAV_SET_ALT_TARGET handler
+ *
+ * Commands the aircraft to climb or descend to 'altCm' centimetres above home
+ * while altitude-controlled navigation (altitude hold, cruise, or WP mode) is
+ * active.  The altitude controller already implements this path for waypoint
+ * navigation (ROC_TO_ALT_TARGET); this function exposes it via MSP.
+ *
+ * Returns true on success, false when altitude control is not currently active.
+ */
+bool navSetDesiredAltitude(int32_t altCm)
+{
+    if (!ARMING_FLAG(ARMED)) {
+        return false;
+    }
+
+    // Require active altitude control — covers althold, cruise and WP modes
+    if (!(navGetCurrentStateFlags() & NAV_CTL_ALT)) {
+        return false;
+    }
+
+    updateClimbRateToAltitudeController(0.0f, (float)altCm, ROC_TO_ALT_TARGET);
+    return true;
+}
+
+/*
+ * navSetCruiseHeading - MSP2_INAV_SET_CRUISE_HEADING handler
+ *
+ * Sets the target heading while Cruise or Course Hold mode is active.
+ * 'headingCd' is in centidegrees (0-35999), matching the internal
+ * posControl.cruise.course representation.
+ *
+ * Returns true on success, false when the preconditions are not met.
+ */
+bool navSetCruiseHeading(int32_t headingCd)
+{
+    if (!ARMING_FLAG(ARMED)) {
+        return false;
+    }
+
+    // Only valid while Cruise or Course Hold is the active navigation mode
+    if (!FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
+        return false;
+    }
+
+    // Clamp to valid centidegree range
+    headingCd = ((headingCd % 36000) + 36000) % 36000;
+
+    posControl.cruise.course         = headingCd;
+    posControl.cruise.previousCourse = headingCd;
+    return true;
+}
+
 void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
 {
     gpsLocation_t wpLLH;
diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
index ddc7e23e76d..bb540d94f04 100644
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@@ -708,6 +708,9 @@ int isGCSValid(void);
 void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData);
 void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData);
 void resetWaypointList(void);
+bool navSetActiveWaypointIndex(uint8_t index);  // MSP2_INAV_SET_WP_INDEX: jump to WP during active mission
+bool navSetDesiredAltitude(int32_t altCm);      // MSP2_INAV_SET_ALT_TARGET: set target altitude (cm, relative to home)
+bool navSetCruiseHeading(int32_t headingCd);    // MSP2_INAV_SET_CRUISE_HEADING: set cruise/course-hold heading (centidegrees)
 bool loadNonVolatileWaypointList(bool clearIfLoaded);
 bool saveNonVolatileWaypointList(void);
 #ifdef USE_MULTI_MISSION
diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
index f6cf9329e98..a1f07e470c0 100644
--- a/src/main/navigation/navigation_private.h
+++ b/src/main/navigation/navigation_private.h
@@ -178,6 +178,7 @@ typedef enum {
     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_WAYPOINT_JUMP     = NAV_FSM_EVENT_STATE_SPECIFIC_4,  // jump to a different WP index mid-mission
     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,

From 25dd9ddec6c96655a0eaf1fd7a04c7e8174a7d05 Mon Sep 17 00:00:00 2001
From: Daniel Ribeiro <danarrib@gmail.com>
Date: Mon, 30 Mar 2026 14:53:18 -0300
Subject: [PATCH 052/102] =?UTF-8?q?Remove=20MSP2=5FINAV=5FSET=5FALT=5FTARG?=
 =?UTF-8?q?ET=20(0x2222)=20=E2=80=94=20superseded=20by=20existing=20implem?=
 =?UTF-8?q?entation?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

The set-altitude functionality is already covered by a more mature existing
implementation in INAV. Remove the MSP2_INAV_SET_ALT_TARGET command added in
the previous commit, keeping only MSP2_INAV_SET_WP_INDEX (0x2221) and
MSP2_INAV_SET_CRUISE_HEADING (0x2223).

Removes: define in msp_protocol_v2_inav.h, navSetDesiredAltitude() in
navigation.c/.h, the MSP handler in fc_msp.c, and the docs section.
---
 docs/development/msp/README.md      | 32 ++++++++++++++++++++++++++++-
 src/main/fc/fc_msp.c                | 11 ----------
 src/main/msp/msp_protocol_v2_inav.h |  1 -
 src/main/navigation/navigation.c    | 25 ----------------------
 src/main/navigation/navigation.h    |  1 -
 5 files changed, 31 insertions(+), 39 deletions(-)

diff --git a/docs/development/msp/README.md b/docs/development/msp/README.md
index d85974a65c0..696e7ab7fc1 100644
--- a/docs/development/msp/README.md
+++ b/docs/development/msp/README.md
@@ -411,7 +411,9 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i
 [8722 - MSP2_INAV_GEOZONE_VERTEX](#msp2_inav_geozone_vertex)  
 [8723 - MSP2_INAV_SET_GEOZONE_VERTEX](#msp2_inav_set_geozone_vertex)  
 [8724 - MSP2_INAV_SET_GVAR](#msp2_inav_set_gvar)  
-[8736 - MSP2_INAV_FULL_LOCAL_POSE](#msp2_inav_full_local_pose)  
+[8736 - MSP2_INAV_FULL_LOCAL_POSE](#msp2_inav_full_local_pose)
+[8737 - MSP2_INAV_SET_WP_INDEX](#msp2_inav_set_wp_index)
+[8739 - MSP2_INAV_SET_CRUISE_HEADING](#msp2_inav_set_cruise_heading)
 [12288 - MSP2_BETAFLIGHT_BIND](#msp2_betaflight_bind)
 [12289 - MSP2_RX_BIND](#msp2_rx_bind)
 
@@ -4526,6 +4528,34 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i
 
 **Notes:** All attitude angles are in deci-degrees.
 
+## <a id="msp2_inav_set_wp_index"></a>`MSP2_INAV_SET_WP_INDEX (8737 / 0x2221)`
+**Description:** Jumps to a specific waypoint during an active waypoint mission, causing the aircraft to immediately begin navigating toward the new target waypoint.
+
+**Request Payload:**
+|Field|C Type|Size (Bytes)|Units|Description|
+|---|---|---|---|---|
+| `wp_index` | `uint8_t` | 1 | - | 0-based waypoint index to jump to, relative to the mission start waypoint (`posControl.startWpIndex`) |
+
+**Reply Payload:** **None**
+
+**Notes:** Returns error if the aircraft is not armed, `NAV_WP_MODE` is not active, or the index is outside the valid mission range (`startWpIndex` to `startWpIndex + waypointCount - 1`). On success, sets `posControl.activeWaypointIndex` to the requested index and fires `NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP`, transitioning the navigation FSM back to `NAV_STATE_WAYPOINT_PRE_ACTION` so the flight controller re-initialises navigation for the new target.
+
+---
+
+## <a id="msp2_inav_set_cruise_heading"></a>`MSP2_INAV_SET_CRUISE_HEADING (8739 / 0x2223)`
+**Description:** Sets the course heading target while Cruise or Course Hold mode is active, causing the aircraft to turn to and maintain the new heading.
+
+**Request Payload:**
+|Field|C Type|Size (Bytes)|Units|Description|
+|---|---|---|---|---|
+| `heading_centidegrees` | `int32_t` | 4 | centidegrees | Target heading in centidegrees (0–35999). Values are wrapped modulo 36000 before being applied. |
+
+**Reply Payload:** **None**
+
+**Notes:** Returns error if the aircraft is not armed or `NAV_COURSE_HOLD_MODE` is not active. On success, sets both `posControl.cruise.course` and `posControl.cruise.previousCourse` to the normalised value, preventing spurious heading adjustments from `getCruiseHeadingAdjustment()` on the next control cycle.
+
+---
+
 ## <a id="msp2_betaflight_bind"></a>`MSP2_BETAFLIGHT_BIND (12288 / 0x3000)`
 **Description:** Initiates the receiver binding procedure for supported serial protocols (CRSF, SRXL2).  
 
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 0e33178664d..b7da917520f 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -3706,17 +3706,6 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
         }
         return MSP_RESULT_ERROR;
 
-    case MSP2_INAV_SET_ALT_TARGET:
-        // Set a new target altitude while altitude-controlled navigation is active.
-        // Payload: I32  altitude_cm  (centimetres, relative to home)
-        if (dataSize == 4) {
-            int32_t altCm;
-            if (sbufReadI32Safe(&altCm, src) && navSetDesiredAltitude(altCm)) {
-                break;
-            }
-        }
-        return MSP_RESULT_ERROR;
-
     default:
         return MSP_RESULT_ERROR;
     }
diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h
index 3f20df8e61e..21ad7e3d135 100755
--- a/src/main/msp/msp_protocol_v2_inav.h
+++ b/src/main/msp/msp_protocol_v2_inav.h
@@ -128,5 +128,4 @@
 #define MSP2_INAV_FULL_LOCAL_POSE               0x2220
 
 #define MSP2_INAV_SET_WP_INDEX                  0x2221  //in message  jump to waypoint N during active WP mission; payload: U8 wp_index (0-based, relative to mission start)
-#define MSP2_INAV_SET_ALT_TARGET                0x2222  //in message  set target altitude for active altitude-hold / cruise / WP mode; payload: I32 altitude_cm (relative to home)
 #define MSP2_INAV_SET_CRUISE_HEADING            0x2223  //in message  set heading while in Cruise/Course Hold mode; payload: I32 heading_centidegrees (0-35999)
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 1fd7edc0d30..03d4a3da36b 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -3916,31 +3916,6 @@ bool navSetActiveWaypointIndex(uint8_t index)
     return true;
 }
 
-/*
- * navSetDesiredAltitude - MSP2_INAV_SET_ALT_TARGET handler
- *
- * Commands the aircraft to climb or descend to 'altCm' centimetres above home
- * while altitude-controlled navigation (altitude hold, cruise, or WP mode) is
- * active.  The altitude controller already implements this path for waypoint
- * navigation (ROC_TO_ALT_TARGET); this function exposes it via MSP.
- *
- * Returns true on success, false when altitude control is not currently active.
- */
-bool navSetDesiredAltitude(int32_t altCm)
-{
-    if (!ARMING_FLAG(ARMED)) {
-        return false;
-    }
-
-    // Require active altitude control — covers althold, cruise and WP modes
-    if (!(navGetCurrentStateFlags() & NAV_CTL_ALT)) {
-        return false;
-    }
-
-    updateClimbRateToAltitudeController(0.0f, (float)altCm, ROC_TO_ALT_TARGET);
-    return true;
-}
-
 /*
  * navSetCruiseHeading - MSP2_INAV_SET_CRUISE_HEADING handler
  *
diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
index bb540d94f04..eb1621e9f8d 100644
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@@ -709,7 +709,6 @@ void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData);
 void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData);
 void resetWaypointList(void);
 bool navSetActiveWaypointIndex(uint8_t index);  // MSP2_INAV_SET_WP_INDEX: jump to WP during active mission
-bool navSetDesiredAltitude(int32_t altCm);      // MSP2_INAV_SET_ALT_TARGET: set target altitude (cm, relative to home)
 bool navSetCruiseHeading(int32_t headingCd);    // MSP2_INAV_SET_CRUISE_HEADING: set cruise/course-hold heading (centidegrees)
 bool loadNonVolatileWaypointList(bool clearIfLoaded);
 bool saveNonVolatileWaypointList(void);

From ddef1638bf60190c2068592900808c102747b2f5 Mon Sep 17 00:00:00 2001
From: nventurino <nventurino@gmail.com>
Date: Tue, 31 Mar 2026 14:58:58 -0700
Subject: [PATCH 053/102] Add support for MS5525DSO digital airspeed sensor

This commit introduces a hardware driver for the TE Connectivity MS5525DSO digital airspeed sensor over I2C, specifically calibrated for the 1 PSI variant.

Changes included:

- Registered MS5525 in pitot_hardware for CLI assignment

- Implemented pitotmeter_ms5525.c/h conforming to the 20-byte scratchpad limit

- Alternating D1/D2 polling for non-blocking execution length

- Added standard I2C bus device discovery for 0x76/0x77 addresses

- Added missing USE_PITOT_MS5525 flag to standard build targets

- Included I2C transaction success checks to fallback on failure

Tested compiling and executing successfully on KAKUTEH7WING target.

Resolves #4881
---
 src/main/CMakeLists.txt                       |   2 +
 src/main/drivers/bus.h                        |   1 +
 .../drivers/pitotmeter/pitotmeter_ms5525.c    | 222 ++++++++++++++++++
 .../drivers/pitotmeter/pitotmeter_ms5525.h    |  22 ++
 src/main/fc/settings.yaml                     |   2 +-
 src/main/sensors/pitotmeter.c                 |  14 ++
 src/main/sensors/pitotmeter.h                 |   1 +
 src/main/target/common.h                      |   1 +
 src/main/target/common_hardware.c             |   8 +
 9 files changed, 272 insertions(+), 1 deletion(-)
 create mode 100644 src/main/drivers/pitotmeter/pitotmeter_ms5525.c
 create mode 100644 src/main/drivers/pitotmeter/pitotmeter_ms5525.h

diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt
index 101c7223372..77837715925 100755
--- a/src/main/CMakeLists.txt
+++ b/src/main/CMakeLists.txt
@@ -217,6 +217,8 @@ main_sources(COMMON_SRC
     drivers/pitotmeter/pitotmeter_adc.h
     drivers/pitotmeter/pitotmeter_ms4525.c
     drivers/pitotmeter/pitotmeter_ms4525.h
+    drivers/pitotmeter/pitotmeter_ms5525.c
+    drivers/pitotmeter/pitotmeter_ms5525.h
     drivers/pitotmeter/pitotmeter_dlvr_l10d.c
     drivers/pitotmeter/pitotmeter_dlvr_l10d.h
     drivers/pitotmeter/pitotmeter_msp.c
diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h
index 5a3e6dba453..0ac47c46613 100644
--- a/src/main/drivers/bus.h
+++ b/src/main/drivers/bus.h
@@ -140,6 +140,7 @@ typedef enum {
 
     /* Other hardware */
     DEVHW_MS4525,       // Pitot meter
+    DEVHW_MS5525,       // Pitot meter
     DEVHW_DLVR,         // Pitot meter
     DEVHW_M25P16,       // SPI NOR flash
     DEVHW_W25N,         // SPI 128MB or 256MB flash from Winbond W25N family
diff --git a/src/main/drivers/pitotmeter/pitotmeter_ms5525.c b/src/main/drivers/pitotmeter/pitotmeter_ms5525.c
new file mode 100644
index 00000000000..e26114b2e95
--- /dev/null
+++ b/src/main/drivers/pitotmeter/pitotmeter_ms5525.c
@@ -0,0 +1,222 @@
+/*
+ * 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 "build/debug.h"
+
+#include "common/utils.h"
+#include "common/maths.h"
+#include "drivers/time.h"
+#include "drivers/bus_i2c.h"
+#include "drivers/pitotmeter/pitotmeter.h"
+#include "drivers/pitotmeter/pitotmeter_ms5525.h"
+
+// MS5525 I2C Addresses
+#define MS5525_ADDR_1             0x76
+#define MS5525_ADDR_2             0x77
+
+#define CMD_RESET               0x1E // ADC reset command
+#define CMD_ADC_READ            0x00 // ADC read command
+#define CMD_ADC_CONV            0x40 // ADC conversion command
+#define CMD_ADC_D1              0x00 // ADC D1 conversion (Pressure)
+#define CMD_ADC_D2              0x10 // ADC D2 conversion (Temperature)
+#define CMD_ADC_4096            0x08 // ADC OSR=4096
+#define CMD_PROM_RD             0xA0 // Prom read command
+#define PROM_NB                 8
+
+typedef struct __attribute__ ((__packed__)) ms5525Ctx_s {
+    uint16_t c[6]; // c1 through c6
+    uint32_t up;   // up (24 bits) + step (top 8 bits)
+    uint32_t ut;   // ut (24 bits)
+} ms5525Ctx_t;
+
+STATIC_ASSERT(sizeof(ms5525Ctx_t) <= BUS_SCRATCHPAD_MEMORY_SIZE, busDevice_scratchpad_memory_too_small);
+
+static int8_t ms5525_crc(uint16_t *prom)
+{
+    int32_t i, j;
+    uint32_t res = 0;
+    uint8_t crc = prom[7] & 0xF;
+    prom[7] &= 0xFF00;
+
+    bool blankEeprom = true;
+
+    for (i = 0; i < 16; i++) {
+        if (prom[i >> 1]) {
+            blankEeprom = false;
+        }
+        if (i & 1)
+            res ^= ((prom[i >> 1]) & 0x00FF);
+        else
+            res ^= (prom[i >> 1] >> 8);
+        for (j = 8; j > 0; j--) {
+            if (res & 0x8000)
+                res ^= 0x1800;
+            res <<= 1;
+        }
+    }
+    prom[7] |= crc;
+    if (!blankEeprom && crc == ((res >> 12) & 0xF))
+        return 0;
+
+    return -1;
+}
+
+static bool ms5525_read_adc(pitotDev_t *pitot, uint32_t *result)
+{
+    uint8_t rxbuf[3];
+    if (busReadBuf(pitot->busDev, CMD_ADC_READ, rxbuf, 3)) {
+        *result = (rxbuf[0] << 16) | (rxbuf[1] << 8) | rxbuf[2];
+        return true;
+    }
+    return false;
+}
+
+static bool ms5525_start(pitotDev_t * pitot)
+{
+    ms5525Ctx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev);
+    uint8_t step = ctx->up >> 24;
+    
+    if (step == 0) {
+        return busWrite(pitot->busDev, CMD_ADC_CONV + CMD_ADC_D1 + CMD_ADC_4096, 1);
+    } else {
+        return busWrite(pitot->busDev, CMD_ADC_CONV + CMD_ADC_D2 + CMD_ADC_4096, 1);
+    }
+}
+
+static bool ms5525_read(pitotDev_t * pitot)
+{
+    ms5525Ctx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev);
+    uint8_t step = ctx->up >> 24;
+    
+    uint32_t adc_val = 0;
+    if (!ms5525_read_adc(pitot, &adc_val)) {
+        return false;
+    }
+    
+    if (step == 0) {
+        ctx->up = adc_val | (1 << 24);
+        return true; 
+    } else {
+        ctx->ut = adc_val;
+        ctx->up &= 0x00FFFFFF; // clear step back to 0
+        return true; 
+    }
+}
+
+static void ms5525_calculate(pitotDev_t * pitot, float *pressure, float *temperature)
+{
+    ms5525Ctx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev);
+
+    uint32_t up = ctx->up & 0x00FFFFFF;
+    uint32_t ut = ctx->ut;
+
+    if (up == 0 || ut == 0) {
+        return; // Wait until both are read at least once
+    }
+
+    // 5525DSO-pp001DS coefficients (1 psi):
+    // Q1=15, Q2=17, Q3=7, Q4=5, Q5=7, Q6=21
+    int64_t dT   = (int64_t)ut - ((int64_t)ctx->c[4] << 7); // c[5] is c[4] (0-indexed)
+    int64_t off  = ((int64_t)ctx->c[1] << 17) + (((int64_t)ctx->c[3] * dT) >> 5); // c[2]=c[1], c[4]=c[3]
+    int64_t sens = ((int64_t)ctx->c[0] << 15) + (((int64_t)ctx->c[2] * dT) >> 7); // c[1]=c[0], c[3]=c[2]
+    int64_t temp = 2000 + ((dT * (int64_t)ctx->c[5]) >> 21); // c[6]=c[5]
+
+    int64_t p_raw = ((((int64_t)up * sens) >> 21) - off) >> 15;
+    
+    // Convert 1 PSI sensor output to Pa
+    // 1 psi = 6894.757 Pa. p_raw is 0.0001 psi per bit -> 0.6894757 Pa per bit.
+    
+    if (pressure) {
+        *pressure = (float)p_raw * 0.6894757f;
+    }
+
+    if (temperature) {
+        *temperature = C_TO_KELVIN(temp / 100.0f);
+    }
+}
+
+static bool deviceDetect(busDevice_t * dev)
+{
+    // Verify an I2C transaction works: read PROM
+    uint8_t rxbuf[2];
+    bool ack = busReadBuf(dev, CMD_PROM_RD, rxbuf, 2);
+    if (ack) {
+        return true;
+    }
+    return false;
+}
+
+bool ms5525Detect(pitotDev_t * pitot)
+{
+    pitot->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_MS5525, 0, OWNER_AIRSPEED);
+    if (pitot->busDev == NULL) {
+        return false;
+    }
+
+    // Try primary address 0x76
+    pitot->busDev->busdev.i2c.address = MS5525_ADDR_1;
+    if (!deviceDetect(pitot->busDev)) {
+        // Fallback to secondary 0x77
+        pitot->busDev->busdev.i2c.address = MS5525_ADDR_2;
+        if (!deviceDetect(pitot->busDev)) {
+            busDeviceDeInit(pitot->busDev);
+            return false;
+        }
+    }
+
+    // Sensor found, initialize
+    busWrite(pitot->busDev, CMD_RESET, 1);
+    delay(5);
+
+    ms5525Ctx_t * ctx = busDeviceGetScratchpadMemory(pitot->busDev);
+    
+    // Read PROM 
+    uint16_t prom[8];
+    for (int i = 0; i < PROM_NB; i++) {
+        uint8_t rxbuf[2] = { 0, 0 };
+        busReadBuf(pitot->busDev, CMD_PROM_RD + i * 2, rxbuf, 2);
+        prom[i] = (rxbuf[0] << 8 | rxbuf[1]);
+    }
+
+    // Check CRC
+    if (ms5525_crc(prom) != 0) {
+        busDeviceDeInit(pitot->busDev);
+        return false;
+    }
+
+    // Copy to ctx starting from c1 to c6
+    for (int i = 0; i < 6; i++) {
+        ctx->c[i] = prom[i+1];
+    }
+
+    // Setup Context
+    ctx->up = 0;
+    ctx->ut = 0;
+
+    // Pitot delays
+    pitot->delay = 10000; // max 9.04ms for OSR4096
+    pitot->calibThreshold = 0.00005f;
+    pitot->start = ms5525_start;
+    pitot->get = ms5525_read;
+    pitot->calculate = ms5525_calculate;
+
+    return true;
+}
diff --git a/src/main/drivers/pitotmeter/pitotmeter_ms5525.h b/src/main/drivers/pitotmeter/pitotmeter_ms5525.h
new file mode 100644
index 00000000000..822ba72e2ed
--- /dev/null
+++ b/src/main/drivers/pitotmeter/pitotmeter_ms5525.h
@@ -0,0 +1,22 @@
+/*
+ * 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
+
+#include "drivers/pitotmeter/pitotmeter.h"
+
+bool ms5525Detect(pitotDev_t *pitot);
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 3e353e9dfcd..8ea60dfd7db 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -17,7 +17,7 @@ tables:
     values: ["NONE", "AUTO", "BMP085", "MS5611", "BMP280", "MS5607", "LPS25H", "SPL06", "BMP388", "DPS310", "B2SMPB", "MSP", "FAKE"]
     enum: baroSensor_e
   - name: pitot_hardware
-    values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE", "MSP", "DLVR-L10D"]
+    values: ["NONE", "AUTO", "MS4525", "ADC", "VIRTUAL", "FAKE", "MSP", "DLVR-L10D", "MS5525"]
     enum: pitotSensor_e
   - name: receiver_type
     values: ["NONE", "SERIAL", "MSP", "SIM (SITL)"]
diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c
index b4b61f57669..12225128eae 100755
--- a/src/main/sensors/pitotmeter.c
+++ b/src/main/sensors/pitotmeter.c
@@ -31,6 +31,7 @@
 
 #include "drivers/pitotmeter/pitotmeter.h"
 #include "drivers/pitotmeter/pitotmeter_ms4525.h"
+#include "drivers/pitotmeter/pitotmeter_ms5525.h"
 #include "drivers/pitotmeter/pitotmeter_dlvr_l10d.h"
 #include "drivers/pitotmeter/pitotmeter_adc.h"
 #include "drivers/pitotmeter/pitotmeter_msp.h"
@@ -111,6 +112,19 @@ bool pitotDetect(pitotDev_t *dev, uint8_t pitotHardwareToUse)
             }
             FALLTHROUGH;
 
+        case PITOT_MS5525:
+#ifdef USE_PITOT_MS5525
+            if (ms5525Detect(dev)) {
+                pitotHardware = PITOT_MS5525;
+                break;
+            }
+#endif
+            /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
+            if (pitotHardwareToUse != PITOT_AUTODETECT) {
+                break;
+            }
+            FALLTHROUGH;
+
         case PITOT_DLVR:
 
 			// Skip autodetection for DLVR (it is indistinguishable from MS4525) and allow only manual config
diff --git a/src/main/sensors/pitotmeter.h b/src/main/sensors/pitotmeter.h
index 69451098ec8..dc5ac422974 100755
--- a/src/main/sensors/pitotmeter.h
+++ b/src/main/sensors/pitotmeter.h
@@ -32,6 +32,7 @@ typedef enum {
     PITOT_FAKE = 5,
     PITOT_MSP = 6,
     PITOT_DLVR = 7,
+    PITOT_MS5525 = 8,
 } pitotSensor_e;
 
 #define PITOT_MAX  PITOT_FAKE
diff --git a/src/main/target/common.h b/src/main/target/common.h
index 45eb12ac4bc..58b572495fe 100644
--- a/src/main/target/common.h
+++ b/src/main/target/common.h
@@ -95,6 +95,7 @@
 // Allow default airspeed sensors
 #define USE_PITOT
 #define USE_PITOT_MS4525
+#define USE_PITOT_MS5525
 #define USE_PITOT_MSP
 #define USE_PITOT_DLVR
 
diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c
index aac3db564cc..d77bb535648 100755
--- a/src/main/target/common_hardware.c
+++ b/src/main/target/common_hardware.c
@@ -397,6 +397,14 @@
     BUSDEV_REGISTER_I2C(busdev_ms4525,      DEVHW_MS4525,       MS4525_I2C_BUS,     0x28,               NONE,           DEVFLAGS_USE_RAW_REGISTERS,  0);    // Requires 0xFF to passthrough
 #endif
 
+#if defined(PITOT_I2C_BUS) && !defined(MS5525_I2C_BUS)
+    #define MS5525_I2C_BUS PITOT_I2C_BUS
+#endif
+
+#if defined(USE_PITOT_MS5525) && defined(MS5525_I2C_BUS)
+    BUSDEV_REGISTER_I2C(busdev_ms5525,      DEVHW_MS5525,       MS5525_I2C_BUS,     0x76,               NONE,           DEVFLAGS_NONE,               0);
+#endif
+
 
 #if defined(PITOT_I2C_BUS) && !defined(DLVR_I2C_BUS)
     #define DLVR_I2C_BUS PITOT_I2C_BUS

From 3b2981c0352b3a52d6319e25bf60cb01718d967c Mon Sep 17 00:00:00 2001
From: LiuYongNan <844550332@qq.com>
Date: Wed, 1 Apr 2026 15:45:32 +0800
Subject: [PATCH 054/102] Fix HUMMINGBIRD FC305 baro

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

diff --git a/src/main/target/HUMMINGBIRD_FC305/config.c b/src/main/target/HUMMINGBIRD_FC305/config.c
index f024a26d54c..9932306ba02 100644
--- a/src/main/target/HUMMINGBIRD_FC305/config.c
+++ b/src/main/target/HUMMINGBIRD_FC305/config.c
@@ -27,7 +27,7 @@
 
 void targetConfiguration(void)
 {
-    barometerConfigMutable()->baro_hardware = BARO_SPL06;
+    barometerConfigMutable()->baro_hardware = BARO_AUTODETECT;
     serialConfigMutable()->portConfigs[3].functionMask = FUNCTION_ESCSERIAL;
     pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1;
 }
diff --git a/src/main/target/HUMMINGBIRD_FC305/target.c b/src/main/target/HUMMINGBIRD_FC305/target.c
index 005d5a16990..298eff767e2 100644
--- a/src/main/target/HUMMINGBIRD_FC305/target.c
+++ b/src/main/target/HUMMINGBIRD_FC305/target.c
@@ -27,7 +27,8 @@
 BUSDEV_REGISTER_SPI_TAG(busdev_icm42688, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_ICM42605_ALIGN);
 BUSDEV_REGISTER_SPI(busdev_sdcard_spi,  DEVHW_SDCARD,       SDCARD_SPI_BUS,     SDCARD_CS_PIN,      NONE,           DEVFLAGS_SPI_MODE_0,  0);
 BUSDEV_REGISTER_SPI(busdev_max7456,    DEVHW_MAX7456,  MAX7456_SPI_BUS,    MAX7456_CS_PIN, NONE, DEVFLAGS_USE_RAW_REGISTERS,  0);
-BUSDEV_REGISTER_I2C(busdev_spl06,      DEVHW_SPL06,        SPL06_I2C_BUS,      SPL06_I2C_ADDR,     NONE,           DEVFLAGS_NONE,      0);
+BUSDEV_REGISTER_I2C(busdev_spl06,      DEVHW_SPL06,        BARO_I2C_BUS,       SPL06_I2C_ADDR,     NONE,           DEVFLAGS_NONE,      0);
+BUSDEV_REGISTER_I2C(busdev_dps310,     DEVHW_DPS310,       BARO_I2C_BUS,       0x76,               NONE,           DEVFLAGS_NONE,      0);
 
 BUSDEV_REGISTER_I2C(busdev_qmc5883,     DEVHW_QMC5883,      MAG_I2C_BUS,    0x0D,               NONE,           DEVFLAGS_NONE,  0);
 BUSDEV_REGISTER_I2C(busdev_hmc5883,     DEVHW_HMC5883,      MAG_I2C_BUS,    0x1E,               NONE,           DEVFLAGS_NONE,  0);
diff --git a/src/main/target/HUMMINGBIRD_FC305/target.h b/src/main/target/HUMMINGBIRD_FC305/target.h
index 6fd5fa705f0..73ab820d5cb 100644
--- a/src/main/target/HUMMINGBIRD_FC305/target.h
+++ b/src/main/target/HUMMINGBIRD_FC305/target.h
@@ -54,9 +54,10 @@
 #define I2C1_SDA                PB9
 
 #define USE_BARO
+#define BARO_I2C_BUS            BUS_I2C1
 #define USE_BARO_SPL06
+#define USE_BARO_DPS310
 #define SPL06_I2C_ADDR          (0x76)
-#define SPL06_I2C_BUS           BUS_I2C1
 
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C1

From 20745ec7397f93d70d0d5601653f7c1122d7f7fa Mon Sep 17 00:00:00 2001
From: b14ckyy <he_black_dragon_88@live.de>
Date: Sun, 5 Apr 2026 21:45:49 +0200
Subject: [PATCH 055/102] Fix MSP-over-telemetry multi-frame request assembly

Fix initSharedMsp() to initialize the response buffer as empty instead
of setting it to the full buffer size. This was causing the response-
pending check in handleMspFrame() to incorrectly detect a pending
response after every buffer reinitialization.

Root cause: initSharedMsp() set responsePacket->buf.end to
responseBuffer + sizeof(mspTxBuffer), putting the buffer in writer mode
with sbufBytesRemaining() returning the full buffer size (e.g. 512).
The response-pending check in handleMspFrame() interpreted this as
unsent response data and reset mspStarted to 0. This triggered another
call to initSharedMsp(), which reset the buffer to full size again,
creating a permanent cycle that dropped every continuation frame.

The response-pending check itself is correct and necessary: SmartPort
is half-duplex, so new request assembly must be blocked while a
response is still being sent. The bug was that initSharedMsp()
corrupted the buffer state that the check relies on.

The fix changes initSharedMsp() to set buf.end = buf.ptr (empty
reader), so sbufBytesRemaining() returns 0 when no response is
actually pending. processMspPacket() already sets up the response
buffer in writer mode before processing, so this change is safe.

Impact: all MSPv1/MSPv2 requests with payload requiring more than one
telemetry frame were silently dropped over SmartPort, CRSF, and FPort
MSP passthrough. Only zero-payload single-frame requests succeeded.

Tested on MATEKF765 with SmartPort MSP passthrough.
---
 src/main/telemetry/msp_shared.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/telemetry/msp_shared.c b/src/main/telemetry/msp_shared.c
index 1589a513596..f3c32eaf7ef 100644
--- a/src/main/telemetry/msp_shared.c
+++ b/src/main/telemetry/msp_shared.c
@@ -73,7 +73,7 @@ void initSharedMsp(void)
     mspPackage.responseBuffer = (uint8_t *)&mspTxBuffer;
     mspPackage.responsePacket = &mspTxPacket;
     mspPackage.responsePacket->buf.ptr = mspPackage.responseBuffer;
-    mspPackage.responsePacket->buf.end = mspPackage.responseBuffer + sizeof(mspTxBuffer);
+    mspPackage.responsePacket->buf.end = mspPackage.responseBuffer;
 }
 
 static void processMspPacket(void)

From e93adf376240a6bcd04f699a4d909a0e88bd596d Mon Sep 17 00:00:00 2001
From: b14ckyy <he_black_dragon_88@live.de>
Date: Sun, 5 Apr 2026 22:52:16 +0200
Subject: [PATCH 056/102] Add MSP2_INAV_SET_AUX_RC (0x2230)

---
 docs/development/msp/msp_messages.json |  26 ++++++
 src/main/fc/fc_msp.c                   | 105 +++++++++++++++++++++++++
 src/main/msp/msp_protocol_v2_inav.h    |   2 +
 src/main/rx/rx.c                       |  17 ++++
 src/main/rx/rx.h                       |   4 +
 5 files changed, 154 insertions(+)

diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json
index 12d67369bd7..866eb87b3b8 100644
--- a/docs/development/msp/msp_messages.json
+++ b/docs/development/msp/msp_messages.json
@@ -10943,6 +10943,32 @@
         "notes": "All attitude angles are in deci-degrees.",
         "description": "Provides estimates of current attitude, local NEU position, and velocity."
     },
+    "MSP2_INAV_SET_AUX_RC": {
+        "code": 8752,
+        "mspv": 2,
+        "request": {
+            "payload": [
+                {
+                    "name": "definitionByte",
+                    "ctype": "uint8_t",
+                    "desc": "Packed start channel and resolution. Bits 7-3: start channel index (valid range 8-31 for CH9-CH32; 0-7 rejected as error). Bits 2-0: resolution mode (0=2-bit, 1=4-bit, 2=8-bit, 3=16-bit; 4-7 reserved/error).",
+                    "units": ""
+                },
+                {
+                    "name": "channelData",
+                    "ctype": "uint8_t",
+                    "desc": "Packed channel values, sequential from start channel. Number of channels is derived from data size and resolution. Value 0 means skip (no update). Sub-byte modes (2-bit, 4-bit) are packed MSB-first. 2-bit values 1-3 map to 1000/1500/2000us. 4-bit values 1-15 map to 1000 + (val-1)*1000/14 us. 8-bit values 1-255 map to 1000 + (val-1)*1000/254 us. 16-bit values are direct PWM.",
+                    "units": "PWM (encoded)",
+                    "array": true,
+                    "array_size": 0
+                }
+            ]
+        },
+        "reply": null,
+        "variable_len": true,
+        "notes": "CH1-CH8 (index 0-7) are protected and will return `MSP_RESULT_ERROR`. Constraint: `startChannel + channelCount <= 32`. Values persist until overwritten; no timeout. Applied as a post-RX overlay in `calculateRxChannelsAndUpdateFailsafe()` after MSP RC Override but before failsafe. Does not require `USE_RX_MSP` or MSP-RC-OVERRIDE flight mode. Does not affect failsafe detection. Recommended to send with `MSP_FLAG_DONT_REPLY` (flags=0x01) to save bandwidth on telemetry passthrough links. 16-bit mode requires even number of data bytes.",
+        "description": "Bandwidth-efficient auxiliary RC channel update. Sets CH9-CH32 with configurable resolution (2/4/8/16-bit) without affecting primary flight controls. Designed for extending channel count beyond native RC link capacity via MSP passthrough."
+    },
     "MSP2_BETAFLIGHT_BIND": {
         "code": 12288,
         "mspv": 2,
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index cf5308067e9..d0e673f5fcc 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -2412,6 +2412,110 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
         }
         break;
 #endif
+
+    case MSP2_INAV_SET_AUX_RC:
+        {
+            if (dataSize < 2) {
+                return MSP_RESULT_ERROR;
+            }
+
+            const uint8_t defByte = sbufReadU8(src);
+            const uint8_t startChannel = defByte >> 3;          // Bits 7-3: start channel index (0-31)
+            const uint8_t resolutionMode = defByte & 0x07;      // Bits 2-0: resolution
+
+            // Safety: CH1-CH8 (index 0-7) are protected
+            if (startChannel < 8) {
+                return MSP_RESULT_ERROR;
+            }
+
+            const uint8_t dataBytes = dataSize - 1;
+            uint8_t channelCount;
+            uint8_t bitsPerChannel;
+
+            switch (resolutionMode) {
+                case 0: // 2-bit
+                    bitsPerChannel = 2;
+                    channelCount = dataBytes * 4;
+                    break;
+                case 1: // 4-bit
+                    bitsPerChannel = 4;
+                    channelCount = dataBytes * 2;
+                    break;
+                case 2: // 8-bit
+                    bitsPerChannel = 8;
+                    channelCount = dataBytes;
+                    break;
+                case 3: // 16-bit
+                    bitsPerChannel = 16;
+                    if (dataBytes % 2 != 0) {
+                        return MSP_RESULT_ERROR;
+                    }
+                    channelCount = dataBytes / 2;
+                    break;
+                default:
+                    return MSP_RESULT_ERROR;
+            }
+
+            if (channelCount == 0 || startChannel + channelCount > 32) {
+                return MSP_RESULT_ERROR;
+            }
+
+            // Decode and apply channel values
+            if (bitsPerChannel >= 8) {
+                // Byte-aligned modes: 8-bit and 16-bit
+                for (int i = 0; i < channelCount; i++) {
+                    uint16_t rawValue;
+                    if (bitsPerChannel == 16) {
+                        rawValue = sbufReadU16(src);
+                    } else {
+                        rawValue = sbufReadU8(src);
+                    }
+
+                    if (rawValue == 0) {
+                        continue; // skip: no update
+                    }
+
+                    uint16_t pwmValue;
+                    if (bitsPerChannel == 16) {
+                        pwmValue = constrain(rawValue, 750, 2250);
+                    } else {
+                        // 8-bit: 1-255 → 1000-2000
+                        pwmValue = 1000 + ((uint32_t)(rawValue - 1) * 1000) / 254;
+                    }
+
+                    rxMspAuxOverlaySet(startChannel + i, pwmValue);
+                }
+            } else {
+                // Sub-byte modes: 2-bit and 4-bit
+                const uint8_t mask = (1 << bitsPerChannel) - 1;
+                const uint8_t channelsPerByte = 8 / bitsPerChannel;
+                int ch = 0;
+
+                for (int byteIdx = 0; byteIdx < (int)dataBytes && ch < channelCount; byteIdx++) {
+                    const uint8_t dataByte = sbufReadU8(src);
+                    for (int sub = channelsPerByte - 1; sub >= 0 && ch < channelCount; sub--, ch++) {
+                        const uint8_t rawValue = (dataByte >> (sub * bitsPerChannel)) & mask;
+
+                        if (rawValue == 0) {
+                            continue; // skip: no update
+                        }
+
+                        uint16_t pwmValue;
+                        if (bitsPerChannel == 2) {
+                            // 2-bit: 1→1000, 2→1500, 3→2000
+                            pwmValue = 1000 + (rawValue - 1) * 500;
+                        } else {
+                            // 4-bit: 1-15 → 1000-2000
+                            pwmValue = 1000 + ((uint32_t)(rawValue - 1) * 1000) / 14;
+                        }
+
+                        rxMspAuxOverlaySet(startChannel + ch, pwmValue);
+                    }
+                }
+            }
+        }
+        break;
+
     case MSP2_COMMON_SET_MOTOR_MIXER:
         sbufReadU8Safe(&tmp_u8, src);
         if ((dataSize == 9) && (tmp_u8 < MAX_SUPPORTED_MOTORS)) {
@@ -4486,6 +4590,7 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro
     sbuf_t *dst = &reply->buf;
     sbuf_t *src = &cmd->buf;
     const uint16_t cmdMSP = cmd->cmd;
+
     // initialize reply by default
     reply->cmd = cmd->cmd;
 
diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h
index 0b893916895..aacccc7806b 100755
--- a/src/main/msp/msp_protocol_v2_inav.h
+++ b/src/main/msp/msp_protocol_v2_inav.h
@@ -126,3 +126,5 @@
 #define MSP2_INAV_SET_GVAR                      0x2214
 
 #define MSP2_INAV_FULL_LOCAL_POSE               0x2220
+
+#define MSP2_INAV_SET_AUX_RC                    0x2230
diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c
index 0531904d063..7df78cc5e50 100755
--- a/src/main/rx/rx.c
+++ b/src/main/rx/rx.c
@@ -94,6 +94,9 @@ static bool isRxSuspended = false;
 
 static rcChannel_t rcChannels[MAX_SUPPORTED_RC_CHANNEL_COUNT];
 
+// MSP aux channel overlay: non-zero values override rcChannels[].data for CH9-CH32
+static uint16_t mspAuxOverlay[MAX_SUPPORTED_RC_CHANNEL_COUNT];
+
 rxLinkStatistics_t rxLinkStatistics;
 rxRuntimeConfig_t rxRuntimeConfig;
 static uint8_t rcSampleIndex = 0;
@@ -512,6 +515,13 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
     }
 #endif
 
+    // Apply MSP aux channel overlay (CH9-CH32)
+    for (int i = 8; i < 32; i++) {
+        if (mspAuxOverlay[i] > 0) {
+            rcChannels[i].data = mspAuxOverlay[i];
+        }
+    }
+
     // Update failsafe
     if (rxFlightChannelsValid && rxSignalReceived) {
         failsafeOnValidDataReceived();
@@ -663,6 +673,13 @@ int16_t rxGetChannelValue(unsigned channelNumber)
     }
 }
 
+void rxMspAuxOverlaySet(uint8_t channelIndex, uint16_t value)
+{
+    if (channelIndex >= 8 && channelIndex < 32) {
+        mspAuxOverlay[channelIndex] = value;
+    }
+}
+
 void lqTrackerReset(rxLinkQualityTracker_e * lqTracker)
 {
     lqTracker->lastUpdatedMs = millis();
diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h
index 3ed6add3e48..6eb63c83325 100644
--- a/src/main/rx/rx.h
+++ b/src/main/rx/rx.h
@@ -232,3 +232,7 @@ void resumeRxSignal(void);
 // filtering and some extra processing like value holding
 // during failsafe.
 int16_t rxGetChannelValue(unsigned channelNumber);
+
+// MSP aux channel overlay (CH9-CH32). Sets a channel value that persists
+// across RX update cycles. value=0 clears the overlay for that channel.
+void rxMspAuxOverlaySet(uint8_t channelIndex, uint16_t value);

From ed06c84def2279c64cdcdff395526478fa9fd66b Mon Sep 17 00:00:00 2001
From: Sensei <deepmagicbeginshere@gmail.com>
Date: Wed, 8 Apr 2026 22:29:53 -0500
Subject: [PATCH 057/102] Fix bugs in ICM-45686 IMU driver (#11455)

- Fix settings.yaml: quote "ICM45686" string in acc_hardware table
- Fix accel LPF fallback: write BYPASS not ODR_DIV_8 on IREG failure
- Fix IREG polling: use delayMicroseconds(10) not delay(1) to match waited_us counter
- Fix reset polling: replace while(1) with do/while pattern used by all other drivers
- Fix temperature formula: use float literals (12.8f) to avoid double promotion on F4
- Add named constant ICM456XX_INT_CONFIG_DELAY_MS for post-interrupt settle delay
- Remove unused endianness defines (ICM456XX_RA_SREG_CTRL, SREG_DATA_ENDIAN_SEL_*)
- Add missing trailing newlines to all modified files
---
 src/main/drivers/accgyro/accgyro_icm45686.c | 32 ++++++++-------------
 src/main/drivers/accgyro/accgyro_icm45686.h |  2 +-
 src/main/drivers/accgyro/accgyro_mpu.h      |  2 +-
 src/main/drivers/bus.h                      |  2 +-
 src/main/fc/settings.yaml                   |  2 +-
 src/main/sensors/acceleration.c             |  2 +-
 src/main/sensors/acceleration.h             |  2 +-
 src/main/sensors/gyro.c                     |  2 +-
 src/main/sensors/gyro.h                     |  2 +-
 src/main/target/common_hardware.c           |  2 +-
 10 files changed, 21 insertions(+), 29 deletions(-)

diff --git a/src/main/drivers/accgyro/accgyro_icm45686.c b/src/main/drivers/accgyro/accgyro_icm45686.c
index a9e0ee0b8a5..9b249fb607f 100644
--- a/src/main/drivers/accgyro/accgyro_icm45686.c
+++ b/src/main/drivers/accgyro/accgyro_icm45686.c
@@ -104,16 +104,6 @@ Note: Now implemented only UI Interface with Low-Noise Mode
 #define ICM456XX_ACCEL_CONFIG0                  0x1B
 #define ICM456XX_PWR_MGMT0                      0x10
 #define ICM456XX_TEMP_DATA1                     0x0C
-// Register map IPREG_TOP1 (for future use)
-// Note: SREG_CTRL register provides endian selection capability. 
-// Currently not utilized as UI interface reads are in device's native endianness.
-// Kept as reference for potential future optimization.
-#define ICM456XX_RA_SREG_CTRL                   0xA267 // To access: 0xA200 + 0x67
-
-// SREG_CTRL - 0x67 (bits 1:0 select data endianness)
-#define ICM456XX_SREG_DATA_ENDIAN_SEL_LITTLE    (0 << 1) // Little endian (native)
-#define ICM456XX_SREG_DATA_ENDIAN_SEL_BIG       (1 << 1) // Big endian (requires IREG write)
-
 // MGMT0 - 0x10 - Gyro
 #define ICM456XX_GYRO_MODE_OFF                  (0x00 << 2)
 #define ICM456XX_GYRO_MODE_STANDBY              (0x01 << 2)
@@ -264,6 +254,7 @@ Note: Now implemented only UI Interface with Low-Noise Mode
 #define ICM456XX_ACCEL_STARTUP_TIME_MS          10  // Min accel startup from OFF/STANDBY/LP
 #define ICM456XX_GYRO_STARTUP_TIME_MS           35  // Min gyro startup from OFF/STANDBY/LP
 #define ICM456XX_SENSOR_ENABLE_DELAY_MS         1   // Allow sensors to power on and stabilize
+#define ICM456XX_INT_CONFIG_DELAY_MS            15  // Register settle time after interrupt config (matches ICM-426xx convention)
 #define ICM456XX_IREG_TIMEOUT_US                5000 // IREG operation timeout (5ms max)
 
 #define ICM456XX_DATA_LENGTH                    6  // 3 axes * 2 bytes per axis
@@ -305,7 +296,7 @@ static bool icm45686WriteIREG(const busDevice_t *dev, uint16_t reg, uint8_t valu
         if (misc2 & ICM456XX_BIT_IREG_DONE) {
             return true;
         }
-        delay(1);
+        delayMicroseconds(10);
     }
 
     return false; // timeout
@@ -357,7 +348,7 @@ static bool icm45686ReadTemperature(gyroDev_t *gyro, int16_t * temp)
         return false;
     }
     // From datasheet: Temperature in Degrees Centigrade = (TEMP_DATA / 128) + 25 
-    *temp = ( int16_val_little_endian(data, 0) / 12.8 ) + 250; // Temperature stored as degC*10
+    *temp = ( int16_val_little_endian(data, 0) / 12.8f ) + 250.0f; // Temperature stored as degC*10
 
     return true;
 }
@@ -398,7 +389,7 @@ static void icm45686AccAndGyroInit(gyroDev_t *gyro)
     // Set the Accel UI LPF bandwidth cut-off to ODR/8 (Section 7.3 of datasheet)
     if (!icm45686WriteIREG(dev, ICM456XX_ACCEL_UI_LPF_CFG_IREG_ADDR, ICM456XX_ACCEL_UI_LPFBW_ODR_DIV_8)) {
         // If LPF configuration fails, fallback to BYPASS
-        icm45686WriteIREG(dev, ICM456XX_ACCEL_UI_LPF_CFG_IREG_ADDR, ICM456XX_ACCEL_UI_LPFBW_ODR_DIV_8);
+        icm45686WriteIREG(dev, ICM456XX_ACCEL_UI_LPF_CFG_IREG_ADDR, ICM456XX_ACCEL_UI_LPFBW_BYPASS);
     }
     // Setup scale and odr values for gyro
     busWrite(dev, ICM456XX_GYRO_CONFIG0, ICM456XX_GYRO_FS_SEL_2000DPS | config->gyroConfigValues[1]);
@@ -409,7 +400,7 @@ static void icm45686AccAndGyroInit(gyroDev_t *gyro)
                                             ICM456XX_INT1_POLARITY_ACTIVE_HIGH);
     busWrite(dev, ICM456XX_INT1_CONFIG0, ICM456XX_INT1_STATUS_EN_DRDY);
 
-    delay(15);
+    delay(ICM456XX_INT_CONFIG_DELAY_MS);
     busSetSpeed(dev, BUS_SPEED_FAST);
 }
 
@@ -424,17 +415,18 @@ static bool icm45686DeviceDetect(busDevice_t * dev)
     // Perform soft reset directly
     // Soft reset
     busWrite(dev, ICM456XX_REG_MISC2, ICM456XX_SOFT_RESET);
-    // Wait for reset to complete (bit 1 of REG_MISC2 becomes 0)
-    while (1) {
+    // Poll until soft reset completes (SOFT_RESET bit clears) per datasheet Section 9.4
+    do {
         busRead(dev, ICM456XX_REG_MISC2, &tmp);
         if (!(tmp & ICM456XX_SOFT_RESET)) {
             break;
         }
         delay(1);
         waitedMs++;
-        if (waitedMs >= 20) {
-            return false;
-        }
+    } while (waitedMs < 20);
+
+    if (tmp & ICM456XX_SOFT_RESET) {
+        return false;
     }
     // Initialize power management to a known state after reset
     // This ensures sensors are off and ready for proper initialization
@@ -497,4 +489,4 @@ bool icm45686GyroDetect(gyroDev_t *gyro)
 }
 
 
-#endif
\ No newline at end of file
+#endif
diff --git a/src/main/drivers/accgyro/accgyro_icm45686.h b/src/main/drivers/accgyro/accgyro_icm45686.h
index 83c630e13bc..aac195aa220 100644
--- a/src/main/drivers/accgyro/accgyro_icm45686.h
+++ b/src/main/drivers/accgyro/accgyro_icm45686.h
@@ -19,4 +19,4 @@
 
 
 bool icm45686AccDetect(accDev_t *acc);
-bool icm45686GyroDetect(gyroDev_t *gyro);
\ No newline at end of file
+bool icm45686GyroDetect(gyroDev_t *gyro);
diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h
index 16089f5f542..0f59d10d3e8 100644
--- a/src/main/drivers/accgyro/accgyro_mpu.h
+++ b/src/main/drivers/accgyro/accgyro_mpu.h
@@ -182,4 +182,4 @@ const gyroFilterAndRateConfig_t * mpuChooseGyroConfig(uint8_t desiredLpf, uint16
 bool mpuGyroRead(struct gyroDev_s *gyro);
 bool mpuGyroReadScratchpad(struct gyroDev_s *gyro);
 bool mpuAccReadScratchpad(struct accDev_s *acc);
-bool mpuTemperatureReadScratchpad(struct gyroDev_s *gyro, int16_t * data);
\ No newline at end of file
+bool mpuTemperatureReadScratchpad(struct gyroDev_s *gyro, int16_t * data);
diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h
index 3c26067a557..353a06b306e 100644
--- a/src/main/drivers/bus.h
+++ b/src/main/drivers/bus.h
@@ -331,4 +331,4 @@ bool busWrite(const busDevice_t * busdev, uint8_t reg, uint8_t data);
 bool busTransfer(const busDevice_t * dev, uint8_t * rxBuf, const uint8_t * txBuf, int length);
 bool busTransferMultiple(const busDevice_t * dev, busTransferDescriptor_t * buffers, int count);
 
-bool busIsBusy(const busDevice_t * dev);
\ No newline at end of file
+bool busIsBusy(const busDevice_t * dev);
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 58274b1b660..91ef0ff1b3e 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -2,7 +2,7 @@ tables:
   - name: alignment
     values: ["DEFAULT", "CW0", "CW90", "CW180", "CW270", "CW0FLIP", "CW90FLIP", "CW180FLIP", "CW270FLIP"]
   - name: acc_hardware
-    values: ["NONE", "AUTO", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "BMI270","LSM6DXX", ICM45686, "FAKE"]
+    values: ["NONE", "AUTO", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "BMI270","LSM6DXX", "ICM45686", "FAKE"]
     enum: accelerationSensor_e
   - name: rangefinder_hardware
     values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C", "FAKE", "TERARANGER_EVO", "USD1_V0", "NRA"]
diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c
index d0137c28a59..cc52be637a9 100644
--- a/src/main/sensors/acceleration.c
+++ b/src/main/sensors/acceleration.c
@@ -709,4 +709,4 @@ void accInitFilters(void)
 bool accIsHealthy(void)
 {
     return true;
-}
\ No newline at end of file
+}
diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h
index 1037f85ca37..ea76b63ca75 100644
--- a/src/main/sensors/acceleration.h
+++ b/src/main/sensors/acceleration.h
@@ -98,4 +98,4 @@ void accSetCalibrationValues(void);
 void accInitFilters(void);
 bool accIsHealthy(void);
 bool accGetCalibrationAxisStatus(int axis);
-uint8_t accGetCalibrationAxisFlags(void);
\ No newline at end of file
+uint8_t accGetCalibrationAxisFlags(void);
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 5abc4980f97..8544b45dac1 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -619,4 +619,4 @@ void gyroUpdateDynamicLpf(float cutoffFreq) {
 float averageAbsGyroRates(void)
 {
     return (fabsf(gyro.gyroADCf[ROLL]) + fabsf(gyro.gyroADCf[PITCH]) + fabsf(gyro.gyroADCf[YAW])) / 3.0f;
-}
\ No newline at end of file
+}
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index d04eb2f705d..18fe5d9e517 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -120,4 +120,4 @@ bool gyroReadTemperature(void);
 int16_t gyroGetTemperature(void);
 int16_t gyroRateDps(int axis);
 void gyroUpdateDynamicLpf(float cutoffFreq);
-float averageAbsGyroRates(void);
\ No newline at end of file
+float averageAbsGyroRates(void);
diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c
index 020e3f7b35c..ddaae1b7b16 100755
--- a/src/main/target/common_hardware.c
+++ b/src/main/target/common_hardware.c
@@ -477,4 +477,4 @@
     BUSDEV_REGISTER_I2C(busdev_pcf8574,      DEVHW_PCF8574,       PCF8574_I2C_BUS,     0x20,               NONE,           DEVFLAGS_NONE, 0);
 #endif
 
-#endif  // USE_TARGET_HARDWARE_DESCRIPTORS
\ No newline at end of file
+#endif  // USE_TARGET_HARDWARE_DESCRIPTORS

From 9ce07526928a57ff2037e12825d58d2d42df64d9 Mon Sep 17 00:00:00 2001
From: Marc Hoffmann <33039058+b14ckyy@users.noreply.github.com>
Date: Thu, 9 Apr 2026 05:58:35 +0200
Subject: [PATCH 058/102] Fix MSP_RESULT_NO_REPLY ignored in MSP-over-telemetry
 passthrough (#11481)

processMspPacket() in msp_shared.c did not check for MSP_RESULT_NO_REPLY
returned by mspFcProcessCommand(). When a command was sent with the
MSP_FLAG_DONT_REPLY flag (bit 0 of the MSPv2 flags byte), the FC
correctly set the result to MSP_RESULT_NO_REPLY, but processMspPacket()
unconditionally called sbufSwitchToReader() on the response buffer,
causing a reply to be sent anyway.

The serial MSP path (msp_serial.c) already handles this correctly by
checking 'if (status != MSP_RESULT_NO_REPLY)' before sending.

Fix: store the return value of mspFcProcessCommand() and skip
sbufSwitchToReader() when the result is MSP_RESULT_NO_REPLY, leaving
the response buffer empty so no reply is transmitted.

Affects: SmartPort, CRSF, and FPort MSP passthrough.

Co-authored-by: b14ckyy <he_black_dragon_88@live.de>
---
 src/main/telemetry/msp_shared.c | 13 +++++++++----
 1 file changed, 9 insertions(+), 4 deletions(-)

diff --git a/src/main/telemetry/msp_shared.c b/src/main/telemetry/msp_shared.c
index f3c32eaf7ef..f6d4c6eafbd 100644
--- a/src/main/telemetry/msp_shared.c
+++ b/src/main/telemetry/msp_shared.c
@@ -76,7 +76,7 @@ void initSharedMsp(void)
     mspPackage.responsePacket->buf.end = mspPackage.responseBuffer;
 }
 
-static void processMspPacket(void)
+static bool processMspPacket(void)
 {
     mspPackage.responsePacket->cmd = 0;
     mspPackage.responsePacket->result = 0;
@@ -84,14 +84,20 @@ static void processMspPacket(void)
     mspPackage.responsePacket->buf.end = mspPackage.responseBuffer + sizeof(mspTxBuffer);
 
     mspPostProcessFnPtr mspPostProcessFn = NULL;
-    if (mspFcProcessCommand(mspPackage.requestPacket, mspPackage.responsePacket, &mspPostProcessFn) == MSP_RESULT_ERROR) {
+    const mspResult_e status = mspFcProcessCommand(mspPackage.requestPacket, mspPackage.responsePacket, &mspPostProcessFn);
+    if (status == MSP_RESULT_ERROR) {
         sbufWriteU8(&mspPackage.responsePacket->buf, TELEMETRY_MSP_ERROR);
     }
     if (mspPostProcessFn) {
         mspPostProcessFn(NULL);
     }
 
+    if (status == MSP_RESULT_NO_REPLY) {
+        return false;
+    }
+
     sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer);
+    return true;
 }
 
 void sendMspErrorResponse(uint8_t error, int16_t cmd)
@@ -194,8 +200,7 @@ bool handleMspFrame(uint8_t *const frameStart, const int payloadLength)
     sbufAdvance(&mspPackage.requestFrame, payloadExpecting);
     sbufWriteData(&requestPacket->buf, payload, payloadExpecting);
     sbufSwitchToReader(&requestPacket->buf, mspPackage.requestBuffer);
-    processMspPacket();
-    return true;
+    return processMspPacket();
 }
 
 bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn)

From 6b0aaa8f63ea2cfc3be2711485fac2584d1440f5 Mon Sep 17 00:00:00 2001
From: b14ckyy <he_black_dragon_88@live.de>
Date: Thu, 9 Apr 2026 09:42:59 +0200
Subject: [PATCH 059/102] Guard overlay against active RX and MSP_RC_OVERRIDE
 channels

- Clamp 16-bit PWM values to 750-2250 (prevent int16_t overflow)
- Reject oversized payloads (dataSize > 49)
- Skip channels actively provided by primary RX (SBUS/CRSF/etc.)
- When MSP is the RX provider, use actual MSP_SET_RAW_RC channel count
- Skip channels controlled by MSP_RC_OVERRIDE bitmask when active
---
 src/main/fc/fc_msp.c |  3 ++-
 src/main/rx/msp.c    |  7 +++++++
 src/main/rx/msp.h    |  1 +
 src/main/rx/rx.c     | 29 +++++++++++++++++++++++++----
 src/main/rx/rx.h     |  2 +-
 5 files changed, 36 insertions(+), 6 deletions(-)

diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 4fff9e169c9..9606302abd3 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -2415,7 +2415,8 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
 
     case MSP2_INAV_SET_AUX_RC:
         {
-            if (dataSize < 2) {
+            // Max valid payload: 1 def byte + 24 channels × 2 bytes (16-bit) = 49 bytes
+            if (dataSize < 2 || dataSize > 49) {
                 return MSP_RESULT_ERROR;
             }
 
diff --git a/src/main/rx/msp.c b/src/main/rx/msp.c
index b338fbc2d97..e2d310423a6 100644
--- a/src/main/rx/msp.c
+++ b/src/main/rx/msp.c
@@ -31,6 +31,7 @@
 
 static uint16_t mspFrame[MAX_SUPPORTED_RC_CHANNEL_COUNT];
 static bool rxMspFrameDone = false;
+static uint8_t mspLastChannelCount = 0;
 
 static uint16_t rxMspReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t chan)
 {
@@ -49,9 +50,15 @@ void rxMspFrameReceive(uint16_t *frame, int channelCount)
         mspFrame[i] = 0;
     }
 
+    mspLastChannelCount = channelCount;
     rxMspFrameDone = true;
 }
 
+uint8_t rxMspGetLastChannelCount(void)
+{
+    return mspLastChannelCount;
+}
+
 static uint8_t rxMspFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 {
     UNUSED(rxRuntimeConfig);
diff --git a/src/main/rx/msp.h b/src/main/rx/msp.h
index 78a9bfd7132..c99fe44d641 100644
--- a/src/main/rx/msp.h
+++ b/src/main/rx/msp.h
@@ -21,3 +21,4 @@
 
 void rxMspFrameReceive(uint16_t *frame, int channelCount);
 void rxMspInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
+uint8_t rxMspGetLastChannelCount(void);
diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c
index 7df78cc5e50..d1b8b7833f0 100755
--- a/src/main/rx/rx.c
+++ b/src/main/rx/rx.c
@@ -515,10 +515,31 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
     }
 #endif
 
-    // Apply MSP aux channel overlay (CH9-CH32)
-    for (int i = 8; i < 32; i++) {
-        if (mspAuxOverlay[i] > 0) {
-            rcChannels[i].data = mspAuxOverlay[i];
+    // Apply MSP aux channel overlay — only to channels NOT actively
+    // provided by the primary RX (avoids jitter from competing sources)
+    {
+        // Determine the effective RX channel count
+        uint8_t activeRxChannels = rxChannelCount;
+#ifdef USE_RX_MSP
+        // When MSP is the primary RX, rxChannelCount is always 32.
+        // Use the actual channel count from the last MSP_SET_RAW_RC message.
+        if (rxConfig()->receiverType == RX_TYPE_MSP) {
+            const uint8_t mspChannels = rxMspGetLastChannelCount();
+            if (mspChannels > 0) {
+                activeRxChannels = mspChannels;
+            }
+        }
+#endif
+        for (int i = MAX(8, activeRxChannels); i < 32; i++) {
+            if (mspAuxOverlay[i] > 0) {
+#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
+                // Skip channels controlled by MSP RC Override when active
+                if (IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) && (rxConfig()->mspOverrideChannels & (1U << i))) {
+                    continue;
+                }
+#endif
+                rcChannels[i].data = mspAuxOverlay[i];
+            }
         }
     }
 
diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h
index 6eb63c83325..1c2989fa3dc 100644
--- a/src/main/rx/rx.h
+++ b/src/main/rx/rx.h
@@ -234,5 +234,5 @@ void resumeRxSignal(void);
 int16_t rxGetChannelValue(unsigned channelNumber);
 
 // MSP aux channel overlay (CH9-CH32). Sets a channel value that persists
-// across RX update cycles. value=0 clears the overlay for that channel.
+// across RX update cycles. value=0 ignores that channel and skips it.
 void rxMspAuxOverlaySet(uint8_t channelIndex, uint16_t value);

From 3bf93e8f093e0d1a7a80b0f9d00ab16e2952c4ea Mon Sep 17 00:00:00 2001
From: b14ckyy <he_black_dragon_88@live.de>
Date: Thu, 9 Apr 2026 17:45:32 +0200
Subject: [PATCH 060/102] Restrict overlay to CH13-CH32, remove rxChannelCount
 guard
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

- Raise minimum channel from CH9 to CH13 (index 12)
  All common protocols deliver at least 12 channels on the wire.
- Remove rxChannelCount-based guard (protocol drivers report max
  capacity, not actual sender channel count — unusable as guard)
- Remove rxMspGetLastChannelCount (no longer needed)
- Keep MSP_RC_OVERRIDE bitmask guard
---
 src/main/fc/fc_msp.c |  4 ++--
 src/main/rx/msp.c    |  7 -------
 src/main/rx/msp.h    |  1 -
 src/main/rx/rx.c     | 33 +++++++++------------------------
 src/main/rx/rx.h     |  2 +-
 5 files changed, 12 insertions(+), 35 deletions(-)

diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 9606302abd3..b4ba53ea724 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -2424,8 +2424,8 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             const uint8_t startChannel = defByte >> 3;          // Bits 7-3: start channel index (0-31)
             const uint8_t resolutionMode = defByte & 0x07;      // Bits 2-0: resolution
 
-            // Safety: CH1-CH8 (index 0-7) are protected
-            if (startChannel < 8) {
+            // Safety: CH1-CH12 (index 0-11) are protected
+            if (startChannel < 12) {
                 return MSP_RESULT_ERROR;
             }
 
diff --git a/src/main/rx/msp.c b/src/main/rx/msp.c
index e2d310423a6..b338fbc2d97 100644
--- a/src/main/rx/msp.c
+++ b/src/main/rx/msp.c
@@ -31,7 +31,6 @@
 
 static uint16_t mspFrame[MAX_SUPPORTED_RC_CHANNEL_COUNT];
 static bool rxMspFrameDone = false;
-static uint8_t mspLastChannelCount = 0;
 
 static uint16_t rxMspReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t chan)
 {
@@ -50,15 +49,9 @@ void rxMspFrameReceive(uint16_t *frame, int channelCount)
         mspFrame[i] = 0;
     }
 
-    mspLastChannelCount = channelCount;
     rxMspFrameDone = true;
 }
 
-uint8_t rxMspGetLastChannelCount(void)
-{
-    return mspLastChannelCount;
-}
-
 static uint8_t rxMspFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 {
     UNUSED(rxRuntimeConfig);
diff --git a/src/main/rx/msp.h b/src/main/rx/msp.h
index c99fe44d641..78a9bfd7132 100644
--- a/src/main/rx/msp.h
+++ b/src/main/rx/msp.h
@@ -21,4 +21,3 @@
 
 void rxMspFrameReceive(uint16_t *frame, int channelCount);
 void rxMspInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
-uint8_t rxMspGetLastChannelCount(void);
diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c
index d1b8b7833f0..b85350d8333 100755
--- a/src/main/rx/rx.c
+++ b/src/main/rx/rx.c
@@ -515,31 +515,16 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
     }
 #endif
 
-    // Apply MSP aux channel overlay — only to channels NOT actively
-    // provided by the primary RX (avoids jitter from competing sources)
-    {
-        // Determine the effective RX channel count
-        uint8_t activeRxChannels = rxChannelCount;
-#ifdef USE_RX_MSP
-        // When MSP is the primary RX, rxChannelCount is always 32.
-        // Use the actual channel count from the last MSP_SET_RAW_RC message.
-        if (rxConfig()->receiverType == RX_TYPE_MSP) {
-            const uint8_t mspChannels = rxMspGetLastChannelCount();
-            if (mspChannels > 0) {
-                activeRxChannels = mspChannels;
-            }
-        }
-#endif
-        for (int i = MAX(8, activeRxChannels); i < 32; i++) {
-            if (mspAuxOverlay[i] > 0) {
+    // Apply MSP aux channel overlay (CH13-CH32)
+    for (int i = 12; i < 32; i++) {
+        if (mspAuxOverlay[i] > 0) {
 #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
-                // Skip channels controlled by MSP RC Override when active
-                if (IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) && (rxConfig()->mspOverrideChannels & (1U << i))) {
-                    continue;
-                }
-#endif
-                rcChannels[i].data = mspAuxOverlay[i];
+            // Skip channels controlled by MSP RC Override when active
+            if (IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) && (rxConfig()->mspOverrideChannels & (1U << i))) {
+                continue;
             }
+#endif
+            rcChannels[i].data = mspAuxOverlay[i];
         }
     }
 
@@ -696,7 +681,7 @@ int16_t rxGetChannelValue(unsigned channelNumber)
 
 void rxMspAuxOverlaySet(uint8_t channelIndex, uint16_t value)
 {
-    if (channelIndex >= 8 && channelIndex < 32) {
+    if (channelIndex >= 12 && channelIndex < 32) {
         mspAuxOverlay[channelIndex] = value;
     }
 }
diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h
index 1c2989fa3dc..5b5bcea50c3 100644
--- a/src/main/rx/rx.h
+++ b/src/main/rx/rx.h
@@ -233,6 +233,6 @@ void resumeRxSignal(void);
 // during failsafe.
 int16_t rxGetChannelValue(unsigned channelNumber);
 
-// MSP aux channel overlay (CH9-CH32). Sets a channel value that persists
+// MSP aux channel overlay (CH13-CH32). Sets a channel value that persists
 // across RX update cycles. value=0 ignores that channel and skips it.
 void rxMspAuxOverlaySet(uint8_t channelIndex, uint16_t value);

From 86615b9f40bcbabebce6c5c1dbae7dd89fcfe9f2 Mon Sep 17 00:00:00 2001
From: b14ckyy <he_black_dragon_88@live.de>
Date: Thu, 9 Apr 2026 18:12:57 +0200
Subject: [PATCH 061/102] Re-add MSP RX channel count guard for overlay

When MSP is the primary RX provider (e.g. mLRS), skip channels
already covered by MSP_SET_RAW_RC to avoid wasting bandwidth on
overlapping updates.
---
 src/main/rx/msp.c |  7 +++++++
 src/main/rx/msp.h |  1 +
 src/main/rx/rx.c  | 26 +++++++++++++++++++-------
 3 files changed, 27 insertions(+), 7 deletions(-)

diff --git a/src/main/rx/msp.c b/src/main/rx/msp.c
index b338fbc2d97..e2d310423a6 100644
--- a/src/main/rx/msp.c
+++ b/src/main/rx/msp.c
@@ -31,6 +31,7 @@
 
 static uint16_t mspFrame[MAX_SUPPORTED_RC_CHANNEL_COUNT];
 static bool rxMspFrameDone = false;
+static uint8_t mspLastChannelCount = 0;
 
 static uint16_t rxMspReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t chan)
 {
@@ -49,9 +50,15 @@ void rxMspFrameReceive(uint16_t *frame, int channelCount)
         mspFrame[i] = 0;
     }
 
+    mspLastChannelCount = channelCount;
     rxMspFrameDone = true;
 }
 
+uint8_t rxMspGetLastChannelCount(void)
+{
+    return mspLastChannelCount;
+}
+
 static uint8_t rxMspFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 {
     UNUSED(rxRuntimeConfig);
diff --git a/src/main/rx/msp.h b/src/main/rx/msp.h
index 78a9bfd7132..c99fe44d641 100644
--- a/src/main/rx/msp.h
+++ b/src/main/rx/msp.h
@@ -21,3 +21,4 @@
 
 void rxMspFrameReceive(uint16_t *frame, int channelCount);
 void rxMspInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
+uint8_t rxMspGetLastChannelCount(void);
diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c
index b85350d8333..b897238ed1d 100755
--- a/src/main/rx/rx.c
+++ b/src/main/rx/rx.c
@@ -516,15 +516,27 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
 #endif
 
     // Apply MSP aux channel overlay (CH13-CH32)
-    for (int i = 12; i < 32; i++) {
-        if (mspAuxOverlay[i] > 0) {
-#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
-            // Skip channels controlled by MSP RC Override when active
-            if (IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) && (rxConfig()->mspOverrideChannels & (1U << i))) {
-                continue;
+    {
+        int overlayStart = 12;
+#ifdef USE_RX_MSP
+        // When MSP is the primary RX, skip channels covered by MSP_SET_RAW_RC
+        if (rxConfig()->receiverType == RX_TYPE_MSP) {
+            const uint8_t mspChannels = rxMspGetLastChannelCount();
+            if (mspChannels > overlayStart) {
+                overlayStart = mspChannels;
             }
+        }
 #endif
-            rcChannels[i].data = mspAuxOverlay[i];
+        for (int i = overlayStart; i < 32; i++) {
+            if (mspAuxOverlay[i] > 0) {
+#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
+                // Skip channels controlled by MSP RC Override when active
+                if (IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) && (rxConfig()->mspOverrideChannels & (1U << i))) {
+                    continue;
+                }
+#endif
+                rcChannels[i].data = mspAuxOverlay[i];
+            }
         }
     }
 

From f025071bcc84efc9df6e0247304045a52e4e30e5 Mon Sep 17 00:00:00 2001
From: b14ckyy <he_black_dragon_88@live.de>
Date: Thu, 9 Apr 2026 18:50:20 +0200
Subject: [PATCH 062/102] Add MSP2_INAV_SET_AUX_RC documentation

- Rx.md: Add 'MSP Auxiliary RC Channel Overlay' section with key
  properties, use case, priority order, and safeguard descriptions
- Rx.md: Fix MSP RX channel count (18 -> 34)
- msp_messages.json: Update channel range to CH13-CH32, add clamp
  range, payload size limits, and guard descriptions
---
 docs/Rx.md                             | 27 +++++++++++++++++++++++++-
 docs/development/msp/msp_messages.json |  8 ++++----
 2 files changed, 30 insertions(+), 5 deletions(-)

diff --git a/docs/Rx.md b/docs/Rx.md
index 0f0ae64a453..3c9f5497448 100644
--- a/docs/Rx.md
+++ b/docs/Rx.md
@@ -201,7 +201,7 @@ bind_msp_rx <port>
 
 ## MultiWii serial protocol (MSP RX)
 
-Allows you to use MSP commands as the RC input. Up to 18 channels are supported.
+Allows you to use MSP commands as the RC input. Up to 34 channels are supported.
 Note:
 * It is necessary to update `MSP_SET_RAW_RC` at 5Hz or faster.
 * `MSP_SET_RAW_RC` uses the defined RC channel map
@@ -213,6 +213,31 @@ Note:
 
 Enables the use of a joystick in the INAV SITL with a flight simulator. See the [SITL documentation](SITL/SITL.md).
 
+## MSP Auxiliary RC Channel Overlay (MSP2_INAV_SET_AUX_RC)
+
+Allows extending the available RC channel count beyond the native RC link capacity using `MSP2_INAV_SET_AUX_RC` (`0x2230`). This is a lightweight, bandwidth-efficient alternative to `MSP_SET_RAW_RC` for auxiliary channels only.
+
+**Key properties:**
+- Controls **CH13–CH32** only (CH1–CH12 are protected and rejected)
+- Configurable resolution: 2-bit (3 positions), 4-bit (~71µs steps), 8-bit (~3.9µs steps), or 16-bit (raw PWM)
+- Value `0` = skip (no update) — previous value persists indefinitely
+- No flight mode or special configuration required — always active
+- Does **not** affect failsafe detection
+- Recommended to send with `MSP_FLAG_DONT_REPLY` (`flags=0x01`) on telemetry passthrough links
+
+**Typical use case:** A Lua script on the radio sends `MSP2_INAV_SET_AUX_RC` via SmartPort/CRSF/ELRS telemetry passthrough to control auxiliary functions (lights, camera triggers, gimbal modes) on channels beyond the RC link's native capacity.
+
+**Priority order** (last writer wins):
+1. Primary RX (SBUS, CRSF, FPort, etc.)
+2. MSP RC Override (if active)
+3. **MSP AUX Overlay** (CH13–CH32)
+
+**Important:** For serial RX protocols, the firmware cannot detect which channels the sender actively uses. If AUX_RC targets a channel that the RX link also sends, AUX_RC will override it. Configure the start channel above your RC link's active channel range.
+
+When MSP is the primary RX provider (`receiver_type = MSP`), channels covered by `MSP_SET_RAW_RC` are automatically protected. Channels in the `msp_override_channels` bitmask are also protected when MSP RC Override mode is active.
+
+See the [MSP documentation](development/msp/README.md) for the full message format.
+
 ## Configuration
 
 The receiver type can be set from the configurator or CLI.
diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json
index 866eb87b3b8..bbb7eac3429 100644
--- a/docs/development/msp/msp_messages.json
+++ b/docs/development/msp/msp_messages.json
@@ -10951,13 +10951,13 @@
                 {
                     "name": "definitionByte",
                     "ctype": "uint8_t",
-                    "desc": "Packed start channel and resolution. Bits 7-3: start channel index (valid range 8-31 for CH9-CH32; 0-7 rejected as error). Bits 2-0: resolution mode (0=2-bit, 1=4-bit, 2=8-bit, 3=16-bit; 4-7 reserved/error).",
+                    "desc": "Packed start channel and resolution. Bits 7-3: start channel index (valid range 12-31 for CH13-CH32; 0-11 rejected as error). Bits 2-0: resolution mode (0=2-bit, 1=4-bit, 2=8-bit, 3=16-bit; 4-7 reserved/error).",
                     "units": ""
                 },
                 {
                     "name": "channelData",
                     "ctype": "uint8_t",
-                    "desc": "Packed channel values, sequential from start channel. Number of channels is derived from data size and resolution. Value 0 means skip (no update). Sub-byte modes (2-bit, 4-bit) are packed MSB-first. 2-bit values 1-3 map to 1000/1500/2000us. 4-bit values 1-15 map to 1000 + (val-1)*1000/14 us. 8-bit values 1-255 map to 1000 + (val-1)*1000/254 us. 16-bit values are direct PWM.",
+                    "desc": "Packed channel values, sequential from start channel. Number of channels is derived from data size and resolution. Value 0 means skip (no update). Sub-byte modes (2-bit, 4-bit) are packed MSB-first. 2-bit values 1-3 map to 1000/1500/2000us. 4-bit values 1-15 map to 1000 + (val-1)*1000/14 us. 8-bit values 1-255 map to 1000 + (val-1)*1000/254 us. 16-bit values are direct PWM, clamped to 750-2250us.",
                     "units": "PWM (encoded)",
                     "array": true,
                     "array_size": 0
@@ -10966,8 +10966,8 @@
         },
         "reply": null,
         "variable_len": true,
-        "notes": "CH1-CH8 (index 0-7) are protected and will return `MSP_RESULT_ERROR`. Constraint: `startChannel + channelCount <= 32`. Values persist until overwritten; no timeout. Applied as a post-RX overlay in `calculateRxChannelsAndUpdateFailsafe()` after MSP RC Override but before failsafe. Does not require `USE_RX_MSP` or MSP-RC-OVERRIDE flight mode. Does not affect failsafe detection. Recommended to send with `MSP_FLAG_DONT_REPLY` (flags=0x01) to save bandwidth on telemetry passthrough links. 16-bit mode requires even number of data bytes.",
-        "description": "Bandwidth-efficient auxiliary RC channel update. Sets CH9-CH32 with configurable resolution (2/4/8/16-bit) without affecting primary flight controls. Designed for extending channel count beyond native RC link capacity via MSP passthrough."
+        "notes": "CH1-CH12 (index 0-11) are protected and will return `MSP_RESULT_ERROR`. Payload size must be 2-49 bytes. Constraint: `startChannel + channelCount <= 32`. Values persist until overwritten; no timeout. Applied as a post-RX overlay in `calculateRxChannelsAndUpdateFailsafe()` after MSP RC Override but before failsafe. Does not require `USE_RX_MSP` or MSP-RC-OVERRIDE flight mode. Does not affect failsafe detection. When MSP is the primary RX provider, channels covered by `MSP_SET_RAW_RC` are automatically skipped. Channels in the `mspOverrideChannels` bitmask are skipped when MSP RC Override mode is active. Recommended to send with `MSP_FLAG_DONT_REPLY` (flags=0x01) to save bandwidth on telemetry passthrough links. 16-bit mode requires even number of data bytes and values are clamped to 750-2250us.",
+        "description": "Bandwidth-efficient auxiliary RC channel update. Sets CH13-CH32 with configurable resolution (2/4/8/16-bit) without affecting primary flight controls. Designed for extending channel count beyond native RC link capacity via MSP passthrough."
     },
     "MSP2_BETAFLIGHT_BIND": {
         "code": 12288,

From f397f7ed043a30c7f5e45604bb9a8ffbb33513a7 Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Thu, 9 Apr 2026 22:40:50 -0500
Subject: [PATCH 063/102] telemetry: fix CRSF airspeed integer truncation and
 empty RPM/temp frames

- Fix airspeed always transmitting zero: integer `36 / 100` evaluates
  to 0 in C; use `36.0f / 100.0f` for correct cm/s to 0.1 km/h conversion
- crsfRpm/crsfTemperature now return bool; crsfFinalize is only called
  when data was actually written, preventing malformed single-byte frames
  when ESC sensor or temperature sources are absent
---
 src/main/telemetry/crsf.c | 20 +++++++++++++-------
 1 file changed, 13 insertions(+), 7 deletions(-)

diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c
index 159a8c16ce2..e8214fc8c1f 100755
--- a/src/main/telemetry/crsf.c
+++ b/src/main/telemetry/crsf.c
@@ -321,7 +321,7 @@ static void crsfFrameAirSpeedSensor(sbuf_t *dst)
     // use sbufWrite since CRC does not include frame length
     sbufWriteU8(dst, CRSF_FRAME_AIRSPEED_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
     crsfSerialize8(dst, CRSF_FRAMETYPE_AIRSPEED_SENSOR);
-    crsfSerialize16(dst, (uint16_t)(getAirspeedEstimate() * 36 / 100));
+    crsfSerialize16(dst, (uint16_t)(getAirspeedEstimate() * 36.0f / 100.0f));
 }
 #endif
 
@@ -332,7 +332,7 @@ static void crsfFrameAirSpeedSensor(sbuf_t *dst)
 uint8_t    rpm_source_id;  // Identifies the source of the RPM data (e.g., 0 = Motor 1, 1 = Motor 2, etc.)
 int24_t    rpm_value[];     // 1 - 19 RPM values with negative ones representing the motor spinning in reverse
 */
-static void crsfRpm(sbuf_t *dst)
+static bool crsfRpm(sbuf_t *dst)
 {
     const uint8_t MAX_CRSF_RPM_VALUES = 19;  // CRSF protocol limit: 1-19 RPM values
     uint8_t motorCount = getMotorCount();
@@ -352,7 +352,9 @@ static void crsfRpm(sbuf_t *dst)
             const escSensorData_t *escState = getEscTelemetry(i);
             crsfSerialize24(dst, (escState) ? escState->rpm : 0);
         }
+        return true;
     }
+    return false;
 }
 #endif
 
@@ -362,7 +364,7 @@ static void crsfRpm(sbuf_t *dst)
 uint8_t temp_source_id; // Identifies the source of the temperature data (e.g., 0 = FC including all ESCs, 1 = Ambient, etc.)
 int16_t temperature[]; // up to 20 temperature values in deci-degree (tenths of a degree) Celsius (e.g., 250 = 25.0°C, -50 = -5.0°C)
 */
-static void crsfTemperature(sbuf_t *dst)
+static bool crsfTemperature(sbuf_t *dst)
 {
     const uint8_t MAX_CRSF_TEMPS = 20;  // Maximum temperatures per CRSF frame
     uint8_t tempCount = 0;
@@ -393,7 +395,9 @@ static void crsfTemperature(sbuf_t *dst)
         crsfSerialize8(dst, 0);
         for (uint8_t i = 0; i < tempCount; i++)
             crsfSerialize16(dst, temperatures[i]);
+        return true;
     }
+    return false;
 }
 
 typedef enum {
@@ -632,15 +636,17 @@ static void processCrsf(void)
 #ifdef USE_ESC_SENSOR
     if (currentSchedule & BV(CRSF_FRAME_RPM_INDEX)) {
         crsfInitializeFrame(dst);
-        crsfRpm(dst);
-        crsfFinalize(dst);
+        if (crsfRpm(dst)) {
+            crsfFinalize(dst);
+        }
     }
 #endif
 #if defined(USE_ESC_SENSOR) || defined(USE_TEMPERATURE_SENSOR)
     if (currentSchedule & BV(CRSF_FRAME_TEMP_INDEX)) {
         crsfInitializeFrame(dst);
-        crsfTemperature(dst);
-        crsfFinalize(dst);
+        if (crsfTemperature(dst)) {
+            crsfFinalize(dst);
+        }
     }
 #endif
 #ifdef USE_GPS

From 0f18cbc6946a55a7391bfd2129da5331bd9d62ac Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Thu, 5 Mar 2026 23:11:59 -0600
Subject: [PATCH 064/102] Use single-precision math where double is unnecessary

Replace three uses of double-precision library functions with their
float equivalents, eliminating software-emulated double math on the
Cortex-M7 FPV5-SP-D16 (single-precision-only) FPU:

- gps_ublox.c: atof() -> fastA2F() in gpsDecodeProtocolVersion().
  atof() pulled in libc_nano strtod (28 KB of double-precision parsing
  machinery) to parse simple version strings like "18.00". fastA2F()
  is already in the codebase and covers this use case.

- maths.c: exp/pow (double) -> expf/powf in gaussian().
  The explicit (double) casts defeated the -fsingle-precision-constant
  flag. gaussian() is called from the IMU loop at up to 1000 Hz, so
  switching to hardware-accelerated single-precision also improves
  runtime performance.

- vtx_smartaudio.c: pow(10.0, ...) -> powf(10.0f, ...) in saDbiToMw().
  Combined with the maths.c change, removes all callers of double pow,
  dropping libm e_pow.o from the link entirely.

Flash savings on MATEKF722 (STM32F722, 512 KB):
  text: -16,560 B  data: -376 B  total: -16,936 B (~16.5 KB)
---
 src/main/common/maths.c      | 2 +-
 src/main/io/gps_ublox.c      | 2 +-
 src/main/io/vtx_smartaudio.c | 2 +-
 3 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/common/maths.c b/src/main/common/maths.c
index 993634d902d..2881d2b8938 100644
--- a/src/main/common/maths.c
+++ b/src/main/common/maths.c
@@ -526,7 +526,7 @@ bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float resu
 }
 
 float gaussian(const float x, const float mu, const float sigma) {
-    return exp(-pow((double)(x - mu), 2) / (2 * pow((double)sigma, 2)));
+    return expf(-powf(x - mu, 2.0f) / (2.0f * powf(sigma, 2.0f)));
 }
 
 float bellCurve(const float x, const float curveWidth)
diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c
index f0899aa69d5..d198ed67dc6 100755
--- a/src/main/io/gps_ublox.c
+++ b/src/main/io/gps_ublox.c
@@ -549,7 +549,7 @@ static void gpsDecodeProtocolVersion(const char *proto, size_t bufferLength)
     if (bufferLength > 13 && (!strncmp(proto, "PROTVER=", 8) || !strncmp(proto, "PROTVER ", 8))) {
         proto+=8;
 
-        float ver = atof(proto);
+        float ver = fastA2F(proto);
 
         gpsState.swVersionMajor = (uint8_t)ver;
         gpsState.swVersionMinor = (uint8_t)((ver - gpsState.swVersionMajor) * 100.0f);
diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c
index 7d5e213cce0..7e24d552929 100644
--- a/src/main/io/vtx_smartaudio.c
+++ b/src/main/io/vtx_smartaudio.c
@@ -212,7 +212,7 @@ int saDacToPowerIndex(int dac)
 
 int saDbiToMw(uint16_t dbi) {
 
-    uint16_t mw = (uint16_t)pow(10.0, dbi / 10.0);
+    uint16_t mw = (uint16_t)powf(10.0f, (float)dbi / 10.0f);
 
     if (dbi > 14) {
         // For powers greater than 25mW round up to a multiple of 50 to match expectations

From 03751bc76a9a6e60d6b28cebb3004af5d5149f98 Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Thu, 5 Mar 2026 23:25:04 -0600
Subject: [PATCH 065/102] Code review fixes for float math flash savings

Follow-up to the single-precision math change that saves ~17 KB of
flash on STM32F722 targets:

- maths.c: Replace powf(x, 2.0f) with explicit multiplies in
  gaussian(). Clearer intent and avoids any dependency on libm
  reducing integer-exponent powf to a multiply.

- vtx_smartaudio.c: Add roundf() before uint16_t cast in saDbiToMw().
  powf(10.0f, 1.0f) may return 9.999...f on some libm implementations,
  which would truncate to 9 mW instead of the correct 10 mW. roundf()
  closes the hazard at the cost of one FPU instruction.
---
 src/main/common/maths.c      | 2 +-
 src/main/io/vtx_smartaudio.c | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/common/maths.c b/src/main/common/maths.c
index 2881d2b8938..d591e40de81 100644
--- a/src/main/common/maths.c
+++ b/src/main/common/maths.c
@@ -526,7 +526,7 @@ bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float resu
 }
 
 float gaussian(const float x, const float mu, const float sigma) {
-    return expf(-powf(x - mu, 2.0f) / (2.0f * powf(sigma, 2.0f)));
+    return expf(-((x - mu) * (x - mu)) / (2.0f * sigma * sigma));
 }
 
 float bellCurve(const float x, const float curveWidth)
diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c
index 7e24d552929..acb2a34e6dc 100644
--- a/src/main/io/vtx_smartaudio.c
+++ b/src/main/io/vtx_smartaudio.c
@@ -212,7 +212,7 @@ int saDacToPowerIndex(int dac)
 
 int saDbiToMw(uint16_t dbi) {
 
-    uint16_t mw = (uint16_t)powf(10.0f, (float)dbi / 10.0f);
+    uint16_t mw = (uint16_t)roundf(powf(10.0f, (float)dbi / 10.0f));
 
     if (dbi > 14) {
         // For powers greater than 25mW round up to a multiple of 50 to match expectations

From a66b346c24b7f6741c418d0104b564233b898ad4 Mon Sep 17 00:00:00 2001
From: Sensei <deepmagicbeginshere@gmail.com>
Date: Fri, 10 Apr 2026 12:48:31 -0500
Subject: [PATCH 066/102] blueberry h743: Restore PWM beeper timer

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

diff --git a/src/main/target/BLUEBERRYH743/target.c b/src/main/target/BLUEBERRYH743/target.c
index 5245d05e257..2c924ecac48 100644
--- a/src/main/target/BLUEBERRYH743/target.c
+++ b/src/main/target/BLUEBERRYH743/target.c
@@ -47,8 +47,9 @@ timerHardware_t timerHardware[] = {
 
     DEF_TIM(TIM15, CH1, PE5, TIM_USE_OUTPUT_AUTO, 0, 0),   // S11
     DEF_TIM(TIM15, CH2, PE6, TIM_USE_OUTPUT_AUTO, 0, 0),   // S12 DMA_NONE
-
+    DEF_TIM(TIM2,  CH1, PA15, TIM_USE_BEEPER, 0, 0),  // BEEPER PWM
     DEF_TIM(TIM1,  CH1, PA8,  TIM_USE_LED, 0, 9),    // LED_2812
+
 };
 
 const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

From 50de29acfb02eb836da6f6dec3007639cb0e85b5 Mon Sep 17 00:00:00 2001
From: b14ckyy <he_black_dragon_88@live.de>
Date: Sat, 11 Apr 2026 15:54:44 +0200
Subject: [PATCH 067/102] docs: add Backup and Restore documentation for
 Configurator auto-backup feature

- Add new docs/Backup and Restore.md covering automatic backup/restore
  during firmware flash, migration preview, file locations and formats
- Update Installation.md: reference auto-backup in Backup/Restore section
- Update Configuration.md: replace outdated GUI backup warning
- Update Cli.md: note that Configurator now handles backup automatically

Related: iNavFlight/inav-configurator#2603
---
 docs/Backup and Restore.md | 107 +++++++++++++++++++++++++++++++++++++
 docs/Cli.md                |   2 +
 docs/Configuration.md      |   2 +-
 docs/Installation.md       |   6 ++-
 4 files changed, 115 insertions(+), 2 deletions(-)
 create mode 100644 docs/Backup and Restore.md

diff --git a/docs/Backup and Restore.md b/docs/Backup and Restore.md
new file mode 100644
index 00000000000..9206abafa03
--- /dev/null
+++ b/docs/Backup and Restore.md	
@@ -0,0 +1,107 @@
+# Backup and Restore
+
+INAV Configurator can automatically back up your configuration before flashing firmware and offer to restore it afterwards. When upgrading across major versions (e.g. 7.x → 8.x → 9.x), settings are automatically migrated to the new firmware format.
+
+For manual CLI-based backup and restore, see the [CLI documentation](Cli.md#backup-via-cli).
+
+## Automatic Backup & Restore During Firmware Flash
+
+### What happens automatically
+
+1. **Before flashing** (with Full Chip Erase enabled): Your current CLI configuration (`diff all`) is automatically captured and saved to the backup directory.
+2. **After flashing**: Depending on the situation, the Configurator offers to restore your settings:
+
+| Scenario | Behavior |
+|----------|----------|
+| **Patch update** (e.g. 8.0.0 → 8.0.1) | Auto-restore offered immediately |
+| **Minor update** (e.g. 8.0.0 → 8.1.0) | Auto-restore offered immediately |
+| **Major upgrade** (e.g. 7.x → 8.x) with migration profile available | Migration preview shown — confirm to restore with converted settings |
+| **Major upgrade** without migration profile | Warning shown — restore still possible but some settings may fail |
+| **Major downgrade** (e.g. 9.x → 7.x) | Auto-restore blocked — manual restore only (settings may be incompatible) |
+| **Local firmware file** (loaded from disk) | No auto-restore offered — backup is still saved |
+| **Flash without Full Chip Erase** | Backup taken, no restore offered |
+
+### Migration Preview
+
+When updating across major versions (e.g. 7.x → 9.x), the Configurator shows a **migration preview overlay** before restoring. This lists:
+
+- **Removed settings** — settings that no longer exist in the new firmware (will be skipped)
+- **Renamed settings** — settings whose name changed (automatically converted)
+- **Renamed commands** — CLI commands that were renamed (automatically converted)
+- **Value replacements** — setting values that changed meaning (automatically converted)
+- **Setting remappings** — numeric IDs that were renumbered (automatically converted)
+- **Warnings** — settings whose semantics changed and require manual review
+
+You can review all changes before confirming or cancelling the restore.
+
+Multi-step migrations are handled automatically. For example, a 7.x → 9.x upgrade applies migration profiles in sequence (7→8, then 8→9).
+
+## Manual Backup & Restore
+
+The Firmware Flasher tab provides three buttons:
+
+- **Backup Config** — saves your current CLI configuration to a file (opens save dialog)
+- **Restore Config** — loads a backup file and restores it to your flight controller
+  - If the backup is from a different major version, the migration preview is shown first
+  - If no migration profile exists for the version gap, a warning is shown but you can still proceed
+- **Open Backups Folder** — opens the backup directory in your file manager
+
+For CLI-based backup and restore procedures, see [Backup via CLI](Cli.md#backup-via-cli) and [Restore via CLI](Cli.md#restore-via-cli).
+
+## Backup File Location
+
+Backups are stored in your OS-specific application data directory:
+
+| OS | Path |
+|----|------|
+| **Windows** | `%APPDATA%/inav-configurator/backups/` |
+| **macOS** | `~/Library/Application Support/inav-configurator/backups/` |
+| **Linux** | `~/.config/inav-configurator/backups/` |
+
+Use the **Open Backups Folder** button in the Firmware Flasher tab to open this directory.
+
+## Backup File Naming
+
+| Type | Format |
+|------|--------|
+| Auto-backups | `UPDATE_inav_backup_{version}_{board}_{YYYY-MM-DD_HHMMSS}.txt` |
+| Manual backups | `inav_backup_{version}_{board}_{YYYY-MM-DD_HHMMSS}.txt` |
+
+Auto-backups are pruned automatically — only the 10 most recent are kept. Files you rename are never pruned.
+
+## Backup File Format
+
+Backup files are plain-text CLI dumps with a metadata header:
+
+```
+# INAV Backup
+# Version: 8.0.0
+# Board: SPEEDYBEEF405V4
+# Date: 2026-04-11T10:30:00.000Z
+# Craft: MyQuad
+#
+# INAV/SPEEDYBEEF405V4 8.0.0 Apr  1 2026 / 12:00:00 (abc1234)
+# GCC-13.2.1
+# ...
+set gyro_main_lpf_hz = 110
+set acc_hardware = AUTO
+...
+```
+
+You can open and edit these files with any text editor.
+
+## Restore Error Handling
+
+If errors occur during restore (e.g. unknown settings, invalid values):
+
+- An error dialog shows the affected lines
+- You can choose:
+  - **Save anyway** — saves the successfully applied settings and reboots
+  - **Abort** — discards all changes and exits CLI mode
+
+## Tips
+
+- **Always flash with Full Chip Erase** when upgrading to a new version. This ensures clean defaults and triggers automatic backup and restore.
+- **Review migration previews carefully** — especially the warnings section, which highlights settings whose meaning may have changed.
+- **Keep manual backups** before major upgrades. While auto-backup handles this, having an extra copy in a known location gives peace of mind.
+- **Use `diff` over `dump`** for backups. The `diff` format only stores settings that differ from defaults, which makes restoring safer across versions. The auto-backup feature already uses `diff all`.
diff --git a/docs/Cli.md b/docs/Cli.md
index d949937a61b..d27029ee1b5 100644
--- a/docs/Cli.md
+++ b/docs/Cli.md
@@ -22,6 +22,8 @@ See the other documentation sections for details of the cli commands and setting
 
 ## Backup via CLI
 
+> **Note:** The INAV Configurator now performs automatic backups before flashing and can restore settings afterwards, including migration across major versions. See [Backup and Restore](Backup%20and%20Restore.md) for details. The CLI method below remains available for manual backup.
+
 Disconnect main power, connect to cli via USB/FTDI.
 
 dump using cli
diff --git a/docs/Configuration.md b/docs/Configuration.md
index 1ccea15d110..f48de1a10d6 100644
--- a/docs/Configuration.md
+++ b/docs/Configuration.md
@@ -10,7 +10,7 @@ See the Serial section for more information and see the Board specific sections
 The GUI cannot currently configure all aspects of the system, the CLI must be used to enable or configure
 some features and settings.
 
-__Due to ongoing development, the fact that the GUI cannot yet backup all your settings and automatic chrome updates of the GUI app it is highly advisable to backup your settings (using the CLI) so that when a new version of the configurator or firmware is released you can re-apply your settings.__
+__The INAV Configurator now (versions after 9.0.x) automatically backs up your settings before flashing firmware and can restore them afterwards (when Full Chip Erase is enabled), including automatic migration across major versions. For details, see [Backup and Restore](Backup%20and%20Restore.md). You can also manually backup and restore settings using the [CLI](Cli.md#backup-via-cli).__
 
 
 ## GUI
diff --git a/docs/Installation.md b/docs/Installation.md
index 38e9470fd81..140f47bd6c9 100644
--- a/docs/Installation.md
+++ b/docs/Installation.md
@@ -27,4 +27,8 @@ When upgrading be sure to backup / dump your existing settings.  Some firmware r
 
 ## Backup/Restore process
 
-See the CLI section of the docs for details on how to backup and restore your configuration via the CLI.
+The INAV Configurator (after version 9.0.x) automatically backs up your configuration before flashing and offers to restore it afterwards (when Full Chip Erase is enabled) — including automatic settings migration when upgrading across major versions.
+
+For details on automatic and manual backup/restore, see [Backup and Restore](Backup%20and%20Restore.md).
+
+For CLI-based backup and restore, see the [CLI documentation](Cli.md#backup-via-cli).

From 959aa7848c99864a5457067820885a00e051ea95 Mon Sep 17 00:00:00 2001
From: b14ckyy <he_black_dragon_88@live.de>
Date: Sat, 11 Apr 2026 16:00:46 +0200
Subject: [PATCH 068/102] docs: add Backup/Restore architecture reference for
 migration profiles

Developer-facing documentation covering migration engine internals,
migration profile schema, and instructions for creating new profiles
when major INAV versions are released.

Related: iNavFlight/inav-configurator#2603
---
 .../Backup Restore Architecture.md            | 251 ++++++++++++++++++
 1 file changed, 251 insertions(+)
 create mode 100644 docs/development/Backup Restore Architecture.md

diff --git a/docs/development/Backup Restore Architecture.md b/docs/development/Backup Restore Architecture.md
new file mode 100644
index 00000000000..9dd971230c6
--- /dev/null
+++ b/docs/development/Backup Restore Architecture.md	
@@ -0,0 +1,251 @@
+# Backup, Restore & Settings Migration — Architecture
+
+> **Note:** This document describes the internals of the INAV Configurator's backup/restore and settings migration system. It is intended for Configurator developers, not end users. For user-facing documentation, see the [INAV docs](https://github.com/iNavFlight/inav/blob/master/docs/Backup%20and%20Restore.md).
+
+## Architecture Overview
+
+```
+User → Firmware Flasher Tab → STM32.connect(onCliReady) → CLI mode
+                                                              ↓
+                                          BackupRestore.captureCliDiffAll()
+                                                              ↓
+                                           Save to file, prune old backups
+                                                              ↓
+                                               STM32 flash (DFU or serial)
+                                                              ↓
+                                              onFlashComplete() callback
+                                                              ↓
+                                      Version check → Migration check → UI overlay
+                                                              ↓
+                                      User confirms → Poll for FC reconnect
+                                                              ↓
+                        BackupRestore.performRestore() or performRestoreWithMigration()
+                                                              ↓
+                                    saveAndReboot() or abortRestore()
+```
+
+## Files
+
+| File | Purpose |
+|------|---------|
+| `js/backup_restore.js` | Core backup/restore module — CLI protocol, file I/O, auto-backup |
+| `js/migration/migration_handler.js` | Version migration engine — profile chaining, line transformation |
+| `js/migration/7_to_8.json` | Migration profile: INAV 7.x → 8.0 |
+| `js/migration/8_to_9.json` | Migration profile: INAV 8.0 → 9.0 |
+| `tabs/firmware_flasher.js` | Flash integration — auto-backup trigger, restore UI, version gating |
+| `tabs/firmware_flasher.html` | Overlays and buttons for backup/restore/migration UI |
+| `src/css/tabs/firmware_flasher.css` | Overlay styles |
+| `js/protocols/stm32.js` | STM32 flash protocol — `onCliReady` callback, DFU timeout fix |
+| `js/main/main.js` | Electron main process — IPC handlers for file operations |
+| `js/main/preload.js` | IPC bridge — exposes backup API to renderer |
+| `locale/en/messages.json` | All i18n translation keys |
+
+## Adding a New Migration Profile
+
+When a new major INAV version is released (e.g. 9.x → 10.x), create a migration profile:
+
+### Step 1: Create the JSON profile
+
+Create `js/migration/9_to_10.json`:
+
+```json
+{
+    "fromVersion": "9",
+    "toVersion": "10",
+    "description": "INAV 9.x → 10.0 migration profile",
+
+    "commandRenames": {
+        "old_command_name": "new_command_name"
+    },
+
+    "settingRenames": {
+        "old_setting_name": "new_setting_name"
+    },
+
+    "valueReplacements": {
+        "setting_name": {
+            "OLD_VALUE": "NEW_VALUE"
+        }
+    },
+
+    "removed": [
+        "deleted_setting_1",
+        "deleted_setting_2"
+    ],
+
+    "settingPatternMappings": [
+        {
+            "pattern": "^regex_matching_setting_names$",
+            "valueMap": { "old_numeric_id": "new_numeric_id" },
+            "description": "Human-readable description of remapping"
+        }
+    ],
+
+    "warnings": [
+        "Human-readable warning about settings whose semantics changed and need manual review."
+    ]
+}
+```
+
+### Step 2: Register the profile
+
+In `js/migration/migration_handler.js`, add the import and append to the array:
+
+```javascript
+import profile_9_to_10 from './9_to_10.json';
+
+const MIGRATION_PROFILES = [
+    profile_7_to_8,
+    profile_8_to_9,
+    profile_9_to_10,   // ← add here
+];
+```
+
+The migration engine automatically chains profiles. A 7.x → 10.x upgrade will apply all three profiles in sequence (7→8, 8→9, 9→10).
+
+### How to determine what goes into a migration profile
+
+Compare CLI settings between the old and new firmware version:
+
+1. **Removed settings**: Run `diff all` on old and new firmware with default settings. Settings present in old but not in new → add to `removed`
+2. **Renamed settings**: Check INAV release notes and source code for renamed settings → add to `settingRenames`
+3. **Renamed commands**: Check for CLI command name changes (e.g. `profile` → `control_profile`) → add to `commandRenames`
+4. **Value replacements**: Check for enum value name changes → add to `valueReplacements`
+5. **Pattern mappings**: Check for bulk ID renumbering (OSD elements, etc.) → add to `settingPatternMappings`
+6. **Warnings**: Check for settings where the meaning/units changed but name stayed the same → add to `warnings`
+
+Key INAV source files to check:
+- `src/main/fc/settings.yaml` — all CLI settings definitions
+- `src/main/fc/cli.c` — CLI command implementations
+- Release notes on GitHub
+
+## Migration Profile Schema Reference
+
+| Field | Type | Required | Description |
+|-------|------|----------|-------------|
+| `fromVersion` | `string` | Yes | Source major version number (e.g. `"9"`) |
+| `toVersion` | `string` | Yes | Target major version number (e.g. `"10"`) |
+| `description` | `string` | Yes | Human-readable description |
+| `commandRenames` | `object` | Yes | Maps old CLI command names to new names. Applied to any token in the command line. E.g. `"profile" → "control_profile"` transforms `profile 2` to `control_profile 2` |
+| `settingRenames` | `object` | Yes | Maps old `set` setting names to new names. Only applies to `set <name> = <value>` lines |
+| `valueReplacements` | `object` | Yes | Maps setting names to value replacement objects `{ "oldval": "newval" }`. Only modifies the value portion after `=` |
+| `removed` | `string[]` | Yes | List of setting names to remove entirely. Lines with `set <name> = ...` matching these are dropped |
+| `settingPatternMappings` | `array` | Yes | Array of pattern-based value remappings for settings matching a regex. Each entry has `pattern` (regex), `valueMap` (object), `description` (string) |
+| `warnings` | `string[]` | Yes | Warning messages about semantic changes requiring manual review. Displayed in migration preview overlay |
+
+## Existing Migration Profile Details
+
+### 7_to_8.json (INAV 7.x → 8.0)
+
+| Category | Changes |
+|----------|---------|
+| **Command renames** | `profile` → `control_profile` |
+| **Value replacements** | `gps_provider`: `UBLOX7` → `UBLOX` |
+| **Removed settings** (18) | `control_deadband`, `cpu_underclock`, `disarm_kill_switch`, `dji_workarounds`, `fw_iterm_limit_stick_position`, `gyro_anti_aliasing_lpf_type`, `gyro_hardware_lpf`, `gyro_main_lpf_type`, `gyro_use_dyn_lpf`, `inav_use_gps_no_baro`, `inav_use_gps_velned`, `ledstrip_visual_beeper`, `max_throttle`, `nav_auto_climb_rate`, `nav_manual_climb_rate`, `osd_stats_min_voltage_unit`, `pidsum_limit`, `pidsum_limit_yaw` |
+| **Pattern mappings** | `osd_custom_element_N_type`: IDs remapped 4→9, 5→16, 6→7, 7→10 |
+| **Warnings** | `nav_fw_wp_tracking_accuracy` semantics changed: was arbitrary tracking response, now distance in meters |
+
+### 8_to_9.json (INAV 8.0 → 9.0)
+
+| Category | Changes |
+|----------|---------|
+| **Command renames** | `controlrate_profile` → `use_control_profile` |
+| **Setting renames** | `mixer_pid_profile_linking` → `mixer_control_profile_linking`, `osd_pan_servo_pwm2centideg` → `osd_pan_servo_range_decadegrees` |
+| **Value replacements** | None |
+| **Removed settings** | None |
+| **Pattern mappings** | None |
+| **Warnings** | Position estimator defaults changed (`w_z_baro_v`, `inav_w_z_gps_p`, `inav_w_z_gps_v`). `ahrs_acc_ignore_rate` default changed 20→15 |
+
+## Migration Engine Internals
+
+### Profile chaining
+
+`buildMigrationChain(fromVersion, toVersion)` selects all profiles where `profileFrom >= fromMajor` and `profileTo <= toMajor`, sorted by `fromVersion`. A 7.x → 9.x migration applies both 7→8 and 8→9 profiles in sequence.
+
+### Line processing
+
+Each non-comment, non-empty line passes through every profile in the chain. For each profile, transformations are applied in this order:
+
+1. Command renames (any token in the line)
+2. Removed settings (line dropped if `set <name>` matches)
+3. Setting renames (`set <name>` replacement)
+4. Value replacements (value after `=` replaced)
+5. Setting pattern mappings (regex-matched settings with value remapping)
+
+### Missing profile detection
+
+`hasMissingProfiles()` returns `true` when the number of profiles in the chain is fewer than the number of major version steps. The UI shows a warning but still allows restore — some settings may fail.
+
+## Edge Cases Handled
+
+1. **Stale FC version after flash**: Real FC version is queried via `MSP_FC_VERSION` after connect, not the cached value
+2. **DFU mode (no MSP)**: `FC.CONFIG` null checks prevent crashes when connected in DFU mode
+3. **DFU timeout**: UI unlock and progress label update on timeout (no permanent lock)
+4. **Local firmware files**: `localFirmwareLoaded` flag prevents stale dropdown version from triggering wrong migration
+5. **Backup pruning with mixed versions**: Sort by timestamp portion, not full filename
+6. **Multi-step migration**: 7.x → 9.x automatically chains 7→8 + 8→9 profiles
+7. **Missing migration profiles**: Warning shown but restore allowed — graceful degradation
+8. **Version detection from backup**: Parsed from backup header (`# Version: X.Y.Z`), not from FC state
+
+## i18n Keys
+
+All backup/restore/migration translation keys in `locale/en/messages.json`:
+
+### Backup status
+| Key | Text |
+|-----|------|
+| `backupRestoreStatusEnteringCli` | Entering CLI |
+| `backupRestoreStatusReadingConfig` | Reading configuration via CLI |
+| `backupRestoreStatusSavingFile` | Saving backup file... |
+| `backupRestoreStatusExitingCli` | Exiting CLI mode |
+| `backupRestoreBackupSaved` | Backup saved $1 |
+| `backupRestoreAutoBackupSaved` | Auto-backup saved to $1 |
+| `backupRestoreBackupComplete` | Backup complete |
+| `backupRestoreBackupCancelled` | Backup cancelled |
+| `backupRestoreBackupFailed` | Backup failed |
+
+### Restore status
+| Key | Text |
+|-----|------|
+| `backupRestoreStatusConnecting` | Connecting to flight controller |
+| `backupRestoreStatusRestoringConfig` | Restoring configuration |
+| `backupRestoreStatusRestoringProgress` | Restoring... $1 / $2 |
+| `backupRestoreStatusSaving` | Saving configuration |
+| `backupRestoreRestoreComplete` | Configuration restored. Flight controller is rebooting. |
+| `backupRestoreRestoreCancelled` | Restore cancelled. |
+| `backupRestoreRestoreFailed` | Restore failed. |
+
+### Auto-restore UI
+| Key | Text |
+|-----|------|
+| `backupRestoreAutoRestoreConfirm` | Restore confirmation prompt |
+| `backupRestoreAutoRestoreWaiting` | Waiting for FC to reboot after flash |
+| `backupRestoreAutoRestoreYes` | Yes, restore settings |
+| `backupRestoreAutoRestoreNo` | No, keep current settings |
+| `backupRestoreAutoRestoreWaitingPort` | Waiting for port $1 to reconnect |
+| `backupRestoreDowngradeNoAutoRestore` | Major downgrade warning |
+| `backupRestoreFlashCompleteBackupSaved` | Backup saved (local firmware, no restore offer) |
+| `backupRestoreMigrationApplied` | Migration applied: $1 → $2 ($3 changes) |
+| `backupRestoreMigrationWarningsHeader` | Migration Warnings: |
+
+### Migration preview
+| Key | Text |
+|-----|------|
+| `migrationPreviewTitle` | Settings Migration Required |
+| `migrationPreviewSubtitle` | Conversion explanation |
+| `migrationPreviewRemovedHeader` | Removed Settings: |
+| `migrationPreviewRenamedSettingsHeader` | Renamed Settings: |
+| `migrationPreviewRenamedCommandsHeader` | Renamed Commands: |
+| `migrationPreviewValueReplacementsHeader` | Value Replacements: |
+| `migrationPreviewSettingRemappingsHeader` | Setting Remappings: |
+| `migrationPreviewContinue` | Continue with migration |
+| `migrationPreviewCancel` | Cancel restore |
+| `migrationMissingProfileWarning` | Missing profile warning |
+
+### Error messages
+| Key | Text |
+|-----|------|
+| `backupRestoreErrorTitle` | Restore Errors Detected |
+| `backupRestoreErrorText` | Error explanation |
+| `backupRestoreErrorAbort` | Abort |
+| `backupRestoreErrorSave` | Save anyway |

From ea2c1f12c9e2c5aed4b8461d04650e4e4bb0b925 Mon Sep 17 00:00:00 2001
From: b14ckyy <he_black_dragon_88@live.de>
Date: Sat, 11 Apr 2026 16:04:00 +0200
Subject: [PATCH 069/102] docs: add migration profile reminder to release
 checklist

Reference Backup Restore Architecture doc in pre-release checklist
and breaking changes branch usage section to ensure migration profiles
are created when major versions introduce settings changes.
---
 docs/development/release-create.md | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/docs/development/release-create.md b/docs/development/release-create.md
index 16dccb6292d..f1ba7cf2369 100644
--- a/docs/development/release-create.md
+++ b/docs/development/release-create.md
@@ -109,6 +109,7 @@ Version numbers are set in:
 - [ ] Release notes drafted
 - [ ] Breaking changes documented
 - [ ] New features documented
+- [ ] **Configurator migration profile created** for major version bumps (see [Backup Restore Architecture](Backup%20Restore%20Architecture.md#adding-a-new-migration-profile))
 
 ## Release Workflow
 
@@ -492,6 +493,7 @@ gh api repos/iNavFlight/inav-configurator/git/refs -f ref="refs/heads/maintenanc
 
 - **Changes maintaining backward compatibility** → PR to maintenance-X.x (e.g., maintenance-9.x)
 - **Breaking changes** (MSP protocol, settings structure) → PR to maintenance-(X+1).x (e.g., maintenance-10.x)
+  - When breaking changes affect CLI settings (renames, removals, value changes), a **Configurator migration profile** must be created. See [Backup Restore Architecture](Backup%20Restore%20Architecture.md#adding-a-new-migration-profile)
 - **Master** → NOT a PR target (receives merges only)
 
 Lower version branches are periodically merged into higher version branches (e.g., maintenance-9.x → master → maintenance-10.x).

From 392d21b2c3efaec22bd846fba1667346ea0d645c Mon Sep 17 00:00:00 2001
From: b14ckyy <he_black_dragon_88@live.de>
Date: Sat, 11 Apr 2026 16:14:35 +0200
Subject: [PATCH 070/102] clearer wording for backup and restore doc

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

diff --git a/docs/Backup and Restore.md b/docs/Backup and Restore.md
index 9206abafa03..771b129762d 100644
--- a/docs/Backup and Restore.md	
+++ b/docs/Backup and Restore.md	
@@ -8,8 +8,8 @@ For manual CLI-based backup and restore, see the [CLI documentation](Cli.md#back
 
 ### What happens automatically
 
-1. **Before flashing** (with Full Chip Erase enabled): Your current CLI configuration (`diff all`) is automatically captured and saved to the backup directory.
-2. **After flashing**: Depending on the situation, the Configurator offers to restore your settings:
+1. **Before flashing** (with or without Full Chip Erase enabled): Your current CLI configuration (`diff all`) is automatically captured and saved to the backup directory.
+2. **After flashing**: Depending on the situation, the Configurator offers to restore your settings if Full Chip Erase was enabled:
 
 | Scenario | Behavior |
 |----------|----------|

From 69db786b614620e12af337f3c6b440ab6bbf4d65 Mon Sep 17 00:00:00 2001
From: frogmane <7685285+xznhj8129@users.noreply.github.com>
Date: Sat, 11 Apr 2026 22:23:42 -0400
Subject: [PATCH 071/102] Add MSP2 link stats message + fix missing
 msp-set-wp-index-and-altitude spec + msp_gen_docs

---
 docs/development/msp/README.md             |  56 ++++++----
 docs/development/msp/gen_docs.sh           |   2 +-
 docs/development/msp/inav_enums.json       | 121 ++++++++++----------
 docs/development/msp/inav_enums_ref.md     | 123 +++++++++++----------
 docs/development/msp/msp_messages.checksum |   2 +-
 docs/development/msp/msp_messages.json     |  63 +++++++++++
 docs/development/msp/rev                   |   2 +-
 src/main/fc/fc_msp.c                       |   6 +
 src/main/msp/msp_protocol_v2_inav.h        |   2 +
 9 files changed, 240 insertions(+), 137 deletions(-)

diff --git a/docs/development/msp/README.md b/docs/development/msp/README.md
index 696e7ab7fc1..833a9f6873c 100644
--- a/docs/development/msp/README.md
+++ b/docs/development/msp/README.md
@@ -9,7 +9,8 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i
 
 
 
-**JSON file rev: 4**
+**JSON file rev: 5
+**
 
 **Warning: Verification needed, exercise caution until completely verified for accuracy and cleared, especially for integer signs. Source-based generation/validation is forthcoming. Refer to source for absolute certainty** 
 
@@ -66,6 +67,7 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i
 **variable_len**: Optional boolean, if true, message does not have a predefined fixed length and needs appropriate handling\
 **variants**: Optional special case, message has different cases of reply/request. Key/description is not a strict expression or code; just a readable condition\
 **not_implemented**: Optional special case, message is not implemented (never or deprecated)\
+**replaced_by**: Optional array of MSP message names that replace this command. Present when a command is deprecated and scheduled for removal. Empty array if no replacement is needed\
 **notes**: String with details of message
 
 ## Data dict fields:
@@ -403,6 +405,7 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i
 [8448 - MSP2_INAV_CUSTOM_OSD_ELEMENTS](#msp2_inav_custom_osd_elements)  
 [8449 - MSP2_INAV_CUSTOM_OSD_ELEMENT](#msp2_inav_custom_osd_element)  
 [8450 - MSP2_INAV_SET_CUSTOM_OSD_ELEMENTS](#msp2_inav_set_custom_osd_elements)  
+[8451 - MSP2_INAV_GET_LINK_STATS](#msp2_inav_get_link_stats)  
 [8461 - MSP2_INAV_OUTPUT_MAPPING_EXT2](#msp2_inav_output_mapping_ext2)  
 [8704 - MSP2_INAV_SERVO_CONFIG](#msp2_inav_servo_config)  
 [8705 - MSP2_INAV_SET_SERVO_CONFIG](#msp2_inav_set_servo_config)  
@@ -411,11 +414,11 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i
 [8722 - MSP2_INAV_GEOZONE_VERTEX](#msp2_inav_geozone_vertex)  
 [8723 - MSP2_INAV_SET_GEOZONE_VERTEX](#msp2_inav_set_geozone_vertex)  
 [8724 - MSP2_INAV_SET_GVAR](#msp2_inav_set_gvar)  
-[8736 - MSP2_INAV_FULL_LOCAL_POSE](#msp2_inav_full_local_pose)
-[8737 - MSP2_INAV_SET_WP_INDEX](#msp2_inav_set_wp_index)
-[8739 - MSP2_INAV_SET_CRUISE_HEADING](#msp2_inav_set_cruise_heading)
-[12288 - MSP2_BETAFLIGHT_BIND](#msp2_betaflight_bind)
-[12289 - MSP2_RX_BIND](#msp2_rx_bind)
+[8736 - MSP2_INAV_FULL_LOCAL_POSE](#msp2_inav_full_local_pose)  
+[8737 - MSP2_INAV_SET_WP_INDEX](#msp2_inav_set_wp_index)  
+[8739 - MSP2_INAV_SET_CRUISE_HEADING](#msp2_inav_set_cruise_heading)  
+[12288 - MSP2_BETAFLIGHT_BIND](#msp2_betaflight_bind)  
+[12289 - MSP2_RX_BIND](#msp2_rx_bind)  
 
 ## <a id="msp_api_version"></a>`MSP_API_VERSION (1 / 0x1)`
 **Description:** Provides the MSP protocol version and the INAV API version.  
@@ -2208,7 +2211,7 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i
 | `armingFlags` | `uint16_t` | 2 | Bitmask | Bitmask: Flight controller arming flags (`armingFlags`). Note: Truncated to 16 bits |
 | `accCalibAxisFlags` | `uint8_t` | 1 | Bitmask | Bitmask: Accelerometer calibrated axes flags (`accGetCalibrationAxisFlags()`) |
 
-**Notes:** Superseded by `MSP2_INAV_STATUS` which provides the full 32-bit `armingFlags` and other enhancements.
+**Notes:** Superseded by `MSP2_INAV_STATUS` which provides the full 32-bit `armingFlags` and other enhancements. The `accCalibAxisFlags` field is not present in `MSP2_INAV_STATUS` but is available via `MSP_CALIBRATION_DATA`.
 
 ## <a id="msp_sensor_status"></a>`MSP_SENSOR_STATUS (151 / 0x97)`
 **Description:** Provides the hardware status for each individual sensor system.  
@@ -2275,6 +2278,7 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i
 | `hdop` | `uint16_t` | 2 | HDOP * 100 | Horizontal Dilution of Precision (`gpsSol.hdop`) |
 | `eph` | `uint16_t` | 2 | cm | Estimated Horizontal Position Accuracy (`gpsSol.eph`) |
 | `epv` | `uint16_t` | 2 | cm | Estimated Vertical Position Accuracy (`gpsSol.epv`) |
+| `hwVersion` | `uint32_t` | 4 | Version code | GPS hardware version (`gpsState.hwVersion`). Values: 500=UBLOX5, 600=UBLOX6, 700=UBLOX7, 800=UBLOX8, 900=UBLOX9, 1000=UBLOX10, 0=UNKNOWN |
 
 **Notes:** Requires `USE_GPS`.
 
@@ -4357,6 +4361,20 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i
 
 **Notes:** Payload length must be (OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1) + (CUSTOM_ELEMENTS_PARTS * 3) + 4 bytes including elementIndex. elementIndex must be < MAX_CUSTOM_ELEMENTS. Each partType must be < CUSTOM_ELEMENT_TYPE_END. Firmware NUL-terminates elementText internally.
 
+## <a id="msp2_inav_get_link_stats"></a>`MSP2_INAV_GET_LINK_STATS (8451 / 0x2103)`
+**Description:** Provides uplink RC link statistics for monitoring on a GCS.  
+
+**Request Payload:** **None**  
+  
+**Reply Payload:**
+|Field|C Type|Size (Bytes)|Units|Description|
+|---|---|---|---|---|
+| `uplinkRSSI_dBm` | `uint8_t` | 1 | -dBm | Uplink RSSI in dBm, sent as a positive magnitude (`getRSSI()`). For example, 70 means -70dBm. |
+| `uplinkLQ` | `uint8_t` | 1 | % | Uplink Link Quality (`rxLinkStatistics.uplinkLQ`) |
+| `uplinkSNR` | `int8_t` | 1 | dB | Uplink Signal-to-Noise Ratio (`rxLinkStatistics.uplinkSNR`) |
+
+**Notes:** Useful for GCS monitoring of the active RC link quality and signal margin.
+
 ## <a id="msp2_inav_output_mapping_ext2"></a>`MSP2_INAV_OUTPUT_MAPPING_EXT2 (8461 / 0x210d)`
 **Description:** Retrieves the full extended output mapping configuration (timer ID, full 32-bit usage flags, and pin label). Supersedes `MSP2_INAV_OUTPUT_MAPPING_EXT`.  
 
@@ -4529,33 +4547,29 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i
 **Notes:** All attitude angles are in deci-degrees.
 
 ## <a id="msp2_inav_set_wp_index"></a>`MSP2_INAV_SET_WP_INDEX (8737 / 0x2221)`
-**Description:** Jumps to a specific waypoint during an active waypoint mission, causing the aircraft to immediately begin navigating toward the new target waypoint.
-
+**Description:** Jumps to a specific waypoint during an active waypoint mission, causing the aircraft to immediately begin navigating toward the new target waypoint.  
+  
 **Request Payload:**
 |Field|C Type|Size (Bytes)|Units|Description|
 |---|---|---|---|---|
 | `wp_index` | `uint8_t` | 1 | - | 0-based waypoint index to jump to, relative to the mission start waypoint (`posControl.startWpIndex`) |
 
-**Reply Payload:** **None**
+**Reply Payload:** **None**  
 
 **Notes:** Returns error if the aircraft is not armed, `NAV_WP_MODE` is not active, or the index is outside the valid mission range (`startWpIndex` to `startWpIndex + waypointCount - 1`). On success, sets `posControl.activeWaypointIndex` to the requested index and fires `NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP`, transitioning the navigation FSM back to `NAV_STATE_WAYPOINT_PRE_ACTION` so the flight controller re-initialises navigation for the new target.
 
----
-
 ## <a id="msp2_inav_set_cruise_heading"></a>`MSP2_INAV_SET_CRUISE_HEADING (8739 / 0x2223)`
-**Description:** Sets the course heading target while Cruise or Course Hold mode is active, causing the aircraft to turn to and maintain the new heading.
-
+**Description:** Sets the course heading target while Cruise or Course Hold mode is active, causing the aircraft to turn to and maintain the new heading.  
+  
 **Request Payload:**
 |Field|C Type|Size (Bytes)|Units|Description|
 |---|---|---|---|---|
-| `heading_centidegrees` | `int32_t` | 4 | centidegrees | Target heading in centidegrees (0–35999). Values are wrapped modulo 36000 before being applied. |
+| `heading_centidegrees` | `int32_t` | 4 | centidegrees | Target heading in centidegrees (0-35999). Values are wrapped modulo 36000 before being applied. |
 
-**Reply Payload:** **None**
+**Reply Payload:** **None**  
 
 **Notes:** Returns error if the aircraft is not armed or `NAV_COURSE_HOLD_MODE` is not active. On success, sets both `posControl.cruise.course` and `posControl.cruise.previousCourse` to the normalised value, preventing spurious heading adjustments from `getCruiseHeadingAdjustment()` on the next control cycle.
 
----
-
 ## <a id="msp2_betaflight_bind"></a>`MSP2_BETAFLIGHT_BIND (12288 / 0x3000)`
 **Description:** Initiates the receiver binding procedure for supported serial protocols (CRSF, SRXL2).  
 
@@ -4566,14 +4580,14 @@ For list of enums, see [Enum documentation page](https://github.com/iNavFlight/i
 **Notes:** Requires `rxConfig()->receiverType == RX_TYPE_SERIAL`. Requires `USE_SERIALRX_CRSF` or `USE_SERIALRX_SRXL2`. Calls `crsfBind()` or `srxl2Bind()` respectively. Returns error if receiver type or provider is not supported for binding.
 
 ## <a id="msp2_rx_bind"></a>`MSP2_RX_BIND (12289 / 0x3001)`
-**Description:** Initiates binding for MSP receivers (mLRS).
-
+**Description:** Initiates binding for MSP receivers (mLRS).  
+  
 **Request Payload:**
 |Field|C Type|Size (Bytes)|Description|
 |---|---|---|---|
 | `port_id` | `uint8_t` | 1 | Port ID |
 | `reserved_for_custom_use` | `uint8_t[3]` | 3 | Reserved for custom use |
-
+  
 **Reply Payload:**
 |Field|C Type|Size (Bytes)|Description|
 |---|---|---|---|
diff --git a/docs/development/msp/gen_docs.sh b/docs/development/msp/gen_docs.sh
index cd368cf7f00..84aa852841f 100755
--- a/docs/development/msp/gen_docs.sh
+++ b/docs/development/msp/gen_docs.sh
@@ -11,7 +11,7 @@ expected="$(awk '{print $1}' msp_messages.checksum)"
 echo "Hash:" $actual
 if [[ "$actual" != "$expected" ]]; then
   n="$(cat rev)"
-  printf '%d' "$(n + 1)" > rev
+  printf '%d' "$((n + 1))" > rev
   echo "File changed, incrementing revision"
   echo $actual > msp_messages.checksum
 fi
diff --git a/docs/development/msp/inav_enums.json b/docs/development/msp/inav_enums.json
index 04a045ad3b9..63f3eb2941b 100644
--- a/docs/development/msp/inav_enums.json
+++ b/docs/development/msp/inav_enums.json
@@ -12,7 +12,8 @@
         "ACC_ICM42605": "8",
         "ACC_BMI270": "9",
         "ACC_LSM6DXX": "10",
-        "ACC_FAKE": "11",
+        "ACC_ICM45686": "11",
+        "ACC_FAKE": "12",
         "ACC_MAX": "ACC_FAKE"
     },
     "accEvent_t": {
@@ -736,6 +737,9 @@
         "CRSF_FRAMETYPE_VARIO_SENSOR": "7",
         "CRSF_FRAMETYPE_BATTERY_SENSOR": "8",
         "CRSF_FRAMETYPE_BAROMETER_ALTITUDE": "9",
+        "CRSF_FRAMETYPE_AIRSPEED_SENSOR": "10",
+        "CRSF_FRAMETYPE_RPM": "12",
+        "CRSF_FRAMETYPE_TEMP": "13",
         "CRSF_FRAMETYPE_LINK_STATISTICS": "20",
         "CRSF_FRAMETYPE_RC_CHANNELS_PACKED": "22",
         "CRSF_FRAMETYPE_ATTITUDE": "30",
@@ -760,6 +764,9 @@
         "CRSF_FRAME_GPS_INDEX": "",
         "CRSF_FRAME_VARIO_SENSOR_INDEX": "",
         "CRSF_FRAME_BAROMETER_ALTITUDE_INDEX": "",
+        "CRSF_FRAME_TEMP_INDEX": "",
+        "CRSF_FRAME_RPM_INDEX": "",
+        "CRSF_FRAME_AIRSPEED_INDEX": "",
         "CRSF_SCHEDULE_COUNT_MAX": ""
     },
     "crsrRfMode_e": {
@@ -801,53 +808,54 @@
         "DEVHW_ICM42605": "7",
         "DEVHW_BMI270": "8",
         "DEVHW_LSM6D": "9",
-        "DEVHW_MPU9250": "10",
-        "DEVHW_BMP085": "11",
-        "DEVHW_BMP280": "12",
-        "DEVHW_MS5611": "13",
-        "DEVHW_MS5607": "14",
-        "DEVHW_LPS25H": "15",
-        "DEVHW_SPL06": "16",
-        "DEVHW_BMP388": "17",
-        "DEVHW_DPS310": "18",
-        "DEVHW_B2SMPB": "19",
-        "DEVHW_HMC5883": "20",
-        "DEVHW_AK8963": "21",
-        "DEVHW_AK8975": "22",
-        "DEVHW_IST8310_0": "23",
-        "DEVHW_IST8310_1": "24",
-        "DEVHW_IST8308": "25",
-        "DEVHW_QMC5883": "26",
-        "DEVHW_QMC5883P": "27",
-        "DEVHW_MAG3110": "28",
-        "DEVHW_LIS3MDL": "29",
-        "DEVHW_RM3100": "30",
-        "DEVHW_VCM5883": "31",
-        "DEVHW_MLX90393": "32",
-        "DEVHW_LM75_0": "33",
-        "DEVHW_LM75_1": "34",
-        "DEVHW_LM75_2": "35",
-        "DEVHW_LM75_3": "36",
-        "DEVHW_LM75_4": "37",
-        "DEVHW_LM75_5": "38",
-        "DEVHW_LM75_6": "39",
-        "DEVHW_LM75_7": "40",
-        "DEVHW_DS2482": "41",
-        "DEVHW_MAX7456": "42",
-        "DEVHW_SRF10": "43",
-        "DEVHW_VL53L0X": "44",
-        "DEVHW_VL53L1X": "45",
-        "DEVHW_US42": "46",
-        "DEVHW_TOF10120_I2C": "47",
-        "DEVHW_TERARANGER_EVO_I2C": "48",
-        "DEVHW_MS4525": "49",
-        "DEVHW_DLVR": "50",
-        "DEVHW_M25P16": "51",
-        "DEVHW_W25N": "52",
-        "DEVHW_UG2864": "53",
-        "DEVHW_SDCARD": "54",
-        "DEVHW_IRLOCK": "55",
-        "DEVHW_PCF8574": "56"
+        "DEVHW_ICM45686": "10",
+        "DEVHW_MPU9250": "11",
+        "DEVHW_BMP085": "12",
+        "DEVHW_BMP280": "13",
+        "DEVHW_MS5611": "14",
+        "DEVHW_MS5607": "15",
+        "DEVHW_LPS25H": "16",
+        "DEVHW_SPL06": "17",
+        "DEVHW_BMP388": "18",
+        "DEVHW_DPS310": "19",
+        "DEVHW_B2SMPB": "20",
+        "DEVHW_HMC5883": "21",
+        "DEVHW_AK8963": "22",
+        "DEVHW_AK8975": "23",
+        "DEVHW_IST8310_0": "24",
+        "DEVHW_IST8310_1": "25",
+        "DEVHW_IST8308": "26",
+        "DEVHW_QMC5883": "27",
+        "DEVHW_QMC5883P": "28",
+        "DEVHW_MAG3110": "29",
+        "DEVHW_LIS3MDL": "30",
+        "DEVHW_RM3100": "31",
+        "DEVHW_VCM5883": "32",
+        "DEVHW_MLX90393": "33",
+        "DEVHW_LM75_0": "34",
+        "DEVHW_LM75_1": "35",
+        "DEVHW_LM75_2": "36",
+        "DEVHW_LM75_3": "37",
+        "DEVHW_LM75_4": "38",
+        "DEVHW_LM75_5": "39",
+        "DEVHW_LM75_6": "40",
+        "DEVHW_LM75_7": "41",
+        "DEVHW_DS2482": "42",
+        "DEVHW_MAX7456": "43",
+        "DEVHW_SRF10": "44",
+        "DEVHW_VL53L0X": "45",
+        "DEVHW_VL53L1X": "46",
+        "DEVHW_US42": "47",
+        "DEVHW_TOF10120_I2C": "48",
+        "DEVHW_TERARANGER_EVO_I2C": "49",
+        "DEVHW_MS4525": "50",
+        "DEVHW_DLVR": "51",
+        "DEVHW_M25P16": "52",
+        "DEVHW_W25N": "53",
+        "DEVHW_UG2864": "54",
+        "DEVHW_SDCARD": "55",
+        "DEVHW_IRLOCK": "56",
+        "DEVHW_PCF8574": "57"
     },
     "deviceFlags_e": {
         "_source": "inav/src/main/drivers/bus.h",
@@ -1527,7 +1535,8 @@
         "GYRO_ICM42605": "8",
         "GYRO_BMI270": "9",
         "GYRO_LSM6DXX": "10",
-        "GYRO_FAKE": "11"
+        "GYRO_ICM45686": "11",
+        "GYRO_FAKE": "12"
     },
     "HardwareMotorTypes_e": {
         "_source": "inav/src/main/drivers/pwm_esc_detect.h",
@@ -1861,7 +1870,8 @@
         "LED_MODE_ANGLE": "3",
         "LED_MODE_MAG": "4",
         "LED_MODE_BARO": "5",
-        "LED_SPECIAL": "6"
+        "LED_MODE_LOITER": "6",
+        "LED_SPECIAL": "7"
     },
     "ledOverlayId_e": {
         "_source": "inav/src/main/io/ledstrip.h",
@@ -2325,8 +2335,7 @@
         "MULTI_FUNC_3": "3",
         "MULTI_FUNC_4": "4",
         "MULTI_FUNC_5": "5",
-        "MULTI_FUNC_6": "6",
-        "MULTI_FUNC_END": "7"
+        "MULTI_FUNC_END": "6"
     },
     "multiFunctionFlags_e": {
         "_source": "inav/src/main/fc/multifunction.h",
@@ -2407,6 +2416,7 @@
         "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_WAYPOINT_JUMP": "NAV_FSM_EVENT_STATE_SPECIFIC_4",
         "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",
@@ -3291,9 +3301,8 @@
         "_source": "inav/src/main/io/displayport_msp_osd.c",
         "SD_3016": "0",
         "HD_5018": "1",
-        "HD_3016": "2",
-        "HD_6022": "3",
-        "HD_5320": "4"
+        "HD_6022": "2",
+        "HD_5320": "3"
     },
     "resourceOwner_e": {
         "_source": "inav/src/main/drivers/resource.h",
@@ -3829,7 +3838,7 @@
         "THR_HI": "(2 << (2 * THROTTLE))"
     },
     "systemState_e": {
-        "_source": "inav/src/main/fc/fc_init.c",
+        "_source": "inav/src/main/fc/fc_init.h",
         "SYSTEM_STATE_INITIALISING": "0",
         "SYSTEM_STATE_CONFIG_LOADED": "(1 << 0)",
         "SYSTEM_STATE_SENSORS_READY": "(1 << 1)",
diff --git a/docs/development/msp/inav_enums_ref.md b/docs/development/msp/inav_enums_ref.md
index b3aabb41467..802e813ab43 100644
--- a/docs/development/msp/inav_enums_ref.md
+++ b/docs/development/msp/inav_enums_ref.md
@@ -370,7 +370,8 @@
 | `ACC_ICM42605` | 8 |  |
 | `ACC_BMI270` | 9 |  |
 | `ACC_LSM6DXX` | 10 |  |
-| `ACC_FAKE` | 11 |  |
+| `ACC_ICM45686` | 11 |  |
+| `ACC_FAKE` | 12 |  |
 | `ACC_MAX` | ACC_FAKE |  |
 
 ---
@@ -1317,6 +1318,9 @@
 | `CRSF_FRAMETYPE_VARIO_SENSOR` | 7 |  |
 | `CRSF_FRAMETYPE_BATTERY_SENSOR` | 8 |  |
 | `CRSF_FRAMETYPE_BAROMETER_ALTITUDE` | 9 |  |
+| `CRSF_FRAMETYPE_AIRSPEED_SENSOR` | 10 |  |
+| `CRSF_FRAMETYPE_RPM` | 12 |  |
+| `CRSF_FRAMETYPE_TEMP` | 13 |  |
 | `CRSF_FRAMETYPE_LINK_STATISTICS` | 20 |  |
 | `CRSF_FRAMETYPE_RC_CHANNELS_PACKED` | 22 |  |
 | `CRSF_FRAMETYPE_ATTITUDE` | 30 |  |
@@ -1346,6 +1350,9 @@
 | `CRSF_FRAME_GPS_INDEX` |  |  |
 | `CRSF_FRAME_VARIO_SENSOR_INDEX` |  |  |
 | `CRSF_FRAME_BAROMETER_ALTITUDE_INDEX` |  |  |
+| `CRSF_FRAME_TEMP_INDEX` |  |  |
+| `CRSF_FRAME_RPM_INDEX` |  |  |
+| `CRSF_FRAME_AIRSPEED_INDEX` |  |  |
 | `CRSF_SCHEDULE_COUNT_MAX` |  |  |
 
 ---
@@ -1407,53 +1414,54 @@
 | `DEVHW_ICM42605` | 7 |  |
 | `DEVHW_BMI270` | 8 |  |
 | `DEVHW_LSM6D` | 9 |  |
-| `DEVHW_MPU9250` | 10 |  |
-| `DEVHW_BMP085` | 11 |  |
-| `DEVHW_BMP280` | 12 |  |
-| `DEVHW_MS5611` | 13 |  |
-| `DEVHW_MS5607` | 14 |  |
-| `DEVHW_LPS25H` | 15 |  |
-| `DEVHW_SPL06` | 16 |  |
-| `DEVHW_BMP388` | 17 |  |
-| `DEVHW_DPS310` | 18 |  |
-| `DEVHW_B2SMPB` | 19 |  |
-| `DEVHW_HMC5883` | 20 |  |
-| `DEVHW_AK8963` | 21 |  |
-| `DEVHW_AK8975` | 22 |  |
-| `DEVHW_IST8310_0` | 23 |  |
-| `DEVHW_IST8310_1` | 24 |  |
-| `DEVHW_IST8308` | 25 |  |
-| `DEVHW_QMC5883` | 26 |  |
-| `DEVHW_QMC5883P` | 27 |  |
-| `DEVHW_MAG3110` | 28 |  |
-| `DEVHW_LIS3MDL` | 29 |  |
-| `DEVHW_RM3100` | 30 |  |
-| `DEVHW_VCM5883` | 31 |  |
-| `DEVHW_MLX90393` | 32 |  |
-| `DEVHW_LM75_0` | 33 |  |
-| `DEVHW_LM75_1` | 34 |  |
-| `DEVHW_LM75_2` | 35 |  |
-| `DEVHW_LM75_3` | 36 |  |
-| `DEVHW_LM75_4` | 37 |  |
-| `DEVHW_LM75_5` | 38 |  |
-| `DEVHW_LM75_6` | 39 |  |
-| `DEVHW_LM75_7` | 40 |  |
-| `DEVHW_DS2482` | 41 |  |
-| `DEVHW_MAX7456` | 42 |  |
-| `DEVHW_SRF10` | 43 |  |
-| `DEVHW_VL53L0X` | 44 |  |
-| `DEVHW_VL53L1X` | 45 |  |
-| `DEVHW_US42` | 46 |  |
-| `DEVHW_TOF10120_I2C` | 47 |  |
-| `DEVHW_TERARANGER_EVO_I2C` | 48 |  |
-| `DEVHW_MS4525` | 49 |  |
-| `DEVHW_DLVR` | 50 |  |
-| `DEVHW_M25P16` | 51 |  |
-| `DEVHW_W25N` | 52 |  |
-| `DEVHW_UG2864` | 53 |  |
-| `DEVHW_SDCARD` | 54 |  |
-| `DEVHW_IRLOCK` | 55 |  |
-| `DEVHW_PCF8574` | 56 |  |
+| `DEVHW_ICM45686` | 10 |  |
+| `DEVHW_MPU9250` | 11 |  |
+| `DEVHW_BMP085` | 12 |  |
+| `DEVHW_BMP280` | 13 |  |
+| `DEVHW_MS5611` | 14 |  |
+| `DEVHW_MS5607` | 15 |  |
+| `DEVHW_LPS25H` | 16 |  |
+| `DEVHW_SPL06` | 17 |  |
+| `DEVHW_BMP388` | 18 |  |
+| `DEVHW_DPS310` | 19 |  |
+| `DEVHW_B2SMPB` | 20 |  |
+| `DEVHW_HMC5883` | 21 |  |
+| `DEVHW_AK8963` | 22 |  |
+| `DEVHW_AK8975` | 23 |  |
+| `DEVHW_IST8310_0` | 24 |  |
+| `DEVHW_IST8310_1` | 25 |  |
+| `DEVHW_IST8308` | 26 |  |
+| `DEVHW_QMC5883` | 27 |  |
+| `DEVHW_QMC5883P` | 28 |  |
+| `DEVHW_MAG3110` | 29 |  |
+| `DEVHW_LIS3MDL` | 30 |  |
+| `DEVHW_RM3100` | 31 |  |
+| `DEVHW_VCM5883` | 32 |  |
+| `DEVHW_MLX90393` | 33 |  |
+| `DEVHW_LM75_0` | 34 |  |
+| `DEVHW_LM75_1` | 35 |  |
+| `DEVHW_LM75_2` | 36 |  |
+| `DEVHW_LM75_3` | 37 |  |
+| `DEVHW_LM75_4` | 38 |  |
+| `DEVHW_LM75_5` | 39 |  |
+| `DEVHW_LM75_6` | 40 |  |
+| `DEVHW_LM75_7` | 41 |  |
+| `DEVHW_DS2482` | 42 |  |
+| `DEVHW_MAX7456` | 43 |  |
+| `DEVHW_SRF10` | 44 |  |
+| `DEVHW_VL53L0X` | 45 |  |
+| `DEVHW_VL53L1X` | 46 |  |
+| `DEVHW_US42` | 47 |  |
+| `DEVHW_TOF10120_I2C` | 48 |  |
+| `DEVHW_TERARANGER_EVO_I2C` | 49 |  |
+| `DEVHW_MS4525` | 50 |  |
+| `DEVHW_DLVR` | 51 |  |
+| `DEVHW_M25P16` | 52 |  |
+| `DEVHW_W25N` | 53 |  |
+| `DEVHW_UG2864` | 54 |  |
+| `DEVHW_SDCARD` | 55 |  |
+| `DEVHW_IRLOCK` | 56 |  |
+| `DEVHW_PCF8574` | 57 |  |
 
 ---
 ## <a id="enum-deviceflags_e"></a>`deviceFlags_e`
@@ -2488,7 +2496,8 @@
 | `GYRO_ICM42605` | 8 |  |
 | `GYRO_BMI270` | 9 |  |
 | `GYRO_LSM6DXX` | 10 |  |
-| `GYRO_FAKE` | 11 |  |
+| `GYRO_ICM45686` | 11 |  |
+| `GYRO_FAKE` | 12 |  |
 
 ---
 ## <a id="enum-hardwaremotortypes_e"></a>`HardwareMotorTypes_e`
@@ -2924,7 +2933,8 @@
 | `LED_MODE_ANGLE` | 3 |  |
 | `LED_MODE_MAG` | 4 |  |
 | `LED_MODE_BARO` | 5 |  |
-| `LED_SPECIAL` | 6 |  |
+| `LED_MODE_LOITER` | 6 |  |
+| `LED_SPECIAL` | 7 |  |
 
 ---
 ## <a id="enum-ledoverlayid_e"></a>`ledOverlayId_e`
@@ -3531,8 +3541,7 @@
 | `MULTI_FUNC_3` | 3 |  |
 | `MULTI_FUNC_4` | 4 |  |
 | `MULTI_FUNC_5` | 5 |  |
-| `MULTI_FUNC_6` | 6 |  |
-| `MULTI_FUNC_END` | 7 |  |
+| `MULTI_FUNC_END` | 6 |  |
 
 ---
 ## <a id="enum-multifunctionflags_e"></a>`multiFunctionFlags_e`
@@ -3658,6 +3667,7 @@
 | `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_WAYPOINT_JUMP` | NAV_FSM_EVENT_STATE_SPECIFIC_4 |  |
 | `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 |  |
@@ -4819,9 +4829,8 @@
 |---|---:|---|
 | `SD_3016` | 0 |  |
 | `HD_5018` | 1 |  |
-| `HD_3016` | 2 |  |
-| `HD_6022` | 3 |  |
-| `HD_5320` | 4 |  |
+| `HD_6022` | 2 |  |
+| `HD_5320` | 3 |  |
 
 ---
 ## <a id="enum-resourceowner_e"></a>`resourceOwner_e`
@@ -5627,7 +5636,7 @@
 ---
 ## <a id="enum-systemstate_e"></a>`systemState_e`
 
-> Source: ../../../src/main/fc/fc_init.h
+> Source: ../../../src/main/fc/fc_init.c
 
 | Enumerator | Value | Condition |
 |---|---:|---|
@@ -5641,7 +5650,7 @@
 ---
 ## <a id="enum-systemstate_e"></a>`systemState_e`
 
-> Source: ../../../src/main/fc/fc_init.c
+> Source: ../../../src/main/fc/fc_init.h
 
 | Enumerator | Value | Condition |
 |---|---:|---|
diff --git a/docs/development/msp/msp_messages.checksum b/docs/development/msp/msp_messages.checksum
index ff3e21a1d69..031f3b14179 100644
--- a/docs/development/msp/msp_messages.checksum
+++ b/docs/development/msp/msp_messages.checksum
@@ -1 +1 @@
-82a3d2eee9d0d1fe609363a08405ed21
+c9458e9a712b7a4f3bc9333aa7bc3dcb
diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json
index 12d67369bd7..c88be881d50 100644
--- a/docs/development/msp/msp_messages.json
+++ b/docs/development/msp/msp_messages.json
@@ -6953,6 +6953,35 @@
         "notes": "Requires `USE_CURRENT_METER`/`USE_ADC` for current-related fields; values fall back to zero when unavailable. Capacity fields are reported in the units configured by `batteryMetersConfig()->capacity_unit` (mAh or mWh).",
         "description": "Provides detailed analog sensor readings, superseding `MSP_ANALOG` with higher precision and additional fields."
     },
+    "MSP2_INAV_GET_LINK_STATS": {
+        "code": 8451,
+        "mspv": 2,
+        "request": null,
+        "reply": {
+            "payload": [
+                {
+                    "name": "uplinkRSSI_dBm",
+                    "ctype": "uint8_t",
+                    "desc": "Uplink RSSI in dBm, sent as a positive magnitude (`getRSSI()`). For example, 70 means -70dBm.",
+                    "units": "-dBm"
+                },
+                {
+                    "name": "uplinkLQ",
+                    "ctype": "uint8_t",
+                    "desc": "Uplink Link Quality (`rxLinkStatistics.uplinkLQ`)",
+                    "units": "%"
+                },
+                {
+                    "name": "uplinkSNR",
+                    "ctype": "int8_t",
+                    "desc": "Uplink Signal-to-Noise Ratio (`rxLinkStatistics.uplinkSNR`)",
+                    "units": "dB"
+                }
+            ]
+        },
+        "notes": "Useful for GCS monitoring of the active RC link quality and signal margin.",
+        "description": "Provides uplink RC link statistics for monitoring on a GCS."
+    },
     "MSP2_INAV_MISC": {
         "code": 8195,
         "mspv": 2,
@@ -10943,6 +10972,40 @@
         "notes": "All attitude angles are in deci-degrees.",
         "description": "Provides estimates of current attitude, local NEU position, and velocity."
     },
+    "MSP2_INAV_SET_WP_INDEX": {
+        "code": 8737,
+        "mspv": 2,
+        "request": {
+            "payload": [
+                {
+                    "name": "wp_index",
+                    "ctype": "uint8_t",
+                    "desc": "0-based waypoint index to jump to, relative to the mission start waypoint (`posControl.startWpIndex`)",
+                    "units": "-"
+                }
+            ]
+        },
+        "reply": null,
+        "notes": "Returns error if the aircraft is not armed, `NAV_WP_MODE` is not active, or the index is outside the valid mission range (`startWpIndex` to `startWpIndex + waypointCount - 1`). On success, sets `posControl.activeWaypointIndex` to the requested index and fires `NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP`, transitioning the navigation FSM back to `NAV_STATE_WAYPOINT_PRE_ACTION` so the flight controller re-initialises navigation for the new target.",
+        "description": "Jumps to a specific waypoint during an active waypoint mission, causing the aircraft to immediately begin navigating toward the new target waypoint."
+    },
+    "MSP2_INAV_SET_CRUISE_HEADING": {
+        "code": 8739,
+        "mspv": 2,
+        "request": {
+            "payload": [
+                {
+                    "name": "heading_centidegrees",
+                    "ctype": "int32_t",
+                    "desc": "Target heading in centidegrees (0-35999). Values are wrapped modulo 36000 before being applied.",
+                    "units": "centidegrees"
+                }
+            ]
+        },
+        "reply": null,
+        "notes": "Returns error if the aircraft is not armed or `NAV_COURSE_HOLD_MODE` is not active. On success, sets both `posControl.cruise.course` and `posControl.cruise.previousCourse` to the normalised value, preventing spurious heading adjustments from `getCruiseHeadingAdjustment()` on the next control cycle.",
+        "description": "Sets the course heading target while Cruise or Course Hold mode is active, causing the aircraft to turn to and maintain the new heading."
+    },
     "MSP2_BETAFLIGHT_BIND": {
         "code": 12288,
         "mspv": 2,
diff --git a/docs/development/msp/rev b/docs/development/msp/rev
index bf0d87ab1b2..7ed6ff82de6 100644
--- a/docs/development/msp/rev
+++ b/docs/development/msp/rev
@@ -1 +1 @@
-4
\ No newline at end of file
+5
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index b7da917520f..802442ca5b9 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -742,6 +742,12 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         sbufWriteU16(dst, getRSSI());
         break;
 
+    case MSP2_INAV_GET_LINK_STATS:
+        sbufWriteU8(dst, (uint8_t)-rxLinkStatistics.uplinkRSSI);
+        sbufWriteU8(dst, rxLinkStatistics.uplinkLQ);
+        sbufWriteU8(dst, (uint8_t)rxLinkStatistics.uplinkSNR);
+        break;
+
     case MSP_LOOP_TIME:
         sbufWriteU16(dst, gyroConfig()->looptime);
         break;
diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h
index 21ad7e3d135..7d092ed41fd 100755
--- a/src/main/msp/msp_protocol_v2_inav.h
+++ b/src/main/msp/msp_protocol_v2_inav.h
@@ -116,6 +116,8 @@
 #define MSP2_INAV_CUSTOM_OSD_ELEMENT            0x2101
 #define MSP2_INAV_SET_CUSTOM_OSD_ELEMENTS       0x2102
 
+#define MSP2_INAV_GET_LINK_STATS                0x2103
+
 #define MSP2_INAV_SERVO_CONFIG                  0x2200
 #define MSP2_INAV_SET_SERVO_CONFIG              0x2201
 

From 16b916ab3709e1383a4e0640ceb08a0a408b7f18 Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Sun, 12 Apr 2026 15:02:04 -0500
Subject: [PATCH 072/102] Add new target: PAURCF405V2

STM32F405RGT6-based flight controller by PauRC.

Hardware features:
- IMU: ICM42605/ICM42688P or BMI270 (board variant, same CS pin PC14)
- OSD: MAX7456 on SPI1
- Blackbox: 16MB SPI flash on SPI2
- Baro: SPL06/DPS310/BMP280/MS5611 (I2C1)
- 6x UART + 1x SoftSerial (PA2, shared with UART2 TX)
- 11x PWM outputs with DShot/DMAR support
- LED strip, PINIO (2 channels), airspeed ADC
- 30.5x30.5mm mounting pattern

Closes #11089
---
 src/main/target/PAURCF405V2/CMakeLists.txt |   1 +
 src/main/target/PAURCF405V2/config.c       |  28 ++++
 src/main/target/PAURCF405V2/target.c       |  48 ++++++
 src/main/target/PAURCF405V2/target.h       | 166 +++++++++++++++++++++
 4 files changed, 243 insertions(+)
 create mode 100644 src/main/target/PAURCF405V2/CMakeLists.txt
 create mode 100644 src/main/target/PAURCF405V2/config.c
 create mode 100644 src/main/target/PAURCF405V2/target.c
 create mode 100644 src/main/target/PAURCF405V2/target.h

diff --git a/src/main/target/PAURCF405V2/CMakeLists.txt b/src/main/target/PAURCF405V2/CMakeLists.txt
new file mode 100644
index 00000000000..2034128e311
--- /dev/null
+++ b/src/main/target/PAURCF405V2/CMakeLists.txt
@@ -0,0 +1 @@
+target_stm32f405xg(PAURCF405V2)
diff --git a/src/main/target/PAURCF405V2/config.c b/src/main/target/PAURCF405V2/config.c
new file mode 100644
index 00000000000..79a0eb18472
--- /dev/null
+++ b/src/main/target/PAURCF405V2/config.c
@@ -0,0 +1,28 @@
+/*
+ * 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 "fc/fc_msp_box.h"
+#include "fc/config.h"
+#include "io/piniobox.h"
+
+void targetConfiguration(void)
+{
+    pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1;
+    pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2;
+}
diff --git a/src/main/target/PAURCF405V2/target.c b/src/main/target/PAURCF405V2/target.c
new file mode 100644
index 00000000000..efc300259d5
--- /dev/null
+++ b/src/main/target/PAURCF405V2/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 <stdbool.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"
+#include "drivers/timer_def_stm32f4xx.h"
+
+timerHardware_t timerHardware[] = {
+    DEF_TIM(TIM8,  CH4,  PC9,  TIM_USE_OUTPUT_AUTO,   0, 0), // S1 D(2,7,7) UP217
+    DEF_TIM(TIM8,  CH3,  PC8,  TIM_USE_OUTPUT_AUTO,   0, 0), // S2 D(2,2,0) UP217
+    DEF_TIM(TIM1,  CH3N, PB15, TIM_USE_OUTPUT_AUTO,   0, 0), // S3 D(2,6,0) UP256
+    DEF_TIM(TIM1,  CH1,  PA8,  TIM_USE_OUTPUT_AUTO,   0, 2), // S4 D(2,3,6) UP256
+
+    DEF_TIM(TIM2,  CH4,  PB11, TIM_USE_OUTPUT_AUTO,   0, 0), // S5 D(1,7,3) UP173
+    DEF_TIM(TIM2,  CH3,  PB10, TIM_USE_OUTPUT_AUTO,   0, 0), // S6 D(1,1,3) UP173
+    DEF_TIM(TIM2,  CH2,  PB3,  TIM_USE_OUTPUT_AUTO,   0, 0), // S7 D(1,6,3) UP173
+    DEF_TIM(TIM2,  CH1,  PA15, TIM_USE_OUTPUT_AUTO,   0, 0), // S8 D(1,5,3) UP173
+
+    DEF_TIM(TIM12, CH1,  PB14, TIM_USE_OUTPUT_AUTO,   0, 0), // S9  DMA NONE
+    DEF_TIM(TIM13, CH1,  PA6,  TIM_USE_OUTPUT_AUTO,   0, 0), // S10 DMA NONE
+    DEF_TIM(TIM4,  CH1,  PB6,  TIM_USE_OUTPUT_AUTO,   0, 0), // S11 D(1,0,2)
+
+    DEF_TIM(TIM3,  CH4,  PB1,  TIM_USE_LED,    0, 0), // 2812LED  D(1,2,5)
+    DEF_TIM(TIM11, CH1,  PB9,  TIM_USE_BEEPER, 0, 0), // BEEPER PWM
+
+    DEF_TIM(TIM5,  CH3,  PA2,  TIM_USE_ANY,    0, 0), //TX2  softserial1_Tx  ISR-driven, does not use DMA1_Stream0
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
diff --git a/src/main/target/PAURCF405V2/target.h b/src/main/target/PAURCF405V2/target.h
new file mode 100644
index 00000000000..67813a2c511
--- /dev/null
+++ b/src/main/target/PAURCF405V2/target.h
@@ -0,0 +1,166 @@
+/*
+ * 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 "PA42"
+#define USBD_PRODUCT_STRING  "PauRcF405v2"
+
+#define LED0                    PA14  //Blue
+#define LED1                    PA13  //Green
+
+#define BEEPER                  PB9
+#define BEEPER_INVERTED
+#define BEEPER_PWM_FREQUENCY    2500
+
+// *************** SPI1 IMU & OSD *******************
+#define USE_SPI
+#define USE_SPI_DEVICE_1
+
+#define SPI1_SCK_PIN            PA5
+#define SPI1_MISO_PIN           PB4
+#define SPI1_MOSI_PIN           PA7
+
+// Board ships with either ICM42605/ICM42688P or BMI270 - same CS pin (PC14)
+#define USE_IMU_ICM42605
+#define IMU_ICM42605_ALIGN      CW270_DEG_FLIP
+#define ICM42605_SPI_BUS        BUS_SPI1
+#define ICM42605_CS_PIN         PC14
+
+#define USE_IMU_BMI270
+#define IMU_BMI270_ALIGN        CW270_DEG_FLIP
+#define BMI270_SPI_BUS          BUS_SPI1
+#define BMI270_CS_PIN           PC14
+
+
+
+#define USE_MAX7456
+#define MAX7456_SPI_BUS         BUS_SPI1
+#define MAX7456_CS_PIN          PB12
+
+// *************** SPI2 Flash ****************
+#define USE_SPI_DEVICE_2
+#define SPI2_SCK_PIN            PB13
+#define SPI2_MISO_PIN           PC2
+#define SPI2_MOSI_PIN           PC3
+
+#define USE_FLASHFS
+#define USE_FLASH_M25P16
+#define M25P16_SPI_BUS          BUS_SPI2
+#define M25P16_CS_PIN           PC13
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+
+// *************** I2C /Baro/Mag *********************
+#define USE_I2C
+#define USE_I2C_DEVICE_1
+#define I2C1_SCL                PB8
+#define I2C1_SDA                PB7
+
+#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
+
+#define USE_RANGEFINDER
+#define RANGEFINDER_I2C_BUS     BUS_I2C1
+#define PITOT_I2C_BUS           BUS_I2C1
+#define TEMPERATURE_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            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
+
+#define USE_SOFTSERIAL1
+#define SOFTSERIAL_1_TX_PIN      PA2  // Shared with UART2_TX - mutually exclusive with UART2
+#define SOFTSERIAL_1_RX_PIN      PA2  // Shared with UART2_TX - mutually exclusive with UART2
+
+#define SERIAL_PORT_COUNT       8
+
+#define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER       SERIALRX_CRSF
+#define SERIALRX_UART           SERIAL_PORT_USART2
+
+// *************** ADC ***************************
+#define USE_ADC
+#define ADC_INSTANCE                ADC1
+#define ADC1_DMA_STREAM             DMA2_Stream4
+#define ADC_CHANNEL_1_PIN           PC4
+#define ADC_CHANNEL_2_PIN           PC5
+#define ADC_CHANNEL_3_PIN           PB0
+#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
+
+// *************** PINIO ***************************
+#define USE_PINIO
+#define USE_PINIOBOX
+#define PINIO1_PIN                  PA4
+#define PINIO2_PIN                  PB5
+
+// *************** LEDSTRIP ************************
+#define USE_LED_STRIP
+#define WS2811_PIN                  PB1
+
+// *************** others  ************************
+#define DEFAULT_FEATURES   (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX)
+#define VBAT_SCALE_DEFAULT      2100
+#define CURRENT_METER_SCALE     150
+
+#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
+
+#define USE_SERIAL_4WAY_BLHELI_INTERFACE
+#define USE_DSHOT
+#define USE_DSHOT_DMAR
+#define USE_ESC_SENSOR
+

From 0c240ec25321644f43629c9d654356c788d19567 Mon Sep 17 00:00:00 2001
From: Leander-vh <Leander@Vickyenleander.be>
Date: Mon, 13 Apr 2026 23:12:02 +0200
Subject: [PATCH 073/102] Added ledstrip support for FlyingRCF4Wing target thx
 to 3sr3v3r

---
 src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.c | 2 +-
 src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.h | 4 ++++
 2 files changed, 5 insertions(+), 1 deletion(-)

diff --git a/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.c b/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.c
index d86bbb17379..2962c394a3e 100644
--- a/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.c
+++ b/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.c
@@ -34,7 +34,7 @@ timerHardware_t timerHardware[] = {
     DEF_TIM(TIM2,  CH4,  PB11, TIM_USE_OUTPUT_AUTO,   0, 0), // S5 D(1,7,3) UP173
     DEF_TIM(TIM2,  CH3,  PB10, TIM_USE_OUTPUT_AUTO,   0, 0), // S6 D(1,1,3) UP173
 
-    DEF_TIM(TIM3,  CH4,  PB1,  TIM_USE_OUTPUT_AUTO,    0, 0), // 2812LED  D(1,2,5)
+    DEF_TIM(TIM3,  CH4,  PB1,  TIM_USE_LED,    0, 0), // 2812LED  D(1,2,5)
 
     DEF_TIM(TIM5,  CH3,  PA2,  TIM_USE_ANY,    0, 0), //TX2  softserial1_Tx
 };
diff --git a/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.h b/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.h
index cf98fcaca14..8eecb2c466c 100644
--- a/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.h
+++ b/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.h
@@ -95,6 +95,10 @@
 #define ADC_CHANNEL_1_PIN           PC4
 #define VBAT_ADC_CHANNEL            ADC_CHN_1
 
+// *************** LEDSTRIP ************************
+#define USE_LED_STRIP
+#define WS2811_PIN PB1
+
 // *************** others  ************************
 #define DEFAULT_FEATURES   (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX)
 #define VBAT_SCALE_DEFAULT      2100

From c49b564e448934bcf5606e095e9e151c85293a54 Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Mon, 13 Apr 2026 17:34:02 -0500
Subject: [PATCH 074/102] PAURCF405V2: correct IMU alignment

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

diff --git a/src/main/target/PAURCF405V2/target.h b/src/main/target/PAURCF405V2/target.h
index 67813a2c511..658ee136d4f 100644
--- a/src/main/target/PAURCF405V2/target.h
+++ b/src/main/target/PAURCF405V2/target.h
@@ -38,12 +38,12 @@
 
 // Board ships with either ICM42605/ICM42688P or BMI270 - same CS pin (PC14)
 #define USE_IMU_ICM42605
-#define IMU_ICM42605_ALIGN      CW270_DEG_FLIP
+#define IMU_ICM42605_ALIGN      CW0_DEG
 #define ICM42605_SPI_BUS        BUS_SPI1
 #define ICM42605_CS_PIN         PC14
 
 #define USE_IMU_BMI270
-#define IMU_BMI270_ALIGN        CW270_DEG_FLIP
+#define IMU_BMI270_ALIGN        CW180_DEG
 #define BMI270_SPI_BUS          BUS_SPI1
 #define BMI270_CS_PIN           PC14
 

From 9d2484289f67a7cc33c8b199f2391a3eca12f4f2 Mon Sep 17 00:00:00 2001
From: Daria Cacic <daria.cacic@orqafpv.com>
Date: Tue, 14 Apr 2026 14:20:47 +0200
Subject: [PATCH 075/102] Removed SpiModeOverride function

---
 src/main/drivers/max7456.c | 15 ---------------
 1 file changed, 15 deletions(-)

diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c
index 9cb2750de4d..73493b50f6e 100644
--- a/src/main/drivers/max7456.c
+++ b/src/main/drivers/max7456.c
@@ -304,20 +304,6 @@ static void max7456ApplyBusSpeed(void)
 #endif
 }
 
-// Used when standard DEVFLAGS_SPI_MODE_0 is omitted in common_hardware 
-static void max7456SpiModeOverride(void)
-{
-#if defined(STM32H7) && defined(MAX7456_MANUAL_SPI_CONFIG)
-    SPI_TypeDef *maxSpiInstance = spiInstanceByDevice(state.dev->busdev.spi.spiBus);
-    if (!maxSpiInstance){
-        return;
-    }
-   
-    maxSpiInstance->CR1 &= ~SPI_CR1_SPE;
-    maxSpiInstance->CFG2 |= (SPI_CFG2_CPHA | SPI_CFG2_CPOL);
-#endif
-}
-
 uint16_t max7456GetScreenSize(void)
 {
     // Default to PAL while the display is not yet initialized. This
@@ -412,7 +398,6 @@ void max7456Init(const videoSystem_e videoSystem)
     }
 
     max7456ApplyBusSpeed();
-    max7456SpiModeOverride(); 
 
     // force soft reset on Max7456
     busWrite(state.dev, MAX7456ADD_VM0, MAX7456_RESET);

From 2ed109625bc2408401378d83bca8aca324e897ba Mon Sep 17 00:00:00 2001
From: RunnyCow <thankyous@gmail.com>
Date: Fri, 17 Apr 2026 12:21:48 +0800
Subject: [PATCH 076/102] Flash supports MX35LF2G model

---
 src/main/drivers/flash.h      |  3 +++
 src/main/drivers/flash_w25n.c | 46 ++++++++++++++++++++++++++++-------
 2 files changed, 40 insertions(+), 9 deletions(-)

diff --git a/src/main/drivers/flash.h b/src/main/drivers/flash.h
index 4dfa8cb1953..abe1f79545f 100644
--- a/src/main/drivers/flash.h
+++ b/src/main/drivers/flash.h
@@ -40,6 +40,9 @@ typedef struct flashGeometry_s {
     uint32_t totalSize;    // This is just sectorSize * sectors
     uint16_t pagesPerSector;
     flashType_e flashType;
+    int32_t bbReplacementBlocks;
+    uint8_t bufReadModeSet;
+    uint8_t bblutTableEntryCount;
 } flashGeometry_t;
 
 typedef struct
diff --git a/src/main/drivers/flash_w25n.c b/src/main/drivers/flash_w25n.c
index 2791d3d89e1..9590a4dcecd 100644
--- a/src/main/drivers/flash_w25n.c
+++ b/src/main/drivers/flash_w25n.c
@@ -23,7 +23,7 @@
 
 #include "platform.h"
 
-#if defined(USE_FLASH_W25N01G) || defined(USE_FLASH_W25N02K)
+#if defined(USE_FLASH_W25N01G) || defined(USE_FLASH_W25N02K)|| defined(USE_FLASH_MX35LF2G)
 
 #include "drivers/bus.h"
 #include "drivers/io.h"
@@ -37,12 +37,12 @@
 
 #define W25N01GV_BLOCKS_PER_DIE  1024
 #define W25N02KV_BLOCKS_PER_DIE   2048
-
+#define MX35LF2G_BLOCKS_PER_DIE   2048
 
 
 // BB replacement area
 #define W25N_BB_MARKER_BLOCKS      1
-#define W25N_BB_REPLACEMENT_BLOCKS 20
+#define W25N_BB_REPLACEMENT_BLOCKS (geometry.bbReplacementBlocks)
 #define W25N_BB_MANAGEMENT_BLOCKS  (W25N_BB_REPLACEMENT_BLOCKS + W25N_BB_MARKER_BLOCKS)
 
 // blocks are zero-based index
@@ -53,9 +53,9 @@
 // Instructions
 #define W25N_INSTRUCTION_RDID                       0x9F
 #define W25N_INSTRUCTION_DEVICE_RESET               0xFF
-#define W25N_INSTRUCTION_READ_STATUS_REG            0x05
+#define W25N_INSTRUCTION_READ_STATUS_REG            g_readStatusReg
 #define W25N_INSTRUCTION_READ_STATUS_ALTERNATE_REG  0x0F
-#define W25N_INSTRUCTION_WRITE_STATUS_REG           0x01
+#define W25N_INSTRUCTION_WRITE_STATUS_REG           g_writeStatusReg
 #define W25N_INSTRUCTION_WRITE_STATUS_ALTERNATE_REG 0x1F
 #define W25N_INSTRUCTION_WRITE_ENABLE               0x06
 #define W25N_INSTRUCTION_DIE_SELECT                 0xC2
@@ -99,7 +99,7 @@
 #define W25N_STATUS_ERASE_FAIL         (1 << 2)
 #define W25N_STATUS_FLAG_WRITE_ENABLED (1 << 1)
 #define W25N_STATUS_FLAG_BUSY          (1 << 0)
-#define W25N_BBLUT_TABLE_ENTRY_COUNT    20
+#define W25N_BBLUT_TABLE_ENTRY_COUNT    geometry.bblutTableEntryCount
 #define W25N_BBLUT_TABLE_ENTRY_SIZE     4  // in bytes
 
 // Bits in LBA for BB LUT
@@ -110,8 +110,8 @@
 // Some useful defs and macros
 #define W25N_LINEAR_TO_COLUMN(laddr) ((laddr) % W25N_PAGE_SIZE)
 #define W25N_LINEAR_TO_PAGE(laddr) ((laddr) / W25N_PAGE_SIZE)
-#define W25N_LINEAR_TO_BLOCK(laddr) (W25N_LINEAR_TO_PAGE(laddr) / W25N_PAGES_PER_BLOCK)
-#define W25N_BLOCK_TO_PAGE(block) ((block) * W25N_PAGES_PER_BLOCK)
+#define W25N_LINEAR_TO_BLOCK(laddr) (W25N_LINEAR_TO_PAGE(laddr) / geometry.pagesPerSector)
+#define W25N_BLOCK_TO_PAGE(block) ((block) * geometry.pagesPerSector)
 #define W25N_BLOCK_TO_LINEAR(block) (W25N_BLOCK_TO_PAGE(block) * W25N_PAGE_SIZE)
 
 // IMPORTANT: Timeout values are currently required to be set to the highest value required by any of the supported flash chips by this driver
@@ -126,9 +126,13 @@
 #define W28N_STATUS_PAGE_ADDRESS_SIZE    16
 #define W28N_STATUS_COLUMN_ADDRESS_SIZE  16
 
+static uint8_t g_readStatusReg;
+static uint8_t g_writeStatusReg;
+
 // JEDEC ID
 #define JEDEC_ID_WINBOND_W25N01GV 0xEFAA21
 #define JEDEC_ID_WINBOND_W25N02KV 0xEFAA22
+#define JEDEC_ID_MACRONIX_MX35LF2G 0xC22603
 
 static busDevice_t *busDev = NULL;
 static flashGeometry_t geometry;
@@ -242,19 +246,43 @@ bool w25n_detect(uint32_t chipID)
         geometry.sectors = W25N01GV_BLOCKS_PER_DIE;      // Blocks
         geometry.pagesPerSector = W25N_PAGES_PER_BLOCK;  // Pages/Blocks
         geometry.pageSize = W25N_PAGE_SIZE;
+        g_readStatusReg = 0x05;
+        g_writeStatusReg = 0x01;
+        geometry.bbReplacementBlocks = 20;
+        geometry.bufReadModeSet = 0x02;
+        geometry.bblutTableEntryCount = 20;
         break;
     case JEDEC_ID_WINBOND_W25N02KV:
         geometry.sectors = W25N02KV_BLOCKS_PER_DIE;      // Blocks
         geometry.pagesPerSector = W25N_PAGES_PER_BLOCK;  // Pages/Blocks
         geometry.pageSize = W25N_PAGE_SIZE;
+        g_readStatusReg = 0x05;
+        g_writeStatusReg = 0x01;
+        geometry.bbReplacementBlocks = 20;
+        geometry.bufReadModeSet = 0x02;
+        geometry.bblutTableEntryCount = 20;
+        break;
+    case JEDEC_ID_MACRONIX_MX35LF2G:
+        geometry.sectors = MX35LF2G_BLOCKS_PER_DIE;      // Blocks
+        geometry.pagesPerSector = W25N_PAGES_PER_BLOCK;  // Pages/Blocks
+        geometry.pageSize = W25N_PAGE_SIZE;
+        g_readStatusReg = 0x0F;
+        g_writeStatusReg = 0x1F;
+        geometry.bbReplacementBlocks = 40;
+        geometry.bufReadModeSet = 0;
+        geometry.bblutTableEntryCount = 40;
         break;
-
     default:
         // Unsupported chip
         geometry.sectors = 0;
         geometry.pagesPerSector = 0;
         geometry.sectorSize = 0;
         geometry.totalSize = 0;
+        g_readStatusReg = 0;
+        g_writeStatusReg = 0;
+        geometry.bbReplacementBlocks = 0;
+        geometry.bufReadModeSet = 0;
+        geometry.bblutTableEntryCount = 0;
         return false;
     }
 

From 52f0d682c539046467a41692e734a30cb81ba2fd Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Fri, 17 Apr 2026 15:08:51 +0100
Subject: [PATCH 077/102] Clean up and fix OSD stats

---
 src/main/io/osd.c | 329 +++++++++++++++++++++-------------------------
 1 file changed, 153 insertions(+), 176 deletions(-)

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index f4fe19a4040..ce3f16f575a 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -244,11 +244,7 @@ bool osdDisplayIsPAL(void)
 
 bool osdDisplayIsHD(void)
 {
-    if (displayScreenSize(osdDisplayPort) >= VIDEO_BUFFER_CHARS_HDZERO)
-    {
-        return true;
-    }
-    return false;
+    return displayScreenSize(osdDisplayPort) >= VIDEO_BUFFER_CHARS_HDZERO;
 }
 
 bool osdIsNotMetric(void) {
@@ -4636,10 +4632,11 @@ uint8_t drawStat_Stats(uint8_t statNameX, uint8_t row, uint8_t statValueX, bool
     char string_buffer[osdDisplayPort->cols - statValueX];
 
     if (statsConfig()->stats_enabled) {
-        if (isBootStats)
+        if (isBootStats) {
             displayWrite(osdDisplayPort, statNameX, row, "ODOMETER:");
-        else
+        } else {
             displayWrite(osdDisplayPort, statNameX, row, "ODOMETER");
+        }
 
         switch (osdConfig()->units) {
             case OSD_UNIT_UK:
@@ -4687,25 +4684,27 @@ uint8_t drawStat_Stats(uint8_t statNameX, uint8_t row, uint8_t statValueX, bool
         string_buffer[buffLen] = '\0';
         displayWrite(osdDisplayPort, statValueX-(isBootStats ? 5 : 0), row,  string_buffer);
 
-        if (isBootStats)
+        if (isBootStats) {
             displayWrite(osdDisplayPort, statNameX, ++row, "TOTAL TIME:");
-        else
+        } else {
             displayWrite(osdDisplayPort, statNameX, ++row, "TOTAL TIME");
+        }
 
         uint32_t tot_mins = statsConfig()->stats_total_time / 60;
-        if (isBootStats)
+        if (isBootStats) {
             tfp_sprintf(string_buffer, "%d:%02dH:M%c", (int)(tot_mins / 60), (int)(tot_mins % 60), '\0');
-        else
+        } 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
         if (feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER) && statsConfig()->stats_total_energy) {
             uint8_t buffOffset = 0;
-            if (isBootStats)
+            if (isBootStats) {
                 displayWrite(osdDisplayPort, statNameX, ++row, "TOTAL ENERGY:");
-            else {
+            } else {
                 displayWrite(osdDisplayPort, statNameX, ++row, "TOTAL ENERGY");
                 string_buffer[0] = ':';
                 buffOffset = 2;
@@ -4723,9 +4722,9 @@ uint8_t drawStat_Stats(uint8_t statNameX, uint8_t row, uint8_t statValueX, bool
             }
 
             if (statsConfig()->stats_total_dist) {
-                if (isBootStats)
+                if (isBootStats) {
                     displayWrite(osdDisplayPort, statNameX, ++row, "AVG EFFICIENCY:");
-                else {
+                } else {
                     displayWrite(osdDisplayPort, statNameX, ++row, "AVG EFFICIENCY");
                     strcat(avgEffBuff, ": ");
                 }
@@ -4751,10 +4750,11 @@ uint8_t drawStat_Stats(uint8_t statNameX, uint8_t row, uint8_t statValueX, bool
                         break;
                 }
 
-                if (isBootStats)
+                if (isBootStats) {
                     strcat(avgEffBuff, string_buffer);
-                else
+                } else {
                     strcat(avgEffBuff, osdFormatTrimWhiteSpace(string_buffer));
+                }
             } else {
                 strcat(avgEffBuff, "----");
             }
@@ -4846,8 +4846,7 @@ static void osdCompleteAsyncInitialization(void)
 
 void osdInit(displayPort_t *osdDisplayPortToUse)
 {
-    if (!osdDisplayPortToUse)
-        return;
+    if (!osdDisplayPortToUse) return;
 
     BUILD_BUG_ON(OSD_POS_MAX != OSD_POS(63,63));
 
@@ -4896,23 +4895,17 @@ static void osdUpdateStats(void)
         value = osdGet3DSpeed();
         const float airspeed_estimate = getAirspeedEstimate();
 
-        if (stats.max_3D_speed < value)
-            stats.max_3D_speed = value;
+        if (stats.max_3D_speed < value) stats.max_3D_speed = value;
 
-        if (stats.max_speed < gpsSol.groundSpeed)
-            stats.max_speed = gpsSol.groundSpeed;
+        if (stats.max_speed < gpsSol.groundSpeed) stats.max_speed = gpsSol.groundSpeed;
 
-        if (stats.max_air_speed < airspeed_estimate)
-            stats.max_air_speed = airspeed_estimate;
+        if (stats.max_air_speed < airspeed_estimate) stats.max_air_speed = airspeed_estimate;
 
-        if (stats.max_distance < GPS_distanceToHome)
-            stats.max_distance = GPS_distanceToHome;
+        if (stats.max_distance < GPS_distanceToHome) stats.max_distance = GPS_distanceToHome;
 
-        if (stats.min_sats > gpsSol.numSat)
-            stats.min_sats = gpsSol.numSat;
+        if (stats.min_sats > gpsSol.numSat) stats.min_sats = gpsSol.numSat;
 
-        if (stats.max_sats < gpsSol.numSat)
-            stats.max_sats = gpsSol.numSat;
+        if (stats.max_sats < gpsSol.numSat) stats.max_sats = gpsSol.numSat;
     }
 #if defined(USE_ESC_SENSOR)
     if (STATE(ESC_SENSOR_ENABLED)) {
@@ -4920,41 +4913,31 @@ static void osdUpdateStats(void)
         bool escTemperatureValid = escSensor && escSensor->dataAge <= ESC_DATA_MAX_AGE;
 
         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;
+            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;
         }
     }
 #endif
 
     value = getBatteryVoltage();
-    if (stats.min_voltage > value)
-        stats.min_voltage = value;
+    if (stats.min_voltage > value) stats.min_voltage = value;
 
     value = abs(getAmperage());
-    if (stats.max_current < value)
-        stats.max_current = value;
+    if (stats.max_current < value) stats.max_current = value;
 
     value = labs(getPower());
-    if (stats.max_power < value)
-        stats.max_power = value;
+    if (stats.max_power < value) stats.max_power = value;
 
     value = osdConvertRSSI();
-    if (stats.min_rssi > value)
-        stats.min_rssi = value;
+    if (stats.min_rssi > value) stats.min_rssi = value;
 
     value = osdGetCrsfLQ();
-    if (stats.min_lq > value)
-        stats.min_lq = value;
+    if (stats.min_lq > value) stats.min_lq = value;
 
-    if (!failsafeIsReceivingRxData())
-        stats.min_lq = 0;
+    if (!failsafeIsReceivingRxData()) stats.min_lq = 0;
 
     value = osdGetCrsfdBm();
-    if (stats.min_rssi_dbm > value)
-        stats.min_rssi_dbm = value;
+    if (stats.min_rssi_dbm > value) stats.min_rssi_dbm = value;
 
     stats.max_altitude = MAX(stats.max_altitude, osdGetAltitude());
 }
@@ -4992,10 +4975,10 @@ uint8_t drawStat_MaxDistanceFromHome(uint8_t col, uint8_t row, uint8_t statValX)
     uint8_t valueXOffset = 0;
     if (!osdDisplayIsHD()) {
         displayWrite(osdDisplayPort, col, row, "DISTANCE FROM ");
-    valueXOffset = 14;
+        valueXOffset = 14;
     } else {
         displayWrite(osdDisplayPort, col, row, "MAX DISTANCE FROM ");
-    valueXOffset = 18;
+        valueXOffset = 18;
     }
     displayWriteChar(osdDisplayPort, col + valueXOffset, row, SYM_HOME);
     tfp_sprintf(buff, ": ");
@@ -5039,10 +5022,11 @@ uint8_t drawStat_BatteryVoltage(uint8_t col, uint8_t row, uint8_t statValX)
 {
     char buff[12];
     uint8_t multiValueXOffset = 0;
-    if (!osdDisplayIsHD())
+    if (!osdDisplayIsHD()) {
         displayWrite(osdDisplayPort, col, row, "MIN VOLTS P/C");
-    else
+    } else {
         displayWrite(osdDisplayPort, col, row, "MIN VOLTS PACK/CELL");
+    }
 
     // Pack voltage
     tfp_sprintf(buff, ": ");
@@ -5070,10 +5054,11 @@ uint8_t drawStat_MaximumPowerAndCurrent(uint8_t col, uint8_t row, uint8_t statVa
     strcat(outBuff, osdFormatTrimWhiteSpace(buff));
     displayWrite(osdDisplayPort, statValX, row, outBuff);
 
-    if (kiloWatt)
+    if (kiloWatt) {
         displayWrite(osdDisplayPort, col, row, "MAX AMPS/K WATTS");
-    else
+    } else {
         displayWrite(osdDisplayPort, col, row, "MAX AMPS/WATTS");
+    }
 
     return ++row;
 }
@@ -5082,10 +5067,12 @@ uint8_t drawStat_UsedEnergy(uint8_t col, uint8_t row, uint8_t statValX)
 {
     char    buff[12];
 
-    if (osdDisplayIsHD())
+    if (osdDisplayIsHD()) {
         displayWrite(osdDisplayPort, col, row, "USED ENERGY FLT/TOT");
-    else
+    } else {
         displayWrite(osdDisplayPort, col, row, "USED ENERGY F/T");
+    }
+
     tfp_sprintf(buff, ": ");
     if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) {
         tfp_sprintf(buff + 2, "%d/%d%c", (int)(getMAhDrawn() - stats.flightStartMAh), (int)getMAhDrawn(), SYM_MAH);
@@ -5111,10 +5098,11 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b
     bool moreThanAh = false;
     bool efficiencyValid = totalDistance >= 10000;
 
-    if (osdDisplayIsHD())
+    if (osdDisplayIsHD()) {
         displayWrite(osdDisplayPort, col, row, "AVG EFFICIENCY FLT/TOT");
-    else
+    } else {
         displayWrite(osdDisplayPort, col, row, "AV EFFICIENCY F/T");
+    }
 
     tfp_sprintf(outBuff, ": ");
     uint8_t digits = 3U;    // Total number of digits (including decimal point)
@@ -5134,10 +5122,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_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;
                         }
@@ -5146,10 +5135,11 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b
                         moreThanAh = moreThanAh || osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_MILE / totalDistance), 1000, 0, 2, digits, false);
                         strcat(outBuff, osdFormatTrimWhiteSpace(buff));
 
-                        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);
+                        }
                     } else {
                         tfp_sprintf(outBuff + strlen(outBuff), "---/---%c%c", SYM_MAH_MI_0, SYM_MAH_MI_1);
                     }
@@ -5172,10 +5162,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;
                         }
@@ -5217,10 +5208,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;
                 }
@@ -5259,14 +5251,12 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b
 uint8_t drawStat_RXStats(uint8_t col, uint8_t row, uint8_t statValX)
 {
     char buff[20];
-    uint8_t multiValueXOffset = 0;
 
     tfp_sprintf(buff, "MIN RSSI");
     if (rxConfig()->serialrx_provider == SERIALRX_CRSF) {
         strcat(buff, "/LQ");
 
-        if (osdDisplayIsHD())
-            strcat(buff, "/DBM");
+        if (osdDisplayIsHD()) strcat(buff, "/DBM");
     }
     displayWrite(osdDisplayPort, col, row, buff);
 
@@ -5277,15 +5267,13 @@ uint8_t drawStat_RXStats(uint8_t col, uint8_t row, uint8_t statValX)
 
     if (rxConfig()->serialrx_provider == SERIALRX_CRSF) {
         strcat(osdFormatTrimWhiteSpace(buff), "/");
-        multiValueXOffset = strlen(buff);
-        itoa(stats.min_lq, buff + multiValueXOffset, 10);
+        itoa(stats.min_lq, buff + strlen(buff), 10);
         strcat(osdFormatTrimWhiteSpace(buff), "%");
 
         if (osdDisplayIsHD()) {
             strcat(osdFormatTrimWhiteSpace(buff), "/");
-            itoa(stats.min_rssi_dbm, buff + 2, 10);
+            itoa(stats.min_rssi_dbm, buff + strlen(buff), 10);
             tfp_sprintf(buff + strlen(buff), "%c", SYM_DBM);
-            displayWrite(osdDisplayPort, statValX, row++, buff);
         }
     }
 
@@ -5336,10 +5324,11 @@ uint8_t drawStat_GForce(uint8_t col, uint8_t row, uint8_t statValX)
     const float acc_extremes_min = acc_extremes[Z].min;
     const float acc_extremes_max = acc_extremes[Z].max;
 
-    if (!osdDisplayIsHD())
+    if (!osdDisplayIsHD()) {
         displayWrite(osdDisplayPort, col, row, "MAX G-FORCE");
-    else
-        displayWrite(osdDisplayPort, col, row, "MAX/MIN Z/MAX Z 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);
@@ -5380,23 +5369,18 @@ uint8_t drawStat_DisarmMethod(uint8_t col, uint8_t row, uint8_t statValX)
 
 static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
 {
-    const char * statsHeader[2] = {"*** STATS   1/2 -> ***", "*** STATS   <- 2/2 ***"};
-    uint8_t row = 1;  // Start one line down leaving space at the top of the screen.
+    uint8_t row = 1;  // Start one line down leaving space at the top of the screen (Top row = index 0)
 
     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;
-
     displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING);
     displayClearScreen(osdDisplayPort);
 
     if (isSinglePageStatsCompatible) {
         char buff[25];
         tfp_sprintf(buff, "*** STATS ");
-#ifdef USE_BLACKBOX
-#ifdef USE_SDCARD
+#if defined(USE_BLACKBOX) && defined(USE_SDCARD)
         if (feature(FEATURE_BLACKBOX)) {
             int32_t logNumber = blackboxGetLogNumber();
             if (logNumber >= 0)
@@ -5404,16 +5388,19 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
             else
                 tfp_sprintf(buff + strlen(buff), " %c ", SYM_BLACKBOX);
         }
-#endif
 #endif
         strcat(buff, "***");
 
         displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buff)) / 2, row++, buff);
-    } else
-        displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(statsHeader[page + 1])) / 2, row++, statsHeader[page]);
+    } else {
+        const char * statsHeader[2] = {"*** STATS   1/2 -> ***", "*** STATS   <- 2/2 ***"};
+
+        if (page > 1) page = 0;
+        displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(statsHeader[page])) / 2, row++, statsHeader[page]);
+    }
 
     if (isSinglePageStatsCompatible) {
-        // Top 15 rows for most important stats. Max 19 rows (WTF)
+        /* 13 rows used for most important stats */
         row = drawStat_FlightTime(statNameX, row, statValuesX); // 1 row
         row = drawStat_FlightDistance(statNameX, row, statValuesX); // 1 row
         if (feature(FEATURE_GPS)) row = drawStat_MaxDistanceFromHome(statNameX, row, statValuesX); // 1 row
@@ -5424,67 +5411,63 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
         if (feature(FEATURE_CURRENT_METER)) row = drawStat_UsedEnergy(statNameX, row, statValuesX); // 1 row
         if (feature(FEATURE_CURRENT_METER) && feature(FEATURE_GPS)) row = drawStat_AverageEfficiency(statNameX, row, statValuesX, false); // 1 row
         if (osdConfig()->stats_show_metric_efficiency && osdIsNotMetric() && feature(FEATURE_CURRENT_METER) && feature(FEATURE_GPS)) row = drawStat_AverageEfficiency(statNameX, row, statValuesX, true); // 1 row
-        row = drawStat_RXStats(statNameX, row, statValuesX); // 1 row if non-CRSF else 2 rows
+        row = drawStat_RXStats(statNameX, row, statValuesX); // 1 row
         if (feature(FEATURE_GPS)) row = drawStat_GPS(statNameX, row, statValuesX); // 1 row
         if (STATE(ESC_SENSOR_ENABLED)) row = drawStat_ESCTemperature(statNameX, row, statValuesX); // 1 row
 
-        // Draw these if there is space space
-        if (row < (osdDisplayPort->cols-3)) row = drawStat_GForce(statNameX, row, statValuesX); // 1 row HD or 2 rows SD
+        /* Additional stats added if space available whilst leaving 3 rows available at the bottom for
+         * disarm method (1 row), save settings message (1 row) and an empty last row */
+        if (row < (osdDisplayPort->rows - 3)) row = drawStat_GForce(statNameX, row, statValuesX); // 1 row
 #ifdef USE_STATS
-        if (row < (osdDisplayPort->cols-7) && statsConfig()->stats_enabled) row = drawStat_Stats(statNameX, row, statValuesX, false); // 4 rows
+        if (row < (osdDisplayPort->rows - 6) && statsConfig()->stats_enabled) row = drawStat_Stats(statNameX, row, statValuesX, false); // 4 rows
 #endif
     } else {
-        switch (page) {
-            case 0:
-                // Max 10 rows
-                row = drawStat_FlightTime(statNameX, row, statValuesX); // 1 row
-                row = drawStat_FlightDistance(statNameX, row, statValuesX); // 1 row
-                if (feature(FEATURE_GPS)) row = drawStat_MaxDistanceFromHome(statNameX, row, statValuesX); // 1 row
-                if (feature(FEATURE_GPS)) row = drawStat_Speed(statNameX, row, statValuesX); // 1 row
-                row = drawStat_MaximumAltitude(statNameX, row, statValuesX); // 1 row
-                row = drawStat_BatteryVoltage(statNameX, row, statValuesX); // 1 row
-                if (feature(FEATURE_CURRENT_METER)) row = drawStat_MaximumPowerAndCurrent(statNameX, row, statValuesX); // 1 row
-                if (feature(FEATURE_CURRENT_METER))row = drawStat_UsedEnergy(statNameX, row, statValuesX); // 1 row
-                if (feature(FEATURE_CURRENT_METER) && feature(FEATURE_GPS)) row = drawStat_AverageEfficiency(statNameX, row, statValuesX, false); // 1 row
-                if (feature(FEATURE_GPS))row = drawStat_GPS(statNameX, row, statValuesX); // 1 row
-                break;
-            case 1:
-                // Max 10 rows
-                row = drawStat_RXStats(statNameX, row, statValuesX); // 1 row if non-CRSF else 2 rows
-                if (STATE(ESC_SENSOR_ENABLED)) row = drawStat_ESCTemperature(statNameX, row, statValuesX); // 1 row
-                row = drawStat_GForce(statNameX, row, statValuesX); // 1 row HD or 2 rows SD
-                if (osdConfig()->stats_show_metric_efficiency && osdIsNotMetric() && feature(FEATURE_CURRENT_METER) && feature(FEATURE_GPS)) row = drawStat_AverageEfficiency(statNameX, row, statValuesX, true); // 1 row
-#ifdef USE_BLACKBOX
-#ifdef USE_SDCARD
-                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);
-                    else
-                        strcat(buff, ": INVALID");
+        if (page == 0) {
+            // Max 10 rows
+            row = drawStat_FlightTime(statNameX, row, statValuesX); // 1 row
+            row = drawStat_FlightDistance(statNameX, row, statValuesX); // 1 row
+            if (feature(FEATURE_GPS)) row = drawStat_MaxDistanceFromHome(statNameX, row, statValuesX); // 1 row
+            if (feature(FEATURE_GPS)) row = drawStat_Speed(statNameX, row, statValuesX); // 1 row
+            row = drawStat_MaximumAltitude(statNameX, row, statValuesX); // 1 row
+            row = drawStat_BatteryVoltage(statNameX, row, statValuesX); // 1 row
+            if (feature(FEATURE_CURRENT_METER)) row = drawStat_MaximumPowerAndCurrent(statNameX, row, statValuesX); // 1 row
+            if (feature(FEATURE_CURRENT_METER))row = drawStat_UsedEnergy(statNameX, row, statValuesX); // 1 row
+            if (feature(FEATURE_CURRENT_METER) && feature(FEATURE_GPS)) row = drawStat_AverageEfficiency(statNameX, row, statValuesX, false); // 1 row
+            if (feature(FEATURE_GPS))row = drawStat_GPS(statNameX, row, statValuesX); // 1 row
+        } else {
+            // Max 10 rows
+            row = drawStat_RXStats(statNameX, row, statValuesX); // 1 row if non-CRSF else 2 rows
+            if (STATE(ESC_SENSOR_ENABLED)) row = drawStat_ESCTemperature(statNameX, row, statValuesX); // 1 row
+            row = drawStat_GForce(statNameX, row, statValuesX); // 1 row HD or 2 rows SD
+            if (osdConfig()->stats_show_metric_efficiency && osdIsNotMetric() && feature(FEATURE_CURRENT_METER) && feature(FEATURE_GPS)) row = drawStat_AverageEfficiency(statNameX, row, statValuesX, true); // 1 row
+#if defined(USE_BLACKBOX) && defined(USE_SDCARD)
+            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);
+                else
+                    strcat(buff, ": INVALID");
 
-                    displayWrite(osdDisplayPort, statValuesX, row++, buff); // 1 row
-                }
-#endif
+                displayWrite(osdDisplayPort, statValuesX, row++, buff); // 1 row
+            }
 #endif
 #ifdef USE_STATS
-                if (row < (osdDisplayPort->cols-7) && statsConfig()->stats_enabled) row = drawStat_Stats(statNameX, row, statValuesX, false); // 4 rows
+            if (row < (osdDisplayPort->rows - 6) && statsConfig()->stats_enabled) row = drawStat_Stats(statNameX, row, statValuesX, false); // 4 rows
 #endif
-
-                break;
         }
     }
 
-    row = drawStat_DisarmMethod(statNameX, row, statValuesX);
+    row = drawStat_DisarmMethod(statNameX, row, statValuesX);  // 1 row
 
     // The following has been commented out as it will be added in #9688
     // uint16_t rearmMs = (emergInflightRearmEnabled()) ? emergencyInFlightRearmTimeMS() : 0;
 
+    // Adds 1 row
     if (savingSettings == true) {
         displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(OSD_MESSAGE_STR(OSD_MSG_SAVING_SETTNGS))) / 2, row++, OSD_MESSAGE_STR(OSD_MSG_SAVING_SETTNGS));
     /*} else if (rearmMs > 0) { // Show rearming time if settings not actively being saved. Ignore the settings saved message if rearm available.
@@ -5545,14 +5528,12 @@ static void osdShowHDArmScreen(void)
     memset(buf, '\0', sizeof(buf));
     memset(buf2, '\0', sizeof(buf2));
 
-#if defined(USE_GPS)
-#if defined (USE_SAFE_HOME)
+#if defined(USE_GPS) && defined(USE_SAFE_HOME)
     if (posControl.safehomeState.distance) {
         safehomeRow = armScreenRow;
         armScreenRow +=2;
     }
-#endif // USE_SAFE_HOME
-#endif // USE_GPS
+#endif // USE_GPS && USE_SAFE_HOME
 
     if (posControl.waypointListValid && posControl.waypointCount > 0) {
 #ifdef USE_MULTI_MISSION
@@ -5653,15 +5634,12 @@ static void osdShowSDArmScreen(void)
     strcpy(buf, "! ARMED !");
     displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf);
     memset(buf, '\0', sizeof(buf));
-#if defined(USE_GPS)
-#if defined (USE_SAFE_HOME)
+#if defined(USE_GPS) && defined(USE_SAFE_HOME)
     if (posControl.safehomeState.distance) {
         safehomeRow = armScreenRow;
         armScreenRow += 2;
     }
 #endif
-#endif
-
     memset(buf2, '\0', sizeof(buf2));
     if (strlen(systemConfig()->pilotName) > 0) {
         osdFormatPilotName(buf2);
@@ -5870,17 +5848,18 @@ static void osdRefresh(timeUs_t currentTimeUs)
         if (ARMING_FLAG(ARMED)) {
             // Display the "Arming" screen
             statsDisplayed = false;
-            if (!STATE(IN_FLIGHT_EMERG_REARM))
-                osdResetStats();
+            if (!STATE(IN_FLIGHT_EMERG_REARM)) osdResetStats();
 
             osdShowArmed();
             uint16_t delay = osdConfig()->arm_screen_display_time;
-            if (STATE(IN_FLIGHT_EMERG_REARM))
+            if (STATE(IN_FLIGHT_EMERG_REARM)) {
                 delay = 500;
 #if defined(USE_SAFE_HOME)
-            else if (posControl.safehomeState.distance)
+            } else if (posControl.safehomeState.distance) {
                 delay += 3000;
+
 #endif
+            }
             osdSetNextRefreshIn(delay);
         } else {
             // Display the "Stats" screen
@@ -5898,39 +5877,38 @@ static void osdRefresh(timeUs_t currentTimeUs)
     // This block is entered when we're showing the "Splash", "Armed" or "Stats" screens
     if (resumeRefreshAt) {
 
-        // Handle events only when the "Stats" screen is being displayed.
-        if (statsDisplayed) {
-
-             // Manual paging stick commands are only applicable to multi-page stats.
-             // ******************************
-             // For single-page stats, this effectively disables the ability to cancel the
-             // 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.
-             // ******************************
-             // 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.
+        // Handle events only when the "multi-page Stats" screen is being displayed.
+        if (statsDisplayed && !statsSinglePageCompatible) {
+
+            // Manual paging stick commands are only applicable to multi-page stats.
+            // ******************************
+            // 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.
+            // ******************************
+            // 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.
+
             bool manualPageUpRequested = 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()) {
-                    manualPageUpRequested = true;
-                    statsAutoPagingEnabled = false;
-                } else if (osdIsPageDownStickCommandHeld()) {
-                    manualPageDownRequested = true;
-                    statsAutoPagingEnabled = false;
-                }
+
+            // 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()) {
+                manualPageUpRequested = true;
+                statsAutoPagingEnabled = false;
+            } else if (osdIsPageDownStickCommandHeld()) {
+                manualPageDownRequested = true;
+                statsAutoPagingEnabled = false;
             }
 
             if (statsAutoPagingEnabled) {
-                // Alternate screens for multi-page stats.
-                // Also, refreshes screen at swap interval for single-page stats.
+                // Auto alternate screens
                 if (OSD_ALTERNATING_CHOICES((osdConfig()->stats_page_auto_swap_time * 1000), 2)) {
                     if (statsCurrentPage == 0) {
                         osdShowStats(statsSinglePageCompatible, statsCurrentPage);
@@ -5943,7 +5921,7 @@ static void osdRefresh(timeUs_t currentTimeUs)
                     }
                 }
             } else {
-                // Process manual page change events for multi-page stats.
+                // Process manual page change events
                 if (manualPageUpRequested) {
                     osdShowStats(statsSinglePageCompatible, 1);
                     statsCurrentPage = 1;
@@ -5956,8 +5934,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 && !isThrottleHigh)) {
-            // Time elapsed or canceled by stick commands.
-            // Exit to normal OSD operation.
+            // Time elapsed or canceled by stick commands. Exit to normal OSD operation.
             displayClearScreen(osdDisplayPort);
             resumeRefreshAt = 0;
             statsDisplayed = false;

From 6bdc98b1ce5dbbe9d6be7861cfea6ce971e75cc9 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Sat, 18 Apr 2026 10:01:27 +0100
Subject: [PATCH 078/102] Update osd.c

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

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index ce3f16f575a..110e30b75a8 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -5448,10 +5448,11 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
                 tfp_sprintf(buff, ": %u/%u", stats.min_sats, stats.max_sats);
 
                 int32_t logNumber = blackboxGetLogNumber();
-                if (logNumber >= 0)
+                if (logNumber >= 0) {
                     tfp_sprintf(buff, ": %05ld ", logNumber);
-                else
+                } else {
                     strcat(buff, ": INVALID");
+                }
 
                 displayWrite(osdDisplayPort, statValuesX, row++, buff); // 1 row
             }
@@ -5464,6 +5465,9 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
 
     row = drawStat_DisarmMethod(statNameX, row, statValuesX);  // 1 row
 
+    /* "Saved Settings" messages currently don't work as intended because osdShowStats may not be called often enough to update the messages correctly.
+     * NEEDS FIXING */
+
     // The following has been commented out as it will be added in #9688
     // uint16_t rearmMs = (emergInflightRearmEnabled()) ? emergencyInFlightRearmTimeMS() : 0;
 
@@ -5879,20 +5883,6 @@ static void osdRefresh(timeUs_t currentTimeUs)
 
         // Handle events only when the "multi-page Stats" screen is being displayed.
         if (statsDisplayed && !statsSinglePageCompatible) {
-
-            // Manual paging stick commands are only applicable to multi-page stats.
-            // ******************************
-            // 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.
-            // ******************************
-            // 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.
-
             bool manualPageUpRequested = false;
             bool manualPageDownRequested = false;
 

From 7e824ee93d880b63e0254753f90c094935129e16 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Sat, 18 Apr 2026 15:25:08 +0100
Subject: [PATCH 079/102] fix Save Setting message for stats screen

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

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 110e30b75a8..beadf8c96b4 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -5465,14 +5465,11 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
 
     row = drawStat_DisarmMethod(statNameX, row, statValuesX);  // 1 row
 
-    /* "Saved Settings" messages currently don't work as intended because osdShowStats may not be called often enough to update the messages correctly.
-     * NEEDS FIXING */
-
     // The following has been commented out as it will be added in #9688
     // uint16_t rearmMs = (emergInflightRearmEnabled()) ? emergencyInFlightRearmTimeMS() : 0;
 
     // Adds 1 row
-    if (savingSettings == true) {
+    if (savingSettings) {
         displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(OSD_MESSAGE_STR(OSD_MSG_SAVING_SETTNGS))) / 2, row++, OSD_MESSAGE_STR(OSD_MSG_SAVING_SETTNGS));
     /*} else if (rearmMs > 0) { // Show rearming time if settings not actively being saved. Ignore the settings saved message if rearm available.
         char emReArmMsg[23];
@@ -5481,11 +5478,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
         strcat(emReArmMsg, " **\0");
         displayWrite(osdDisplayPort, statNameX, top++, OSD_MESSAGE_STR(emReArmMsg));*/
     } else if (notify_settings_saved > 0) {
-        if (millis() > notify_settings_saved) {
-            notify_settings_saved = 0;
-        } else {
-            displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(OSD_MESSAGE_STR(OSD_MSG_SETTINGS_SAVED))) / 2, row++, OSD_MESSAGE_STR(OSD_MSG_SETTINGS_SAVED));
-        }
+        displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(OSD_MESSAGE_STR(OSD_MSG_SETTINGS_SAVED))) / 2, row++, OSD_MESSAGE_STR(OSD_MSG_SETTINGS_SAVED));
     }
 
     displayCommitTransaction(osdDisplayPort);
@@ -5841,11 +5834,12 @@ static void osdRefresh(timeUs_t currentTimeUs)
       return;
     }
 
-    bool statsSinglePageCompatible = (osdDisplayPort->rows >= OSD_STATS_SINGLE_PAGE_MIN_ROWS);
     static uint8_t statsCurrentPage = 0;
     static bool statsDisplayed = false;
-    static bool statsAutoPagingEnabled = true;
+    static bool statsAutoPagingEnabled = false;
     static bool isThrottleHigh = false;
+    bool statsSinglePageCompatible = osdDisplayPort->rows >= OSD_STATS_SINGLE_PAGE_MIN_ROWS;
+    bool updateShowStats = false;
 
     // Detect arm/disarm
     if (armState != ARMING_FLAG(ARMED)) {
@@ -5866,11 +5860,11 @@ static void osdRefresh(timeUs_t currentTimeUs)
             }
             osdSetNextRefreshIn(delay);
         } else {
-            // Display the "Stats" screen
+            // Setup display of the "Stats" screen
             statsDisplayed = true;
             statsCurrentPage = 0;
-            statsAutoPagingEnabled = osdConfig()->stats_page_auto_swap_time > 0 ? true : false;
-            osdShowStats(statsSinglePageCompatible, statsCurrentPage);
+            statsAutoPagingEnabled = !statsSinglePageCompatible && osdConfig()->stats_page_auto_swap_time > 0 ? true : false;
+            updateShowStats = true;
             osdSetNextRefreshIn(STATS_SCREEN_DISPLAY_TIME);
             isThrottleHigh = checkStickPosition(THR_HI);
         }
@@ -5882,43 +5876,63 @@ static void osdRefresh(timeUs_t currentTimeUs)
     if (resumeRefreshAt) {
 
         // Handle events only when the "multi-page Stats" screen is being displayed.
-        if (statsDisplayed && !statsSinglePageCompatible) {
-            bool manualPageUpRequested = false;
-            bool manualPageDownRequested = false;
-
-            // 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()) {
-                manualPageUpRequested = true;
-                statsAutoPagingEnabled = false;
-            } else if (osdIsPageDownStickCommandHeld()) {
-                manualPageDownRequested = true;
-                statsAutoPagingEnabled = false;
-            }
+        if (statsDisplayed) {
+            if (!statsSinglePageCompatible) {
+                bool manualPageUpRequested = false;
+                bool manualPageDownRequested = false;
+
+                // 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()) {
+                    manualPageUpRequested = true;
+                    statsAutoPagingEnabled = false;
+                } else if (osdIsPageDownStickCommandHeld()) {
+                    manualPageDownRequested = true;
+                    statsAutoPagingEnabled = false;
+                }
 
-            if (statsAutoPagingEnabled) {
-                // Auto alternate screens
-                if (OSD_ALTERNATING_CHOICES((osdConfig()->stats_page_auto_swap_time * 1000), 2)) {
-                    if (statsCurrentPage == 0) {
-                        osdShowStats(statsSinglePageCompatible, statsCurrentPage);
-                        statsCurrentPage = 1;
+                if (statsAutoPagingEnabled) {
+                    // Auto alternate screens
+                    if (OSD_ALTERNATING_CHOICES((osdConfig()->stats_page_auto_swap_time * 1000), 2)) {
+                        if (statsCurrentPage == 0) {
+                            osdShowStats(statsSinglePageCompatible, statsCurrentPage);
+                            statsCurrentPage = 1;
+                        }
+                    } else {
+                        if (statsCurrentPage == 1) {
+                            osdShowStats(statsSinglePageCompatible, statsCurrentPage);
+                            statsCurrentPage = 0;
+                        }
                     }
                 } else {
-                    if (statsCurrentPage == 1) {
-                        osdShowStats(statsSinglePageCompatible, statsCurrentPage);
+                    // Process manual page change events
+                    if (manualPageUpRequested) {
+                        updateShowStats = true;
+                        statsCurrentPage = 1;
+                    } else if (manualPageDownRequested) {
+                        updateShowStats = true;
                         statsCurrentPage = 0;
                     }
                 }
-            } else {
-                // Process manual page change events
-                if (manualPageUpRequested) {
-                    osdShowStats(statsSinglePageCompatible, 1);
-                    statsCurrentPage = 1;
-                } else if (manualPageDownRequested) {
-                    osdShowStats(statsSinglePageCompatible, 0);
-                    statsCurrentPage = 0;
+            }
+
+            // Process Save Settings messages
+            static bool saveSettingsToggle = false;
+            if (notify_settings_saved) {
+                if (!saveSettingsToggle) {
+                    updateShowStats = true;
+                    saveSettingsToggle = true;
                 }
+                if (millis() > notify_settings_saved) notify_settings_saved = 0;
+            } else if (saveSettingsToggle) {
+                updateShowStats = true;
+                saveSettingsToggle = false;
+            }
+
+            // Update stats page display only when required
+            if (!statsAutoPagingEnabled && updateShowStats) {
+                osdShowStats(statsSinglePageCompatible, statsCurrentPage);
             }
         }
 

From 62acef3d286c76c5d393155f9c4726c981f78e49 Mon Sep 17 00:00:00 2001
From: LiuYongNan <844550332@qq.com>
Date: Mon, 20 Apr 2026 17:32:44 +0800
Subject: [PATCH 080/102] remove spl06 driver

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

diff --git a/src/main/target/HUMMINGBIRD_FC305/target.c b/src/main/target/HUMMINGBIRD_FC305/target.c
index 298eff767e2..275d3f27935 100644
--- a/src/main/target/HUMMINGBIRD_FC305/target.c
+++ b/src/main/target/HUMMINGBIRD_FC305/target.c
@@ -27,8 +27,7 @@
 BUSDEV_REGISTER_SPI_TAG(busdev_icm42688, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_ICM42605_ALIGN);
 BUSDEV_REGISTER_SPI(busdev_sdcard_spi,  DEVHW_SDCARD,       SDCARD_SPI_BUS,     SDCARD_CS_PIN,      NONE,           DEVFLAGS_SPI_MODE_0,  0);
 BUSDEV_REGISTER_SPI(busdev_max7456,    DEVHW_MAX7456,  MAX7456_SPI_BUS,    MAX7456_CS_PIN, NONE, DEVFLAGS_USE_RAW_REGISTERS,  0);
-BUSDEV_REGISTER_I2C(busdev_spl06,      DEVHW_SPL06,        BARO_I2C_BUS,       SPL06_I2C_ADDR,     NONE,           DEVFLAGS_NONE,      0);
-BUSDEV_REGISTER_I2C(busdev_dps310,     DEVHW_DPS310,       BARO_I2C_BUS,       0x76,               NONE,           DEVFLAGS_NONE,      0);
+BUSDEV_REGISTER_I2C(busdev_dps310,     DEVHW_DPS310,       BARO_I2C_BUS,       DPS310_I2C_ADDR,               NONE,           DEVFLAGS_NONE,      0);
 
 BUSDEV_REGISTER_I2C(busdev_qmc5883,     DEVHW_QMC5883,      MAG_I2C_BUS,    0x0D,               NONE,           DEVFLAGS_NONE,  0);
 BUSDEV_REGISTER_I2C(busdev_hmc5883,     DEVHW_HMC5883,      MAG_I2C_BUS,    0x1E,               NONE,           DEVFLAGS_NONE,  0);
diff --git a/src/main/target/HUMMINGBIRD_FC305/target.h b/src/main/target/HUMMINGBIRD_FC305/target.h
index 73ab820d5cb..7b121a775b2 100644
--- a/src/main/target/HUMMINGBIRD_FC305/target.h
+++ b/src/main/target/HUMMINGBIRD_FC305/target.h
@@ -55,9 +55,8 @@
 
 #define USE_BARO
 #define BARO_I2C_BUS            BUS_I2C1
-#define USE_BARO_SPL06
 #define USE_BARO_DPS310
-#define SPL06_I2C_ADDR          (0x76)
+#define DPS310_I2C_ADDR         (0x76)
 
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C1

From 04e188c74c0939832622c5681982535dcc08ff36 Mon Sep 17 00:00:00 2001
From: b14ckyy <he_black_dragon_88@live.de>
Date: Tue, 21 Apr 2026 13:23:13 +0200
Subject: [PATCH 081/102] Resolve merge conflict in msp_messages.json

Merge MSP2_INAV_SET_WP_INDEX (8737) and MSP2_INAV_SET_CRUISE_HEADING (8739)
from upstream with MSP2_INAV_SET_AUX_RC (8752), maintaining numeric order.
---
 docs/development/msp/msp_messages.json | 34 ++++++++++++++++++++++++++
 1 file changed, 34 insertions(+)

diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json
index bbb7eac3429..b687e222c53 100644
--- a/docs/development/msp/msp_messages.json
+++ b/docs/development/msp/msp_messages.json
@@ -10943,6 +10943,40 @@
         "notes": "All attitude angles are in deci-degrees.",
         "description": "Provides estimates of current attitude, local NEU position, and velocity."
     },
+    "MSP2_INAV_SET_WP_INDEX": {
+        "code": 8737,
+        "mspv": 2,
+        "request": {
+            "payload": [
+                {
+                    "name": "wp_index",
+                    "ctype": "uint8_t",
+                    "desc": "0-based waypoint index to jump to, relative to the mission start waypoint (`posControl.startWpIndex`)",
+                    "units": "-"
+                }
+            ]
+        },
+        "reply": null,
+        "notes": "Returns error if the aircraft is not armed, `NAV_WP_MODE` is not active, or the index is outside the valid mission range (`startWpIndex` to `startWpIndex + waypointCount - 1`). On success, sets `posControl.activeWaypointIndex` to the requested index and fires `NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_JUMP`, transitioning the navigation FSM back to `NAV_STATE_WAYPOINT_PRE_ACTION` so the flight controller re-initialises navigation for the new target.",
+        "description": "Jumps to a specific waypoint during an active waypoint mission, causing the aircraft to immediately begin navigating toward the new target waypoint."
+    },
+    "MSP2_INAV_SET_CRUISE_HEADING": {
+        "code": 8739,
+        "mspv": 2,
+        "request": {
+            "payload": [
+                {
+                    "name": "heading_centidegrees",
+                    "ctype": "int32_t",
+                    "desc": "Target heading in centidegrees (0-35999). Values are wrapped modulo 36000 before being applied.",
+                    "units": "centidegrees"
+                }
+            ]
+        },
+        "reply": null,
+        "notes": "Returns error if the aircraft is not armed or `NAV_COURSE_HOLD_MODE` is not active. On success, sets both `posControl.cruise.course` and `posControl.cruise.previousCourse` to the normalised value, preventing spurious heading adjustments from `getCruiseHeadingAdjustment()` on the next control cycle.",
+        "description": "Sets the course heading target while Cruise or Course Hold mode is active, causing the aircraft to turn to and maintain the new heading."
+    },
     "MSP2_INAV_SET_AUX_RC": {
         "code": 8752,
         "mspv": 2,

From 21ea4c8bf4603caf46522ed0a7ba732daaa3b1b3 Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Fri, 24 Apr 2026 22:27:43 -0500
Subject: [PATCH 082/102] GPS: fix hwVersion field size from uint32_t to
 uint8_t
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Change gpsState.hwVersion from uint32_t to uint8_t and introduce a
structured bit-field encoding for the hardware version byte:
  bits [7:6] = series  (0b01 = u-blox Neo/M series)
  bits [5:0] = generation (e.g. 8=M8, 9=M9, 10=M10)

Update UBX_HW_VERSION_* constants accordingly (e.g. M8=0x48, M9=0x49,
M10=0x4A). Change MSP_GPSSTATISTICS to send hwVersion as a single byte
(sbufWriteU8) instead of four bytes (sbufWriteU32).

The bit-field layout reserves series values 0b10 and 0b11 for future
chip families (e.g. u-blox F9, other manufacturers).

Tested with M9 and M10 receivers — auto-detection working correctly.
---
 src/main/fc/fc_msp.c      |  2 +-
 src/main/io/gps_private.h |  2 +-
 src/main/io/gps_ublox.c   |  2 +-
 src/main/io/gps_ublox.h   | 26 +++++++++++++++++++-------
 4 files changed, 22 insertions(+), 10 deletions(-)

diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index cf5308067e9..ac466ece592 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -1009,7 +1009,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         sbufWriteU16(dst, gpsSol.hdop);
         sbufWriteU16(dst, gpsSol.eph);
         sbufWriteU16(dst, gpsSol.epv);
-        sbufWriteU32(dst, gpsState.hwVersion);
+        sbufWriteU8(dst, gpsState.hwVersion);
         break;
 #endif
     case MSP2_ADSB_VEHICLE_LIST:
diff --git a/src/main/io/gps_private.h b/src/main/io/gps_private.h
index e5234e70248..bad8992ce45 100755
--- a/src/main/io/gps_private.h
+++ b/src/main/io/gps_private.h
@@ -42,7 +42,7 @@ typedef struct {
     const serialConfig_t * serialConfig;
     serialPort_t *  gpsPort;                // Serial GPS only
 
-    uint32_t        hwVersion;
+    uint8_t         hwVersion;          // See UBX_HW_VERSION_* in gps_ublox.h
     uint8_t         swVersionMajor;
     uint8_t         swVersionMinor;
 
diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c
index 102284723df..4fd0c332a21 100755
--- a/src/main/io/gps_ublox.c
+++ b/src/main/io/gps_ublox.c
@@ -556,7 +556,7 @@ static void gpsDecodeProtocolVersion(const char *proto, size_t bufferLength)
     }
 }
 
-static uint32_t gpsDecodeHardwareVersion(const char * szBuf, unsigned nBufSize)
+static uint8_t gpsDecodeHardwareVersion(const char * szBuf, unsigned nBufSize)
 {
     // ublox_5   hwVersion 00040005
     if (strncmp(szBuf, "00040005", nBufSize) == 0) {
diff --git a/src/main/io/gps_ublox.h b/src/main/io/gps_ublox.h
index ab0ab930275..d54f4997be0 100644
--- a/src/main/io/gps_ublox.h
+++ b/src/main/io/gps_ublox.h
@@ -62,13 +62,25 @@ STATIC_ASSERT(MAX_UBLOX_PAYLOAD_SIZE >= 256, ubx_size_too_small);
 #define UBX_VALID_GPS_TIME(valid) (valid & 1 << 1)
 #define UBX_VALID_GPS_DATE_TIME(valid) (UBX_VALID_GPS_DATE(valid) && UBX_VALID_GPS_TIME(valid))
 
-#define UBX_HW_VERSION_UNKNOWN  0
-#define UBX_HW_VERSION_UBLOX5   500
-#define UBX_HW_VERSION_UBLOX6   600
-#define UBX_HW_VERSION_UBLOX7   700
-#define UBX_HW_VERSION_UBLOX8   800
-#define UBX_HW_VERSION_UBLOX9   900
-#define UBX_HW_VERSION_UBLOX10  1000
+/*
+ * hwVersion encoding (fits in uint8_t):
+ *   bits [7:6]  series:  0b00=unknown, 0b01=u-blox Neo/M series
+ *   bits [5:0]  generation within series (e.g. 8=M8, 9=M9, 10=M10)
+ *
+ * This leaves 0b10 and 0b11 available for future series (e.g. u-blox F9,
+ * other manufacturers).
+ */
+#define UBX_HW_SERIES_MASK          0xC0
+#define UBX_HW_GEN_MASK             0x3F
+#define UBX_HW_SERIES_UBLOX_NM      0x40    // 0b01 << 6: u-blox Neo/M series
+
+#define UBX_HW_VERSION_UNKNOWN      0
+#define UBX_HW_VERSION_UBLOX5       (UBX_HW_SERIES_UBLOX_NM | 5)   // 0x45
+#define UBX_HW_VERSION_UBLOX6       (UBX_HW_SERIES_UBLOX_NM | 6)   // 0x46
+#define UBX_HW_VERSION_UBLOX7       (UBX_HW_SERIES_UBLOX_NM | 7)   // 0x47
+#define UBX_HW_VERSION_UBLOX8       (UBX_HW_SERIES_UBLOX_NM | 8)   // 0x48
+#define UBX_HW_VERSION_UBLOX9       (UBX_HW_SERIES_UBLOX_NM | 9)   // 0x49
+#define UBX_HW_VERSION_UBLOX10      (UBX_HW_SERIES_UBLOX_NM | 10)  // 0x4A
 
 #define UBLOX_CFG_MSGOUT_NAV_POSLLH_UART1   0x2091002a // U1
 #define UBLOX_CFG_MSGOUT_NAV_SAT_UART1      0x20910016 // U1

From 690c1ec04d036ee16aa58984e6ef72535295b0b6 Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Fri, 24 Apr 2026 23:58:53 -0500
Subject: [PATCH 083/102] GPS: update MSP_GPSSTATISTICS docs for uint8_t
 hwVersion encoding

---
 docs/development/msp/msp_messages.json | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json
index 12d67369bd7..f0b6ab0acce 100644
--- a/docs/development/msp/msp_messages.json
+++ b/docs/development/msp/msp_messages.json
@@ -4914,9 +4914,9 @@
                 },
                 {
                     "name": "hwVersion",
-                    "ctype": "uint32_t",
-                    "desc": "GPS hardware version (`gpsState.hwVersion`). Values: 500=UBLOX5, 600=UBLOX6, 700=UBLOX7, 800=UBLOX8, 900=UBLOX9, 1000=UBLOX10, 0=UNKNOWN",
-                    "units": "Version code"
+                    "ctype": "uint8_t",
+                    "desc": "GPS hardware version bit-field: bits[7:6]=series (0b01=u-blox Neo/M), bits[5:0]=generation. E.g. 0x48=M8, 0x49=M9, 0x4A=M10, 0=unknown.",
+                    "units": ""
                 }
             ]
         },

From ebf10d795c53930188fa1434294ac1a8b5701681 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Sat, 2 May 2026 23:24:16 +0100
Subject: [PATCH 084/102] allow yaw reset to 0 with no mag

---
 src/main/flight/imu.c | 109 +++++++++++++++++++++---------------------
 src/main/flight/imu.h |   2 +-
 2 files changed, 56 insertions(+), 55 deletions(-)

diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index 9ba1134981b..39475e9cb83 100644
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -56,6 +56,7 @@
 #endif
 
 #include "io/gps.h"
+#include "io/beeper.h"
 
 #include "sensors/acceleration.h"
 #include "sensors/barometer.h"
@@ -85,7 +86,7 @@
 FASTRAM fpVector3_t imuMeasuredAccelBF;
 FASTRAM fpVector3_t imuMeasuredRotationBF;
 //centrifugal force compensated using gps
-FASTRAM fpVector3_t compansatedGravityBF;// cm/s/s
+FASTRAM fpVector3_t compensatedGravityBF;// cm/s/s
 
 STATIC_FASTRAM float smallAngleCosZ;
 
@@ -173,8 +174,7 @@ void imuConfigure(void)
      * 1000× per second; the kP gains only change when the user saves settings,
      * so computing it here (called once per save) avoids one add, one multiply,
      * and one divide on every PID cycle. */
-    imuRuntimeConfig.dcm_i_limit = DEGREES_TO_RADIANS(2.0f)
-        * (imuRuntimeConfig.dcm_kp_acc + imuRuntimeConfig.dcm_kp_mag) * 0.5f;
+    imuRuntimeConfig.dcm_i_limit = DEGREES_TO_RADIANS(2.0f) * (imuRuntimeConfig.dcm_kp_acc + imuRuntimeConfig.dcm_kp_mag) * 0.5f;
 }
 
 void imuInit(void)
@@ -192,7 +192,7 @@ void imuInit(void)
     // Create magnetic declination matrix
 #ifdef USE_MAG
     const int deg = compassConfig()->mag_declination / 100;
-    const int min = compassConfig()->mag_declination   % 100;
+    const int min = compassConfig()->mag_declination % 100;
 #else
     const int deg = 0;
     const int min = 0;
@@ -362,7 +362,7 @@ static float imuCalculateMcCogAccWeight(void)
 {
     fpVector3_t accBFNorm;
     vectorScale(&accBFNorm, &imuMeasuredAccelBFFiltered, 1.0f / GRAVITY_CMSS);
-    float wCoGAcc = constrainf((accBFNorm.z - 1.0f)* 2, 0.0f, 1.0f); //z direction is verified via SITL
+    float wCoGAcc = constrainf((accBFNorm.z - 1.0f) * 2.0f, 0.0f, 1.0f); //z direction is verified via SITL
     wCoGAcc = wCoGAcc * imuCalculateAccelerometerWeightRateIgnore(4.0f); //reduce weight when fast angular rate, gps acc is considered lagging
     return wCoGAcc;
 }
@@ -437,7 +437,7 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
             //vForward as trust vector
             if (STATE(MULTIROTOR) && (!isMixerTransitionMixing)){
                 vForward.z = 1.0f;
-            }else{
+            } else {
                 vForward.x = 1.0f;
             }
             fpVector3_t vHeadingEF;
@@ -448,23 +448,22 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
                 float airSpeed = gpsSol.groundSpeed;
     #if defined(USE_WIND_ESTIMATOR)
                 // remove wind elements in vCoGlocal for better heading estimation
-                if (isEstimatedWindSpeedValid() && imuConfig()->gps_yaw_windcomp)
-                {
+                if (isEstimatedWindSpeedValid() && imuConfig()->gps_yaw_windcomp) {
                     vectorScale(&vCoGlocal, &vCoGlocal, gpsSol.groundSpeed);
                     vCoGlocal.x += getEstimatedWindSpeed(X);
                     vCoGlocal.y -= getEstimatedWindSpeed(Y);
                     airSpeed = fast_fsqrtf(vectorNormSquared(&vCoGlocal));
                 }
     #endif
-                wCoG *= scaleRangef(constrainf((airSpeed+gpsSol.groundSpeed)/2, 400, 1000), 400, 1000, 0.0f, 1.0f);
+                wCoG *= scaleRangef(constrainf((airSpeed+gpsSol.groundSpeed) / 2.0f, 400.0f, 1000.0f), 400.0f, 1000.0f, 0.0f, 1.0f);
             } else { //vCOG is not avaliable and vCOGAcc is avaliable, set the weight of vCOG to zero
                 wCoG = 0.0f;
             }
-            if (STATE(MULTIROTOR)){
+            if (STATE(MULTIROTOR)) {
                 //when multicopter`s orientation or speed is changing rapidly. less weight on gps heading
                 wCoG *= imuCalculateMcCogWeight();
                 //handle acc based vector
-                if(vCOGAcc){
+                if (vCOGAcc) {
                     float wCoGAcc = imuCalculateMcCogAccWeight();//stronger weight on acc if body frame z axis greate than 1G
                     if (wCoGAcc > wCoG){
                         //when copter is accelerating use gps acc vector instead of gps speed vector
@@ -627,8 +626,7 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
 		attitude.values.yaw = RADIANS_TO_DECIDEGREES(-atan2_approx(rMat[1][0], rMat[0][0]));
 	}
 
-    if (attitude.values.yaw < 0)
-        attitude.values.yaw += 3600;
+    if (attitude.values.yaw < 0) attitude.values.yaw += 3600;
 
     /* Update small angle state */
     if (calculateCosTiltAngle() > smallAngleCosZ) {
@@ -667,23 +665,17 @@ static float imuCalculateAccelerometerWeightRateIgnore(const float acc_ignore_sl
     // Default - don't apply rate/ignore scaling
     float accWeight_RateIgnore = 1.0f;
 
-    if (ARMING_FLAG(ARMED) && imuConfig()->acc_ignore_rate)
-    {
+    if (ARMING_FLAG(ARMED) && imuConfig()->acc_ignore_rate) {
         float rotRateMagnitude = fast_fsqrtf(vectorNormSquared(&imuMeasuredRotationBFFiltered));
         rotRateMagnitude = rotRateMagnitude / (acc_ignore_slope_multipiler + 0.001f);
-        if (imuConfig()->acc_ignore_slope)
-        {
+
+        if (imuConfig()->acc_ignore_slope) {
             const float rateSlopeMin = DEGREES_TO_RADIANS((imuConfig()->acc_ignore_rate - imuConfig()->acc_ignore_slope));
             const float rateSlopeMax = DEGREES_TO_RADIANS((imuConfig()->acc_ignore_rate + imuConfig()->acc_ignore_slope));
 
             accWeight_RateIgnore = scaleRangef(constrainf(rotRateMagnitude, rateSlopeMin, rateSlopeMax), rateSlopeMin, rateSlopeMax, 1.0f, 0.0f);
-        }
-        else
-        {
-            if (rotRateMagnitude > DEGREES_TO_RADIANS(imuConfig()->acc_ignore_rate))
-            {
-                accWeight_RateIgnore = 0.0f;
-            }
+        } else if (rotRateMagnitude > DEGREES_TO_RADIANS(imuConfig()->acc_ignore_rate)) {
+            accWeight_RateIgnore = 0.0f;
         }
     }
 
@@ -696,7 +688,7 @@ static void imuCalculateFilters(float dT)
     imuMeasuredRotationBFFiltered.x = pt1FilterApply4(&rotRateFilterX, imuMeasuredRotationBF.x, IMU_ROTATION_LPF, dT);
     imuMeasuredRotationBFFiltered.y = pt1FilterApply4(&rotRateFilterY, imuMeasuredRotationBF.y, IMU_ROTATION_LPF, dT);
     imuMeasuredRotationBFFiltered.z = pt1FilterApply4(&rotRateFilterZ, imuMeasuredRotationBF.z, IMU_ROTATION_LPF, dT);
-    
+
     imuMeasuredAccelBFFiltered.x = pt1FilterApply4(&accelFilterX, imuMeasuredAccelBF.x, IMU_ROTATION_LPF, dT);
     imuMeasuredAccelBFFiltered.y = pt1FilterApply4(&accelFilterY, imuMeasuredAccelBF.y, IMU_ROTATION_LPF, dT);
     imuMeasuredAccelBFFiltered.z = pt1FilterApply4(&accelFilterZ, imuMeasuredAccelBF.z, IMU_ROTATION_LPF, dT);
@@ -718,8 +710,7 @@ static void imuCalculateGPSacceleration(fpVector3_t *vEstAccelEF,fpVector3_t *vE
 
     // on first gps data acquired, time_delta_ms will be large, vEstcentrifugalAccelBF will be minimal to disable the compensation
     rtcTime_t time_delta_ms = currenttime - lastGPSNewDataTime;
-    if (lastGPSHeartbeat != gpsSol.flags.gpsHeartbeat && time_delta_ms > 0)
-    {
+    if (lastGPSHeartbeat != gpsSol.flags.gpsHeartbeat && time_delta_ms > 0) {
         // on new gps frame, update accEF and estimate centrifugal accleration
         vEstAccelEF->x = -(currentGPSvel.x - lastGPSvel.x) / (MS2S(time_delta_ms)); // the x axis of accerometer is pointing backward
         vEstAccelEF->y = (currentGPSvel.y - lastGPSvel.y) / (MS2S(time_delta_ms));
@@ -734,11 +725,11 @@ static void imuCalculateGPSacceleration(fpVector3_t *vEstAccelEF,fpVector3_t *vE
 }
 
 static void imuCalculateTurnRateacceleration(fpVector3_t *vEstcentrifugalAccelBF, float dT, float *acc_ignore_slope_multipiler)
-{   
+{
     //fixed wing only
     static float lastspeed = -1.0f;
     float currentspeed = 0;
-    if (isGPSTrustworthy()){
+    if (isGPSTrustworthy()) {
         //first speed choice is gps
         static bool lastGPSHeartbeat;
         static pt1Filter_t GPS3DspeedFilter;
@@ -752,8 +743,7 @@ static void imuCalculateTurnRateacceleration(fpVector3_t *vEstcentrifugalAccelBF
         *acc_ignore_slope_multipiler = 4.0f;
     }
 #ifdef USE_PITOT
-    else if (sensors(SENSOR_PITOT) && pitotIsHealthy())
-    {
+    else if (sensors(SENSOR_PITOT) && pitotIsHealthy()) {
         // second choice is pitot
 		currentspeed = getAirspeedEstimate();
         *acc_ignore_slope_multipiler = 2.0f;
@@ -776,7 +766,7 @@ fpQuaternion_t* getTailSitterQuaternion(bool normal2tail){
     static bool firstRun = true;
     static fpQuaternion_t qNormal2Tail;
     static fpQuaternion_t qTail2Normal;
-    if(firstRun){
+    if (firstRun) {
         fpAxisAngle_t axisAngle;
         axisAngle.axis.x = 0;
         axisAngle.axis.y = 1;
@@ -792,7 +782,7 @@ fpQuaternion_t* getTailSitterQuaternion(bool normal2tail){
 void imuUpdateTailSitter(void)
 {
     static bool lastTailSitter=false;
-    if (((bool)STATE(TAILSITTER)) != lastTailSitter){
+    if (((bool)STATE(TAILSITTER)) != lastTailSitter) {
         fpQuaternion_t* rotation_for_tailsitter= getTailSitterQuaternion(STATE(TAILSITTER));
         quaternionMultiply(&orientation, &orientation, rotation_for_tailsitter);
     }
@@ -842,65 +832,76 @@ static void imuCalculateEstimatedAttitude(float dT)
             resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
         }
     } else if (!ARMING_FLAG(ARMED)) {
-        gpsHeadingInitialized = false;
+        if (STATE(AIRPLANE)) {
+            gpsHeadingInitialized = false;  // required for fixed wing flight detection to work correctly
+        } else if (!canUseMAG && STATE(CALIBRATE_MAG)) {
+            // When no compass available allow yaw to be set to 0 (North) as required using compass calibration stick command
+            DISABLE_STATE(CALIBRATE_MAG);
+            beeper(BEEPER_ACTION_SUCCESS);
+
+            // Re-initialize quaternion from known Roll, Pitch with yaw set to 0 (North)
+            imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, 0);
+            gpsHeadingInitialized = true;
+        }
     }
 
     imuCalculateFilters(dT);
+
     // centrifugal force compensation
     static fpVector3_t vEstcentrifugalAccelBF_velned;
     static fpVector3_t vEstcentrifugalAccelBF_turnrate;
     float acc_ignore_slope_multipiler = 1.0f; // when using gps centrifugal_force_compensation, AccelerometerWeightRateIgnore slope will be multiplied by this value
-    if (isGPSTrustworthy())
-    {
+
+    if (isGPSTrustworthy()) {
         imuCalculateGPSacceleration(&vCOGAcc, &vEstcentrifugalAccelBF_velned, &acc_ignore_slope_multipiler);
         useCOGAcc = true; //currently only for multicopter
     }
-    if (STATE(AIRPLANE))
-    {
+
+    if (STATE(AIRPLANE)) {
         imuCalculateTurnRateacceleration(&vEstcentrifugalAccelBF_turnrate, dT, &acc_ignore_slope_multipiler);
     }
-    if (imuConfig()->inertia_comp_method == COMPMETHOD_ADAPTIVE && isGPSTrustworthy() && STATE(AIRPLANE))
-    {
+
+    if (imuConfig()->inertia_comp_method == COMPMETHOD_ADAPTIVE && isGPSTrustworthy() && STATE(AIRPLANE)) {
         //pick the best centrifugal acceleration between velned and turnrate
-        fpVector3_t compansatedGravityBF_velned;
-        vectorAdd(&compansatedGravityBF_velned, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_velned);
-        float velned_error = fabsf(fast_fsqrtf(vectorNormSquared(&compansatedGravityBF_velned)) - GRAVITY_CMSS);
+        fpVector3_t compensatedGravityBF_velned;
+        vectorAdd(&compensatedGravityBF_velned, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_velned);
+        float velned_error = fabsf(fast_fsqrtf(vectorNormSquared(&compensatedGravityBF_velned)) - GRAVITY_CMSS);
 
-        fpVector3_t compansatedGravityBF_turnrate;
-        vectorAdd(&compansatedGravityBF_turnrate, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_turnrate);
-        float turnrate_error = fabsf(fast_fsqrtf(vectorNormSquared(&compansatedGravityBF_turnrate)) - GRAVITY_CMSS);
+        fpVector3_t compensatedGravityBF_turnrate;
+        vectorAdd(&compensatedGravityBF_turnrate, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_turnrate);
+        float turnrate_error = fabsf(fast_fsqrtf(vectorNormSquared(&compensatedGravityBF_turnrate)) - GRAVITY_CMSS);
 
-        compansatedGravityBF = velned_error > turnrate_error? compansatedGravityBF_turnrate:compansatedGravityBF_velned;
+        compensatedGravityBF = velned_error > turnrate_error ? compensatedGravityBF_turnrate : compensatedGravityBF_velned;
     }
     else if (((imuConfig()->inertia_comp_method == COMPMETHOD_VELNED) || (imuConfig()->inertia_comp_method == COMPMETHOD_ADAPTIVE)) && isGPSTrustworthy())
     {
         //velned centrifugal force compensation, quad will use this method
-        vectorAdd(&compansatedGravityBF, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_velned);
+        vectorAdd(&compensatedGravityBF, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_velned);
     }
     else if (STATE(AIRPLANE))
     {
         //turnrate centrifugal force compensation
-        vectorAdd(&compansatedGravityBF, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_turnrate);
+        vectorAdd(&compensatedGravityBF, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_turnrate);
     }
     else
     {
-        compansatedGravityBF = imuMeasuredAccelBF;
+        compensatedGravityBF = imuMeasuredAccelBF;
     }
 #else
     // In absence of GPS MAG is the only option
     if (canUseMAG) {
         useMag = true;
     }
-    compansatedGravityBF = imuMeasuredAccelBF
+    compensatedGravityBF = imuMeasuredAccelBF;
 #endif
-    float accWeight = imuGetPGainScaleFactor() * imuCalculateAccelerometerWeightNearness(&compansatedGravityBF);
+    float accWeight = imuGetPGainScaleFactor() * imuCalculateAccelerometerWeightNearness(&compensatedGravityBF);
     accWeight = accWeight * imuCalculateAccelerometerWeightRateIgnore(acc_ignore_slope_multipiler);
     const bool useAcc = (accWeight > 0.001f);
 
     const float magWeight = imuGetPGainScaleFactor() * 1.0f;
     fpVector3_t measuredMagBF = {.v = {mag.magADC[X], mag.magADC[Y], mag.magADC[Z]}};
     imuMahonyAHRSupdate(dT, &imuMeasuredRotationBF,
-                            useAcc ? &compansatedGravityBF : NULL,
+                            useAcc ? &compensatedGravityBF : NULL,
                             useMag ? &measuredMagBF : NULL,
                             useCOG ? &vCOG : NULL,
                             useCOGAcc ? &vCOGAcc : NULL,
@@ -950,7 +951,7 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
         acc.accADCf[Z] = 0.0f;
     }
 }
- 
+
 
 bool isImuReady(void)
 {
diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h
index 14244c9cefc..044355603f4 100644
--- a/src/main/flight/imu.h
+++ b/src/main/flight/imu.h
@@ -28,7 +28,7 @@ extern fpVector3_t imuMeasuredAccelBF;         // cm/s/s
 extern fpVector3_t imuMeasuredAccelBFFiltered; // cm/s/s
 extern fpVector3_t imuMeasuredRotationBF;       // rad/s
 extern fpVector3_t imuMeasuredRotationBFFiltered;       // rad/s
-extern fpVector3_t compansatedGravityBF;         // cm/s/s
+extern fpVector3_t compensatedGravityBF;         // cm/s/s
 extern fpVector3_t HeadVecEFFiltered;
 
 typedef union {

From 50f8353fbc9b33cfdc6855042ebcc394d53868f5 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Sun, 3 May 2026 00:27:32 +0100
Subject: [PATCH 085/102] fixes

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

diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index 39475e9cb83..eefdfed9215 100644
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -834,7 +834,7 @@ static void imuCalculateEstimatedAttitude(float dT)
     } else if (!ARMING_FLAG(ARMED)) {
         if (STATE(AIRPLANE)) {
             gpsHeadingInitialized = false;  // required for fixed wing flight detection to work correctly
-        } else if (!canUseMAG && STATE(CALIBRATE_MAG)) {
+        } else if (!sensors(SENSOR_MAG) && STATE(CALIBRATE_MAG)) {
             // When no compass available allow yaw to be set to 0 (North) as required using compass calibration stick command
             DISABLE_STATE(CALIBRATE_MAG);
             beeper(BEEPER_ACTION_SUCCESS);
diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h
index 044355603f4..b2489fd288c 100644
--- a/src/main/flight/imu.h
+++ b/src/main/flight/imu.h
@@ -25,10 +25,7 @@
 #include "config/parameter_group.h"
 
 extern fpVector3_t imuMeasuredAccelBF;         // cm/s/s
-extern fpVector3_t imuMeasuredAccelBFFiltered; // cm/s/s
 extern fpVector3_t imuMeasuredRotationBF;       // rad/s
-extern fpVector3_t imuMeasuredRotationBFFiltered;       // rad/s
-extern fpVector3_t compensatedGravityBF;         // cm/s/s
 extern fpVector3_t HeadVecEFFiltered;
 
 typedef union {

From e9fedba8451804b03202be557bcd06e3d411b1ab Mon Sep 17 00:00:00 2001
From: Roberto <RobertoD91@users.noreply.github.com>
Date: Sun, 3 May 2026 02:09:14 +0200
Subject: [PATCH 086/102] Fix link case sensitivity in Telemetry.md

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

diff --git a/docs/Telemetry.md b/docs/Telemetry.md
index 67e43312406..0adc36cb929 100644
--- a/docs/Telemetry.md
+++ b/docs/Telemetry.md
@@ -413,4 +413,4 @@ It runs at a fixed baud rate of 100000, so it needs a hardware uart capable of i
     \_______/                                              \-------------/
 ```
 
-For more information and sensor slot numbering, refer to [SBUS2 Documentation](SBUS2_telemetry.md)
\ No newline at end of file
+For more information and sensor slot numbering, refer to [SBUS2 Documentation](SBUS2_Telemetry.md)

From 73d9e07726c27c203687173395c887f3a86d39fe Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Sat, 2 May 2026 19:45:29 -0500
Subject: [PATCH 087/102] BLUEBERRYF405: add PWM beeper timer entry for PB9

TIM11/CH1 is the correct timer mapping for PB9 on STM32F405.
Without this DEF_TIM entry, beeperPwmInit() silently fails because
timerGetByTag(PB9, TIM_USE_BEEPER) returns NULL, leaving the beeper
non-functional when beeper_pwm_mode is enabled.

Fixes beeper_pwm_mode regression on this target. Same fix was
previously applied to BLUEBERRYH743 (commit a66b346c).
---
 src/main/target/BLUEBERRYF405/target.c | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/target/BLUEBERRYF405/target.c b/src/main/target/BLUEBERRYF405/target.c
index b712d9f0974..8151b81e77e 100644
--- a/src/main/target/BLUEBERRYF405/target.c
+++ b/src/main/target/BLUEBERRYF405/target.c
@@ -40,6 +40,7 @@ timerHardware_t timerHardware[] = {
     DEF_TIM(TIM4,  CH1,  PB6,  TIM_USE_OUTPUT_AUTO,   0, 0), // S11 D(1,0,2)
 
     DEF_TIM(TIM3,  CH4,  PB1,  TIM_USE_LED,    0, 0), // 2812LED  D(1,2,5)
+    DEF_TIM(TIM11, CH1,  PB9,  TIM_USE_BEEPER, 0, 0), // BEEPER PWM
 
     DEF_TIM(TIM5,  CH3,  PA2,  TIM_USE_ANY,    0, 0), // softserial1_TX (PA2 shared with UART2_TX)
 };

From 3303405bb4255070dfdb2337b1acf8b409415ae6 Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Sat, 2 May 2026 19:11:53 -0500
Subject: [PATCH 088/102] drivers: use NAND-standard opcodes for W25N register
 read/write

Replace the per-chip dynamic opcode dispatch (g_readStatusReg /
g_writeStatusReg) with static use of the NAND-standard "Get Features"
(0x0F) and "Set Features" (0x1F) opcodes for all supported chips.

Winbond W25N01GV and W25N02KV accept both the legacy NOR-style aliases
(0x05/0x01) and the NAND-standard opcodes (0x0F/0x1F). Macronix
MX35LF2GE4AD only accepts the NAND-standard opcodes; using 0x01 leaves
its block-protection register at 0x38 (fully write-protected), causing
all blackbox writes to silently fail.

Tested on W25N01G (NexusXR) and W25N02K (AOCODARCH7DUAL): 2.2 MB
written to flash in a 30-second arm cycle with no errors.
---
 src/main/drivers/flash.h      |  3 +--
 src/main/drivers/flash_w25n.c | 27 +++++----------------------
 2 files changed, 6 insertions(+), 24 deletions(-)

diff --git a/src/main/drivers/flash.h b/src/main/drivers/flash.h
index abe1f79545f..0e522b87890 100644
--- a/src/main/drivers/flash.h
+++ b/src/main/drivers/flash.h
@@ -41,8 +41,7 @@ typedef struct flashGeometry_s {
     uint16_t pagesPerSector;
     flashType_e flashType;
     int32_t bbReplacementBlocks;
-    uint8_t bufReadModeSet;
-    uint8_t bblutTableEntryCount;
+    uint8_t bblutTableEntryCount; // Used by W25N_BBLUT_TABLE_ENTRY_COUNT for bad-block scanning
 } flashGeometry_t;
 
 typedef struct
diff --git a/src/main/drivers/flash_w25n.c b/src/main/drivers/flash_w25n.c
index 9590a4dcecd..8eef2e6944f 100644
--- a/src/main/drivers/flash_w25n.c
+++ b/src/main/drivers/flash_w25n.c
@@ -23,7 +23,7 @@
 
 #include "platform.h"
 
-#if defined(USE_FLASH_W25N01G) || defined(USE_FLASH_W25N02K)|| defined(USE_FLASH_MX35LF2G)
+#if defined(USE_FLASH_W25N01G) || defined(USE_FLASH_W25N02K) || defined(USE_FLASH_MX35LF2G)
 
 #include "drivers/bus.h"
 #include "drivers/io.h"
@@ -46,17 +46,15 @@
 #define W25N_BB_MANAGEMENT_BLOCKS  (W25N_BB_REPLACEMENT_BLOCKS + W25N_BB_MARKER_BLOCKS)
 
 // blocks are zero-based index
-#define W25N_BB_REPLACEMENT_START_BLOCK (W25N_BLOCKS_PER_DIE - W25N_BB_REPLACEMENT_BLOCKS)
-#define W25N_BB_MANAGEMENT_START_BLOCK  (W25N_BLOCKS_PER_DIE - W25N_BB_MANAGEMENT_BLOCKS)
+#define W25N_BB_REPLACEMENT_START_BLOCK (geometry.sectors - W25N_BB_REPLACEMENT_BLOCKS)
+#define W25N_BB_MANAGEMENT_START_BLOCK  (geometry.sectors - W25N_BB_MANAGEMENT_BLOCKS)
 #define W25N_BB_MARKER_BLOCK            (W25N_BB_REPLACEMENT_START_BLOCK - W25N_BB_MARKER_BLOCKS)
 
 // Instructions
 #define W25N_INSTRUCTION_RDID                       0x9F
 #define W25N_INSTRUCTION_DEVICE_RESET               0xFF
-#define W25N_INSTRUCTION_READ_STATUS_REG            g_readStatusReg
-#define W25N_INSTRUCTION_READ_STATUS_ALTERNATE_REG  0x0F
-#define W25N_INSTRUCTION_WRITE_STATUS_REG           g_writeStatusReg
-#define W25N_INSTRUCTION_WRITE_STATUS_ALTERNATE_REG 0x1F
+#define W25N_INSTRUCTION_READ_STATUS_REG            0x0F
+#define W25N_INSTRUCTION_WRITE_STATUS_REG           0x1F
 #define W25N_INSTRUCTION_WRITE_ENABLE               0x06
 #define W25N_INSTRUCTION_DIE_SELECT                 0xC2
 #define W25N_INSTRUCTION_BLOCK_ERASE                0xD8
@@ -126,9 +124,6 @@
 #define W28N_STATUS_PAGE_ADDRESS_SIZE    16
 #define W28N_STATUS_COLUMN_ADDRESS_SIZE  16
 
-static uint8_t g_readStatusReg;
-static uint8_t g_writeStatusReg;
-
 // JEDEC ID
 #define JEDEC_ID_WINBOND_W25N01GV 0xEFAA21
 #define JEDEC_ID_WINBOND_W25N02KV 0xEFAA22
@@ -246,30 +241,21 @@ bool w25n_detect(uint32_t chipID)
         geometry.sectors = W25N01GV_BLOCKS_PER_DIE;      // Blocks
         geometry.pagesPerSector = W25N_PAGES_PER_BLOCK;  // Pages/Blocks
         geometry.pageSize = W25N_PAGE_SIZE;
-        g_readStatusReg = 0x05;
-        g_writeStatusReg = 0x01;
         geometry.bbReplacementBlocks = 20;
-        geometry.bufReadModeSet = 0x02;
         geometry.bblutTableEntryCount = 20;
         break;
     case JEDEC_ID_WINBOND_W25N02KV:
         geometry.sectors = W25N02KV_BLOCKS_PER_DIE;      // Blocks
         geometry.pagesPerSector = W25N_PAGES_PER_BLOCK;  // Pages/Blocks
         geometry.pageSize = W25N_PAGE_SIZE;
-        g_readStatusReg = 0x05;
-        g_writeStatusReg = 0x01;
         geometry.bbReplacementBlocks = 20;
-        geometry.bufReadModeSet = 0x02;
         geometry.bblutTableEntryCount = 20;
         break;
     case JEDEC_ID_MACRONIX_MX35LF2G:
         geometry.sectors = MX35LF2G_BLOCKS_PER_DIE;      // Blocks
         geometry.pagesPerSector = W25N_PAGES_PER_BLOCK;  // Pages/Blocks
         geometry.pageSize = W25N_PAGE_SIZE;
-        g_readStatusReg = 0x0F;
-        g_writeStatusReg = 0x1F;
         geometry.bbReplacementBlocks = 40;
-        geometry.bufReadModeSet = 0;
         geometry.bblutTableEntryCount = 40;
         break;
     default:
@@ -278,10 +264,7 @@ bool w25n_detect(uint32_t chipID)
         geometry.pagesPerSector = 0;
         geometry.sectorSize = 0;
         geometry.totalSize = 0;
-        g_readStatusReg = 0;
-        g_writeStatusReg = 0;
         geometry.bbReplacementBlocks = 0;
-        geometry.bufReadModeSet = 0;
         geometry.bblutTableEntryCount = 0;
         return false;
     }

From 8bd85d7bab89d340d773f06beb5d7a57eb1ca885 Mon Sep 17 00:00:00 2001
From: Benedikt Kleiner <benedikt.kleiner@tbs-avionics.com>
Date: Tue, 21 Apr 2026 11:06:33 +0800
Subject: [PATCH 089/102] lucid: add more baros, enable led strip, align gyo
 orientations

---
 src/main/target/TBS_LUCID_H7/target.c           | 3 ++-
 src/main/target/TBS_LUCID_H7/target.h           | 6 ++++--
 src/main/target/TBS_LUCID_H7_WING/target.c      | 1 +
 src/main/target/TBS_LUCID_H7_WING/target.h      | 7 ++++++-
 src/main/target/TBS_LUCID_H7_WING_MINI/target.c | 1 +
 src/main/target/TBS_LUCID_H7_WING_MINI/target.h | 7 ++++++-
 6 files changed, 20 insertions(+), 5 deletions(-)

diff --git a/src/main/target/TBS_LUCID_H7/target.c b/src/main/target/TBS_LUCID_H7/target.c
index 88b2df3e3b7..8202f462b0f 100644
--- a/src/main/target/TBS_LUCID_H7/target.c
+++ b/src/main/target/TBS_LUCID_H7/target.c
@@ -54,7 +54,8 @@ timerHardware_t timerHardware[] = {
     DEF_TIM(TIM15, CH1, PE5, TIM_USE_OUTPUT_AUTO, 0, 0),   // S11
     DEF_TIM(TIM15, CH2, PE6, TIM_USE_OUTPUT_AUTO, 0, 0),   // S12 DMA_NONE
 
-    DEF_TIM(TIM1,  CH1, PA8,  TIM_USE_LED, 0, 9),    // RGB_LED
+    DEF_TIM(TIM1,  CH1, PA8, TIM_USE_OUTPUT_AUTO | TIM_USE_LED, 0, 9),    // RGB_LED
+
     DEF_TIM(TIM2,  CH1, PA15, TIM_USE_BEEPER, 0, 0),  // BEEPER
 };
 
diff --git a/src/main/target/TBS_LUCID_H7/target.h b/src/main/target/TBS_LUCID_H7/target.h
index e9eef8686bf..7cecc767868 100644
--- a/src/main/target/TBS_LUCID_H7/target.h
+++ b/src/main/target/TBS_LUCID_H7/target.h
@@ -101,8 +101,8 @@
 #define GYRO2_CS_PIN            PE11
 
 #define USE_IMU_MPU6000
-#define IMU_1_MPU6000_ALIGN       CW0_DEG_FLIP
-#define IMU_2_MPU6000_ALIGN       CW90_DEG_FLIP
+#define IMU_1_MPU6000_ALIGN       CW90_DEG_FLIP
+#define IMU_2_MPU6000_ALIGN       CW0_DEG_FLIP
 
 #define USE_IMU_ICM42605
 #define IMU_1_ICM42605_ALIGN      CW90_DEG_FLIP
@@ -124,6 +124,8 @@
 
 #define USE_BARO
 #define USE_BARO_DPS310
+#define USE_BARO_BMP388
+#define USE_BARO_BMP390
 #define BARO_I2C_BUS            BUS_I2C2
 
 #define USE_MAG
diff --git a/src/main/target/TBS_LUCID_H7_WING/target.c b/src/main/target/TBS_LUCID_H7_WING/target.c
index 1dd790735d0..f8ecaa5a29b 100644
--- a/src/main/target/TBS_LUCID_H7_WING/target.c
+++ b/src/main/target/TBS_LUCID_H7_WING/target.c
@@ -55,6 +55,7 @@ timerHardware_t timerHardware[] = {
     DEF_TIM(TIM15, CH2, PE6, TIM_USE_OUTPUT_AUTO, 0, 0),   // S12 DMA_NONE
 
     DEF_TIM(TIM1,  CH1, PA8,  TIM_USE_OUTPUT_AUTO, 0, 9),   // S13
+    DEF_TIM(TIM2,  CH1, PA15, TIM_USE_OUTPUT_AUTO | TIM_USE_LED, 0, 8),   // LED_STRIP
 };
 
 const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
diff --git a/src/main/target/TBS_LUCID_H7_WING/target.h b/src/main/target/TBS_LUCID_H7_WING/target.h
index 8871cee7398..9dd98e1527d 100644
--- a/src/main/target/TBS_LUCID_H7_WING/target.h
+++ b/src/main/target/TBS_LUCID_H7_WING/target.h
@@ -100,7 +100,7 @@
 #define GYRO2_CS_PIN            PE11
 
 #define USE_IMU_MPU6000
-#define IMU_1_MPU6000_ALIGN       CW0_DEG_FLIP
+#define IMU_1_MPU6000_ALIGN       CW180_DEG_FLIP
 #define IMU_2_MPU6000_ALIGN       CW90_DEG_FLIP
 
 #define USE_IMU_ICM42605
@@ -124,6 +124,8 @@
 
 #define USE_BARO
 #define USE_BARO_DPS310
+#define USE_BARO_BMP388
+#define USE_BARO_BMP390
 #define BARO_I2C_BUS            BUS_I2C2
 
 #define USE_MAG
@@ -161,6 +163,9 @@
 #define PINIO1_PIN                  PD10 
 #define PINIO2_PIN                  PD11
 
+#define USE_LED_STRIP
+#define WS2811_PIN                  PA15
+
 #define DEFAULT_FEATURES            (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX)
 #define CURRENT_METER_SCALE         200
 #define VBAT_SCALE_DEFAULT          2100
diff --git a/src/main/target/TBS_LUCID_H7_WING_MINI/target.c b/src/main/target/TBS_LUCID_H7_WING_MINI/target.c
index 837f05985a9..1c27e0d6d04 100644
--- a/src/main/target/TBS_LUCID_H7_WING_MINI/target.c
+++ b/src/main/target/TBS_LUCID_H7_WING_MINI/target.c
@@ -47,6 +47,7 @@ timerHardware_t timerHardware[] = {
     DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 5),   // S6
 
     DEF_TIM(TIM4, CH1, PD12, TIM_USE_OUTPUT_AUTO, 0, 6),  // S7
+    DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO | TIM_USE_LED, 0, 8),  // LED_STRIP
 };
 
 const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
diff --git a/src/main/target/TBS_LUCID_H7_WING_MINI/target.h b/src/main/target/TBS_LUCID_H7_WING_MINI/target.h
index f211decb7bd..4f475a46a0f 100644
--- a/src/main/target/TBS_LUCID_H7_WING_MINI/target.h
+++ b/src/main/target/TBS_LUCID_H7_WING_MINI/target.h
@@ -100,7 +100,7 @@
 #define GYRO2_CS_PIN            PE11
 
 #define USE_IMU_MPU6000
-#define IMU_1_MPU6000_ALIGN       CW0_DEG_FLIP
+#define IMU_1_MPU6000_ALIGN       CW180_DEG_FLIP
 #define IMU_2_MPU6000_ALIGN       CW90_DEG_FLIP
 
 #define USE_IMU_ICM42605
@@ -124,6 +124,8 @@
 
 #define USE_BARO
 #define USE_BARO_DPS310
+#define USE_BARO_BMP388
+#define USE_BARO_BMP390
 #define BARO_I2C_BUS            BUS_I2C2
 
 #define USE_MAG
@@ -160,6 +162,9 @@
 #define USE_PINIOBOX
 #define PINIO1_PIN                  PD10 
 
+#define USE_LED_STRIP
+#define WS2811_PIN                  PA15
+
 #define DEFAULT_FEATURES            (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX)
 #define CURRENT_METER_SCALE         250
 #define VBAT_SCALE_DEFAULT          2100

From e309823b88d8153f3eda7ae07ea6e607661a2796 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Mon, 4 May 2026 17:51:42 +0100
Subject: [PATCH 090/102] Update Controls.md

---
 docs/Controls.md | 10 +++++++++-
 1 file changed, 9 insertions(+), 1 deletion(-)

diff --git a/docs/Controls.md b/docs/Controls.md
index 6dbc26df5fc..a84267ffbfe 100644
--- a/docs/Controls.md
+++ b/docs/Controls.md
@@ -31,7 +31,7 @@ The stick positions are combined to activate different functions:
 | Battery profile 3             | HIGH     | LOW     | CENTER | HIGH   |
 | Calibrate Gyro                | LOW      | LOW     | LOW    | CENTER |
 | Calibrate Acc                 | HIGH     | LOW     | LOW    | CENTER |
-| Calibrate Mag/Compass         | HIGH     | HIGH    | LOW    | CENTER |
+| Calibrate Compass/Zero Yaw    | HIGH     | HIGH    | LOW    | CENTER |
 | Trim Acc Left                 | HIGH     | CENTER  | CENTER | LOW    |
 | Trim Acc Right                | HIGH     | CENTER  | CENTER | HIGH   |
 | Trim Acc Forwards             | HIGH     | CENTER  | HIGH   | CENTER |
@@ -52,6 +52,14 @@ The stick positions are combined to activate different functions:
 For graphical stick position in all transmitter modes, check out [this page](https://www.mrd-rc.com/tutorials-tools-and-testing/inav-flight/inav-stick-commands-for-all-transmitter-modes/).
 ![Stick Positions](assets/images/StickPositions.png)
 
+## Compass Calibration and Yaw Zero Reset
+
+The stick function `Calibrate Compass/Zero Yaw` provides 2 functions depending on whether or not a compass is available.
+
+If a compass is available the stick function initiates the compass calibration routine.
+
+If no compass is available the stick function will reset the current yaw/heading estimate to zero (North) and also set the heading as trusted. This is useful on multirotors, allowing the craft yaw/heading to be correctly aligned to actual North simply by physically pointing the craft North then using the stick function to zero the yaw estimate. Since this also sets the heading as trusted Nav modes reliant on heading will be available immediately after arming without the need to fly fast enough to obtain a valid heading from GPS ground course.
+
 ## Yaw control
 
 While arming/disarming with sticks, your yaw stick will be moving to extreme values. In order to prevent your craft from trying to yaw during arming/disarming while on the ground, your yaw input will not cause the craft to yaw when the throttle is LOW (i.e. below the `min_check` setting).

From 49ef6fc7e568e7550f7ad5b678f278b8e0a30d5f Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Tue, 5 May 2026 22:58:28 -0500
Subject: [PATCH 091/102] Fix DMA disable timeout: uint32_t wraparound made
 abort path unreachable

uint32_t post-decrement from 0 wraps to UINT32_MAX, so timeout == 0 can
never be true after the loop exits. The stream-still-enabled check alone
correctly identifies whether the timeout was exhausted.
---
 src/main/drivers/timer_impl_hal.c            | 4 ++--
 src/main/drivers/timer_impl_stdperiph.c      | 4 ++--
 src/main/drivers/timer_impl_stdperiph_at32.c | 2 +-
 3 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/main/drivers/timer_impl_hal.c b/src/main/drivers/timer_impl_hal.c
index 111fa6ffb19..8cfe3642021 100644
--- a/src/main/drivers/timer_impl_hal.c
+++ b/src/main/drivers/timer_impl_hal.c
@@ -534,7 +534,7 @@ void impl_pwmBurstDMASetCircular(burstDmaTimer_t * burstDmaTimer, TCH_t * tch, b
             __NOP();
         }
 
-        if (timeout == 0 && LL_DMA_IsEnabledStream(burstDmaTimer->dma, burstDmaTimer->streamLL)) {
+        if (LL_DMA_IsEnabledStream(burstDmaTimer->dma, burstDmaTimer->streamLL)) {
             LL_TIM_EnableDMAReq_CCx(burstDmaTimer->timer, burstDmaTimer->burstRequestSource);
             return;
         }
@@ -647,7 +647,7 @@ void impl_timerPWMSetDMACircular(TCH_t * tch, bool circular, uint32_t dmaBufferS
             __NOP();
         }
 
-        if (timeout == 0 && LL_DMA_IsEnabledStream(dmaBase, streamLL)) {
+        if (LL_DMA_IsEnabledStream(dmaBase, streamLL)) {
             LL_TIM_EnableDMAReq_CCx(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex]);
             return;
         }
diff --git a/src/main/drivers/timer_impl_stdperiph.c b/src/main/drivers/timer_impl_stdperiph.c
index 5f939b1ac34..5b52cb862a4 100644
--- a/src/main/drivers/timer_impl_stdperiph.c
+++ b/src/main/drivers/timer_impl_stdperiph.c
@@ -486,7 +486,7 @@ void impl_pwmBurstDMASetCircular(burstDmaTimer_t * burstDmaTimer, TCH_t * tch, b
             __NOP();
         }
 
-        if (timeout == 0 && (burstDmaTimer->dmaBurstStream->CR & DMA_SxCR_EN)) {
+        if (burstDmaTimer->dmaBurstStream->CR & DMA_SxCR_EN) {
             TIM_DMACmd(burstDmaTimer->timer, burstDmaTimer->burstRequestSource, ENABLE);
             return;
         }
@@ -584,7 +584,7 @@ void impl_timerPWMSetDMACircular(TCH_t * tch, bool circular, uint32_t dmaBufferS
             __NOP();
         }
 
-        if (timeout == 0 && (tch->dma->ref->CR & DMA_SxCR_EN)) {
+        if (tch->dma->ref->CR & DMA_SxCR_EN) {
             TIM_DMACmd(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex], ENABLE);
             return;
         }
diff --git a/src/main/drivers/timer_impl_stdperiph_at32.c b/src/main/drivers/timer_impl_stdperiph_at32.c
index 8317f1f5795..782943ab25d 100644
--- a/src/main/drivers/timer_impl_stdperiph_at32.c
+++ b/src/main/drivers/timer_impl_stdperiph_at32.c
@@ -432,7 +432,7 @@ void impl_timerPWMSetDMACircular(TCH_t * tch, bool circular, uint32_t dmaBufferS
             __NOP();
         }
 
-        if (timeout == 0 && tch->dma->ref->ctrl_bit.chen) {
+        if (tch->dma->ref->ctrl_bit.chen) {
             tmr_dma_request_enable(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex], TRUE);
             return;
         }

From cf28e0fc6d3a160429d246ea53c0bd589a3c3d22 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Thu, 7 May 2026 21:07:32 +0100
Subject: [PATCH 092/102] multifunction improvement

---
 src/main/fc/multifunction.c | 48 ++++++++++++++-----------------------
 src/main/fc/multifunction.h |  1 -
 src/main/io/osd.c           |  6 +----
 3 files changed, 19 insertions(+), 36 deletions(-)

diff --git a/src/main/fc/multifunction.c b/src/main/fc/multifunction.c
index 2ac3a558278..7e48e948c0f 100644
--- a/src/main/fc/multifunction.c
+++ b/src/main/fc/multifunction.c
@@ -38,7 +38,6 @@
 
 multi_function_e selectedItem = MULTI_FUNC_NONE;
 uint8_t multiFunctionFlags;
-bool nextItemIsAvailable = false;
 
 static void multiFunctionApply(multi_function_e selectedItem)
 {
@@ -76,52 +75,41 @@ static void multiFunctionApply(multi_function_e selectedItem)
     }
 }
 
-bool isNextMultifunctionItemAvailable(void)
-{
-    return nextItemIsAvailable;
-}
-
 void setMultifunctionSelection(multi_function_e item)
 {
     selectedItem = item == MULTI_FUNC_END ? MULTI_FUNC_1 : item;
-    nextItemIsAvailable = false;
 }
 
 multi_function_e multiFunctionSelection(void)
 {
-    static timeMs_t startTimer;
     static timeMs_t selectTimer;
-    static bool toggle = true;
     const timeMs_t currentTime = millis();
+    static uint8_t toggle = 0;
 
     if (IS_RC_MODE_ACTIVE(BOXMULTIFUNCTION)) {
-        if (selectTimer) {
+        if (!selectTimer) {
+            selectTimer = currentTime;
+            selectedItem = MULTI_FUNC_1;
+        } else if (toggle && selectedItem != MULTI_FUNC_END) {
+            toggle = 2;
             if (currentTime - selectTimer > 3000) {     // 3s selection duration to activate selected function
                 multiFunctionApply(selectedItem);
-                selectTimer = 0;
-                selectedItem = MULTI_FUNC_NONE;
-                nextItemIsAvailable = false;
-            }
-        } else if (toggle) {
-            if (selectedItem == MULTI_FUNC_NONE) {
-                selectedItem++;
-            } else {
-                selectTimer = currentTime;
-                nextItemIsAvailable = true;
+                selectedItem = MULTI_FUNC_END;
             }
         }
-        startTimer = currentTime;
-        toggle = false;
-    } else if (startTimer) {
-        if (!toggle && selectTimer) {
-            setMultifunctionSelection(++selectedItem);
+    } else if (selectTimer) {
+        if (toggle == 2) {
+            selectTimer = 0;
+            toggle = 0;
+            return selectedItem = MULTI_FUNC_NONE;
         }
-        if (currentTime - startTimer > 4000) {      // 4s reset delay after mode deselected
-            startTimer = 0;
-            selectedItem = MULTI_FUNC_NONE;
+
+        if (currentTime - selectTimer > 2000) {
+            setMultifunctionSelection(++selectedItem);
+            selectTimer = currentTime;
         }
-        selectTimer = 0;
-        toggle = true;
+
+        toggle = 1;
     }
 
     return selectedItem;
diff --git a/src/main/fc/multifunction.h b/src/main/fc/multifunction.h
index 6e60da2bdd8..e6aaad03fe5 100644
--- a/src/main/fc/multifunction.h
+++ b/src/main/fc/multifunction.h
@@ -58,6 +58,5 @@ typedef enum {
 } multi_function_e;
 
 multi_function_e multiFunctionSelection(void);
-bool isNextMultifunctionItemAvailable(void);
 void setMultifunctionSelection(multi_function_e item);
 #endif
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index beadf8c96b4..68adaef5e7a 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -6469,6 +6469,7 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
             message = ARMING_FLAG(ARMED) ? "NOW ARMED " : "EMERG ARM ";
             break;
         case MULTI_FUNC_END:
+            message = "ACTIVATED!";
             break;
         }
 
@@ -6478,11 +6479,6 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
 
         strcpy(buff, message);
 
-        if (isNextMultifunctionItemAvailable()) {
-            // provides feedback indicating when a new selection command has been received by flight controller
-            buff[9] = '>';
-        }
-
         return elemAttr;
     }
 #endif  // MULTIFUNCTION - functions only, warnings always defined

From 8bd9fdd55407cac1228ca5817c9b70f3ec3a74a4 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Fri, 8 May 2026 11:42:17 +0100
Subject: [PATCH 093/102] tweaks

---
 src/main/fc/multifunction.c | 30 ++++++++++++++++--------------
 1 file changed, 16 insertions(+), 14 deletions(-)

diff --git a/src/main/fc/multifunction.c b/src/main/fc/multifunction.c
index 7e48e948c0f..e0041fa750d 100644
--- a/src/main/fc/multifunction.c
+++ b/src/main/fc/multifunction.c
@@ -82,34 +82,36 @@ void setMultifunctionSelection(multi_function_e item)
 
 multi_function_e multiFunctionSelection(void)
 {
-    static timeMs_t selectTimer;
+    static timeMs_t functionTimer;
     const timeMs_t currentTime = millis();
-    static uint8_t toggle = 0;
+    static uint8_t functionTracker = 0;
 
     if (IS_RC_MODE_ACTIVE(BOXMULTIFUNCTION)) {
-        if (!selectTimer) {
-            selectTimer = currentTime;
+        if (!functionTimer) {    // initiate function on first BOXMULTIFUNCTION activation
+            functionTimer = currentTime;
             selectedItem = MULTI_FUNC_1;
-        } else if (toggle && selectedItem != MULTI_FUNC_END) {
-            toggle = 2;
-            if (currentTime - selectTimer > 3000) {     // 3s selection duration to activate selected function
+        } else if (functionTracker && selectedItem != MULTI_FUNC_END) {
+            functionTracker = 2;
+            if (currentTime - functionTimer > 3000) {    // 3s BOXMULTIFUNCTION activation to trigger selected function
                 multiFunctionApply(selectedItem);
                 selectedItem = MULTI_FUNC_END;
             }
         }
-    } else if (selectTimer) {
-        if (toggle == 2) {
-            selectTimer = 0;
-            toggle = 0;
+    } else if (functionTimer) {
+        if (!functionTracker) {
+            functionTimer = currentTime;
+        } else if (functionTracker == 2) {    // cancel and reset function after second BOXMULTIFUNCTION deactivation
+            functionTimer = 0;
+            functionTracker = 0;
             return selectedItem = MULTI_FUNC_NONE;
         }
 
-        if (currentTime - selectTimer > 2000) {
+        if (currentTime - functionTimer > 1500) {    // display available functions on 1.5s rolling cycle
             setMultifunctionSelection(++selectedItem);
-            selectTimer = currentTime;
+            functionTimer = currentTime;
         }
 
-        toggle = 1;
+        functionTracker = 1;
     }
 
     return selectedItem;

From 2c4aa6d7984f8dbb957e8763a2cbf30bfd19511a Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Sat, 9 May 2026 23:43:29 +0100
Subject: [PATCH 094/102] Update osd.c

---
 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 68adaef5e7a..77a1dd62875 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -6469,7 +6469,7 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
             message = ARMING_FLAG(ARMED) ? "NOW ARMED " : "EMERG ARM ";
             break;
         case MULTI_FUNC_END:
-            message = "ACTIVATED!";
+            message = "*FUNC SET*";
             break;
         }
 

From f5bd7be0948d4d3bc21e7aaf78dc4b7c880936d8 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Mon, 11 May 2026 11:08:42 +0100
Subject: [PATCH 095/102] multifunction fixes

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

diff --git a/src/main/fc/multifunction.c b/src/main/fc/multifunction.c
index e0041fa750d..841ebf3fac7 100644
--- a/src/main/fc/multifunction.c
+++ b/src/main/fc/multifunction.c
@@ -61,7 +61,7 @@ static void multiFunctionApply(multi_function_e selectedItem)
         break;
     case MULTI_FUNC_4:
 #ifdef USE_DSHOT
-        if (STATE(MULTIROTOR)) {    // toggle Turtle mode
+        if (!ARMING_FLAG(ARMED) && STATE(MULTIROTOR)) {    // toggle Turtle mode
             MULTI_FUNC_FLAG(MF_TURTLE_MODE) ? MULTI_FUNC_FLAG_DISABLE(MF_TURTLE_MODE) : MULTI_FUNC_FLAG_ENABLE(MF_TURTLE_MODE);
         }
 #endif
@@ -100,7 +100,8 @@ multi_function_e multiFunctionSelection(void)
     } else if (functionTimer) {
         if (!functionTracker) {
             functionTimer = currentTime;
-        } else if (functionTracker == 2) {    // cancel and reset function after second BOXMULTIFUNCTION deactivation
+        } else if (functionTracker == 2 || selectedItem == MULTI_FUNC_NONE) {
+            // cancel and reset function after second BOXMULTIFUNCTION deactivation or if no functions available
             functionTimer = 0;
             functionTracker = 0;
             return selectedItem = MULTI_FUNC_NONE;
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 77a1dd62875..c7c4898294d 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -6438,8 +6438,12 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
         switch (selectedFunction) {
         case MULTI_FUNC_NONE:
         case MULTI_FUNC_1:
-            message = posControl.flags.manualEmergLandActive ? "ABORT LAND" : "EMERG LAND";
-            break;
+            if (ARMING_FLAG(ARMED)) {
+                message = posControl.flags.manualEmergLandActive ? "ABORT LAND" : "EMERG LAND";
+                break;
+            }
+            activeFunction++;
+            FALLTHROUGH;
         case MULTI_FUNC_2:
 #if defined(USE_SAFE_HOME)
             if (navConfig()->general.flags.safehome_usage_mode != SAFEHOME_USAGE_OFF) {
@@ -6458,7 +6462,7 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
             FALLTHROUGH;
         case MULTI_FUNC_4:
 #ifdef USE_DSHOT
-            if (STATE(MULTIROTOR)) {
+            if (!ARMING_FLAG(ARMED) && STATE(MULTIROTOR)) {
                 message = MULTI_FUNC_FLAG(MF_TURTLE_MODE) ? "END TURTLE" : "USE TURTLE";
                 break;
             }
@@ -6466,7 +6470,11 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
             activeFunction++;
             FALLTHROUGH;
         case MULTI_FUNC_5:
-            message = ARMING_FLAG(ARMED) ? "NOW ARMED " : "EMERG ARM ";
+            if (!ARMING_FLAG(ARMED)) {
+                message = "EMERG ARM ";
+                break;
+            }
+            activeFunction++;
             break;
         case MULTI_FUNC_END:
             message = "*FUNC SET*";
@@ -6474,7 +6482,15 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
         }
 
         if (activeFunction != selectedFunction) {
-            setMultifunctionSelection(activeFunction);
+            if (selectedFunction == MULTI_FUNC_1 && activeFunction == MULTI_FUNC_END) {  // no functions available so end process
+                message = "*NO FUNCS*";
+                setMultifunctionSelection(MULTI_FUNC_NONE);
+            } else {
+                setMultifunctionSelection(activeFunction);
+                if (activeFunction == MULTI_FUNC_END) {   // no messages to display so return
+                    return elemAttr;
+                }
+            }
         }
 
         strcpy(buff, message);

From 07cd33f431144e4ff9cd57eef8cfe46ec837eec3 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Mon, 11 May 2026 12:48:35 +0100
Subject: [PATCH 096/102] Add mag calibration to multifunction

---
 src/main/fc/multifunction.c |  6 ++++++
 src/main/fc/multifunction.h |  1 +
 src/main/flight/imu.c       |  9 ++++++---
 src/main/flight/imu.h       |  1 +
 src/main/io/osd.c           | 17 +++++++++++++++++
 5 files changed, 31 insertions(+), 3 deletions(-)

diff --git a/src/main/fc/multifunction.c b/src/main/fc/multifunction.c
index 841ebf3fac7..93c8ef51fe1 100644
--- a/src/main/fc/multifunction.c
+++ b/src/main/fc/multifunction.c
@@ -70,6 +70,12 @@ static void multiFunctionApply(multi_function_e selectedItem)
         if (!ARMING_FLAG(ARMED)) {
             emergencyArmingUpdate(true, true);
         }
+        break;
+    case MULTI_FUNC_6:  // Calibrate compass/Zero yaw heading
+#if defined(USE_GPS) || defined(USE_MAG)
+        ENABLE_STATE(CALIBRATE_MAG);
+#endif
+        break;
     case MULTI_FUNC_END:
         break;
     }
diff --git a/src/main/fc/multifunction.h b/src/main/fc/multifunction.h
index e6aaad03fe5..709e229d6c1 100644
--- a/src/main/fc/multifunction.h
+++ b/src/main/fc/multifunction.h
@@ -54,6 +54,7 @@ typedef enum {
     MULTI_FUNC_3,
     MULTI_FUNC_4,
     MULTI_FUNC_5,
+    MULTI_FUNC_6,
     MULTI_FUNC_END,
 } multi_function_e;
 
diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index eefdfed9215..381a7f589fc 100644
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -952,7 +952,6 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
     }
 }
 
-
 bool isImuReady(void)
 {
     return sensors(SENSOR_ACC) && STATE(ACCELEROMETER_CALIBRATED) && gyroIsCalibrationComplete();
@@ -967,9 +966,13 @@ float calculateCosTiltAngle(void)
 {
     return 1.0f - 2.0f * sq(orientation.q1) - 2.0f * sq(orientation.q2);
 }
-
+#if defined(USE_GPS)
+bool isYawZeroResetAllowed(void)
+{
+    return !ARMING_FLAG(ARMED) && !STATE(AIRPLANE) && !gpsHeadingInitialized && sensors(SENSOR_GPS) && !sensors(SENSOR_MAG);
+}
+#endif
 #if defined(SITL_BUILD) || defined (USE_SIMULATOR)
-
 void imuSetAttitudeRPY(int16_t roll, int16_t pitch, int16_t yaw)
 {
     attitude.values.roll = roll;
diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h
index b2489fd288c..60eb964bd68 100644
--- a/src/main/flight/imu.h
+++ b/src/main/flight/imu.h
@@ -85,6 +85,7 @@ void imuUpdateAccelerometer(void);
 float calculateCosTiltAngle(void);
 bool isImuReady(void);
 bool isImuHeadingValid(void);
+bool isYawZeroResetAllowed(void);
 
 void imuTransformVectorBodyToEarth(fpVector3_t * v);
 void imuTransformVectorEarthToBody(fpVector3_t * v);
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index c7c4898294d..ab99cbb5fa5 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -6475,6 +6475,23 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
                 break;
             }
             activeFunction++;
+            FALLTHROUGH;
+        case MULTI_FUNC_6:
+            if (!ARMING_FLAG(ARMED)) {
+#if defined(USE_MAG)
+                if (sensors(SENSOR_MAG)) {
+                    message = "CAL COMPAS";
+                    break;
+                }
+#endif
+#if defined(USE_GPS)
+                if (isYawZeroResetAllowed()) {
+                    message = "SET HEADIN";
+                    break;
+                }
+#endif
+            }
+            activeFunction++;
             break;
         case MULTI_FUNC_END:
             message = "*FUNC SET*";

From 9cf6945785f92efcfda9d043c4e00509130b455b Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Mon, 11 May 2026 23:29:36 +0100
Subject: [PATCH 097/102] Update imu.c

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

diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index 381a7f589fc..379a023ffd8 100644
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -969,7 +969,7 @@ float calculateCosTiltAngle(void)
 #if defined(USE_GPS)
 bool isYawZeroResetAllowed(void)
 {
-    return !ARMING_FLAG(ARMED) && !STATE(AIRPLANE) && !gpsHeadingInitialized && sensors(SENSOR_GPS) && !sensors(SENSOR_MAG);
+    return !ARMING_FLAG(ARMED) && !STATE(AIRPLANE) && sensors(SENSOR_GPS) && !sensors(SENSOR_MAG);
 }
 #endif
 #if defined(SITL_BUILD) || defined (USE_SIMULATOR)

From 49e277642521c98d1b392edef0349991d0bee12a Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Tue, 12 May 2026 14:57:35 +0100
Subject: [PATCH 098/102] multifunction refactor

---
 src/main/fc/fc_msp_box.c    |   2 -
 src/main/fc/multifunction.c | 253 +++++++++++++++++++++++++++++++++---
 src/main/fc/multifunction.h |   2 +
 src/main/io/osd.c           | 222 -------------------------------
 4 files changed, 239 insertions(+), 240 deletions(-)

diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c
index a20a23e0b24..65654ccd97b 100644
--- a/src/main/fc/fc_msp_box.c
+++ b/src/main/fc/fc_msp_box.c
@@ -190,9 +190,7 @@ void initActiveBoxIds(void)
     RESET_BOX_ID_COUNT;
     ADD_ACTIVE_BOX(BOXARM);
     ADD_ACTIVE_BOX(BOXPREARM);
-#ifdef USE_MULTI_FUNCTIONS
     ADD_ACTIVE_BOX(BOXMULTIFUNCTION);
-#endif
 
     if (sensors(SENSOR_ACC) && STATE(ALTITUDE_CONTROL)) {
         ADD_ACTIVE_BOX(BOXANGLE);
diff --git a/src/main/fc/multifunction.c b/src/main/fc/multifunction.c
index 93c8ef51fe1..50cf3af1aea 100644
--- a/src/main/fc/multifunction.c
+++ b/src/main/fc/multifunction.c
@@ -26,16 +26,29 @@
 #include "build/debug.h"
 #include "drivers/time.h"
 
-#ifdef USE_MULTI_FUNCTIONS
-
+#include "fc/config.h"
 #include "fc/fc_core.h"
 #include "fc/multifunction.h"
 #include "fc/rc_modes.h"
-#include "fc/runtime_config.h"
+
+#include "flight/imu.h"
 
 #include "io/osd.h"
+
 #include "navigation/navigation.h"
+#include "navigation/navigation_private.h"
+
+#include "sensors/acceleration.h"
+#include "sensors/battery.h"
+#include "sensors/compass.h"
+#include "sensors/diagnostics.h"
+#include "sensors/pitotmeter.h"
+#include "sensors/sensors.h"
 
+textAttributes_t osdGetMultiFunctionMessage(char *buff);
+multiFunctionWarning_t multiFunctionWarning;
+
+#ifdef USE_MULTI_FUNCTIONS
 multi_function_e selectedItem = MULTI_FUNC_NONE;
 uint8_t multiFunctionFlags;
 
@@ -49,27 +62,19 @@ static void multiFunctionApply(multi_function_e selectedItem)
         break;
     case MULTI_FUNC_2:  // toggle Safehome suspend
 #if defined(USE_SAFE_HOME)
-        if (navConfig()->general.flags.safehome_usage_mode != SAFEHOME_USAGE_OFF) {
-            MULTI_FUNC_FLAG(MF_SUSPEND_SAFEHOMES) ? MULTI_FUNC_FLAG_DISABLE(MF_SUSPEND_SAFEHOMES) : MULTI_FUNC_FLAG_ENABLE(MF_SUSPEND_SAFEHOMES);
-        }
+        MULTI_FUNC_FLAG(MF_SUSPEND_SAFEHOMES) ? MULTI_FUNC_FLAG_DISABLE(MF_SUSPEND_SAFEHOMES) : MULTI_FUNC_FLAG_ENABLE(MF_SUSPEND_SAFEHOMES);
 #endif
         break;
     case MULTI_FUNC_3:  // toggle RTH Trackback suspend
-        if (navConfig()->general.flags.rth_trackback_mode != RTH_TRACKBACK_OFF) {
-            MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK) ? MULTI_FUNC_FLAG_DISABLE(MF_SUSPEND_TRACKBACK) : MULTI_FUNC_FLAG_ENABLE(MF_SUSPEND_TRACKBACK);
-        }
+        MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK) ? MULTI_FUNC_FLAG_DISABLE(MF_SUSPEND_TRACKBACK) : MULTI_FUNC_FLAG_ENABLE(MF_SUSPEND_TRACKBACK);
         break;
-    case MULTI_FUNC_4:
+    case MULTI_FUNC_4:  // toggle Turtle mode
 #ifdef USE_DSHOT
-        if (!ARMING_FLAG(ARMED) && STATE(MULTIROTOR)) {    // toggle Turtle mode
-            MULTI_FUNC_FLAG(MF_TURTLE_MODE) ? MULTI_FUNC_FLAG_DISABLE(MF_TURTLE_MODE) : MULTI_FUNC_FLAG_ENABLE(MF_TURTLE_MODE);
-        }
+        MULTI_FUNC_FLAG(MF_TURTLE_MODE) ? MULTI_FUNC_FLAG_DISABLE(MF_TURTLE_MODE) : MULTI_FUNC_FLAG_ENABLE(MF_TURTLE_MODE);
 #endif
         break;
     case MULTI_FUNC_5:  // emergency ARM
-        if (!ARMING_FLAG(ARMED)) {
-            emergencyArmingUpdate(true, true);
-        }
+        emergencyArmingUpdate(true, true);
         break;
     case MULTI_FUNC_6:  // Calibrate compass/Zero yaw heading
 #if defined(USE_GPS) || defined(USE_MAG)
@@ -123,4 +128,220 @@ multi_function_e multiFunctionSelection(void)
 
     return selectedItem;
 }
+#endif  // multifunction
+
+static bool osdCheckWarning(bool condition, uint8_t warningFlag)
+{
+    static timeMs_t newWarningEndTime = 0;
+    static uint8_t newWarningFlags = 0;  // bitfield
+    const timeMs_t currentTimeMs = millis();
+
+    /* New warnings dislayed individually for 10s with blinking after which
+     * all current warnings displayed without blinking on 1 second cycle */
+    if (condition) {    // condition required to trigger warning
+        if (!(multiFunctionWarning.osdWarningsFlags & warningFlag)) {  // check for new warnings
+            multiFunctionWarning.osdWarningsFlags |= warningFlag;
+            newWarningFlags |= warningFlag;
+            newWarningEndTime = currentTimeMs + 10000;
+            multiFunctionWarning.newWarningActive = true;
+        }
+#ifdef USE_DEV_TOOLS
+        if (systemConfig()->groundTestMode) {
+            return true;
+        }
 #endif
+        if (currentTimeMs < newWarningEndTime) {
+            return (newWarningFlags & warningFlag);  // filter out new warnings excluding older warnings
+        } else {
+            newWarningFlags = 0;
+            multiFunctionWarning.newWarningActive = false;
+        }
+        return true;
+    } else if (multiFunctionWarning.osdWarningsFlags & warningFlag) {
+        multiFunctionWarning.osdWarningsFlags &= ~warningFlag;
+    }
+
+    return false;
+}
+
+textAttributes_t osdGetMultiFunctionMessage(char *buff)
+{
+    /* Message length limit 10 char max */
+
+    textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE;
+    const char *message = NULL;
+
+#ifdef USE_MULTI_FUNCTIONS
+    /* --- FUNCTIONS --- */
+    multi_function_e selectedFunction = multiFunctionSelection();
+
+    if (selectedFunction) {
+        multi_function_e activeFunction = selectedFunction;
+
+        switch (selectedFunction) {
+        case MULTI_FUNC_NONE:
+        case MULTI_FUNC_1:
+            if (ARMING_FLAG(ARMED)) {
+                message = posControl.flags.manualEmergLandActive ? "ABORT LAND" : "EMERG LAND";
+                break;
+            }
+            activeFunction++;
+            FALLTHROUGH;
+        case MULTI_FUNC_2:
+#if defined(USE_SAFE_HOME)
+            if (navConfig()->general.flags.safehome_usage_mode != SAFEHOME_USAGE_OFF) {
+                message = MULTI_FUNC_FLAG(MF_SUSPEND_SAFEHOMES) ? "USE SFHOME" : "SUS SFHOME";
+                break;
+            }
+#endif
+            activeFunction++;
+            FALLTHROUGH;
+        case MULTI_FUNC_3:
+            if (navConfig()->general.flags.rth_trackback_mode != RTH_TRACKBACK_OFF) {
+                message = MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK) ? "USE TKBACK" : "SUS TKBACK";
+                break;
+            }
+            activeFunction++;
+            FALLTHROUGH;
+        case MULTI_FUNC_4:
+#ifdef USE_DSHOT
+            if (!ARMING_FLAG(ARMED) && STATE(MULTIROTOR)) {
+                message = MULTI_FUNC_FLAG(MF_TURTLE_MODE) ? "END TURTLE" : "USE TURTLE";
+                break;
+            }
+#endif
+            activeFunction++;
+            FALLTHROUGH;
+        case MULTI_FUNC_5:
+            if (!ARMING_FLAG(ARMED)) {
+                message = "EMERG ARM ";
+                break;
+            }
+            activeFunction++;
+            FALLTHROUGH;
+        case MULTI_FUNC_6:
+            if (!ARMING_FLAG(ARMED)) {
+#if defined(USE_MAG)
+                if (sensors(SENSOR_MAG)) {
+                    message = "CAL COMPAS";
+                    break;
+                }
+#endif
+#if defined(USE_GPS)
+                if (isYawZeroResetAllowed()) {
+                    message = "SET HEADIN";
+                    break;
+                }
+#endif
+            }
+            activeFunction++;
+            break;
+        case MULTI_FUNC_END:
+            message = "*FUNC SET*";
+            break;
+        }
+
+        if (activeFunction != selectedFunction) {
+            if (selectedFunction == MULTI_FUNC_1 && activeFunction == MULTI_FUNC_END) {  // no functions available so end process
+                message = "*NO FUNCS*";
+                setMultifunctionSelection(MULTI_FUNC_NONE);
+            } else {
+                setMultifunctionSelection(activeFunction);
+                if (activeFunction == MULTI_FUNC_END) {   // no messages to display so return
+                    return elemAttr;
+                }
+            }
+        }
+
+        strcpy(buff, message);
+
+        return elemAttr;
+    }
+#endif  // MULTIFUNCTION - functions only, warnings always defined
+
+    /* --- WARNINGS --- */
+    const char *messages[8];
+    uint8_t messageCount = 0;
+    bool warningCondition = false;
+    uint8_t warningFlagID = 1;
+
+    // Low Battery Voltage
+    const batteryState_e batteryVoltageState = checkBatteryVoltageState();
+    warningCondition = batteryVoltageState == BATTERY_CRITICAL || batteryVoltageState == BATTERY_WARNING;
+    if (osdCheckWarning(warningCondition, warningFlagID)) {
+        messages[messageCount++] = batteryVoltageState == BATTERY_CRITICAL ? "VBATT LAND" : "VBATT LOW ";
+    }
+
+    // Low Battery Capacity
+    if (batteryUsesCapacityThresholds()) {
+        const batteryState_e batteryState = getBatteryState();
+        warningCondition = batteryState == BATTERY_CRITICAL || batteryState == BATTERY_WARNING;
+        if (osdCheckWarning(warningCondition, warningFlagID <<= 1)) {
+            messages[messageCount++] = batteryState == BATTERY_CRITICAL ? "BATT EMPTY" : "BATT DYING";
+        }
+    }
+#if defined(USE_GPS)
+    // GPS Fix and Failure
+    if (feature(FEATURE_GPS)) {
+        if (osdCheckWarning(!STATE(GPS_FIX), warningFlagID <<= 1)) {
+            bool gpsFailed = getHwGPSStatus() == HW_SENSOR_UNAVAILABLE;
+            messages[messageCount++] = gpsFailed ? "GPS FAILED" : "NO GPS FIX";
+        }
+    }
+
+    // RTH sanity (warning if RTH heads 200m further away from home than closest point)
+    warningCondition = NAV_Status.state == MW_NAV_STATE_RTH_ENROUTE && !posControl.flags.rthTrackbackActive &&
+                       (posControl.homeDistance - posControl.rthSanityChecker.minimalDistanceToHome) > 20000;
+    if (osdCheckWarning(warningCondition, warningFlagID <<= 1)) {
+        messages[messageCount++] = "RTH SANITY";
+    }
+
+    // Altitude sanity (warning if significant mismatch between estimated and GPS altitude)
+    if (osdCheckWarning(posControl.flags.gpsCfEstimatedAltitudeMismatch, warningFlagID <<= 1)) {
+        messages[messageCount++] = "ALT SANITY";
+    }
+#endif
+
+#if defined(USE_MAG)
+    // Magnetometer failure
+    if (requestedSensors[SENSOR_INDEX_MAG] != MAG_NONE) {
+        hardwareSensorStatus_e magStatus = getHwCompassStatus();
+        if (osdCheckWarning(magStatus == HW_SENSOR_UNAVAILABLE || magStatus == HW_SENSOR_UNHEALTHY, warningFlagID <<= 1)) {
+            messages[messageCount++] = "MAG FAILED";
+        }
+    }
+#endif
+
+#if defined(USE_PITOT)
+    // Pitot sensor validation failure (blocked/failed pitot tube)
+    if (sensors(SENSOR_PITOT) && detectedSensors[SENSOR_INDEX_PITOT] != PITOT_VIRTUAL) {
+        if (osdCheckWarning(pitotHasFailed(), warningFlagID <<= 1)) {
+            messages[messageCount++] = "PITOT FAIL";
+        }
+    }
+#endif
+
+    // Vibration levels   TODO - needs better vibration measurement to be useful
+    // const float vibrationLevel = accGetVibrationLevel();
+    // warningCondition = vibrationLevel > 1.5f;
+    // if (osdCheckWarning(warningCondition, warningFlagID <<= 1, &warningsCount)) {
+        // messages[messageCount++] = vibrationLevel > 2.5f ? "BAD VIBRTN" : "VIBRATION!";
+    // }
+
+#ifdef USE_DEV_TOOLS
+    if (osdCheckWarning(systemConfig()->groundTestMode, warningFlagID <<= 1)) {
+        messages[messageCount++] = "GRD TEST !";
+    }
+#endif
+
+    if (messageCount) {
+        message = messages[(millis() / 1000) % messageCount];     // display each warning on 1s cycle
+        strcpy(buff, message);
+        if (multiFunctionWarning.newWarningActive) {
+            TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+        }
+    }
+
+    return elemAttr;
+}
+
diff --git a/src/main/fc/multifunction.h b/src/main/fc/multifunction.h
index 709e229d6c1..cdbdd17ea0f 100644
--- a/src/main/fc/multifunction.h
+++ b/src/main/fc/multifunction.h
@@ -25,6 +25,7 @@
 #pragma once
 
 #include <stdbool.h>
+#include "drivers/display.h"
 
 typedef struct multiFunctionWarning_s {
     uint8_t osdWarningsFlags;  // bitfield
@@ -61,3 +62,4 @@ typedef enum {
 multi_function_e multiFunctionSelection(void);
 void setMultifunctionSelection(multi_function_e item);
 #endif
+textAttributes_t osdGetMultiFunctionMessage(char *buff);
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index ab99cbb5fa5..47eaa6e29ce 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -84,8 +84,6 @@
 #include "fc/multifunction.h"
 #include "fc/rc_adjustments.h"
 #include "fc/rc_controls.h"
-#include "fc/rc_modes.h"
-#include "fc/runtime_config.h"
 #include "fc/settings.h"
 
 #include "flight/imu.h"
@@ -107,7 +105,6 @@
 #include "sensors/boardalignment.h"
 #include "sensors/compass.h"
 #include "sensors/diagnostics.h"
-#include "sensors/sensors.h"
 #include "sensors/pitotmeter.h"
 #include "sensors/temperature.h"
 #include "sensors/esc_sensor.h"
@@ -203,10 +200,6 @@ static bool fullRedraw = false;
 
 static uint8_t armState;
 
-// Multifunction
-static textAttributes_t osdGetMultiFunctionMessage(char *buff);
-multiFunctionWarning_t multiFunctionWarning;
-
 typedef struct osdMapData_s {
     uint32_t scale;
     char referenceSymbol;
@@ -6387,221 +6380,6 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
     return elemAttr;
 }
 
-static bool osdCheckWarning(bool condition, uint8_t warningFlag)
-{
-    static timeMs_t newWarningEndTime = 0;
-    static uint8_t newWarningFlags = 0;  // bitfield
-    const timeMs_t currentTimeMs = millis();
-
-    /* New warnings dislayed individually for 10s with blinking after which
-     * all current warnings displayed without blinking on 1 second cycle */
-    if (condition) {    // condition required to trigger warning
-        if (!(multiFunctionWarning.osdWarningsFlags & warningFlag)) {  // check for new warnings
-            multiFunctionWarning.osdWarningsFlags |= warningFlag;
-            newWarningFlags |= warningFlag;
-            newWarningEndTime = currentTimeMs + 10000;
-            multiFunctionWarning.newWarningActive = true;
-        }
-#ifdef USE_DEV_TOOLS
-        if (systemConfig()->groundTestMode) {
-            return true;
-        }
-#endif
-        if (currentTimeMs < newWarningEndTime) {
-            return (newWarningFlags & warningFlag);  // filter out new warnings excluding older warnings
-        } else {
-            newWarningFlags = 0;
-            multiFunctionWarning.newWarningActive = false;
-        }
-        return true;
-    } else if (multiFunctionWarning.osdWarningsFlags & warningFlag) {
-        multiFunctionWarning.osdWarningsFlags &= ~warningFlag;
-    }
-
-    return false;
-}
-
-static textAttributes_t osdGetMultiFunctionMessage(char *buff)
-{
-    /* Message length limit 10 char max */
-
-    textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE;
-    const char *message = NULL;
-
-#ifdef USE_MULTI_FUNCTIONS
-    /* --- FUNCTIONS --- */
-    multi_function_e selectedFunction = multiFunctionSelection();
-
-    if (selectedFunction) {
-        multi_function_e activeFunction = selectedFunction;
-
-        switch (selectedFunction) {
-        case MULTI_FUNC_NONE:
-        case MULTI_FUNC_1:
-            if (ARMING_FLAG(ARMED)) {
-                message = posControl.flags.manualEmergLandActive ? "ABORT LAND" : "EMERG LAND";
-                break;
-            }
-            activeFunction++;
-            FALLTHROUGH;
-        case MULTI_FUNC_2:
-#if defined(USE_SAFE_HOME)
-            if (navConfig()->general.flags.safehome_usage_mode != SAFEHOME_USAGE_OFF) {
-                message = MULTI_FUNC_FLAG(MF_SUSPEND_SAFEHOMES) ? "USE SFHOME" : "SUS SFHOME";
-                break;
-            }
-#endif
-            activeFunction++;
-            FALLTHROUGH;
-        case MULTI_FUNC_3:
-            if (navConfig()->general.flags.rth_trackback_mode != RTH_TRACKBACK_OFF) {
-                message = MULTI_FUNC_FLAG(MF_SUSPEND_TRACKBACK) ? "USE TKBACK" : "SUS TKBACK";
-                break;
-            }
-            activeFunction++;
-            FALLTHROUGH;
-        case MULTI_FUNC_4:
-#ifdef USE_DSHOT
-            if (!ARMING_FLAG(ARMED) && STATE(MULTIROTOR)) {
-                message = MULTI_FUNC_FLAG(MF_TURTLE_MODE) ? "END TURTLE" : "USE TURTLE";
-                break;
-            }
-#endif
-            activeFunction++;
-            FALLTHROUGH;
-        case MULTI_FUNC_5:
-            if (!ARMING_FLAG(ARMED)) {
-                message = "EMERG ARM ";
-                break;
-            }
-            activeFunction++;
-            FALLTHROUGH;
-        case MULTI_FUNC_6:
-            if (!ARMING_FLAG(ARMED)) {
-#if defined(USE_MAG)
-                if (sensors(SENSOR_MAG)) {
-                    message = "CAL COMPAS";
-                    break;
-                }
-#endif
-#if defined(USE_GPS)
-                if (isYawZeroResetAllowed()) {
-                    message = "SET HEADIN";
-                    break;
-                }
-#endif
-            }
-            activeFunction++;
-            break;
-        case MULTI_FUNC_END:
-            message = "*FUNC SET*";
-            break;
-        }
-
-        if (activeFunction != selectedFunction) {
-            if (selectedFunction == MULTI_FUNC_1 && activeFunction == MULTI_FUNC_END) {  // no functions available so end process
-                message = "*NO FUNCS*";
-                setMultifunctionSelection(MULTI_FUNC_NONE);
-            } else {
-                setMultifunctionSelection(activeFunction);
-                if (activeFunction == MULTI_FUNC_END) {   // no messages to display so return
-                    return elemAttr;
-                }
-            }
-        }
-
-        strcpy(buff, message);
-
-        return elemAttr;
-    }
-#endif  // MULTIFUNCTION - functions only, warnings always defined
-
-    /* --- WARNINGS --- */
-    const char *messages[8];
-    uint8_t messageCount = 0;
-    bool warningCondition = false;
-    uint8_t warningFlagID = 1;
-
-    // Low Battery Voltage
-    const batteryState_e batteryVoltageState = checkBatteryVoltageState();
-    warningCondition = batteryVoltageState == BATTERY_CRITICAL || batteryVoltageState == BATTERY_WARNING;
-    if (osdCheckWarning(warningCondition, warningFlagID)) {
-        messages[messageCount++] = batteryVoltageState == BATTERY_CRITICAL ? "VBATT LAND" : "VBATT LOW ";
-    }
-
-    // Low Battery Capacity
-    if (batteryUsesCapacityThresholds()) {
-        const batteryState_e batteryState = getBatteryState();
-        warningCondition = batteryState == BATTERY_CRITICAL || batteryState == BATTERY_WARNING;
-        if (osdCheckWarning(warningCondition, warningFlagID <<= 1)) {
-            messages[messageCount++] = batteryState == BATTERY_CRITICAL ? "BATT EMPTY" : "BATT DYING";
-        }
-    }
-#if defined(USE_GPS)
-    // GPS Fix and Failure
-    if (feature(FEATURE_GPS)) {
-        if (osdCheckWarning(!STATE(GPS_FIX), warningFlagID <<= 1)) {
-            bool gpsFailed = getHwGPSStatus() == HW_SENSOR_UNAVAILABLE;
-            messages[messageCount++] = gpsFailed ? "GPS FAILED" : "NO GPS FIX";
-        }
-    }
-
-    // RTH sanity (warning if RTH heads 200m further away from home than closest point)
-    warningCondition = NAV_Status.state == MW_NAV_STATE_RTH_ENROUTE && !posControl.flags.rthTrackbackActive &&
-                       (posControl.homeDistance - posControl.rthSanityChecker.minimalDistanceToHome) > 20000;
-    if (osdCheckWarning(warningCondition, warningFlagID <<= 1)) {
-        messages[messageCount++] = "RTH SANITY";
-    }
-
-    // Altitude sanity (warning if significant mismatch between estimated and GPS altitude)
-    if (osdCheckWarning(posControl.flags.gpsCfEstimatedAltitudeMismatch, warningFlagID <<= 1)) {
-        messages[messageCount++] = "ALT SANITY";
-    }
-#endif
-
-#if defined(USE_MAG)
-    // Magnetometer failure
-    if (requestedSensors[SENSOR_INDEX_MAG] != MAG_NONE) {
-        hardwareSensorStatus_e magStatus = getHwCompassStatus();
-        if (osdCheckWarning(magStatus == HW_SENSOR_UNAVAILABLE || magStatus == HW_SENSOR_UNHEALTHY, warningFlagID <<= 1)) {
-            messages[messageCount++] = "MAG FAILED";
-        }
-    }
-#endif
-
-#if defined(USE_PITOT)
-    // Pitot sensor validation failure (blocked/failed pitot tube)
-    if (sensors(SENSOR_PITOT) && detectedSensors[SENSOR_INDEX_PITOT] != PITOT_VIRTUAL) {
-        if (osdCheckWarning(pitotHasFailed(), warningFlagID <<= 1)) {
-            messages[messageCount++] = "PITOT FAIL";
-        }
-    }
-#endif
-
-    // Vibration levels   TODO - needs better vibration measurement to be useful
-    // const float vibrationLevel = accGetVibrationLevel();
-    // warningCondition = vibrationLevel > 1.5f;
-    // if (osdCheckWarning(warningCondition, warningFlagID <<= 1, &warningsCount)) {
-        // messages[messageCount++] = vibrationLevel > 2.5f ? "BAD VIBRTN" : "VIBRATION!";
-    // }
-
-#ifdef USE_DEV_TOOLS
-    if (osdCheckWarning(systemConfig()->groundTestMode, warningFlagID <<= 1)) {
-        messages[messageCount++] = "GRD TEST !";
-    }
-#endif
-
-    if (messageCount) {
-        message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)];    // display each warning on 1s cycle
-        strcpy(buff, message);
-        if (multiFunctionWarning.newWarningActive) {
-            TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
-        }
-    }
-
-    return elemAttr;
-}
-
 void osdDrawCustomItem(uint8_t item){
     osdDrawSingleElement(item);
 }

From 5837d2cf97b6aca43708cddda8ba543815de11df Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Fri, 15 May 2026 10:49:48 -0500
Subject: [PATCH 099/102] Revert "use explicitly assign motor and servo timers
 before auto timers" (#11445)
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Reverts PR #11445 (commit f6281a600c, merge commit 4955d4922c) for 9.x
compatibility.

The two-pass priority algorithm is correct behavior but introduces a
backward-compatibility break: Configurator 9.0.1 re-implements the
firmware assignment algorithm in JS. Old Configurator + new firmware
(or vice versa) shows wrong pin assignments for targets with explicit
timerOverrides (e.g. OUTPUT_MODE_MOTORS / SERVOS in config.c).

To re-implement for 10.x without this problem:
  1. Merge this revert commit into maintenance-10.x (so both branches
     share the reverted state as their common ancestor).
  2. Apply the two-pass algorithm again as new commits on top of 10.x.
  3. Add MSP2_INAV_OUTPUT_ASSIGNMENT so the firmware reports final
     assignments directly — eliminates the JS/C algorithm duplication
     that caused this issue.
See: claude/developer/workspace/pwm-compat-project/project-plan-output-assignment-api.md
---
 src/main/drivers/pwm_mapping.c | 134 +++++++++++++++++++--------------
 1 file changed, 77 insertions(+), 57 deletions(-)

diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c
index 120b3d4fc91..2d01127a504 100644
--- a/src/main/drivers/pwm_mapping.c
+++ b/src/main/drivers/pwm_mapping.c
@@ -270,23 +270,43 @@ uint8_t pwmClaimTimer(HAL_Timer_t *tim, uint32_t usageFlags) {
     return changed;
 }
 
-static void pwmAssignOutput(timMotorServoHardware_t *timOutputs, timerHardware_t *timHw, int type)
+void pwmEnsureEnoughtMotors(uint8_t motorCount)
 {
-    switch (type) {
-        case MAP_TO_MOTOR_OUTPUT:
-            timHw->usageFlags &= TIM_USE_MOTOR;
-            timOutputs->timMotors[timOutputs->maxTimMotorCount++] = timHw;
-            pwmClaimTimer(timHw->tim, timHw->usageFlags);
-            break;
-        case MAP_TO_SERVO_OUTPUT:
-            timHw->usageFlags &= TIM_USE_SERVO;
-            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;
+    uint8_t motorOnlyOutputs = 0;
+
+    for (int idx = 0; idx < timerHardwareCount; idx++) {
+        timerHardware_t *timHw = &timerHardware[idx];
+
+        timerHardwareOverride(timHw);
+
+        if (checkPwmTimerConflicts(timHw)) {
+            continue;
+        }
+
+        if (TIM_IS_MOTOR_ONLY(timHw->usageFlags)) {
+            motorOnlyOutputs++;
+            motorOnlyOutputs += pwmClaimTimer(timHw->tim, timHw->usageFlags);
+        }
+    }
+
+    for (int idx = 0; idx < timerHardwareCount; idx++) {
+        timerHardware_t *timHw = &timerHardware[idx];
+
+        if (checkPwmTimerConflicts(timHw)) {
+            continue;
+        }
+
+        if (TIM_IS_MOTOR(timHw->usageFlags) && !TIM_IS_MOTOR_ONLY(timHw->usageFlags)) {
+            if (motorOnlyOutputs < motorCount) {
+                timHw->usageFlags &= ~TIM_USE_SERVO;
+                timHw->usageFlags |= TIM_USE_MOTOR;
+                motorOnlyOutputs++;
+                motorOnlyOutputs += pwmClaimTimer(timHw->tim, timHw->usageFlags);
+            } else {
+                timHw->usageFlags &= ~TIM_USE_MOTOR;
+                pwmClaimTimer(timHw->tim, timHw->usageFlags);
+            }
+        }
     }
 }
 
@@ -296,54 +316,54 @@ void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerU
     timOutputs->maxTimMotorCount = 0;
     timOutputs->maxTimServoCount = 0;
 
-    const uint8_t motorCount = getMotorCount();
+    uint8_t motorCount = getMotorCount();
+    uint8_t motorIdx = 0;
 
-    // Apply configurator overrides to all timer outputs
-    for (int idx = 0; idx < timerHardwareCount; idx++) {
-        timerHardwareOverride(&timerHardware[idx]);
-    }
+    pwmEnsureEnoughtMotors(motorCount);
 
-    // Assign outputs in priority order: dedicated first, then auto.
-    // Within each pass, array order (S1, S2, ...) is preserved.
-    // Motors and servos cannot share a timer, enforced by pwmHasMotorOnTimer/pwmHasServoOnTimer.
-    for (int priority = 0; priority < 2; priority++) {
-        uint8_t motorIdx = timOutputs->maxTimMotorCount;
+    for (int idx = 0; idx < timerHardwareCount; idx++) {
+        timerHardware_t *timHw = &timerHardware[idx];
 
-        for (int idx = 0; idx < timerHardwareCount; idx++) {
-            timerHardware_t *timHw = &timerHardware[idx];
-            outputMode_e mode = timerOverrides(timer2id(timHw->tim))->outputMode;
-            bool isDedicated = (priority == 0);
+        int type = MAP_TO_NONE;
 
-            if (checkPwmTimerConflicts(timHw)) {
-                if (priority == 0) {
-                    LOG_WARNING(PWM, "Timer output %d skipped", idx);
-                }
-                continue;
-            }
+        // Check for known conflicts (i.e. UART, LEDSTRIP, Rangefinder and ADC)
+        if (checkPwmTimerConflicts(timHw)) {
+            LOG_WARNING(PWM, "Timer output %d skipped", idx);
+            continue;
+        }
 
-            // Motors: dedicated (OUTPUT_MODE_MOTORS) first, then auto
-            if (TIM_IS_MOTOR(timHw->usageFlags) && motorIdx < motorCount
-                    && !pwmHasServoOnTimer(timOutputs, timHw->tim)
-                    && (isDedicated ? mode == OUTPUT_MODE_MOTORS : mode != OUTPUT_MODE_MOTORS)) {
-                pwmAssignOutput(timOutputs, timHw, MAP_TO_MOTOR_OUTPUT);
-                motorIdx++;
-                continue;
-            }
+        // Make sure first motorCount motor outputs get assigned to motor
+        if (TIM_IS_MOTOR(timHw->usageFlags) && (motorIdx < motorCount)) {
+            timHw->usageFlags &= ~TIM_USE_SERVO;
+            pwmClaimTimer(timHw->tim, timHw->usageFlags);
+            motorIdx += 1;
+        }
 
-            // Servos: dedicated (OUTPUT_MODE_SERVOS) first, then auto
-            if (TIM_IS_SERVO(timHw->usageFlags)
-                    && !pwmHasMotorOnTimer(timOutputs, timHw->tim)
-                    && (isDedicated ? mode == OUTPUT_MODE_SERVOS : mode != OUTPUT_MODE_SERVOS)) {
-                pwmAssignOutput(timOutputs, timHw, MAP_TO_SERVO_OUTPUT);
-                continue;
-            }
+        if (TIM_IS_SERVO(timHw->usageFlags) && !pwmHasMotorOnTimer(timOutputs, timHw->tim)) {
+            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;
+        }
 
-            // LEDs: only on the auto pass, and only if timer is uncontested
-            if (!isDedicated && TIM_IS_LED(timHw->usageFlags)
-                    && !pwmHasMotorOnTimer(timOutputs, timHw->tim)
-                    && !pwmHasServoOnTimer(timOutputs, timHw->tim)) {
-                pwmAssignOutput(timOutputs, timHw, MAP_TO_LED_OUTPUT);
-            }
+        switch(type) {
+            case MAP_TO_MOTOR_OUTPUT:
+                timHw->usageFlags &= TIM_USE_MOTOR;
+                timOutputs->timMotors[timOutputs->maxTimMotorCount++] = timHw;
+                pwmClaimTimer(timHw->tim, timHw->usageFlags);
+                break;
+            case MAP_TO_SERVO_OUTPUT:
+                timHw->usageFlags &= TIM_USE_SERVO;
+                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;
         }
     }
 }

From 73c59e867ca282cea1632d5f54bacc44d1098be0 Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Tue, 19 May 2026 01:13:49 -0500
Subject: [PATCH 100/102] fix(pitotmeter): guard MS5525 i2c address writes with
 USE_I2C

SITL target undefines USE_I2C, so the i2c member of the busdev union
does not exist in that build. Wrap the two address-assignment lines
with #ifdef USE_I2C so the code compiles without hardware I2C support.
---
 src/main/drivers/pitotmeter/pitotmeter_ms5525.c | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/src/main/drivers/pitotmeter/pitotmeter_ms5525.c b/src/main/drivers/pitotmeter/pitotmeter_ms5525.c
index e26114b2e95..303c4becb5f 100644
--- a/src/main/drivers/pitotmeter/pitotmeter_ms5525.c
+++ b/src/main/drivers/pitotmeter/pitotmeter_ms5525.c
@@ -172,10 +172,14 @@ bool ms5525Detect(pitotDev_t * pitot)
     }
 
     // Try primary address 0x76
+#ifdef USE_I2C
     pitot->busDev->busdev.i2c.address = MS5525_ADDR_1;
+#endif
     if (!deviceDetect(pitot->busDev)) {
         // Fallback to secondary 0x77
+#ifdef USE_I2C
         pitot->busDev->busdev.i2c.address = MS5525_ADDR_2;
+#endif
         if (!deviceDetect(pitot->busDev)) {
             busDeviceDeInit(pitot->busDev);
             return false;

From 1d5d95b4a0b0de0505fdaf6eff017b126a7a97b1 Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Tue, 19 May 2026 01:45:22 -0500
Subject: [PATCH 101/102] osd: guard outBuff null-termination in
 drawStat_AverageEfficiency

On HD displays with valid efficiency data and 4-digit (DJI compat) mode,
the efficiency string can reach 15 chars + null = 16 bytes, overflowing
outBuff[15] by one byte. Adding an explicit null-termination guard at the
end of all build paths prevents this and matches the inav3 reference.
---
 src/main/io/osd.c | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 9208a1b572e..55bca838f34 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -5277,6 +5277,7 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b
             osdWriteChar(outBuff + strlen(outBuff), SYM_WH_KM);
         }
     }
+    osdWriteChar(outBuff + strlen(outBuff), '\0');
 
     displayWrite(osdDisplayPort, statValX, row++, outBuff);
 

From b72ae269bbd0830304ca8e41012f49878a3498b1 Mon Sep 17 00:00:00 2001
From: Ray Morris <deepmagicbeginshere@gmail.com>
Date: Tue, 19 May 2026 01:45:28 -0500
Subject: [PATCH 102/102] cmake: add ASAN option for SITL builds

Enables AddressSanitizer via -DASAN=ON. Use alongside -DDEBUG=ON for
readable stack traces. Follows the existing DEBUG flag pattern.
---
 cmake/sitl.cmake | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/cmake/sitl.cmake b/cmake/sitl.cmake
index 9adecab53cc..1983175c8de 100644
--- a/cmake/sitl.cmake
+++ b/cmake/sitl.cmake
@@ -58,6 +58,12 @@ if(DEBUG)
     list(APPEND SITL_COMPILE_OPTIONS -g)
 endif()
 
+if(ASAN)
+    message(STATUS "AddressSanitizer enabled.")
+    list(APPEND SITL_COMPILE_OPTIONS -fsanitize=address -fno-omit-frame-pointer)
+    list(APPEND SITL_LINK_OPTIONS -fsanitize=address)
+endif()
+
 if(NOT MACOSX)
     set(SITL_COMPILE_OPTIONS ${SITL_COMPILE_OPTIONS}
         -Wno-return-local-addr