Skip to content

Pr add submarine #496

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 7 commits into
base: develop
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -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;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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");

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
Original file line number Diff line number Diff line change
@@ -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<MAVLinkMessage> 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;
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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){
Expand All @@ -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()) {
Expand All @@ -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()) {
Expand Down Expand Up @@ -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;
}
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand Down Expand Up @@ -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);
}
}

Expand Down Expand Up @@ -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);
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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){
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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"),

/**
Expand Down
Loading