From c31b54041b50d6c7e8320bd2d6bc0f76ac916abb Mon Sep 17 00:00:00 2001 From: 3djc Date: Tue, 1 Jul 2025 04:53:42 +0200 Subject: [PATCH] feat(telem): add support for CRSF 0x09 packed vertical speed (#6402) --- radio/src/telemetry/crossfire.cpp | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/radio/src/telemetry/crossfire.cpp b/radio/src/telemetry/crossfire.cpp index 4ba1fd784..dd516ab99 100644 --- a/radio/src/telemetry/crossfire.cpp +++ b/radio/src/telemetry/crossfire.cpp @@ -21,6 +21,7 @@ #include "crossfire.h" #include "edgetx.h" +#include "math.h" #include "trainer.h" @@ -180,8 +181,21 @@ void processCrossfireTelemetryFrame(uint8_t module, uint8_t* rxBuffer, } 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 + // but support including TBS VARIO if the declared payload length is 5 bytes + if (crsfPayloadLen == 5 && + getCrossfireTelemetryValue<1>(5, value, rxBuffer)) { + constexpr int Kl = 100; // linearity constant; + constexpr float Kr = .026; // range constant; + + int8_t sign = value < 0 ? -1 : 1; + value =((expf(value * sign * Kr) - 1) * Kl) * sign; + processCrossfireTelemetryValue(VERTICAL_SPEED_INDEX, value); + } + + // Length of TBS BARO_ALT has 4 payload bytes with just 2 bytes of altitude + // but support including ELRS VARIO if the declared payload length is 6 bytes or more if (crsfPayloadLen > 5 && getCrossfireTelemetryValue<2>(5, value, rxBuffer)) processCrossfireTelemetryValue(VERTICAL_SPEED_INDEX, value);