From 92d6315e59446cda53422ea9c08d06fe6f19a0dd Mon Sep 17 00:00:00 2001
From: MUSTARDTIGERFPV <mustardtiger@thetacoduck.com>
Date: Thu, 11 Jul 2024 20:19:47 +0000
Subject: [PATCH] Improve MAVLink behavior with half-duplex links, update
 default SRs

---
 docs/Settings.md             |  4 ++--
 src/main/fc/settings.yaml    |  4 ++--
 src/main/telemetry/mavlink.c | 19 ++++++++++++-------
 3 files changed, 16 insertions(+), 11 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index a217a26a9..393960c26 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -2518,7 +2518,7 @@ Rate of the extra1 message for MAVLink telemetry
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 10 | 0 | 255 |
+| 2 | 0 | 255 |
 
 ---
 
@@ -2558,7 +2558,7 @@ Rate of the RC channels message for MAVLink telemetry
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 5 | 0 | 255 |
+| 1 | 0 | 255 |
 
 ---
 
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 6670148d0..5caf1dd82 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -3155,7 +3155,7 @@ groups:
         type: uint8_t
         min: 0
         max: 255
-        default_value: 5
+        default_value: 1
       - name: mavlink_pos_rate
         description: "Rate of the position message for MAVLink telemetry"
         field: mavlink.position_rate
@@ -3169,7 +3169,7 @@ groups:
         type: uint8_t
         min: 0
         max: 255
-        default_value: 10
+        default_value: 2
       - name: mavlink_extra2_rate
         description: "Rate of the extra2 message for MAVLink telemetry"
         field: mavlink.extra2_rate
diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index be10ad1fd..8931ed83c 100644
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -1222,6 +1222,11 @@ static bool processMAVLinkIncomingTelemetry(void)
     return false;
 }
 
+static bool isMAVLinkTelemetryHalfDuplex(void) {
+    return telemetryConfig()->halfDuplex ||
+            (rxConfig()->receiverType == RX_TYPE_SERIAL && rxConfig()->serialrx_provider == SERIALRX_MAVLINK && tristateWithDefaultOffIsActive(rxConfig()->halfDuplex));
+}
+
 void handleMAVLinkTelemetry(timeUs_t currentTimeUs)
 {
     if (!mavlinkTelemetryEnabled) {
@@ -1232,18 +1237,18 @@ void handleMAVLinkTelemetry(timeUs_t currentTimeUs)
         return;
     }
 
-    // Process incoming MAVLink - ignore the return indicating whether or not a message was processed
-    // Very few telemetry links are dynamic uplink/downlink split so uplink telemetry shouldn't reduce downlink bandwidth availability
-    processMAVLinkIncomingTelemetry();
-
-    // Determine whether to send telemetry back
+    // Process incoming MAVLink
+    bool receivedMessage = processMAVLinkIncomingTelemetry();
     bool shouldSendTelemetry = false;
+
+    // Determine whether to send telemetry back based on flow control / pacing
     if (txbuff_valid) {
         // Use flow control if available
         shouldSendTelemetry = txbuff_free >= 33;
     } else {
-        // If not, use blind frame pacing
-        shouldSendTelemetry = (currentTimeUs - lastMavlinkMessage) >= TELEMETRY_MAVLINK_DELAY;
+        // If not, use blind frame pacing - and back off for collision avoidance if half-duplex
+        bool halfDuplexBackoff = (isMAVLinkTelemetryHalfDuplex() && receivedMessage);
+        shouldSendTelemetry = ((currentTimeUs - lastMavlinkMessage) >= TELEMETRY_MAVLINK_DELAY) && !halfDuplexBackoff;
     }
 
     if (shouldSendTelemetry) {
-- 
2.11.4.GIT