Add CRSF vario sensor support (betaflight#13558)

KarateBrot · May 9, 2024 · d5af7d2 · d5af7d2
1 parent d1ffa46
commit d5af7d2
Show file tree

Hide file tree

Showing 5 changed files with 39 additions and 1 deletion.
diff --git 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
@@ -257,6 +257,19 @@ void crsfFrameGps(sbuf_t *dst)
     sbufWriteU8(dst, gpsSol.numSat);
 }
 
+/*
+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:
@@ -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
@@ -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
@@ -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
@@ -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)
 {
-Original file line number
+Diff line change
@@ Expand Up / @@ -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)
     {
@@ Expand Down @@