CRSF Baro Altitude and Vario, AirSpeed

docs/Settings.md

@@ -592,6 +592,16 @@ Control rate profile to switch to when the battery profile is selected, 0 to dis
 
 ---
 
+### 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'
+
+| Default | Min | Max |
+| --- | --- | --- |
+| OFF | OFF | ON |
+
+---
+
 ### cruise_power
 
 Power draw at cruise throttle used for remaining flight time/distance estimation in 0.01W unit

src/main/fc/settings.yaml

@@ -3285,6 +3285,10 @@ groups:
         min: 1
         max: 255
         default_value: 1
+      - name: crsf_use_legacy_baro_packet
+        description: "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'"
+        default_value: OFF
+        type: bool
 
   - name: PG_OSD_CONFIG
     type: osdConfig_t

src/main/rx/crsf.h

@@ -46,6 +46,8 @@ enum {
     CRSF_FRAME_VARIO_SENSOR_PAYLOAD_SIZE = 2,
     CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE = 8,
     CRSF_FRAME_BAROMETER_ALTITUDE_PAYLOAD_SIZE = 2,
+    CRSF_FRAME_BAROMETER_ALTITUDE_VARIO_PAYLOAD_SIZE = 3,
+    CRSF_FRAME_AIRSPEED_PAYLOAD_SIZE = 2,
     CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE = 10,
     CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE = 22, // 11 bits per channel * 16 channels = 22 bytes.
     CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE = 6,
@@ -87,7 +89,8 @@ typedef enum {
     CRSF_FRAMETYPE_GPS = 0x02,
     CRSF_FRAMETYPE_VARIO_SENSOR = 0x07,
     CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08,
-    CRSF_FRAMETYPE_BAROMETER_ALTITUDE = 0x09,
+    CRSF_FRAMETYPE_BAROMETER_ALTITUDE_VARIO_SENSOR = 0x09,
+    CRSF_FRAMETYPE_AIRSPEED_SENSOR = 0x0A,
     CRSF_FRAMETYPE_LINK_STATISTICS = 0x14,
     CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16,
     CRSF_FRAMETYPE_ATTITUDE = 0x1E,

src/main/telemetry/crsf.c

@@ -18,7 +18,7 @@
 #include <stdbool.h>
 #include <stdint.h>
 #include <string.h>
-
+#include <limits.h>
 #include "platform.h"
 
 #if defined(USE_TELEMETRY) && defined(USE_SERIALRX_CRSF) && defined(USE_TELEMETRY_CRSF)
@@ -57,6 +57,7 @@
 
 #include "sensors/battery.h"
 #include "sensors/sensors.h"
+#include "sensors/pitotmeter.h"
 
 #include "telemetry/crsf.h"
 #include "telemetry/telemetry.h"
@@ -225,8 +226,7 @@ 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;
-    crsfSerialize16(dst, altitude);
+    crsfSerialize16(dst, (uint16_t)( (telemetryConfig()->crsf_use_legacy_baro_packet ? getEstimatedActualPosition(Z) : gpsSol.llh.alt ) / 100 + 1000) );
     crsfSerialize8(dst, gpsSol.numSat);
 }
 
@@ -269,16 +269,20 @@ static void crsfFrameBatterySensor(sbuf_t *dst)
     crsfSerialize8(dst, batteryRemainingPercentage);
 }
 
-const int32_t ALT_MIN_DM = 10000;
-const int32_t ALT_THRESHOLD_DM = 0x8000 - ALT_MIN_DM;
-const int32_t ALT_MAX_DM = 0x7ffe * 10 - 5;
 
 /*
 0x09 Barometer altitude and vertical speed
 Payload:
 uint16_t    altitude_packed ( dm - 10000 )
 */
-static void crsfBarometerAltitude(sbuf_t *dst)
+
+const int32_t ALT_MIN_DM = 10000;
+const int32_t ALT_THRESHOLD_DM = 0x8000 - ALT_MIN_DM;
+const int32_t ALT_MAX_DM = 0x7ffe * 10 - 5;
+const float VARIO_KL = 100.0f;   // TBS CRSF standard
+const float VARIO_KR = .026f;    // TBS CRSF standard
+
+static void crsfFrameBarometerAltitudeVarioSensor(sbuf_t *dst)
 {
     int32_t altitude_dm = lrintf(getEstimatedActualPosition(Z) / 10);
     uint16_t altitude_packed;
@@ -291,9 +295,15 @@ static void crsfBarometerAltitude(sbuf_t *dst)
     } else {
         altitude_packed = ((altitude_dm + 5) / 10) | 0x8000;
     }
