From 55a689c7b17681b4f76f9877d154de884bfa3d5d Mon Sep 17 00:00:00 2001 From: earocorn Date: Wed, 16 Jul 2025 20:50:08 +0800 Subject: [PATCH 01/11] add some reference code --- .../impl/comm/mavlink2/UnmannedConfig.java | 10 ++++++++++ .../impl/comm/mavlink2/UnmannedSwarm.java | 16 ++++++++++++++++ .../impl/comm/mavlink2/config/MissionConfig.java | 9 +++++++++ .../impl/comm/mavlink2/config/PointConfig.java | 8 ++++++++ .../org.sensorhub.api.module.IModuleProvider | 3 ++- 5 files changed, 45 insertions(+), 1 deletion(-) create mode 100644 other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedSwarm.java create mode 100644 other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/config/MissionConfig.java create mode 100644 other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/config/PointConfig.java diff --git a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedConfig.java b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedConfig.java index 67d2c93..fb0ddab 100644 --- a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedConfig.java +++ b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedConfig.java @@ -13,6 +13,10 @@ import org.sensorhub.api.config.DisplayInfo; import org.sensorhub.api.sensor.SensorConfig; +import org.sensorhub.impl.comm.mavlink2.config.MissionConfig; + +import java.util.ArrayList; +import java.util.List; /** * Configuration settings for the {@link UnmannedSystem} driver exposed via the OpenSensorHub Admin panel. @@ -34,4 +38,10 @@ public class UnmannedConfig extends SensorConfig { @DisplayInfo.Required @DisplayInfo(desc = "Serial number or unique identifier") public String serialNumber = "sensor001"; + + public List stuff; + + public String address; + public int port; + public int systemID; } diff --git a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedSwarm.java b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedSwarm.java new file mode 100644 index 0000000..4f2e3df --- /dev/null +++ b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedSwarm.java @@ -0,0 +1,16 @@ +package org.sensorhub.impl.comm.mavlink2; + +import org.sensorhub.impl.sensor.SensorSystem; + +public class UnmannedSwarm extends SensorSystem { + + public UnmannedSwarm() { + super(); + + getMembers(); + + // Add control input that distributes LLA command among members + addControlInput(); + } + +} diff --git a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/config/MissionConfig.java b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/config/MissionConfig.java new file mode 100644 index 0000000..233fc02 --- /dev/null +++ b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/config/MissionConfig.java @@ -0,0 +1,9 @@ +package org.sensorhub.impl.comm.mavlink2.config; + +import java.util.List; + +public class MissionConfig { + + public List points; + +} diff --git a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/config/PointConfig.java b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/config/PointConfig.java new file mode 100644 index 0000000..4bdfc5a --- /dev/null +++ b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/config/PointConfig.java @@ -0,0 +1,8 @@ +package org.sensorhub.impl.comm.mavlink2.config; + +public class PointConfig { + + public double lat; + public double lon; + +} diff --git a/other-modules/comm/sensorhub-comm-mavlink2/src/main/resources/META-INF/services/org.sensorhub.api.module.IModuleProvider b/other-modules/comm/sensorhub-comm-mavlink2/src/main/resources/META-INF/services/org.sensorhub.api.module.IModuleProvider index 0ea39d3..203dd8e 100644 --- a/other-modules/comm/sensorhub-comm-mavlink2/src/main/resources/META-INF/services/org.sensorhub.api.module.IModuleProvider +++ b/other-modules/comm/sensorhub-comm-mavlink2/src/main/resources/META-INF/services/org.sensorhub.api.module.IModuleProvider @@ -1 +1,2 @@ -org.sensorhub.impl.comm.mavlink2.MavLinkNetworkProvider \ No newline at end of file +org.sensorhub.impl.comm.mavlink2.MavLinkNetworkProvider +org.sensorhub.impl.comm.mavlink2.UnmannedDescriptor \ No newline at end of file From f45dab570bc07ee8b9c2e85d5fb83523e4b7a454 Mon Sep 17 00:00:00 2001 From: earocorn Date: Wed, 16 Jul 2025 20:51:30 +0800 Subject: [PATCH 02/11] edit uid --- .../java/org/sensorhub/impl/comm/mavlink2/UnmannedSystem.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedSystem.java b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedSystem.java index daf8758..20fd193 100644 --- a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedSystem.java +++ b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedSystem.java @@ -23,8 +23,8 @@ * and performing initialization and shutdown for the driver and its outputs. */ public class UnmannedSystem extends AbstractSensorModule { - static final String UID_PREFIX = "urn:osh:template_driver:"; - static final String XML_PREFIX = "TEMPLATE_DRIVER_"; + static final String UID_PREFIX = "urn:osh:driver:mavlink2:"; + static final String XML_PREFIX = "MAVLINK2_DRIVER_"; private static final Logger logger = LoggerFactory.getLogger(UnmannedSystem.class); From 3638ee6f11130d4d936403dea14ca91f9114214f Mon Sep 17 00:00:00 2001 From: Michael Stinson Date: Wed, 16 Jul 2025 10:41:27 -0500 Subject: [PATCH 03/11] Move UnmannedSystem to a separate module to be able to add Mavlink Unmanned System under "Sensors" as an individual sensor driver. --- .../comm/sensorhub-comm-mavlink2/build.gradle | 2 +- .../impl/comm/mavlink2/UnmannedOutput.java | 153 ------------------ .../impl/comm/mavlink2/UnmannedSwarm.java | 2 +- .../impl/comm/mavlink2/UnmannedSystem.java | 146 ++++++++++++++++- .../src/main/resources/config.json | 31 +--- 5 files changed, 151 insertions(+), 183 deletions(-) diff --git a/other-modules/comm/sensorhub-comm-mavlink2/build.gradle b/other-modules/comm/sensorhub-comm-mavlink2/build.gradle index 5c59242..5f05ee1 100644 --- a/other-modules/comm/sensorhub-comm-mavlink2/build.gradle +++ b/other-modules/comm/sensorhub-comm-mavlink2/build.gradle @@ -1,4 +1,4 @@ -description = 'MAVLink Connection' +description = 'Unmanned System' ext.details = 'MAVLink adapter' dependencies{ diff --git a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedOutput.java b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedOutput.java index a9e8997..9f21c52 100644 --- a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedOutput.java +++ b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedOutput.java @@ -13,7 +13,6 @@ import io.mavsdk.action.Action; import io.mavsdk.camera.Camera; -import io.mavsdk.core.Core; import io.mavsdk.telemetry.Telemetry; import net.opengis.swe.v20.DataBlock; import net.opengis.swe.v20.DataComponent; @@ -25,10 +24,8 @@ import java.util.ArrayList; import java.util.concurrent.CountDownLatch; -import java.util.concurrent.TimeUnit; import java.util.concurrent.locks.ReentrantLock; -import static java.lang.Math.abs; /** * UnmannedOutput specification and provider for {@link UnmannedSystem}. @@ -63,9 +60,6 @@ public class UnmannedOutput extends AbstractSensorOutput { static Telemetry.Imu currentImu = null; private final ReentrantLock lock = new ReentrantLock(); - private UnmannedControlLocation unmannedControlLocation = null; - private UnmannedControlTakeoff unmannedControlTakeoff = null; - private UnmannedControlLanding unmannedControlLanding = null; /** * Creates a new output for the sensor driver. @@ -76,16 +70,6 @@ public class UnmannedOutput extends AbstractSensorOutput { super(SENSOR_OUTPUT_NAME, parentSensor); } - void doInit( UnmannedControlLocation location, - UnmannedControlLanding landing, - UnmannedControlTakeoff takeoff ) { - - unmannedControlLocation = location; - unmannedControlLanding = landing; - unmannedControlTakeoff = takeoff; - doInit(); - } - /** * Initializes the data structure for the output, defining the fields, their ordering, and data types. */ @@ -125,8 +109,6 @@ void doInit() { .build(); dataEncoding = sweFactory.newTextEncoding(",", "\n"); - - receiveDrone(); } @Override @@ -441,31 +423,6 @@ public static void printDroneInfo( io.mavsdk.System drone ) { } - /*** - * Currently only handles one drone at a time - */ - private void receiveDrone( ) { - - System.out.println("Listening for drone connection..."); - - io.mavsdk.System drone = new io.mavsdk.System(); - drone.getCore().getConnectionState() - .filter(Core.ConnectionState::getIsConnected) - .firstElement() - .subscribe(state -> { - System.out.println("Drone connection detected."); - - unmannedControlLocation.setSystem(drone); - unmannedControlTakeoff.setSystem(drone); - unmannedControlLanding.setSystem(drone); - parentSensor.unmannedControlOffboard.setSystem(drone); - subscribeTelemetry(drone); - //setUpScenario(drone); - //sendMission(drone); - - }); - } - private static void sendMission( io.mavsdk.System drone ) { drone.getAction().arm() @@ -483,114 +440,4 @@ private static void sendMission( io.mavsdk.System drone ) { }); } - - private static void setUpScenario( io.mavsdk.System drone ) { - - System.out.println("Setting up scenario..."); - - CountDownLatch latch = new CountDownLatch(1); - //downloadLog(drone); - - //subscribeTelemetry(drone); - //printVideoStreamInfo(drone); - - //printParams(drone); - //printHealth(drone); - - //drone.getOffboard(). - - //printTransponderInfo(drone); - - drone.getAction().arm() - .doOnComplete(() -> { - - System.out.println("Arming..."); - - printDroneInfo(drone); - - }) - .doOnError(throwable -> { - - System.out.println("Failed to arm: " + ((Action.ActionException) throwable).getCode()); - - }) - .andThen(drone.getAction().setTakeoffAltitude(homeAltitude)) - .andThen(drone.getAction().takeoff() - .doOnComplete(() -> { - - System.out.println("Taking off..."); - - }) - .doOnError(throwable -> { - - System.out.println("Failed to take off: " + ((Action.ActionException) throwable).getCode()); - - })) - .delay(5, TimeUnit.SECONDS) - .andThen(drone.getTelemetry().getPosition() - .filter(pos -> pos.getRelativeAltitudeM() >= homeAltitude) - .firstElement() - .ignoreElement() - ) - .delay(5, TimeUnit.SECONDS) - .andThen(drone.getAction().gotoLocation(destLatitude, destLongitude, - destAltitude + drone.getTelemetry().getPosition().blockingFirst().getAbsoluteAltitudeM(), - 45.0F) - .doOnComplete( () -> { - - System.out.println("Moving to target location"); - - })) - .doOnError( throwable -> { - - System.out.println("Failed to go to target: " + ((Action.ActionException) throwable).getCode()); - - }) - .andThen(drone.getTelemetry().getPosition() - .filter(pos -> (abs(pos.getLatitudeDeg() - destLatitude) <= deltaSuccess && abs(pos.getLongitudeDeg() - destLongitude) <= deltaSuccess)) - .firstElement() - .ignoreElement() - ) - .delay( 8, TimeUnit.SECONDS ) - .andThen(drone.getAction().gotoLocation(homeLatitude, homeLongitude, - homeAltitude + drone.getTelemetry().getPosition().blockingFirst().getAbsoluteAltitudeM() - , 0.0F)) - .doOnComplete( () -> { - - System.out.println("Moving to landing location"); - - }) - .doOnError( throwable -> { - - System.out.println("Failed to go to landing location: " + ((Action.ActionException) throwable).getCode()); - - }) - .andThen(drone.getTelemetry().getPosition() - .filter(pos -> (abs(pos.getLatitudeDeg() - homeLatitude) <= deltaSuccess && abs(pos.getLongitudeDeg() - homeLongitude) <= deltaSuccess)) - .firstElement() - .ignoreElement() - ) - .andThen(drone.getAction().land().doOnComplete(() -> { - - System.out.println("Landing..."); - - }) - .doOnError(throwable -> { - - System.out.println("Failed to land: " + ((Action.ActionException) throwable).getCode()); - - })) - .subscribe(latch::countDown, throwable -> { - - latch.countDown(); - - }); - - try { - latch.await(); - } catch (InterruptedException ignored) { - // This is expected - } - } - } diff --git a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedSwarm.java b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedSwarm.java index 4f2e3df..f071dbe 100644 --- a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedSwarm.java +++ b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedSwarm.java @@ -10,7 +10,7 @@ public UnmannedSwarm() { getMembers(); // Add control input that distributes LLA command among members - addControlInput(); + //addControlInput(); } } diff --git a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedSystem.java b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedSystem.java index 20fd193..6601de1 100644 --- a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedSystem.java +++ b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedSystem.java @@ -11,11 +11,19 @@ ******************************* END LICENSE BLOCK ***************************/ package org.sensorhub.impl.comm.mavlink2; +import io.mavsdk.action.Action; +import io.mavsdk.core.Core; import org.sensorhub.api.common.SensorHubException; import org.sensorhub.impl.sensor.AbstractSensorModule; import org.slf4j.Logger; import org.slf4j.LoggerFactory; +import java.util.concurrent.CountDownLatch; +import java.util.concurrent.TimeUnit; + +import static java.lang.Math.abs; +import static org.sensorhub.impl.comm.mavlink2.UnmannedOutput.*; + /** * Driver implementation for the sensor. *

@@ -66,12 +74,15 @@ public void doInit() throws SensorHubException { addControlInput(this.unmannedControlOffboard); unmannedControlOffboard.init(); - output.doInit(unmannedControlLocation,unmannedControlLanding, unmannedControlTakeoff); + output.doInit(); } @Override public void doStart() throws SensorHubException { super.doStart(); + + receiveDrone(); + //startProcessing(); } @@ -116,4 +127,137 @@ public void startProcessing() { public void stopProcessing() { doProcessing = false; } + + + private void receiveDrone( ) { + + System.out.println("Listening for drone connection..."); + + io.mavsdk.System drone = new io.mavsdk.System(); + drone.getCore().getConnectionState() + .filter(Core.ConnectionState::getIsConnected) + .firstElement() + .subscribe(state -> { + System.out.println("Drone connection detected."); + + unmannedControlLocation.setSystem(drone); + unmannedControlTakeoff.setSystem(drone); + unmannedControlLanding.setSystem(drone); + output.subscribeTelemetry(drone); + //setUpScenario(drone); + //sendMission(drone); + + }); + } + + + private static void setUpScenario( io.mavsdk.System drone ) { + + System.out.println("Setting up scenario..."); + + CountDownLatch latch = new CountDownLatch(1); + //downloadLog(drone); + + //subscribeTelemetry(drone); + //printVideoStreamInfo(drone); + + //printParams(drone); + //printHealth(drone); + + //drone.getOffboard(). + + //printTransponderInfo(drone); + + drone.getAction().arm() + .doOnComplete(() -> { + + System.out.println("Arming..."); + + printDroneInfo(drone); + + }) + .doOnError(throwable -> { + + System.out.println("Failed to arm: " + ((Action.ActionException) throwable).getCode()); + + }) + .andThen(drone.getAction().setTakeoffAltitude(homeAltitude)) + .andThen(drone.getAction().takeoff() + .doOnComplete(() -> { + + System.out.println("Taking off..."); + + }) + .doOnError(throwable -> { + + System.out.println("Failed to take off: " + ((Action.ActionException) throwable).getCode()); + + })) + .delay(5, TimeUnit.SECONDS) + .andThen(drone.getTelemetry().getPosition() + .filter(pos -> pos.getRelativeAltitudeM() >= homeAltitude) + .firstElement() + .ignoreElement() + ) + .delay(5, TimeUnit.SECONDS) + .andThen(drone.getAction().gotoLocation(destLatitude, destLongitude, + destAltitude + drone.getTelemetry().getPosition().blockingFirst().getAbsoluteAltitudeM(), + 45.0F) + .doOnComplete( () -> { + + System.out.println("Moving to target location"); + + })) + .doOnError( throwable -> { + + System.out.println("Failed to go to target: " + ((Action.ActionException) throwable).getCode()); + + }) + .andThen(drone.getTelemetry().getPosition() + .filter(pos -> (abs(pos.getLatitudeDeg() - destLatitude) <= deltaSuccess && abs(pos.getLongitudeDeg() - destLongitude) <= deltaSuccess)) + .firstElement() + .ignoreElement() + ) + .delay( 8, TimeUnit.SECONDS ) + .andThen(drone.getAction().gotoLocation(homeLatitude, homeLongitude, + homeAltitude + drone.getTelemetry().getPosition().blockingFirst().getAbsoluteAltitudeM() + , 0.0F)) + .doOnComplete( () -> { + + System.out.println("Moving to landing location"); + + }) + .doOnError( throwable -> { + + System.out.println("Failed to go to landing location: " + ((Action.ActionException) throwable).getCode()); + + }) + .andThen(drone.getTelemetry().getPosition() + .filter(pos -> (abs(pos.getLatitudeDeg() - homeLatitude) <= deltaSuccess && abs(pos.getLongitudeDeg() - homeLongitude) <= deltaSuccess)) + .firstElement() + .ignoreElement() + ) + .andThen(drone.getAction().land().doOnComplete(() -> { + + System.out.println("Landing..."); + + }) + .doOnError(throwable -> { + + System.out.println("Failed to land: " + ((Action.ActionException) throwable).getCode()); + + })) + .subscribe(latch::countDown, throwable -> { + + latch.countDown(); + + }); + + try { + latch.await(); + } catch (InterruptedException ignored) { + // This is expected + } + } + } diff --git a/tools/sensorhub-test/src/main/resources/config.json b/tools/sensorhub-test/src/main/resources/config.json index 60fa32d..0042d73 100644 --- a/tools/sensorhub-test/src/main/resources/config.json +++ b/tools/sensorhub-test/src/main/resources/config.json @@ -293,7 +293,7 @@ "zoomMax": 1, "zoomMin": 0 }, - "lastUpdated": "2025-07-12T13:19:21.307+0800", + "lastUpdated": "2025-07-12T00:19:21.307-0500", "id": "f5ffee01-95c4-4cee-bab1-5f558cc3419a", "autoStart": false, "moduleClass": "org.sensorhub.impl.sensor.onvif.OnvifCameraDriver", @@ -307,29 +307,6 @@ "moduleClass": "org.sensorhub.impl.comm.mavlink2.MavLinkCommNetwork", "name": "New MAVLink" }, - { - "objClass": "org.sensorhub.impl.sensor.SensorSystemConfig", - "uniqueID": "8105965d-dcf6-40ad-a2fb-a719083e7054", - "subsystems": [ - { - "objClass": "org.sensorhub.impl.sensor.SensorSystemConfig$SystemMember", - "config": { - "objClass": "org.sensorhub.impl.comm.mavlink2.UnmannedConfig", - "serialNumber": "sensor001", - "lastUpdated": "2025-07-12T13:26:34.806+0800", - "id": "c6fd4095-a96e-4706-ab1a-7f9427f74354", - "autoStart": true, - "moduleClass": "org.sensorhub.impl.comm.mavlink2.UnmannedSystem", - "name": "Unmanned System" - } - } - ], - "lastUpdated": "2025-07-12T13:26:34.787+0800", - "id": "1406a8d8-ccc5-4508-a1ba-be02fd7318cb", - "autoStart": true, - "moduleClass": "org.sensorhub.impl.sensor.SensorSystem", - "name": "MAVLink Connected Systems" - }, { "objClass": "org.sensorhub.impl.processing.SMLProcessConfig", "sensorML": "dist/processes/drone-location-to-ptz-onvif.xml", @@ -345,7 +322,7 @@ "controllerLayerConfig": { "objClass": "com.botts.impl.sensor.universalcontroller.ControllerLayerConfig", "presets": [], - "lastUpdated": "2025-07-13T15:05:06.115+0800", + "lastUpdated": "2025-07-13T02:05:06.115-0500", "autoStart": false }, "numControlStreams": 0, @@ -355,7 +332,7 @@ "GAMEPAD" ], "controllerSearchTime": 15, - "lastUpdated": "2025-07-13T15:05:06.114+0800", + "lastUpdated": "2025-07-13T02:05:06.114-0500", "id": "e5a64e1f-9011-47ec-b52e-ae47f8fe52bd", "autoStart": false, "moduleClass": "com.botts.impl.sensor.universalcontroller.UniversalControllerSensor", @@ -422,7 +399,7 @@ }, "enableH264": false, "enableMJPEG": true, - "lastUpdated": "2025-07-14T13:20:46.221+0800", + "lastUpdated": "2025-07-14T00:20:46.221-0500", "id": "b4f99362-def9-4875-8dca-e654776f8866", "autoStart": false, "moduleClass": "org.sensorhub.impl.sensor.axis.AxisCameraDriver", From 84b875a098691b10a128df5bd93f786fa7c15f69 Mon Sep 17 00:00:00 2001 From: Michael Stinson Date: Wed, 16 Jul 2025 11:03:58 -0500 Subject: [PATCH 04/11] Add SDK Address and SDK Port, this is the connection to mavsdk_server from the java module. eg: "127.0.0.1" and port 50051. To set the drone port from there we will first have to enable starting mavsdk_server from within our Java driver. --- .../org/sensorhub/impl/comm/mavlink2/UnmannedConfig.java | 6 +++--- .../org/sensorhub/impl/comm/mavlink2/UnmannedSystem.java | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedConfig.java b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedConfig.java index fb0ddab..c80e916 100644 --- a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedConfig.java +++ b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedConfig.java @@ -41,7 +41,7 @@ public class UnmannedConfig extends SensorConfig { public List stuff; - public String address; - public int port; - public int systemID; + public String SDKAddress = "127.0.0.1"; + public int SDKPort = 50051; + public String systemId; } diff --git a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedSystem.java b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedSystem.java index 6601de1..420de90 100644 --- a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedSystem.java +++ b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedSystem.java @@ -133,7 +133,7 @@ private void receiveDrone( ) { System.out.println("Listening for drone connection..."); - io.mavsdk.System drone = new io.mavsdk.System(); + io.mavsdk.System drone = new io.mavsdk.System(config.SDKAddress, config.SDKPort); drone.getCore().getConnectionState() .filter(Core.ConnectionState::getIsConnected) .firstElement() From 207c04f42988c7f257c3e05b31fc4fb765423892 Mon Sep 17 00:00:00 2001 From: Michael Stinson Date: Wed, 16 Jul 2025 13:06:45 -0500 Subject: [PATCH 05/11] First attempt at adding Shell Command control stream to UnmannedSystem MAV driver. It doesn't work. Suspect that the command long target system and target component field aren't filled out during the command_long call --- .../comm/mavlink2/UnmannedControlShell.java | 158 ++++++++++++++++++ .../impl/comm/mavlink2/UnmannedSystem.java | 6 + 2 files changed, 164 insertions(+) create mode 100644 other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedControlShell.java diff --git a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedControlShell.java b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedControlShell.java new file mode 100644 index 0000000..86b5d98 --- /dev/null +++ b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedControlShell.java @@ -0,0 +1,158 @@ +/***************************** BEGIN LICENSE BLOCK *************************** + + The contents of this file are subject to the Mozilla Public License, v. 2.0. + If a copy of the MPL was not distributed with this file, You can obtain one + at http://mozilla.org/MPL/2.0/. + + Software distributed under the License is distributed on an "AS IS" basis, + WITHOUT WARRANTY OF ANY KIND, either express or implied. See the License + for the specific language governing rights and limitations under the License. + + The Initial Developer is Botts Innovative Research Inc. Portions created by the Initial + Developer are Copyright (C) 2025 the Initial Developer. All Rights Reserved. + + ******************************* END LICENSE BLOCK ***************************/ + + +package org.sensorhub.impl.comm.mavlink2; + +import io.mavsdk.shell.Shell; +import net.opengis.swe.v20.DataBlock; +import net.opengis.swe.v20.DataComponent; +import net.opengis.swe.v20.DataRecord; +import org.sensorhub.api.command.CommandException; +import org.sensorhub.impl.sensor.AbstractSensorControl; +import org.vast.swe.helper.GeoPosHelper; + + +/** + *

+ * This particular class provides control stream capabilities + *

+ * + * @author Michael Stinson + * @since Jul 2025 + */ +public class UnmannedControlShell extends AbstractSensorControl +{ + private DataRecord commandDataStruct; + + /** + * Name of the control + */ + private static final String SENSOR_CONTROL_NAME = "UnmannedControlShell"; + + /** + * Label for the control + */ + private static final String SENSOR_CONTROL_LABEL = "System Shell Control"; + + /** + * Control description + */ + private static final String SENSOR_CONTROL_DESCRIPTION = + "Interfaces with MAVLINK and OSH to send System Shell Commands to the platform"; + + /** + * ROS Node name assigned at creation + */ + private static final String NODE_NAME_STR = "/SensorHub/spot/shell_control"; + + private io.mavsdk.System system = null; + + private static final String CMD_EXIT = "exit"; + + public UnmannedControlShell( UnmannedSystem parentSensor) { + super("mavShellControl", parentSensor); + } + + + @Override + public DataComponent getCommandDescription() { + return commandDataStruct; + } + + public void setSystem( io.mavsdk.System systemParam ) { + system = systemParam; + } + + public void init() { + + GeoPosHelper factory = new GeoPosHelper(); + + commandDataStruct = factory.createRecord() + .name(SENSOR_CONTROL_NAME) + .label(SENSOR_CONTROL_LABEL) + .description(SENSOR_CONTROL_DESCRIPTION) + .addField("command", factory.createText().value("")) //land + .build(); + } + + + @Override + protected boolean execCommand(DataBlock command) throws CommandException { + String shellCommand = command.getStringValue(0); + + System.out.println("Command received - Shell Command: " + command.getStringValue(0)); + + if ( system != null ) { + sendShellCommand( shellCommand ); + } else { + throw new CommandException("Unmanned System not initialized"); + } + + return true; + } + + + private void sendShellCommand( String shellCommand ) { + + System.out.println("Sending Command..."); + + // Logging shell response + system.getShell().getReceive() + .subscribe( throwable -> { + throw new CommandException(throwable); + }); + + + system.getShell().send("command long 21 0 0 0 0 0 0 0" + "\n") + .doOnError(throwable -> { + System.out.println("Error during shell"); + throw new CommandException(throwable.getMessage()); + }) + .subscribe(() -> { }, throwable -> { + System.out.println("Failed to send: " + + ((Shell.ShellException) throwable).getCode()); + throw new CommandException(throwable.getMessage()); + }); + + //var test = MAV_CMD_NAV_LAND; + //example: To Land: + //message COMMAND_LONG 0 0 176 0 1 6 0 0 0 0 0 + +// // Reading and sending command +// while (true) { +// +// if (shellCommand == null || shellCommand.isEmpty()) { +// continue; +// } +// if (shellCommand.equalsIgnoreCase(CMD_EXIT)) { +// break; +// } +// +// system.getShell().send(shellCommand) +// .subscribe(() -> { }, throwable -> { +// System.out.println("Failed to send: " +// + ((Shell.ShellException) throwable).getCode()); +// throw new CommandException(throwable.getMessage()); +// }); +// } + } + + + public void stop() { + // TODO Auto-generated method stub + } +} + diff --git a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedSystem.java b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedSystem.java index 420de90..0f4f1eb 100644 --- a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedSystem.java +++ b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedSystem.java @@ -44,6 +44,7 @@ public class UnmannedSystem extends AbstractSensorModule { UnmannedControlLocation unmannedControlLocation; UnmannedControlLanding unmannedControlLanding; UnmannedControlOffboard unmannedControlOffboard; + UnmannedControlShell unmannedControlShell; @Override public void doInit() throws SensorHubException { @@ -74,6 +75,10 @@ public void doInit() throws SensorHubException { addControlInput(this.unmannedControlOffboard); unmannedControlOffboard.init(); + this.unmannedControlShell = new UnmannedControlShell(this); + addControlInput(this.unmannedControlShell); + unmannedControlShell.init(); + output.doInit(); } @@ -143,6 +148,7 @@ private void receiveDrone( ) { unmannedControlLocation.setSystem(drone); unmannedControlTakeoff.setSystem(drone); unmannedControlLanding.setSystem(drone); + unmannedControlShell.setSystem(drone); output.subscribeTelemetry(drone); //setUpScenario(drone); //sendMission(drone); From e9f1ad3071477335b9ea04c708c60a4dae9cb208 Mon Sep 17 00:00:00 2001 From: Michael Stinson Date: Wed, 16 Jul 2025 13:57:49 -0500 Subject: [PATCH 06/11] Enable "Return to Start" and "Hover Seconds" command parameters to Location Control Stream. --- .../mavlink2/UnmannedControlLocation.java | 23 ++++++++++++++++--- 1 file changed, 20 insertions(+), 3 deletions(-) diff --git a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedControlLocation.java b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedControlLocation.java index c3642d0..a247b9a 100644 --- a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedControlLocation.java +++ b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedControlLocation.java @@ -17,6 +17,7 @@ Developer are Copyright (C) 2025 the Initial Developer. All Rights Reserved. package org.sensorhub.impl.comm.mavlink2; import io.mavsdk.action.Action; +import io.reactivex.Completable; import net.opengis.swe.v20.*; import org.sensorhub.api.command.CommandException; import org.sensorhub.impl.sensor.AbstractSensorControl; @@ -154,6 +155,9 @@ private void sendToLocation( double latitudeParam, float altMsl = (float)altAglParam + terrainOffset; System.out.println("setting altitude MSL to: " + altMsl); + double homeLat = system.getTelemetry().getPosition().blockingFirst().getLatitudeDeg(); + double homeLon = system.getTelemetry().getPosition().blockingFirst().getLongitudeDeg(); + system.getAction().gotoLocation(latitudeParam, longitudeParam, altMsl, 45.0F) .doOnComplete( () -> { @@ -169,10 +173,23 @@ private void sendToLocation( double latitudeParam, .filter(pos -> (abs(pos.getLatitudeDeg() - latitudeParam) <= deltaSuccess && abs(pos.getLongitudeDeg() - longitudeParam) <= deltaSuccess)) .firstElement() .ignoreElement() + .delay(hoverSecondsParam, TimeUnit.SECONDS) //hover if the value is marked + .andThen( Completable.fromAction(() -> { + + //return home if the value is checked + if ( returnHomeParam ) { + system.getAction().gotoLocation(homeLat, homeLon, altMsl, 45.0F) + .doOnComplete(() -> { + System.out.println("Moving back to home"); + }) + .doOnError(throwable -> { + System.out.println("Failed to go home: " + ((Action.ActionException) throwable).getCode()); + }) + .subscribe(); + } + })) ) - .subscribe(() -> { - - }); + .subscribe(); } From b3830d60521d8d60444459ebca1edae928925191 Mon Sep 17 00:00:00 2001 From: Michael Stinson Date: Wed, 16 Jul 2025 17:43:12 -0500 Subject: [PATCH 07/11] Enable Unmanned Mission support through the configuration and the control stream. --- .../impl/comm/mavlink2/UnmannedConfig.java | 2 +- .../mavlink2/UnmannedControlLocation.java | 58 ++-- .../comm/mavlink2/UnmannedControlMission.java | 247 ++++++++++++++++++ .../impl/comm/mavlink2/UnmannedSystem.java | 7 + .../comm/mavlink2/config/PointConfig.java | 6 +- .../src/main/resources/config.json | 39 +++ 6 files changed, 332 insertions(+), 27 deletions(-) create mode 100644 other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedControlMission.java diff --git a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedConfig.java b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedConfig.java index c80e916..4865a94 100644 --- a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedConfig.java +++ b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedConfig.java @@ -39,7 +39,7 @@ public class UnmannedConfig extends SensorConfig { @DisplayInfo(desc = "Serial number or unique identifier") public String serialNumber = "sensor001"; - public List stuff; + public List missions; public String SDKAddress = "127.0.0.1"; public int SDKPort = 50051; diff --git a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedControlLocation.java b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedControlLocation.java index a247b9a..a2b9299 100644 --- a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedControlLocation.java +++ b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedControlLocation.java @@ -64,7 +64,7 @@ public class UnmannedControlLocation extends AbstractSensorControl { + return goLocation(latitudeParam, longitudeParam, returnHomeParam, hoverSecondsParam, altMsl, homeLat, homeLon); + } + + + public Completable goLocation( double latitudeParam, double longitudeParam, boolean returnHomeParam, long hoverSecondsParam, float altMsl, double homeLat, double homeLon ) { + return system.getAction().gotoLocation(latitudeParam, longitudeParam, altMsl, 45.0F) + .doOnComplete(() -> { System.out.println("Moving to target location"); }) - .doOnError( throwable -> { + .doOnError(throwable -> { System.out.println("Failed to go to target: " + ((Action.ActionException) throwable).getCode()); @@ -174,10 +167,10 @@ private void sendToLocation( double latitudeParam, .firstElement() .ignoreElement() .delay(hoverSecondsParam, TimeUnit.SECONDS) //hover if the value is marked - .andThen( Completable.fromAction(() -> { + .andThen(Completable.fromAction(() -> { //return home if the value is checked - if ( returnHomeParam ) { + if (returnHomeParam) { system.getAction().gotoLocation(homeLat, homeLon, altMsl, 45.0F) .doOnComplete(() -> { System.out.println("Moving back to home"); @@ -188,9 +181,26 @@ private void sendToLocation( double latitudeParam, .subscribe(); } })) - ) - .subscribe(); + ); + } + + + public float getAltMsl( float altAglParam ) { + float absoluteMsl = system.getTelemetry().getPosition().blockingFirst().getAbsoluteAltitudeM(); + System.out.println("absolute MSL (m): " + absoluteMsl); + + float relativeAltitude = system.getTelemetry().getPosition().blockingFirst().getRelativeAltitudeM(); + System.out.println("current Relative MSL (m): " + relativeAltitude); + float terrainOffset = absoluteMsl - relativeAltitude; + System.out.println("terrain offset: " + terrainOffset); + + // We can use the last + // known relative altitude to set an approximate MSL. This will sometimes not be adequate as the + // terrain will change. For now it's good for a quick demo, but we'll want to eventually + // constantly adjust based on terrain altitude. + float altMsl = altAglParam + terrainOffset; + return altMsl; } diff --git a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedControlMission.java b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedControlMission.java new file mode 100644 index 0000000..3e90a90 --- /dev/null +++ b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedControlMission.java @@ -0,0 +1,247 @@ +/***************************** BEGIN LICENSE BLOCK *************************** + + The contents of this file are subject to the Mozilla Public License, v. 2.0. + If a copy of the MPL was not distributed with this file, You can obtain one + at http://mozilla.org/MPL/2.0/. + + Software distributed under the License is distributed on an "AS IS" basis, + WITHOUT WARRANTY OF ANY KIND, either express or implied. See the License + for the specific language governing rights and limitations under the License. + + The Initial Developer is Botts Innovative Research Inc. Portions created by the Initial + Developer are Copyright (C) 2025 the Initial Developer. All Rights Reserved. + + ******************************* END LICENSE BLOCK ***************************/ + + +package org.sensorhub.impl.comm.mavlink2; + +import io.mavsdk.action.Action; +import io.mavsdk.mission.Mission; +import io.reactivex.Completable; +import net.opengis.swe.v20.DataBlock; +import net.opengis.swe.v20.DataComponent; +import net.opengis.swe.v20.DataRecord; +import org.sensorhub.api.command.CommandException; +import org.sensorhub.impl.comm.mavlink2.config.MissionConfig; +import org.sensorhub.impl.comm.mavlink2.config.PointConfig; +import org.sensorhub.impl.sensor.AbstractSensorControl; +import org.vast.swe.helper.GeoPosHelper; + +import java.util.ArrayList; +import java.util.List; + +import static java.lang.Math.abs; + +/** + *

+ * This particular class provides control stream capabilities + *

+ * + * @author Michael Stinson + * @since Jul 2025 + */ +public class UnmannedControlMission extends AbstractSensorControl +{ + private DataRecord commandDataStruct; + + /** + * Name of the control + */ + private static final String SENSOR_CONTROL_NAME = "UnmannedControlMission"; + + /** + * Label for the control + */ + private static final String SENSOR_CONTROL_LABEL = "Mission Control"; + + /** + * Control description + */ + private static final String SENSOR_CONTROL_DESCRIPTION = + "Interfaces with MAVLINK and OSH to effectuate mission control over the platform"; + + /** + * ROS Node name assigned at creation + */ + private static final String NODE_NAME_STR = "/SensorHub/spot/mission_control"; + + private io.mavsdk.System system = null; + + private UnmannedControlLocation locationControl = null; + + static double deltaSuccess = 0.000003; //distance from lat/lon to determine success + + public UnmannedControlMission( UnmannedSystem parentSensor) { + super("mavMissionControl", parentSensor); + } + + + @Override + public DataComponent getCommandDescription() { + return commandDataStruct; + } + + public void setSystem( io.mavsdk.System systemParam ) { + system = systemParam; + } + + public void setLocationControl( UnmannedControlLocation locationControlParam ) { + locationControl = locationControlParam; + } + + public void init() { + + GeoPosHelper factory = new GeoPosHelper(); + + commandDataStruct = factory.createRecord() + .name(SENSOR_CONTROL_NAME) + .label(SENSOR_CONTROL_LABEL) + .description(SENSOR_CONTROL_DESCRIPTION) + .addField("mission", factory.createCount().value(1)) + .addField( "returnToStart", factory.createBoolean().value(false)) + .build(); + } + + + @Override + protected boolean execCommand(DataBlock command) throws CommandException { + int missionNo = command.getIntValue(0); + boolean returnToStart = command.getBooleanValue(1); + + if ( system != null ) { + mission(missionNo, returnToStart); + } else { + throw new CommandException("Unmanned System not initialized"); + } + + return true; + } + + + //Get the configuration + //build a mission for the drone + //send the mission via mavlink + + + /** + * Currently missions through MAVSDK appear to not work via ArduPilot SITL. For now + * lets control the mission directly + * @param missionNumberParam + */ + private void mission( int missionNumberParam, boolean returnToStartParam ) { + + var sensor = this.getParentProducer(); + var config = sensor.getConfiguration(); + MissionConfig mission = config.missions.get(missionNumberParam-1); + + List finalMission = new ArrayList<>(); + + double homeLat = system.getTelemetry().getPosition().blockingFirst().getLatitudeDeg(); + double homeLon = system.getTelemetry().getPosition().blockingFirst().getLongitudeDeg(); + double homeAGL = mission.points.get(0).altitudeAGL; + + double altMsl = locationControl.getAltMsl((float)homeAGL); + + Completable missionChain = Completable.concat( + mission.points.stream() + //.map(point -> locationControl.goLocation(point.latitude, point.longitude, false, (long) point.hoverSeconds, (float)altMsl ) + .map(point -> locationControl.goLocation(point.latitude,point.longitude,false,(long)point.hoverSeconds,(float)altMsl,homeLat, homeLon) + .doOnError(throwable -> { + System.out.println("Failed to go to mission target point: " + ((Action.ActionException) throwable).getCode()); + }) + .andThen(system.getTelemetry().getPosition() + .filter(pos -> (abs(pos.getLatitudeDeg() - point.latitude) <= deltaSuccess && + abs(pos.getLongitudeDeg() - point.longitude) <= deltaSuccess)) + .firstElement() + .ignoreElement()) + ) + .toList() + ); + + missionChain.subscribe( + () -> { + System.out.println("Mission complete"); + if ( returnToStartParam ) { + locationControl.goLocation(homeLat, homeLon,false,0,(float)altMsl,homeLat, homeLon) + .subscribe(); + } + }, + throwable -> System.out.println("Mission failed: " + throwable) + ); + + //Re-enable when we get direct ArduPilot SITL missions working. + //it appears to now show UNSUPPORTED. Needs further investigation. +// System.out.println("Running mission..."); +// +// Mission.MissionPlan missionPlan = new Mission.MissionPlan(finalMission); +// System.out.println("About to upload " + finalMission.size() + " mission items"); +// +// CountDownLatch latch = new CountDownLatch(1); +// system.getMission() +// .setReturnToLaunchAfterMission(true) +// .andThen(system.getMission().uploadMission(missionPlan) +// .doOnComplete(() -> System.out.println("Upload succeeded")) +// .doOnError(e -> { +// System.out.println("error: " + e.getMessage() + " "); +// e.printStackTrace(); +// })) +// .andThen((CompletableSource) cs -> latch.countDown()) +// .subscribe(); +// +// try { +// latch.await(); +// } catch (InterruptedException ignored) { +// // This is expected +// } + } + + + public void stop() { + // TODO Auto-generated method stub + } + + + /** + * Currently missions through MAVSDK appear to not work via ArduPilot SITL. For now + * lets control the mission directly + * @param latitudeDeg + * @param longitudeDeg + * @param hoverSecondsParam + * @return + */ + public static Mission.MissionItem generateMissionItem( double latitudeDeg, double longitudeDeg, double hoverSecondsParam ) { + + //MissionItem(java.lang.Double latitudeDeg, + // java.lang.Double longitudeDeg, + // java.lang.Float relativeAltitudeM, + // java.lang.Float speedMS, + // java.lang.Boolean isFlyThrough, + // java.lang.Float gimbalPitchDeg, + // java.lang.Float gimbalYawDeg, + // io.mavsdk.mission.Mission.MissionItem.CameraAction cameraAction, + // java.lang.Float loiterTimeS, + // java.lang.Double cameraPhotoIntervalS, + // java.lang.Float acceptanceRadiusM, + // java.lang.Float yawDeg, + // java.lang.Float cameraPhotoDistanceM, + // io.mavsdk.mission.Mission.MissionItem.VehicleAction vehicleAction) + + return new Mission.MissionItem( + latitudeDeg, + longitudeDeg, + 20f, + 1.0f, + true, + 0f, + 0f, + Mission.MissionItem.CameraAction.NONE, + (float)hoverSecondsParam, + 1.0, + 0f, + 0f, + 0f, + Mission.MissionItem.VehicleAction.NONE); + } +} + diff --git a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedSystem.java b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedSystem.java index 0f4f1eb..12c83aa 100644 --- a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedSystem.java +++ b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedSystem.java @@ -43,6 +43,7 @@ public class UnmannedSystem extends AbstractSensorModule { UnmannedControlTakeoff unmannedControlTakeoff; UnmannedControlLocation unmannedControlLocation; UnmannedControlLanding unmannedControlLanding; + UnmannedControlMission unmannedControlMission; UnmannedControlOffboard unmannedControlOffboard; UnmannedControlShell unmannedControlShell; @@ -71,6 +72,11 @@ public void doInit() throws SensorHubException { addControlInput(this.unmannedControlLanding); unmannedControlLanding.init(); + this.unmannedControlMission = new UnmannedControlMission(this); + addControlInput(this.unmannedControlMission); + unmannedControlMission.setLocationControl(unmannedControlLocation); + unmannedControlMission.init(); + this.unmannedControlOffboard = new UnmannedControlOffboard(this); addControlInput(this.unmannedControlOffboard); unmannedControlOffboard.init(); @@ -148,6 +154,7 @@ private void receiveDrone( ) { unmannedControlLocation.setSystem(drone); unmannedControlTakeoff.setSystem(drone); unmannedControlLanding.setSystem(drone); + unmannedControlMission.setSystem(drone); unmannedControlShell.setSystem(drone); output.subscribeTelemetry(drone); //setUpScenario(drone); diff --git a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/config/PointConfig.java b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/config/PointConfig.java index 4bdfc5a..b38a266 100644 --- a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/config/PointConfig.java +++ b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/config/PointConfig.java @@ -2,7 +2,9 @@ public class PointConfig { - public double lat; - public double lon; + public double latitude; + public double longitude; + public double altitudeAGL; + public double hoverSeconds; } diff --git a/tools/sensorhub-test/src/main/resources/config.json b/tools/sensorhub-test/src/main/resources/config.json index 0042d73..5a0ba5d 100644 --- a/tools/sensorhub-test/src/main/resources/config.json +++ b/tools/sensorhub-test/src/main/resources/config.json @@ -464,5 +464,44 @@ "autoStart": false, "moduleClass": "org.sensorhub.impl.processing.SMLProcessImpl", "name": "Controller PTZ" + }, + { + "objClass": "org.sensorhub.impl.comm.mavlink2.UnmannedConfig", + "serialNumber": "sensor001", + "missions": [ + { + "objClass": "org.sensorhub.impl.comm.mavlink2.config.MissionConfig", + "points": [ + { + "objClass": "org.sensorhub.impl.comm.mavlink2.config.PointConfig", + "latitude": 24.18097034, + "longitude": 120.64360321, + "altitudeAGL": 15.0, + "hoverSeconds": 3.0 + }, + { + "objClass": "org.sensorhub.impl.comm.mavlink2.config.PointConfig", + "latitude": 24.18163587, + "longitude": 120.64138286, + "altitudeAGL": 15.0, + "hoverSeconds": 1.0 + }, + { + "objClass": "org.sensorhub.impl.comm.mavlink2.config.PointConfig", + "latitude": 24.17945118, + "longitude": 120.64204897, + "altitudeAGL": 10.0, + "hoverSeconds": 3.0 + } + ] + } + ], + "SDKAddress": "127.0.0.1", + "SDKPort": 50051, + "lastUpdated": "2025-07-16T15:24:07.403-0500", + "id": "1fa37bed-c763-402f-b8b3-29d33b58489d", + "autoStart": false, + "moduleClass": "org.sensorhub.impl.comm.mavlink2.UnmannedSystem", + "name": "New Unmanned System" } ] \ No newline at end of file From e6bf70a44b03ac187057297b5d1c63f531a3888a Mon Sep 17 00:00:00 2001 From: Michael Stinson Date: Wed, 16 Jul 2025 17:50:49 -0500 Subject: [PATCH 08/11] Fix config back to orig --- .../src/main/resources/config.json | 39 ------------------- 1 file changed, 39 deletions(-) diff --git a/tools/sensorhub-test/src/main/resources/config.json b/tools/sensorhub-test/src/main/resources/config.json index 5a0ba5d..0042d73 100644 --- a/tools/sensorhub-test/src/main/resources/config.json +++ b/tools/sensorhub-test/src/main/resources/config.json @@ -464,44 +464,5 @@ "autoStart": false, "moduleClass": "org.sensorhub.impl.processing.SMLProcessImpl", "name": "Controller PTZ" - }, - { - "objClass": "org.sensorhub.impl.comm.mavlink2.UnmannedConfig", - "serialNumber": "sensor001", - "missions": [ - { - "objClass": "org.sensorhub.impl.comm.mavlink2.config.MissionConfig", - "points": [ - { - "objClass": "org.sensorhub.impl.comm.mavlink2.config.PointConfig", - "latitude": 24.18097034, - "longitude": 120.64360321, - "altitudeAGL": 15.0, - "hoverSeconds": 3.0 - }, - { - "objClass": "org.sensorhub.impl.comm.mavlink2.config.PointConfig", - "latitude": 24.18163587, - "longitude": 120.64138286, - "altitudeAGL": 15.0, - "hoverSeconds": 1.0 - }, - { - "objClass": "org.sensorhub.impl.comm.mavlink2.config.PointConfig", - "latitude": 24.17945118, - "longitude": 120.64204897, - "altitudeAGL": 10.0, - "hoverSeconds": 3.0 - } - ] - } - ], - "SDKAddress": "127.0.0.1", - "SDKPort": 50051, - "lastUpdated": "2025-07-16T15:24:07.403-0500", - "id": "1fa37bed-c763-402f-b8b3-29d33b58489d", - "autoStart": false, - "moduleClass": "org.sensorhub.impl.comm.mavlink2.UnmannedSystem", - "name": "New Unmanned System" } ] \ No newline at end of file From 7ecd9111e8470d1f4b1643dd13b4afa5712bed06 Mon Sep 17 00:00:00 2001 From: Michael Stinson Date: Wed, 16 Jul 2025 21:01:24 -0500 Subject: [PATCH 09/11] Fix first through the door bug where only a single Unmanned System can be added and the first one can control the drone. --- .../org/sensorhub/impl/comm/mavlink2/MavLinkCommNetwork.java | 1 + 1 file changed, 1 insertion(+) diff --git a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/MavLinkCommNetwork.java b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/MavLinkCommNetwork.java index 44abc96..cf4dddd 100644 --- a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/MavLinkCommNetwork.java +++ b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/MavLinkCommNetwork.java @@ -120,6 +120,7 @@ private void registerModule() { unmannedConfig.name = "Unmanned System"; unmannedConfig.autoStart = true; unmannedConfig.moduleClass = UnmannedSystem.class.getCanonicalName(); + unmannedConfig.serialNumber = UUID.randomUUID().toString(); try { From b249cc527bdaa6719e3b457d93921a3b1e91b118 Mon Sep 17 00:00:00 2001 From: Michael Stinson Date: Wed, 16 Jul 2025 21:17:11 -0500 Subject: [PATCH 10/11] Fix to allow multiple Unmanned Systems to be added manually under "Sensors". For now just works for a single drone. --- .../java/org/sensorhub/impl/comm/mavlink2/UnmannedConfig.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedConfig.java b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedConfig.java index 4865a94..49f0ef0 100644 --- a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedConfig.java +++ b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedConfig.java @@ -17,6 +17,7 @@ import java.util.ArrayList; import java.util.List; +import java.util.UUID; /** * Configuration settings for the {@link UnmannedSystem} driver exposed via the OpenSensorHub Admin panel. @@ -37,7 +38,7 @@ public class UnmannedConfig extends SensorConfig { */ @DisplayInfo.Required @DisplayInfo(desc = "Serial number or unique identifier") - public String serialNumber = "sensor001"; + public String serialNumber = UUID.randomUUID().toString();; public List missions; From 2b0d71fb6e9f4dfbc9edd680313073b3e3a1025e Mon Sep 17 00:00:00 2001 From: micstinso Date: Thu, 28 Aug 2025 09:39:01 -0500 Subject: [PATCH 11/11] Fix issue where telemetry doesn't come in when no GPS or Velocity is found --- .../impl/comm/mavlink2/UnmannedOutput.java | 81 +++++++++++++------ 1 file changed, 58 insertions(+), 23 deletions(-) diff --git a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedOutput.java b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedOutput.java index 9f21c52..1921341 100644 --- a/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedOutput.java +++ b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedOutput.java @@ -144,38 +144,73 @@ public void setData( long timestamp ) { int index = 0; // Populate the data block - if ( currentPosition != null && currentVelocity != null && currentImu != null ) { + if ( true ) { dataBlock.setDoubleValue(index++, timestamp / 1000d); //Location - dataBlock.setDoubleValue(index++, currentPosition.getLatitudeDeg()); - dataBlock.setDoubleValue(index++, currentPosition.getLongitudeDeg()); - dataBlock.setDoubleValue(index++, currentPosition.getAbsoluteAltitudeM()); + if ( null != currentPosition ) { + dataBlock.setDoubleValue(index++, currentPosition.getLatitudeDeg()); + dataBlock.setDoubleValue(index++, currentPosition.getLongitudeDeg()); + dataBlock.setDoubleValue(index++, currentPosition.getAbsoluteAltitudeM()); + } else { + dataBlock.setDoubleValue(index++, 0.0); + dataBlock.setDoubleValue(index++, 0.0); + dataBlock.setDoubleValue(index++, 0.0); + } // Orientation - dataBlock.setDoubleValue(index++, currentAttitude.getYawDeg()); - dataBlock.setDoubleValue(index++, currentAttitude.getPitchDeg()); - dataBlock.setDoubleValue(index++, currentAttitude.getRollDeg()); + if ( null != currentAttitude ) { + dataBlock.setDoubleValue(index++, currentAttitude.getYawDeg()); + dataBlock.setDoubleValue(index++, currentAttitude.getPitchDeg()); + dataBlock.setDoubleValue(index++, currentAttitude.getRollDeg()); + } else { + dataBlock.setDoubleValue(index++, 0.0); + dataBlock.setDoubleValue(index++, 0.0); + dataBlock.setDoubleValue(index++, 0.0); + } //Velocity - dataBlock.setDoubleValue(index++, currentVelocity.getNorthMS()); - dataBlock.setDoubleValue(index++, currentVelocity.getEastMS()); - dataBlock.setDoubleValue(index++, currentVelocity.getDownMS()); + if ( null != currentVelocity ) { + dataBlock.setDoubleValue(index++, currentVelocity.getNorthMS()); + dataBlock.setDoubleValue(index++, currentVelocity.getEastMS()); + dataBlock.setDoubleValue(index++, currentVelocity.getDownMS()); + } else { + dataBlock.setDoubleValue(index++, 0.0); + dataBlock.setDoubleValue(index++, 0.0); + dataBlock.setDoubleValue(index++, 0.0); + } //Acceleration - dataBlock.setDoubleValue(index++, currentImu.getAccelerationFrd().getForwardMS2()); - dataBlock.setDoubleValue(index++, currentImu.getAccelerationFrd().getRightMS2()); - dataBlock.setDoubleValue(index++, currentImu.getAccelerationFrd().getDownMS2()); - //Angular Velocity - dataBlock.setDoubleValue(index++, currentImu.getAngularVelocityFrd().getForwardRadS()); - dataBlock.setDoubleValue(index++, currentImu.getAngularVelocityFrd().getRightRadS()); - dataBlock.setDoubleValue(index++, currentImu.getAngularVelocityFrd().getDownRadS()); - //Temperature - dataBlock.setDoubleValue(index++, currentImu.getTemperatureDegc()); - //Magnetic Field - dataBlock.setDoubleValue(index++, currentImu.getMagneticFieldFrd().getForwardGauss()); - dataBlock.setDoubleValue(index++, currentImu.getMagneticFieldFrd().getRightGauss()); - dataBlock.setDoubleValue(index++, currentImu.getMagneticFieldFrd().getDownGauss()); + if ( null != currentImu ) { + dataBlock.setDoubleValue(index++, currentImu.getAccelerationFrd().getForwardMS2()); + dataBlock.setDoubleValue(index++, currentImu.getAccelerationFrd().getRightMS2()); + dataBlock.setDoubleValue(index++, currentImu.getAccelerationFrd().getDownMS2()); + //Angular Velocity + dataBlock.setDoubleValue(index++, currentImu.getAngularVelocityFrd().getForwardRadS()); + dataBlock.setDoubleValue(index++, currentImu.getAngularVelocityFrd().getRightRadS()); + dataBlock.setDoubleValue(index++, currentImu.getAngularVelocityFrd().getDownRadS()); + //Temperature + dataBlock.setDoubleValue(index++, currentImu.getTemperatureDegc()); + //Magnetic Field + dataBlock.setDoubleValue(index++, currentImu.getMagneticFieldFrd().getForwardGauss()); + dataBlock.setDoubleValue(index++, currentImu.getMagneticFieldFrd().getRightGauss()); + dataBlock.setDoubleValue(index++, currentImu.getMagneticFieldFrd().getDownGauss()); + } else { + + dataBlock.setDoubleValue(index++, 0.0); + dataBlock.setDoubleValue(index++, 0.0); + dataBlock.setDoubleValue(index++, 0.0); + //Angular Velocity + dataBlock.setDoubleValue(index++, 0.0); + dataBlock.setDoubleValue(index++, 0.0); + dataBlock.setDoubleValue(index++, 0.0); + //Temperature + dataBlock.setDoubleValue(index++, 0.0); + //Magnetic Field + dataBlock.setDoubleValue(index++, 0.0); + dataBlock.setDoubleValue(index++, 0.0); + dataBlock.setDoubleValue(index++, 0.0); + } } } finally {