From d2515b4eeae3bca747b500afd4e422c98137473c Mon Sep 17 00:00:00 2001 From: Roman Sorokin Date: Thu, 2 Nov 2023 19:39:57 +0300 Subject: [PATCH 1/2] Add support baro altitude combined vario --- src/main/rx/crsf.h | 2 ++ src/main/telemetry/crsf.c | 41 +++++++++++++++++++++++++++++++++++---- 2 files changed, 39 insertions(+), 4 deletions(-) diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h index f3bc7933494..80e356223fb 100755 --- a/src/main/rx/crsf.h +++ b/src/main/rx/crsf.h @@ -45,6 +45,7 @@ enum { CRSF_FRAME_GPS_PAYLOAD_SIZE = 15, CRSF_FRAME_VARIO_SENSOR_PAYLOAD_SIZE = 2, CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE = 8, + CRSF_FRAME_BAROVARIO_SENSOR_PAYLOAD_SIZE = 4, 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, @@ -86,6 +87,7 @@ typedef enum { CRSF_FRAMETYPE_GPS = 0x02, CRSF_FRAMETYPE_VARIO_SENSOR = 0x07, CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08, + CRSF_FRAMETYPE_BAROVARIO_SENSOR = 0x09, 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 dca381adea9..885e8ff3535 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -57,7 +57,7 @@ #include "sensors/battery.h" #include "sensors/sensors.h" - +#include "sensors/barometer.h" #include "telemetry/crsf.h" #include "telemetry/telemetry.h" #include "telemetry/msp_shared.h" @@ -258,6 +258,22 @@ static void crsfFrameBatterySensor(sbuf_t *dst) crsfSerialize8(dst, batteryRemainingPercentage); } +/* +0x09 Baro+Vario sensor +Payload: +uint16 Altitude +int16 Vertical speed ( cm/s ) +*/ +static void crsfFrameBaroVarioSensor(sbuf_t *dst) +{ + // use sbufWrite since CRC does not include frame length + sbufWriteU8(dst, CRSF_FRAME_BAROVARIO_SENSOR_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); + crsfSerialize8(dst, CRSF_FRAMETYPE_BAROVARIO_SENSOR); + int32_t altitude = baroGetLatestAltitude() / 10; // Altitude in decimeters + crsfSerialize16(dst, altitude + 10000 < 0x8000 ? altitude + 10000 : 0x8000 + altitude / 10); + crsfSerialize16(dst, lrintf(getEstimatedActualVelocity(Z))); +} + typedef enum { CRSF_ACTIVE_ANTENNA1 = 0, CRSF_ACTIVE_ANTENNA2 = 1 @@ -409,6 +425,7 @@ typedef enum { CRSF_FRAME_FLIGHT_MODE_INDEX, CRSF_FRAME_GPS_INDEX, CRSF_FRAME_VARIO_SENSOR_INDEX, + CRSF_FRAME_BAROVARIO_SENSOR_INDEX, CRSF_SCHEDULE_COUNT_MAX } crsfFrameTypeIndex_e; @@ -469,12 +486,19 @@ static void processCrsf(void) crsfFinalize(dst); } #endif -#if defined(USE_BARO) || defined(USE_GPS) +#if !defined(USE_BARO) && defined(USE_GPS) if (currentSchedule & BV(CRSF_FRAME_VARIO_SENSOR_INDEX)) { crsfInitializeFrame(dst); crsfFrameVarioSensor(dst); crsfFinalize(dst); } +#endif +#if defined(USE_BARO) + if (currentSchedule & BV(CRSF_FRAME_BAROVARIO_SENSOR_INDEX)) { + crsfInitializeFrame(dst); + crsfFrameBaroVarioSensor(dst); + crsfFinalize(dst); + } #endif crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount; } @@ -504,10 +528,15 @@ void initCrsfTelemetry(void) crsfSchedule[index++] = BV(CRSF_FRAME_GPS_INDEX); } #endif -#if defined(USE_BARO) || defined(USE_GPS) - if (sensors(SENSOR_BARO) || (STATE(FIXED_WING_LEGACY) && feature(FEATURE_GPS))) { +#if !defined(USE_BARO) && defined(USE_GPS) + if (STATE(FIXED_WING_LEGACY) && feature(FEATURE_GPS)) { crsfSchedule[index++] = BV(CRSF_FRAME_VARIO_SENSOR_INDEX); } +#endif +#if defined(USE_BARO) + if (sensors(SENSOR_BARO)) { + crsfSchedule[index++] = BV(CRSF_FRAME_BAROVARIO_SENSOR_INDEX); + } #endif crsfScheduleCount = (uint8_t)index; } @@ -585,7 +614,11 @@ int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType) case CRSF_FRAMETYPE_VARIO_SENSOR: crsfFrameVarioSensor(sbuf); break; + case CRSF_FRAMETYPE_BAROVARIO_SENSOR: + crsfFrameBaroVarioSensor(sbuf); + break; } + const int frameSize = crsfFinalizeBuf(sbuf, frame); return frameSize; } From 7bf0ceeef51f7cb80bbc645621b17ca1e0c35349 Mon Sep 17 00:00:00 2001 From: Roman Sorokin Date: Sun, 5 Nov 2023 09:36:31 +0300 Subject: [PATCH 2/2] Vario 0x07 when baro is not present --- src/main/telemetry/crsf.c | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 885e8ff3535..b3db9b62c45 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -485,8 +485,6 @@ static void processCrsf(void) crsfFrameGps(dst); crsfFinalize(dst); } -#endif -#if !defined(USE_BARO) && defined(USE_GPS) if (currentSchedule & BV(CRSF_FRAME_VARIO_SENSOR_INDEX)) { crsfInitializeFrame(dst); crsfFrameVarioSensor(dst); @@ -523,19 +521,23 @@ void initCrsfTelemetry(void) crsfSchedule[index++] = BV(CRSF_FRAME_ATTITUDE_INDEX); crsfSchedule[index++] = BV(CRSF_FRAME_BATTERY_SENSOR_INDEX); crsfSchedule[index++] = BV(CRSF_FRAME_FLIGHT_MODE_INDEX); -#ifdef USE_GPS +#if defined(USE_GPS) if (feature(FEATURE_GPS)) { crsfSchedule[index++] = BV(CRSF_FRAME_GPS_INDEX); - } -#endif -#if !defined(USE_BARO) && defined(USE_GPS) - if (STATE(FIXED_WING_LEGACY) && feature(FEATURE_GPS)) { + #if !defined(USE_BARO) crsfSchedule[index++] = BV(CRSF_FRAME_VARIO_SENSOR_INDEX); + #endif } #endif #if defined(USE_BARO) if (sensors(SENSOR_BARO)) { crsfSchedule[index++] = BV(CRSF_FRAME_BAROVARIO_SENSOR_INDEX); + } else { + #if defined(USE_GPS) + if (feature(FEATURE_GPS)) { + crsfSchedule[index++] = BV(CRSF_FRAME_VARIO_SENSOR_INDEX); + } + #endif } #endif crsfScheduleCount = (uint8_t)index;