USE_CRSF_LINK_STATISTICS Split from pr 8042

src/main/build/debug.c

@@ -87,4 +87,7 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "AC_ERROR",
     "DUAL_GYRO_SCALED",
     "DSHOT_RPM_ERRORS",
+    "CRSF_LINK_STATISTICS_UPLINK",
+    "CRSF_LINK_STATISTICS_PWR",
+    "CRSF_LINK_STATISTICS_DOWN",
 };

src/main/build/debug.h

@@ -103,6 +103,9 @@ typedef enum {
     DEBUG_AC_ERROR,
     DEBUG_DUAL_GYRO_SCALED,
     DEBUG_DSHOT_RPM_ERRORS,
+    DEBUG_CRSF_LINK_STATISTICS_UPLINK,
+    DEBUG_CRSF_LINK_STATISTICS_PWR,
+    DEBUG_CRSF_LINK_STATISTICS_DOWN,
     DEBUG_COUNT
 } debugType_e;
 

src/main/rx/crsf.c

@@ -56,6 +56,8 @@
 
 #define CRSF_PAYLOAD_OFFSET offsetof(crsfFrameDef_t, type)
 
+#define CRSF_LINK_STATUS_UPDATE_US                   250000 // 250ms, 4 Hz mode 1 telemetry
+
 STATIC_UNIT_TESTED bool crsfFrameDone = false;
 STATIC_UNIT_TESTED crsfFrame_t crsfFrame;
 STATIC_UNIT_TESTED uint32_t crsfChannelData[CRSF_MAX_CHANNEL];
@@ -114,6 +116,80 @@ struct crsfPayloadRcChannelsPacked_s {
 
 typedef struct crsfPayloadRcChannelsPacked_s crsfPayloadRcChannelsPacked_t;
 
+#if defined(USE_CRSF_LINK_STATISTICS)
+/*
+ * 0x14 Link statistics
+ * Payload:
+ *
+ * uint8_t Uplink RSSI Ant. 1 ( dBm * -1 )
+ * uint8_t Uplink RSSI Ant. 2 ( dBm * -1 )
+ * uint8_t Uplink Package success rate / Link quality ( % )
+ * int8_t Uplink SNR ( db )
+ * uint8_t Diversity active antenna ( enum ant. 1 = 0, ant. 2 )
+ * uint8_t RF Mode ( enum 4fps = 0 , 50fps, 150hz)
+ * uint8_t Uplink TX Power ( enum 0mW = 0, 10mW, 25 mW, 100 mW, 500 mW, 1000 mW, 2000mW )
+ * uint8_t Downlink RSSI ( dBm * -1 )
+ * uint8_t Downlink package success rate / Link quality ( % )
+ * int8_t Downlink SNR ( db )
+ * Uplink is the connection from the ground to the UAV and downlink the opposite direction.
+ */
+
+typedef struct crsfPayloadLinkstatistics_s {
+    uint8_t uplink_RSSI_1;
+    uint8_t uplink_RSSI_2;
+    uint8_t uplink_Link_quality;
+    int8_t uplink_SNR;
+    uint8_t active_antenna;
+    uint8_t rf_Mode;
+    uint8_t uplink_TX_Power;
+    uint8_t downlink_RSSI;
+    uint8_t downlink_Link_quality;
+    int8_t downlink_SNR;
+} crsfLinkStatistics_t;
+
+static timeUs_t lastLinkStatisticsFrameUs;
+
+static void handleCrsfLinkStatisticsFrame(const crsfLinkStatistics_t* statsPtr, timeUs_t currentTimeUs)
+{
+    const crsfLinkStatistics_t stats = *statsPtr;
+    lastLinkStatisticsFrameUs = currentTimeUs;
+
+    const uint8_t rssiDbm = stats.active_antenna ? stats.uplink_RSSI_2 : stats.uplink_RSSI_1;
+    const uint16_t rssiPercentScaled = scaleRange(rssiDbm, 130, 0, 0, RSSI_MAX_VALUE);
+    setRssi(rssiPercentScaled, RSSI_SOURCE_RX_PROTOCOL_CRSF);
+
+    switch (debugMode) {
+    case DEBUG_CRSF_LINK_STATISTICS_UPLINK:
+        DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_UPLINK, 0, stats.uplink_RSSI_1);
+        DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_UPLINK, 1, stats.uplink_RSSI_2);
+        DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_UPLINK, 2, stats.uplink_Link_quality);
+        DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_UPLINK, 3, stats.rf_Mode);
+        break;
+    case DEBUG_CRSF_LINK_STATISTICS_PWR:
+        DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_PWR, 0, stats.active_antenna);
+        DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_PWR, 1, stats.uplink_SNR);
+        DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_PWR, 2, stats.uplink_TX_Power);
+        break;
+    case DEBUG_CRSF_LINK_STATISTICS_DOWN:
+        DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_DOWN, 0, stats.downlink_RSSI);
+        DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_DOWN, 1, stats.downlink_Link_quality);
+        DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_DOWN, 2, stats.downlink_SNR);
+        break;
+    }
+
+}
+#endif
+
+static void crsfCheckRssi(uint32_t currentTimeUs) {
+    UNUSED(currentTimeUs);
+
+#if defined(USE_CRSF_LINK_STATISTICS)
+    if (cmpTimeUs(currentTimeUs, lastLinkStatisticsFrameUs) > CRSF_LINK_STATUS_UPDATE_US) {
+        setRssiDirect(0,RSSI_SOURCE_RX_PROTOCOL_CRSF);
+    }
+#endif
+}
+
 STATIC_UNIT_TESTED uint8_t crsfFrameCRC(void)
 {
     // CRC includes type and payload
@@ -178,6 +254,19 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *data)
                             crsfProcessDisplayPortCmd(frameStart);
                             break;
                         }
