diff --git a/docs/Settings.md b/docs/Settings.md index 34af8698bf4..ae0d3006ae6 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2572,6 +2572,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 @@ -2582,6 +2592,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 diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 1016e26ddab..e43ad233b0c 100644 --- a/src/main/fc/settings.yaml +++ b/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 - name: default_altitude_source values: ["GPS", "BARO", "GPS ONLY", "BARO ONLY"] enum: navDefaultAltitudeSensor_e @@ -3209,6 +3212,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 diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 4e8d54535a3..92ddeb7a990 100644 --- a/src/main/telemetry/mavlink.c +++ b/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); diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 82fc2d2d5fb..e0820234e65 100644 --- a/src/main/telemetry/telemetry.c +++ b/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 } ); diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index 7be5e50a9ce..c8808959277 100644 --- a/src/main/telemetry/telemetry.h +++ b/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;