From 755e978b088f5a7b599ab741c8b687e9d46a6087 Mon Sep 17 00:00:00 2001 From: Roman Sorokin Date: Mon, 3 Nov 2025 20:39:07 +0300 Subject: [PATCH 1/3] Baro Altitude and Vario, AirSpeed --- docs/Settings.md | 10 +++++ src/main/fc/settings.yaml | 4 ++ src/main/rx/crsf.h | 5 ++- src/main/telemetry/crsf.c | 71 ++++++++++++++++++++++++---------- src/main/telemetry/telemetry.c | 5 ++- src/main/telemetry/telemetry.h | 1 + 6 files changed, 73 insertions(+), 23 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 0647e1c206b..9a92103ff9f 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -592,6 +592,16 @@ Control rate profile to switch to when the battery profile is selected, 0 to dis --- +### crsf_use_legacy_baro_packet + +CRSF telemetry: If `ON`, send altitude about start point in GPS telemetry packet. No vario, no If `OFF`, BaroVario packet will have vario speed and altitude about start point and GPS packet will have ASL altitude (about sea level). These are deprecated, and will be removed in INAV 11.0. Tools and scripts using these GPS Altitude should be updated to use the BaroVario packet and GPS packet. Default: 'OFF' + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + ### cruise_power Power draw at cruise throttle used for remaining flight time/distance estimation in 0.01W unit diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 2900875a235..9c32fc58997 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3261,6 +3261,10 @@ groups: table: mavlink_radio_type default_value: "GENERIC" type: uint8_t + - name: crsf_use_legacy_baro_packet + description: "CRSF telemetry: If `ON`, send altitude about start point in GPS telemetry packet. If `OFF`, GPS has ASL altitude, altitude about start point in separate packet. Default: 'OFF'" + default_value: OFF + type: bool - name: PG_OSD_CONFIG type: osdConfig_t diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h index c5136e3b7ae..e7318b179aa 100755 --- a/src/main/rx/crsf.h +++ b/src/main/rx/crsf.h @@ -46,6 +46,8 @@ enum { CRSF_FRAME_VARIO_SENSOR_PAYLOAD_SIZE = 2, CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE = 8, CRSF_FRAME_BAROMETER_ALTITUDE_PAYLOAD_SIZE = 2, + CRSF_FRAME_BAROMETER_ALTITUDE_VARIO_PAYLOAD_SIZE = 3, + CRSF_FRAME_AIRSPEED_PAYLOAD_SIZE = 2, CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE = 10, CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE = 22, // 11 bits per channel * 16 channels = 22 bytes. CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE = 6, @@ -87,7 +89,8 @@ typedef enum { CRSF_FRAMETYPE_GPS = 0x02, CRSF_FRAMETYPE_VARIO_SENSOR = 0x07, CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08, - CRSF_FRAMETYPE_BAROMETER_ALTITUDE = 0x09, + CRSF_FRAMETYPE_BAROMETER_ALTITUDE_VARIO_SENSOR = 0x09, + CRSF_FRAMETYPE_AIRSPEED_SENSOR = 0x0A, CRSF_FRAMETYPE_LINK_STATISTICS = 0x14, CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16, CRSF_FRAMETYPE_ATTITUDE = 0x1E, diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 2c078e1c01d..7ee217be895 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -18,7 +18,7 @@ #include #include #include - +#include #include "platform.h" #if defined(USE_TELEMETRY) && defined(USE_SERIALRX_CRSF) && defined(USE_TELEMETRY_CRSF) @@ -57,6 +57,7 @@ #include "sensors/battery.h" #include "sensors/sensors.h" +#include "sensors/pitotmeter.h" #include "telemetry/crsf.h" #include "telemetry/telemetry.h" @@ -214,8 +215,7 @@ static void crsfFrameGps(sbuf_t *dst) crsfSerialize32(dst, gpsSol.llh.lon); crsfSerialize16(dst, (gpsSol.groundSpeed * 36 + 50) / 100); // gpsSol.groundSpeed is in cm/s crsfSerialize16(dst, DECIDEGREES_TO_CENTIDEGREES(gpsSol.groundCourse)); // gpsSol.groundCourse is 0.1 degrees, need 0.01 deg - const uint16_t altitude = (getEstimatedActualPosition(Z) / 100) + 1000; - crsfSerialize16(dst, altitude); + crsfSerialize16(dst, (uint16_t)( (telemetryConfig()->crsf_use_legacy_baro_packet ? getEstimatedActualPosition(Z) : gpsSol.llh.alt ) / 100 + 1000) ); crsfSerialize8(dst, gpsSol.numSat); } @@ -258,16 +258,20 @@ static void crsfFrameBatterySensor(sbuf_t *dst) crsfSerialize8(dst, batteryRemainingPercentage); } -const int32_t ALT_MIN_DM = 10000; -const int32_t ALT_THRESHOLD_DM = 0x8000 - ALT_MIN_DM; -const int32_t ALT_MAX_DM = 0x7ffe * 10 - 5; /* 0x09 Barometer altitude and vertical speed Payload: uint16_t altitude_packed ( dm - 10000 ) */ -static void crsfBarometerAltitude(sbuf_t *dst) + +const int32_t ALT_MIN_DM = 10000; +const int32_t ALT_THRESHOLD_DM = 0x8000 - ALT_MIN_DM; +const int32_t ALT_MAX_DM = 0x7ffe * 10 - 5; +const float VARIO_KL = 100.0f; // TBS CRSF standard +const float VARIO_KR = .026f; // TBS CRSF standard + +static void crsfFrameBarometerAltitudeVarioSensor(sbuf_t *dst) { int32_t altitude_dm = lrintf(getEstimatedActualPosition(Z) / 10); uint16_t altitude_packed; @@ -280,9 +284,28 @@ static void crsfBarometerAltitude(sbuf_t *dst) } else { altitude_packed = ((altitude_dm + 5) / 10) | 0x8000; } - sbufWriteU8(dst, CRSF_FRAME_BAROMETER_ALTITUDE_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); - crsfSerialize8(dst, CRSF_FRAMETYPE_BAROMETER_ALTITUDE); + + float vario_sm = getEstimatedActualVelocity(Z); + int8_t sign = vario_sm < 0 ? -1 : ( vario_sm > 0 ? 1 : 0 ); + int8_t vario_packed = (int8_t)constrain( lrintf(__builtin_logf(ABS(vario_sm) / VARIO_KL + 1) / VARIO_KR * sign ), SCHAR_MIN, SCHAR_MAX ); + + sbufWriteU8(dst, CRSF_FRAME_BAROMETER_ALTITUDE_VARIO_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); + crsfSerialize8(dst, CRSF_FRAMETYPE_BAROMETER_ALTITUDE_VARIO_SENSOR); crsfSerialize16(dst, altitude_packed); + 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)); } typedef enum { @@ -444,8 +467,8 @@ typedef enum { CRSF_FRAME_BATTERY_SENSOR_INDEX, CRSF_FRAME_FLIGHT_MODE_INDEX, CRSF_FRAME_GPS_INDEX, - CRSF_FRAME_VARIO_SENSOR_INDEX, - CRSF_FRAME_BAROMETER_ALTITUDE_INDEX, + CRSF_FRAME_VARIO_OR_ALT_VARIO_SENSOR_INDEX, + CRSF_FRAME_AIRSPEED_SENSOR_INDEX, CRSF_SCHEDULE_COUNT_MAX } crsfFrameTypeIndex_e; @@ -507,14 +530,16 @@ static void processCrsf(void) } #endif #if defined(USE_BARO) || defined(USE_GPS) - if (currentSchedule & BV(CRSF_FRAME_VARIO_SENSOR_INDEX)) { + if (currentSchedule & BV(CRSF_FRAME_VARIO_OR_ALT_VARIO_SENSOR_INDEX) ) { crsfInitializeFrame(dst); - crsfFrameVarioSensor(dst); + telemetryConfig()->crsf_use_legacy_baro_packet ? crsfFrameVarioSensor(dst) : crsfFrameBarometerAltitudeVarioSensor(dst); crsfFinalize(dst); } - if (currentSchedule & BV(CRSF_FRAME_BAROMETER_ALTITUDE_INDEX)) { +#endif +#if defined(USE_PITOT) + if (currentSchedule & BV(CRSF_FRAME_AIRSPEED_SENSOR_INDEX)) { crsfInitializeFrame(dst); - crsfBarometerAltitude(dst); + crsfFrameAirSpeedSensor(dst); crsfFinalize(dst); } #endif @@ -548,12 +573,12 @@ void initCrsfTelemetry(void) #endif #if defined(USE_BARO) || defined(USE_GPS) if (sensors(SENSOR_BARO) || (STATE(FIXED_WING_LEGACY) && feature(FEATURE_GPS))) { - crsfSchedule[index++] = BV(CRSF_FRAME_VARIO_SENSOR_INDEX); + crsfSchedule[index++] = BV(CRSF_FRAME_VARIO_OR_ALT_VARIO_SENSOR_INDEX); } #endif -#ifdef USE_BARO - if (sensors(SENSOR_BARO)) { - crsfSchedule[index++] = BV(CRSF_FRAME_BAROMETER_ALTITUDE_INDEX); +#ifdef USE_PITOT + if (sensors(SENSOR_PITOT)) { + crsfSchedule[index++] = BV(CRSF_FRAME_AIRSPEED_SENSOR_INDEX); } #endif crsfScheduleCount = (uint8_t)index; @@ -632,7 +657,13 @@ int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType) case CRSF_FRAMETYPE_VARIO_SENSOR: crsfFrameVarioSensor(sbuf); break; - } + case CRSF_FRAMETYPE_BAROMETER_ALTITUDE_VARIO_SENSOR: + crsfFrameBarometerAltitudeVarioSensor(sbuf); + break; + case CRSF_FRAMETYPE_AIRSPEED_SENSOR: + crsfFrameAirSpeedSensor(sbuf); + break; + } const int frameSize = crsfFinalizeBuf(sbuf, frame); return frameSize; } diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index e0820234e65..16313accaee 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -56,7 +56,7 @@ #include "telemetry/ghst.h" -PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 8); +PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 9); PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .telemetry_switch = SETTING_TELEMETRY_SWITCH_DEFAULT, @@ -96,7 +96,8 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .version = SETTING_MAVLINK_VERSION_DEFAULT, .min_txbuff = SETTING_MAVLINK_MIN_TXBUFFER_DEFAULT, .radio_type = SETTING_MAVLINK_RADIO_TYPE_DEFAULT - } + }, + .crsf_use_legacy_baro_packet = SETTING_CRSF_USE_LEGACY_BARO_PACKET_DEFAULT ); void telemetryInit(void) diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index c8808959277..cf170c0e644 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -82,6 +82,7 @@ typedef struct telemetryConfig_s { uint8_t min_txbuff; uint8_t radio_type; } mavlink; + bool crsf_use_legacy_baro_packet; } telemetryConfig_t; PG_DECLARE(telemetryConfig_t, telemetryConfig); From 4e44baa23225890ee8d53bf67a451cee03e1272b Mon Sep 17 00:00:00 2001 From: Roman Sorokin Date: Wed, 5 Nov 2025 06:51:40 +0300 Subject: [PATCH 2/3] Fix settings.md --- docs/Settings.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index 9a92103ff9f..fa2c1baef13 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -594,7 +594,7 @@ Control rate profile to switch to when the battery profile is selected, 0 to dis ### crsf_use_legacy_baro_packet -CRSF telemetry: If `ON`, send altitude about start point in GPS telemetry packet. No vario, no If `OFF`, BaroVario packet will have vario speed and altitude about start point and GPS packet will have ASL altitude (about sea level). These are deprecated, and will be removed in INAV 11.0. Tools and scripts using these GPS Altitude should be updated to use the BaroVario packet and GPS packet. Default: 'OFF' +CRSF telemetry: If `ON`, send altitude about start point in GPS telemetry packet. If `OFF`, GPS has ASL altitude, altitude about start point in separate packet. Default: 'OFF' | Default | Min | Max | | --- | --- | --- | From f0c57db12f7b31114cbe75a5d3002174b93dc783 Mon Sep 17 00:00:00 2001 From: Roman Sorokin Date: Thu, 13 Nov 2025 07:11:16 +0300 Subject: [PATCH 3/3] Remove airspeed sensor (it in 11025) --- src/main/telemetry/crsf.c | 29 ----------------------------- 1 file changed, 29 deletions(-) diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 62e28489716..cb7c34c36b1 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -306,19 +306,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)); -} - typedef enum { CRSF_ACTIVE_ANTENNA1 = 0, CRSF_ACTIVE_ANTENNA2 = 1 @@ -487,7 +474,6 @@ typedef enum { CRSF_FRAME_FLIGHT_MODE_INDEX, CRSF_FRAME_GPS_INDEX, CRSF_FRAME_VARIO_OR_ALT_VARIO_SENSOR_INDEX, - CRSF_FRAME_AIRSPEED_SENSOR_INDEX, CRSF_SCHEDULE_COUNT_MAX } crsfFrameTypeIndex_e; @@ -562,13 +548,6 @@ static void processCrsf(void) telemetryConfig()->crsf_use_legacy_baro_packet ? crsfFrameVarioSensor(dst) : crsfFrameBarometerAltitudeVarioSensor(dst); crsfFinalize(dst); } -#endif -#if defined(USE_PITOT) - if (currentSchedule & BV(CRSF_FRAME_AIRSPEED_SENSOR_INDEX)) { - crsfInitializeFrame(dst); - crsfFrameAirSpeedSensor(dst); - crsfFinalize(dst); - } #endif crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount; } @@ -602,11 +581,6 @@ void initCrsfTelemetry(void) if (sensors(SENSOR_BARO) || (STATE(FIXED_WING_LEGACY) && feature(FEATURE_GPS))) { crsfSchedule[index++] = BV(CRSF_FRAME_VARIO_OR_ALT_VARIO_SENSOR_INDEX); } -#endif -#ifdef USE_PITOT - if (sensors(SENSOR_PITOT)) { - crsfSchedule[index++] = BV(CRSF_FRAME_AIRSPEED_SENSOR_INDEX); - } #endif crsfScheduleCount = (uint8_t)index; } @@ -687,9 +661,6 @@ int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType) case CRSF_FRAMETYPE_BAROMETER_ALTITUDE_VARIO_SENSOR: crsfFrameBarometerAltitudeVarioSensor(sbuf); break; - case CRSF_FRAMETYPE_AIRSPEED_SENSOR: - crsfFrameAirSpeedSensor(sbuf); - break; } const int frameSize = crsfFinalizeBuf(sbuf, frame); return frameSize;