From 8422640199ddeb6262e2f581f797d3d2b7566214 Mon Sep 17 00:00:00 2001
From: Bryan Mayland <bmayland@capnbry.net>
Date: Wed, 31 Aug 2022 07:38:27 -0400
Subject: [PATCH] feat: Optionally support CRSF_VARIO in BARO_ALT telemetry

---
 radio/src/telemetry/crossfire.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/radio/src/telemetry/crossfire.cpp b/radio/src/telemetry/crossfire.cpp
index 0a81e6d23..f51ce1b49 100644
--- a/radio/src/telemetry/crossfire.cpp
+++ b/radio/src/telemetry/crossfire.cpp
@@ -120,6 +120,7 @@ void processCrossfireTelemetryFrame(uint8_t module)
     moduleState[module].counter = CRSF_FRAME_MODELID;
   }
 
+  uint8_t crsfPayloadLen = rxBuffer[1];
   uint8_t id = rxBuffer[2];
   int32_t value;
   switch(id) {
@@ -156,6 +157,10 @@ void processCrossfireTelemetryFrame(uint8_t module)
         }
         processCrossfireTelemetryValue(BARO_ALTITUDE_INDEX, value);
       }
+      // Length of TBS BARO_ALT has 4 payload bytes with just 2 bytes of altitude
+      // but support including VARIO if the declared payload length is 6 bytes or more
+      if (crsfPayloadLen > 5 && getCrossfireTelemetryValue<2>(5, value, module))
+        processCrossfireTelemetryValue(VERTICAL_SPEED_INDEX, value);
       break;
 
     case LINK_ID: