diff --git a/src/main/mavlink/mavlink_ports.c b/src/main/mavlink/mavlink_ports.c index f2c66fb1941..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->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) diff --git a/src/main/mavlink/mavlink_routing.c b/src/main/mavlink/mavlink_routing.c index 09144962fe4..88aee8ab239 100644 --- a/src/main/mavlink/mavlink_routing.c +++ b/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; diff --git a/src/main/mavlink/mavlink_routing.h b/src/main/mavlink/mavlink_routing.h index ae63f372416..b567d190584 100644 --- a/src/main/mavlink/mavlink_routing.h +++ b/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); 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..aeea48e60e1 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,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); 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 fded8e6292f..8fb2f9e6b7e 100644 --- a/src/main/mavlink/mavlink_types.h +++ b/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;