From 37779c4abcd1ab72ea15a11018a2b8d45d041e52 Mon Sep 17 00:00:00 2001
From: frogmane <7685285+xznhj8129@users.noreply.github.com>
Date: Sun, 19 Jul 2026 02:32:19 -0400
Subject: [PATCH 1/2] Refine reconnect detection: per-peer tracking, snapshot
 rate limit

Review feedback on the reconnect STATUSTEXT commit:

- Track heartbeat presence per peer in the route table instead of one
  timestamp per port, so a steady peer cannot mask a newly joining one
  behind the same port, and a peer failing over to another port
  registers as a reconnect there. If the route table is full the new
  peer degrades to broadcast-only behavior.
- Rate-limit arming snapshots per port so a peer heartbeating slower
  than the gap threshold cannot elicit a resend on every beat.
- Drop the enable-transition snapshot: it fired into a port nobody had
  connected to yet (boot) and duplicated the first-heartbeat snapshot on
  shared-port enable. Reconnect is now driven purely by the heartbeat
  signal; the enable transition still resets per-port one-shot state via
  the port reset path.
---
 src/main/mavlink/mavlink_ports.c   |  2 +-
 src/main/mavlink/mavlink_routing.c | 14 +++++++++
 src/main/mavlink/mavlink_routing.h |  1 +
 src/main/mavlink/mavlink_runtime.c |  1 -
 src/main/mavlink/mavlink_streams.c | 49 ++++++++++++++++++++++--------
 src/main/mavlink/mavlink_types.h   |  7 ++++-
 6 files changed, 58 insertions(+), 16 deletions(-)

diff --git a/src/main/mavlink/mavlink_ports.c b/src/main/mavlink/mavlink_ports.c
index f2c66fb1941..4c480dbdc1d 100644
--- a/src/main/mavlink/mavlink_ports.c
+++ b/src/main/mavlink/mavlink_ports.c
@@ -22,7 +22,7 @@ static void resetMAVLinkPortRuntimeState(uint8_t portIndex)
     state->lastStatusTextSeverity = 0;
     state->firstStatusTextMs = 0;
     state->lastStatusTextMs = 0;
-    state->lastRemoteHeartbeatMs = 0;
+    state->lastArmingSnapshotMs = 0;
     memset(state->mavStreamNextDue, 0, sizeof(state->mavStreamNextDue));
     memset(state->mavMessageOverrideIntervalsUs, 0, sizeof(state->mavMessageOverrideIntervalsUs));
     memset(state->mavMessageNextDue, 0, sizeof(state->mavMessageNextDue));
diff --git a/src/main/mavlink/mavlink_routing.c b/src/main/mavlink/mavlink_routing.c
index 09144962fe4..cf83bb0eb08 100644
--- a/src/main/mavlink/mavlink_routing.c
+++ b/src/main/mavlink/mavlink_routing.c
@@ -31,9 +31,23 @@ void mavlinkLearnRoute(uint8_t ingressPortIndex)
     mavRouteTable[mavRouteCount].sysid = mavlinkContext.recvMsg.sysid;
     mavRouteTable[mavRouteCount].compid = mavlinkContext.recvMsg.compid;
     mavRouteTable[mavRouteCount].ingressPortIndex = ingressPortIndex;
+    mavRouteTable[mavRouteCount].lastHeartbeatPortIndex = 0;
+    mavRouteTable[mavRouteCount].lastHeartbeatMs = 0;
     mavRouteCount++;
 }
 
