From afa43b0681279f2ff76e98f6134ed9624e551ace Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Tue, 19 May 2026 11:26:39 -0500 Subject: [PATCH 1/2] Fix CRSF 0x09 baro/vario frame unpack in sensor input - Rename CRSF_FRAMETYPE_BAROMETER_ALTITUDE to CRSF_FRAMETYPE_BAROMETER_ALTITUDE_VARIO_SENSOR and CRSF_FRAME_BAROMETER_ALTITUDE_PAYLOAD_SIZE to CRSF_FRAME_BAROMETER_ALTITUDE_VARIO_PAYLOAD_SIZE, matching current crsf.h definitions after the combined frame was introduced - Fix altitude MSB=1 decode: remove spurious -5dm offset; per TBS spec get_altitude_dm() is (packed & 0x7fff) * 10 with no offset - Add vertical speed decode from payload[2] using the log formula specified by TBS: (exp(|packed| * 0.026) - 1) * 100 * sign - Mark crsfSensorVario and crsfSensorVarioLastUpdateMs volatile, consistent with other cross-task shared variables in this file --- src/main/io/crsf_sensor.c | 24 +++++++++++++++--------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/src/main/io/crsf_sensor.c b/src/main/io/crsf_sensor.c index 3958dfc7f29..c140fef6320 100644 --- a/src/main/io/crsf_sensor.c +++ b/src/main/io/crsf_sensor.c @@ -72,8 +72,8 @@ static volatile timeUs_t crsfSensorFrameStartAtUs; static volatile bool crsfSensorFrameDone; // Vario data storage -static int16_t crsfSensorVario; -static timeMs_t crsfSensorVarioLastUpdateMs; +static volatile int16_t crsfSensorVario; +static volatile timeMs_t crsfSensorVarioLastUpdateMs; static uint8_t crsfSensorFrameCRC(const uint8_t *frame) { @@ -149,19 +149,25 @@ static void crsfSensorHandleGPS(const uint8_t *payload) #ifdef USE_BARO_CRSF static void crsfSensorHandleBaro(const uint8_t *payload) { - // Baro payload: altitude_packed (uint16_t big-endian) - // Format matches outgoing CRSF baro encoding: - // bit 15 clear: altitude_dm = packed - 10000 (fine, dm resolution) - // bit 15 set: altitude_dm = (packed & 0x7fff) * 10 - 5 (coarse, meter resolution) + // Altitude: uint16_t big-endian, bit 15 selects resolution + // MSB=0: altitude_dm = packed - 10000 (dm resolution, ±1000m range) + // MSB=1: altitude_dm = (packed & 0x7fff) * 10 (meter resolution, 0-32766m range) uint16_t packed = crsfSensorReadU16(payload); int32_t altitude_dm; if (packed & 0x8000) { - altitude_dm = (int32_t)(packed & 0x7fff) * 10 - 5; + altitude_dm = (int32_t)(packed & 0x7fff) * 10; } else { altitude_dm = (int32_t)packed - 10000; } + // Vertical speed: int8_t log-encoded, Kr=0.026 Kl=100 + // Unpack: (exp(|packed| * Kr) - 1) * Kl * sign + int8_t vario_packed = (int8_t)payload[2]; + float vario_abs = (expf(fabsf((float)vario_packed * 0.026f)) - 1.0f) * 100.0f; + crsfSensorVario = (int16_t)(vario_packed < 0 ? -vario_abs : vario_abs); + crsfSensorVarioLastUpdateMs = millis(); + // Convert altitude (dm) to pressure using ISA formula: // P = 101325 * (1 - h/44330)^5.255 float altitude_m = altitude_dm / 10.0f; @@ -231,8 +237,8 @@ static void crsfSensorDispatchFrame(const uint8_t *frame) #endif #ifdef USE_BARO_CRSF - case CRSF_FRAMETYPE_BAROMETER_ALTITUDE: - if (frameLength >= CRSF_FRAME_BAROMETER_ALTITUDE_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC) { + case CRSF_FRAMETYPE_BAROMETER_ALTITUDE_VARIO_SENSOR: + if (frameLength >= CRSF_FRAME_BAROMETER_ALTITUDE_VARIO_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC) { crsfSensorHandleBaro(payload); } break; From 29a763a86d726ef6e137f62a973086f61c835de6 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Tue, 19 May 2026 11:26:46 -0500 Subject: [PATCH 2/2] Fix duplicate crsfFrameAirSpeedSensor definition in telemetry/crsf.c The commit adding combined baro+vario/airspeed support introduced a USE_PITOT-guarded definition of crsfFrameAirSpeedSensor but did not remove the original unconditional definition, causing a redefinition error. The original also had an integer division bug (36/100 == 0). Remove the unconditional definition and guard the getCrsfFrame() dispatch case with USE_PITOT to match the function's availability. --- src/main/telemetry/crsf.c | 15 ++------------- 1 file changed, 2 insertions(+), 13 deletions(-) diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 6e7588a3b2a..16392362830 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -330,19 +330,6 @@ static void crsfFrameBarometerAltitudeVarioSensor(sbuf_t *dst) crsfSerialize8(dst, vario_packed); } -/* -0x0A Airspeed sensor -Payload: -int16 Air speed ( dm/s ) -*/ -static void crsfFrameAirSpeedSensor(sbuf_t *dst) -{ - // use sbufWrite since CRC does not include frame length - sbufWriteU8(dst, CRSF_FRAME_AIRSPEED_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); - crsfSerialize8(dst, CRSF_FRAMETYPE_AIRSPEED_SENSOR); - crsfSerialize16(dst, (uint16_t)(getAirspeedEstimate() * 36 / 100)); -} - #ifdef USE_PITOT /* 0x0A Airspeed sensor @@ -848,9 +835,11 @@ int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType) case CRSF_FRAMETYPE_BAROMETER_ALTITUDE_VARIO_SENSOR: crsfFrameBarometerAltitudeVarioSensor(sbuf); break; +#ifdef USE_PITOT case CRSF_FRAMETYPE_AIRSPEED_SENSOR: crsfFrameAirSpeedSensor(sbuf); break; +#endif } const int frameSize = crsfFinalizeBuf(sbuf, frame); return frameSize;