Skip to content

fix(mavlink): harden twin state management #2165

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
mbuckton merged 1 commit into from
Aug 10, 2026
Merged

fix(mavlink): harden twin state management #2165

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
124 changes: 106 additions & 18 deletions src/main/java/io/mapsmessaging/state/mavlink/MavlinkDroneMonitor.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,12 +19,26 @@

package io.mapsmessaging.state.mavlink;

import io.mapsmessaging.state.drone.core.*;
import io.mapsmessaging.state.drone.core.EntityTwin;
import io.mapsmessaging.state.drone.core.TwinLifecycleStatus;
import io.mapsmessaging.state.drone.core.TwinManager;
import io.mapsmessaging.state.drone.core.TwinObserver;
import io.mapsmessaging.state.drone.core.TwinRelationship;
import io.mapsmessaging.state.drone.core.TwinUpdateContext;
import io.mapsmessaging.state.drone.drone.DroneTwin;
import io.mapsmessaging.state.mavlink.bootstrap.*;
import io.mapsmessaging.state.mavlink.bootstrap.DroneTwinMissingState;
import io.mapsmessaging.state.mavlink.bootstrap.DroneTwinReadinessEvaluator;
import io.mapsmessaging.state.mavlink.bootstrap.DroneTwinReadinessResult;
import io.mapsmessaging.state.mavlink.bootstrap.MavlinkBootstrapEvent;
import io.mapsmessaging.state.mavlink.bootstrap.MavlinkBootstrapEventPublisher;
import io.mapsmessaging.state.mavlink.bootstrap.MavlinkBootstrapStateEngine;

import java.util.List;
import java.util.Map;
import java.util.Set;
import java.util.concurrent.ConcurrentHashMap;
import java.util.concurrent.atomic.AtomicBoolean;
import java.util.concurrent.atomic.AtomicInteger;