+mavlinkRouteEntry_t *mavlinkFindRoute(uint8_t sysid, uint8_t compid)
+{
+    for (uint8_t routeIndex = 0; routeIndex < mavRouteCount; routeIndex++) {
+        mavlinkRouteEntry_t *route = &mavRouteTable[routeIndex];
+        if (route->sysid == sysid && route->compid == compid) {
+            return route;
+        }
+    }
+
+    return NULL;
+}
+
 void mavlinkExtractTargets(const mavlink_message_t *msg, int16_t *targetSystem, int16_t *targetComponent)
 {
     *targetSystem = -1;
diff --git a/src/main/mavlink/mavlink_routing.h b/src/main/mavlink/mavlink_routing.h
index ae63f372416..9809af875f4 100644
--- a/src/main/mavlink/mavlink_routing.h
+++ b/src/main/mavlink/mavlink_routing.h
@@ -7,6 +7,7 @@
 
 bool mavlinkIsFromLocalIdentity(uint8_t sysid, uint8_t compid);
 void mavlinkLearnRoute(uint8_t ingressPortIndex);
+mavlinkRouteEntry_t *mavlinkFindRoute(uint8_t sysid, uint8_t compid);
 void mavlinkExtractTargets(const mavlink_message_t *msg, int16_t *targetSystem, int16_t *targetComponent);
 void mavlinkForwardMessage(uint8_t ingressPortIndex, int16_t targetSystem, int16_t targetComponent);
 int8_t mavlinkResolveLocalPortForTarget(int16_t targetSystem, int16_t targetComponent, uint8_t ingressPortIndex);
diff --git a/src/main/mavlink/mavlink_runtime.c b/src/main/mavlink/mavlink_runtime.c
index 94551728ef4..6fce0de2422 100644
--- a/src/main/mavlink/mavlink_runtime.c
+++ b/src/main/mavlink/mavlink_runtime.c
@@ -126,7 +126,6 @@ void mavlinkRuntimeCheckState(void)
             configureMAVLinkTelemetryPort(portIndex);
             if (state->telemetryEnabled) {
                 configureMAVLinkStreamRates(portIndex);
-                mavlinkPortReconnected(portIndex);
             }
         } else {
             freeMAVLinkTelemetryPortByIndex(portIndex);
diff --git a/src/main/mavlink/mavlink_streams.c b/src/main/mavlink/mavlink_streams.c
index b1fd5327d63..62d8ec287f2 100644
--- a/src/main/mavlink/mavlink_streams.c
+++ b/src/main/mavlink/mavlink_streams.c
@@ -3,6 +3,7 @@
 #include "common/time.h"
 
 #include "mavlink/mavlink_modes.h"
+#include "mavlink/mavlink_routing.h"
 #include "mavlink/mavlink_runtime.h"
 #include "mavlink/mavlink_streams.h"
 
@@ -24,10 +25,15 @@ const uint8_t mavSecondaryRates[MAVLINK_STREAM_COUNT] = {
 #define MAVLINK_STATUS_TEXT_WARNING_REPEAT_MS 10000
 #define MAVLINK_STATUS_TEXT_CRITICAL_REPEAT_MS 5000
 
-// GCS heartbeats nominally arrive at 1 Hz; a gap this long means the peer on
-// this port went away and whatever comes back needs fresh one-shot state.
+// GCS heartbeats nominally arrive at 1 Hz; a gap this long means the peer
+// went away and needs fresh one-shot state when it returns.
 #define MAVLINK_HEARTBEAT_RECONNECT_GAP_MS 5000
 
+// Peers heartbeating slower than the gap threshold trigger a reconnect on
+// every beat; this floor caps the arming-snapshot rate per port regardless
+// of peer behavior.
+#define MAVLINK_ARMING_SNAPSHOT_MIN_INTERVAL_MS 10000
+
 static const char * const mavlinkInavFlightModeNames[FLM_COUNT] = {
     [FLM_MANUAL] = "MANUAL",
     [FLM_ACRO] = "ACRO",
@@ -1215,15 +1221,24 @@ bool mavlinkHandleIncomingHeartbeat(void)
     mavlink_heartbeat_t msg;
     mavlink_msg_heartbeat_decode(&mavlinkContext.recvMsg, &msg);
 
-    // A framed HEARTBEAT is the protocol's presence signal. First one ever on
-    // this port, or one arriving after a gap, means a peer just (re)connected.
-    mavlinkPortRuntime_t *ingressState = &mavPortStates[mavRecvPortIndex];
-    const timeMs_t nowMs = millis();
-    const bool firstHeartbeat = ingressState->lastRemoteHeartbeatMs == 0;
-    const bool heartbeatGap = nowMs - ingressState->lastRemoteHeartbeatMs >= MAVLINK_HEARTBEAT_RECONNECT_GAP_MS;
-    ingressState->lastRemoteHeartbeatMs = nowMs;
-    if (firstHeartbeat || heartbeatGap) {
-        mavlinkPortReconnected(mavRecvPortIndex);
+    // A framed HEARTBEAT is the protocol's presence signal. Track it per peer
+    // (route table entry) rather than per port, so a steady peer cannot mask a
+    // newly joining one behind the same port, and a peer moving to another
+    // port (failover) registers as a reconnect there. mavlinkLearnRoute() ran
+    // before dispatch, so the sender already has a route entry unless the
+    // table is full - in which case reconnect detection degrades gracefully
+    // to the pre-existing broadcast-only behavior for that peer.
+    mavlinkRouteEntry_t *route = mavlinkFindRoute(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid);
+    if (route) {
+        const timeMs_t nowMs = millis();
+        const bool firstHeartbeat = route->lastHeartbeatMs == 0;
+        const bool heartbeatGap = nowMs - route->lastHeartbeatMs >= MAVLINK_HEARTBEAT_RECONNECT_GAP_MS;
+        const bool portChanged = !firstHeartbeat && route->lastHeartbeatPortIndex != mavRecvPortIndex;
+        route->lastHeartbeatMs = nowMs;
+        route->lastHeartbeatPortIndex = mavRecvPortIndex;
+        if (firstHeartbeat || heartbeatGap || portChanged) {
+            mavlinkPortReconnected(mavRecvPortIndex);
+        }
     }
 
     switch (msg.type) {
@@ -1346,6 +1361,14 @@ void mavlinkSendArmingStatusTextToPort(uint8_t portIndex)
         return;
     }
 
+    mavlinkPortRuntime_t *state = &mavPortStates[portIndex];
+    const timeMs_t nowMs = millis();
+    if (state->lastArmingSnapshotMs != 0 &&
+        nowMs - state->lastArmingSnapshotMs < MAVLINK_ARMING_SNAPSHOT_MIN_INTERVAL_MS) {
+        return;
+    }
+    state->lastArmingSnapshotMs = nowMs;
+
     char text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
     mavlinkBuildArmingDisabledText(text, sizeof(text), disableFlags);
 
@@ -1366,8 +1389,8 @@ void mavlinkSendArmingStatusTextToPort(uint8_t portIndex)
 #endif
 }
 
-// Fired when a port is judged to have just (re)connected: shared-port enable
-// transition, or a remote HEARTBEAT after a gap on an always-open port.
+// Fired when a peer is judged to have just (re)connected on this port: its
+// first HEARTBEAT, one after a gap, or one after moving from another port.
 void mavlinkPortReconnected(uint8_t portIndex)
 {
     if (portIndex >= mavPortCount) {
diff --git a/src/main/mavlink/mavlink_types.h b/src/main/mavlink/mavlink_types.h
index fded8e6292f..213dc90c2b7 100644
--- a/src/main/mavlink/mavlink_types.h
+++ b/src/main/mavlink/mavlink_types.h
@@ -71,6 +71,11 @@ typedef struct mavlinkRouteEntry_s {
     uint8_t sysid;
     uint8_t compid;
     uint8_t ingressPortIndex;
+    // Reconnect detection state, updated only on HEARTBEAT from this peer.
+    // ingressPortIndex above is refreshed by every routed message, so it
+    // cannot be used to detect the port a heartbeat last arrived on.
+    uint8_t lastHeartbeatPortIndex;
+    timeMs_t lastHeartbeatMs;
 } mavlinkRouteEntry_t;
 
 typedef enum {
@@ -145,7 +150,7 @@ typedef struct mavlinkPortRuntime_s {
     uint8_t lastStatusTextSeverity;
     timeMs_t firstStatusTextMs;
     timeMs_t lastStatusTextMs;
-    timeMs_t lastRemoteHeartbeatMs;
+    timeMs_t lastArmingSnapshotMs;
     uint8_t txSeq;
     uint32_t txDroppedFrames;
     mavlink_message_t mavRecvMsg;

From 179b54880a42636ac793cd2646e6265303d88d02 Mon Sep 17 00:00:00 2001
From: frogmane <7685285+xznhj8129@users.noreply.github.com>
Date: Fri, 21 Aug 2026 15:17:44 -0400
Subject: [PATCH 2/2] Address reconnect-detection review: per-peer snapshot
 floor, port teardown

- Scope the arming-snapshot floor to the peer (route entry) instead of the
  port. Two peers sharing a port each get their own allowance, so a second
  peer's first-ever snapshot is no longer swallowed by the first peer's
  window. mavlinkPortReconnected() becomes mavlinkPeerReconnected() and
  takes the route it fired for.
- Clear heartbeat and snapshot state for peers last heard on a port when
  that port is torn down. The port's one-shot state is reset there, so a
  peer resuming inside the gap window must not skip its fresh snapshot.
- Say plainly what happens when the route table is full: that peer gets no
  snapshot at all. The broadcast path is edge-triggered on the arming flags
  changing, so calling it a "broadcast-only" fallback was wrong.
---
 src/main/mavlink/mavlink_ports.c   |  6 +++++-
 src/main/mavlink/mavlink_routing.c | 11 +++++++++++
 src/main/mavlink/mavlink_routing.h |  1 +
 src/main/mavlink/mavlink_streams.c | 28 ++++++++++++++++------------
 src/main/mavlink/mavlink_streams.h |  2 +-
 src/main/mavlink/mavlink_types.h   |  4 +++-
 6 files changed, 37 insertions(+), 15 deletions(-)

diff --git a/src/main/mavlink/mavlink_ports.c b/src/main/mavlink/mavlink_ports.c
index 4c480dbdc1d..91fbce18581 100644
--- a/src/main/mavlink/mavlink_ports.c
+++ b/src/main/mavlink/mavlink_ports.c
@@ -1,6 +1,7 @@
 #include "mavlink/mavlink_internal.h"
 
 #include "mavlink/mavlink_ports.h"
+#include "mavlink/mavlink_routing.h"
 #include "mavlink/mavlink_runtime.h"
 #include "mavlink/mavlink_streams.h"
 
@@ -22,7 +23,6 @@ static void resetMAVLinkPortRuntimeState(uint8_t portIndex)
     state->lastStatusTextSeverity = 0;
     state->firstStatusTextMs = 0;
     state->lastStatusTextMs = 0;
-    state->lastArmingSnapshotMs = 0;
     memset(state->mavStreamNextDue, 0, sizeof(state->mavStreamNextDue));
     memset(state->mavMessageOverrideIntervalsUs, 0, sizeof(state->mavMessageOverrideIntervalsUs));
     memset(state->mavMessageNextDue, 0, sizeof(state->mavMessageNextDue));
@@ -42,6 +42,10 @@ void freeMAVLinkTelemetryPortByIndex(uint8_t portIndex)
     state->port = NULL;
     state->telemetryEnabled = false;
     resetMAVLinkPortRuntimeState(portIndex);
+
+    // The port's one-shot state is gone, so peers last heard here must not be
+    // able to resume inside the gap window and skip a fresh snapshot.
+    mavlinkForgetHeartbeatsForPort(portIndex);
 }
 
 void configureMAVLinkTelemetryPort(uint8_t portIndex)
diff --git a/src/main/mavlink/mavlink_routing.c b/src/main/mavlink/mavlink_routing.c
index cf83bb0eb08..88aee8ab239 100644
--- a/src/main/mavlink/mavlink_routing.c
+++ b/src/main/mavlink/mavlink_routing.c
@@ -48,6 +48,17 @@ mavlinkRouteEntry_t *mavlinkFindRoute(uint8_t sysid, uint8_t compid)
     return NULL;
 }
 
+void mavlinkForgetHeartbeatsForPort(uint8_t portIndex)
+{
+    for (uint8_t routeIndex = 0; routeIndex < mavRouteCount; routeIndex++) {
+        mavlinkRouteEntry_t *route = &mavRouteTable[routeIndex];
+        if (route->lastHeartbeatMs != 0 && route->lastHeartbeatPortIndex == portIndex) {
+            route->lastHeartbeatMs = 0;
+            route->lastArmingSnapshotMs = 0;
+        }
+    }
+}
+
 void mavlinkExtractTargets(const mavlink_message_t *msg, int16_t *targetSystem, int16_t *targetComponent)
 {
     *targetSystem = -1;
diff --git a/src/main/mavlink/mavlink_routing.h b/src/main/mavlink/mavlink_routing.h
index 9809af875f4..b567d190584 100644
--- a/src/main/mavlink/mavlink_routing.h
+++ b/src/main/mavlink/mavlink_routing.h
@@ -8,6 +8,7 @@
 bool mavlinkIsFromLocalIdentity(uint8_t sysid, uint8_t compid);
 void mavlinkLearnRoute(uint8_t ingressPortIndex);
 mavlinkRouteEntry_t *mavlinkFindRoute(uint8_t sysid, uint8_t compid);
+void mavlinkForgetHeartbeatsForPort(uint8_t portIndex);
 void mavlinkExtractTargets(const mavlink_message_t *msg, int16_t *targetSystem, int16_t *targetComponent);
 void mavlinkForwardMessage(uint8_t ingressPortIndex, int16_t targetSystem, int16_t targetComponent);
 int8_t mavlinkResolveLocalPortForTarget(int16_t targetSystem, int16_t targetComponent, uint8_t ingressPortIndex);
diff --git a/src/main/mavlink/mavlink_streams.c b/src/main/mavlink/mavlink_streams.c
index 62d8ec287f2..aeea48e60e1 100644
--- a/src/main/mavlink/mavlink_streams.c
+++ b/src/main/mavlink/mavlink_streams.c
@@ -1226,8 +1226,10 @@ bool mavlinkHandleIncomingHeartbeat(void)
     // newly joining one behind the same port, and a peer moving to another
     // port (failover) registers as a reconnect there. mavlinkLearnRoute() ran
     // before dispatch, so the sender already has a route entry unless the
-    // table is full - in which case reconnect detection degrades gracefully
-    // to the pre-existing broadcast-only behavior for that peer.
+    // route table is full. A peer that finds no free slot gets no reconnect
+    // snapshot at all: the broadcast path is edge-triggered on the arming
+    // flags changing, so it stays silent while they hold steady. The table
+    // holds MAVLINK_MAX_ROUTES peers and is not expected to fill in practice.
     mavlinkRouteEntry_t *route = mavlinkFindRoute(mavlinkContext.recvMsg.sysid, mavlinkContext.recvMsg.compid);
     if (route) {
         const timeMs_t nowMs = millis();
@@ -1237,7 +1239,7 @@ bool mavlinkHandleIncomingHeartbeat(void)
         route->lastHeartbeatMs = nowMs;
         route->lastHeartbeatPortIndex = mavRecvPortIndex;
         if (firstHeartbeat || heartbeatGap || portChanged) {
-            mavlinkPortReconnected(mavRecvPortIndex);
+            mavlinkPeerReconnected(mavRecvPortIndex, route);
         }
     }
 
@@ -1361,14 +1363,6 @@ void mavlinkSendArmingStatusTextToPort(uint8_t portIndex)
         return;
     }
 
-    mavlinkPortRuntime_t *state = &mavPortStates[portIndex];
-    const timeMs_t nowMs = millis();
-    if (state->lastArmingSnapshotMs != 0 &&
-        nowMs - state->lastArmingSnapshotMs < MAVLINK_ARMING_SNAPSHOT_MIN_INTERVAL_MS) {
-        return;
-    }
-    state->lastArmingSnapshotMs = nowMs;
-
     char text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
     mavlinkBuildArmingDisabledText(text, sizeof(text), disableFlags);
 
@@ -1391,7 +1385,7 @@ void mavlinkSendArmingStatusTextToPort(uint8_t portIndex)
 
 // Fired when a peer is judged to have just (re)connected on this port: its
 // first HEARTBEAT, one after a gap, or one after moving from another port.
-void mavlinkPortReconnected(uint8_t portIndex)
+void mavlinkPeerReconnected(uint8_t portIndex, mavlinkRouteEntry_t *route)
 {
     if (portIndex >= mavPortCount) {
         return;
@@ -1405,6 +1399,16 @@ void mavlinkPortReconnected(uint8_t portIndex)
     state->firstStatusTextMs = 0;
     state->lastStatusTextMs = 0;
 
+    // A peer heartbeating slower than the gap threshold looks like it
+    // reconnects on every beat; floor the snapshot rate per peer, so peers
+    // sharing a port keep their own allowance.
+    const timeMs_t nowMs = millis();
+    if (route->lastArmingSnapshotMs != 0 &&
+        nowMs - route->lastArmingSnapshotMs < MAVLINK_ARMING_SNAPSHOT_MIN_INTERVAL_MS) {
+        return;
+    }
+    route->lastArmingSnapshotMs = nowMs;
+
     // Give this port the current arming-disable reason now, independent of
     // whether the global flags have changed since the last broadcast.
     mavlinkSendArmingStatusTextToPort(portIndex);
diff --git a/src/main/mavlink/mavlink_streams.h b/src/main/mavlink/mavlink_streams.h
index 05fb6572615..9f4b47f9463 100644
--- a/src/main/mavlink/mavlink_streams.h
+++ b/src/main/mavlink/mavlink_streams.h
@@ -27,5 +27,5 @@ bool mavlinkHandleIncomingTimesync(void);
 void mavlinkSendModeStatusText(void);
 void mavlinkSendArmingStatusText(void);
 void mavlinkSendArmingStatusTextToPort(uint8_t portIndex);
-void mavlinkPortReconnected(uint8_t portIndex);
+void mavlinkPeerReconnected(uint8_t portIndex, mavlinkRouteEntry_t *route);
 bool mavlinkHandleIncomingRequestDataStream(void);
diff --git a/src/main/mavlink/mavlink_types.h b/src/main/mavlink/mavlink_types.h
index 213dc90c2b7..8fb2f9e6b7e 100644
--- a/src/main/mavlink/mavlink_types.h
+++ b/src/main/mavlink/mavlink_types.h
@@ -76,6 +76,9 @@ typedef struct mavlinkRouteEntry_s {
     // cannot be used to detect the port a heartbeat last arrived on.
     uint8_t lastHeartbeatPortIndex;
     timeMs_t lastHeartbeatMs;
+    // Snapshot rate limit, per peer rather than per port: two peers sharing a
+    // port must not consume each other's allowance.
+    timeMs_t lastArmingSnapshotMs;
 } mavlinkRouteEntry_t;
 
 typedef enum {
@@ -150,7 +153,6 @@ typedef struct mavlinkPortRuntime_s {
     uint8_t lastStatusTextSeverity;
     timeMs_t firstStatusTextMs;
     timeMs_t lastStatusTextMs;
-    timeMs_t lastArmingSnapshotMs;
     uint8_t txSeq;
     uint32_t txDroppedFrames;
     mavlink_message_t mavRecvMsg;