Merge pull request #4786 from shellixyz/add_vario_sensor_to_crsf_telemetry

Add vario sensor to CRSF telemetry

src/main/rx/crsf.h

@@ -43,6 +43,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_LINK_STATISTICS_PAYLOAD_SIZE = 10,
     CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE = 22, // 11 bits per channel * 16 channels = 22 bytes.
@@ -83,6 +84,7 @@ typedef enum {
 
 typedef enum {
     CRSF_FRAMETYPE_GPS = 0x02,
+    CRSF_FRAMETYPE_VARIO_SENSOR = 0x07,
     CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08,
     CRSF_FRAMETYPE_LINK_STATISTICS = 0x14,
     CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16,

src/main/telemetry/crsf.c

@@ -56,6 +56,7 @@
 #include "rx/rx.h"
 
 #include "sensors/battery.h"
+#include "sensors/sensors.h"
 
 #include "telemetry/crsf.h"
 #include "telemetry/telemetry.h"
@@ -204,7 +205,7 @@ uint16_t    GPS heading ( degree / 100 )
 uint16      Altitude ( meter ­1000m offset )
 uint8_t     Satellites in use ( counter )
 */
-void crsfFrameGps(sbuf_t *dst)
+static void crsfFrameGps(sbuf_t *dst)
 {
     // use sbufWrite since CRC does not include frame length
     sbufWriteU8(dst, CRSF_FRAME_GPS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
@@ -218,6 +219,19 @@ void crsfFrameGps(sbuf_t *dst)
     crsfSerialize8(dst, gpsSol.numSat);
 }
 
+/*
+0x07 Vario sensor
+Payload:
+int16      Vertical speed ( cm/s )
+*/
+static 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);
+    crsfSerialize8(dst, CRSF_FRAMETYPE_VARIO_SENSOR);
+    crsfSerialize16(dst, getEstimatedActualVelocity(Z));
+}
+
 /*
 0x08 Battery sensor
 Payload:
@@ -226,7 +240,7 @@ uint16_t    Current ( mA * 100 )
 uint24_t    Capacity ( mAh )
 uint8_t     Battery remaining ( percent )
 */
-void crsfFrameBatterySensor(sbuf_t *dst)
+static void crsfFrameBatterySensor(sbuf_t *dst)
 {
     // use sbufWrite since CRC does not include frame length
     sbufWriteU8(dst, CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
@@ -271,7 +285,7 @@ int16_t     Yaw angle ( rad / 10000 )
 
 #define DECIDEGREES_TO_RADIANS10000(angle) ((int16_t)(1000.0f * (angle) * RAD))
 
-void crsfFrameAttitude(sbuf_t *dst)
+static void crsfFrameAttitude(sbuf_t *dst)
 {
      sbufWriteU8(dst, CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
      crsfSerialize8(dst, CRSF_FRAMETYPE_ATTITUDE);
@@ -285,7 +299,7 @@ void crsfFrameAttitude(sbuf_t *dst)
 Payload:
 char[]      Flight mode ( Null­terminated string )
 */
-void crsfFrameFlightMode(sbuf_t *dst)
+static void crsfFrameFlightMode(sbuf_t *dst)
 {
     // just do "OK" for the moment as a placeholder
     // write zero for frame length, since we don't know it yet
@@ -350,8 +364,8 @@ uint32_t    Null Bytes
 uint8_t     255 (Max MSP Parameter)
 uint8_t     0x01 (Parameter version 1)
 */
-void crsfFrameDeviceInfo(sbuf_t *dst) {
+static void crsfFrameDeviceInfo(sbuf_t *dst)
-
+{
     char buff[30];
     tfp_sprintf(buff, "%s %s: %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, TARGET_BOARD_IDENTIFIER);
 
@@ -379,6 +393,7 @@ typedef enum {
     CRSF_FRAME_BATTERY_SENSOR_INDEX,
     CRSF_FRAME_FLIGHT_MODE_INDEX,
     CRSF_FRAME_GPS_INDEX,
+    CRSF_FRAME_VARIO_SENSOR_INDEX,
     CRSF_SCHEDULE_COUNT_MAX
 } crsfFrameTypeIndex_e;
 
@@ -438,6 +453,13 @@ 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);
+        crsfFinalize(dst);
+    }
 #endif
     crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount;
 }
@@ -462,12 +484,18 @@ 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 (feature(FEATURE_GPS)) {
         crsfSchedule[index++] = BV(CRSF_FRAME_GPS_INDEX);
     }
+#endif
+#if defined(USE_BARO) || defined(USE_GPS)
+    if (sensors(SENSOR_BARO) || (STATE(FIXED_WING) && feature(FEATURE_GPS))) {
+        crsfSchedule[index++] = BV(CRSF_FRAME_VARIO_SENSOR_INDEX);
+    }
+#endif
     crsfScheduleCount = (uint8_t)index;
-
+}
- }
 
 bool checkCrsfTelemetryState(void)
 {
@@ -489,7 +517,7 @@ void handleCrsfTelemetry(timeUs_t currentTimeUs)
     // in between the RX frames.
     crsfRxSendTelemetryData();
 
-   // Send ad-hoc response frames as soon as possible
+    // Send ad-hoc response frames as soon as possible
 #if defined(USE_MSP_OVER_TELEMETRY)
     if (mspReplyPending) {
         mspReplyPending = handleCrsfMspFrameBuffer(CRSF_FRAME_TX_MSP_FRAME_SIZE, &crsfSendMspResponse);
@@ -510,7 +538,7 @@ void handleCrsfTelemetry(timeUs_t currentTimeUs)
     }
 
     // Actual telemetry data only needs to be sent at a low frequency, ie 10Hz
-	// Spread out scheduled frames evenly so each frame is sent at the same frequency.
+    // Spread out scheduled frames evenly so each frame is sent at the same frequency.
     if (currentTimeUs >= crsfLastCycleTime + (CRSF_CYCLETIME_US / crsfScheduleCount)) {
         crsfLastCycleTime = currentTimeUs;
         processCrsf();
@@ -539,6 +567,9 @@ int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType)
         crsfFrameGps(sbuf);
         break;
 #endif
+    case CRSF_FRAMETYPE_VARIO_SENSOR:
+        crsfFrameVarioSensor(sbuf);
+        break;
     }
     const int frameSize = crsfFinalizeBuf(sbuf, frame);
     return frameSize;