fix(mavlink): request autopilot version during bootstrap

.github/workflows/mavlink-area-mission-validation.yml

@@ -25,3 +25,8 @@ jobs:
           mvn -B
           -Dtest=MissionPlanRepeatTest,MavlinkCommandSetPreparerTest,MavlinkEventListSenderConcurrencyTest,SticklebackPassiveDetectionCapabilityTest,PlanTaskTypeTest,TwinManagerConfigPlanTaskTypeTest
           test
+      - name: Run MAVLink bootstrap tests
+        run: >-
+          mvn -B
+          -Dtest=MavlinkBootstrapRequestPublisherTest,AutopilotVersionListenerTest,MavlinkBootstrapStateEngineTest
+          test

src/main/java/io/mapsmessaging/rest/RestApiServerManager.java

@@ -339,7 +339,6 @@ private List<String> buildInbuilt(){
     endpoints.add("io.mapsmessaging.rest.api.impl.logging");
     endpoints.add("io.mapsmessaging.rest.api.impl.ml");
     endpoints.add("io.mapsmessaging.rest.api.impl.config");
-    endpoints.add("io.mapsmessaging.rest.api.impl.twins");
     return endpoints;
   }
 

src/main/java/io/mapsmessaging/state/StateManagerAgent.java

@@ -132,7 +132,7 @@ public GeoSpatialAreaRegistry getGeoSpatialAreaRegistry() {
   private void loadStateMessageAdapters(TwinManagerConfigDTO config) {
     StateMessageAdapterContext context = new StateMessageAdapterContext(twinManager, config);
     ServiceLoader<StateMessageAdapterFactory> adapterFactories = ServiceLoader.load(StateMessageAdapterFactory.class);
-
+    restApiPackageList.add("io.mapsmessaging.state.rest.twins");
     for (StateMessageAdapterFactory adapterFactory : adapterFactories) {
       Optional<StateMessageAdapter> optionalAdapter = adapterFactory.create(context);
       if (optionalAdapter.isPresent()) {

src/main/java/io/mapsmessaging/state/logging/StateLogMessages.java

@@ -68,6 +68,9 @@ public enum StateLogMessages implements LogMessage {
   MAVLINK_STATE_CORRELATION_DATA_MISSING(LEVEL.DEBUG, SERVER_CATEGORY.PROTOCOL, "MAVLink message {} from '{}' contains no correlation data"),
   MAVLINK_STATE_PROCESSING_FAILED(LEVEL.ERROR, SERVER_CATEGORY.PROTOCOL, "Failed to process MAVLink state message from '{}'"),
   MAVLINK_STATE_TWIN_UPDATE_FAILED(LEVEL.ERROR, SERVER_CATEGORY.PROTOCOL, "Failed to update drone '{}' from MAVLink message {} received from '{}'"),
+  MAVLINK_BOOTSTRAP_REQUEST_SKIPPED(LEVEL.DEBUG, SERVER_CATEGORY.PROTOCOL, "Skipping MAVLink bootstrap request for twin '{}': {}"),
+  MAVLINK_BOOTSTRAP_REQUEST_SENT(LEVEL.DEBUG, SERVER_CATEGORY.PROTOCOL, "Sent MAVLink bootstrap request for message {} to system {} component {} for twin '{}'"),
+  MAVLINK_BOOTSTRAP_REQUEST_FAILED(LEVEL.WARN, SERVER_CATEGORY.PROTOCOL, "Failed to send MAVLink bootstrap request for message {} to twin '{}'"),
   // </editor-fold>
 
   // <editor-fold desc="State Manager">

src/main/java/io/mapsmessaging/state/mavlink/MavlinkStateSubscriber.java

@@ -40,6 +40,7 @@
 import io.mapsmessaging.state.config.MavlinkTwinConfigDTO;
 import io.mapsmessaging.state.drone.core.TwinManager;
 import io.mapsmessaging.state.drone.core.TwinUpdateContext;
+import io.mapsmessaging.state.mavlink.bootstrap.MavlinkBootstrapRequestPublisher;
 import io.mapsmessaging.state.mavlink.listener.ListenerManager;
 import io.mapsmessaging.state.mavlink.packet.MavlinkPacket;
 import io.mapsmessaging.state.mavlink.packet.MavlinkPacketFactory;
@@ -91,7 +92,7 @@ public MavlinkStateSubscriber(@NonNull @NotNull TwinManager twinManager, @NonNul
     this.namespaceTopicPath = mavlinkConfig.getTopic();
     this.sourceRegistry = new MavlinkSourceRegistry(mavlinkConfig);
     this.droneRegistry = registry;
-    this.twinUpdater = new MavlinkTwinUpdater(twinManager, new ListenerManager(twinManager));
+    this.twinUpdater = new MavlinkTwinUpdater(twinManager, new ListenerManager(twinManager), new MavlinkBootstrapRequestPublisher(twinManager, protocol));
   }
 
   MavlinkStateSubscriber(

src/main/java/io/mapsmessaging/state/mavlink/MavlinkTwinUpdater.java

@@ -35,6 +35,7 @@
 import io.mapsmessaging.state.drone.model.DetectionEvent;
 import io.mapsmessaging.state.drone.model.DroneContactManager;
 import io.mapsmessaging.state.mavlink.bootstrap.DroneTwinReadinessEvaluator;
+import io.mapsmessaging.state.mavlink.bootstrap.MavlinkBootstrapEventPublisher;
 import io.mapsmessaging.state.mavlink.bootstrap.MavlinkBootstrapProfile;
 import io.mapsmessaging.state.mavlink.bootstrap.MavlinkBootstrapStateEngine;
 import io.mapsmessaging.state.mavlink.listener.ListenerManager;
@@ -59,9 +60,13 @@ public class MavlinkTwinUpdater implements AutoCloseable {
   private final AtomicBoolean closed;
 
   public MavlinkTwinUpdater(@NonNull @NotNull TwinManager twinManager, @NonNull @NotNull ListenerManager listenerManager) {
+    this(twinManager, listenerManager, (MavlinkBootstrapEventPublisher) null);
+  }
+
+  public MavlinkTwinUpdater(@NonNull @NotNull TwinManager twinManager, @NonNull @NotNull ListenerManager listenerManager, MavlinkBootstrapEventPublisher bootstrapEventPublisher) {
     this.twinManager = twinManager;
     this.listenerManager = listenerManager;
-    this.droneMonitor = new MavlinkDroneMonitor(twinManager, new DroneTwinReadinessEvaluator(), new MavlinkBootstrapStateEngine(new MavlinkBootstrapProfile()), null);
+    this.droneMonitor = new MavlinkDroneMonitor(twinManager, new DroneTwinReadinessEvaluator(), new MavlinkBootstrapStateEngine(new MavlinkBootstrapProfile()), bootstrapEventPublisher);
     this.closed = new AtomicBoolean();
     twinManager.addObserver(droneMonitor);
   }

src/main/java/io/mapsmessaging/state/mavlink/bootstrap/MavlinkBootstrapProfile.java

@@ -49,6 +49,11 @@ public MavlinkBootstrapProfile() {
         AUTOPILOT_VERSION
     );
 
+    addRequestMessage(
+        DroneTwinMissingState.MISSING_CAPABILITIES,
+        AUTOPILOT_VERSION
+    );
+
     addRequestMessage(
         DroneTwinMissingState.MISSING_HOME_POSITION,
         HOME_POSITION
@@ -101,4 +106,4 @@ private void addMessageInterval(
         )
     );
   }
-}
+}

src/main/java/io/mapsmessaging/state/mavlink/bootstrap/MavlinkBootstrapRequestPublisher.java

@@ -0,0 +1,110 @@
+/*
+ *
+ *  Copyright [ 2020 - 2024 ] Matthew Buckton
+ *  Copyright [ 2024 - 2026 ] MapsMessaging B.V.
+ *
+ *  Licensed under the Apache License, Version 2.0 with the Commons Clause
+ *  (the "License"); you may not use this file except in compliance with the License.
+ *  You may obtain a copy of the License at:
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *      https://commonsclause.com/
+ *
+ *  Unless required by applicable law or agreed to in writing, software
+ *  distributed under the License is distributed on an "AS IS" BASIS,
+ *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *  See the License for the specific language governing permissions and
+ *  limitations under the License.
+ */
+
+package io.mapsmessaging.state.mavlink.bootstrap;
+
+import static io.mapsmessaging.state.logging.StateLogMessages.MAVLINK_BOOTSTRAP_REQUEST_FAILED;
+import static io.mapsmessaging.state.logging.StateLogMessages.MAVLINK_BOOTSTRAP_REQUEST_SENT;
+import static io.mapsmessaging.state.logging.StateLogMessages.MAVLINK_BOOTSTRAP_REQUEST_SKIPPED;
+import static io.mapsmessaging.state.mavlink.packet.MavlinkMessageIds.AUTOPILOT_VERSION;
+
+import io.mapsmessaging.api.Destination;
+import io.mapsmessaging.api.MessageBuilder;
+import io.mapsmessaging.api.features.DestinationType;
+import io.mapsmessaging.api.features.QualityOfService;
+import io.mapsmessaging.api.message.Message;
+import io.mapsmessaging.logging.Logger;
+import io.mapsmessaging.logging.LoggerFactory;
+import io.mapsmessaging.state.StateLoopProtocol;
+import io.mapsmessaging.state.drone.core.EntityTwin;
+import io.mapsmessaging.state.drone.core.TwinManager;
+import io.mapsmessaging.state.mavlink.messages.MavlinkCommandLong;
+import io.mapsmessaging.state.mavlink.messages.MavlinkCommandLongFactory;
+import java.io.IOException;
+import java.nio.charset.StandardCharsets;
+import java.util.Optional;
+
+public class MavlinkBootstrapRequestPublisher implements MavlinkBootstrapEventPublisher {
+
+  private final Logger logger = LoggerFactory.getLogger(MavlinkBootstrapRequestPublisher.class);
+  private final TwinManager twinManager;
+  private final StateLoopProtocol protocol;
+
+  public MavlinkBootstrapRequestPublisher(TwinManager twinManager, StateLoopProtocol protocol) {
+    this.twinManager = twinManager;
+    this.protocol = protocol;
+  }
+
+  @Override
+  public void publish(MavlinkBootstrapEvent event) {
+    if (!isAutopilotVersionRequest(event)) {
+      return;
+    }
+
+    Optional<EntityTwin> optionalTwin = twinManager.getTwin(event.getTwinId());
+    if (optionalTwin.isEmpty()) {
+      logger.log(MAVLINK_BOOTSTRAP_REQUEST_SKIPPED, event.getTwinId(), "twin is not registered");
+      return;
+    }
+
+    EntityTwin twin = optionalTwin.get();
+    String responseTopic = twin.getResponseTopicName();
+    String correlationData = twin.getUniqueOutboundIdentifier();
+    if (responseTopic == null || responseTopic.isBlank() || correlationData == null || correlationData.isBlank()) {
+      logger.log(MAVLINK_BOOTSTRAP_REQUEST_SKIPPED, event.getTwinId(), "MAVLink response route is unavailable");
+      return;
+    }
+
+    MavlinkCommandLong request = MavlinkCommandLongFactory.requestMessage(event.getTargetSystem(), event.getTargetComponent(), 0, AUTOPILOT_VERSION);
+    Message message = new MessageBuilder()
+        .setOpaqueData(request.toMavlinkJsonObject().toString().getBytes(StandardCharsets.UTF_8))
+        .setContentType("application/json")
+        .setQoS(QualityOfService.AT_MOST_ONCE)
+        .setCorrelationData(correlationData)
+        .build();
+
+    if (protocol.getSession() == null) {
+      logger.log(MAVLINK_BOOTSTRAP_REQUEST_SKIPPED, event.getTwinId(), "MAVLink state session is unavailable");
+      return;
+    }
+
+    protocol.getSession().findDestination(responseTopic, DestinationType.TOPIC).whenComplete((destination, failure) -> publishToDestination(event, message, destination, failure));
+  }
+
+  private boolean isAutopilotVersionRequest(MavlinkBootstrapEvent event) {
+    return event != null
+        && event.getEventType() == MavlinkBootstrapEventType.REQUEST
+        && event.getRequestType() == MavlinkBootstrapRequestType.REQUEST_MESSAGE
+        && event.getMavlinkMessageId() == AUTOPILOT_VERSION;
+  }
+
+  private void publishToDestination(MavlinkBootstrapEvent event, Message message, Destination destination, Throwable failure) {
+    if (failure != null || destination == null) {
+      logger.log(MAVLINK_BOOTSTRAP_REQUEST_FAILED, failure, event.getMavlinkMessageId(), event.getTwinId());
+      return;
+    }
+
+    try {
+      destination.storeMessage(message);
+      logger.log(MAVLINK_BOOTSTRAP_REQUEST_SENT, event.getMavlinkMessageId(), event.getTargetSystem(), event.getTargetComponent(), event.getTwinId());
+    } catch (IOException exception) {
+      logger.log(MAVLINK_BOOTSTRAP_REQUEST_FAILED, exception, event.getMavlinkMessageId(), event.getTwinId());
+    }
+  }
+}

src/main/java/io/mapsmessaging/state/mavlink/bootstrap/MavlinkBootstrapStateEngine.java

@@ -24,6 +24,7 @@
 
 import java.time.Instant;
 import java.util.ArrayList;
+import java.util.HashSet;
 import java.util.List;
 import java.util.Map;
 import java.util.Set;
@@ -146,6 +147,7 @@ private void handleMissingStates(
       return;
     }
 
+    Set<MavlinkBootstrapRequestDefinition> emittedRequests = new HashSet<>();
     for (DroneTwinMissingState missingState : readinessResult.getMissingStates()) {
       MavlinkBootstrapRequestDefinition requestDefinition =
           bootstrapProfile.getRequestDefinitions().get(missingState);
@@ -174,13 +176,15 @@ private void handleMissingStates(
         continue;
       }
 
-      events.add(
-          createRequestEvent(
-              droneTwin,
-              missingState,
-              requestDefinition
-          )
-      );
+      if (emittedRequests.add(requestDefinition)) {
+        events.add(
+            createRequestEvent(
+                droneTwin,
+                missingState,
+                requestDefinition
+            )
+        );
+      }
 
       requestTracker.markRequested(now);
     }

src/main/java/io/mapsmessaging/state/mavlink/messages/MavlinkCommandLongFactory.java

@@ -30,6 +30,7 @@ public final class MavlinkCommandLongFactory {
   public static final int MAV_CMD_DO_SET_MISSION_CURRENT = 224;
   public static final int MAV_CMD_MISSION_START = 300;
   public static final int MAV_CMD_COMPONENT_ARM_DISARM = 400;
+  public static final int MAV_CMD_REQUEST_MESSAGE = 512;
 
   public static final float ARM = 1.0f;
   public static final float DISARM = 0.0f;
@@ -128,6 +129,12 @@ public static MavlinkCommandLong missionStart(
     return commandLong;
   }
 
+  public static MavlinkCommandLong requestMessage(int targetSystem, int targetComponent, int sequence, int messageId) {
+    MavlinkCommandLong commandLong = command(targetSystem, targetComponent, MAV_CMD_REQUEST_MESSAGE, sequence);
+    commandLong.setParam1(messageId);
+    return commandLong;
+  }
+
   public static MavlinkCommandLong setMissionCurrent(
       int targetSystem,
       int targetComponent,

src/main/java/io/mapsmessaging/rest/api/impl/twins/TwinConfigurationApi.java → src/main/java/io/mapsmessaging/state/rest/twins/TwinConfigurationApi.java

@@ -17,7 +17,7 @@
  *  limitations under the License.
  */
 
-package io.mapsmessaging.rest.api.impl.twins;
+package io.mapsmessaging.state.rest.twins;
 
 import io.mapsmessaging.configuration.ConfigurationProperties;
 import io.mapsmessaging.dto.rest.config.protocol.impl.TakProtocolDTO;

src/main/java/io/mapsmessaging/rest/api/impl/twins/TwinConfigurationStore.java → src/main/java/io/mapsmessaging/state/rest/twins/TwinConfigurationStore.java

@@ -17,7 +17,7 @@
  *  limitations under the License.
  */
 
-package io.mapsmessaging.rest.api.impl.twins;
+package io.mapsmessaging.state.rest.twins;
 
 import io.mapsmessaging.configuration.ConfigurationProperties;
 import io.mapsmessaging.dto.rest.config.protocol.impl.TakProtocolDTO;

src/main/java/io/mapsmessaging/rest/api/impl/twins/TwinCoreConfigDTO.java → src/main/java/io/mapsmessaging/state/rest/twins/TwinCoreConfigDTO.java

@@ -17,7 +17,7 @@
  *  limitations under the License.
  */
 
-package io.mapsmessaging.rest.api.impl.twins;
+package io.mapsmessaging.state.rest.twins;
 
 import io.swagger.v3.oas.annotations.media.Schema;
 

src/main/java/io/mapsmessaging/rest/api/impl/twins/TwinManagementApi.java → src/main/java/io/mapsmessaging/state/rest/twins/TwinManagementApi.java

@@ -17,7 +17,7 @@
  *  limitations under the License.
  */
 
-package io.mapsmessaging.rest.api.impl.twins;
+package io.mapsmessaging.state.rest.twins;
 
 import io.mapsmessaging.MessageDaemon;
 import io.mapsmessaging.rest.api.impl.BaseRestApi;