diff --git a/docs/Settings.md b/docs/Settings.md index 775e44a6617..a34b732b54d 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. If `OFF`, GPS has ASL altitude, altitude about start point in separate 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 0240ae16b5e..870879a729c 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3285,6 +3285,10 @@ groups: min: 1 max: 255 default_value: 1 + - 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 8d84d52b8e5..3626ba09312 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 d86822ada7a..cb7c34c36b1 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" @@ -225,8 +226,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); } @@ -269,16 +269,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; @@ -291,9 +295,15 @@ 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); } typedef enum { @@ -463,8 +473,7 @@ 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_SCHEDULE_COUNT_MAX } crsfFrameTypeIndex_e; @@ -534,14 +543,9 @@ static void processCrsf(void) } #endif #if defined(USE_BARO) || defined(USE_GPS) - if (currentSchedule & BV(CRSF_FRAME_VARIO_SENSOR_INDEX)) { - crsfInitializeFrame(dst); - crsfFrameVarioSensor(dst); - crsfFinalize(dst); - } - if (currentSchedule & BV(CRSF_FRAME_BAROMETER_ALTITUDE_INDEX)) { + if (currentSchedule & BV(CRSF_FRAME_VARIO_OR_ALT_VARIO_SENSOR_INDEX) ) { crsfInitializeFrame(dst); - crsfBarometerAltitude(dst); + telemetryConfig()->crsf_use_legacy_baro_packet ? crsfFrameVarioSensor(dst) : crsfFrameBarometerAltitudeVarioSensor(dst); crsfFinalize(dst); } #endif @@ -575,12 +579,7 @@ 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); - } -#endif -#ifdef USE_BARO - if (sensors(SENSOR_BARO)) { - crsfSchedule[index++] = BV(CRSF_FRAME_BAROMETER_ALTITUDE_INDEX); + crsfSchedule[index++] = BV(CRSF_FRAME_VARIO_OR_ALT_VARIO_SENSOR_INDEX); } #endif crsfScheduleCount = (uint8_t)index; @@ -659,7 +658,10 @@ 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; + } const int frameSize = crsfFinalizeBuf(sbuf, frame); return frameSize; } diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 2c896945843..a73bc8ca998 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, @@ -98,7 +98,8 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .min_txbuff = SETTING_MAVLINK_MIN_TXBUFFER_DEFAULT, .radio_type = SETTING_MAVLINK_RADIO_TYPE_DEFAULT, .sysid = SETTING_MAVLINK_SYSID_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 7fb26781c11..58f1ece9db1 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -89,6 +89,7 @@ typedef struct telemetryConfig_s { uint8_t radio_type; uint8_t sysid; } mavlink; + bool crsf_use_legacy_baro_packet; } telemetryConfig_t; PG_DECLARE(telemetryConfig_t, telemetryConfig);