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

docs/Settings.md

@@ -645,6 +645,18 @@ Blackbox logging rate numerator. Use num/denom settings to decide if a frame sho
 
 ---
 
+### crsf_gps_alt_source
+
+CRSF telemetry: Altitude source for the GPS frame (GAlt sensor on EdgeTX/OpenTX radios). AUTO follows crsf_use_legacy_baro_packet (legacy packet ON = estimated altitude above the arming point, OFF = GNSS altitude above mean sea level as intended by the CRSF specification), ESTIMATED and MSL force one source regardless of the baro packet format. [AUTO/ESTIMATED/MSL]
+
+| Allowed Values |  |
+| --- | --- |
+| AUTO | Default |
+| ESTIMATED |  |
+| MSL |  |
+
+---
+
 ### crsf_use_legacy_baro_packet
 
 CRSF telemetry: If `ON`, send altitude about start point in GPS telemetry packet. If `OFF`, GPS has ASL altitude, altitude about start point in separate packet. Default: 'OFF'

src/main/fc/settings.yaml

@@ -212,6 +212,9 @@ tables:
   - name: mavlink_autopilot_type
     values: ["GENERIC", "ARDUPILOT"]
     enum: mavlinkAutopilotType_e
+  - name: crsf_gps_alt_source
+    values: ["AUTO", "ESTIMATED", "MSL"]
+    enum: crsfGpsAltSource_e
   - name: default_altitude_source
     values: ["GPS", "BARO", "GPS_ONLY", "BARO_ONLY"]
     enum: navDefaultAltitudeSensor_e
@@ -3313,6 +3316,12 @@ groups:
         field: ltmUpdateRate
         condition: USE_TELEMETRY_LTM
         table: ltm_rates
+      - name: crsf_gps_alt_source
+        description: "CRSF telemetry: Altitude source for the GPS frame (GAlt sensor on EdgeTX/OpenTX radios). AUTO follows crsf_use_legacy_baro_packet (legacy packet ON = estimated altitude above the arming point, OFF = GNSS altitude above mean sea level as intended by the CRSF specification), ESTIMATED and MSL force one source regardless of the baro packet format. [AUTO/ESTIMATED/MSL]"
+        default_value: "AUTO"
+        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

@@ -241,7 +241,13 @@ 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
-    crsfSerialize16(dst, (uint16_t)( (telemetryConfig()->crsf_use_legacy_baro_packet ? getEstimatedActualPosition(Z) : gpsSol.llh.alt ) / 100 + 1000) );
+    // The GPS frame's altitude: AUTO follows crsf_use_legacy_baro_packet (legacy packet ON
+    // sends the estimated altitude above the arming point, OFF the GNSS altitude above mean
+    // sea level as the CRSF spec intends); ESTIMATED and MSL force one source regardless of
+    // the baro packet format.
+    const bool sendEstimatedAltitude = telemetryConfig()->crsfGpsAltSource == CRSF_GPS_ALT_ESTIMATED ||
+        (telemetryConfig()->crsfGpsAltSource == CRSF_GPS_ALT_AUTO && telemetryConfig()->crsf_use_legacy_baro_packet);
+    crsfSerialize16(dst, (uint16_t)( (sendEstimatedAltitude ? getEstimatedActualPosition(Z) : gpsSol.llh.alt ) / 100 + 1000) );
     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, 11);
+PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 12);
 
 PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig,
     .telemetry_switch = SETTING_TELEMETRY_SWITCH_DEFAULT,
@@ -72,6 +72,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

@@ -75,6 +75,12 @@ typedef struct mavlinkTelemetryPortConfig_s {
     bool high_latency;
 } mavlinkTelemetryPortConfig_t;
 
+typedef enum {
+    CRSF_GPS_ALT_AUTO,          // Follow crsf_use_legacy_baro_packet
+    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.
@@ -100,6 +106,7 @@ typedef struct telemetryConfig_s {
     mavlinkTelemetryCommonConfig_t mavlink_common;
     mavlinkTelemetryPortConfig_t mavlink[MAX_MAVLINK_PORTS];
     bool crsf_use_legacy_baro_packet;
+    uint8_t crsfGpsAltSource;               // crsfGpsAltSource_e
 } telemetryConfig_t;
 
 PG_DECLARE(telemetryConfig_t, telemetryConfig);