From d5af7d2254a4c6ed04853859b3494cb33cf45fd8 Mon Sep 17 00:00:00 2001
From: Mark Haslinghuis <mark@numloq.nl>
Date: Thu, 9 May 2024 22:42:42 +0200
Subject: [PATCH] Add CRSF vario sensor support (#13558)

---
 src/main/rx/crsf_protocol.h                        |  2 ++
 src/main/telemetry/crsf.c                          | 32 +++++++++++++++++++++-
 .../unit/rx_spi_expresslrs_telemetry_unittest.cc   |  2 ++
 src/test/unit/telemetry_crsf_msp_unittest.cc       |  2 ++
 src/test/unit/telemetry_crsf_unittest.cc           |  2 ++
 5 files changed, 39 insertions(+), 1 deletion(-)

diff --git a/src/main/rx/crsf_protocol.h b/src/main/rx/crsf_protocol.h
index 0b56233f5..dc9803933 100644
--- a/src/main/rx/crsf_protocol.h
+++ b/src/main/rx/crsf_protocol.h
@@ -36,6 +36,7 @@ enum { CRSF_PAYLOAD_SIZE_MAX = CRSF_FRAME_SIZE_MAX - 6 };
 
 typedef enum {
     CRSF_FRAMETYPE_GPS = 0x02,
+    CRSF_FRAMETYPE_VARIO_SENSOR = 0x07,
     CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08,
     CRSF_FRAMETYPE_HEARTBEAT = 0x0B,
     CRSF_FRAMETYPE_LINK_STATISTICS = 0x14,
@@ -88,6 +89,7 @@ enum {
 
 enum {
     CRSF_FRAME_GPS_PAYLOAD_SIZE = 15,
+    CRSF_FRAME_VARIO_SENSOR_PAYLOAD_SIZE = 2,
     CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE = 8,
     CRSF_FRAME_HEARTBEAT_PAYLOAD_SIZE = 2,
     CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE = 10,
diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c
index e06ba340e..90fedba3b 100644
--- a/src/main/telemetry/crsf.c
+++ b/src/main/telemetry/crsf.c
@@ -258,6 +258,19 @@ void crsfFrameGps(sbuf_t *dst)
 }
 
 /*
+0x07 Vario sensor
+Payload:
+int16_t     Vertical speed ( cm/s )
+*/
+void crsfFrameVarioSensor(sbuf_t *dst)
+{
+    // use sbufWrite since CRC does not include frame length
+    sbufWriteU8(dst, CRSF_FRAME_VARIO_SENSOR_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
+    sbufWriteU8(dst, CRSF_FRAMETYPE_VARIO_SENSOR);
+    sbufWriteU16BigEndian(dst, getEstimatedVario()); // vario, cm/s(Z));
+}
+
+/*
 0x08 Battery sensor
 Payload:
 uint16_t    Voltage ( mV * 100 )
@@ -622,6 +635,7 @@ typedef enum {
     CRSF_FRAME_BATTERY_SENSOR_INDEX,
     CRSF_FRAME_FLIGHT_MODE_INDEX,
     CRSF_FRAME_GPS_INDEX,
+    CRSF_FRAME_VARIO_SENSOR_INDEX,
     CRSF_FRAME_HEARTBEAT_INDEX,
     CRSF_SCHEDULE_COUNT_MAX
 } crsfFrameTypeIndex_e;
@@ -692,7 +706,13 @@ static void processCrsf(void)
         crsfFinalize(dst);
     }
 #endif
-
+#ifdef USE_VARIO
+    if (currentSchedule & BIT(CRSF_FRAME_VARIO_SENSOR_INDEX)) {
+        crsfInitializeFrame(dst);
+        crsfFrameVarioSensor(dst);
+        crsfFinalize(dst);
+    }
+#endif
 #if defined(USE_CRSF_V3)
     if (currentSchedule & BIT(CRSF_FRAME_HEARTBEAT_INDEX)) {
         crsfInitializeFrame(dst);
@@ -760,6 +780,11 @@ void initCrsfTelemetry(void)
         crsfSchedule[index++] = BIT(CRSF_FRAME_GPS_INDEX);
     }
 #endif
+#ifdef USE_VARIO
+    if ((sensors(SENSOR_BARO) || featureIsEnabled(FEATURE_GPS)) && telemetryIsSensorEnabled(SENSOR_VARIO)) {
+        crsfSchedule[index++] = BIT(CRSF_FRAME_VARIO_SENSOR_INDEX);
+    }
+#endif
 
 #if defined(USE_CRSF_V3)
     while (index < (CRSF_CYCLETIME_US / CRSF_TELEMETRY_FRAME_INTERVAL_MAX_US) && index < CRSF_SCHEDULE_COUNT_MAX) {
@@ -959,6 +984,11 @@ int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType)
         crsfFrameGps(sbuf);
         break;
 #endif
+#if defined(USE_VARIO)
+    case CRSF_FRAMETYPE_VARIO_SENSOR:
+        crsfFrameVarioSensor(sbuf);
+        break;
+#endif
 #if defined(USE_MSP_OVER_TELEMETRY)
     case CRSF_FRAMETYPE_DEVICE_INFO:
         crsfFrameDeviceInfo(sbuf);
diff --git a/src/test/unit/rx_spi_expresslrs_telemetry_unittest.cc b/src/test/unit/rx_spi_expresslrs_telemetry_unittest.cc
index 40bae84c4..2221b09d6 100644
--- a/src/test/unit/rx_spi_expresslrs_telemetry_unittest.cc
+++ b/src/test/unit/rx_spi_expresslrs_telemetry_unittest.cc
@@ -428,6 +428,8 @@ extern "C" {
 
     int32_t getEstimatedAltitudeCm(void) { return gpsSol.llh.altCm; }
 
+    int16_t getEstimatedVario(void) { return 0; }
+
     int32_t getMAhDrawn(void) { return testmAhDrawn; }
 
     bool isArmingDisabled(void) { return false; }
diff --git a/src/test/unit/telemetry_crsf_msp_unittest.cc b/src/test/unit/telemetry_crsf_msp_unittest.cc
index ef0f4e3cb..b1675213d 100644
--- a/src/test/unit/telemetry_crsf_msp_unittest.cc
+++ b/src/test/unit/telemetry_crsf_msp_unittest.cc
@@ -285,6 +285,8 @@ extern "C" {
     	return 0;
     }
 
+    int16_t getEstimatedVario(void) { return 0; }
+
     bool featureIsEnabled(uint32_t) {return false;}
 
     bool airmodeIsEnabled(void) {return true;}
diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc
index 270a085c7..6530fa3dc 100644
--- a/src/test/unit/telemetry_crsf_unittest.cc
+++ b/src/test/unit/telemetry_crsf_unittest.cc
@@ -376,6 +376,8 @@ int32_t getEstimatedAltitudeCm(void)
 {
 	return gpsSol.llh.altCm;    // function returns cm not m.
 }
+
+int16_t getEstimatedVario(void) { return 0; }
     
 int32_t getMAhDrawn(void)
 {
-- 
2.11.4.GIT