From 7b4014cc2e991c2a7cd45a4ed3a4dc1b341fdfc9 Mon Sep 17 00:00:00 2001 From: mhefny Date: Wed, 18 Dec 2019 20:18:10 +0200 Subject: [PATCH] Adding SubMarine Support --- .../android/lib/drone/property/Type.java | 9 ++- .../lib/drone/property/VehicleMode.java | 22 ++++++ .../impl/core/MAVLink/MavLinkMsgHandler.java | 4 ++ .../core/drone/autopilot/apm/ArduSub.java | 32 +++++++++ .../drone/manager/MavLinkDroneManager.java | 6 ++ .../impl/core/drone/variables/ApmModes.java | 56 ++++++++++----- .../core/drone/variables/GuidedPoint.java | 8 +++ .../impl/core/drone/variables/Type.java | 4 ++ .../impl/core/firmware/FirmwareType.java | 1 + .../android/impl/utils/CommonApiUtils.java | 70 ++++++++++++++++--- 10 files changed, 185 insertions(+), 27 deletions(-) create mode 100644 ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/autopilot/apm/ArduSub.java diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/property/Type.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/property/Type.java index 1e1675f71b..66271f1529 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/property/Type.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/property/Type.java @@ -11,11 +11,13 @@ public class Type implements DroneAttribute { public static final int TYPE_PLANE = 1; public static final int TYPE_COPTER = 2; public static final int TYPE_ROVER = 10; + public static final int TYPE_SUBMARINE = 12; - public static enum Firmware { + public enum Firmware { ARDU_PLANE("ArduPlane"), ARDU_COPTER("ArduCopter"), - APM_ROVER("APMRover"); + APM_ROVER("APMRover"), + ARDU_SUB("ArduSub"); private final String label; @@ -51,6 +53,9 @@ private static Firmware getTypeFirmware(int droneType) { case TYPE_ROVER: return Firmware.APM_ROVER; + case TYPE_SUBMARINE: + return Firmware.ARDU_SUB; + case TYPE_UNKNOWN: default: return null; diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/property/VehicleMode.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/property/VehicleMode.java index dda33e2ab0..a52eacceb4 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/property/VehicleMode.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/property/VehicleMode.java @@ -39,15 +39,37 @@ public enum VehicleMode implements DroneAttribute { COPTER_AUTOTUNE(15, Type.TYPE_COPTER, "Autotune"), COPTER_POSHOLD(16, Type.TYPE_COPTER, "PosHold"), COPTER_BRAKE(17,Type.TYPE_COPTER,"Brake"), + COPTER_THROW(18,Type.TYPE_COPTER,"Throw"), + COPTER_AVOID_ADSB(19,Type.TYPE_COPTER,"Throw"), + COPTER_GUIDED_NOGPS(20,Type.TYPE_COPTER,"Throw"), + COPTER_SMART_RTL(21,Type.TYPE_COPTER,"SmartRTL"), + COPTER_FLOWHOLD(22,Type.TYPE_COPTER,"FollowHold"), + COPTER_FOLLOW(23,Type.TYPE_COPTER,"Follow"), + COPTER_ZIGZAG(24,Type.TYPE_COPTER,"ZigZag"), ROVER_MANUAL(0, Type.TYPE_ROVER, "Manual"), + ROVER_ACRO(1, Type.TYPE_ROVER, "Acro"), ROVER_LEARNING(2, Type.TYPE_ROVER, "Learning"), ROVER_STEERING(3, Type.TYPE_ROVER, "Steering"), ROVER_HOLD(4, Type.TYPE_ROVER, "Hold"), + ROVER_FOLLOW(6, Type.TYPE_ROVER, "Follow"), ROVER_AUTO(10, Type.TYPE_ROVER, "Auto"), ROVER_RTL(11, Type.TYPE_ROVER, "RTL"), ROVER_GUIDED(15, Type.TYPE_ROVER, "Guided"), ROVER_INITIALIZING(16, Type.TYPE_ROVER, "Initializing"), + ROVER_SMART_RTL(12, Type.TYPE_ROVER, "SmartRTL"), + + + SUBMARINE_STABILIZE (0, Type.TYPE_SUBMARINE, "Stabilize"), + SUBMARINE_ACRO (1, Type.TYPE_SUBMARINE, "Acro"), + SUBMARINE_ALT_HOLD (2, Type.TYPE_SUBMARINE, "Alt Hold"), + SUBMARINE_AUTO (3, Type.TYPE_SUBMARINE, "Auto"), + SUBMARINE_GUIDED (4, Type.TYPE_SUBMARINE, "GUIDED"), + SUBMARINE_CIRCLE (7, Type.TYPE_SUBMARINE, "Circle"), + SUBMARINE_SURFACE (9, Type.TYPE_SUBMARINE, "Surface"), + SUBMARINE_POSHOLD (16, Type.TYPE_SUBMARINE, "PosHold"), + SUBMARINE_MANUAL(19, Type.TYPE_SUBMARINE,"MANUAL"), + SUBMARINE_MOTOR_DETECT (20, Type.TYPE_SUBMARINE, "Motor Detect"), UNKNOWN(-1, Type.TYPE_UNKNOWN, "Unknown"); diff --git a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/MAVLink/MavLinkMsgHandler.java b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/MAVLink/MavLinkMsgHandler.java index ee18e77ff8..7aef4a33bb 100644 --- a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/MAVLink/MavLinkMsgHandler.java +++ b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/MAVLink/MavLinkMsgHandler.java @@ -56,6 +56,10 @@ private void handleHeartbeat(msg_heartbeat heartbeat) { droneMgr.onVehicleTypeReceived(FirmwareType.ARDU_COPTER); break; + case MAV_TYPE.MAV_TYPE_SUBMARINE: + droneMgr.onVehicleTypeReceived(FirmwareType.ARDU_SUB); + break; + case MAV_TYPE.MAV_TYPE_GROUND_ROVER: case MAV_TYPE.MAV_TYPE_SURFACE_BOAT: droneMgr.onVehicleTypeReceived(FirmwareType.ARDU_ROVER); diff --git a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/autopilot/apm/ArduSub.java b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/autopilot/apm/ArduSub.java new file mode 100644 index 0000000000..9f9608222a --- /dev/null +++ b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/autopilot/apm/ArduSub.java @@ -0,0 +1,32 @@ +package org.droidplanner.services.android.impl.core.drone.autopilot.apm; + +import android.content.Context; +import android.os.Handler; + +import com.MAVLink.Messages.MAVLinkMessage; +import com.MAVLink.enums.MAV_TYPE; + +import org.droidplanner.services.android.impl.communication.model.DataLink; +import org.droidplanner.services.android.impl.core.drone.LogMessageListener; +import org.droidplanner.services.android.impl.core.firmware.FirmwareType; +import org.droidplanner.services.android.impl.core.model.AutopilotWarningParser; + +public class ArduSub extends ArduPilot { + + public ArduSub(String droneId, Context context, DataLink.DataLinkProvider mavClient, Handler handler, AutopilotWarningParser warningParser, LogMessageListener logListener) { + super(droneId, context, mavClient, handler, warningParser, logListener); + } + + @Override + public int getType(){ + return MAV_TYPE.MAV_TYPE_SUBMARINE; + } + + @Override + public void setType(int type){} + + @Override + public FirmwareType getFirmwareType() { + return FirmwareType.ARDU_SUB; + } +} diff --git a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/manager/MavLinkDroneManager.java b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/manager/MavLinkDroneManager.java index 8f598ffd9b..c53be77639 100644 --- a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/manager/MavLinkDroneManager.java +++ b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/manager/MavLinkDroneManager.java @@ -34,6 +34,7 @@ import org.droidplanner.services.android.impl.core.drone.autopilot.apm.ArduCopter; import org.droidplanner.services.android.impl.core.drone.autopilot.apm.ArduPlane; import org.droidplanner.services.android.impl.core.drone.autopilot.apm.ArduRover; +import org.droidplanner.services.android.impl.core.drone.autopilot.apm.ArduSub; import org.droidplanner.services.android.impl.core.drone.autopilot.apm.solo.ArduSolo; import org.droidplanner.services.android.impl.core.drone.autopilot.generic.GenericMavLinkDrone; import org.droidplanner.services.android.impl.core.drone.autopilot.px4.Px4Native; @@ -121,6 +122,11 @@ public void onVehicleTypeReceived(FirmwareType type) { this.drone = new ArduRover(droneId, context, mavClient, handler, new AndroidApWarningParser(), this); break; + case ARDU_SUB: + Timber.i("Instantiating ArduSub autopilot."); + this.drone = new ArduSub(droneId, context, mavClient, handler, new AndroidApWarningParser(), this); + break; + case PX4_NATIVE: Timber.i("Instantiating PX4 Native autopilot."); this.drone = new Px4Native(droneId, context, handler, mavClient, new AndroidApWarningParser(), this); diff --git a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/variables/ApmModes.java b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/variables/ApmModes.java index 5246e570ea..1749b2b984 100644 --- a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/variables/ApmModes.java +++ b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/variables/ApmModes.java @@ -29,25 +29,47 @@ public enum ApmModes { ROTOR_RTL(6, "RTL",MAV_TYPE.MAV_TYPE_QUADROTOR), ROTOR_CIRCLE(7, "Circle",MAV_TYPE.MAV_TYPE_QUADROTOR), ROTOR_LAND(9, "Land",MAV_TYPE.MAV_TYPE_QUADROTOR), - ROTOR_TOY(11, "Drift",MAV_TYPE.MAV_TYPE_QUADROTOR), + ROTOR_DRIFT(11, "Drift",MAV_TYPE.MAV_TYPE_QUADROTOR), ROTOR_SPORT(13, "Sport",MAV_TYPE.MAV_TYPE_QUADROTOR), ROTOR_AUTOTUNE(15, "Autotune",MAV_TYPE.MAV_TYPE_QUADROTOR), ROTOR_POSHOLD(16, "PosHold",MAV_TYPE.MAV_TYPE_QUADROTOR), ROTOR_BRAKE(17,"Brake",MAV_TYPE.MAV_TYPE_QUADROTOR), + ROTOR_THROW(18,"Brake",MAV_TYPE.MAV_TYPE_QUADROTOR), + ROTOR_AVOID_ADSB(19,"Brake",MAV_TYPE.MAV_TYPE_QUADROTOR), + ROTOR_GUIDED_NOGPS(20,"Brake",MAV_TYPE.MAV_TYPE_QUADROTOR), + ROTOR_SMART_RTL(21,"SmartRTL",MAV_TYPE.MAV_TYPE_QUADROTOR), + ROTOR_FLOWHOLD(22,"Follow",MAV_TYPE.MAV_TYPE_QUADROTOR), + ROTOR_FOLLOW(23,"Follow",MAV_TYPE.MAV_TYPE_QUADROTOR), + ROTOR_ZIGZAG(24,"Follow",MAV_TYPE.MAV_TYPE_QUADROTOR), ROVER_MANUAL(0, "MANUAL", MAV_TYPE.MAV_TYPE_GROUND_ROVER), + ROVER_ACRO(1, "LEARNING", MAV_TYPE.MAV_TYPE_GROUND_ROVER), ROVER_LEARNING(2, "LEARNING", MAV_TYPE.MAV_TYPE_GROUND_ROVER), ROVER_STEERING(3, "STEERING", MAV_TYPE.MAV_TYPE_GROUND_ROVER), ROVER_HOLD(4, "HOLD", MAV_TYPE.MAV_TYPE_GROUND_ROVER), + ROVER_FOLLOW(6, "FOLLOW", MAV_TYPE.MAV_TYPE_GROUND_ROVER), ROVER_AUTO(10, "AUTO", MAV_TYPE.MAV_TYPE_GROUND_ROVER), ROVER_RTL(11, "RTL", MAV_TYPE.MAV_TYPE_GROUND_ROVER), + ROVER_SMART_RTL(12, "SmartRTL", MAV_TYPE.MAV_TYPE_GROUND_ROVER), ROVER_GUIDED(15, "GUIDED", MAV_TYPE.MAV_TYPE_GROUND_ROVER), ROVER_INITIALIZING(16, "INITIALIZING", MAV_TYPE.MAV_TYPE_GROUND_ROVER), + + SUBMARINE_STABILIZE (0, "Stabilize", MAV_TYPE.MAV_TYPE_SUBMARINE), + SUBMARINE_ACRO (1, "Acro", MAV_TYPE.MAV_TYPE_SUBMARINE), + SUBMARINE_ALT_HOLD (2, "Alt Hold", MAV_TYPE.MAV_TYPE_SUBMARINE), + SUBMARINE_AUTO (3, "Auto", MAV_TYPE.MAV_TYPE_SUBMARINE), + SUBMARINE_GUIDED (4, "GUIDED", MAV_TYPE.MAV_TYPE_SUBMARINE), + SUBMARINE_CIRCLE (7, "Circle", MAV_TYPE.MAV_TYPE_SUBMARINE), + SUBMARINE_SURFACE (9, "Surface", MAV_TYPE.MAV_TYPE_SUBMARINE), + SUBMARINE_POSHOLD (16, "PosHold", MAV_TYPE.MAV_TYPE_SUBMARINE), + SUBMARINE_MANUAL(19,"MANUAL", MAV_TYPE.MAV_TYPE_SUBMARINE), + SUBMARINE_MOTOR_DETECT (20, "Motor Detect", MAV_TYPE.MAV_TYPE_SUBMARINE), + UNKNOWN(-1, "Unknown", MAV_TYPE.MAV_TYPE_GENERIC); private final long number; - private final String name; + private final String name; private final int type; ApmModes(long number,String name, int type){ @@ -69,9 +91,9 @@ public int getType() { } public static ApmModes getMode(long i, int type) { - if (isCopter(type)) { - type = MAV_TYPE.MAV_TYPE_QUADROTOR; - } + if (isCopter(type)) { + type = MAV_TYPE.MAV_TYPE_QUADROTOR; + } for (ApmModes mode : ApmModes.values()) { if (i == mode.getNumber() && type == mode.getType()) { @@ -82,9 +104,9 @@ public static ApmModes getMode(long i, int type) { } public static ApmModes getMode(String str, int type) { - if (isCopter(type)) { - type = MAV_TYPE.MAV_TYPE_QUADROTOR; - } + if (isCopter(type)) { + type = MAV_TYPE.MAV_TYPE_QUADROTOR; + } for (ApmModes mode : ApmModes.values()) { if (str.equals(mode.getName()) && type == mode.getType()) { @@ -116,15 +138,15 @@ public static boolean isValid(ApmModes mode) { public static boolean isCopter(int type){ switch (type) { - case MAV_TYPE.MAV_TYPE_TRICOPTER: - case MAV_TYPE.MAV_TYPE_QUADROTOR: - case MAV_TYPE.MAV_TYPE_HEXAROTOR: - case MAV_TYPE.MAV_TYPE_OCTOROTOR: - case MAV_TYPE.MAV_TYPE_HELICOPTER: - return true; - - default: - return false; + case MAV_TYPE.MAV_TYPE_TRICOPTER: + case MAV_TYPE.MAV_TYPE_QUADROTOR: + case MAV_TYPE.MAV_TYPE_HEXAROTOR: + case MAV_TYPE.MAV_TYPE_OCTOROTOR: + case MAV_TYPE.MAV_TYPE_HELICOPTER: + return true; + + default: + return false; } } diff --git a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/variables/GuidedPoint.java b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/variables/GuidedPoint.java index 7f88143cda..29d1b9d7e9 100644 --- a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/variables/GuidedPoint.java +++ b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/variables/GuidedPoint.java @@ -80,6 +80,11 @@ public static boolean isGuidedMode(MavLinkDrone drone) { return droneMode == ApmModes.ROVER_GUIDED || droneMode == ApmModes.ROVER_HOLD; } + if (Type.isSubMarine(droneType)) { + return droneMode == ApmModes.SUBMARINE_GUIDED; + } + + return false; } @@ -112,6 +117,8 @@ public static void changeToGuidedMode(MavLinkDrone drone, ICommandListener liste forceSendGuidedPoint(drone, getGpsPosition(drone), getDroneAltConstrained(drone)); } else if (Type.isRover(droneType)) { droneState.changeFlightMode(ApmModes.ROVER_GUIDED, listener); + }else if (Type.isSubMarine(droneType)) { + droneState.changeFlightMode(ApmModes.SUBMARINE_GUIDED, listener); } } @@ -314,6 +321,7 @@ private void sendGuidedPoint() { public static void forceSendGuidedPoint(MavLinkDrone drone, LatLong coord, double altitudeInMeters) { drone.notifyDroneEvent(DroneEventsType.GUIDEDPOINT); + //MHEFNY: BUG when calling change altitude without setting a first destination point coord is NULL if (coord != null) { MavLinkCommands.setGuidedMode(drone, coord.getLatitude(), coord.getLongitude(), altitudeInMeters); } diff --git a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/variables/Type.java b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/variables/Type.java index 836c7b6185..8cbe5b0be2 100644 --- a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/variables/Type.java +++ b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/variables/Type.java @@ -63,6 +63,10 @@ public static boolean isRover(int type){ return type == MAV_TYPE.MAV_TYPE_GROUND_ROVER; } + public static boolean isSubMarine(int type){ + return type == MAV_TYPE.MAV_TYPE_SUBMARINE; + } + @Override public void onDroneEvent(DroneEventsType event, MavLinkDrone drone) { switch(event){ diff --git a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/firmware/FirmwareType.java b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/firmware/FirmwareType.java index 9863d0aa5e..b153a49f2e 100644 --- a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/firmware/FirmwareType.java +++ b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/firmware/FirmwareType.java @@ -7,6 +7,7 @@ public enum FirmwareType { ARDU_PLANE(MAV_AUTOPILOT.MAV_AUTOPILOT_ARDUPILOTMEGA, "ArduPlane", "ArduPlane"), ARDU_COPTER(MAV_AUTOPILOT.MAV_AUTOPILOT_ARDUPILOTMEGA, "ArduCopter2", "ArduCopter"), ARDU_ROVER(MAV_AUTOPILOT.MAV_AUTOPILOT_ARDUPILOTMEGA, "ArduRover", "ArduRover"), + ARDU_SUB(MAV_AUTOPILOT.MAV_AUTOPILOT_ARDUPILOTMEGA, "ArduSub", "ArduSub"), ARDU_SOLO(MAV_AUTOPILOT.MAV_AUTOPILOT_ARDUPILOTMEGA, "ArduCopter2", "ArduSolo"), /** diff --git a/ClientLib/src/main/java/org/droidplanner/services/android/impl/utils/CommonApiUtils.java b/ClientLib/src/main/java/org/droidplanner/services/android/impl/utils/CommonApiUtils.java index b5e76b05c8..92c11fc62f 100644 --- a/ClientLib/src/main/java/org/droidplanner/services/android/impl/utils/CommonApiUtils.java +++ b/ClientLib/src/main/java/org/droidplanner/services/android/impl/utils/CommonApiUtils.java @@ -172,13 +172,16 @@ public static VehicleMode getVehicleMode(ApmModes mode) { case ROTOR_RTL: return VehicleMode.COPTER_RTL; + case ROTOR_SMART_RTL: + return VehicleMode.COPTER_SMART_RTL; + case ROTOR_CIRCLE: return VehicleMode.COPTER_CIRCLE; case ROTOR_LAND: return VehicleMode.COPTER_LAND; - case ROTOR_TOY: + case ROTOR_DRIFT: return VehicleMode.COPTER_DRIFT; case ROTOR_SPORT: @@ -193,10 +196,31 @@ public static VehicleMode getVehicleMode(ApmModes mode) { case ROTOR_BRAKE: return VehicleMode.COPTER_BRAKE; + case ROTOR_THROW: + return VehicleMode.COPTER_THROW; + + case ROTOR_AVOID_ADSB: + return VehicleMode.COPTER_AVOID_ADSB; + + case ROTOR_GUIDED_NOGPS: + return VehicleMode.COPTER_GUIDED_NOGPS; + + case ROTOR_FLOWHOLD: + return VehicleMode.COPTER_FOLLOW; + + case ROTOR_FOLLOW: + return VehicleMode.COPTER_FOLLOW; + + case ROTOR_ZIGZAG: + return VehicleMode.COPTER_FOLLOW; + case ROVER_MANUAL: return VehicleMode.ROVER_MANUAL; + case ROVER_ACRO: + return VehicleMode.ROVER_ACRO; + case ROVER_LEARNING: return VehicleMode.ROVER_LEARNING; @@ -209,15 +233,42 @@ public static VehicleMode getVehicleMode(ApmModes mode) { case ROVER_AUTO: return VehicleMode.ROVER_AUTO; + case ROVER_FOLLOW: + return VehicleMode.ROVER_FOLLOW; + case ROVER_RTL: return VehicleMode.ROVER_RTL; + case ROVER_SMART_RTL: + return VehicleMode.ROVER_SMART_RTL; + case ROVER_GUIDED: return VehicleMode.ROVER_GUIDED; case ROVER_INITIALIZING: return VehicleMode.ROVER_INITIALIZING; + case SUBMARINE_STABILIZE: + return VehicleMode.SUBMARINE_STABILIZE; + case SUBMARINE_ACRO: + return VehicleMode.SUBMARINE_ACRO; + case SUBMARINE_ALT_HOLD: + return VehicleMode.SUBMARINE_ALT_HOLD; + case SUBMARINE_AUTO: + return VehicleMode.SUBMARINE_AUTO; + case SUBMARINE_GUIDED: + return VehicleMode.SUBMARINE_GUIDED; + case SUBMARINE_CIRCLE: + return VehicleMode.SUBMARINE_CIRCLE; + case SUBMARINE_SURFACE: + return VehicleMode.SUBMARINE_SURFACE; + case SUBMARINE_POSHOLD: + return VehicleMode.SUBMARINE_POSHOLD; + case SUBMARINE_MANUAL: + return VehicleMode.SUBMARINE_MANUAL; + case SUBMARINE_MOTOR_DETECT: + return VehicleMode.SUBMARINE_MOTOR_DETECT; + default: case UNKNOWN: return null; @@ -241,6 +292,9 @@ public static int getDroneProxyType(int originalType) { case MAV_TYPE.MAV_TYPE_SURFACE_BOAT: return Type.TYPE_ROVER; + case MAV_TYPE.MAV_TYPE_SUBMARINE: + return Type.TYPE_SUBMARINE; + default: return -1; } @@ -386,9 +440,9 @@ public static State getState(MavLinkDrone drone, boolean isConnected, Vibration : null; return new State(isConnected, CommonApiUtils.getVehicleMode(droneMode), droneState.isArmed(), - droneState.isFlying(), droneState.getErrorId(), drone.getMavlinkVersion(), calibrationMessage, - droneState.getFlightStartTime(), generateEkfStatus(droneState.getEkfStatus()), - isConnected && drone.isConnectionAlive(), vibration); + droneState.isFlying(), droneState.getErrorId(), drone.getMavlinkVersion(), calibrationMessage, + droneState.getFlightStartTime(), generateEkfStatus(droneState.getEkfStatus()), + isConnected && drone.isConnectionAlive(), vibration); } public static EkfStatus generateEkfStatus(msg_ekf_status_report ekfStatus) { @@ -397,8 +451,8 @@ public static EkfStatus generateEkfStatus(msg_ekf_status_report ekfStatus) { } EkfStatus proxyEkfStatus = new EkfStatus(ekfStatus.flags, ekfStatus.compass_variance, - ekfStatus.pos_horiz_variance, ekfStatus.terrain_alt_variance, ekfStatus.velocity_variance, - ekfStatus.pos_vert_variance); + ekfStatus.pos_horiz_variance, ekfStatus.terrain_alt_variance, ekfStatus.velocity_variance, + ekfStatus.pos_vert_variance); return proxyEkfStatus; } @@ -987,7 +1041,7 @@ public static void stopVideoStream(Drone drone, String appId, String videoTag, } public static void startVideoStreamForObserver(Drone drone, String appId, String videoTag, - ICommandListener listener) { + ICommandListener listener) { if (!(drone instanceof GenericMavLinkDrone)) { postErrorEvent(CommandExecutionError.COMMAND_UNSUPPORTED, listener); return; @@ -998,7 +1052,7 @@ public static void startVideoStreamForObserver(Drone drone, String appId, String } public static void stopVideoStreamForObserver(Drone drone, String appId, String videoTag, - ICommandListener listener) { + ICommandListener listener) { if (!(drone instanceof GenericMavLinkDrone)) { postErrorEvent(CommandExecutionError.COMMAND_UNSUPPORTED, listener); return;