From 6f58bdbf7f70b88c477394e51e2be712ba8985f1 Mon Sep 17 00:00:00 2001
From: Darren Lines <darren@darrenlines.uk>
Date: Fri, 29 Mar 2024 13:27:30 +0000
Subject: [PATCH 01/39] Change SmartPort sensor IDs for GPS and Modes

The current implementation of INAV SmartPort telemetry uses temperature sensor IDs for Modes and GNSS data. This causes issues on transmitter firmware that more strictly adhere to the telemetry ID's intended use. The Tmp sensors can't be converted to a different type, so they can't be read correctly.

This PR changes the IDs to some that are not associated with specific functions or types. There is also an option to use the legacy IDs to allow for a deprecation period. This legacy option should be removed in INAV 10.0.
---
 docs/Settings.md               |  10 ++
 docs/Telemetry.md              |   8 +-
 src/main/fc/settings.yaml      |   4 +
 src/main/telemetry/smartport.c | 180 +++++++++++++++++----------------
 src/main/telemetry/telemetry.c |   1 +
 src/main/telemetry/telemetry.h |   1 +
 6 files changed, 117 insertions(+), 87 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index 61195109625..f34552ec430 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -1162,6 +1162,16 @@ S.Port telemetry: Send pitch and roll degrees*10 instead of raw accelerometer da
 
 ---
 
+### frsky_use_legacy_gps_mode_sensor_ids
+
+S.Port telemetry: If `ON`, send the legacy telemetry IDs for modes (Tmp1) and GNSS (Tmp2). These are old IDs, deprecated, and will be removed in INAV 10.0. Tools and scripts using these IDs should be updated to use the new IDs of **470** for Modes and **480** for GNSS. Default: 'OFF'
+
+| Default | Min | Max |
+| --- | --- | --- |
+| OFF | OFF | ON |
+
+---
+
 ### fw_autotune_max_rate_deflection
 
 The target percentage of maximum mixer output used for determining the rates in `AUTO` and `LIMIT`.
diff --git a/docs/Telemetry.md b/docs/Telemetry.md
index 996630e66fa..46118e033d9 100644
--- a/docs/Telemetry.md
+++ b/docs/Telemetry.md
@@ -102,17 +102,21 @@ The following sensors are transmitted
 * **VSpd** : vertical speed, unit is cm/s.
 * **Hdg** : heading, North is 0°, South is 180°.
 * **AccX,Y,Z** : accelerometer values (not sent if `frsky_pitch_roll = ON`).
-* **Tmp1** : flight mode, sent as 5 digits. Number is sent as **ABCDE** detailed below. The numbers are additives (for example: if digit C is 6, it means both position hold and altitude hold are active) :
+* **470** : flight mode, sent as 5 digits. Number is sent as **ABCDE** detailed below. The numbers are additives (for example: if digit C is 6, it means both position hold and altitude hold are active) :
   * **A** : 1 = flaperon mode, 2 = auto tune mode, 4 = failsafe mode
   * **B** : 1 = return to home, 2 = waypoint mode, 4 = headfree mode
   * **C** : 1 = heading hold, 2 = altitude hold, 4 = position hold
   * **D** : 1 = angle mode, 2 = horizon mode, 4 = passthru mode
   * **E** : 1 = ok to arm, 2 = arming is prevented, 4 = armed
-* **Tmp2** : GPS lock status, accuracy, home reset trigger, and number of satellites. Number is sent as **ABCD** detailed below. Typical minimum GPS 3D lock value is 3906 (GPS locked and home fixed, HDOP highest accuracy, 6 satellites).
+  
+  _NOTE_ This sensor used to be **Tmp1**. The ID has been reassigned in INAV 8.0. The old ID of **Tmp1** can still be used, by using `set frsky_use_legacy_gps_mode_sensor_ids = ON`. This is deprecated and will be removed in INAV 10.0. All tools and scripts using the old IDs should be updated to use the new ID.
+* **480** : GPS lock status, accuracy, home reset trigger, and number of satellites. Number is sent as **ABCD** detailed below. Typical minimum GPS 3D lock value is 3906 (GPS locked and home fixed, HDOP highest accuracy, 6 satellites).
   * **A** : 1 = GPS fix, 2 = GPS home fix, 4 = home reset (numbers are additive)
   * **B** : GPS accuracy based on HDOP (0 = lowest to 9 = highest accuracy)
   * **C** : number of satellites locked (digit C & D are the number of locked satellites)
   * **D** : number of satellites locked (if 14 satellites are locked, C = 1 & D = 4)
+  
+  _NOTE_ This sensor used to be **Tmp2**. The ID has been reassigned in INAV 8.0. The old ID of **Tmp2** can still be used, by using `set frsky_use_legacy_gps_mode_sensor_ids = ON`. This is deprecated and will be removed in INAV 10.0. All tools and scripts using the old IDs should be updated to use the new ID.
 * **GAlt** : GPS altitude, sea level is zero.
 * **ASpd** : true air speed, from pitot sensor. This is _Knots * 10_
 * **A4** : average cell value. Warning : unlike FLVSS and MLVSS sensors, you do not get actual lowest value of a cell, but an average : (total lipo voltage) / (number of cells)
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index e3e69c7487f..6a82489718a 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -2986,6 +2986,10 @@ groups:
         description: "S.Port telemetry: Send pitch and roll degrees*10 instead of raw accelerometer data"
         default_value: OFF
         type: bool
+      - name: frsky_use_legacy_gps_mode_sensor_ids
+        description: "S.Port telemetry: If `ON`, send the legacy telemetry IDs for modes (Tmp1) and GNSS (Tmp2). These are old IDs, deprecated, and will be removed in INAV 10.0. Tools and scripts using these IDs should be updated to use the new IDs of **470** for Modes and **480** for GNSS. Default: 'OFF'"
+        default_value: OFF
+        type: bool
       - name: report_cell_voltage
         description: "S.Port and IBUS telemetry: Send the average cell voltage if set to ON"
         default_value: OFF
diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c
index 9d804be52d8..94e318e8a7a 100755
--- a/src/main/telemetry/smartport.c
+++ b/src/main/telemetry/smartport.c
@@ -63,66 +63,68 @@
 // these data identifiers are obtained from https://github.com/opentx/opentx/blob/2.3/radio/src/telemetry/frsky.h
 enum
 {
-    FSSP_DATAID_SPEED      = 0x0830 ,
-    FSSP_DATAID_VFAS       = 0x0210 ,
-    FSSP_DATAID_CURRENT    = 0x0200 ,
-    FSSP_DATAID_RPM        = 0x050F ,
-    FSSP_DATAID_ALTITUDE   = 0x0100 ,
-    FSSP_DATAID_FUEL       = 0x0600 ,
-    FSSP_DATAID_ADC1       = 0xF102 ,
-    FSSP_DATAID_ADC2       = 0xF103 ,
-    FSSP_DATAID_LATLONG    = 0x0800 ,
-    FSSP_DATAID_CAP_USED   = 0x0600 ,
-    FSSP_DATAID_VARIO      = 0x0110 ,
-    FSSP_DATAID_CELLS      = 0x0300 ,
-    FSSP_DATAID_CELLS_LAST = 0x030F ,
-    FSSP_DATAID_HEADING    = 0x0840 ,
-    FSSP_DATAID_FPV        = 0x0450 ,
-    FSSP_DATAID_PITCH      = 0x0430 ,
-    FSSP_DATAID_ROLL       = 0x0440 ,
-    FSSP_DATAID_ACCX       = 0x0700 ,
-    FSSP_DATAID_ACCY       = 0x0710 ,
-    FSSP_DATAID_ACCZ       = 0x0720 ,
-    FSSP_DATAID_T1         = 0x0400 ,
-    FSSP_DATAID_T2         = 0x0410 ,
-    FSSP_DATAID_HOME_DIST  = 0x0420 ,
-    FSSP_DATAID_GPS_ALT    = 0x0820 ,
-    FSSP_DATAID_ASPD       = 0x0A00 ,
-    FSSP_DATAID_A3         = 0x0900 ,
-    FSSP_DATAID_A4         = 0x0910 ,
-    FSSP_DATAID_AZIMUTH    = 0x0460
+    FSSP_DATAID_SPEED           = 0x0830,
+    FSSP_DATAID_VFAS            = 0x0210,
+    FSSP_DATAID_CURRENT         = 0x0200,
+    FSSP_DATAID_RPM             = 0x050F,
+    FSSP_DATAID_ALTITUDE        = 0x0100,
+    FSSP_DATAID_FUEL            = 0x0600,
+    FSSP_DATAID_ADC1            = 0xF102,
+    FSSP_DATAID_ADC2            = 0xF103,
+    FSSP_DATAID_LATLONG         = 0x0800,
+    FSSP_DATAID_CAP_USED        = 0x0600,
+    FSSP_DATAID_VARIO           = 0x0110,
+    FSSP_DATAID_CELLS           = 0x0300,
+    FSSP_DATAID_CELLS_LAST      = 0x030F,
+    FSSP_DATAID_HEADING         = 0x0840,
+    FSSP_DATAID_FPV             = 0x0450,
+    FSSP_DATAID_PITCH           = 0x0430,
+    FSSP_DATAID_ROLL            = 0x0440,
+    FSSP_DATAID_ACCX            = 0x0700,
+    FSSP_DATAID_ACCY            = 0x0710,
+    FSSP_DATAID_ACCZ            = 0x0720,
+    FSSP_DATAID_LEGACY_MODES    = 0x0400, // Deprecated. Should be removed in INAV 10.0
+    FSSP_DATAID_LEGACY_GNSS     = 0x0410, // Deprecated. Should be removed in INAV 10.0
+    FSSP_DATAID_HOME_DIST       = 0x0420,
+    FSSP_DATAID_GPS_ALT         = 0x0820,
+    FSSP_DATAID_ASPD            = 0x0A00,
+    FSSP_DATAID_A3              = 0x0900,
+    FSSP_DATAID_A4              = 0x0910,
+    FSSP_DATAID_AZIMUTH         = 0x0460,
+    FSSP_DATAID_MODES           = 0x0470,
+    FSSP_DATAID_GNSS            = 0x0480,
 };
 
 const uint16_t frSkyDataIdTable[] = {
-    FSSP_DATAID_SPEED     ,
-    FSSP_DATAID_VFAS      ,
-    FSSP_DATAID_CURRENT   ,
-    //FSSP_DATAID_RPM       ,
-    FSSP_DATAID_ALTITUDE  ,
-    FSSP_DATAID_FUEL      ,
-    //FSSP_DATAID_ADC1      ,
-    //FSSP_DATAID_ADC2      ,
-    FSSP_DATAID_LATLONG   ,
-    FSSP_DATAID_LATLONG   , // twice
-    //FSSP_DATAID_CAP_USED  ,
-    FSSP_DATAID_VARIO     ,
-    //FSSP_DATAID_CELLS     ,
+    FSSP_DATAID_SPEED,
+    FSSP_DATAID_VFAS,
+    FSSP_DATAID_CURRENT,
+    //FSSP_DATAID_RPM,
+    FSSP_DATAID_ALTITUDE,
+    FSSP_DATAID_FUEL,
+    //FSSP_DATAID_ADC1,
+    //FSSP_DATAID_ADC2,
+    FSSP_DATAID_LATLONG,
+    FSSP_DATAID_LATLONG, // twice
+    //FSSP_DATAID_CAP_USED,
+    FSSP_DATAID_VARIO,
+    //FSSP_DATAID_CELLS,
     //FSSP_DATAID_CELLS_LAST,
-    FSSP_DATAID_HEADING   ,
-    FSSP_DATAID_FPV       ,
-    FSSP_DATAID_PITCH     ,
-    FSSP_DATAID_ROLL      ,
-    FSSP_DATAID_ACCX      ,
-    FSSP_DATAID_ACCY      ,
-    FSSP_DATAID_ACCZ      ,
-    FSSP_DATAID_T1        ,
-    FSSP_DATAID_T2        ,
-    FSSP_DATAID_HOME_DIST ,
-    FSSP_DATAID_GPS_ALT   ,
-    FSSP_DATAID_ASPD      ,
-    // FSSP_DATAID_A3        ,
-    FSSP_DATAID_A4        ,
-    FSSP_DATAID_AZIMUTH ,
+    FSSP_DATAID_HEADING,
+    FSSP_DATAID_FPV,
+    FSSP_DATAID_PITCH,
+    FSSP_DATAID_ROLL,
+    FSSP_DATAID_ACCX,
+    FSSP_DATAID_ACCY,
+    FSSP_DATAID_ACCZ,
+    FSSP_DATAID_MODES,
+    FSSP_DATAID_GNSS,
+    FSSP_DATAID_HOME_DIST,
+    FSSP_DATAID_GPS_ALT,
+    FSSP_DATAID_ASPD,
+    // FSSP_DATAID_A3,
+    FSSP_DATAID_A4,
+    FSSP_DATAID_AZIMUTH,
     0
 };
 
