Further tweaks to mavlink

docs/Settings.md

@@ -2542,6 +2542,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
@@ -2552,6 +2562,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
@@ -3191,6 +3194,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,
@@ -92,7 +92,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,
@@ -72,6 +78,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;