diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/MissionItemType.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/MissionItemType.java index 050871d409..e83a3454ee 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/MissionItemType.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/MissionItemType.java @@ -16,10 +16,12 @@ import com.o3dr.services.android.lib.drone.mission.item.complex.StructureScanner; import com.o3dr.services.android.lib.drone.mission.item.complex.Survey; import com.o3dr.services.android.lib.drone.mission.item.spatial.Circle; +import com.o3dr.services.android.lib.drone.mission.item.spatial.DoLandStart; import com.o3dr.services.android.lib.drone.mission.item.spatial.Land; import com.o3dr.services.android.lib.drone.mission.item.spatial.RegionOfInterest; import com.o3dr.services.android.lib.drone.mission.item.spatial.SplineWaypoint; import com.o3dr.services.android.lib.drone.mission.item.spatial.Waypoint; +import com.o3dr.services.android.lib.drone.property.Type; import com.o3dr.services.android.lib.util.ParcelableUtils; /** @@ -113,6 +115,23 @@ protected Creator getMissionItemCreator() { } }, + DO_LAND_START("Do Land start") { + @Override + public boolean isTypeSupported(Type vehicleType){ + return super.isTypeSupported(vehicleType) && vehicleType.getDroneType() == Type.TYPE_PLANE; + } + + @Override + public MissionItem getNewItem() { + return new DoLandStart(); + } + + @Override + protected Creator getMissionItemCreator() { + return DoLandStart.CREATOR; + } + }, + LAND("Land") { @Override public MissionItem getNewItem() { @@ -259,4 +278,12 @@ public static T restoreMissionItemFromBundle(Bundle bund return missionItem; } + /** + * Indicates if the mission item is supported on the given vehicle type. + * @param vehicleType Vehicle type to check against (i.e: plane, copter, rover...) + * @return true the mission item is supported, false otherwise. + */ + public boolean isTypeSupported(Type vehicleType){ + return vehicleType != null; + } } diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/DoLandStart.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/DoLandStart.java new file mode 100644 index 0000000000..49169c6f24 --- /dev/null +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/DoLandStart.java @@ -0,0 +1,36 @@ +package com.o3dr.services.android.lib.drone.mission.item.spatial; + +import android.os.Parcel; + +import com.o3dr.services.android.lib.drone.mission.MissionItemType; +import com.o3dr.services.android.lib.drone.mission.item.MissionItem; + +public class DoLandStart extends BaseSpatialItem implements android.os.Parcelable { + + public DoLandStart(){ + super(MissionItemType.DO_LAND_START); + } + + public DoLandStart(DoLandStart copy){ + super(copy); + } + + private DoLandStart(Parcel in) { + super(in); + } + + @Override + public MissionItem clone() { + return new DoLandStart(this); + } + + public static final Creator CREATOR = new Creator() { + public DoLandStart createFromParcel(Parcel source) { + return new DoLandStart(source); + } + + public DoLandStart[] newArray(int size) { + return new DoLandStart[size]; + } + }; +} \ No newline at end of file diff --git a/ServiceApp/src/org/droidplanner/services/android/utils/ProxyUtils.java b/ServiceApp/src/org/droidplanner/services/android/utils/ProxyUtils.java index a99fbc5409..564ee58a18 100644 --- a/ServiceApp/src/org/droidplanner/services/android/utils/ProxyUtils.java +++ b/ServiceApp/src/org/droidplanner/services/android/utils/ProxyUtils.java @@ -16,6 +16,7 @@ import com.o3dr.services.android.lib.drone.mission.item.complex.Survey; import com.o3dr.services.android.lib.drone.mission.item.complex.SurveyDetail; import com.o3dr.services.android.lib.drone.mission.item.spatial.Circle; +import com.o3dr.services.android.lib.drone.mission.item.spatial.DoLandStart; import com.o3dr.services.android.lib.drone.mission.item.spatial.Land; import com.o3dr.services.android.lib.drone.mission.item.spatial.RegionOfInterest; import com.o3dr.services.android.lib.drone.mission.item.spatial.SplineWaypoint; @@ -26,6 +27,7 @@ import org.droidplanner.core.mission.commands.ConditionYaw; import org.droidplanner.core.mission.commands.ReturnToHome; import org.droidplanner.core.mission.commands.SetRelayImpl; +import org.droidplanner.core.mission.waypoints.DoLandStartImpl; import org.droidplanner.core.survey.CameraInfo; import org.droidplanner.core.survey.SurveyData; @@ -154,6 +156,15 @@ public static org.droidplanner.core.mission.MissionItem getMissionItemImpl(Missi missionItemImpl = temp; break; } + case DO_LAND_START: { + DoLandStart proxy = (DoLandStart) proxyItem; + + DoLandStartImpl temp = new DoLandStartImpl(mission, MathUtils.latLongToCoord2D(proxy + .getCoordinate())); + + missionItemImpl = temp; + break; + } case REGION_OF_INTEREST: { RegionOfInterest proxy = (RegionOfInterest) proxyItem; @@ -316,7 +327,15 @@ public static MissionItem getProxyMissionItem(org.droidplanner.core.mission.Miss proxyMissionItem = temp; break; } + case DO_LAND_START: { + DoLandStartImpl source = (DoLandStartImpl) itemImpl; + DoLandStart temp = new DoLandStart(); + temp.setCoordinate(MathUtils.coord3DToLatLongAlt(source.getCoordinate())); + + proxyMissionItem = temp; + break; + } case CIRCLE: { org.droidplanner.core.mission.waypoints.Circle source = (org.droidplanner.core.mission.waypoints.Circle) itemImpl; diff --git a/dependencyLibs/Core/src/org/droidplanner/core/mission/Mission.java b/dependencyLibs/Core/src/org/droidplanner/core/mission/Mission.java index c41aa8298f..d824b746ee 100644 --- a/dependencyLibs/Core/src/org/droidplanner/core/mission/Mission.java +++ b/dependencyLibs/Core/src/org/droidplanner/core/mission/Mission.java @@ -17,6 +17,7 @@ import org.droidplanner.core.mission.commands.SetServo; import org.droidplanner.core.mission.commands.Takeoff; import org.droidplanner.core.mission.waypoints.Circle; +import org.droidplanner.core.mission.waypoints.DoLandStartImpl; import org.droidplanner.core.mission.waypoints.Land; import org.droidplanner.core.mission.waypoints.RegionOfInterest; import org.droidplanner.core.mission.waypoints.SpatialCoordItem; @@ -266,6 +267,9 @@ private List processMavLinkMessages(List msgs) { case MAV_CMD.MAV_CMD_NAV_LAND: received.add(new Land(msg, this)); break; + case MAV_CMD.MAV_CMD_DO_LAND_START: + received.add(new DoLandStartImpl(msg, this)); + break; case MAV_CMD.MAV_CMD_NAV_TAKEOFF: received.add(new Takeoff(msg, this)); break; diff --git a/dependencyLibs/Core/src/org/droidplanner/core/mission/MissionItemType.java b/dependencyLibs/Core/src/org/droidplanner/core/mission/MissionItemType.java index 22af8ae20f..ba6681e7aa 100644 --- a/dependencyLibs/Core/src/org/droidplanner/core/mission/MissionItemType.java +++ b/dependencyLibs/Core/src/org/droidplanner/core/mission/MissionItemType.java @@ -11,6 +11,7 @@ import org.droidplanner.core.mission.commands.Takeoff; import org.droidplanner.core.mission.survey.Survey; import org.droidplanner.core.mission.waypoints.Circle; +import org.droidplanner.core.mission.waypoints.DoLandStartImpl; import org.droidplanner.core.mission.waypoints.Land; import org.droidplanner.core.mission.waypoints.RegionOfInterest; import org.droidplanner.core.mission.waypoints.SplineWaypoint; @@ -25,6 +26,7 @@ public enum MissionItemType { TAKEOFF("Takeoff"), RTL("Return to Launch"), LAND("Land"), + DO_LAND_START("Do Land Start"), CIRCLE("Circle"), ROI("Region of Interest"), SURVEY("Survey"), @@ -64,6 +66,8 @@ public MissionItem getNewItem(MissionItem referenceItem) throws IllegalArgumentE return new ReturnToHome(referenceItem); case LAND: return new Land(referenceItem); + case DO_LAND_START: + return new DoLandStartImpl(referenceItem); case CIRCLE: return new Circle(referenceItem); case ROI: diff --git a/dependencyLibs/Core/src/org/droidplanner/core/mission/waypoints/DoLandStartImpl.java b/dependencyLibs/Core/src/org/droidplanner/core/mission/waypoints/DoLandStartImpl.java new file mode 100644 index 0000000000..b28ef9818b --- /dev/null +++ b/dependencyLibs/Core/src/org/droidplanner/core/mission/waypoints/DoLandStartImpl.java @@ -0,0 +1,53 @@ +package org.droidplanner.core.mission.waypoints; + +import com.MAVLink.common.msg_mission_item; +import com.MAVLink.enums.MAV_CMD; + +import org.droidplanner.core.helpers.coordinates.Coord2D; +import org.droidplanner.core.helpers.coordinates.Coord3D; +import org.droidplanner.core.mission.Mission; +import org.droidplanner.core.mission.MissionItem; +import org.droidplanner.core.mission.MissionItemType; + +import java.util.List; + +public class DoLandStartImpl extends SpatialCoordItem { + + public DoLandStartImpl(MissionItem item) { + super(item); + setAltitude((0.0)); + } + + public DoLandStartImpl(Mission mission) { + this(mission, new Coord2D(0, 0)); + } + + public DoLandStartImpl(Mission mMission, Coord2D coord) { + super(mMission, new Coord3D(coord, (0))); + } + + public DoLandStartImpl(msg_mission_item msg, Mission mission) { + super(mission, null); + unpackMAVMessage(msg); + } + + + @Override + public List packMissionItem() { + List list = super.packMissionItem(); + msg_mission_item mavMsg = list.get(0); + mavMsg.command = MAV_CMD.MAV_CMD_DO_LAND_START; + return list; + } + + @Override + public void unpackMAVMessage(msg_mission_item mavMsg) { + super.unpackMAVMessage(mavMsg); + } + + @Override + public MissionItemType getType() { + return MissionItemType.DO_LAND_START; + } + +} \ No newline at end of file