@@ -467,27 +469,27 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
         smartPortIdCnt++;
 
         switch (id) {
-            case FSSP_DATAID_VFAS       :
+            case FSSP_DATAID_VFAS:
                 if (isBatteryVoltageConfigured()) {
                     uint16_t vfasVoltage = telemetryConfig()->report_cell_voltage ? getBatteryAverageCellVoltage() : getBatteryVoltage();
                     smartPortSendPackage(id, vfasVoltage);
                     *clearToSend = false;
                 }
                 break;
-            case FSSP_DATAID_CURRENT    :
+            case FSSP_DATAID_CURRENT:
                 if (isAmperageConfigured()) {
                     smartPortSendPackage(id, getAmperage() / 10); // given in 10mA steps, unknown requested unit
                     *clearToSend = false;
                 }
                 break;
-            //case FSSP_DATAID_RPM        :
-            case FSSP_DATAID_ALTITUDE   :
+            //case FSSP_DATAID_RPM:
+            case FSSP_DATAID_ALTITUDE:
                 if (sensors(SENSOR_BARO)) {
                     smartPortSendPackage(id, getEstimatedActualPosition(Z)); // unknown given unit, requested 100 = 1 meter
                     *clearToSend = false;
                 }
                 break;
-            case FSSP_DATAID_FUEL       :
+            case FSSP_DATAID_FUEL:
                 if (telemetryConfig()->smartportFuelUnit == SMARTPORT_FUEL_UNIT_PERCENT) {
                     smartPortSendPackage(id, calculateBatteryPercentage()); // Show remaining battery % if smartport_fuel_percent=ON
                     *clearToSend = false;
@@ -496,63 +498,71 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
                     *clearToSend = false;
                 }
                 break;
-            //case FSSP_DATAID_ADC1       :
-            //case FSSP_DATAID_ADC2       :
-            //case FSSP_DATAID_CAP_USED   :
-            case FSSP_DATAID_VARIO      :
+            //case FSSP_DATAID_ADC1:
+            //case FSSP_DATAID_ADC2:
+            //case FSSP_DATAID_CAP_USED:
+            case FSSP_DATAID_VARIO:
                 if (sensors(SENSOR_BARO)) {
                     smartPortSendPackage(id, lrintf(getEstimatedActualVelocity(Z))); // unknown given unit but requested in 100 = 1m/s
                     *clearToSend = false;
                 }
                 break;
-            case FSSP_DATAID_HEADING    :
+            case FSSP_DATAID_HEADING:
                 smartPortSendPackage(id, attitude.values.yaw * 10); // given in 10*deg, requested in 10000 = 100 deg
                 *clearToSend = false;
                 break;
-            case FSSP_DATAID_PITCH      :
+            case FSSP_DATAID_PITCH:
                 if (telemetryConfig()->frsky_pitch_roll) {
                     smartPortSendPackage(id, attitude.values.pitch); // given in 10*deg
                     *clearToSend = false;
                 }
                 break;
-            case FSSP_DATAID_ROLL       :
+            case FSSP_DATAID_ROLL:
                 if (telemetryConfig()->frsky_pitch_roll) {
                     smartPortSendPackage(id, attitude.values.roll); // given in 10*deg
                     *clearToSend = false;
                 }
                 break;
-            case FSSP_DATAID_ACCX       :
+            case FSSP_DATAID_ACCX:
                 if (!telemetryConfig()->frsky_pitch_roll) {
                     smartPortSendPackage(id, lrintf(100 * acc.accADCf[X]));
                     *clearToSend = false;
                 }
                 break;
-            case FSSP_DATAID_ACCY       :
+            case FSSP_DATAID_ACCY:
                 if (!telemetryConfig()->frsky_pitch_roll) {
                     smartPortSendPackage(id, lrintf(100 * acc.accADCf[Y]));
                     *clearToSend = false;
                 }
                 break;
-            case FSSP_DATAID_ACCZ       :
+            case FSSP_DATAID_ACCZ:
                 if (!telemetryConfig()->frsky_pitch_roll) {
                     smartPortSendPackage(id, lrintf(100 * acc.accADCf[Z]));
                     *clearToSend = false;
                 }
                 break;
-            case FSSP_DATAID_T1         :
+            case FSSP_DATAID_MODES:
                 {
+                    if (telemetryConfig()->frsky_use_legacy_gps_mode_sensor_ids)
+                        id = FSSP_DATAID_LEGACY_MODES;
+
                     smartPortSendPackage(id, frskyGetFlightMode());
                     *clearToSend = false;
                     break;
                 }
 #ifdef USE_GPS
-            case FSSP_DATAID_T2         :
-                if (smartPortShouldSendGPSData()) {
-                    smartPortSendPackage(id, frskyGetGPSState());
-                    *clearToSend = false;
+            case FSSP_DATAID_GNSS:
+                {
+                    if (telemetryConfig()->frsky_use_legacy_gps_mode_sensor_ids)
+                        id = FSSP_DATAID_LEGACY_GNSS;
+
+                    if (smartPortShouldSendGPSData()) {
+                        smartPortSendPackage(id, frskyGetGPSState());
+                        *clearToSend = false;
+                    }
+                    break;
                 }
-                break;
-            case FSSP_DATAID_SPEED      :
+            case FSSP_DATAID_SPEED:
                 if (smartPortShouldSendGPSData()) {
                     //convert to knots: 1cm/s = 0.0194384449 knots
                     //Speed should be sent in knots/1000 (GPS speed is in cm/s)
@@ -561,7 +571,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
                     *clearToSend = false;
                 }
                 break;
-            case FSSP_DATAID_LATLONG    :
+            case FSSP_DATAID_LATLONG:
                 if (smartPortShouldSendGPSData()) {
                     uint32_t tmpui = 0;
                     // the same ID is sent twice, one for longitude, one for latitude
@@ -581,25 +591,25 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
                     *clearToSend = false;
                 }
                 break;
-            case FSSP_DATAID_HOME_DIST  :
+            case FSSP_DATAID_HOME_DIST:
                 if (smartPortShouldSendGPSData()) {
                     smartPortSendPackage(id, GPS_distanceToHome);
                      *clearToSend = false;
                 }
                 break;
-            case FSSP_DATAID_GPS_ALT    :
+            case FSSP_DATAID_GPS_ALT:
                 if (smartPortShouldSendGPSData()) {
                     smartPortSendPackage(id, gpsSol.llh.alt); // cm
                     *clearToSend = false;
                 }
                 break;
-            case FSSP_DATAID_FPV       :
+            case FSSP_DATAID_FPV:
                 if (smartPortShouldSendGPSData()) {
                     smartPortSendPackage(id, gpsSol.groundCourse); // given in 10*deg
                     *clearToSend = false;
                 }
                 break;
-            case FSSP_DATAID_AZIMUTH    :
+            case FSSP_DATAID_AZIMUTH:
                 if (smartPortShouldSendGPSData()) {
                     int16_t h = GPS_directionToHome;
                     if (h < 0) {
@@ -614,13 +624,13 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
                 }
                 break;
 #endif
-            case FSSP_DATAID_A4         :
+            case FSSP_DATAID_A4:
                 if (isBatteryVoltageConfigured()) {
                     smartPortSendPackage(id, getBatteryAverageCellVoltage());
                     *clearToSend = false;
                 }
                 break;
-            case FSSP_DATAID_ASPD       :
+            case FSSP_DATAID_ASPD:
 #ifdef USE_PITOT
                 if (sensors(SENSOR_PITOT) && pitotIsHealthy()) {
                     smartPortSendPackage(id, getAirspeedEstimate() * 0.194384449f); // cm/s to knots*1
diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c
index d6c8e355d86..26f9631a89d 100644
--- a/src/main/telemetry/telemetry.c
+++ b/src/main/telemetry/telemetry.c
@@ -59,6 +59,7 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig,
     .telemetry_switch = SETTING_TELEMETRY_SWITCH_DEFAULT,
     .telemetry_inverted = SETTING_TELEMETRY_INVERTED_DEFAULT,
     .frsky_pitch_roll = SETTING_FRSKY_PITCH_ROLL_DEFAULT,
+    .frsky_use_legacy_gps_mode_sensor_ids = SETTING_FRSKY_USE_LEGACY_GPS_MODE_SENSOR_IDS_DEFAULT,
     .report_cell_voltage = SETTING_REPORT_CELL_VOLTAGE_DEFAULT,
     .hottAlarmSoundInterval = SETTING_HOTT_ALARM_SOUND_INTERVAL_DEFAULT,
     .halfDuplex = SETTING_TELEMETRY_HALFDUPLEX_DEFAULT,
diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h
index c7edad973b8..7be5e50a9ce 100644
--- a/src/main/telemetry/telemetry.h
+++ b/src/main/telemetry/telemetry.h
@@ -46,6 +46,7 @@ typedef struct telemetryConfig_s {
     uint8_t telemetry_switch;               // Use aux channel to change serial output & baudrate( MSP / Telemetry ). It disables automatic switching to Telemetry when armed.
     uint8_t telemetry_inverted;             // Flip the default inversion of the protocol - Same as serialrx_inverted in rx.c, but for telemetry.
     uint8_t frsky_pitch_roll;
+    bool    frsky_use_legacy_gps_mode_sensor_ids;
     uint8_t report_cell_voltage;
     uint8_t hottAlarmSoundInterval;
     uint8_t halfDuplex;

From e6ddb2440e8c7f8d821408ee2b3a57a83eb0d2b5 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Wed, 10 Jul 2024 17:51:52 +0200
Subject: [PATCH 02/39] Add setting for minimum ammount of tx buffer

---
 docs/Settings.md               | 10 ++++++++++
 src/main/fc/settings.yaml      |  6 ++++++
 src/main/telemetry/mavlink.c   |  2 +-
 src/main/telemetry/telemetry.c |  3 ++-
 src/main/telemetry/telemetry.h |  1 +
 5 files changed, 20 insertions(+), 2 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index da0b77f8ce1..10a4e1bfd7f 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -2542,6 +2542,16 @@ Rate of the extra3 message for MAVLink telemetry
 
 ---
 
+### mavlink_min_txbuffer
+
+Minimum percent of TX buffer space free, before attempting to transmit telemetry. Requuires RADIO_STATUS messages to be processed. 0 = always transmits.
+
+| Default | Min | Max |
+| --- | --- | --- |
+|  | 0 | 100 |
+
+---
+
 ### mavlink_pos_rate
 
 Rate of the position message for MAVLink telemetry
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 52d6d63f66e..2f19a8d0a49 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -3191,6 +3191,12 @@ groups:
         min: 1
         max: 2
         default_value: 2
+      - name: mavlink_min_txbuffer
+        file: mavlink.min_txbuff
+        description: "Minimum percent of TX buffer space free, before attempting to transmit telemetry. Requuires RADIO_STATUS messages to be processed. 0 = always transmits."
+        default: 100
+        min: 0
+        max: 100
 
   - name: PG_OSD_CONFIG
     type: osdConfig_t
diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index 59d1ef69463..4becf6375f4 100644
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -1217,7 +1217,7 @@ void handleMAVLinkTelemetry(timeUs_t currentTimeUs)
         incomingRequestServed = true;
     }
 
-    if ((currentTimeUs - lastMavlinkMessage) >= TELEMETRY_MAVLINK_DELAY && txbuff_free >= 90) {
+    if ((currentTimeUs - lastMavlinkMessage) >= TELEMETRY_MAVLINK_DELAY && txbuff_free >= telemetryConfig()->mavlink.min_txbuff) {
         // Only process scheduled data if we didn't serve any incoming request this cycle
         if (!incomingRequestServed ||
             (
diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c
index d6c8e355d86..d2334de891a 100644
--- a/src/main/telemetry/telemetry.c
+++ b/src/main/telemetry/telemetry.c
@@ -89,7 +89,8 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig,
         .extra1_rate = SETTING_MAVLINK_EXTRA1_RATE_DEFAULT,
         .extra2_rate = SETTING_MAVLINK_EXTRA2_RATE_DEFAULT,
         .extra3_rate = SETTING_MAVLINK_EXTRA3_RATE_DEFAULT,
-        .version = SETTING_MAVLINK_VERSION_DEFAULT
+        .version = SETTING_MAVLINK_VERSION_DEFAULT,
+        .min_txbuff = SETTING_MAVLINK_MIN_TXBUFF_DEFAULT
     }
 );
 
diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h
index c7edad973b8..d74c32d77e7 100644
--- a/src/main/telemetry/telemetry.h
+++ b/src/main/telemetry/telemetry.h
@@ -72,6 +72,7 @@ typedef struct telemetryConfig_s {
         uint8_t extra2_rate;
         uint8_t extra3_rate;
         uint8_t version;
+        uint8_t min_txbuff;
     } mavlink;
 } telemetryConfig_t;
 

From 6e549d69a418c133d1382e6f788c9a661b22b558 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Wed, 10 Jul 2024 18:14:54 +0200
Subject: [PATCH 03/39] Update receiver quality statistics, if using mavlink as
 serialrx_provider

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

diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index 4becf6375f4..67c4cf59b9a 100644
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -1116,6 +1116,13 @@ static bool handleIncoming_RADIO_STATUS(void) {
     mavlink_radio_status_t msg;
     mavlink_msg_radio_status_decode(&mavRecvMsg, &msg);
     txbuff_free = msg.txbuf;
+
+    if (rxConfig()->receiverType == RX_TYPE_SERIAL &&
+        rxConfig()->serialrx_provider == SERIALRX_MAVLINK) {
+        rxLinkStatistics.uplinkRSSI = -msg.remrssi;  // dbM
+        rxLinkStatistics.uplinkSNR = msg.noise;      // dbM * 2?
+        rxLinkStatistics.uplinkLQ = scaleRange(msg.rssi, 0, 254, 0, 100);  // May be elrs specific
+    }
     return true;
 }
 

From 309412df22bae620ad7f7c645585b6566d443ae0 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Wed, 10 Jul 2024 18:22:36 +0200
Subject: [PATCH 04/39] Small fixes

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

diff --git a/docs/Settings.md b/docs/Settings.md
index 10a4e1bfd7f..889cb55da17 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -2548,7 +2548,7 @@ Minimum percent of TX buffer space free, before attempting to transmit telemetry
 
 | Default | Min | Max |
 | --- | --- | --- |
-|  | 0 | 100 |
+| 100 | 0 | 100 |
 
 ---
 
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 2f19a8d0a49..4ea5e73d2f2 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -3192,9 +3192,9 @@ groups:
         max: 2
         default_value: 2
       - name: mavlink_min_txbuffer
-        file: mavlink.min_txbuff
+        field: mavlink.min_txbuff
         description: "Minimum percent of TX buffer space free, before attempting to transmit telemetry. Requuires RADIO_STATUS messages to be processed. 0 = always transmits."
-        default: 100
+        default_value: 100
         min: 0
         max: 100
 
diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c
index d2334de891a..4200c18c6a0 100644
--- a/src/main/telemetry/telemetry.c
+++ b/src/main/telemetry/telemetry.c
@@ -90,7 +90,7 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig,
         .extra2_rate = SETTING_MAVLINK_EXTRA2_RATE_DEFAULT,
         .extra3_rate = SETTING_MAVLINK_EXTRA3_RATE_DEFAULT,
         .version = SETTING_MAVLINK_VERSION_DEFAULT,
-        .min_txbuff = SETTING_MAVLINK_MIN_TXBUFF_DEFAULT
+        .min_txbuff = SETTING_MAVLINK_MIN_TXBUFFER_DEFAULT
     }
 );
 

From 3125fc08fdbcac60b5779b5acff372f21bf72501 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Wed, 10 Jul 2024 18:24:18 +0200
Subject: [PATCH 05/39] Bump pg version

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

diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c
index 4200c18c6a0..0d846ad19a8 100644
--- a/src/main/telemetry/telemetry.c
+++ b/src/main/telemetry/telemetry.c
@@ -53,7 +53,7 @@
 #include "telemetry/ghst.h"
 
 
-PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 6);
+PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 7);
 
 PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig,
     .telemetry_switch = SETTING_TELEMETRY_SWITCH_DEFAULT,

From 44d99f3dd415064298a2a009f22d60e7e9193d65 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Thu, 11 Jul 2024 15:48:39 +0200
Subject: [PATCH 06/39] improve half duplex checks

---
 src/main/telemetry/mavlink.c | 15 +++++++--------
 1 file changed, 7 insertions(+), 8 deletions(-)

diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index 67c4cf59b9a..39ac3524d29 100644
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -1207,6 +1207,11 @@ static bool processMAVLinkIncomingTelemetry(void)
     return false;
 }
 
