CRSF telemetry: selectable altitude source for the GPS frame (ESTIMATED/MSL)

docs/Settings.md

@@ -592,6 +592,16 @@ Blackbox logging rate numerator. Use num/denom settings to decide if a frame sho
 
 ---
 
+### crsf_gps_alt_source
+
+CRSF telemetry only: Altitude source for the GPS frame (GAlt sensor on EdgeTX/OpenTX radios). ESTIMATED sends INAV's estimated altitude relative to the arming point (legacy behaviour), MSL sends the GNSS altitude above mean sea level as intended by the CRSF specification. [ESTIMATED/MSL]
+
+| Default | Min | Max |
+| --- | --- | --- |
+| ESTIMATED |  |  |
+
+---
+
 ### cruise_power
 
 Power draw at cruise throttle used for remaining flight time/distance estimation in 0.01W unit

src/main/fc/settings.yaml

@@ -204,6 +204,9 @@ tables:
   - name: mavlink_autopilot_type
     values: ["GENERIC", "ARDUPILOT"]
     enum: mavlinkAutopilotType_e
+  - name: crsf_gps_alt_source
+    values: ["ESTIMATED", "MSL"]
+    enum: crsfGpsAltSource_e
   - name: default_altitude_source
     values: ["GPS", "BARO", "GPS_ONLY", "BARO_ONLY"]
     enum: navDefaultAltitudeSensor_e
@@ -3168,6 +3171,12 @@ groups:
         field: ltmUpdateRate
         condition: USE_TELEMETRY_LTM
         table: ltm_rates
+      - name: crsf_gps_alt_source
+        description: "CRSF telemetry only: Altitude source for the GPS frame (GAlt sensor on EdgeTX/OpenTX radios). ESTIMATED sends INAV's estimated altitude relative to the arming point (legacy behaviour), MSL sends the GNSS altitude above mean sea level as intended by the CRSF specification. [ESTIMATED/MSL]"
+        default_value: "ESTIMATED"
+        field: crsfGpsAltSource
+        table: crsf_gps_alt_source
+        type: uint8_t
       - name: sim_ground_station_number
         description: "Number of phone that is used to communicate with SIM module. Messages / calls from other numbers are ignored. If undefined, can be set by calling or sending a message to the module."
         default_value: ""

src/main/telemetry/crsf.c

@@ -239,7 +239,14 @@ static void crsfFrameGps(sbuf_t *dst)
     crsfSerialize32(dst, gpsSol.llh.lon);
     crsfSerialize16(dst, (gpsSol.groundSpeed * 36 + 50) / 100); // gpsSol.groundSpeed is in cm/s
     crsfSerialize16(dst, DECIDEGREES_TO_CENTIDEGREES(gpsSol.groundCourse)); // gpsSol.groundCourse is 0.1 degrees, need 0.01 deg
-    const uint16_t altitude = (getEstimatedActualPosition(Z) / 100) + 1000;
+    // Altitude source is selectable: the estimated altitude above the arming point (legacy
+    // behaviour) or the GNSS altitude above mean sea level, as the CRSF spec intends
+    uint16_t altitude;
+    if (telemetryConfig()->crsfGpsAltSource == CRSF_GPS_ALT_MSL) {
+        altitude = (gpsSol.llh.alt / 100) + 1000; // gpsSol.llh.alt is in cm
+    } else {
+        altitude = (getEstimatedActualPosition(Z) / 100) + 1000;
+    }
     crsfSerialize16(dst, altitude);
     crsfSerialize8(dst, gpsSol.numSat);
 }

src/main/telemetry/telemetry.c

@@ -56,7 +56,7 @@
 #include "telemetry/ghst.h"
 
 
-PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 8);
+PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 9);
 
 PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig,
     .telemetry_switch = SETTING_TELEMETRY_SWITCH_DEFAULT,
@@ -73,6 +73,7 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig,
 #endif
     .ibusTelemetryType = SETTING_IBUS_TELEMETRY_TYPE_DEFAULT,
     .ltmUpdateRate = SETTING_LTM_UPDATE_RATE_DEFAULT,
+    .crsfGpsAltSource = SETTING_CRSF_GPS_ALT_SOURCE_DEFAULT,
 
 #ifdef USE_TELEMETRY_SIM
     .simTransmitInterval = SETTING_SIM_TRANSMIT_INTERVAL_DEFAULT,

src/main/telemetry/telemetry.h

@@ -53,6 +53,11 @@ typedef enum {
     SMARTPORT_FUEL_UNIT_MWH
 } smartportFuelUnit_e;
 
+typedef enum {
+    CRSF_GPS_ALT_ESTIMATED,     // Estimated altitude above the arming point (legacy behaviour)
+    CRSF_GPS_ALT_MSL            // GNSS altitude above mean sea level
+} crsfGpsAltSource_e;
+
 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.
@@ -64,6 +69,7 @@ typedef struct telemetryConfig_s {
     smartportFuelUnit_e smartportFuelUnit;
     uint8_t ibusTelemetryType;
     uint8_t ltmUpdateRate;
+    uint8_t crsfGpsAltSource;
 
 #ifdef USE_TELEMETRY_SIM
     int16_t simLowAltitude;