MAVLink: refine reconnect STATUSTEXT (per-peer tracking, snapshot rate limit)

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->lastRemoteHeartbeatMs = 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)

src/main/mavlink/mavlink_routing.c

@@ -31,9 +31,34 @@ 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 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;

src/main/mavlink/mavlink_routing.h

@@ -7,6 +7,8 @@
 
 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);

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);

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,26 @@ 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
+    // 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();
+        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) {
+            mavlinkPeerReconnected(mavRecvPortIndex, route);
+        }
     }
 
     switch (msg.type) {
@@ -1366,9 +1383,9 @@ 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.
-void mavlinkPortReconnected(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 mavlinkPeerReconnected(uint8_t portIndex, mavlinkRouteEntry_t *route)
 {
     if (portIndex >= mavPortCount) {
         return;
@@ -1382,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);

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);

src/main/mavlink/mavlink_types.h

@@ -71,6 +71,14 @@ 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;
+    // 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 {
@@ -145,7 +153,6 @@ typedef struct mavlinkPortRuntime_s {
     uint8_t lastStatusTextSeverity;
     timeMs_t firstStatusTextMs;
     timeMs_t lastStatusTextMs;
-    timeMs_t lastRemoteHeartbeatMs;
     uint8_t txSeq;
     uint32_t txDroppedFrames;
     mavlink_message_t mavRecvMsg;