From 867edae23bc8bbc18fcb2dea427a1aac8b0d2497 Mon Sep 17 00:00:00 2001 From: micstinso Date: Thu, 25 Jun 2026 08:45:43 -0500 Subject: [PATCH] MavSDK Driver - Add ARM, DISARM, RTL, REBOOT --- .../impl/sensor/mavsdk/UnmannedSystem.java | 16 +- .../mavsdk/control/UnmannedControlArm.java | 163 ++++++++++++++++++ .../mavsdk/control/UnmannedControlReboot.java | 138 +++++++++++++++ 3 files changed, 315 insertions(+), 2 deletions(-) create mode 100644 sensors/robotics/sensorhub-driver-mavsdk/src/main/java/org/sensorhub/impl/sensor/mavsdk/control/UnmannedControlArm.java create mode 100644 sensors/robotics/sensorhub-driver-mavsdk/src/main/java/org/sensorhub/impl/sensor/mavsdk/control/UnmannedControlReboot.java diff --git a/sensors/robotics/sensorhub-driver-mavsdk/src/main/java/org/sensorhub/impl/sensor/mavsdk/UnmannedSystem.java b/sensors/robotics/sensorhub-driver-mavsdk/src/main/java/org/sensorhub/impl/sensor/mavsdk/UnmannedSystem.java index 7d7103b3f..996083ce9 100644 --- a/sensors/robotics/sensorhub-driver-mavsdk/src/main/java/org/sensorhub/impl/sensor/mavsdk/UnmannedSystem.java +++ b/sensors/robotics/sensorhub-driver-mavsdk/src/main/java/org/sensorhub/impl/sensor/mavsdk/UnmannedSystem.java @@ -61,8 +61,10 @@ public class UnmannedSystem extends AbstractSensorModule { log.debug("Error sending heartbeat: " + t.getMessage()); }); @@ -269,6 +279,8 @@ private void receiveDrone( ) { unmannedControlShell.setSystem(drone); unmannedControlFlightMode.setSystem(drone); unmannedControlPauseMission.setSystem(drone); + unmannedControlArm.setSystem(drone); + unmannedControlReboot.setSystem(drone); locationOutput.subscribe(drone); homeOutput.subscribe(drone); attitudeOutput.subscribe(drone); @@ -305,4 +317,4 @@ public static void printDroneInfo( io.mavsdk.System drone ) { } -} +} \ No newline at end of file diff --git a/sensors/robotics/sensorhub-driver-mavsdk/src/main/java/org/sensorhub/impl/sensor/mavsdk/control/UnmannedControlArm.java b/sensors/robotics/sensorhub-driver-mavsdk/src/main/java/org/sensorhub/impl/sensor/mavsdk/control/UnmannedControlArm.java new file mode 100644 index 000000000..d1281f75e --- /dev/null +++ b/sensors/robotics/sensorhub-driver-mavsdk/src/main/java/org/sensorhub/impl/sensor/mavsdk/control/UnmannedControlArm.java @@ -0,0 +1,163 @@ +/***************************** 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 GeoRobotix Innovative Research Inc. Portions created by the Initial + Developer are Copyright (C) 2026 the Initial Developer. All Rights Reserved. + + ******************************* END LICENSE BLOCK ***************************/ + +package org.sensorhub.impl.sensor.mavsdk.control; + +import io.reactivex.Completable; +import io.reactivex.disposables.Disposable; +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.mavsdk.UnmannedSystem; +import org.sensorhub.impl.sensor.AbstractSensorControl; +import org.vast.swe.helper.GeoPosHelper; + +/** + *

+ * This particular class provides control stream capabilities + *

+ * + * @since Jul 2025 + */ +public class UnmannedControlArm extends AbstractSensorControl +{ + private DataRecord commandDataStruct; + + /** + * Name of the control + */ + private static final String SENSOR_CONTROL_NAME = "UnmannedControlArm"; + + /** + * Label for the control + */ + private static final String SENSOR_CONTROL_LABEL = "Arming Control"; + + /** + * Control description + */ + private static final String SENSOR_CONTROL_DESCRIPTION = + "Interfaces with MAVLINK and OSH to effectuate arm/disarm control over the platform. " + + "Send true to arm (start the motors), false to disarm (stop the motors)."; + + /** + * ROS Node name assigned at creation + */ + private static final String NODE_NAME_STR = "/SensorHub/spot/arm_control"; + + private io.mavsdk.System system = null; + + public UnmannedControlArm( UnmannedSystem parentSensor) { + super("mavArmControl", 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) + .definition("http://sensorml.com/ont/isa/property/Enabled") + .addField("ARM", factory.createBoolean() + .definition("http://sensorml.com/ont/isa/property/Enabled") + .value(true) ) + .build(); + } + + @Override + protected boolean execCommand(DataBlock command) throws CommandException { + + try { + // The ARM field is a boolean: true -> arm, false -> disarm + boolean armCommand = command.getBooleanValue(0); + + log.debug("Command received - Arm: " + armCommand); + + if (system != null) { + if (armCommand) { + arm(); + } else { + disarm(); + } + } else { + throw new CommandException("Unmanned System not initialized"); + } + } catch (CommandException e) { + getLogger().error("Error processing command", e); + + parentSensor.reportError( e.getMessage(), e); + } + + return true; + } + + private void arm() { + + log.debug("Arming"); + + Disposable disposable = system.getAction().arm() + .doOnComplete(() -> { + + log.debug("Arming..."); + + }) + .onErrorResumeNext(throwable -> + Completable.error(new CommandException("Error arming drone", throwable))) + .subscribe(() -> { + log.debug("Armed"); + }, + throwable -> { + throw new CommandException(throwable.getMessage()); + }); + + } + + private void disarm() { + + log.debug("Disarming"); + + // NOTE: MAVSDK/autopilots will reject a disarm command while the vehicle is + // flying (a drone that considers itself in-air refuses to disarm). To + // disarm after a landing, see UnmannedControlLanding which waits for the + // ON_GROUND state before disarming. + Disposable disposable = system.getAction().disarm() + .doOnComplete(() -> { + + log.debug("Disarming..."); + + }) + .onErrorResumeNext(throwable -> + Completable.error(new CommandException("Error disarming drone", throwable))) + .subscribe(() -> { + log.debug("Disarmed"); + }, + throwable -> { + throw new CommandException(throwable.getMessage()); + }); + + } +} \ No newline at end of file diff --git a/sensors/robotics/sensorhub-driver-mavsdk/src/main/java/org/sensorhub/impl/sensor/mavsdk/control/UnmannedControlReboot.java b/sensors/robotics/sensorhub-driver-mavsdk/src/main/java/org/sensorhub/impl/sensor/mavsdk/control/UnmannedControlReboot.java new file mode 100644 index 000000000..a50ae69a1 --- /dev/null +++ b/sensors/robotics/sensorhub-driver-mavsdk/src/main/java/org/sensorhub/impl/sensor/mavsdk/control/UnmannedControlReboot.java @@ -0,0 +1,138 @@ +/***************************** 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 GeoRobotix Innovative Research Inc. Portions created by the Initial + Developer are Copyright (C) 2026 the Initial Developer. All Rights Reserved. + + ******************************* END LICENSE BLOCK ***************************/ + +package org.sensorhub.impl.sensor.mavsdk.control; + +import io.mavsdk.action.Action; +import io.reactivex.Completable; +import io.reactivex.disposables.Disposable; +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.mavsdk.UnmannedSystem; +import org.sensorhub.impl.sensor.AbstractSensorControl; +import org.vast.swe.SWEHelper; +import org.vast.swe.helper.GeoPosHelper; + +/** + *

+ * This particular class provides control stream capabilities + *

+ * + * @since Jul 2025 + */ +public class UnmannedControlReboot extends AbstractSensorControl +{ + private DataRecord commandDataStruct; + + /** + * Name of the control + */ + private static final String SENSOR_CONTROL_NAME = "UnmannedControlReboot"; + + /** + * Label for the control + */ + private static final String SENSOR_CONTROL_LABEL = "Reboot Control"; + + /** + * Control description + */ + private static final String SENSOR_CONTROL_DESCRIPTION = + "Interfaces with MAVLINK and OSH to reboot the platform. This reboots the autopilot " + + "(and companion computer, camera, and gimbal where present). Should only be sent while " + + "the vehicle is disarmed and on the ground."; + + /** + * ROS Node name assigned at creation + */ + private static final String NODE_NAME_STR = "/SensorHub/spot/reboot_control"; + + private io.mavsdk.System system = null; + + public UnmannedControlReboot( UnmannedSystem parentSensor) { + super("mavRebootControl", 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) + .definition(SWEHelper.getPropertyUri("Control")) + .addField("reboot", factory.createBoolean() + .definition(SWEHelper.getPropertyUri("Reboot")) + .value(true)) + .build(); + } + + @Override + protected boolean execCommand(DataBlock command) throws CommandException { + + boolean reboot = command.getBooleanValue(0); + + log.debug("Command received - Reboot: " + reboot); + + // Only act on an explicit "true". A false value is a no-op so an + // accidental command can't reboot the flight controller. + if ( !reboot ) { + return true; + } + + if ( system != null ) { + reboot(); + } else { + throw new CommandException("Unmanned System not initialized"); + } + + return true; + } + + private void reboot() { + + log.debug("Sending reboot command..."); + + Disposable disposable = system.getAction().reboot() + .doOnComplete(() -> { + + log.debug("Reboot command sent"); + + }) + .doOnError(throwable -> + log.debug("Failed to send reboot: " + ((Action.ActionException) throwable).getCode())) + .onErrorResumeNext(throwable -> + Completable.error(new CommandException("Error rebooting drone", throwable))) + .subscribe(() -> { + log.debug("Rebooted"); + }, + throwable -> { + throw new CommandException(throwable.getMessage()); + }); + + } +} \ No newline at end of file