+static bool isMAVLinkTelemetryHalfDuplex(void) {
+    return  (rxConfig()->receiverType == RX_TYPE_SERIAL && rxConfig()->serialrx_provider == SERIALRX_MAVLINK && tristateWithDefaultOffIsActive(rxConfig()->halfDuplex))
+            || telemetryConfig()->halfDuplex;
+}
+
 void handleMAVLinkTelemetry(timeUs_t currentTimeUs)
 {
     static bool incomingRequestServed;
@@ -1226,16 +1231,10 @@ void handleMAVLinkTelemetry(timeUs_t currentTimeUs)
 
     if ((currentTimeUs - lastMavlinkMessage) >= TELEMETRY_MAVLINK_DELAY && txbuff_free >= telemetryConfig()->mavlink.min_txbuff) {
         // Only process scheduled data if we didn't serve any incoming request this cycle
-        if (!incomingRequestServed ||
-            (
-                 (rxConfig()->receiverType == RX_TYPE_SERIAL) &&
-                 (rxConfig()->serialrx_provider == SERIALRX_MAVLINK) &&
-                 !tristateWithDefaultOnIsActive(rxConfig()->halfDuplex)
-            )
-        ) {
+        if (!incomingRequestServed || !isMAVLinkTelemetryHalfDuplex()) {
             processMAVLinkTelemetry(currentTimeUs);
+            lastMavlinkMessage = currentTimeUs;
         }
-        lastMavlinkMessage = currentTimeUs;
         incomingRequestServed = false;
     }
 }

From edd2a67ce8cf77b22371e7e17922f05905eaaee9 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sun, 14 Jul 2024 16:14:31 +0200
Subject: [PATCH 07/39] Sync txbuff value with @MUSTARDTIGERFPV

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

diff --git a/docs/Settings.md b/docs/Settings.md
index c588e31169c..3926fb1bb5f 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -2548,7 +2548,7 @@ Minimum percent of TX buffer space free, before attempting to transmit telemetry
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 100 | 0 | 100 |
+| 33 | 0 | 100 |
 
 ---
 
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 35110159105..57511737f0e 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -3194,7 +3194,7 @@ groups:
       - name: mavlink_min_txbuffer
         field: mavlink.min_txbuff
         description: "Minimum percent of TX buffer space free, before attempting to transmit telemetry. Requuires RADIO_STATUS messages to be processed. 0 = always transmits."
-        default_value: 100
+        default_value: 33
         min: 0
         max: 100
 
diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index ee372a3cfc1..1c58f36e493 100644
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -1253,7 +1253,7 @@ void handleMAVLinkTelemetry(timeUs_t currentTimeUs)
     // Determine whether to send telemetry back based on flow control / pacing
     if (txbuff_valid) {
         // Use flow control if available
-        shouldSendTelemetry = txbuff_free >= 33;
+        shouldSendTelemetry = txbuff_free >= telmetryConfig()->mavlink.min_txbuff;
     } else {
         // If not, use blind frame pacing - and back off for collision avoidance if half-duplex
         bool halfDuplexBackoff = (isMAVLinkTelemetryHalfDuplex() && receivedMessage);

From ca22dba0ca581bd4514404308876713751fd63c9 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sun, 14 Jul 2024 16:40:21 +0200
Subject: [PATCH 08/39] Fix typo

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

diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index 1c58f36e493..0e30dc00ac6 100644
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -1253,7 +1253,7 @@ void handleMAVLinkTelemetry(timeUs_t currentTimeUs)
     // Determine whether to send telemetry back based on flow control / pacing
     if (txbuff_valid) {
         // Use flow control if available
-        shouldSendTelemetry = txbuff_free >= telmetryConfig()->mavlink.min_txbuff;
+        shouldSendTelemetry = txbuff_free >= telemetryConfig()->mavlink.min_txbuff;
     } else {
         // If not, use blind frame pacing - and back off for collision avoidance if half-duplex
         bool halfDuplexBackoff = (isMAVLinkTelemetryHalfDuplex() && receivedMessage);

From 7a420cbdc20c2329cd65cf99f217f57139d42be1 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sun, 14 Jul 2024 21:03:07 +0200
Subject: [PATCH 09/39] Parse rssi/lq infor from radio_status

---
 docs/Settings.md               | 10 ++++++++++
 src/main/fc/settings.yaml      |  9 +++++++++
 src/main/telemetry/mavlink.c   | 26 ++++++++++++++++++++++----
 src/main/telemetry/telemetry.c |  3 ++-
 src/main/telemetry/telemetry.h |  7 +++++++
 5 files changed, 50 insertions(+), 5 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index 3926fb1bb5f..9f34dd5f479 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -2562,6 +2562,16 @@ Rate of the position message for MAVLink telemetry
 
 ---
 
+### mavlink_radio_type
+
+Mavlink radio type. Affects how RSSI and LQ are reported on OSD.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| GENERIC |  |  |
+
+---
+
 ### mavlink_rc_chan_rate
 
 Rate of the RC channels message for MAVLink telemetry
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 57511737f0e..d1fafb27c10 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -198,6 +198,9 @@ tables:
   - name: headtracker_dev_type
     values: ["NONE", "SERIAL", "MSP"]
     enum: headTrackerDevType_e
+  - name: mavlink_radio_type
+    values: ["GENERIC", "ELRS", "SIK"]
+    enum: mavlinkRadio_e
 
 constants:
   RPYL_PID_MIN: 0
@@ -3197,6 +3200,12 @@ groups:
         default_value: 33
         min: 0
         max: 100
+      - name: mavlink_radio_type
+        description: "Mavlink radio type. Affects how RSSI and LQ are reported on OSD."
+        field: mavlink.radio_type
+        table: mavlink_radio_type
+        default_value: "GENERIC"
+        type: uint8_t
 
   - name: PG_OSD_CONFIG
     type: osdConfig_t
diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index 0e30dc00ac6..9d9babe64f7 100644
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -1126,6 +1126,27 @@ static bool handleIncoming_PARAM_REQUEST_LIST(void) {
     return true;
 }
 
+static void mavlinkParseRxStats(const mavlink_radio_status_t *msg) {
+    switch(telemetryConfig()->mavlink.radio_type) {
+        case MAVLINK_RADIO_SIK:
+            rxLinkStatistics.uplinkRSSI = (msg->rssi / 1.9) - 127;
+            rxLinkStatistics.uplinkSNR = msg->noise * 1.9;
+            rxLinkStatistics.uplinkLQ = msg->rssi != 255 ? scaleRange(msg->rssi, 0, 254, 0, 100) : 0;
+            break;
+        case MAVLINK_RADIO_ELRS:
+            rxLinkStatistics.uplinkRSSI = -msg->remrssi;
+            rxLinkStatistics.uplinkSNR = msg->noise;
+            rxLinkStatistics.uplinkLQ = scaleRange(msg->rssi, 0, 255, 0, 100);
+            break;
+        case MAVLINK_RADIO_GENERIC:
+        default:
+            rxLinkStatistics.uplinkRSSI = msg->rssi;
+            rxLinkStatistics.uplinkSNR = msg->noise;
+            rxLinkStatistics.uplinkLQ = msg->rssi != 255 ? scaleRange(msg->rssi, 0, 254, 0, 100) : 0;
+            break;
+    }
+}
+
 static bool handleIncoming_RADIO_STATUS(void) {
     mavlink_radio_status_t msg;
     mavlink_msg_radio_status_decode(&mavRecvMsg, &msg);
@@ -1134,10 +1155,7 @@ static bool handleIncoming_RADIO_STATUS(void) {
        
     if (rxConfig()->receiverType == RX_TYPE_SERIAL &&
         rxConfig()->serialrx_provider == SERIALRX_MAVLINK) {
-        // TODO: decide what to do here
-        rxLinkStatistics.uplinkRSSI = -msg.remrssi;  // dbM
-        rxLinkStatistics.uplinkSNR = msg.noise;      // dbM * 2?
-        rxLinkStatistics.uplinkLQ = scaleRange(msg.rssi, 0, 254, 0, 100);  // May be elrs specific
+        mavlinkParseRxStats(&msg);
     }
 
     return true;
diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c
index 0d846ad19a8..8a8796eecb8 100644
--- a/src/main/telemetry/telemetry.c
+++ b/src/main/telemetry/telemetry.c
@@ -90,7 +90,8 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig,
         .extra2_rate = SETTING_MAVLINK_EXTRA2_RATE_DEFAULT,
         .extra3_rate = SETTING_MAVLINK_EXTRA3_RATE_DEFAULT,
         .version = SETTING_MAVLINK_VERSION_DEFAULT,
-        .min_txbuff = SETTING_MAVLINK_MIN_TXBUFFER_DEFAULT
+        .min_txbuff = SETTING_MAVLINK_MIN_TXBUFFER_DEFAULT,
+        .radio_type = SETTING_MAVLINK_RADIO_TYPE_DEFAULT
     }
 );
 
diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h
index d74c32d77e7..4ace4c5943d 100644
--- a/src/main/telemetry/telemetry.h
+++ b/src/main/telemetry/telemetry.h
@@ -36,6 +36,12 @@ typedef enum {
     LTM_RATE_SLOW
 } ltmUpdateRate_e;
 
+typedef enum {
+    MAVLINK_RADIO_GENERIC,
+    MAVLINK_RADIO_ELRS,
+    MAVLINK_RADIO_SIK,
+} mavlinkRadio_e;
+
 typedef enum {
     SMARTPORT_FUEL_UNIT_PERCENT,
     SMARTPORT_FUEL_UNIT_MAH,
@@ -73,6 +79,7 @@ typedef struct telemetryConfig_s {
         uint8_t extra3_rate;
         uint8_t version;
         uint8_t min_txbuff;
+        uint8_t radio_type;
     } mavlink;
 } telemetryConfig_t;
 

From b83ddda361d63eec884ba467dbc35bf3e5f11929 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sun, 14 Jul 2024 21:04:58 +0200
Subject: [PATCH 10/39] Bump PG_VERSION

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

diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c
index 8a8796eecb8..89eca3b1dd6 100644
--- a/src/main/telemetry/telemetry.c
+++ b/src/main/telemetry/telemetry.c
@@ -53,7 +53,7 @@
 #include "telemetry/ghst.h"
 
 
-PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 7);
+PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 8);
 
 PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig,
     .telemetry_switch = SETTING_TELEMETRY_SWITCH_DEFAULT,

From 3395dadf518f688f2986209a2bf976bcbbcab9d7 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sun, 14 Jul 2024 21:07:28 +0200
Subject: [PATCH 11/39] Fix scaling

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

diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index 9d9babe64f7..19bec9f62c8 100644
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -1130,7 +1130,7 @@ static void mavlinkParseRxStats(const mavlink_radio_status_t *msg) {
     switch(telemetryConfig()->mavlink.radio_type) {
         case MAVLINK_RADIO_SIK:
             rxLinkStatistics.uplinkRSSI = (msg->rssi / 1.9) - 127;
-            rxLinkStatistics.uplinkSNR = msg->noise * 1.9;
+            rxLinkStatistics.uplinkSNR = msg->noise / 1.9;
             rxLinkStatistics.uplinkLQ = msg->rssi != 255 ? scaleRange(msg->rssi, 0, 254, 0, 100) : 0;
             break;
         case MAVLINK_RADIO_ELRS:

From 9d69a15e4d88d7601712ba2d995308df1a8faf72 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Sun, 14 Jul 2024 21:14:29 +0200
Subject: [PATCH 12/39] Add comment with source of dbm formula

---
 src/main/telemetry/mavlink.c | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index 19bec9f62c8..617c883e4f6 100644
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -1129,6 +1129,7 @@ static bool handleIncoming_PARAM_REQUEST_LIST(void) {
 static void mavlinkParseRxStats(const mavlink_radio_status_t *msg) {
     switch(telemetryConfig()->mavlink.radio_type) {
         case MAVLINK_RADIO_SIK:
+            // rssi scaling info from: https://ardupilot.org/rover/docs/common-3dr-radio-advanced-configuration-and-technical-information.html
             rxLinkStatistics.uplinkRSSI = (msg->rssi / 1.9) - 127;
             rxLinkStatistics.uplinkSNR = msg->noise / 1.9;
             rxLinkStatistics.uplinkLQ = msg->rssi != 255 ? scaleRange(msg->rssi, 0, 254, 0, 100) : 0;

From f5e209cbd787a4c07d627c2eb564d5bbff60e2a8 Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Wed, 17 Jul 2024 09:01:27 +0100
Subject: [PATCH 13/39] nav_vel_z_improvement

---
 .../navigation/navigation_pos_estimator.c     | 24 ++++++++++++-------
 1 file changed, 16 insertions(+), 8 deletions(-)

diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
index d5a342173d3..dba1209b078 100755
--- a/src/main/navigation/navigation_pos_estimator.c
+++ b/src/main/navigation/navigation_pos_estimator.c
@@ -342,6 +342,12 @@ static bool gravityCalibrationComplete(void)
 
     return zeroCalibrationIsCompleteS(&posEstimator.imu.gravityCalibration);
 }
+
+static bool gravityCalibrationIsValid(void)
+{
+    return gravityCalibrationComplete() || posEstimator.imu.calibratedGravityCMSS;
+}
+
 #define ACC_VIB_FACTOR_S 1.0f
 #define ACC_VIB_FACTOR_E 3.0f
 static void updateIMUEstimationWeight(const float dt)
@@ -409,12 +415,13 @@ static void updateIMUTopic(timeUs_t currentTimeUs)
 
         /* When unarmed, assume that accelerometer should measure 1G. Use that to correct accelerometer gain */
         if (gyroConfig()->init_gyro_cal_enabled) {
-            if (!ARMING_FLAG(ARMED) && !gravityCalibrationComplete()) {
+            if (!ARMING_FLAG(WAS_EVER_ARMED)) {
                 zeroCalibrationAddValueS(&posEstimator.imu.gravityCalibration, posEstimator.imu.accelNEU.z);
 
                 if (gravityCalibrationComplete()) {
                     zeroCalibrationGetZeroS(&posEstimator.imu.gravityCalibration, &posEstimator.imu.calibratedGravityCMSS);
                     setGravityCalibration(posEstimator.imu.calibratedGravityCMSS);
+                    restartGravityCalibration();    // Repeat calibration until first arm to continuously update zero datum
                     LOG_DEBUG(POS_ESTIMATOR, "Gravity calibration complete (%d)", (int)lrintf(posEstimator.imu.calibratedGravityCMSS));
                 }
             }
@@ -423,8 +430,7 @@ static void updateIMUTopic(timeUs_t currentTimeUs)
             posEstimator.imu.calibratedGravityCMSS = gyroConfig()->gravity_cmss_cal;
         }
 
-        /* If calibration is incomplete - report zero acceleration */
-        if (gravityCalibrationComplete()) {
+        if (gravityCalibrationIsValid()) {
 #ifdef USE_SIMULATOR
             if (ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) {
                 posEstimator.imu.calibratedGravityCMSS = GRAVITY_CMSS;
@@ -432,7 +438,7 @@ static void updateIMUTopic(timeUs_t currentTimeUs)
 #endif
             posEstimator.imu.accelNEU.z -= posEstimator.imu.calibratedGravityCMSS;
         }
-        else {
+        else {      // If calibration is incomplete - report zero acceleration
             posEstimator.imu.accelNEU.x = 0.0f;
             posEstimator.imu.accelNEU.y = 0.0f;
             posEstimator.imu.accelNEU.z = 0.0f;
@@ -521,7 +527,9 @@ static void estimationPredict(estimationContext_t * ctx)
     if ((ctx->newFlags & EST_Z_VALID)) {
         posEstimator.est.pos.z += posEstimator.est.vel.z * ctx->dt;
         posEstimator.est.pos.z += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f;
-        posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt;
+        if (ARMING_FLAG(WAS_EVER_ARMED)) {   // Hold at zero until first armed
+            posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt;
+        }
     }
 
     /* Prediction step: XY-axis */
@@ -599,7 +607,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
             ctx->accBiasCorr.z -= wBaro * baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p);
         }
 
-        correctOK = true;
+        correctOK = ARMING_FLAG(WAS_EVER_ARMED);    // No correction until first armed
     }
 
     if (ctx->newFlags & EST_GPS_Z_VALID) {
@@ -623,7 +631,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
             ctx->accBiasCorr.z -= gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p);
         }
 
-        correctOK = true;
+        correctOK = ARMING_FLAG(WAS_EVER_ARMED);    // No correction until first armed
     }
 
     return correctOK;
@@ -907,5 +915,5 @@ void updatePositionEstimator(void)
 
 bool navIsCalibrationComplete(void)
 {
-    return gravityCalibrationComplete();
+    return gravityCalibrationIsValid();
 }

From 569bd14515a97e55186fde9f982a9d9d1189dddb Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Tue, 23 Jul 2024 10:03:13 +0100
Subject: [PATCH 14/39] add default altitude source setting

---
 docs/Settings.md                              | 10 ++++
 src/main/fc/settings.yaml                     | 10 +++-
 src/main/navigation/navigation.h              | 29 +++++-----
 .../navigation/navigation_pos_estimator.c     | 58 ++++++++++++-------
 .../navigation_pos_estimator_private.h        |  5 ++
 5 files changed, 75 insertions(+), 37 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index 6487b44b6d1..9afdd789685 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -1992,6 +1992,16 @@ Uncertainty value for barometric sensor [cm]
 
 ---
 
+### inav_default_alt_sensor
+
+Sets the default altitude sensor to use, GPS or BARO, when the altitude error between the sensors exceeds a set limit. Only the default sensor will be used in this case. GPS error limit = 2 * inav_max_eph_epv. BARO error limit = 4 * inav_baro_epv.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| GPS |  |  |
+
+---
+
 ### inav_gravity_cal_tolerance
 
 Unarmed gravity calibration tolerance level. Won't finish the calibration until estimated gravity error falls below this value.
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 00b08ca698c..c65d0ffa768 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -83,7 +83,7 @@ tables:
     values: ["NONE", "AGL", "FLOW_RAW", "FLOW", "ALWAYS", "SAG_COMP_VOLTAGE",
       "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
       "NAV_YAW", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "ALTITUDE",
-      "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST", 
+      "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST",
       "ADAPTIVE_FILTER", "HEADTRACKER", "GPS", "LULU"]
   - name: aux_operator
     values: ["OR", "AND"]
@@ -198,6 +198,9 @@ tables:
   - name: headtracker_dev_type
     values: ["NONE", "SERIAL", "MSP"]
     enum: headTrackerDevType_e
+  - name: default_altitude_source
+    values: ["GPS", "BARO"]
+    enum: navDefaultAltitudeSensor_e
 
 constants:
   RPYL_PID_MIN: 0
@@ -2461,6 +2464,11 @@ groups:
         field: baro_epv
         min: 0
         max: 9999
+      - name: inav_default_alt_sensor
+        description: "Sets the default altitude sensor to use, GPS or BARO, when the altitude error between the sensors exceeds a set limit. Only the default sensor will be used in this case. GPS error limit = 2 * inav_max_eph_epv. BARO error limit = 4 * inav_baro_epv."
+        default_value: "GPS"
+        field: default_alt_sensor
+        table: default_altitude_source
 
   - name: PG_NAV_CONFIG
     type: navConfig_t
diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
index 1b7734c8b13..da65f0f3f57 100644
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@@ -231,35 +231,36 @@ typedef enum {
 
 typedef struct positionEstimationConfig_s {
     uint8_t automatic_mag_declination;
-    uint8_t reset_altitude_type; // from nav_reset_type_e
-    uint8_t reset_home_type; // nav_reset_type_e
-    uint8_t gravity_calibration_tolerance;    // Tolerance of gravity calibration (cm/s/s)
+    uint8_t reset_altitude_type;            // from nav_reset_type_e
+    uint8_t reset_home_type;                // nav_reset_type_e
+    uint8_t gravity_calibration_tolerance;  // Tolerance of gravity calibration (cm/s/s)
     uint8_t allow_dead_reckoning;
 
     uint16_t max_surface_altitude;
 
-    float w_z_baro_p;   // Weight (cutoff frequency) for barometer altitude measurements
+    float w_z_baro_p;           // Weight (cutoff frequency) for barometer altitude measurements
 
-    float w_z_surface_p;  // Weight (cutoff frequency) for surface altitude measurements
-    float w_z_surface_v;  // Weight (cutoff frequency) for surface velocity measurements
+    float w_z_surface_p;        // Weight (cutoff frequency) for surface altitude measurements
+    float w_z_surface_v;        // Weight (cutoff frequency) for surface velocity measurements
 
-    float w_z_gps_p;    // GPS altitude data is very noisy and should be used only on airplanes
-    float w_z_gps_v;    // Weight (cutoff frequency) for GPS climb rate measurements
+    float w_z_gps_p;            // GPS altitude data is very noisy and should be used only on airplanes
+    float w_z_gps_v;            // Weight (cutoff frequency) for GPS climb rate measurements
 
-    float w_xy_gps_p;   // Weight (cutoff frequency) for GPS position measurements
-    float w_xy_gps_v;   // Weight (cutoff frequency) for GPS velocity measurements
+    float w_xy_gps_p;           // Weight (cutoff frequency) for GPS position measurements
+    float w_xy_gps_v;           // Weight (cutoff frequency) for GPS velocity measurements
 
     float w_xy_flow_p;
     float w_xy_flow_v;
 
-    float w_z_res_v;    // When velocity sources lost slowly decrease estimated velocity with this weight
+    float w_z_res_v;            // When velocity sources lost slowly decrease estimated velocity with this weight
     float w_xy_res_v;
 
-    float w_acc_bias;   // Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable.
+    float w_acc_bias;           // Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable.
 
-    float max_eph_epv;  // Max estimated position error acceptable for estimation (cm)
-    float baro_epv;     // Baro position error
+    float max_eph_epv;          // Max estimated position error acceptable for estimation (cm)
+    float baro_epv;             // Baro position error
 
+    uint8_t default_alt_sensor; // default altitude sensor source
 #ifdef USE_GPS_FIX_ESTIMATION
     uint8_t allow_gps_fix_estimation;
 #endif
diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
index dba1209b078..e11342ff38c 100755
--- a/src/main/navigation/navigation_pos_estimator.c
+++ b/src/main/navigation/navigation_pos_estimator.c
@@ -56,7 +56,7 @@
 navigationPosEstimator_t posEstimator;
 static float initialBaroAltitudeOffset = 0.0f;
 
-PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 7);
+PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 8);
 
 PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
         // Inertial position estimator parameters
@@ -89,6 +89,8 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
 
         .max_eph_epv = SETTING_INAV_MAX_EPH_EPV_DEFAULT,
         .baro_epv = SETTING_INAV_BARO_EPV_DEFAULT,
+
+        .default_alt_sensor = SETTING_INAV_DEFAULT_ALT_SENSOR_DEFAULT,
 #ifdef USE_GPS_FIX_ESTIMATION
         .allow_gps_fix_estimation = SETTING_INAV_ALLOW_GPS_FIX_ESTIMATION_DEFAULT
 #endif
@@ -560,20 +562,28 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
     DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount());            // Clip count
 
     bool correctOK = false;
+    float wBaro = 1.0f;
+    float wGps = 1.0f;
+
+    if (ctx->newFlags & EST_BARO_VALID && ctx->newFlags & EST_GPS_Z_VALID) {
+        const uint8_t defaultAltitudeSource = positionEstimationConfig()->default_alt_sensor;
+        const float gpsBaroResidual = fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt);
 
-    float wBaro = 0.0f;
-    if (ctx->newFlags & EST_BARO_VALID) {
-        // Ignore baro if difference is too big, baro is probably wrong
-        const float gpsBaroResidual = ctx->newFlags & EST_GPS_Z_VALID ? fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt) : 0.0f;
+        // Fade out the non default sensor to prevent sudden jump
+        uint16_t residualErrorEpvLimit = defaultAltitudeSource == ALTITUDE_SOURCE_BARO ? 2 * positionEstimationConfig()->baro_epv : positionEstimationConfig()->max_eph_epv;
+        const float start_epv = residualErrorEpvLimit;
+        const float end_epv = residualErrorEpvLimit * 2.0f;
 
-        // Fade out the baro to prevent sudden jump
-        const float start_epv = positionEstimationConfig()->max_eph_epv;
-        const float end_epv = positionEstimationConfig()->max_eph_epv * 2.0f;
+        // Calculate residual gps/baro sensor weighting based on assumed default altitude source = GPS
         wBaro = scaleRangef(constrainf(gpsBaroResidual, start_epv, end_epv), start_epv, end_epv, 1.0f, 0.0f);
+
+        if (defaultAltitudeSource == ALTITUDE_SOURCE_BARO) {    // flip residual sensor weighting if default = BARO
+            wGps = wBaro;
+            wBaro = 1.0f;
+        }
     }
 
-    // Always use Baro if no GPS otherwise only use if accuracy OK compared to GPS
-    if (wBaro > 0.01f) {
+    if (ctx->newFlags & EST_BARO_VALID && wBaro) {
         timeUs_t currentTimeUs = micros();
 
         if (!ARMING_FLAG(ARMED)) {
@@ -596,21 +606,24 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
                                              ((ctx->newFlags & EST_BARO_VALID) && posEstimator.state.isBaroGroundValid && posEstimator.baro.alt < posEstimator.state.baroGroundAlt));
 
         // Altitude
-        const float baroAltResidual = (isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z;
-        ctx->estPosCorr.z += wBaro * baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt;
-        ctx->estVelCorr.z += wBaro * baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * ctx->dt;
+        const float baroAltResidual = wBaro * ((isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z);
+        const float baroVelZResidual = isAirCushionEffectDetected ? 0.0f : wBaro * (posEstimator.baro.baroAltRate - posEstimator.est.vel.z);
+
+        ctx->estPosCorr.z += baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt;
+        ctx->estVelCorr.z += baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * ctx->dt;
+        ctx->estVelCorr.z += baroVelZResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt;
 
         ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, positionEstimationConfig()->w_z_baro_p);
 
         // Accelerometer bias
         if (!isAirCushionEffectDetected) {
-            ctx->accBiasCorr.z -= wBaro * baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p);
+            ctx->accBiasCorr.z -= baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p);
         }
 
         correctOK = ARMING_FLAG(WAS_EVER_ARMED);    // No correction until first armed
     }
 
-    if (ctx->newFlags & EST_GPS_Z_VALID) {
+    if (ctx->newFlags & EST_GPS_Z_VALID && wGps) {
         // Reset current estimate to GPS altitude if estimate not valid
         if (!(ctx->newFlags & EST_Z_VALID)) {
             ctx->estPosCorr.z += posEstimator.gps.pos.z - posEstimator.est.pos.z;
@@ -619,16 +632,17 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
         }
         else {
             // Altitude
-            const float gpsAltResudual = posEstimator.gps.pos.z - posEstimator.est.pos.z;
-            const float gpsVelZResudual = posEstimator.gps.vel.z - posEstimator.est.vel.z;
+            const float gpsAltResidual = wGps * (posEstimator.gps.pos.z - posEstimator.est.pos.z);
+            const float gpsVelZResidual = wGps * (posEstimator.gps.vel.z - posEstimator.est.vel.z);
+
+            ctx->estPosCorr.z += gpsAltResidual * positionEstimationConfig()->w_z_gps_p * ctx->dt;
+            ctx->estVelCorr.z += gpsAltResidual * sq(positionEstimationConfig()->w_z_gps_p) * ctx->dt;
+            ctx->estVelCorr.z += gpsVelZResidual * positionEstimationConfig()->w_z_gps_v * ctx->dt;
 
-            ctx->estPosCorr.z += gpsAltResudual * positionEstimationConfig()->w_z_gps_p * ctx->dt;
-            ctx->estVelCorr.z += gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p) * ctx->dt;
-            ctx->estVelCorr.z += gpsVelZResudual * positionEstimationConfig()->w_z_gps_v * ctx->dt;
-            ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, gpsAltResudual), positionEstimationConfig()->w_z_gps_p);
+            ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, gpsAltResidual), positionEstimationConfig()->w_z_gps_p);
 
             // Accelerometer bias
-            ctx->accBiasCorr.z -= gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p);
+            ctx->accBiasCorr.z -= gpsAltResidual * sq(positionEstimationConfig()->w_z_gps_p);
         }
 
         correctOK = ARMING_FLAG(WAS_EVER_ARMED);    // No correction until first armed
diff --git a/src/main/navigation/navigation_pos_estimator_private.h b/src/main/navigation/navigation_pos_estimator_private.h
index 44f0333d149..46fcb42e177 100644
--- a/src/main/navigation/navigation_pos_estimator_private.h
+++ b/src/main/navigation/navigation_pos_estimator_private.h
@@ -146,6 +146,11 @@ typedef enum {
     EST_Z_VALID                 = (1 << 6),
 } navPositionEstimationFlags_e;
 
+typedef enum {
+    ALTITUDE_SOURCE_GPS,
+    ALTITUDE_SOURCE_BARO,
+} navDefaultAltitudeSensor_e;
+
 typedef struct {
     timeUs_t    baroGroundTimeout;
     float       baroGroundAlt;

From 030b52e51d1750377af8e8ebd15fb5b2c1899e97 Mon Sep 17 00:00:00 2001
From: Darren Lines <darren@darrenlines.uk>
Date: Mon, 5 Aug 2024 12:22:15 +0300
Subject: [PATCH 15/39] Removed Switch Indicators and increase custom to 8

Removed switch indicators and increased the number of osd custom elements to 8.

The number of osd custom elements and custom element parts can now be automatically changed using the defined numbers.

MSP change

Requires Configurator
---
 src/main/fc/fc_msp.c                | 42 +++++++------
 src/main/fc/settings.yaml           | 53 -----------------
 src/main/io/osd.c                   | 91 +++++++++--------------------
 src/main/io/osd.h                   | 24 +++-----
 src/main/io/osd/custom_elements.c   |  1 -
 src/main/io/osd/custom_elements.h   |  2 +-
 src/main/msp/msp_protocol_v2_inav.h |  3 +-
 7 files changed, 60 insertions(+), 156 deletions(-)

diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index c8ae27f270d..e8cb5c39054 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -1706,28 +1706,19 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
 #endif
 #ifdef USE_PROGRAMMING_FRAMEWORK
     case MSP2_INAV_CUSTOM_OSD_ELEMENTS:
-        sbufWriteU8(dst, MAX_CUSTOM_ELEMENTS);
-        sbufWriteU8(dst, OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1);
-
-        for (int i = 0; i < MAX_CUSTOM_ELEMENTS; i++) {
-            const osdCustomElement_t *customElement = osdCustomElements(i);
-            for (int ii = 0; ii < CUSTOM_ELEMENTS_PARTS; ii++) {
-                sbufWriteU8(dst, customElement->part[ii].type);
-                sbufWriteU16(dst, customElement->part[ii].value);
-            }
-            sbufWriteU8(dst, customElement->visibility.type);
-            sbufWriteU16(dst, customElement->visibility.value);
-            for (int ii = 0; ii < OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1; ii++) {
-                sbufWriteU8(dst, customElement->osdCustomElementText[ii]);
-            }
+        {
+            sbufWriteU8(dst, MAX_CUSTOM_ELEMENTS);
+            sbufWriteU8(dst, OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1);
+            sbufWriteU8(dst, CUSTOM_ELEMENTS_PARTS);
         }
         break;
+#endif
     default:
         return false;
     }
     return true;
 }