+#endif
+#if defined(USE_CRSF_LINK_STATISTICS)
+
+                        case CRSF_FRAMETYPE_LINK_STATISTICS: {
+                             // if to FC and 10 bytes + CRSF_FRAME_ORIGIN_DEST_SIZE
+                             if ((rssiSource == RSSI_SOURCE_RX_PROTOCOL_CRSF) &&
+                                 (crsfFrame.frame.deviceAddress == CRSF_ADDRESS_FLIGHT_CONTROLLER) &&
+                                 (crsfFrame.frame.frameLength == CRSF_FRAME_ORIGIN_DEST_SIZE + CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE)) {
+                                 const crsfLinkStatistics_t* statsFrame = (const crsfLinkStatistics_t*)&crsfFrame.frame.payload;
+                                 handleCrsfLinkStatisticsFrame(statsFrame, currentTimeUs);
+                             }
+                            break;
+                        }
 #endif
                         default:
                             break;
@@ -192,6 +281,8 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 {
     UNUSED(rxRuntimeConfig);
 
+    crsfCheckRssi(micros());
+
     if (crsfFrameDone) {
         crsfFrameDone = false;
         if (crsfFrame.frame.type == CRSF_FRAMETYPE_RC_CHANNELS_PACKED) {
@@ -280,6 +371,10 @@ bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
         CRSF_PORT_OPTIONS | (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0)
         );
 
+        if (rssiSource == RSSI_SOURCE_NONE) {
+            rssiSource = RSSI_SOURCE_RX_PROTOCOL_CRSF;
+        }
+
     return serialPort != NULL;
 }
 

src/main/rx/rx.h

@@ -143,6 +143,7 @@ typedef enum {
     RSSI_SOURCE_RX_PROTOCOL,
     RSSI_SOURCE_MSP,
     RSSI_SOURCE_FRAME_ERRORS,
+    RSSI_SOURCE_RX_PROTOCOL_CRSF,
 } rssiSource_e;
 
 extern rssiSource_e rssiSource;

src/main/target/common_post.h

@@ -93,6 +93,7 @@
 
 #if !defined(USE_SERIALRX_CRSF)
 #undef USE_TELEMETRY_CRSF
+#undef USE_CRSF_LINK_STATISTICS
 #endif
 
 #if !defined(USE_TELEMETRY_CRSF) || !defined(USE_CMS)

src/main/target/common_pre.h

@@ -262,6 +262,7 @@
 #define USE_RX_MSP
 #define USE_ESC_SENSOR_INFO
 #define USE_CRSF_CMS_TELEMETRY
+#define USE_CRSF_LINK_STATISTICS
 #endif
 
 #endif // FLASH_SIZE > 128

src/test/unit/rx_crsf_unittest.cc

@@ -41,6 +41,8 @@ extern "C" {
 
     #include "telemetry/msp_shared.h"
 
+    rssiSource_e rssiSource;
+
     void crsfDataReceive(uint16_t c);
     uint8_t crsfFrameCRC(void);
     uint8_t crsfFrameStatus(void);

src/test/unit/telemetry_crsf_msp_unittest.cc

@@ -63,6 +63,7 @@ extern "C" {
     #include "telemetry/smartport.h"
     #include "sensors/acceleration.h"
 
+    rssiSource_e rssiSource;
     bool handleMspFrame(uint8_t *frameStart, int frameLength, uint8_t *skipsBeforeResponse);
     bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn);
     uint8_t sbufReadU8(sbuf_t *src);

src/test/unit/telemetry_crsf_unittest.cc

@@ -61,6 +61,7 @@ extern "C" {
     #include "telemetry/telemetry.h"
     #include "telemetry/msp_shared.h"
 
+    rssiSource_e rssiSource;
     bool airMode;
 
     uint16_t testBatteryVoltage = 0;