-    sbufWriteU8(dst, CRSF_FRAME_BAROMETER_ALTITUDE_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
-    crsfSerialize8(dst, CRSF_FRAMETYPE_BAROMETER_ALTITUDE);
+
+    float vario_sm = getEstimatedActualVelocity(Z);
+    int8_t sign = vario_sm < 0 ? -1 : ( vario_sm > 0 ? 1 : 0 );
+    int8_t vario_packed = (int8_t)constrain( lrintf(__builtin_logf(ABS(vario_sm) / VARIO_KL + 1) / VARIO_KR * sign ), SCHAR_MIN, SCHAR_MAX );
+
+    sbufWriteU8(dst, CRSF_FRAME_BAROMETER_ALTITUDE_VARIO_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
+    crsfSerialize8(dst, CRSF_FRAMETYPE_BAROMETER_ALTITUDE_VARIO_SENSOR);
     crsfSerialize16(dst, altitude_packed);
+    crsfSerialize8(dst, vario_packed);
 }
 
 typedef enum {
@@ -463,8 +473,7 @@ typedef enum {
     CRSF_FRAME_BATTERY_SENSOR_INDEX,
     CRSF_FRAME_FLIGHT_MODE_INDEX,
     CRSF_FRAME_GPS_INDEX,
-    CRSF_FRAME_VARIO_SENSOR_INDEX,
-    CRSF_FRAME_BAROMETER_ALTITUDE_INDEX,
+    CRSF_FRAME_VARIO_OR_ALT_VARIO_SENSOR_INDEX,
     CRSF_SCHEDULE_COUNT_MAX
 } crsfFrameTypeIndex_e;
 
@@ -534,14 +543,9 @@ static void processCrsf(void)
     }
 #endif
 #if defined(USE_BARO) || defined(USE_GPS)
-    if (currentSchedule & BV(CRSF_FRAME_VARIO_SENSOR_INDEX)) {
-        crsfInitializeFrame(dst);
-        crsfFrameVarioSensor(dst);
-        crsfFinalize(dst);
-    }
-    if (currentSchedule & BV(CRSF_FRAME_BAROMETER_ALTITUDE_INDEX)) {
+    if (currentSchedule & BV(CRSF_FRAME_VARIO_OR_ALT_VARIO_SENSOR_INDEX) ) {
         crsfInitializeFrame(dst);
-        crsfBarometerAltitude(dst);
+        telemetryConfig()->crsf_use_legacy_baro_packet ? crsfFrameVarioSensor(dst) : crsfFrameBarometerAltitudeVarioSensor(dst);
         crsfFinalize(dst);
     }
 #endif
@@ -575,12 +579,7 @@ void initCrsfTelemetry(void)
 #endif
 #if defined(USE_BARO) || defined(USE_GPS)
     if (sensors(SENSOR_BARO) || (STATE(FIXED_WING_LEGACY) && feature(FEATURE_GPS))) {
-        crsfSchedule[index++] = BV(CRSF_FRAME_VARIO_SENSOR_INDEX);
-    }
-#endif
-#ifdef USE_BARO
-    if (sensors(SENSOR_BARO)) {
-        crsfSchedule[index++] = BV(CRSF_FRAME_BAROMETER_ALTITUDE_INDEX);
+        crsfSchedule[index++] = BV(CRSF_FRAME_VARIO_OR_ALT_VARIO_SENSOR_INDEX);
     }
 #endif
     crsfScheduleCount = (uint8_t)index;
@@ -659,7 +658,10 @@ int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType)
     case CRSF_FRAMETYPE_VARIO_SENSOR:
         crsfFrameVarioSensor(sbuf);
         break;
-    }
+    case CRSF_FRAMETYPE_BAROMETER_ALTITUDE_VARIO_SENSOR:
+        crsfFrameBarometerAltitudeVarioSensor(sbuf);
+        break;
+    }  
     const int frameSize = crsfFinalizeBuf(sbuf, frame);
     return frameSize;
 }

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,
@@ -98,7 +98,8 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig,
         .min_txbuff = SETTING_MAVLINK_MIN_TXBUFFER_DEFAULT,
         .radio_type = SETTING_MAVLINK_RADIO_TYPE_DEFAULT,
         .sysid = SETTING_MAVLINK_SYSID_DEFAULT
-    }
+    },
+    .crsf_use_legacy_baro_packet = SETTING_CRSF_USE_LEGACY_BARO_PACKET_DEFAULT
 );
 
 void telemetryInit(void)

src/main/telemetry/telemetry.h

@@ -89,6 +89,7 @@ typedef struct telemetryConfig_s {
         uint8_t radio_type;
         uint8_t sysid;
     } mavlink;
+    bool crsf_use_legacy_baro_packet;
 } telemetryConfig_t;
 
 PG_DECLARE(telemetryConfig_t, telemetryConfig);