-#endif
+
 
 #ifdef USE_SAFE_HOME
 static mspResult_e mspFcSafeHomeOutCommand(sbuf_t *dst, sbuf_t *src)
@@ -3349,7 +3340,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
 #ifdef USE_PROGRAMMING_FRAMEWORK
     case MSP2_INAV_SET_CUSTOM_OSD_ELEMENTS:
         sbufReadU8Safe(&tmp_u8, src);
-        if ((dataSize == (OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1) + (MAX_CUSTOM_ELEMENTS * 3) + 4) && (tmp_u8 < MAX_CUSTOM_ELEMENTS)) {
+        if ((dataSize == (OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1) + (CUSTOM_ELEMENTS_PARTS * 3) + 4) && (tmp_u8 < MAX_CUSTOM_ELEMENTS)) {
             for (int i = 0; i < CUSTOM_ELEMENTS_PARTS; i++) {
                 osdCustomElementsMutable(tmp_u8)->part[i].type = sbufReadU8(src);
                 osdCustomElementsMutable(tmp_u8)->part[i].value = sbufReadU16(src);
@@ -3365,7 +3356,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
         }
 
         break;
-
+#endif
     case MSP2_BETAFLIGHT_BIND:
         if (rxConfig()->receiverType == RX_TYPE_SERIAL) {
             switch (rxConfig()->serialrx_provider) {
@@ -3392,7 +3383,6 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
     }
     return MSP_RESULT_ACK;
 }
-#endif
 
 static const setting_t *mspReadSetting(sbuf_t *src)
 {
@@ -3849,6 +3839,22 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
     case MSP2_INAV_LOGIC_CONDITIONS_SINGLE:
         *ret = mspFcLogicConditionCommand(dst, src);
         break;
+    case MSP2_INAV_CUSTOM_OSD_ELEMENT:
+        const uint8_t idx = sbufReadU8(src);
+
+        if (idx < MAX_CUSTOM_ELEMENTS) {
+            const osdCustomElement_t *customElement = osdCustomElements(idx);
+            for (int ii = 0; ii < CUSTOM_ELEMENTS_PARTS; ii++) {
+                sbufWriteU8(dst, customElement->part[ii].type);
+                sbufWriteU16(dst, customElement->part[ii].value);
+            }
+            sbufWriteU8(dst, customElement->visibility.type);
+            sbufWriteU16(dst, customElement->visibility.value);
+            for (int ii = 0; ii < OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1; ii++) {
+                sbufWriteU8(dst, customElement->osdCustomElementText[ii]);
+            }
+        }
+        break;
 #endif
 #ifdef USE_SAFE_HOME
     case MSP2_INAV_SAFEHOME:
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 6c16ce93b21..1bd1400d0a6 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -3706,59 +3706,6 @@ groups:
         min: 1000
         max: 5000
         default_value: 1500
-      - name: osd_switch_indicator_zero_name
-        description: "Character to use for OSD switch incicator 0."
-        field: osd_switch_indicator0_name
-        type: string
-        max: 5
-        default_value: "FLAP"
-      - name: osd_switch_indicator_one_name
-        description: "Character to use for OSD switch incicator 1."
-        field: osd_switch_indicator1_name
-        type: string
-        max: 5
-        default_value: "GEAR"
-      - name: osd_switch_indicator_two_name
-        description: "Character to use for OSD switch incicator 2."
-        field: osd_switch_indicator2_name
-        type: string
-        max: 5
-        default_value: "CAM"
-      - name: osd_switch_indicator_three_name
-        description: "Character to use for OSD switch incicator 3."
-        field: osd_switch_indicator3_name
-        type: string
-        max: 5
-        default_value: "LIGT"
-      - name: osd_switch_indicator_zero_channel
-        description: "RC Channel to use for OSD switch indicator 0."
-        field: osd_switch_indicator0_channel
-        min: 5
-        max: MAX_SUPPORTED_RC_CHANNEL_COUNT
-        default_value: 5
-      - name: osd_switch_indicator_one_channel
-        description: "RC Channel to use for OSD switch indicator 1."
-        field: osd_switch_indicator1_channel
-        min: 5
-        max: MAX_SUPPORTED_RC_CHANNEL_COUNT
-        default_value: 5
-      - name: osd_switch_indicator_two_channel
-        description: "RC Channel to use for OSD switch indicator 2."
-        field: osd_switch_indicator2_channel
-        min: 5
-        max: MAX_SUPPORTED_RC_CHANNEL_COUNT
-        default_value: 5
-      - name: osd_switch_indicator_three_channel
-        description: "RC Channel to use for OSD switch indicator 3."
-        field: osd_switch_indicator3_channel
-        min: 5
-        max: MAX_SUPPORTED_RC_CHANNEL_COUNT
-        default_value: 5
-      - name: osd_switch_indicators_align_left
-        description: "Align text to left of switch indicators"
-        field: osd_switch_indicators_align_left
-        type: bool
-        default_value: ON
       - name: osd_system_msg_display_time
         description: System message display cycle time for multiple messages (milliseconds).
         field: system_msg_display_time
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index d682c0299ff..77762c6a003 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -225,7 +225,7 @@ static bool osdDisplayHasCanvas;
 #define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees)
 
 PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 12);
-PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 1);
+PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 2);
 
 void osdStartedSaveProcess(void) {
     savingSettings = true;
@@ -1610,40 +1610,6 @@ int8_t getGeoWaypointNumber(int8_t waypointIndex)
     return geoWaypointIndex - posControl.startWpIndex + 1;
 }
 
-void osdDisplaySwitchIndicator(const char *swName, int rcValue, char *buff) {
-    int8_t ptr = 0;
-
-    if (osdConfig()->osd_switch_indicators_align_left) {
-        for (ptr = 0; ptr < constrain(strlen(swName), 0, OSD_SWITCH_INDICATOR_NAME_LENGTH); ptr++) {
-            buff[ptr] = swName[ptr];
-        }
-
-        if ( rcValue < 1333) {
-            buff[ptr++] = SYM_SWITCH_INDICATOR_LOW;
-        } else if ( rcValue > 1666) {
-            buff[ptr++] = SYM_SWITCH_INDICATOR_HIGH;
-        } else {
-            buff[ptr++] = SYM_SWITCH_INDICATOR_MID;
-        }
-    } else {
-        if ( rcValue < 1333) {
-            buff[ptr++] = SYM_SWITCH_INDICATOR_LOW;
-        } else if ( rcValue > 1666) {
-            buff[ptr++] = SYM_SWITCH_INDICATOR_HIGH;
-        } else {
-            buff[ptr++] = SYM_SWITCH_INDICATOR_MID;
-        }
-
-        for (ptr = 1; ptr < constrain(strlen(swName), 0, OSD_SWITCH_INDICATOR_NAME_LENGTH) + 1; ptr++) {
-            buff[ptr] = swName[ptr-1];
-        }
-
-        ptr++;
-    }
-
-    buff[ptr] = '\0';
-}
-
 static bool osdDrawSingleElement(uint8_t item)
 {
     uint16_t pos = osdLayoutsConfig()->item_pos[currentLayout][item];
@@ -1671,6 +1637,31 @@ static bool osdDrawSingleElement(uint8_t item)
         customElementDrawElement(buff, 2);
         break;
     }
+    case OSD_CUSTOM_ELEMENT_4:
+    {
+        customElementDrawElement(buff, 3);
+        break;
+    }
+    case OSD_CUSTOM_ELEMENT_5:
+    {
+        customElementDrawElement(buff, 4);
+        break;
+    }
+    case OSD_CUSTOM_ELEMENT_6:
+    {
+        customElementDrawElement(buff, 5);
+        break;
+    }
+    case OSD_CUSTOM_ELEMENT_7:
+    {
+        customElementDrawElement(buff, 6);
+        break;
+    }
+    case OSD_CUSTOM_ELEMENT_8:
+    {
+        customElementDrawElement(buff, 7);
+        break;
+    }
     case OSD_RSSI_VALUE:
         {
             uint16_t osdRssi = osdConvertRSSI();
@@ -2873,22 +2864,6 @@ static bool osdDrawSingleElement(uint8_t item)
         }
 #endif
 
