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/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 { 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..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 @@ -13,6 +13,11 @@ 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; +import java.util.UUID; /** * Configuration settings for the {@link UnmannedSystem} driver exposed via the OpenSensorHub Admin panel. @@ -33,5 +38,11 @@ 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; + + 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/UnmannedControlLocation.java b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedControlLocation.java index c3642d0..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 @@ -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; @@ -63,7 +64,7 @@ public class UnmannedControlLocation extends AbstractSensorControl { + 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()); @@ -169,11 +166,41 @@ private void sendToLocation( double latitudeParam, .filter(pos -> (abs(pos.getLatitudeDeg() - latitudeParam) <= deltaSuccess && abs(pos.getLongitudeDeg() - longitudeParam) <= deltaSuccess)) .firstElement() .ignoreElement() - ) - .subscribe(() -> { + .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(); + } + })) + ); + } - }); + 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/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/UnmannedOutput.java b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedOutput.java index a9e8997..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 @@ -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 @@ -162,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 { @@ -441,31 +458,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 +475,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 new file mode 100644 index 0000000..f071dbe --- /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/UnmannedSystem.java b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/UnmannedSystem.java index daf8758..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 @@ -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. *

@@ -23,8 +31,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); @@ -35,7 +43,9 @@ public class UnmannedSystem extends AbstractSensorModule { UnmannedControlTakeoff unmannedControlTakeoff; UnmannedControlLocation unmannedControlLocation; UnmannedControlLanding unmannedControlLanding; + UnmannedControlMission unmannedControlMission; UnmannedControlOffboard unmannedControlOffboard; + UnmannedControlShell unmannedControlShell; @Override public void doInit() throws SensorHubException { @@ -62,16 +72,28 @@ 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(); - output.doInit(unmannedControlLocation,unmannedControlLanding, unmannedControlTakeoff); + this.unmannedControlShell = new UnmannedControlShell(this); + addControlInput(this.unmannedControlShell); + unmannedControlShell.init(); + + output.doInit(); } @Override public void doStart() throws SensorHubException { super.doStart(); + + receiveDrone(); + //startProcessing(); } @@ -116,4 +138,139 @@ 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(config.SDKAddress, config.SDKPort); + 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); + unmannedControlMission.setSystem(drone); + unmannedControlShell.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/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..b38a266 --- /dev/null +++ b/other-modules/comm/sensorhub-comm-mavlink2/src/main/java/org/sensorhub/impl/comm/mavlink2/config/PointConfig.java @@ -0,0 +1,10 @@ +package org.sensorhub.impl.comm.mavlink2.config; + +public class PointConfig { + + public double latitude; + public double longitude; + public double altitudeAGL; + public double hoverSeconds; + +} 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 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",