/**
* Observes MAVLink-backed drone twins and drives MAVLink bootstrap/readiness evaluation.
Expand All @@ -33,12 +47,15 @@
* updates readiness fields on the twin, and publishes bootstrap events for another
* component to translate into MAVLink commands.</p>
*/
public class MavlinkDroneMonitor implements TwinObserver {
public class MavlinkDroneMonitor implements TwinObserver, AutoCloseable {

private final TwinManager twinManager;
private final DroneTwinReadinessEvaluator readinessEvaluator;
private final MavlinkBootstrapStateEngine bootstrapStateEngine;
private final MavlinkBootstrapEventPublisher bootstrapEventPublisher;
private final Set<String> readinessUpdates;
private final Map<String, AtomicInteger> deferredUpdates;
private final AtomicBoolean closed;

public MavlinkDroneMonitor(
TwinManager twinManager,
Expand All @@ -50,6 +67,9 @@ public MavlinkDroneMonitor(
this.readinessEvaluator = readinessEvaluator;
this.bootstrapStateEngine = bootstrapStateEngine;
this.bootstrapEventPublisher = bootstrapEventPublisher;
this.readinessUpdates = ConcurrentHashMap.newKeySet();
this.deferredUpdates = new ConcurrentHashMap<>();
this.closed = new AtomicBoolean();
}

@Override
Expand All @@ -64,8 +84,15 @@ public void onTwinUpdated(String twinId, EntityTwin current, TwinUpdateContext c

@Override
public void onTwinRemoved(EntityTwin removed, TwinUpdateContext context) {
if (closed.get()) {
return;
}

if (removed != null && removed.getTwinId() != null) {
bootstrapStateEngine.remove(removed.getTwinId());
String twinId = removed.getTwinId();
deferredUpdates.remove(twinId);
readinessUpdates.remove(twinId);
bootstrapStateEngine.remove(twinId);
}
}

Expand Down Expand Up @@ -98,7 +125,55 @@ public void onRelationshipRemoved(
// no-op
}

@Override
public void close() {
if (closed.compareAndSet(false, true)) {
twinManager.removeObserver(this);
readinessUpdates.clear();
deferredUpdates.clear();
}
}

void beginTwinUpdate(String twinId) {
if (closed.get() || twinId == null) {
return;
}

deferredUpdates.compute(twinId, (key, depth) -> {
if (depth == null) {
return new AtomicInteger(1);
}
depth.incrementAndGet();
return depth;
});
}

void endTwinUpdate(String twinId, TwinUpdateContext context) {
if (twinId == null) {
return;
}

AtomicBoolean evaluate = new AtomicBoolean();
deferredUpdates.computeIfPresent(twinId, (key, depth) -> {
if (depth.decrementAndGet() <= 0) {
evaluate.set(true);
return null;
}
return depth;
});

if (!evaluate.get() || closed.get()) {
return;
}

twinManager.getTwin(twinId).ifPresent(twin -> evaluateTwin(twin, context));
}

private void evaluateTwin(EntityTwin twin, TwinUpdateContext context) {
if (closed.get()) {
return;
}

if (!(twin instanceof DroneTwin droneTwin)) {
return;
}
Expand All @@ -107,10 +182,16 @@ private void evaluateTwin(EntityTwin twin, TwinUpdateContext context) {
return;
}

String twinId = droneTwin.getTwinId();
if (twinId != null
&& (readinessUpdates.contains(twinId) || deferredUpdates.containsKey(twinId))) {
return;
}

DroneTwinReadinessResult readinessResult = readinessEvaluator.evaluate(droneTwin, context);
updateReadinessIfChanged(droneTwin, readinessResult, context);
List<MavlinkBootstrapEvent> events = bootstrapStateEngine.update(droneTwin, readinessResult, context);
if(bootstrapEventPublisher != null) {
if (bootstrapEventPublisher != null && !closed.get()) {
for (MavlinkBootstrapEvent event : events) {
bootstrapEventPublisher.publish(event);
}
Expand All @@ -134,19 +215,26 @@ private void updateReadinessIfChanged(
}

String twinId = droneTwin.getTwinId();
if (twinId == null || !readinessUpdates.add(twinId)) {
return;
}

twinManager.updateTwin(twinId, twin -> {
DroneTwin updatedDroneTwin = (DroneTwin) twin;

updatedDroneTwin.setReadinessState(readinessResult.getReadinessState().name());
updatedDroneTwin.setRegistrationReady(readinessResult.isRegistrationReady());
updatedDroneTwin.setCommandReady(readinessResult.isCommandReady());
updatedDroneTwin.setMissingReadinessItems(toNames(readinessResult.getMissingStates()));
updatedDroneTwin.setDegradedReadinessItems(toNames(readinessResult.getDegradedStates()));
updatedDroneTwin.setBlockingReadinessItems(toNames(readinessResult.getBlockingStates()));
updatedDroneTwin.setReadinessUpdatedAt(readinessResult.getEvaluatedAt());

}, context);
try {
twinManager.updateTwin(twinId, twin -> {
DroneTwin updatedDroneTwin = (DroneTwin) twin;

updatedDroneTwin.setReadinessState(readinessResult.getReadinessState().name());
updatedDroneTwin.setRegistrationReady(readinessResult.isRegistrationReady());
updatedDroneTwin.setCommandReady(readinessResult.isCommandReady());
updatedDroneTwin.setMissingReadinessItems(toNames(readinessResult.getMissingStates()));
updatedDroneTwin.setDegradedReadinessItems(toNames(readinessResult.getDegradedStates()));
updatedDroneTwin.setBlockingReadinessItems(toNames(readinessResult.getBlockingStates()));
updatedDroneTwin.setReadinessUpdatedAt(readinessResult.getEvaluatedAt());

}, context);
} finally {
readinessUpdates.remove(twinId);
}
}

private boolean hasReadinessChanged(
Expand Down Expand Up @@ -207,4 +295,4 @@ private boolean equalsNullable(Object left, Object right) {

return left.equals(right);
}
}
}
120 changes: 110 additions & 10 deletions src/main/java/io/mapsmessaging/state/mavlink/MavlinkStateSubscriber.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -53,11 +53,27 @@
import java.util.Collections;
import java.util.LinkedHashMap;
import java.util.Map;
import java.util.Objects;
import java.util.UUID;

import static io.mapsmessaging.state.logging.StateLogMessages.*;

public class MavlinkStateSubscriber implements MessageHandler {
import static io.mapsmessaging.state.logging.StateLogMessages.MAVLINK_STATE_CORRELATION_DATA_MISSING;
import static io.mapsmessaging.state.logging.StateLogMessages.MAVLINK_STATE_DRONE_NOT_CONFIGURED;
import static io.mapsmessaging.state.logging.StateLogMessages.MAVLINK_STATE_EMPTY_MESSAGE_IGNORED;
import static io.mapsmessaging.state.logging.StateLogMessages.MAVLINK_STATE_JSON_PARSE_FAILED;
import static io.mapsmessaging.state.logging.StateLogMessages.MAVLINK_STATE_MAVLINK_OBJECT_MISSING;
import static io.mapsmessaging.state.logging.StateLogMessages.MAVLINK_STATE_PAYLOAD_OBJECT_MISSING;
import static io.mapsmessaging.state.logging.StateLogMessages.MAVLINK_STATE_PROCESSING_FAILED;
import static io.mapsmessaging.state.logging.StateLogMessages.MAVLINK_STATE_SOURCE_NOT_CONFIGURED;
import static io.mapsmessaging.state.logging.StateLogMessages.MAVLINK_STATE_SUBSCRIBER_START_FAILED;
import static io.mapsmessaging.state.logging.StateLogMessages.MAVLINK_STATE_SUBSCRIBER_STARTED;
import static io.mapsmessaging.state.logging.StateLogMessages.MAVLINK_STATE_SUBSCRIBER_STARTING;
import static io.mapsmessaging.state.logging.StateLogMessages.MAVLINK_STATE_SUBSCRIBER_STOP_FAILED;
import static io.mapsmessaging.state.logging.StateLogMessages.MAVLINK_STATE_SUBSCRIBER_STOPPED;
import static io.mapsmessaging.state.logging.StateLogMessages.MAVLINK_STATE_SUBSCRIBER_STOPPING;
import static io.mapsmessaging.state.logging.StateLogMessages.MAVLINK_STATE_TWIN_UPDATE_FAILED;
import static io.mapsmessaging.state.logging.StateLogMessages.MAVLINK_STATE_UNSUPPORTED_PACKET_IGNORED;

public class MavlinkStateSubscriber implements MessageHandler, AutoCloseable {

private final Logger logger = LoggerFactory.getLogger(MavlinkStateSubscriber.class);

Expand All @@ -67,6 +83,9 @@ public class MavlinkStateSubscriber implements MessageHandler {
private final DroneInfoRegistry droneRegistry;
private final MavlinkTwinUpdater twinUpdater;

private volatile boolean started;
private volatile boolean closed;

public MavlinkStateSubscriber(@NonNull @NotNull TwinManager twinManager, @NonNull @NotNull MavlinkTwinConfigDTO mavlinkConfig, @NonNull @NotNull DroneInfoRegistry registry) {
this.protocol = SessionHelper.createLoopbackProtocol(this);
this.namespaceTopicPath = mavlinkConfig.getTopic();
Expand All @@ -75,34 +94,115 @@ public MavlinkStateSubscriber(@NonNull @NotNull TwinManager twinManager, @NonNul
this.twinUpdater = new MavlinkTwinUpdater(twinManager, new ListenerManager(twinManager));
}

public void start() throws IOException {
MavlinkStateSubscriber(
StateLoopProtocol protocol,
String namespaceTopicPath,
MavlinkSourceRegistry sourceRegistry,
DroneInfoRegistry droneRegistry,
MavlinkTwinUpdater twinUpdater
) {
this.protocol = Objects.requireNonNull(protocol, "protocol must not be null");
this.namespaceTopicPath = Objects.requireNonNull(namespaceTopicPath, "namespaceTopicPath must not be null");
this.sourceRegistry = Objects.requireNonNull(sourceRegistry, "sourceRegistry must not be null");
this.droneRegistry = Objects.requireNonNull(droneRegistry, "droneRegistry must not be null");
this.twinUpdater = Objects.requireNonNull(twinUpdater, "twinUpdater must not be null");
}

public synchronized void start() throws IOException {
if (closed) {
throw new IllegalStateException("MAVLink state subscriber is closed");
}

if (started) {
return;
}

logger.log(MAVLINK_STATE_SUBSCRIBER_STARTING, namespaceTopicPath);

try {
protocol.connect(UUID.randomUUID().toString(), "anonymous", "anonymous");
protocol.subscribeLocal(namespaceTopicPath, namespaceTopicPath, QualityOfService.AT_MOST_ONCE, null, null, null, null, null);
started = true;
logger.log(MAVLINK_STATE_SUBSCRIBER_STARTED, namespaceTopicPath);
} catch (IOException exception) {
closed = true;
try {
protocol.close();
} catch (IOException closeException) {
exception.addSuppressed(closeException);
}
try {
twinUpdater.close();
} catch (RuntimeException closeException) {
exception.addSuppressed(closeException);
}
logger.log(MAVLINK_STATE_SUBSCRIBER_START_FAILED, exception, namespaceTopicPath);
throw exception;
}
}

public void stop() throws IOException {
public synchronized void stop() throws IOException {
if (closed) {
return;
}

closed = true;
logger.log(MAVLINK_STATE_SUBSCRIBER_STOPPING, namespaceTopicPath);

Throwable failure = null;
if (started) {
try {
protocol.unsubscribeLocal(namespaceTopicPath);
} catch (RuntimeException exception) {
failure = exception;
}
}

try {
protocol.unsubscribeLocal(namespaceTopicPath);
protocol.close();
logger.log(MAVLINK_STATE_SUBSCRIBER_STOPPED, namespaceTopicPath);
} catch (IOException exception) {
logger.log(MAVLINK_STATE_SUBSCRIBER_STOP_FAILED, exception, namespaceTopicPath);
throw exception;
if (failure == null) {
failure = exception;
} else {
failure.addSuppressed(exception);
}
}

started = false;

try {
twinUpdater.close();
} catch (RuntimeException exception) {
if (failure == null) {
failure = exception;
} else {
failure.addSuppressed(exception);
}
}

if (failure != null) {
logger.log(MAVLINK_STATE_SUBSCRIBER_STOP_FAILED, failure, namespaceTopicPath);
if (failure instanceof IOException ioException) {
throw ioException;
}
throw (RuntimeException) failure;
}

logger.log(MAVLINK_STATE_SUBSCRIBER_STOPPED, namespaceTopicPath);
}

@Override
public void close() throws IOException {
stop();
}

@Override
public void handle(@NonNull @NotNull MessageEvent messageEvent) {
if (closed) {
messageEvent.getCompletionTask().run();
return;
}

String sourceName = messageEvent.getDestinationName();
Integer messageId = null;
String droneName = null;
Expand Down Expand Up @@ -224,4 +324,4 @@ private ProcessedFrame parseJson(byte[] opaqueData, String sourceName) {
return null;
}
}
}
}
Loading
Loading