-    case OSD_SWITCH_INDICATOR_0:
-        osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator0_name, rxGetChannelValue(osdConfig()->osd_switch_indicator0_channel - 1), buff);
-        break;
-
-    case OSD_SWITCH_INDICATOR_1:
-        osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator1_name, rxGetChannelValue(osdConfig()->osd_switch_indicator1_channel - 1), buff);
-        break;
-
-    case OSD_SWITCH_INDICATOR_2:
-        osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator2_name, rxGetChannelValue(osdConfig()->osd_switch_indicator2_channel - 1), buff);
-        break;
-
-    case OSD_SWITCH_INDICATOR_3:
-        osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator3_name, rxGetChannelValue(osdConfig()->osd_switch_indicator3_channel - 1), buff);
-        break;
-
     case OSD_PAN_SERVO_CENTRED:
         {
             int16_t panOffset = osdGetPanServoOffset();
@@ -4008,15 +3983,6 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
     .pan_servo_indicator_show_degrees = SETTING_OSD_PAN_SERVO_INDICATOR_SHOW_DEGREES_DEFAULT,
     .esc_rpm_precision = SETTING_OSD_ESC_RPM_PRECISION_DEFAULT,
     .mAh_precision = SETTING_OSD_MAH_PRECISION_DEFAULT,
-    .osd_switch_indicator0_name = SETTING_OSD_SWITCH_INDICATOR_ZERO_NAME_DEFAULT,
-    .osd_switch_indicator0_channel = SETTING_OSD_SWITCH_INDICATOR_ZERO_CHANNEL_DEFAULT,
-    .osd_switch_indicator1_name = SETTING_OSD_SWITCH_INDICATOR_ONE_NAME_DEFAULT,
-    .osd_switch_indicator1_channel = SETTING_OSD_SWITCH_INDICATOR_ONE_CHANNEL_DEFAULT,
-    .osd_switch_indicator2_name = SETTING_OSD_SWITCH_INDICATOR_TWO_NAME_DEFAULT,
-    .osd_switch_indicator2_channel = SETTING_OSD_SWITCH_INDICATOR_TWO_CHANNEL_DEFAULT,
-    .osd_switch_indicator3_name = SETTING_OSD_SWITCH_INDICATOR_THREE_NAME_DEFAULT,
-    .osd_switch_indicator3_channel = SETTING_OSD_SWITCH_INDICATOR_THREE_CHANNEL_DEFAULT,
-    .osd_switch_indicators_align_left = SETTING_OSD_SWITCH_INDICATORS_ALIGN_LEFT_DEFAULT,
     .system_msg_display_time = SETTING_OSD_SYSTEM_MSG_DISPLAY_TIME_DEFAULT,
     .units = SETTING_OSD_UNITS_DEFAULT,
     .main_voltage_decimals = SETTING_OSD_MAIN_VOLTAGE_DECIMALS_DEFAULT,
@@ -4191,11 +4157,6 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
 
     osdLayoutsConfig->item_pos[0][OSD_MULTI_FUNCTION] = OSD_POS(1, 4);
 
-    osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_0] = OSD_POS(2, 7);
-    osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_1] = OSD_POS(2, 8);
-    osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_2] = OSD_POS(2, 9);
-    osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_3] = OSD_POS(2, 10);
-
     osdLayoutsConfig->item_pos[0][OSD_ADSB_WARNING] = OSD_POS(2, 7);
     osdLayoutsConfig->item_pos[0][OSD_ADSB_INFO] = OSD_POS(2, 8);
 #if defined(USE_ESC_SENSOR)
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index a73db015793..2e5200a0483 100644
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -263,10 +263,10 @@ typedef enum {
     OSD_AIR_MAX_SPEED,
     OSD_ACTIVE_PROFILE,
     OSD_MISSION,
-    OSD_SWITCH_INDICATOR_0,
-    OSD_SWITCH_INDICATOR_1,
-    OSD_SWITCH_INDICATOR_2,
-    OSD_SWITCH_INDICATOR_3,
+    OSD_CUSTOM_ELEMENT_4,
+    OSD_CUSTOM_ELEMENT_5,
+    OSD_CUSTOM_ELEMENT_6,
+    OSD_CUSTOM_ELEMENT_7,
     OSD_TPA_TIME_CONSTANT,
     OSD_FW_LEVEL_TRIM,
     OSD_GLIDE_TIME_REMAINING,
@@ -286,7 +286,8 @@ typedef enum {
     OSD_ADSB_WARNING, //150
     OSD_ADSB_INFO,
     OSD_BLACKBOX,
-    OSD_FORMATION_FLIGHT, //153
+    OSD_FORMATION_FLIGHT,
+    OSD_CUSTOM_ELEMENT_8, // 154  
     OSD_ITEM_COUNT // MUST BE LAST
 } osd_items_e;
 
@@ -347,8 +348,6 @@ typedef struct osdLayoutsConfig_s {
 
 PG_DECLARE(osdLayoutsConfig_t, osdLayoutsConfig);
 
-#define OSD_SWITCH_INDICATOR_NAME_LENGTH 4
-
 typedef struct osdConfig_s {
     // Alarms
     uint8_t         rssi_alarm;                         // rssi %
@@ -444,17 +443,8 @@ typedef struct osdConfig_s {
     uint8_t         telemetry;                          // use telemetry on displayed pixel line 0
     uint8_t         esc_rpm_precision;                  // Number of characters used for the RPM numbers.
     uint16_t        system_msg_display_time;            // system message display time for multiple messages (ms)
-    uint8_t         mAh_precision;                 // Number of numbers used for mAh drawn. Plently of packs now are > 9999 mAh
+    uint8_t         mAh_precision;                      // Number of numbers used for mAh drawn. Plently of packs now are > 9999 mAh
     uint8_t         ahi_pitch_interval;                 // redraws AHI at set pitch interval (Not pixel OSD)
-    char            osd_switch_indicator0_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 0.
-    uint8_t         osd_switch_indicator0_channel;      // RC Channel to use for switch indicator 0.
-    char            osd_switch_indicator1_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 1.
-    uint8_t         osd_switch_indicator1_channel;      // RC Channel to use for switch indicator 1.
-    char            osd_switch_indicator2_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 2.
-    uint8_t         osd_switch_indicator2_channel;      // RC Channel to use for switch indicator 2.
-    char            osd_switch_indicator3_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 3.
-    uint8_t         osd_switch_indicator3_channel;      // RC Channel to use for switch indicator 3.
-    bool            osd_switch_indicators_align_left;   // Align switch indicator name to left of the switch.
     bool            use_pilot_logo;                     // If enabled, the pilot logo (last 40 characters of page 2 font) will be used with the INAV logo.
     uint8_t         inav_to_pilot_logo_spacing;         // The space between the INAV and pilot logos, if pilot logo is used. This number may be adjusted so that it fits the odd/even col width.
     uint16_t        arm_screen_display_time;            // Length of time the arm screen is displayed
diff --git a/src/main/io/osd/custom_elements.c b/src/main/io/osd/custom_elements.c
index fbd05e2be61..132b6c820af 100644
--- a/src/main/io/osd/custom_elements.c
+++ b/src/main/io/osd/custom_elements.c
@@ -138,4 +138,3 @@ void customElementDrawElement(char *buff, uint8_t customElementIndex){
     }
     prevLength[customElementIndex] = buffSeek;
 }
-
diff --git a/src/main/io/osd/custom_elements.h b/src/main/io/osd/custom_elements.h
index a55b010f01a..715bae5ab2b 100644
--- a/src/main/io/osd/custom_elements.h
+++ b/src/main/io/osd/custom_elements.h
@@ -21,7 +21,7 @@
 
 #define OSD_CUSTOM_ELEMENT_TEXT_SIZE 16
 #define CUSTOM_ELEMENTS_PARTS 3
-#define MAX_CUSTOM_ELEMENTS 3
+#define MAX_CUSTOM_ELEMENTS 8
 
 typedef enum {
     CUSTOM_ELEMENT_TYPE_NONE            = 0,
diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h
index 8b4a09e87af..02c8c979aae 100755
--- a/src/main/msp/msp_protocol_v2_inav.h
+++ b/src/main/msp/msp_protocol_v2_inav.h
@@ -109,7 +109,8 @@
 #define MSP2_ADSB_VEHICLE_LIST                  0x2090
 
 #define MSP2_INAV_CUSTOM_OSD_ELEMENTS           0x2100
-#define MSP2_INAV_SET_CUSTOM_OSD_ELEMENTS       0x2101
+#define MSP2_INAV_CUSTOM_OSD_ELEMENT            0x2101
+#define MSP2_INAV_SET_CUSTOM_OSD_ELEMENTS       0x2102
 
 #define MSP2_INAV_SERVO_CONFIG                  0x2200
 #define MSP2_INAV_SET_SERVO_CONFIG              0x2201
\ No newline at end of file

From f3c43e8c0dd6db69bae0066ae52ed9dd049d13ff Mon Sep 17 00:00:00 2001
From: Darren Lines <darren@darrenlines.uk>
Date: Tue, 6 Aug 2024 18:32:41 +0300
Subject: [PATCH 16/39] Updates

- Added additional numeric options and LC options to custom element types
- Changed method of displaying numbers. So that negative numbers will work.
---
 src/main/io/osd/custom_elements.c | 111 +++++++++++++++++++++++++++---
 src/main/io/osd/custom_elements.h |  26 +++++--
 2 files changed, 124 insertions(+), 13 deletions(-)

diff --git a/src/main/io/osd/custom_elements.c b/src/main/io/osd/custom_elements.c
index 132b6c820af..07ca43db4fd 100644
--- a/src/main/io/osd/custom_elements.c
+++ b/src/main/io/osd/custom_elements.c
@@ -68,26 +68,119 @@ uint8_t customElementDrawPart(char *buff, uint8_t customElementIndex, uint8_t cu
     const int customPartValue = osdCustomElements(customElementIndex)->part[customElementItemIndex].value;
 
     switch (customPartType) {
-        case CUSTOM_ELEMENT_TYPE_GV:
+        case CUSTOM_ELEMENT_TYPE_GV_1:
         {
-            osdFormatCentiNumber(buff, (int32_t) gvGet(customPartValue) * (int32_t) 100, 1, 0, 0, 6, false);
-            return 6;
+            osdFormatCentiNumber(buff,(int32_t) (constrain(gvGet(customPartValue), -9, 9) * (int32_t) 100), 1, 0, 0, 2, false);
+            return 2;
+        }
+        case CUSTOM_ELEMENT_TYPE_GV_2:
+        {
+            osdFormatCentiNumber(buff, (int32_t) (constrain(gvGet(customPartValue), -99, 99) * (int32_t) 100), 1, 0, 0, 3, false);
+            return 3;
+        }
+        case CUSTOM_ELEMENT_TYPE_GV_3:
+        {
+            osdFormatCentiNumber(buff, (int32_t) (constrain(gvGet(customPartValue), -999, 999) * (int32_t) 100), 1, 0, 0, 4, false);
+            return 4;
+        }
+        case CUSTOM_ELEMENT_TYPE_GV_4:
+        {
+            osdFormatCentiNumber(buff, (int32_t) (constrain(gvGet(customPartValue), -9999, 9999) * (int32_t) 100), 1, 0, 0, 5, false);
+            return 5;
         }
-        case CUSTOM_ELEMENT_TYPE_GV_FLOAT:
+        case CUSTOM_ELEMENT_TYPE_GV_5:
         {
-            osdFormatCentiNumber(buff, (int32_t) gvGet(customPartValue), 1, 2, 0, 6, false);
+            osdFormatCentiNumber(buff, (int32_t) (constrain(gvGet(customPartValue), -99999, 99999) * (int32_t) 100), 1, 0, 0, 6, false);
             return 6;
         }
-        case CUSTOM_ELEMENT_TYPE_GV_SMALL:
+        case CUSTOM_ELEMENT_TYPE_GV_FLOAT_1_1:
         {
-            osdFormatCentiNumber(buff, (int32_t) ((gvGet(customPartValue) % 1000 ) * (int32_t) 100), 1, 0, 0, 3, false);
+            osdFormatCentiNumber(buff, (int32_t) (constrain(gvGet(customPartValue), -99, 99) * (int32_t) 10), 1, 1, 0, 3, false);
             return 3;
         }
-        case CUSTOM_ELEMENT_TYPE_GV_SMALL_FLOAT:
+        case CUSTOM_ELEMENT_TYPE_GV_FLOAT_2_1:
+        {
+            osdFormatCentiNumber(buff, (int32_t) (constrain(gvGet(customPartValue), -999, 999) * (int32_t) 10), 1, 1, 0, 4, false);
+            return 4;
+        }
+        case CUSTOM_ELEMENT_TYPE_GV_FLOAT_2_2:
+        {
+            osdFormatCentiNumber(buff, (int32_t) constrain(gvGet(customPartValue), -9999, 9999), 1, 2, 0, 5, false);
+            return 5;
+        }
+        case CUSTOM_ELEMENT_TYPE_GV_FLOAT_3_1:
+        {
+            osdFormatCentiNumber(buff, (int32_t) (constrain(gvGet(customPartValue), -9999, 9999) * (int32_t) 10), 1, 1, 0, 5, false);
+            return 5;
+        }
+        case CUSTOM_ELEMENT_TYPE_GV_FLOAT_3_2:
+        {
+            osdFormatCentiNumber(buff, (int32_t) constrain(gvGet(customPartValue), -99999, 99999), 1, 2, 0, 6, false);
+            return 6;
+        }
+        case CUSTOM_ELEMENT_TYPE_GV_FLOAT_4_1:
         {
-            osdFormatCentiNumber(buff, (int32_t) ((gvGet(customPartValue) % 100)  * (int32_t) 10), 1, 1, 0, 2, false);
+            osdFormatCentiNumber(buff, (int32_t) (constrain(gvGet(customPartValue), -99999, 99999) * (int32_t) 10), 1, 1, 0, 6, false);
+            return 6;
+        }
+
+        case CUSTOM_ELEMENT_TYPE_LC_1:
+        {
+            osdFormatCentiNumber(buff,(int32_t) (constrain(logicConditionGetValue(customPartValue), -9, 9) * (int32_t) 100), 1, 0, 0, 2, false);
             return 2;
         }
+        case CUSTOM_ELEMENT_TYPE_LC_2:
+        {
+            osdFormatCentiNumber(buff, (int32_t) (constrain(logicConditionGetValue(customPartValue), -99, 99) * (int32_t) 100), 1, 0, 0, 3, false);
+            return 3;
+        }
+        case CUSTOM_ELEMENT_TYPE_LC_3:
+        {
+            osdFormatCentiNumber(buff, (int32_t) (constrain(logicConditionGetValue(customPartValue), -999, 999) * (int32_t) 100), 1, 0, 0, 4, false);
+            return 4;
+        }
+        case CUSTOM_ELEMENT_TYPE_LC_4:
+        {
+            osdFormatCentiNumber(buff, (int32_t) (constrain(logicConditionGetValue(customPartValue), -9999, 9999) * (int32_t) 100), 1, 0, 0, 5, false);
+            return 5;
+        }
+        case CUSTOM_ELEMENT_TYPE_LC_5:
+        {
+            osdFormatCentiNumber(buff, (int32_t) (constrain(logicConditionGetValue(customPartValue), -99999, 99999) * (int32_t) 100), 1, 0, 0, 6, false);
+            return 6;
+        }
+        case CUSTOM_ELEMENT_TYPE_LC_FLOAT_1_1:
+        {
+            osdFormatCentiNumber(buff, (int32_t) (constrain(logicConditionGetValue(customPartValue), -99, 99) * (int32_t) 10), 1, 1, 0, 3, false);
+            return 3;
+        }
+        case CUSTOM_ELEMENT_TYPE_LC_FLOAT_2_1:
+        {
+            osdFormatCentiNumber(buff, (int32_t) (constrain(logicConditionGetValue(customPartValue), -999, 999) * (int32_t) 10), 1, 1, 0, 4, false);
+            return 4;
+        }
+        case CUSTOM_ELEMENT_TYPE_LC_FLOAT_2_2:
+        {
+            osdFormatCentiNumber(buff, (int32_t) constrain(logicConditionGetValue(customPartValue), -9999, 9999), 1, 2, 0, 5, false);
+            return 5;
+        }
+        case CUSTOM_ELEMENT_TYPE_LC_FLOAT_3_1:
+        {
+            osdFormatCentiNumber(buff, (int32_t) (constrain(logicConditionGetValue(customPartValue), -9999, 9999)  * (int32_t) 10), 1, 1, 0, 5, false);
+            return 5;
+        }
+        case CUSTOM_ELEMENT_TYPE_LC_FLOAT_3_2:
+        {
+            osdFormatCentiNumber(buff, (int32_t) constrain(logicConditionGetValue(customPartValue), -99999, 99999), 1, 2, 0, 6, false);
+            return 6;
+        }
+        case CUSTOM_ELEMENT_TYPE_LC_FLOAT_4_1:
+        {
+            osdFormatCentiNumber(buff, (int32_t) (constrain(logicConditionGetValue(customPartValue), -99999, 99999) * (int32_t) 10), 1, 1, 0, 6, false);
+            return 6;
+        }
+        
+        
         case CUSTOM_ELEMENT_TYPE_ICON_GV:
         {
             *buff = (uint8_t)gvGet(customPartValue);
diff --git a/src/main/io/osd/custom_elements.h b/src/main/io/osd/custom_elements.h
index 715bae5ab2b..73425d59ad3 100644
--- a/src/main/io/osd/custom_elements.h
+++ b/src/main/io/osd/custom_elements.h
@@ -28,10 +28,28 @@ typedef enum {
     CUSTOM_ELEMENT_TYPE_TEXT            = 1,
     CUSTOM_ELEMENT_TYPE_ICON_STATIC     = 2,
     CUSTOM_ELEMENT_TYPE_ICON_GV         = 3,
-    CUSTOM_ELEMENT_TYPE_GV              = 4,
-    CUSTOM_ELEMENT_TYPE_GV_FLOAT        = 5,
-    CUSTOM_ELEMENT_TYPE_GV_SMALL        = 6,
-    CUSTOM_ELEMENT_TYPE_GV_SMALL_FLOAT  = 7,
+    CUSTOM_ELEMENT_TYPE_GV_1            = 4,
+    CUSTOM_ELEMENT_TYPE_GV_2            = 5,
+    CUSTOM_ELEMENT_TYPE_GV_3            = 6,
+    CUSTOM_ELEMENT_TYPE_GV_4            = 7,
+    CUSTOM_ELEMENT_TYPE_GV_5            = 8,
+    CUSTOM_ELEMENT_TYPE_GV_FLOAT_1_1    = 9,
+    CUSTOM_ELEMENT_TYPE_GV_FLOAT_2_1    = 10,
+    CUSTOM_ELEMENT_TYPE_GV_FLOAT_2_2    = 11,
+    CUSTOM_ELEMENT_TYPE_GV_FLOAT_3_1    = 12,
+    CUSTOM_ELEMENT_TYPE_GV_FLOAT_3_2    = 13,
+    CUSTOM_ELEMENT_TYPE_GV_FLOAT_4_1    = 14,
+    CUSTOM_ELEMENT_TYPE_LC_1            = 15,
+    CUSTOM_ELEMENT_TYPE_LC_2            = 16,
+    CUSTOM_ELEMENT_TYPE_LC_3            = 17,
+    CUSTOM_ELEMENT_TYPE_LC_4            = 18,
+    CUSTOM_ELEMENT_TYPE_LC_5            = 19,
+    CUSTOM_ELEMENT_TYPE_LC_FLOAT_1_1    = 20,
+    CUSTOM_ELEMENT_TYPE_LC_FLOAT_2_1    = 21,
+    CUSTOM_ELEMENT_TYPE_LC_FLOAT_2_2    = 22,
+    CUSTOM_ELEMENT_TYPE_LC_FLOAT_3_1    = 23,
+    CUSTOM_ELEMENT_TYPE_LC_FLOAT_3_2    = 24,
+    CUSTOM_ELEMENT_TYPE_LC_FLOAT_4_1    = 25,
 } osdCustomElementType_e;
 
 typedef enum {

From dad76ec9610fdbb7e53e7f9d36a068bb8f8848ce Mon Sep 17 00:00:00 2001
From: Darren Lines <darren@darrenlines.uk>
Date: Tue, 6 Aug 2024 19:41:08 +0300
Subject: [PATCH 17/39] Updated docs

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

diff --git a/docs/Settings.md b/docs/Settings.md
index 6487b44b6d1..1e7bf47381c 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -5152,96 +5152,6 @@ Enabling this option will show metric efficiency statistics on the post flight s
 
 ---
 
-### osd_switch_indicator_one_channel
-
-RC Channel to use for OSD switch indicator 1.
-
-| Default | Min | Max |
-| --- | --- | --- |
-| 5 | 5 | MAX_SUPPORTED_RC_CHANNEL_COUNT |
-
----
-
-### osd_switch_indicator_one_name
-
-Character to use for OSD switch incicator 1.
-
-| Default | Min | Max |
-| --- | --- | --- |
-| GEAR |  | 5 |
-
----
-
-### osd_switch_indicator_three_channel
-
-RC Channel to use for OSD switch indicator 3.
-
-| Default | Min | Max |
-| --- | --- | --- |
-| 5 | 5 | MAX_SUPPORTED_RC_CHANNEL_COUNT |
-
----
-
-### osd_switch_indicator_three_name
-
-Character to use for OSD switch incicator 3.
-
-| Default | Min | Max |
-| --- | --- | --- |
-| LIGT |  | 5 |
-
----
-
-### osd_switch_indicator_two_channel
-
-RC Channel to use for OSD switch indicator 2.
-
-| Default | Min | Max |
-| --- | --- | --- |
-| 5 | 5 | MAX_SUPPORTED_RC_CHANNEL_COUNT |
-
----
-
-### osd_switch_indicator_two_name
-
-Character to use for OSD switch incicator 2.
-
-| Default | Min | Max |
-| --- | --- | --- |
-| CAM |  | 5 |
-
----
-
-### osd_switch_indicator_zero_channel
-
-RC Channel to use for OSD switch indicator 0.
-
-| Default | Min | Max |
-| --- | --- | --- |
-| 5 | 5 | MAX_SUPPORTED_RC_CHANNEL_COUNT |
-
----
-
-### osd_switch_indicator_zero_name
-
-Character to use for OSD switch incicator 0.
-
-| Default | Min | Max |
-| --- | --- | --- |
-| FLAP |  | 5 |
-
----
-
-### osd_switch_indicators_align_left
-
-Align text to left of switch indicators
-
-| Default | Min | Max |
-| --- | --- | --- |
-| ON | OFF | ON |
-
----
-
 ### osd_system_msg_display_time
 
 System message display cycle time for multiple messages (milliseconds).

From fa166c61c6e468c70d24e4fb8ed41592748df0cc Mon Sep 17 00:00:00 2001
From: Darren Lines <darren@darrenlines.uk>
Date: Tue, 6 Aug 2024 19:49:22 +0300
Subject: [PATCH 18/39] Mac issues

---
 src/main/fc/fc_msp.c | 24 +++++++++++++-----------
 1 file changed, 13 insertions(+), 11 deletions(-)

diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index e8cb5c39054..2749f016761 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -3840,18 +3840,20 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
         *ret = mspFcLogicConditionCommand(dst, src);
         break;
     case MSP2_INAV_CUSTOM_OSD_ELEMENT:
-        const uint8_t idx = sbufReadU8(src);
+        {
+            const uint8_t idx = sbufReadU8(src);
 
-        if (idx < MAX_CUSTOM_ELEMENTS) {
-            const osdCustomElement_t *customElement = osdCustomElements(idx);
-            for (int ii = 0; ii < CUSTOM_ELEMENTS_PARTS; ii++) {
-                sbufWriteU8(dst, customElement->part[ii].type);
-                sbufWriteU16(dst, customElement->part[ii].value);
-            }
-            sbufWriteU8(dst, customElement->visibility.type);
-            sbufWriteU16(dst, customElement->visibility.value);
-            for (int ii = 0; ii < OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1; ii++) {
-                sbufWriteU8(dst, customElement->osdCustomElementText[ii]);
+            if (idx < MAX_CUSTOM_ELEMENTS) {
+                const osdCustomElement_t *customElement = osdCustomElements(idx);
+                for (int ii = 0; ii < CUSTOM_ELEMENTS_PARTS; ii++) {
+                    sbufWriteU8(dst, customElement->part[ii].type);
+                    sbufWriteU16(dst, customElement->part[ii].value);
+                }
+                sbufWriteU8(dst, customElement->visibility.type);
+                sbufWriteU16(dst, customElement->visibility.value);
+                for (int ii = 0; ii < OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1; ii++) {
+                    sbufWriteU8(dst, customElement->osdCustomElementText[ii]);
+                }
             }
         }
         break;

From 70856d7baa1e89f8cb0be92ac11cb012afaa77ec Mon Sep 17 00:00:00 2001
From: Darren Lines <darren@darrenlines.uk>
Date: Wed, 7 Aug 2024 23:10:33 +0300
Subject: [PATCH 19/39] Added icon from logic condition

---
 src/main/io/osd/custom_elements.c |  5 ++++
 src/main/io/osd/custom_elements.h | 45 ++++++++++++++++---------------
 2 files changed, 28 insertions(+), 22 deletions(-)

diff --git a/src/main/io/osd/custom_elements.c b/src/main/io/osd/custom_elements.c
index 07ca43db4fd..df84ddd76b0 100644
--- a/src/main/io/osd/custom_elements.c
+++ b/src/main/io/osd/custom_elements.c
@@ -186,6 +186,11 @@ uint8_t customElementDrawPart(char *buff, uint8_t customElementIndex, uint8_t cu
             *buff = (uint8_t)gvGet(customPartValue);
             return 1;
         }
+        case CUSTOM_ELEMENT_TYPE_ICON_LC:
+        {
+            *buff = (uint8_t)constrain(logicConditionGetValue(customPartValue), 1, 255);
+            return 1;
+        }
         case CUSTOM_ELEMENT_TYPE_ICON_STATIC:
         {
             *buff = (uint8_t)customPartValue;
diff --git a/src/main/io/osd/custom_elements.h b/src/main/io/osd/custom_elements.h
index 73425d59ad3..9c12ede6734 100644
--- a/src/main/io/osd/custom_elements.h
+++ b/src/main/io/osd/custom_elements.h
@@ -28,28 +28,29 @@ typedef enum {
     CUSTOM_ELEMENT_TYPE_TEXT            = 1,
     CUSTOM_ELEMENT_TYPE_ICON_STATIC     = 2,
     CUSTOM_ELEMENT_TYPE_ICON_GV         = 3,
-    CUSTOM_ELEMENT_TYPE_GV_1            = 4,
-    CUSTOM_ELEMENT_TYPE_GV_2            = 5,
-    CUSTOM_ELEMENT_TYPE_GV_3            = 6,
-    CUSTOM_ELEMENT_TYPE_GV_4            = 7,
-    CUSTOM_ELEMENT_TYPE_GV_5            = 8,
-    CUSTOM_ELEMENT_TYPE_GV_FLOAT_1_1    = 9,
-    CUSTOM_ELEMENT_TYPE_GV_FLOAT_2_1    = 10,
-    CUSTOM_ELEMENT_TYPE_GV_FLOAT_2_2    = 11,
-    CUSTOM_ELEMENT_TYPE_GV_FLOAT_3_1    = 12,
-    CUSTOM_ELEMENT_TYPE_GV_FLOAT_3_2    = 13,
-    CUSTOM_ELEMENT_TYPE_GV_FLOAT_4_1    = 14,
-    CUSTOM_ELEMENT_TYPE_LC_1            = 15,
-    CUSTOM_ELEMENT_TYPE_LC_2            = 16,
-    CUSTOM_ELEMENT_TYPE_LC_3            = 17,
-    CUSTOM_ELEMENT_TYPE_LC_4            = 18,
-    CUSTOM_ELEMENT_TYPE_LC_5            = 19,
-    CUSTOM_ELEMENT_TYPE_LC_FLOAT_1_1    = 20,
-    CUSTOM_ELEMENT_TYPE_LC_FLOAT_2_1    = 21,
-    CUSTOM_ELEMENT_TYPE_LC_FLOAT_2_2    = 22,
-    CUSTOM_ELEMENT_TYPE_LC_FLOAT_3_1    = 23,
-    CUSTOM_ELEMENT_TYPE_LC_FLOAT_3_2    = 24,
-    CUSTOM_ELEMENT_TYPE_LC_FLOAT_4_1    = 25,
+    CUSTOM_ELEMENT_TYPE_ICON_LC         = 4,
+    CUSTOM_ELEMENT_TYPE_GV_1            = 5,
+    CUSTOM_ELEMENT_TYPE_GV_2            = 6,
+    CUSTOM_ELEMENT_TYPE_GV_3            = 7,
+    CUSTOM_ELEMENT_TYPE_GV_4            = 8,
+    CUSTOM_ELEMENT_TYPE_GV_5            = 9,
+    CUSTOM_ELEMENT_TYPE_GV_FLOAT_1_1    = 10,
+    CUSTOM_ELEMENT_TYPE_GV_FLOAT_2_1    = 11,
+    CUSTOM_ELEMENT_TYPE_GV_FLOAT_2_2    = 12,
+    CUSTOM_ELEMENT_TYPE_GV_FLOAT_3_1    = 13,
+    CUSTOM_ELEMENT_TYPE_GV_FLOAT_3_2    = 14,
+    CUSTOM_ELEMENT_TYPE_GV_FLOAT_4_1    = 15,
+    CUSTOM_ELEMENT_TYPE_LC_1            = 16,
+    CUSTOM_ELEMENT_TYPE_LC_2            = 17,
+    CUSTOM_ELEMENT_TYPE_LC_3            = 18,
+    CUSTOM_ELEMENT_TYPE_LC_4            = 19,
+    CUSTOM_ELEMENT_TYPE_LC_5            = 20,
+    CUSTOM_ELEMENT_TYPE_LC_FLOAT_1_1    = 21,
+    CUSTOM_ELEMENT_TYPE_LC_FLOAT_2_1    = 22,
+    CUSTOM_ELEMENT_TYPE_LC_FLOAT_2_2    = 23,
+    CUSTOM_ELEMENT_TYPE_LC_FLOAT_3_1    = 24,
+    CUSTOM_ELEMENT_TYPE_LC_FLOAT_3_2    = 25,
+    CUSTOM_ELEMENT_TYPE_LC_FLOAT_4_1    = 26,
 } osdCustomElementType_e;
 
 typedef enum {

From cc5917558daaa3ba2544105b195dd81c8d4b753a Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Tue, 13 Aug 2024 13:41:28 +0100
Subject: [PATCH 20/39] remove gravity calibration changes

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

diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
index e11342ff38c..84c919c8a8b 100755
--- a/src/main/navigation/navigation_pos_estimator.c
+++ b/src/main/navigation/navigation_pos_estimator.c
@@ -345,11 +345,6 @@ static bool gravityCalibrationComplete(void)
     return zeroCalibrationIsCompleteS(&posEstimator.imu.gravityCalibration);
 }
 
-static bool gravityCalibrationIsValid(void)
-{
-    return gravityCalibrationComplete() || posEstimator.imu.calibratedGravityCMSS;
-}
-
 #define ACC_VIB_FACTOR_S 1.0f
 #define ACC_VIB_FACTOR_E 3.0f
 static void updateIMUEstimationWeight(const float dt)
@@ -417,13 +412,12 @@ static void updateIMUTopic(timeUs_t currentTimeUs)
 
         /* When unarmed, assume that accelerometer should measure 1G. Use that to correct accelerometer gain */
         if (gyroConfig()->init_gyro_cal_enabled) {
-            if (!ARMING_FLAG(WAS_EVER_ARMED)) {
+            if (!ARMING_FLAG(ARMED) && !gravityCalibrationComplete()) {
                 zeroCalibrationAddValueS(&posEstimator.imu.gravityCalibration, posEstimator.imu.accelNEU.z);
 
                 if (gravityCalibrationComplete()) {
                     zeroCalibrationGetZeroS(&posEstimator.imu.gravityCalibration, &posEstimator.imu.calibratedGravityCMSS);
                     setGravityCalibration(posEstimator.imu.calibratedGravityCMSS);
-                    restartGravityCalibration();    // Repeat calibration until first arm to continuously update zero datum
                     LOG_DEBUG(POS_ESTIMATOR, "Gravity calibration complete (%d)", (int)lrintf(posEstimator.imu.calibratedGravityCMSS));
                 }
             }
@@ -432,7 +426,7 @@ static void updateIMUTopic(timeUs_t currentTimeUs)
             posEstimator.imu.calibratedGravityCMSS = gyroConfig()->gravity_cmss_cal;
         }
 
-        if (gravityCalibrationIsValid()) {
+        if (gravityCalibrationComplete()) {
 #ifdef USE_SIMULATOR
             if (ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) {
                 posEstimator.imu.calibratedGravityCMSS = GRAVITY_CMSS;
@@ -929,5 +923,5 @@ void updatePositionEstimator(void)
 
 bool navIsCalibrationComplete(void)
 {
-    return gravityCalibrationIsValid();
+    return gravityCalibrationComplete();
 }

From 954eb3660d484ce88cf4b4678e19f00411a0aee5 Mon Sep 17 00:00:00 2001
From: trailx <36086061+trailx@users.noreply.github.com>
Date: Tue, 3 Sep 2024 09:03:25 -0400
Subject: [PATCH 21/39] Added documentation about GVARs, PIDFF Controllers, &
 Integer Math

- Added information about integer math
- Added divide-by-0 information
- Added numerical limits of GVARs
- Added helpful information about PIDFF controllers
---
 docs/Programming Framework.md | 14 +++++++++++---
 1 file changed, 11 insertions(+), 3 deletions(-)

diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md
index b2ce6fe3b93..7c2953d7165 100644
--- a/docs/Programming Framework.md	
+++ b/docs/Programming Framework.md	
@@ -24,6 +24,8 @@ INAV Programming Framework consists of:
 IPF can be edited using INAV Configurator user interface, or via CLI. To use COnfigurator, click the tab labeled
 "Programming". The various options shown in Configurator are described below.
 
+**Note:** IPF uses non-floating-point math, so it only can return integers.  If your programming line returns a decimal, it will be reduced an integer.  So if your math is `1` / `3` = , IPF will truncate the decimal and return `0`.
+
 ## Logic Conditions
 
 ### CLI
@@ -61,9 +63,8 @@ IPF can be edited using INAV Configurator user interface, or via CLI. To use COn
 | 14            | Basic: Add                    | Add `Operand A` to `Operand B` and returns the result |
 | 15            | Basic: Subtract               | Substract `Operand B` from `Operand A` and returns the result |
 | 16            | Basic: Multiply               | Multiply `Operand A` by `Operand B` and returns the result |
-| 17            | Basic: Divide                 | Divide `Operand A` by `Operand B` and returns the result |
-| 18            | Set GVAR                      | Store value from `Operand B` into the Global Variable addressed by
-`Operand A`. Bear in mind, that operand `Global Variable` means: Value stored in Global Variable of an index! To store in GVAR 1 use `Value 1` not `Global Variable 1` |
+| 17            | Basic: Divide                 | Divide `Operand A` by `Operand B` and returns the result. NOTE: If `Operand B` = `0`, the `Divide` operation will simply return `Operand A`|
+| 18            | Set GVAR                      | Store value from `Operand B` into the Global Variable addressed by `Operand A`. Bear in mind, that operand `Global Variable` means: Value stored in Global Variable of an index! To store in GVAR 1 use `Value 1` not `Global Variable 1` |
 | 19            | Increase GVAR                 | Increase the GVAR indexed by `Operand A` (use `Value 1` for Global Variable 1) with value from `Operand B`  |
 | 20            | Decrease GVAR                 | Decrease the GVAR indexed by `Operand A` (use `Value 1` for Global Variable 1) with value from `Operand B`  |
 | 21            | Set IO Port                   | Set I2C IO Expander pin `Operand A` to value of `Operand B`. `Operand A` accepts values `0-7` and `Operand B` accepts `0` and `1` |
@@ -233,8 +234,15 @@ All flags are reseted on ARM and DISARM event.
 
 `gvar <index> <default value> <min> <max>`
 
+**Note:**  Global Variables (GVARs) are limited to integers between negative `-32768` and positive `32767`.
+
 ## Programming PID
 
+IPF makes a set of general user PIDFF controllers avaliable for use in your program.  These PIDFF controllers are not tied to any roll/pitch/yaw profiles or other controls.
+The output of these controllers can be used in an IPF program by using the `Programming PID` operand.
+The `<setpoint value>` of the controller is the target value for the controller to hit.  The `<measurement value>` is the measurement of the current value.  For instance, `<setpoint value>` could be the speed you want to go, and `<measurement value>` is the current speed.
+P, I, D, and FF values will need to be manually adjusted to determine the appropriate value for the program and controller.
+
 `pid <index> <enabled> <setpoint type> <setpoint value> <measurement type> <measurement value> <P gain> <I gain> <D gain> <FF gain>`
 
 * `<index>` - ID of PID Controller, starting from `0`

From ef2d9c1084bd7883ea9522aafb63bf9648248c7c Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Mon, 9 Sep 2024 13:54:08 +0100
Subject: [PATCH 22/39] add baro v setting, clean up config calls

---
 docs/Settings.md                              | 10 ++++
 src/main/fc/settings.yaml                     |  6 +++
 src/main/navigation/navigation.h              |  1 +
 .../navigation/navigation_pos_estimator.c     | 54 +++++++++++--------
 4 files changed, 48 insertions(+), 23 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index 9afdd789685..c2b3247ff95 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -2122,6 +2122,16 @@ Weight of barometer measurements in estimated altitude and climb rate. Setting i
 
 ---
 
+### inav_w_z_baro_v
+
+Weight of barometer climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0.1 | 0 | 10 |
+
+---
+
 ### inav_w_z_gps_p
 
 Weight of GPS altitude measurements in estimated altitude. Setting is used on both airplanes and multirotors.
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 28279aff1e0..a1f1ef7ff74 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -2410,6 +2410,12 @@ groups:
         min: 0
         max: 10
         default_value: 0.35
+      - name: inav_w_z_baro_v
+        description: "Weight of barometer climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors."
+        field: w_z_baro_v
+        min: 0
+        max: 10
+        default_value: 0.1
       - name: inav_w_z_gps_p
         description: "Weight of GPS altitude measurements in estimated altitude. Setting is used on both airplanes and multirotors."
         field: w_z_gps_p
diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
index da65f0f3f57..24b8daa0402 100644
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@@ -239,6 +239,7 @@ typedef struct positionEstimationConfig_s {
     uint16_t max_surface_altitude;
 
     float w_z_baro_p;           // Weight (cutoff frequency) for barometer altitude measurements
+    float w_z_baro_v;           // Weight (cutoff frequency) for barometer climb rate measurements
 
     float w_z_surface_p;        // Weight (cutoff frequency) for surface altitude measurements
     float w_z_surface_v;        // Weight (cutoff frequency) for surface velocity measurements
diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
index 84c919c8a8b..5bcfc1e11eb 100755
--- a/src/main/navigation/navigation_pos_estimator.c
+++ b/src/main/navigation/navigation_pos_estimator.c
@@ -69,6 +69,7 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
         .max_surface_altitude = SETTING_INAV_MAX_SURFACE_ALTITUDE_DEFAULT,
 
         .w_z_baro_p = SETTING_INAV_W_Z_BARO_P_DEFAULT,
+        .w_z_baro_v = SETTING_INAV_W_Z_BARO_V_DEFAULT,
 
         .w_z_surface_p = SETTING_INAV_W_Z_SURFACE_P_DEFAULT,
         .w_z_surface_v = SETTING_INAV_W_Z_SURFACE_V_DEFAULT,
@@ -478,14 +479,16 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs)
     /* Figure out if we have valid position data from our data sources */
     uint32_t newFlags = 0;
 
+    const float max_eph_epv = positionEstimationConfig()->max_eph_epv;
+
     if ((sensors(SENSOR_GPS)
 #ifdef USE_GPS_FIX_ESTIMATION
             || STATE(GPS_ESTIMATED_FIX)
 #endif
         ) && posControl.gpsOrigin.valid &&
         ((currentTimeUs - posEstimator.gps.lastUpdateTime) <= MS2US(INAV_GPS_TIMEOUT_MS)) &&
-        (posEstimator.gps.eph < positionEstimationConfig()->max_eph_epv)) {
-        if (posEstimator.gps.epv < positionEstimationConfig()->max_eph_epv) {
+        (posEstimator.gps.eph < max_eph_epv)) {
+        if (posEstimator.gps.epv < max_eph_epv) {
             newFlags |= EST_GPS_XY_VALID | EST_GPS_Z_VALID;
         }
         else {
@@ -505,11 +508,11 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs)
         newFlags |= EST_FLOW_VALID;
     }
 
-    if (posEstimator.est.eph < positionEstimationConfig()->max_eph_epv) {
+    if (posEstimator.est.eph < max_eph_epv) {
         newFlags |= EST_XY_VALID;
     }
 
-    if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) {
+    if (posEstimator.est.epv < max_eph_epv) {
         newFlags |= EST_Z_VALID;
     }
 
@@ -602,16 +605,17 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
         // Altitude
         const float baroAltResidual = wBaro * ((isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z);
         const float baroVelZResidual = isAirCushionEffectDetected ? 0.0f : wBaro * (posEstimator.baro.baroAltRate - posEstimator.est.vel.z);
+        const float w_z_baro_p = positionEstimationConfig()->w_z_baro_p;
 
-        ctx->estPosCorr.z += baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt;
-        ctx->estVelCorr.z += baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * ctx->dt;
-        ctx->estVelCorr.z += baroVelZResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt;
+        ctx->estPosCorr.z += baroAltResidual * w_z_baro_p * ctx->dt;
+        ctx->estVelCorr.z += baroAltResidual * sq(w_z_baro_p) * ctx->dt;
+        ctx->estVelCorr.z += baroVelZResidual * positionEstimationConfig()->w_z_baro_v * ctx->dt;
 
-        ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, positionEstimationConfig()->w_z_baro_p);
+        ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, w_z_baro_p);
 
         // Accelerometer bias
         if (!isAirCushionEffectDetected) {
-            ctx->accBiasCorr.z -= baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p);
+            ctx->accBiasCorr.z -= baroAltResidual * sq(w_z_baro_p);
         }
 
         correctOK = ARMING_FLAG(WAS_EVER_ARMED);    // No correction until first armed
@@ -628,15 +632,16 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
             // Altitude
             const float gpsAltResidual = wGps * (posEstimator.gps.pos.z - posEstimator.est.pos.z);
             const float gpsVelZResidual = wGps * (posEstimator.gps.vel.z - posEstimator.est.vel.z);
+            const float w_z_gps_p = positionEstimationConfig()->w_z_gps_p;
 
-            ctx->estPosCorr.z += gpsAltResidual * positionEstimationConfig()->w_z_gps_p * ctx->dt;
-            ctx->estVelCorr.z += gpsAltResidual * sq(positionEstimationConfig()->w_z_gps_p) * ctx->dt;
+            ctx->estPosCorr.z += gpsAltResidual * w_z_gps_p * ctx->dt;
+            ctx->estVelCorr.z += gpsAltResidual * sq(w_z_gps_p) * ctx->dt;
             ctx->estVelCorr.z += gpsVelZResidual * positionEstimationConfig()->w_z_gps_v * ctx->dt;
 
-            ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, gpsAltResidual), positionEstimationConfig()->w_z_gps_p);
+            ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, gpsAltResidual), w_z_gps_p);
 
             // Accelerometer bias
-            ctx->accBiasCorr.z -= gpsAltResidual * sq(positionEstimationConfig()->w_z_gps_p);
+            ctx->accBiasCorr.z -= gpsAltResidual * sq(w_z_gps_p);
         }
 
         correctOK = ARMING_FLAG(WAS_EVER_ARMED);    // No correction until first armed
@@ -716,21 +721,23 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
 {
     estimationContext_t ctx;
 
+    const float max_eph_epv = positionEstimationConfig()->max_eph_epv;
+
     /* Calculate dT */
     ctx.dt = US2S(currentTimeUs - posEstimator.est.lastUpdateTime);
     posEstimator.est.lastUpdateTime = currentTimeUs;
 
     /* If IMU is not ready we can't estimate anything */
     if (!isImuReady()) {
-        posEstimator.est.eph = positionEstimationConfig()->max_eph_epv + 0.001f;
-        posEstimator.est.epv = positionEstimationConfig()->max_eph_epv + 0.001f;
+        posEstimator.est.eph = max_eph_epv + 0.001f;
+        posEstimator.est.epv = max_eph_epv + 0.001f;
         posEstimator.flags = 0;
         return;
     }
 
     /* Calculate new EPH and EPV for the case we didn't update postion */
-    ctx.newEPH = posEstimator.est.eph * ((posEstimator.est.eph <= positionEstimationConfig()->max_eph_epv) ? 1.0f + ctx.dt : 1.0f);
-    ctx.newEPV = posEstimator.est.epv * ((posEstimator.est.epv <= positionEstimationConfig()->max_eph_epv) ? 1.0f + ctx.dt : 1.0f);
+    ctx.newEPH = posEstimator.est.eph * ((posEstimator.est.eph <= max_eph_epv) ? 1.0f + ctx.dt : 1.0f);
+    ctx.newEPV = posEstimator.est.epv * ((posEstimator.est.epv <= max_eph_epv) ? 1.0f + ctx.dt : 1.0f);
     ctx.newFlags = calculateCurrentValidityFlags(currentTimeUs);
     vectorZero(&ctx.estPosCorr);
     vectorZero(&ctx.estVelCorr);
@@ -753,12 +760,12 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
         estimationCalculateCorrection_XY_FLOW(&ctx);
 
     // If we can't apply correction or accuracy is off the charts - decay velocity to zero
-    if (!estXYCorrectOk || ctx.newEPH > positionEstimationConfig()->max_eph_epv) {
+    if (!estXYCorrectOk || ctx.newEPH > max_eph_epv) {
         ctx.estVelCorr.x = (0.0f - posEstimator.est.vel.x) * positionEstimationConfig()->w_xy_res_v * ctx.dt;
         ctx.estVelCorr.y = (0.0f - posEstimator.est.vel.y) * positionEstimationConfig()->w_xy_res_v * ctx.dt;
     }
 
-    if (!estZCorrectOk || ctx.newEPV > positionEstimationConfig()->max_eph_epv) {
+    if (!estZCorrectOk || ctx.newEPV > max_eph_epv) {
         ctx.estVelCorr.z = (0.0f - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_res_v * ctx.dt;
     }
     // Boost the corrections based on accWeight
@@ -770,16 +777,17 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
     vectorAdd(&posEstimator.est.vel, &posEstimator.est.vel, &ctx.estVelCorr);
 
     /* Correct accelerometer bias */
-    if (positionEstimationConfig()->w_acc_bias > 0.0f) {
+    const float w_acc_bias = positionEstimationConfig()->w_acc_bias;
+    if (w_acc_bias > 0.0f) {
         const float accelBiasCorrMagnitudeSq = sq(ctx.accBiasCorr.x) + sq(ctx.accBiasCorr.y) + sq(ctx.accBiasCorr.z);
         if (accelBiasCorrMagnitudeSq < sq(INAV_ACC_BIAS_ACCEPTANCE_VALUE)) {
             /* transform error vector from NEU frame to body frame */
             imuTransformVectorEarthToBody(&ctx.accBiasCorr);
 
             /* Correct accel bias */
-            posEstimator.imu.accelBias.x += ctx.accBiasCorr.x * positionEstimationConfig()->w_acc_bias * ctx.dt;
-            posEstimator.imu.accelBias.y += ctx.accBiasCorr.y * positionEstimationConfig()->w_acc_bias * ctx.dt;
-            posEstimator.imu.accelBias.z += ctx.accBiasCorr.z * positionEstimationConfig()->w_acc_bias * ctx.dt;
+            posEstimator.imu.accelBias.x += ctx.accBiasCorr.x * w_acc_bias * ctx.dt;
+            posEstimator.imu.accelBias.y += ctx.accBiasCorr.y * w_acc_bias * ctx.dt;
+            posEstimator.imu.accelBias.z += ctx.accBiasCorr.z * w_acc_bias * ctx.dt;
         }
     }
 

From d0b5dbee4ed7a832bde6d18bf54ff9d53e9f18ff Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Tue, 10 Sep 2024 09:41:22 +0100
Subject: [PATCH 23/39] add new settings

---
 docs/Settings.md                                       | 2 +-
 src/main/fc/settings.yaml                              | 4 ++--
 src/main/navigation/navigation_pos_estimator.c         | 8 ++++----
 src/main/navigation/navigation_pos_estimator_private.h | 2 ++
 4 files changed, 9 insertions(+), 7 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index c2b3247ff95..817a6e2da28 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -1994,7 +1994,7 @@ Uncertainty value for barometric sensor [cm]
 
 ### inav_default_alt_sensor
 
-Sets the default altitude sensor to use, GPS or BARO, when the altitude error between the sensors exceeds a set limit. Only the default sensor will be used in this case. GPS error limit = 2 * inav_max_eph_epv. BARO error limit = 4 * inav_baro_epv.
+Sets the default altitude sensor to use. Settings GPS and BARO always use both sensors unless there is an altitude error between the sensors that exceeds a set limit. In this case only the selected sensor will be used while the altitude error limit is exceeded. GPS error limit = 2 * inav_max_eph_epv. BARO error limit = 4 * inav_baro_epv. Settings GPS_ONLY and BARO_ONLY will use only the selected sensor even if the other sensor is working. The other sensor will only be used as a backup if the selected sensor is no longer available to use.
 
 | Default | Min | Max |
 | --- | --- | --- |
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index a1f1ef7ff74..486fc54b8c1 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -199,7 +199,7 @@ tables:
     values: ["NONE", "SERIAL", "MSP"]
     enum: headTrackerDevType_e
   - name: default_altitude_source
-    values: ["GPS", "BARO"]
+    values: ["GPS", "BARO", "GPS ONLY", "BARO ONLY"]
     enum: navDefaultAltitudeSensor_e
 
 constants:
@@ -2471,7 +2471,7 @@ groups:
         min: 0
         max: 9999
       - name: inav_default_alt_sensor
-        description: "Sets the default altitude sensor to use, GPS or BARO, when the altitude error between the sensors exceeds a set limit. Only the default sensor will be used in this case. GPS error limit = 2 * inav_max_eph_epv. BARO error limit = 4 * inav_baro_epv."
+        description: "Sets the default altitude sensor to use. Settings GPS and BARO always use both sensors unless there is an altitude error between the sensors that exceeds a set limit. In this case only the selected sensor will be used while the altitude error limit is exceeded. GPS error limit = 2 * inav_max_eph_epv. BARO error limit = 4 * inav_baro_epv. Settings GPS_ONLY and BARO_ONLY will use only the selected sensor even if the other sensor is working. The other sensor will only be used as a backup if the selected sensor is no longer available to use."
         default_value: "GPS"
         field: default_alt_sensor
         table: default_altitude_source
diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
index 5bcfc1e11eb..8332d8a70aa 100755
--- a/src/main/navigation/navigation_pos_estimator.c
+++ b/src/main/navigation/navigation_pos_estimator.c
@@ -559,11 +559,11 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
     DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount());            // Clip count
 
     bool correctOK = false;
-    float wBaro = 1.0f;
-    float wGps = 1.0f;
+    const uint8_t defaultAltitudeSource = positionEstimationConfig()->default_alt_sensor;
+    float wGps = defaultAltitudeSource == ALTITUDE_SOURCE_BARO_ONLY && ctx->newFlags & EST_BARO_VALID ? 0.0f : 1.0f;
+    float wBaro = defaultAltitudeSource == ALTITUDE_SOURCE_GPS_ONLY && ctx->newFlags & EST_GPS_Z_VALID ? 0.0f : 1.0f;
 
-    if (ctx->newFlags & EST_BARO_VALID && ctx->newFlags & EST_GPS_Z_VALID) {
-        const uint8_t defaultAltitudeSource = positionEstimationConfig()->default_alt_sensor;
+    if (wBaro && ctx->newFlags & EST_BARO_VALID && wGps && ctx->newFlags & EST_GPS_Z_VALID) {
         const float gpsBaroResidual = fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt);
 
         // Fade out the non default sensor to prevent sudden jump
diff --git a/src/main/navigation/navigation_pos_estimator_private.h b/src/main/navigation/navigation_pos_estimator_private.h
index 46fcb42e177..5cbdc81c030 100644
--- a/src/main/navigation/navigation_pos_estimator_private.h
+++ b/src/main/navigation/navigation_pos_estimator_private.h
@@ -149,6 +149,8 @@ typedef enum {
 typedef enum {
     ALTITUDE_SOURCE_GPS,
     ALTITUDE_SOURCE_BARO,
+    ALTITUDE_SOURCE_GPS_ONLY,
+    ALTITUDE_SOURCE_BARO_ONLY,
 } navDefaultAltitudeSensor_e;
 
 typedef struct {

From 4f02dc0b50d33d9dc286d16bc64f31627546efc9 Mon Sep 17 00:00:00 2001
From: trailx <36086061+trailx@users.noreply.github.com>
Date: Tue, 10 Sep 2024 08:24:13 -0400
Subject: [PATCH 24/39] Update Programming Framework.md

Updated integer math comment based on feedback
---
 docs/Programming Framework.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md
index 7c2953d7165..c2314dc627e 100644
--- a/docs/Programming Framework.md	
+++ b/docs/Programming Framework.md	
@@ -24,7 +24,7 @@ INAV Programming Framework consists of:
 IPF can be edited using INAV Configurator user interface, or via CLI. To use COnfigurator, click the tab labeled
 "Programming". The various options shown in Configurator are described below.
 
-**Note:** IPF uses non-floating-point math, so it only can return integers.  If your programming line returns a decimal, it will be reduced an integer.  So if your math is `1` / `3` = , IPF will truncate the decimal and return `0`.
+**Note:** IPF uses integer math. If your programming line returns a decimal, it will be truncated to an integer.  So if your math is `1` / `3` = , IPF will truncate the decimal and return `0`.
 
 ## Logic Conditions
 

From 387d51e1dfbb6dfb0f8252d78f384face876ff80 Mon Sep 17 00:00:00 2001
From: Darren Lines <darren@darrenlines.uk>
Date: Wed, 11 Sep 2024 20:50:58 +0100
Subject: [PATCH 25/39] Added switch indicators back in

- Added switch indicators back in
- Updated OSD document
---
 docs/OSD.md               | 13 +++++-
 docs/Settings.md          | 90 +++++++++++++++++++++++++++++++++++++++
 src/main/fc/settings.yaml | 53 +++++++++++++++++++++++
 src/main/io/osd.c         | 63 +++++++++++++++++++++++++++
 src/main/io/osd.h         | 25 ++++++++---
 5 files changed, 238 insertions(+), 6 deletions(-)

diff --git a/docs/OSD.md b/docs/OSD.md
index 5787496687a..b492e537cef 100644
--- a/docs/OSD.md
+++ b/docs/OSD.md
@@ -177,7 +177,18 @@ Here are the OSD Elements provided by INAV.
 | 144 | OSD_MULTI_FUNCTION                               | 7.0.0  |       |
 | 145 | OSD_ODOMETER                                     | 7.0.0  | For this to work correctly, stats must be enabled (`set stats=ON`). Otherwise, this will show the total flight distance. |
 | 146 | OSD_PILOT_LOGO                                   | 7.0.0  |       |
-| 147 | OSD_BLACKBOX                                     | 8.0.0  | The element will be hidden unless blackbox recording is attempted. |
+| 147 | OSD_CUSTOM_ELEMENT_1                             | 7.0.0  |       |
+| 148 | OSD_CUSTOM_ELEMENT_2                             | 7.0.0  |       |
+| 149 | OSD_CUSTOM_ELEMENT_3                             | 7.0.0  |       |
+| 150 | OSD_ADSB_WARNING                                 | 7.0.0  |       |
+| 151 | OSD_ADSB_INFO                                    | 7.0.0  |       |
+| 152 | OSD_BLACKBOX                                     | 8.0.0  | The element will be hidden unless blackbox recording is attempted. |
+| 153 | OSD_FORMATION_FLIGHT                             | 8.0.0  |       |
+| 154 | OSD_CUSTOM_ELEMENT_4                             | 8.0.0  |       |
+| 155 | OSD_CUSTOM_ELEMENT_5                             | 8.0.0  |       |
+| 156 | OSD_CUSTOM_ELEMENT_6                             | 8.0.0  |       |
+| 157 | OSD_CUSTOM_ELEMENT_7                             | 8.0.0  |       |
+| 158 | OSD_CUSTOM_ELEMENT_8                             | 8.0.0  |       |
 
 # Pilot Logos
 
diff --git a/docs/Settings.md b/docs/Settings.md
index 1e7bf47381c..6487b44b6d1 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -5152,6 +5152,96 @@ Enabling this option will show metric efficiency statistics on the post flight s
 
 ---
 
+### osd_switch_indicator_one_channel
+
+RC Channel to use for OSD switch indicator 1.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 5 | 5 | MAX_SUPPORTED_RC_CHANNEL_COUNT |
+
+---
+
+### osd_switch_indicator_one_name
+
+Character to use for OSD switch incicator 1.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| GEAR |  | 5 |
+
+---
+
+### osd_switch_indicator_three_channel
+
+RC Channel to use for OSD switch indicator 3.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 5 | 5 | MAX_SUPPORTED_RC_CHANNEL_COUNT |
+
+---
+
+### osd_switch_indicator_three_name
+
+Character to use for OSD switch incicator 3.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| LIGT |  | 5 |
+
+---
+
+### osd_switch_indicator_two_channel
+
+RC Channel to use for OSD switch indicator 2.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 5 | 5 | MAX_SUPPORTED_RC_CHANNEL_COUNT |
+
+---
+
+### osd_switch_indicator_two_name
+
+Character to use for OSD switch incicator 2.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| CAM |  | 5 |
+
+---
+
+### osd_switch_indicator_zero_channel
+
+RC Channel to use for OSD switch indicator 0.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 5 | 5 | MAX_SUPPORTED_RC_CHANNEL_COUNT |
+
+---
+
+### osd_switch_indicator_zero_name
+
+Character to use for OSD switch incicator 0.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| FLAP |  | 5 |
+
+---
+
+### osd_switch_indicators_align_left
+
+Align text to left of switch indicators
+
+| Default | Min | Max |
+| --- | --- | --- |
+| ON | OFF | ON |
+
+---
+
 ### osd_system_msg_display_time
 
 System message display cycle time for multiple messages (milliseconds).
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 1bd1400d0a6..00119727f34 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -3717,6 +3717,59 @@ groups:
         field: highlight_djis_missing_characters
         default_value: ON
         type: bool
+      - name: osd_switch_indicator_zero_name
+        description: "Character to use for OSD switch incicator 0."
+        field: osd_switch_indicator0_name
+        type: string
+        max: 5
+        default_value: "FLAP"
+      - name: osd_switch_indicator_one_name
+        description: "Character to use for OSD switch incicator 1."
+        field: osd_switch_indicator1_name
+        type: string
+        max: 5
+        default_value: "GEAR"
+      - name: osd_switch_indicator_two_name
+        description: "Character to use for OSD switch incicator 2."
+        field: osd_switch_indicator2_name
+        type: string
+        max: 5
+        default_value: "CAM"
+      - name: osd_switch_indicator_three_name
+        description: "Character to use for OSD switch incicator 3."
+        field: osd_switch_indicator3_name
+        type: string
+        max: 5
+        default_value: "LIGT"
+      - name: osd_switch_indicator_zero_channel
+        description: "RC Channel to use for OSD switch indicator 0."
+        field: osd_switch_indicator0_channel
+        min: 5
+        max: MAX_SUPPORTED_RC_CHANNEL_COUNT
+        default_value: 5
+      - name: osd_switch_indicator_one_channel
+        description: "RC Channel to use for OSD switch indicator 1."
+        field: osd_switch_indicator1_channel
+        min: 5
+        max: MAX_SUPPORTED_RC_CHANNEL_COUNT
+        default_value: 5
+      - name: osd_switch_indicator_two_channel
+        description: "RC Channel to use for OSD switch indicator 2."
+        field: osd_switch_indicator2_channel
+        min: 5
+        max: MAX_SUPPORTED_RC_CHANNEL_COUNT
+        default_value: 5
+      - name: osd_switch_indicator_three_channel
+        description: "RC Channel to use for OSD switch indicator 3."
+        field: osd_switch_indicator3_channel
+        min: 5
+        max: MAX_SUPPORTED_RC_CHANNEL_COUNT
+        default_value: 5
+      - name: osd_switch_indicators_align_left
+        description: "Align text to left of switch indicators"
+        field: osd_switch_indicators_align_left
+        type: bool
+        default_value: ON
   - name: PG_OSD_COMMON_CONFIG
     type: osdCommonConfig_t
     headers: ["io/osd_common.h"]
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 77762c6a003..249359d4b6d 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -1610,6 +1610,40 @@ int8_t getGeoWaypointNumber(int8_t waypointIndex)
     return geoWaypointIndex - posControl.startWpIndex + 1;
 }
 
+void osdDisplaySwitchIndicator(const char *swName, int rcValue, char *buff) {
+    int8_t ptr = 0;
+
+    if (osdConfig()->osd_switch_indicators_align_left) {
+        for (ptr = 0; ptr < constrain(strlen(swName), 0, OSD_SWITCH_INDICATOR_NAME_LENGTH); ptr++) {
+            buff[ptr] = swName[ptr];
+        }
+
+        if ( rcValue < 1333) {
+            buff[ptr++] = SYM_SWITCH_INDICATOR_LOW;
+        } else if ( rcValue > 1666) {
+            buff[ptr++] = SYM_SWITCH_INDICATOR_HIGH;
+        } else {
+            buff[ptr++] = SYM_SWITCH_INDICATOR_MID;
+        }
+    } else {
+        if ( rcValue < 1333) {
+            buff[ptr++] = SYM_SWITCH_INDICATOR_LOW;
+        } else if ( rcValue > 1666) {
+            buff[ptr++] = SYM_SWITCH_INDICATOR_HIGH;
+        } else {
+            buff[ptr++] = SYM_SWITCH_INDICATOR_MID;
+        }
+
+        for (ptr = 1; ptr < constrain(strlen(swName), 0, OSD_SWITCH_INDICATOR_NAME_LENGTH) + 1; ptr++) {
+            buff[ptr] = swName[ptr-1];
+        }
+
+        ptr++;
+    }
+
+    buff[ptr] = '\0';
+}
+
 static bool osdDrawSingleElement(uint8_t item)
 {
     uint16_t pos = osdLayoutsConfig()->item_pos[currentLayout][item];
@@ -2863,6 +2897,21 @@ static bool osdDrawSingleElement(uint8_t item)
             break;
         }
 #endif
+    case OSD_SWITCH_INDICATOR_0:
+        osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator0_name, rxGetChannelValue(osdConfig()->osd_switch_indicator0_channel - 1), buff);
+        break;
+
+    case OSD_SWITCH_INDICATOR_1:
+        osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator1_name, rxGetChannelValue(osdConfig()->osd_switch_indicator1_channel - 1), buff);
+        break;
+
+    case OSD_SWITCH_INDICATOR_2:
+        osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator2_name, rxGetChannelValue(osdConfig()->osd_switch_indicator2_channel - 1), buff);
+        break;
+
+    case OSD_SWITCH_INDICATOR_3:
+        osdDisplaySwitchIndicator(osdConfig()->osd_switch_indicator3_name, rxGetChannelValue(osdConfig()->osd_switch_indicator3_channel - 1), buff);
+        break;
 
     case OSD_PAN_SERVO_CENTRED:
         {
@@ -3983,6 +4032,15 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
     .pan_servo_indicator_show_degrees = SETTING_OSD_PAN_SERVO_INDICATOR_SHOW_DEGREES_DEFAULT,
     .esc_rpm_precision = SETTING_OSD_ESC_RPM_PRECISION_DEFAULT,
     .mAh_precision = SETTING_OSD_MAH_PRECISION_DEFAULT,
+    .osd_switch_indicator0_name = SETTING_OSD_SWITCH_INDICATOR_ZERO_NAME_DEFAULT,
+    .osd_switch_indicator0_channel = SETTING_OSD_SWITCH_INDICATOR_ZERO_CHANNEL_DEFAULT,
+    .osd_switch_indicator1_name = SETTING_OSD_SWITCH_INDICATOR_ONE_NAME_DEFAULT,
+    .osd_switch_indicator1_channel = SETTING_OSD_SWITCH_INDICATOR_ONE_CHANNEL_DEFAULT,
+    .osd_switch_indicator2_name = SETTING_OSD_SWITCH_INDICATOR_TWO_NAME_DEFAULT,
+    .osd_switch_indicator2_channel = SETTING_OSD_SWITCH_INDICATOR_TWO_CHANNEL_DEFAULT,
+    .osd_switch_indicator3_name = SETTING_OSD_SWITCH_INDICATOR_THREE_NAME_DEFAULT,
+    .osd_switch_indicator3_channel = SETTING_OSD_SWITCH_INDICATOR_THREE_CHANNEL_DEFAULT,
+    .osd_switch_indicators_align_left = SETTING_OSD_SWITCH_INDICATORS_ALIGN_LEFT_DEFAULT,
     .system_msg_display_time = SETTING_OSD_SYSTEM_MSG_DISPLAY_TIME_DEFAULT,
     .units = SETTING_OSD_UNITS_DEFAULT,
     .main_voltage_decimals = SETTING_OSD_MAIN_VOLTAGE_DECIMALS_DEFAULT,
@@ -4157,6 +4215,11 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
 
     osdLayoutsConfig->item_pos[0][OSD_MULTI_FUNCTION] = OSD_POS(1, 4);
 
+    osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_0] = OSD_POS(2, 7);
+    osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_1] = OSD_POS(2, 8);
+    osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_2] = OSD_POS(2, 9);
+    osdLayoutsConfig->item_pos[0][OSD_SWITCH_INDICATOR_3] = OSD_POS(2, 10);
+
     osdLayoutsConfig->item_pos[0][OSD_ADSB_WARNING] = OSD_POS(2, 7);
     osdLayoutsConfig->item_pos[0][OSD_ADSB_INFO] = OSD_POS(2, 8);
 #if defined(USE_ESC_SENSOR)
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index 2e5200a0483..0cf93b69162 100644
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -263,10 +263,10 @@ typedef enum {
     OSD_AIR_MAX_SPEED,
     OSD_ACTIVE_PROFILE,
     OSD_MISSION,
-    OSD_CUSTOM_ELEMENT_4,
-    OSD_CUSTOM_ELEMENT_5,
-    OSD_CUSTOM_ELEMENT_6,
-    OSD_CUSTOM_ELEMENT_7,
+    OSD_SWITCH_INDICATOR_0,
+    OSD_SWITCH_INDICATOR_1,
+    OSD_SWITCH_INDICATOR_2,
+    OSD_SWITCH_INDICATOR_3,
     OSD_TPA_TIME_CONSTANT,
     OSD_FW_LEVEL_TRIM,
     OSD_GLIDE_TIME_REMAINING,
@@ -287,7 +287,11 @@ typedef enum {
     OSD_ADSB_INFO,
     OSD_BLACKBOX,
     OSD_FORMATION_FLIGHT,
-    OSD_CUSTOM_ELEMENT_8, // 154  
+    OSD_CUSTOM_ELEMENT_4,
+    OSD_CUSTOM_ELEMENT_5,
+    OSD_CUSTOM_ELEMENT_6,
+    OSD_CUSTOM_ELEMENT_7,
+    OSD_CUSTOM_ELEMENT_8, // 158
     OSD_ITEM_COUNT // MUST BE LAST
 } osd_items_e;
 
@@ -348,6 +352,8 @@ typedef struct osdLayoutsConfig_s {
 
 PG_DECLARE(osdLayoutsConfig_t, osdLayoutsConfig);
 
+#define OSD_SWITCH_INDICATOR_NAME_LENGTH 4
+
 typedef struct osdConfig_s {
     // Alarms
     uint8_t         rssi_alarm;                         // rssi %
@@ -445,6 +451,15 @@ typedef struct osdConfig_s {
     uint16_t        system_msg_display_time;            // system message display time for multiple messages (ms)
     uint8_t         mAh_precision;                      // Number of numbers used for mAh drawn. Plently of packs now are > 9999 mAh
     uint8_t         ahi_pitch_interval;                 // redraws AHI at set pitch interval (Not pixel OSD)
+    char            osd_switch_indicator0_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 0.
+    uint8_t         osd_switch_indicator0_channel;      // RC Channel to use for switch indicator 0.
+    char            osd_switch_indicator1_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 1.
+    uint8_t         osd_switch_indicator1_channel;      // RC Channel to use for switch indicator 1.
+    char            osd_switch_indicator2_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 2.
+    uint8_t         osd_switch_indicator2_channel;      // RC Channel to use for switch indicator 2.
+    char            osd_switch_indicator3_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 3.
+    uint8_t         osd_switch_indicator3_channel;      // RC Channel to use for switch indicator 3.
+    bool            osd_switch_indicators_align_left;   // Align switch indicator name to left of the switch.
     bool            use_pilot_logo;                     // If enabled, the pilot logo (last 40 characters of page 2 font) will be used with the INAV logo.
     uint8_t         inav_to_pilot_logo_spacing;         // The space between the INAV and pilot logos, if pilot logo is used. This number may be adjusted so that it fits the odd/even col width.
     uint16_t        arm_screen_display_time;            // Length of time the arm screen is displayed

From cb41ee70f0eac892f0ff7c8021cbb49ac33f8ab5 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Wed, 11 Sep 2024 17:40:49 -0700
Subject: [PATCH 26/39] Update Settings.md to include units for
 nav_max_terrain_follow_alt

Added units to the description for nav_max_terrain_follow_alt
---
 docs/Settings.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index 6ab03e46334..a20809032ad 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -3654,7 +3654,7 @@ Maximum speed allowed in fully autonomous modes (RTH, WP) [cm/s] [Multirotor onl
 
 ### nav_max_terrain_follow_alt
 
-Max allowed above the ground altitude for terrain following mode
+Max allowed above the ground altitude for terrain following mode [cm]
 
 | Default | Min | Max |
 | --- | --- | --- |

From 3f332ac5ef98af5ed7eff1fc741af0243ddb8038 Mon Sep 17 00:00:00 2001
From: daijoubu <daijoubu@gmail.com>
Date: Wed, 11 Sep 2024 18:26:00 -0700
Subject: [PATCH 27/39] Update settings.yaml

Keep settings.yaml and .md in sync
---
 src/main/fc/settings.yaml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 36646efb2c0..74f525ce5f1 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -2692,7 +2692,7 @@ groups:
       - name: nav_max_terrain_follow_alt
         field: general.max_terrain_follow_altitude
         default_value: "100"
-        description: "Max allowed above the ground altitude for terrain following mode"
+        description: "Max allowed above the ground altitude for terrain following mode [cm]"
         max: 1000
         default_value: 100
       - name: nav_max_altitude

From c1565a77343952ebf2cad95d9d581c16044669e3 Mon Sep 17 00:00:00 2001
From: Roman Lut <11955117+RomanLut@users.noreply.github.com>
Date: Wed, 18 Sep 2024 05:09:52 +0200
Subject: [PATCH 28/39] fixed z position estimator error estimation

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

diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
index d5a342173d3..d20087f9e32 100755
--- a/src/main/navigation/navigation_pos_estimator.c
+++ b/src/main/navigation/navigation_pos_estimator.c
@@ -617,7 +617,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
             ctx->estPosCorr.z += gpsAltResudual * positionEstimationConfig()->w_z_gps_p * ctx->dt;
             ctx->estVelCorr.z += gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p) * ctx->dt;
             ctx->estVelCorr.z += gpsVelZResudual * positionEstimationConfig()->w_z_gps_v * ctx->dt;
-            ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, gpsAltResudual), positionEstimationConfig()->w_z_gps_p);
+            ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, fabsf(gpsAltResudual)), positionEstimationConfig()->w_z_gps_p);
 
             // Accelerometer bias
             ctx->accBiasCorr.z -= gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p);

From ce7e01400172a8837c64a542c80745858bc8caa1 Mon Sep 17 00:00:00 2001
From: flywoo <viper_rc@outlook.com>
Date: Wed, 18 Sep 2024 15:45:45 +0800
Subject: [PATCH 29/39] Update FLYWOOF745 config.c

---
 src/main/target/FLYWOOF745/config.c | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/main/target/FLYWOOF745/config.c b/src/main/target/FLYWOOF745/config.c
index 7dfe40728dd..16cadc0231b 100644
--- a/src/main/target/FLYWOOF745/config.c
+++ b/src/main/target/FLYWOOF745/config.c
@@ -32,6 +32,5 @@ void targetConfiguration(void)
     // Make sure S1-S4 default to Motors
 
     timerOverridesMutable(timer2id(TIM3))->outputMode = OUTPUT_MODE_MOTORS;
-    timerOverridesMutable(timer2id(TIM8))->outputMode = OUTPUT_MODE_MOTORS;
     timerOverridesMutable(timer2id(TIM1))->outputMode = OUTPUT_MODE_MOTORS;
 }

From f9745d7a643e3fcfd7914eef97702b69384900a1 Mon Sep 17 00:00:00 2001
From: Scavanger <andreas.kanzler@outlook.com>
Date: Sun, 15 Sep 2024 19:07:49 -0300
Subject: [PATCH 30/39] Gforce Buffe fix

---
 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 ba4571a8b4c..1b88226b9cb 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -5022,7 +5022,7 @@ uint8_t drawStat_ESCTemperature(uint8_t col, uint8_t row, uint8_t statValX)
 uint8_t drawStat_GForce(uint8_t col, uint8_t row, uint8_t statValX)
 {
     char buff[12];
-    char outBuff[12];
+    char outBuff[20];
 
     const float max_gforce = accGetMeasuredMaxG();
     const acc_extremes_t *acc_extremes = accGetMeasuredExtremes();

From 5260d88f9c09d6d88d1e856caea62f704faed5a1 Mon Sep 17 00:00:00 2001
From: bkleiner <benedikt.kleiner@gmail.com>
Date: Wed, 18 Sep 2024 10:44:34 +0800
Subject: [PATCH 31/39] tbs_lucid: mask PA13/PA14 to enable debugging

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

diff --git a/src/main/target/TBS_LUCID_FC/target.h b/src/main/target/TBS_LUCID_FC/target.h
index 7a9ed60fb52..8ee8716133d 100644
--- a/src/main/target/TBS_LUCID_FC/target.h
+++ b/src/main/target/TBS_LUCID_FC/target.h
@@ -130,7 +130,7 @@
 #define VBAT_ADC_CHANNEL          ADC_CHN_1
 #define CURRENT_METER_ADC_CHANNEL ADC_CHN_2
 
-#define TARGET_IO_PORTA           0xffff
+#define TARGET_IO_PORTA           (0xffff & ~(BIT(14)|BIT(13)))
 #define TARGET_IO_PORTB           0xffff
 #define TARGET_IO_PORTC           0xffff
 #define TARGET_IO_PORTD           (BIT(2) | BIT(12) | BIT(13))

From 9923c306201628d6dc9c416a4122b0be07fa21d2 Mon Sep 17 00:00:00 2001
From: bkleiner <benedikt.kleiner@gmail.com>
Date: Mon, 23 Sep 2024 11:55:38 +0800
Subject: [PATCH 32/39] lucid: use PC5 as adc, use PA13/PA14 as PINIO

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

diff --git a/src/main/target/TBS_LUCID_FC/target.h b/src/main/target/TBS_LUCID_FC/target.h
index 8ee8716133d..f5bdecc0fac 100644
--- a/src/main/target/TBS_LUCID_FC/target.h
+++ b/src/main/target/TBS_LUCID_FC/target.h
@@ -120,17 +120,19 @@
 
 #define USE_PINIO
 #define USE_PINIOBOX
-#define PINIO1_PIN                PC5  // Camera Control
+#define PINIO1_PIN                PA13
+#define PINIO2_PIN                PA14
 
 #define USE_ADC
 #define ADC_INSTANCE              ADC1
 #define ADC1_DMA_STREAM           DMA2_CHANNEL7
 #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 TARGET_IO_PORTA           (0xffff & ~(BIT(14)|BIT(13)))
+#define TARGET_IO_PORTA           0xffff
 #define TARGET_IO_PORTB           0xffff
 #define TARGET_IO_PORTC           0xffff
 #define TARGET_IO_PORTD           (BIT(2) | BIT(12) | BIT(13))

From 302eff2c8e52e08a4509e8dfff2c11f3383ebb96 Mon Sep 17 00:00:00 2001
From: bkleiner <benedikt.kleiner@gmail.com>
Date: Wed, 18 Sep 2024 10:54:57 +0800
Subject: [PATCH 33/39] at32: add abilty to specify uart af

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

diff --git a/src/main/drivers/serial_uart_at32f43x.c b/src/main/drivers/serial_uart_at32f43x.c
index 90ebe05ef82..ad4b4e0c458 100644
--- a/src/main/drivers/serial_uart_at32f43x.c
+++ b/src/main/drivers/serial_uart_at32f43x.c
@@ -54,7 +54,11 @@ static uartDevice_t uart1 =
     .dev = USART1,
     .rx = IO_TAG(UART1_RX_PIN),
     .tx = IO_TAG(UART1_TX_PIN),
+#ifdef UART1_AF
+    .af = CONCAT(GPIO_MUX_, UART1_AF),
+#else
     .af = GPIO_MUX_7,
+#endif
 #ifdef UART1_AHB1_PERIPHERALS
     .rcc_ahb1 = UART1_AHB1_PERIPHERALS,
 #endif
@@ -75,7 +79,11 @@ static uartDevice_t uart2 =
     .dev = USART2,
     .rx = IO_TAG(UART2_RX_PIN),
     .tx = IO_TAG(UART2_TX_PIN),
+#ifdef UART2_AF
+    .af = CONCAT(GPIO_MUX_, UART2_AF),
+#else
     .af = GPIO_MUX_7,
+#endif
 #ifdef UART2_AHB1_PERIPHERALS
     .rcc_ahb1 = UART2_AHB1_PERIPHERALS,
 #endif
@@ -96,7 +104,11 @@ static uartDevice_t uart3 =
     .dev = USART3,
     .rx = IO_TAG(UART3_RX_PIN),
     .tx = IO_TAG(UART3_TX_PIN),
+#ifdef UART3_AF
+    .af = CONCAT(GPIO_MUX_, UART3_AF),
+#else
     .af = GPIO_MUX_7,
+#endif
 #ifdef UART3_AHB1_PERIPHERALS
     .rcc_ahb1 = UART3_AHB1_PERIPHERALS,
 #endif
@@ -117,7 +129,11 @@ static uartDevice_t uart4 =
     .dev = UART4,
     .rx = IO_TAG(UART4_RX_PIN),
     .tx = IO_TAG(UART4_TX_PIN),
+#ifdef UART4_AF
+    .af = CONCAT(GPIO_MUX_, UART4_AF),
+#else
     .af = GPIO_MUX_8,
+#endif
 #ifdef UART4_AHB1_PERIPHERALS
     .rcc_ahb1 = UART4_AHB1_PERIPHERALS,
 #endif
@@ -138,7 +154,11 @@ static uartDevice_t uart5 =
     .dev = UART5,
     .rx = IO_TAG(UART5_RX_PIN),
     .tx = IO_TAG(UART5_TX_PIN),
+#ifdef UART5_AF
+    .af = CONCAT(GPIO_MUX_, UART5_AF),
+#else
     .af = GPIO_MUX_8,
+#endif
 #ifdef UART5_AHB1_PERIPHERALS
     .rcc_ahb1 = UART5_AHB1_PERIPHERALS,
 #endif
@@ -159,7 +179,11 @@ static uartDevice_t uart6 =
     .dev = USART6,
     .rx = IO_TAG(UART6_RX_PIN),
     .tx = IO_TAG(UART6_TX_PIN),
+#ifdef UART6_AF
+    .af = CONCAT(GPIO_MUX_, UART6_AF),
+#else
     .af = GPIO_MUX_8,
+#endif
 #ifdef UART6_AHB1_PERIPHERALS
     .rcc_ahb1 = UART6_AHB1_PERIPHERALS,
 #endif
@@ -180,7 +204,11 @@ static uartDevice_t uart7 =
     .dev = UART7,
     .rx = IO_TAG(UART7_RX_PIN),
     .tx = IO_TAG(UART7_TX_PIN),
+#ifdef UART7_AF
+    .af = CONCAT(GPIO_MUX_, UART7_AF),
+#else
     .af = GPIO_MUX_8,
+#endif
     .rcc_apb1 = RCC_APB1(UART7),
     .irq = UART7_IRQn,
     .irqPriority = NVIC_PRIO_SERIALUART,
@@ -198,7 +226,11 @@ static uartDevice_t uart8 =
     .dev = UART8,
     .rx = IO_TAG(UART8_RX_PIN),
     .tx = IO_TAG(UART8_TX_PIN),
+#ifdef UART8_AF
+    .af = CONCAT(GPIO_MUX_, UART8_AF),
+#else
     .af = GPIO_MUX_8,
+#endif
     .rcc_apb1 = RCC_APB1(UART8),
     .irq = UART8_IRQn,
     .irqPriority = NVIC_PRIO_SERIALUART,

From d5ab77e1a7297cfbf2104e13f9998d4234f3a425 Mon Sep 17 00:00:00 2001
From: bkleiner <benedikt.kleiner@gmail.com>
Date: Wed, 18 Sep 2024 10:58:46 +0800
Subject: [PATCH 34/39] tbs_lucid: specify AF for uart2

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

diff --git a/src/main/target/TBS_LUCID_FC/target.h b/src/main/target/TBS_LUCID_FC/target.h
index f5bdecc0fac..6e755b9a1a5 100644
--- a/src/main/target/TBS_LUCID_FC/target.h
+++ b/src/main/target/TBS_LUCID_FC/target.h
@@ -41,6 +41,7 @@
 #define UART1_RX_PIN              PA10
 
 #define USE_UART2
+#define UART2_AF                  6
 #define UART2_TX_PIN              NONE
 #define UART2_RX_PIN              PB0
 

From a591a83cf7dc3a856c05744a08c6717c25e530c9 Mon Sep 17 00:00:00 2001
From: bfmvsa <bfmvsa@gmail.com>
Date: Thu, 26 Sep 2024 11:17:09 +0200
Subject: [PATCH 35/39] Change bus for built-in compass

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

diff --git a/src/main/target/MICOAIR743/target.h b/src/main/target/MICOAIR743/target.h
index 4ba27f3447e..52ebef24e1b 100644
--- a/src/main/target/MICOAIR743/target.h
+++ b/src/main/target/MICOAIR743/target.h
@@ -100,7 +100,7 @@
 #define BARO_I2C_BUS            BUS_I2C2
 
 #define USE_MAG
-#define MAG_I2C_BUS             BUS_I2C1
+#define MAG_I2C_BUS             BUS_I2C2
 #define USE_MAG_ALL
 
 // *************** ENABLE OPTICAL FLOW & RANGEFINDER *****************************

From 7907c8c70866fd04289437b0fdbaa3bbbd10b0d1 Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Thu, 26 Sep 2024 11:51:11 +0200
Subject: [PATCH 36/39] Add target for external compass

---
 src/main/target/MICOAIR743/CMakeLists.txt | 3 ++-
 src/main/target/MICOAIR743/target.h       | 7 +++++++
 2 files changed, 9 insertions(+), 1 deletion(-)

diff --git a/src/main/target/MICOAIR743/CMakeLists.txt b/src/main/target/MICOAIR743/CMakeLists.txt
index 1d381638780..6847f3daefe 100644
--- a/src/main/target/MICOAIR743/CMakeLists.txt
+++ b/src/main/target/MICOAIR743/CMakeLists.txt
@@ -1 +1,2 @@
-target_stm32h743xi(MICOAIR743)
\ No newline at end of file
+target_stm32h743xi(MICOAIR743)
+target_stm32h743xi(MICOAIR743_EXTMAG)
diff --git a/src/main/target/MICOAIR743/target.h b/src/main/target/MICOAIR743/target.h
index 52ebef24e1b..9326d6622b9 100644
--- a/src/main/target/MICOAIR743/target.h
+++ b/src/main/target/MICOAIR743/target.h
@@ -100,7 +100,14 @@
 #define BARO_I2C_BUS            BUS_I2C2
 
 #define USE_MAG
+
+#ifdef MICOAIR743_EXTMAG
+// External compass
+#define MAG_I2C_BUS             BUS_I2C2
+#else
+// Onboard compass
 #define MAG_I2C_BUS             BUS_I2C2
+#endif
 #define USE_MAG_ALL
 
 // *************** ENABLE OPTICAL FLOW & RANGEFINDER *****************************

From 54adf5002f4358da438c8be2969054bfd965b1db Mon Sep 17 00:00:00 2001
From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com>
Date: Thu, 26 Sep 2024 11:53:36 +0200
Subject: [PATCH 37/39] Missed change

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

diff --git a/src/main/target/MICOAIR743/target.h b/src/main/target/MICOAIR743/target.h
index 9326d6622b9..526cf438d08 100644
--- a/src/main/target/MICOAIR743/target.h
+++ b/src/main/target/MICOAIR743/target.h
@@ -103,7 +103,7 @@
 
 #ifdef MICOAIR743_EXTMAG
 // External compass
-#define MAG_I2C_BUS             BUS_I2C2
+#define MAG_I2C_BUS             BUS_I2C1
 #else
 // Onboard compass
 #define MAG_I2C_BUS             BUS_I2C2

From cd614098584d57b607b0cb280713f774d53a3ae7 Mon Sep 17 00:00:00 2001
From: Mr D - RC <darren@darrenlines.uk>
Date: Fri, 27 Sep 2024 19:35:31 +0100
Subject: [PATCH 38/39] Switch parameters with battery capacity unit

There is a parameter for the OSD called `osd_stats_energy_unit`. This selects whether mAh or Wh are displayed on the end of flight stats screen. People are confused the when the switch the battery capacity unit to Wh from Ah, this value doesn't change.

This PR will change `osd_stats_energy_unit` to match the battery capacity unit if it changes. This will be more an expected behavior. Plus it gives pilots the option to change to the other units for the stats if they want.
---
 src/main/fc/cli.c    | 11 +++++++++++
 src/main/fc/fc_msp.c | 14 ++++++++++++++
 2 files changed, 25 insertions(+)

diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index ee5cb289a94..0a54e489139 100644
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -3714,6 +3714,17 @@ static void cliSet(char *cmdline)
                 }
 
                 if (changeValue) {
+                    // If changing the battery capacity unit, update the osd stats energy unit to match
+                    if (strcmp(name, "battery_capacity_unit") == 0) {
+                        if (batteryMetersConfig()->capacity_unit != tmp.int_value) {
+                            if (tmp.int_value == BAT_CAPACITY_UNIT_MAH) {
+                                osdConfigMutable()->stats_energy_unit = OSD_STATS_ENERGY_UNIT_MAH;
+                            } else {
+                                osdConfigMutable()->stats_energy_unit = OSD_STATS_ENERGY_UNIT_WH;
+                            }
+                        }
+                    }
+
                     cliSetIntFloatVar(val, tmp);
 
                     cliPrintf("%s set to ", name);
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index f82f3cbaeaf..76baa4d41eb 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -2082,6 +2082,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             currentBatteryProfileMutable->capacity.value = sbufReadU32(src);
             currentBatteryProfileMutable->capacity.warning = sbufReadU32(src);
             currentBatteryProfileMutable->capacity.critical = sbufReadU32(src);
+            uint8_t currentCapacityUnit = batteryMetersConfigMutable()->capacity_unit;
             batteryMetersConfigMutable()->capacity_unit = sbufReadU8(src);
             if ((batteryMetersConfig()->voltageSource != BAT_VOLTAGE_RAW) && (batteryMetersConfig()->voltageSource != BAT_VOLTAGE_SAG_COMP)) {
                 batteryMetersConfigMutable()->voltageSource = BAT_VOLTAGE_RAW;
@@ -2090,6 +2091,12 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             if ((batteryMetersConfig()->capacity_unit != BAT_CAPACITY_UNIT_MAH) && (batteryMetersConfig()->capacity_unit != BAT_CAPACITY_UNIT_MWH)) {
                 batteryMetersConfigMutable()->capacity_unit = BAT_CAPACITY_UNIT_MAH;
                 return MSP_RESULT_ERROR;
+            } else if (currentCapacityUnit != batteryMetersConfig()->capacity_unit) {
+                if (batteryMetersConfig()->capacity_unit == BAT_CAPACITY_UNIT_MAH) {
+                    osdConfigMutable()->stats_energy_unit = OSD_STATS_ENERGY_UNIT_MAH;
+                } else {
+                    osdConfigMutable()->stats_energy_unit = OSD_STATS_ENERGY_UNIT_WH;
+                }
             }
         } else
             return MSP_RESULT_ERROR;
@@ -2121,6 +2128,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             currentBatteryProfileMutable->capacity.value = sbufReadU32(src);
             currentBatteryProfileMutable->capacity.warning = sbufReadU32(src);
             currentBatteryProfileMutable->capacity.critical = sbufReadU32(src);
+            uint8_t currentCapacityUnit = batteryMetersConfigMutable()->capacity_unit;
             batteryMetersConfigMutable()->capacity_unit = sbufReadU8(src);
             if ((batteryMetersConfig()->voltageSource != BAT_VOLTAGE_RAW) && (batteryMetersConfig()->voltageSource != BAT_VOLTAGE_SAG_COMP)) {
                 batteryMetersConfigMutable()->voltageSource = BAT_VOLTAGE_RAW;
@@ -2129,6 +2137,12 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             if ((batteryMetersConfig()->capacity_unit != BAT_CAPACITY_UNIT_MAH) && (batteryMetersConfig()->capacity_unit != BAT_CAPACITY_UNIT_MWH)) {
                 batteryMetersConfigMutable()->capacity_unit = BAT_CAPACITY_UNIT_MAH;
                 return MSP_RESULT_ERROR;
+            } else if (currentCapacityUnit != batteryMetersConfig()->capacity_unit) {
+                if (batteryMetersConfig()->capacity_unit == BAT_CAPACITY_UNIT_MAH) {
+                    osdConfigMutable()->stats_energy_unit = OSD_STATS_ENERGY_UNIT_MAH;
+                } else {
+                    osdConfigMutable()->stats_energy_unit = OSD_STATS_ENERGY_UNIT_WH;
+                }
             }
         } else
             return MSP_RESULT_ERROR;

From 5b3b60109d7a4b4e0d5665a3a7ade6e0e3b12188 Mon Sep 17 00:00:00 2001
From: Mr D - RC <darren@darrenlines.uk>
Date: Fri, 27 Sep 2024 20:03:38 +0100
Subject: [PATCH 39/39] Fix for SITL compiles

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

diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index 0a54e489139..5aed69c1d13 100644
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -3716,7 +3716,7 @@ static void cliSet(char *cmdline)
                 if (changeValue) {
                     // If changing the battery capacity unit, update the osd stats energy unit to match
                     if (strcmp(name, "battery_capacity_unit") == 0) {
-                        if (batteryMetersConfig()->capacity_unit != tmp.int_value) {
+                        if (batteryMetersConfig()->capacity_unit != (uint8_t)tmp.int_value) {
                             if (tmp.int_value == BAT_CAPACITY_UNIT_MAH) {
                                 osdConfigMutable()->stats_energy_unit = OSD_STATS_ENERGY_UNIT_MAH;
                             } else {