feat: Optionally support CRSF_VARIO in BARO_ALT telemetry

EdgeTX · Aug 31, 2022 · 8422640 · 8422640
1 parent e576925
commit 8422640
Showing 1 changed file with 5 additions and 0 deletions.
diff --git 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: