Merge pull request #10215 from iNavFlight/mmosca-mavlinkrc

Further tweaks to mavlink

Author: mmosca

docs/Settings.md

@@ -2552,6 +2552,16 @@ Rate of the extra3 message for MAVLink telemetry
 
 ---
 
+### mavlink_min_txbuffer
+
+Minimum percent of TX buffer space free, before attempting to transmit telemetry. Requuires RADIO_STATUS messages to be processed. 0 = always transmits.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 33 | 0 | 100 |
+
+---
+
 ### mavlink_pos_rate
 
 Rate of the position message for MAVLink telemetry
@@ -2562,6 +2572,16 @@ Rate of the position message for MAVLink telemetry
 
 ---
 
+### mavlink_radio_type
+
+Mavlink radio type. Affects how RSSI and LQ are reported on OSD.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| GENERIC |  |  |
+
+---
+
 ### mavlink_rc_chan_rate
 
 Rate of the RC channels message for MAVLink telemetry

src/main/fc/settings.yaml

@@ -198,6 +198,9 @@ tables:
   - name: headtracker_dev_type
     values: ["NONE", "SERIAL", "MSP"]
     enum: headTrackerDevType_e
+  - name: mavlink_radio_type
+    values: ["GENERIC", "ELRS", "SIK"]
+    enum: mavlinkRadio_e
 
 constants:
   RPYL_PID_MIN: 0
@@ -3195,6 +3198,18 @@ groups:
         min: 1
         max: 2
         default_value: 2
+      - name: mavlink_min_txbuffer
+        field: mavlink.min_txbuff
+        description: "Minimum percent of TX buffer space free, before attempting to transmit telemetry. Requuires RADIO_STATUS messages to be processed. 0 = always transmits."
+        default_value: 33
+        min: 0
+        max: 100
+      - name: mavlink_radio_type
+        description: "Mavlink radio type. Affects how RSSI and LQ are reported on OSD."
+        field: mavlink.radio_type
+        table: mavlink_radio_type
+        default_value: "GENERIC"
+        type: uint8_t
 
   - name: PG_OSD_CONFIG
     type: osdConfig_t

src/main/telemetry/mavlink.c

@@ -1126,11 +1126,39 @@ static bool handleIncoming_PARAM_REQUEST_LIST(void) {
     return true;
 }
 
+static void mavlinkParseRxStats(const mavlink_radio_status_t *msg) {
+    switch(telemetryConfig()->mavlink.radio_type) {
+        case MAVLINK_RADIO_SIK:
+            // rssi scaling info from: https://ardupilot.org/rover/docs/common-3dr-radio-advanced-configuration-and-technical-information.html
+            rxLinkStatistics.uplinkRSSI = (msg->rssi / 1.9) - 127;
+            rxLinkStatistics.uplinkSNR = msg->noise / 1.9;
+            rxLinkStatistics.uplinkLQ = msg->rssi != 255 ? scaleRange(msg->rssi, 0, 254, 0, 100) : 0;
+            break;
+        case MAVLINK_RADIO_ELRS:
+            rxLinkStatistics.uplinkRSSI = -msg->remrssi;
+            rxLinkStatistics.uplinkSNR = msg->noise;
+            rxLinkStatistics.uplinkLQ = scaleRange(msg->rssi, 0, 255, 0, 100);
+            break;
+        case MAVLINK_RADIO_GENERIC:
+        default:
+            rxLinkStatistics.uplinkRSSI = msg->rssi;
+            rxLinkStatistics.uplinkSNR = msg->noise;
+            rxLinkStatistics.uplinkLQ = msg->rssi != 255 ? scaleRange(msg->rssi, 0, 254, 0, 100) : 0;
+            break;
+    }
+}
+
 static bool handleIncoming_RADIO_STATUS(void) {
     mavlink_radio_status_t msg;
     mavlink_msg_radio_status_decode(&mavRecvMsg, &msg);
     txbuff_valid = true;
     txbuff_free = msg.txbuf;
+       
+    if (rxConfig()->receiverType == RX_TYPE_SERIAL &&
+        rxConfig()->serialrx_provider == SERIALRX_MAVLINK) {
+        mavlinkParseRxStats(&msg);
+    }
+
     return true;
 }
 
@@ -1227,7 +1255,7 @@ static bool processMAVLinkIncomingTelemetry(void)
 #endif
                 case MAVLINK_MSG_ID_RADIO_STATUS:
                     handleIncoming_RADIO_STATUS();
-                    // Don't set that we handled a message, otherwise radio status packets will block telemetry messages
+                    // Don't set that we handled a message, otherwise radio status packets will block telemetry messages.
                     return false;
                 default:
                     return false;
@@ -1260,7 +1288,7 @@ void handleMAVLinkTelemetry(timeUs_t currentTimeUs)
     // Determine whether to send telemetry back based on flow control / pacing
     if (txbuff_valid) {
         // Use flow control if available
-        shouldSendTelemetry = txbuff_free >= 33;
+        shouldSendTelemetry = txbuff_free >= telemetryConfig()->mavlink.min_txbuff;
     } else {
         // If not, use blind frame pacing - and back off for collision avoidance if half-duplex
         bool halfDuplexBackoff = (isMAVLinkTelemetryHalfDuplex() && receivedMessage);

src/main/telemetry/telemetry.c

@@ -56,7 +56,7 @@
 #include "telemetry/ghst.h"
 
 
-PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 6);
+PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 8);
 
 PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig,
     .telemetry_switch = SETTING_TELEMETRY_SWITCH_DEFAULT,
@@ -93,7 +93,9 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig,
         .extra1_rate = SETTING_MAVLINK_EXTRA1_RATE_DEFAULT,
         .extra2_rate = SETTING_MAVLINK_EXTRA2_RATE_DEFAULT,
         .extra3_rate = SETTING_MAVLINK_EXTRA3_RATE_DEFAULT,
-        .version = SETTING_MAVLINK_VERSION_DEFAULT
+        .version = SETTING_MAVLINK_VERSION_DEFAULT,
+        .min_txbuff = SETTING_MAVLINK_MIN_TXBUFFER_DEFAULT,
+        .radio_type = SETTING_MAVLINK_RADIO_TYPE_DEFAULT
     }
 );
 

src/main/telemetry/telemetry.h

@@ -36,6 +36,12 @@ typedef enum {
     LTM_RATE_SLOW
 } ltmUpdateRate_e;
 
+typedef enum {
+    MAVLINK_RADIO_GENERIC,
+    MAVLINK_RADIO_ELRS,
+    MAVLINK_RADIO_SIK,
+} mavlinkRadio_e;
+
 typedef enum {
     SMARTPORT_FUEL_UNIT_PERCENT,
     SMARTPORT_FUEL_UNIT_MAH,
@@ -73,6 +79,8 @@ typedef struct telemetryConfig_s {
         uint8_t extra2_rate;
         uint8_t extra3_rate;
         uint8_t version;
+        uint8_t min_txbuff;
+        uint8_t radio_type;
     } mavlink;
 } telemetryConfig_t;