diff --git a/AidlLib/build.gradle b/AidlLib/build.gradle index 3678a9d249..dc26b2401c 100644 --- a/AidlLib/build.gradle +++ b/AidlLib/build.gradle @@ -7,8 +7,8 @@ android { defaultConfig { minSdkVersion 14 targetSdkVersion 21 - versionCode 20044 - versionName '2.0.44' + versionCode 20046 + versionName '2.0.46' } defaultPublishConfig "release" diff --git a/AidlLib/src/com/o3dr/services/android/lib/drone/mission/MissionItemType.java b/AidlLib/src/com/o3dr/services/android/lib/drone/mission/MissionItemType.java index 505aea7efb..95c9fa6bfa 100644 --- a/AidlLib/src/com/o3dr/services/android/lib/drone/mission/MissionItemType.java +++ b/AidlLib/src/com/o3dr/services/android/lib/drone/mission/MissionItemType.java @@ -9,6 +9,7 @@ import com.o3dr.services.android.lib.drone.mission.item.command.ChangeSpeed; import com.o3dr.services.android.lib.drone.mission.item.command.EpmGripper; import com.o3dr.services.android.lib.drone.mission.item.command.ReturnToLaunch; +import com.o3dr.services.android.lib.drone.mission.item.command.SetRelay; import com.o3dr.services.android.lib.drone.mission.item.command.SetServo; import com.o3dr.services.android.lib.drone.mission.item.command.Takeoff; import com.o3dr.services.android.lib.drone.mission.item.command.YawCondition; @@ -194,6 +195,18 @@ public MissionItem getNewItem() { protected Creator getMissionItemCreator() { return SetServo.CREATOR; } + }, + + SET_RELAY("Set Relay") { + @Override + public MissionItem getNewItem() { + return new SetRelay(); + } + + @Override + protected Creator getMissionItemCreator() { + return SetRelay.CREATOR; + } }; private final static String EXTRA_MISSION_ITEM_TYPE = "extra_mission_item_type"; diff --git a/AidlLib/src/com/o3dr/services/android/lib/drone/mission/item/command/SetRelay.java b/AidlLib/src/com/o3dr/services/android/lib/drone/mission/item/command/SetRelay.java new file mode 100644 index 0000000000..db42b85644 --- /dev/null +++ b/AidlLib/src/com/o3dr/services/android/lib/drone/mission/item/command/SetRelay.java @@ -0,0 +1,83 @@ +package com.o3dr.services.android.lib.drone.mission.item.command; + +import android.os.Parcel; + +import com.o3dr.services.android.lib.drone.mission.MissionItemType; +import com.o3dr.services.android.lib.drone.mission.item.MissionItem; + +/** + * Set a Relay pin’s voltage high or low. + */ +public class SetRelay extends MissionItem implements MissionItem.Command, android.os.Parcelable { + + private int relayNumber; + private boolean enabled; + + public SetRelay() { + super(MissionItemType.SET_RELAY); + } + + public SetRelay(SetRelay copy) { + this(); + this.relayNumber = copy.relayNumber; + this.enabled = copy.enabled; + } + + /** + * @return relay number + */ + public int getRelayNumber() { + return relayNumber; + } + + /** + * Set the relay number + * + * @param relayNumber + */ + public void setRelayNumber(int relayNumber) { + this.relayNumber = relayNumber; + } + + /** + * @return true if relay is on, false if relay if off. + */ + public boolean isEnabled() { + return enabled; + } + + /** + * @param enabled true for relay to be on, false for relay to be off. + */ + public void setEnabled(boolean enabled) { + this.enabled = enabled; + } + + @Override + public MissionItem clone() { + return new SetRelay(this); + } + + @Override + public void writeToParcel(Parcel dest, int flags) { + super.writeToParcel(dest, flags); + dest.writeInt(this.relayNumber); + dest.writeByte(enabled ? (byte) 1 : (byte) 0); + } + + private SetRelay(Parcel in) { + super(in); + this.relayNumber = in.readInt(); + this.enabled = in.readByte() != 0; + } + + public static final Creator CREATOR = new Creator() { + public SetRelay createFromParcel(Parcel source) { + return new SetRelay(source); + } + + public SetRelay[] newArray(int size) { + return new SetRelay[size]; + } + }; +} diff --git a/ClientLib/mobile/build.gradle b/ClientLib/mobile/build.gradle index 1b267bd683..21d13f04e1 100644 --- a/ClientLib/mobile/build.gradle +++ b/ClientLib/mobile/build.gradle @@ -2,7 +2,7 @@ apply plugin: 'com.android.library' ext { PUBLISH_ARTIFACT_ID = '3dr-services-lib' - PUBLISH_VERSION = '2.2.8' + PUBLISH_VERSION = '2.2.10' PROJECT_DESCRIPTION = "3DR Services Client Library" PROJECT_LABELS = ['3DR', '3DR Services', 'DroneAPI', 'Android'] PROJECT_LICENSES = ['Apache-2.0'] @@ -15,7 +15,7 @@ android { defaultConfig { minSdkVersion 14 targetSdkVersion 21 - versionCode 20208 + versionCode 20210 versionName PUBLISH_VERSION } diff --git a/ClientLib/mobile/libs/AidlLib.jar b/ClientLib/mobile/libs/AidlLib.jar index 7143798fe8..a48ef164f4 100644 Binary files a/ClientLib/mobile/libs/AidlLib.jar and b/ClientLib/mobile/libs/AidlLib.jar differ diff --git a/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java b/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java index 0475b52ce9..8964578e25 100644 --- a/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java +++ b/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java @@ -599,7 +599,7 @@ public void setMission(Mission mission, boolean pushToDrone) { List itemsList = mission.getMissionItems(); for (MissionItem item : itemsList) { - droneMission.addMissionItem(ProxyUtils.getMissionItem(droneMission, item)); + droneMission.addMissionItem(ProxyUtils.getMissionItemImpl(droneMission, item)); } if (pushToDrone) @@ -815,7 +815,7 @@ public void buildComplexMissionItem(Bundle itemBundle) { private Survey buildSurvey(Survey survey) { org.droidplanner.core.mission.Mission droneMission = this.droneMgr.getDrone().getMission(); - org.droidplanner.core.mission.survey.Survey updatedSurvey = (org.droidplanner.core.mission.survey.Survey) ProxyUtils.getMissionItem + org.droidplanner.core.mission.survey.Survey updatedSurvey = (org.droidplanner.core.mission.survey.Survey) ProxyUtils.getMissionItemImpl (droneMission, survey); return (Survey) ProxyUtils.getProxyMissionItem(updatedSurvey); @@ -824,7 +824,7 @@ private Survey buildSurvey(Survey survey) { private StructureScanner buildStructureScanner(StructureScanner item) { org.droidplanner.core.mission.Mission droneMission = this.droneMgr.getDrone().getMission(); org.droidplanner.core.mission.waypoints.StructureScanner updatedScan = (org.droidplanner.core.mission.waypoints.StructureScanner) ProxyUtils - .getMissionItem(droneMission, item); + .getMissionItemImpl(droneMission, item); StructureScanner proxyScanner = (StructureScanner) ProxyUtils.getProxyMissionItem(updatedScan); return proxyScanner; diff --git a/ServiceApp/src/org/droidplanner/services/android/utils/ProxyUtils.java b/ServiceApp/src/org/droidplanner/services/android/utils/ProxyUtils.java index ea8d4d861d..8f9afca5dc 100644 --- a/ServiceApp/src/org/droidplanner/services/android/utils/ProxyUtils.java +++ b/ServiceApp/src/org/droidplanner/services/android/utils/ProxyUtils.java @@ -7,6 +7,7 @@ import com.o3dr.services.android.lib.drone.mission.item.command.ChangeSpeed; import com.o3dr.services.android.lib.drone.mission.item.command.EpmGripper; import com.o3dr.services.android.lib.drone.mission.item.command.ReturnToLaunch; +import com.o3dr.services.android.lib.drone.mission.item.command.SetRelay; import com.o3dr.services.android.lib.drone.mission.item.command.SetServo; import com.o3dr.services.android.lib.drone.mission.item.command.Takeoff; import com.o3dr.services.android.lib.drone.mission.item.command.YawCondition; @@ -27,6 +28,7 @@ import org.droidplanner.core.mission.Mission; 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.survey.CameraInfo; import org.droidplanner.core.survey.SurveyData; @@ -72,12 +74,11 @@ public static SurveyDetail getSurveyDetail(SurveyData surveyData) { return surveyDetail; } - public static org.droidplanner.core.mission.MissionItem getMissionItem(Mission mission, - MissionItem proxyItem) { + public static org.droidplanner.core.mission.MissionItem getMissionItemImpl(Mission mission, MissionItem proxyItem) { if (proxyItem == null) return null; - org.droidplanner.core.mission.MissionItem missionItem; + org.droidplanner.core.mission.MissionItem missionItemImpl; switch (proxyItem.getType()) { case CAMERA_TRIGGER: { @@ -87,7 +88,7 @@ public static org.droidplanner.core.mission.MissionItem getMissionItem(Mission m .core.mission.commands.CameraTrigger(mission, new Length(proxy.getTriggerDistance())); - missionItem = temp; + missionItemImpl = temp; break; } case CHANGE_SPEED: { @@ -96,7 +97,7 @@ public static org.droidplanner.core.mission.MissionItem getMissionItem(Mission m org.droidplanner.core.mission.commands.ChangeSpeed temp = new org.droidplanner .core.mission.commands.ChangeSpeed(mission, new Speed(proxy.getSpeed())); - missionItem = temp; + missionItemImpl = temp; break; } case EPM_GRIPPER: { @@ -105,7 +106,7 @@ public static org.droidplanner.core.mission.MissionItem getMissionItem(Mission m org.droidplanner.core.mission.commands.EpmGripper temp = new org.droidplanner .core.mission.commands.EpmGripper(mission, proxy.isRelease()); - missionItem = temp; + missionItemImpl = temp; break; } case RETURN_TO_LAUNCH: { @@ -114,7 +115,7 @@ public static org.droidplanner.core.mission.MissionItem getMissionItem(Mission m ReturnToHome temp = new ReturnToHome(mission); temp.setHeight(new Altitude(proxy.getReturnAltitude())); - missionItem = temp; + missionItemImpl = temp; break; } case SET_SERVO: { @@ -123,7 +124,7 @@ public static org.droidplanner.core.mission.MissionItem getMissionItem(Mission m org.droidplanner.core.mission.commands.SetServo temp = new org.droidplanner.core .mission.commands.SetServo(mission, proxy.getChannel(), proxy.getPwm()); - missionItem = temp; + missionItemImpl = temp; break; } case TAKEOFF: { @@ -132,7 +133,7 @@ public static org.droidplanner.core.mission.MissionItem getMissionItem(Mission m org.droidplanner.core.mission.commands.Takeoff temp = new org.droidplanner.core .mission.commands.Takeoff(mission, new Altitude(proxy.getTakeoffAltitude())); - missionItem = temp; + missionItemImpl = temp; break; } case CIRCLE: { @@ -144,7 +145,7 @@ public static org.droidplanner.core.mission.MissionItem getMissionItem(Mission m temp.setRadius(proxy.getRadius()); temp.setTurns(proxy.getTurns()); - missionItem = temp; + missionItemImpl = temp; break; } case LAND: { @@ -154,7 +155,7 @@ public static org.droidplanner.core.mission.MissionItem getMissionItem(Mission m .mission.waypoints.Land(mission, MathUtils.latLongToCoord2D(proxy .getCoordinate())); - missionItem = temp; + missionItemImpl = temp; break; } case REGION_OF_INTEREST: { @@ -164,7 +165,7 @@ public static org.droidplanner.core.mission.MissionItem getMissionItem(Mission m .droidplanner.core.mission.waypoints.RegionOfInterest(mission, MathUtils.latLongAltToCoord3D(proxy.getCoordinate())); - missionItem = temp; + missionItemImpl = temp; break; } case SPLINE_WAYPOINT: { @@ -175,7 +176,7 @@ public static org.droidplanner.core.mission.MissionItem getMissionItem(Mission m MathUtils.latLongAltToCoord3D(proxy.getCoordinate())); temp.setDelay(proxy.getDelay()); - missionItem = temp; + missionItemImpl = temp; break; } case STRUCTURE_SCANNER: { @@ -193,7 +194,7 @@ public static org.droidplanner.core.mission.MissionItem getMissionItem(Mission m if(camDetail != null) temp.setCamera(getCameraInfo(camDetail)); - missionItem = temp; + missionItemImpl = temp; break; } case WAYPOINT: { @@ -208,7 +209,7 @@ public static org.droidplanner.core.mission.MissionItem getMissionItem(Mission m temp.setOrbitalRadius(proxy.getOrbitalRadius()); temp.setYawAngle(proxy.getYawAngle()); - missionItem = temp; + missionItemImpl = temp; break; } case SURVEY: { @@ -234,7 +235,7 @@ public static org.droidplanner.core.mission.MissionItem getMissionItem(Mission m Log.e(TAG, e.getMessage(), e); } - missionItem = temp; + missionItemImpl = temp; break; } case YAW_CONDITION: { @@ -243,26 +244,31 @@ public static org.droidplanner.core.mission.MissionItem getMissionItem(Mission m ConditionYaw temp = new ConditionYaw(mission, proxy.getAngle(), proxy.isRelative()); temp.setAngularSpeed(proxy.getAngularSpeed()); - missionItem = temp; + missionItemImpl = temp; break; } + case SET_RELAY: + SetRelay proxy = (SetRelay) proxyItem; + missionItemImpl = new SetRelayImpl(mission, proxy.getRelayNumber(), proxy.isEnabled()); + break; + default: - missionItem = null; + missionItemImpl = null; break; } - return missionItem; + return missionItemImpl; } - public static MissionItem getProxyMissionItem(org.droidplanner.core.mission.MissionItem item) { - if (item == null) + public static MissionItem getProxyMissionItem(org.droidplanner.core.mission.MissionItem itemImpl) { + if (itemImpl == null) return null; MissionItem proxyMissionItem; - switch (item.getType()) { + switch (itemImpl.getType()) { case WAYPOINT: { - org.droidplanner.core.mission.waypoints.Waypoint source = (org.droidplanner.core.mission.waypoints.Waypoint) item; + org.droidplanner.core.mission.waypoints.Waypoint source = (org.droidplanner.core.mission.waypoints.Waypoint) itemImpl; Waypoint temp = new Waypoint(); temp.setCoordinate(MathUtils.coord3DToLatLongAlt(source.getCoordinate())); @@ -277,7 +283,7 @@ public static MissionItem getProxyMissionItem(org.droidplanner.core.mission.Miss } case SPLINE_WAYPOINT: { - org.droidplanner.core.mission.waypoints.SplineWaypoint source = (org.droidplanner.core.mission.waypoints.SplineWaypoint) item; + org.droidplanner.core.mission.waypoints.SplineWaypoint source = (org.droidplanner.core.mission.waypoints.SplineWaypoint) itemImpl; SplineWaypoint temp = new SplineWaypoint(); temp.setCoordinate(MathUtils.coord3DToLatLongAlt(source.getCoordinate())); @@ -288,7 +294,7 @@ public static MissionItem getProxyMissionItem(org.droidplanner.core.mission.Miss } case TAKEOFF: { - org.droidplanner.core.mission.commands.Takeoff source = (org.droidplanner.core.mission.commands.Takeoff) item; + org.droidplanner.core.mission.commands.Takeoff source = (org.droidplanner.core.mission.commands.Takeoff) itemImpl; Takeoff temp = new Takeoff(); temp.setTakeoffAltitude(source.getFinishedAlt().valueInMeters()); @@ -297,7 +303,7 @@ public static MissionItem getProxyMissionItem(org.droidplanner.core.mission.Miss break; } case RTL: { - ReturnToHome source = (ReturnToHome) item; + ReturnToHome source = (ReturnToHome) itemImpl; ReturnToLaunch temp = new ReturnToLaunch(); temp.setReturnAltitude(source.getHeight().valueInMeters()); @@ -306,7 +312,7 @@ public static MissionItem getProxyMissionItem(org.droidplanner.core.mission.Miss break; } case LAND: { - org.droidplanner.core.mission.waypoints.Land source = (org.droidplanner.core.mission.waypoints.Land) item; + org.droidplanner.core.mission.waypoints.Land source = (org.droidplanner.core.mission.waypoints.Land) itemImpl; Land temp = new Land(); temp.setCoordinate(MathUtils.coord3DToLatLongAlt(source.getCoordinate())); @@ -316,7 +322,7 @@ public static MissionItem getProxyMissionItem(org.droidplanner.core.mission.Miss } case CIRCLE: { - org.droidplanner.core.mission.waypoints.Circle source = (org.droidplanner.core.mission.waypoints.Circle) item; + org.droidplanner.core.mission.waypoints.Circle source = (org.droidplanner.core.mission.waypoints.Circle) itemImpl; Circle temp = new Circle(); temp.setCoordinate(MathUtils.coord3DToLatLongAlt(source.getCoordinate())); @@ -328,7 +334,7 @@ public static MissionItem getProxyMissionItem(org.droidplanner.core.mission.Miss } case ROI: { - org.droidplanner.core.mission.waypoints.RegionOfInterest source = (org.droidplanner.core.mission.waypoints.RegionOfInterest) item; + org.droidplanner.core.mission.waypoints.RegionOfInterest source = (org.droidplanner.core.mission.waypoints.RegionOfInterest) itemImpl; RegionOfInterest temp = new RegionOfInterest(); temp.setCoordinate(MathUtils.coord3DToLatLongAlt(source.getCoordinate())); @@ -338,7 +344,7 @@ public static MissionItem getProxyMissionItem(org.droidplanner.core.mission.Miss } case SURVEY: { - org.droidplanner.core.mission.survey.Survey source = (org.droidplanner.core.mission.survey.Survey) item; + org.droidplanner.core.mission.survey.Survey source = (org.droidplanner.core.mission.survey.Survey) itemImpl; boolean isValid = true; try { @@ -364,7 +370,7 @@ public static MissionItem getProxyMissionItem(org.droidplanner.core.mission.Miss } case CYLINDRICAL_SURVEY: { - org.droidplanner.core.mission.waypoints.StructureScanner source = (org.droidplanner.core.mission.waypoints.StructureScanner) item; + org.droidplanner.core.mission.waypoints.StructureScanner source = (org.droidplanner.core.mission.waypoints.StructureScanner) itemImpl; StructureScanner temp = new StructureScanner(); temp.setSurveyDetail(getSurveyDetail(source.getSurveyData())); @@ -379,7 +385,7 @@ public static MissionItem getProxyMissionItem(org.droidplanner.core.mission.Miss break; } case CHANGE_SPEED: { - org.droidplanner.core.mission.commands.ChangeSpeed source = (org.droidplanner.core.mission.commands.ChangeSpeed) item; + org.droidplanner.core.mission.commands.ChangeSpeed source = (org.droidplanner.core.mission.commands.ChangeSpeed) itemImpl; ChangeSpeed temp = new ChangeSpeed(); temp.setSpeed(source.getSpeed().valueInMetersPerSecond()); @@ -389,7 +395,7 @@ public static MissionItem getProxyMissionItem(org.droidplanner.core.mission.Miss } case CAMERA_TRIGGER: { - org.droidplanner.core.mission.commands.CameraTrigger source = (org.droidplanner.core.mission.commands.CameraTrigger) item; + org.droidplanner.core.mission.commands.CameraTrigger source = (org.droidplanner.core.mission.commands.CameraTrigger) itemImpl; CameraTrigger temp = new CameraTrigger(); temp.setTriggerDistance(source.getTriggerDistance().valueInMeters()); @@ -398,7 +404,7 @@ public static MissionItem getProxyMissionItem(org.droidplanner.core.mission.Miss break; } case EPM_GRIPPER: { - org.droidplanner.core.mission.commands.EpmGripper source = (org.droidplanner.core.mission.commands.EpmGripper) item; + org.droidplanner.core.mission.commands.EpmGripper source = (org.droidplanner.core.mission.commands.EpmGripper) itemImpl; EpmGripper temp = new EpmGripper(); temp.setRelease(source.isRelease()); @@ -408,7 +414,7 @@ public static MissionItem getProxyMissionItem(org.droidplanner.core.mission.Miss } case SET_SERVO: { - org.droidplanner.core.mission.commands.SetServo source = (org.droidplanner.core.mission.commands.SetServo) item; + org.droidplanner.core.mission.commands.SetServo source = (org.droidplanner.core.mission.commands.SetServo) itemImpl; SetServo temp = new SetServo(); temp.setChannel(source.getChannel()); @@ -418,7 +424,7 @@ public static MissionItem getProxyMissionItem(org.droidplanner.core.mission.Miss break; } case CONDITION_YAW: { - ConditionYaw source = (ConditionYaw) item; + ConditionYaw source = (ConditionYaw) itemImpl; YawCondition temp = new YawCondition(); temp.setAngle(source.getAngle()); @@ -429,6 +435,17 @@ public static MissionItem getProxyMissionItem(org.droidplanner.core.mission.Miss break; } + case SET_RELAY:{ + SetRelayImpl impl = (SetRelayImpl) itemImpl; + + SetRelay proxy = new SetRelay(); + proxy.setRelayNumber(impl.getRelayNumber()); + proxy.setEnabled(impl.isEnabled()); + + proxyMissionItem = proxy; + break; + } + default: proxyMissionItem = null; break; diff --git a/dependencyLibs/Core/src/org/droidplanner/core/mission/MissionItemType.java b/dependencyLibs/Core/src/org/droidplanner/core/mission/MissionItemType.java index 559110e58a..22af8ae20f 100644 --- a/dependencyLibs/Core/src/org/droidplanner/core/mission/MissionItemType.java +++ b/dependencyLibs/Core/src/org/droidplanner/core/mission/MissionItemType.java @@ -1,13 +1,12 @@ package org.droidplanner.core.mission; -import java.util.Collections; - import org.droidplanner.core.helpers.coordinates.Coord2D; import org.droidplanner.core.mission.commands.CameraTrigger; import org.droidplanner.core.mission.commands.ChangeSpeed; import org.droidplanner.core.mission.commands.ConditionYaw; import org.droidplanner.core.mission.commands.EpmGripper; import org.droidplanner.core.mission.commands.ReturnToHome; +import org.droidplanner.core.mission.commands.SetRelayImpl; import org.droidplanner.core.mission.commands.SetServo; import org.droidplanner.core.mission.commands.Takeoff; import org.droidplanner.core.mission.survey.Survey; @@ -18,54 +17,69 @@ import org.droidplanner.core.mission.waypoints.StructureScanner; import org.droidplanner.core.mission.waypoints.Waypoint; +import java.util.Collections; + public enum MissionItemType { - WAYPOINT("Waypoint"), SPLINE_WAYPOINT("Spline Waypoint"), TAKEOFF("Takeoff"), RTL( - "Return to Launch"), LAND("Land"), CIRCLE("Circle"), ROI("Region of Interest"), SURVEY( - "Survey"), CYLINDRICAL_SURVEY("Structure Scan"), CHANGE_SPEED("Change Speed"), CAMERA_TRIGGER("Camera Trigger"), EPM_GRIPPER("EPM"), SET_SERVO("Set Servo"), CONDITION_YAW("Set Yaw"); + WAYPOINT("Waypoint"), + SPLINE_WAYPOINT("Spline Waypoint"), + TAKEOFF("Takeoff"), + RTL("Return to Launch"), + LAND("Land"), + CIRCLE("Circle"), + ROI("Region of Interest"), + SURVEY("Survey"), + CYLINDRICAL_SURVEY("Structure Scan"), + CHANGE_SPEED("Change Speed"), + CAMERA_TRIGGER("Camera Trigger"), + EPM_GRIPPER("EPM"), + SET_SERVO("Set Servo"), + CONDITION_YAW("Set Yaw"), + SET_RELAY("Set Relay"); - private final String name; + private final String name; - private MissionItemType(String name) { - this.name = name; - } + private MissionItemType(String name) { + this.name = name; + } - public String getName() { - return name; - } + public String getName() { + return name; + } - public MissionItem getNewItem(MissionItem referenceItem) throws IllegalArgumentException { - switch (this) { - case WAYPOINT: - return new Waypoint(referenceItem); - case SPLINE_WAYPOINT: - return new SplineWaypoint(referenceItem); - case TAKEOFF: - return new Takeoff(referenceItem); - case CHANGE_SPEED: - return new ChangeSpeed(referenceItem); - case CAMERA_TRIGGER: - return new CameraTrigger(referenceItem); - case EPM_GRIPPER: - return new EpmGripper(referenceItem); - case RTL: - return new ReturnToHome(referenceItem); - case LAND: - return new Land(referenceItem); - case CIRCLE: - return new Circle(referenceItem); - case ROI: - return new RegionOfInterest(referenceItem); - case SURVEY: - return new Survey(referenceItem.getMission(), Collections. emptyList()); - case CYLINDRICAL_SURVEY: - return new StructureScanner(referenceItem); - case SET_SERVO: - return new SetServo(referenceItem); - case CONDITION_YAW: - return new ConditionYaw(referenceItem); - default: - throw new IllegalArgumentException("Unrecognized mission item type (" + name + ")" - + ""); - } - } + public MissionItem getNewItem(MissionItem referenceItem) throws IllegalArgumentException { + switch (this) { + case WAYPOINT: + return new Waypoint(referenceItem); + case SPLINE_WAYPOINT: + return new SplineWaypoint(referenceItem); + case TAKEOFF: + return new Takeoff(referenceItem); + case CHANGE_SPEED: + return new ChangeSpeed(referenceItem); + case CAMERA_TRIGGER: + return new CameraTrigger(referenceItem); + case EPM_GRIPPER: + return new EpmGripper(referenceItem); + case RTL: + return new ReturnToHome(referenceItem); + case LAND: + return new Land(referenceItem); + case CIRCLE: + return new Circle(referenceItem); + case ROI: + return new RegionOfInterest(referenceItem); + case SURVEY: + return new Survey(referenceItem.getMission(), Collections.emptyList()); + case CYLINDRICAL_SURVEY: + return new StructureScanner(referenceItem); + case SET_SERVO: + return new SetServo(referenceItem); + case CONDITION_YAW: + return new ConditionYaw(referenceItem); + case SET_RELAY: + return new SetRelayImpl(referenceItem); + default: + throw new IllegalArgumentException("Unrecognized mission item type (" + name + ")" + ""); + } + } } diff --git a/dependencyLibs/Core/src/org/droidplanner/core/mission/commands/SetRelayImpl.java b/dependencyLibs/Core/src/org/droidplanner/core/mission/commands/SetRelayImpl.java new file mode 100644 index 0000000000..c8c497d4a2 --- /dev/null +++ b/dependencyLibs/Core/src/org/droidplanner/core/mission/commands/SetRelayImpl.java @@ -0,0 +1,64 @@ +package org.droidplanner.core.mission.commands; + +import com.MAVLink.common.msg_mission_item; +import com.MAVLink.enums.MAV_CMD; + +import org.droidplanner.core.mission.Mission; +import org.droidplanner.core.mission.MissionItem; +import org.droidplanner.core.mission.MissionItemType; + +import java.util.List; + +/** + * Mavlink message builder for the 'SetRelay' mission item. + * Set a Relay pin’s voltage high or low. + */ +public class SetRelayImpl extends MissionCMD { + + private int relayNumber; + private boolean enabled; + + public SetRelayImpl(MissionItem item){ + super(item); + } + + public SetRelayImpl(msg_mission_item msg, Mission mission){ + super(mission); + unpackMAVMessage(msg); + } + + public SetRelayImpl(Mission mission, int relayNumber, boolean enabled){ + super(mission); + this.relayNumber = relayNumber; + this.enabled = enabled; + } + + @Override + public MissionItemType getType(){ + return MissionItemType.SET_RELAY; + } + + @Override + public void unpackMAVMessage(msg_mission_item mavMsg){ + relayNumber = (int) mavMsg.param1; + enabled = mavMsg.param2 != 0; + } + + @Override + public List packMissionItem(){ + List list = super.packMissionItem(); + msg_mission_item mavMsg = list.get(0); + mavMsg.command = MAV_CMD.MAV_CMD_DO_SET_RELAY; + mavMsg.param1 = relayNumber; + mavMsg.param2 = enabled ? 1 : 0; + return list; + } + + public int getRelayNumber() { + return relayNumber; + } + + public boolean isEnabled() { + return enabled; + } +}