Skip to content

Fix CRSF 0x09 baro/vario unpack and duplicate airspeed function #11572

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
sensei-hacker merged 2 commits into from
May 19, 2026
+17 −22
Merged

Fix CRSF 0x09 baro/vario unpack and duplicate airspeed function #11572

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
afa43b0
Fix CRSF 0x09 baro/vario frame unpack in sensor input
sensei-hacker May 19, 2026
29a763a
Fix duplicate crsfFrameAirSpeedSensor definition in telemetry/crsf.c
sensei-hacker May 19, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
24 changes: 15 additions & 9 deletions src/main/io/crsf_sensor.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down
15 changes: 2 additions & 13 deletions src/main/telemetry/crsf.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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;
Expand Down
Loading