diff --git a/ClientLib/src/androidTest/java/com/o3dr/services/android/lib/util/MathUtilsTest.java b/ClientLib/src/androidTest/java/com/o3dr/services/android/lib/util/MathUtilsTest.java index 162db05696..a32c895bb9 100644 --- a/ClientLib/src/androidTest/java/com/o3dr/services/android/lib/util/MathUtilsTest.java +++ b/ClientLib/src/androidTest/java/com/o3dr/services/android/lib/util/MathUtilsTest.java @@ -1,5 +1,6 @@ package com.o3dr.services.android.lib.util; +import com.o3dr.services.android.lib.coordinate.Frame; import com.o3dr.services.android.lib.coordinate.LatLong; import com.o3dr.services.android.lib.coordinate.LatLongAlt; @@ -44,7 +45,8 @@ public class MathUtilsTest extends TestCase { public void testGetDistance3D() throws Exception { //Test get distance altitude only. - double distance = MathUtils.getDistance3D(new LatLongAlt(0.0, 0.0, 50.0), new LatLongAlt(0.0, 0.0, 100.0)); + double distance = MathUtils.getDistance3D(new LatLongAlt(0.0, 0.0, 50.0, Frame.GLOBAL_RELATIVE), + new LatLongAlt(0.0, 0.0, 100.0, Frame.GLOBAL_RELATIVE)); assertEquals(distance, 50.0, MARGIN_OF_ERROR); } @@ -80,7 +82,8 @@ public void testGetDistance() throws Exception { toLongitude = randDouble(rand, MIN_LONGITUDE, MAX_LONGITUDE); //3D Distance equation - distance1 = MathUtils.getDistance3D(new LatLongAlt(fromLatitude, fromLongitude, 0.0), new LatLongAlt(toLatitude, toLongitude, 0.0)); + distance1 = MathUtils.getDistance3D(new LatLongAlt(fromLatitude, fromLongitude, 0.0, Frame.GLOBAL_RELATIVE), + new LatLongAlt(toLatitude, toLongitude, 0.0, Frame.GLOBAL_RELATIVE)); //2D Distance equation distance2 = MathUtils.getDistance2D(new LatLong(fromLatitude, fromLongitude), new LatLong(toLatitude, toLongitude)); diff --git a/ClientLib/src/androidTest/java/org/droidplanner/services/android/impl/core/helpers/coordinates/Coord3DTest.java b/ClientLib/src/androidTest/java/org/droidplanner/services/android/impl/core/helpers/coordinates/Coord3DTest.java index c535556833..a0c81d2433 100644 --- a/ClientLib/src/androidTest/java/org/droidplanner/services/android/impl/core/helpers/coordinates/Coord3DTest.java +++ b/ClientLib/src/androidTest/java/org/droidplanner/services/android/impl/core/helpers/coordinates/Coord3DTest.java @@ -1,5 +1,6 @@ package org.droidplanner.services.android.impl.core.helpers.coordinates; +import com.o3dr.services.android.lib.coordinate.Frame; import com.o3dr.services.android.lib.coordinate.LatLong; import com.o3dr.services.android.lib.coordinate.LatLongAlt; @@ -9,7 +10,7 @@ public class Coord3DTest extends TestCase { public void testConstructor() { double alt = (50.0); - LatLongAlt point = new LatLongAlt(10, -5.6, alt); + LatLongAlt point = new LatLongAlt(10, -5.6, alt, Frame.GLOBAL_RELATIVE); assertEquals(10.0, point.getLatitude()); assertEquals(-5.6, point.getLongitude()); assertEquals(alt, point.getAltitude()); @@ -18,7 +19,7 @@ public void testConstructor() { public void testConstFrom2dCoord() { double alt = (5.0); LatLong point2d = new LatLong(1, -0.6); - LatLongAlt point = new LatLongAlt(point2d, alt); + LatLongAlt point = new LatLongAlt(point2d, alt, Frame.GLOBAL_RELATIVE); assertEquals(1.0, point.getLatitude()); assertEquals(-0.6, point.getLongitude()); @@ -27,7 +28,7 @@ public void testConstFrom2dCoord() { public void testSet() { double alt = (50.0); - LatLongAlt point = new LatLongAlt(10, -5.6, alt); + LatLongAlt point = new LatLongAlt(10, -5.6, alt, Frame.GLOBAL_RELATIVE); point.setLatitude(0); point.setLongitude(0); point.setAltitude(alt); diff --git a/ClientLib/src/androidTest/java/org/droidplanner/services/android/impl/core/helpers/geoTools/GeoToolsTest.java b/ClientLib/src/androidTest/java/org/droidplanner/services/android/impl/core/helpers/geoTools/GeoToolsTest.java index 9e45ebcf2b..ceae663556 100644 --- a/ClientLib/src/androidTest/java/org/droidplanner/services/android/impl/core/helpers/geoTools/GeoToolsTest.java +++ b/ClientLib/src/androidTest/java/org/droidplanner/services/android/impl/core/helpers/geoTools/GeoToolsTest.java @@ -1,5 +1,6 @@ package org.droidplanner.services.android.impl.core.helpers.geoTools; +import com.o3dr.services.android.lib.coordinate.Frame; import com.o3dr.services.android.lib.coordinate.LatLong; import com.o3dr.services.android.lib.coordinate.LatLongAlt; @@ -10,8 +11,8 @@ public class GeoToolsTest extends TestCase { - LatLongAlt p1 = new LatLongAlt(37.85363485683941, -122.4204097390123, (250.0)); - LatLongAlt p2 = new LatLongAlt(37.85130335221235, -122.4142645673542, (0.0)); + LatLongAlt p1 = new LatLongAlt(37.85363485683941, -122.4204097390123, (250.0), Frame.GLOBAL_RELATIVE); + LatLongAlt p2 = new LatLongAlt(37.85130335221235, -122.4142645673542, (0.0), Frame.GLOBAL_RELATIVE); double polygonArea = 502915; double dist2DP1toP2 = 599.26; double dist3DP1toP2 = 649.32; diff --git a/ClientLib/src/androidTest/java/org/droidplanner/services/android/impl/core/mission/waypoints/SpatialCoordItemTest.java b/ClientLib/src/androidTest/java/org/droidplanner/services/android/impl/core/mission/waypoints/SpatialCoordItemTest.java index 7ebed0749d..92c6ea2eb6 100644 --- a/ClientLib/src/androidTest/java/org/droidplanner/services/android/impl/core/mission/waypoints/SpatialCoordItemTest.java +++ b/ClientLib/src/androidTest/java/org/droidplanner/services/android/impl/core/mission/waypoints/SpatialCoordItemTest.java @@ -2,6 +2,7 @@ import com.MAVLink.common.msg_mission_item; import com.MAVLink.enums.MAV_FRAME; +import com.o3dr.services.android.lib.coordinate.Frame; import com.o3dr.services.android.lib.coordinate.LatLongAlt; import junit.framework.TestCase; @@ -12,7 +13,7 @@ public class SpatialCoordItemTest extends TestCase { public void testPackMissionItem() { MissionImpl missionImpl = new MissionImpl(null); - WaypointImpl item = new WaypointImpl(missionImpl, new LatLongAlt(0.1, 1, (2))); + WaypointImpl item = new WaypointImpl(missionImpl, new LatLongAlt(0.1, 1, (2), Frame.GLOBAL_RELATIVE)); msg_mission_item mavMsg = item.packMissionItem().get(0); diff --git a/ClientLib/src/androidTest/java/org/droidplanner/services/android/impl/core/mission/waypoints/WaypointImplTest.java b/ClientLib/src/androidTest/java/org/droidplanner/services/android/impl/core/mission/waypoints/WaypointImplTest.java index 23665b1e05..65e89a6358 100644 --- a/ClientLib/src/androidTest/java/org/droidplanner/services/android/impl/core/mission/waypoints/WaypointImplTest.java +++ b/ClientLib/src/androidTest/java/org/droidplanner/services/android/impl/core/mission/waypoints/WaypointImplTest.java @@ -2,6 +2,7 @@ import com.MAVLink.common.msg_mission_item; import com.MAVLink.enums.MAV_CMD; +import com.o3dr.services.android.lib.coordinate.Frame; import com.o3dr.services.android.lib.coordinate.LatLongAlt; import junit.framework.TestCase; @@ -14,7 +15,7 @@ public class WaypointImplTest extends TestCase { public void testPackMissionItem() { MissionImpl missionImpl = new MissionImpl(null); - WaypointImpl item = new WaypointImpl(missionImpl, new LatLongAlt(0, 1, (2))); + WaypointImpl item = new WaypointImpl(missionImpl, new LatLongAlt(0, 1, (2), Frame.GLOBAL_RELATIVE)); List listOfMsg = item.packMissionItem(); assertEquals(1, listOfMsg.size()); diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/coordinate/Frame.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/coordinate/Frame.java new file mode 100644 index 0000000000..126500fce1 --- /dev/null +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/coordinate/Frame.java @@ -0,0 +1,66 @@ +package com.o3dr.services.android.lib.coordinate; + +import com.MAVLink.enums.MAV_FRAME; + +/** TODO This is the start of using a more generic location/frame class + * The frame for LatLongAlt would only ever be GLOBAL and not LOCAL. + */ + +public enum Frame { + GLOBAL_ABS ("Absolute" , "Abs", 0 ), // Absolute means Above Mean Sea Level AMSL + LOCAL_NED ("Local NED", "NED", 1 ), + MISSION ("Mission" , "MIS", 2 ), + GLOBAL_RELATIVE ("Relative" , "Rel", 3 ), // Relative to HOME location + LOCAL_ENU ("Local ENU", "ENU", 4 ), + GLOBAL_TERRAIN ("Terrain" , "Ter", 10); // Relative to Terrain Level. (Either measured or from STRM) + + private final int frame; + private final String name; + private final String abbreviation; + + Frame(String name, String abbreviation, int frame ) { + this.name = name; + this.abbreviation = abbreviation; + this.frame = frame; + } + + public String getName() { + return name; + } + + public String getAbbreviation() { + return abbreviation; + } + + public int asInt() { + return frame; + } + + public static Frame getFrame(int mavFrame) { + + switch (mavFrame) { + case MAV_FRAME.MAV_FRAME_GLOBAL: + case MAV_FRAME.MAV_FRAME_GLOBAL_INT: + return Frame.GLOBAL_ABS; + + case MAV_FRAME.MAV_FRAME_MISSION: + return Frame.MISSION; + + case MAV_FRAME.MAV_FRAME_LOCAL_NED: + return Frame.LOCAL_NED; + + case MAV_FRAME.MAV_FRAME_LOCAL_ENU: + return Frame.LOCAL_NED; + + case MAV_FRAME.MAV_FRAME_GLOBAL_TERRAIN_ALT: + case MAV_FRAME.MAV_FRAME_GLOBAL_TERRAIN_ALT_INT: + return Frame.GLOBAL_TERRAIN; + + case MAV_FRAME.MAV_FRAME_GLOBAL_RELATIVE_ALT: + case MAV_FRAME.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT: + default: + return Frame.GLOBAL_RELATIVE; + } + + } +} diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/coordinate/LatLong.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/coordinate/LatLong.java index 261104da61..477663c277 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/coordinate/LatLong.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/coordinate/LatLong.java @@ -18,6 +18,11 @@ public class LatLong implements Parcelable, Serializable { private double latitude; private double longitude; + public LatLong(){ + this.latitude = -100.0; // TODO: Should we set this to invalid ie -100.0 ? + this.longitude = -190.0;// TODO: Should we set this to invalid ie -190.0 ? + } + public LatLong(double latitude, double longitude){ this.latitude = latitude; this.longitude = longitude; @@ -32,6 +37,23 @@ public void set(LatLong update){ this.longitude = update.longitude; } + /** + * @return if this is a valid LatLong global point + */ + public boolean isValid() { + + if ( this.longitude > 180.0 || this.latitude > 90.0 + || this.longitude < -180.0 || this.latitude < -90.0 ) { + return false; // Not a valid location + + } if (Double.compare(this.longitude, 0.0) == 0 && Double.compare(this.latitude, 0.0) == 0) { + return false; // Rarely in 0.0,0.0 a valid location, so reject. + + } else { + return true; + } + } + /** * @return the latitude in degrees */ diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/coordinate/LatLongAlt.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/coordinate/LatLongAlt.java index 68c8d5cefe..1d4aaf659f 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/coordinate/LatLongAlt.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/coordinate/LatLongAlt.java @@ -6,6 +6,7 @@ /** * Stores latitude, longitude, and altitude information for a coordinate. */ + public class LatLongAlt extends LatLong { private static final long serialVersionUID =-4771550293045623743L; @@ -14,24 +15,34 @@ public class LatLongAlt extends LatLong { * Stores the altitude in meters. */ private double mAltitude; + private Frame mFrame; + + public LatLongAlt() { + super(); + mAltitude = 0.0; + mFrame = Frame.GLOBAL_RELATIVE; + } - public LatLongAlt(double latitude, double longitude, double altitude) { + public LatLongAlt(double latitude, double longitude, double altitude, Frame frame) { super(latitude, longitude); mAltitude = altitude; + mFrame = frame; } - public LatLongAlt(LatLong location, double altitude){ + public LatLongAlt(LatLong location, double altitude, Frame frame){ super(location); mAltitude = altitude; + mFrame = frame; } public LatLongAlt(LatLongAlt copy) { - this(copy.getLatitude(), copy.getLongitude(), copy.getAltitude()); + this(copy.getLatitude(), copy.getLongitude(), copy.getAltitude(), copy.getFrame()); } - public void set(LatLongAlt source){ + public void set(LatLongAlt source){ // TODO: this looks wrong super.set(source); this.mAltitude = source.mAltitude; + this.mFrame = source.mFrame; } /** @@ -45,6 +56,18 @@ public void setAltitude(double altitude) { this.mAltitude = altitude; } + public Frame getFrame() { + return mFrame; + } + + public boolean setFrame(Frame frame) { + if (mFrame != frame) { + mFrame = frame; + return true; + } + return false; + } + @Override public boolean equals(Object o) { if (this == o) return true; @@ -53,7 +76,9 @@ public boolean equals(Object o) { LatLongAlt that = (LatLongAlt) o; - if (Double.compare(that.mAltitude, mAltitude) != 0) return false; + if ((Double.compare(that.mAltitude, mAltitude) != 0) + && (that.mFrame.asInt() != mFrame.asInt()) ) + return false; return true; } @@ -63,7 +88,7 @@ public int hashCode() { int result = super.hashCode(); long temp; temp = Double.doubleToLongBits(mAltitude); - result = 31 * result + (int) (temp ^ (temp >>> 32)); + result = 31 * result + (int) (temp ^ (temp >>> 32)); // TODO Check this hash is OK with frame return result; } @@ -73,6 +98,7 @@ public String toString() { return "LatLongAlt{" + superToString + ", mAltitude=" + mAltitude + + ", mFrame=" + mFrame.getAbbreviation() + '}'; } diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/companion/solo/tlv/SoloCableCamWaypoint.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/companion/solo/tlv/SoloCableCamWaypoint.java index 2cb062851d..4997f928d2 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/companion/solo/tlv/SoloCableCamWaypoint.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/companion/solo/tlv/SoloCableCamWaypoint.java @@ -2,6 +2,7 @@ import android.os.Parcel; +import com.o3dr.services.android.lib.coordinate.Frame; import com.o3dr.services.android.lib.coordinate.LatLongAlt; import java.nio.ByteBuffer; @@ -26,7 +27,7 @@ public class SoloCableCamWaypoint extends TLVPacket { public SoloCableCamWaypoint(double latitude, double longitude, float altitude, float degreesYaw, float pitch) { super(TLVMessageTypes.TYPE_SOLO_CABLE_CAM_WAYPOINT, 28); - this.coordinate = new LatLongAlt(latitude, longitude, altitude); + this.coordinate = new LatLongAlt(latitude, longitude, altitude, Frame.GLOBAL_RELATIVE); this.degreesYaw = degreesYaw; this.pitch = pitch; } diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/companion/solo/tlv/SoloMessageLocation.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/companion/solo/tlv/SoloMessageLocation.java index 9b7cd7c33b..97fde70811 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/companion/solo/tlv/SoloMessageLocation.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/companion/solo/tlv/SoloMessageLocation.java @@ -2,6 +2,7 @@ import android.os.Parcel; +import com.o3dr.services.android.lib.coordinate.Frame; import com.o3dr.services.android.lib.coordinate.LatLongAlt; import java.nio.ByteBuffer; @@ -15,7 +16,7 @@ public class SoloMessageLocation extends TLVPacket { public SoloMessageLocation(double latitude, double longitude, float altitudeInMeters) { super(TLVMessageTypes.TYPE_SOLO_MESSAGE_LOCATION, 20); - this.coordinate = new LatLongAlt(latitude, longitude, altitudeInMeters); + this.coordinate = new LatLongAlt(latitude, longitude, altitudeInMeters, Frame.GLOBAL_RELATIVE); } public SoloMessageLocation(LatLongAlt coordinate){ diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/companion/solo/tlv/SoloReturnHomeLocationMessage.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/companion/solo/tlv/SoloReturnHomeLocationMessage.java index b972d0ba73..e5354fbd24 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/companion/solo/tlv/SoloReturnHomeLocationMessage.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/companion/solo/tlv/SoloReturnHomeLocationMessage.java @@ -2,6 +2,7 @@ import android.os.Parcel; +import com.o3dr.services.android.lib.coordinate.Frame; import com.o3dr.services.android.lib.coordinate.LatLongAlt; import java.nio.ByteBuffer; @@ -19,7 +20,7 @@ public class SoloReturnHomeLocationMessage extends TLVPacket { public SoloReturnHomeLocationMessage(double latitude, double longitude, float altitudeInMeters) { super(TLVMessageTypes.TYPE_RTL_HOME_POINT, 20); - this.coordinate = new LatLongAlt(latitude, longitude, altitudeInMeters); + this.coordinate = new LatLongAlt(latitude, longitude, altitudeInMeters, Frame.GLOBAL_RELATIVE); } public SoloReturnHomeLocationMessage(LatLongAlt coordinate){ diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/companion/solo/tlv/mpcc/SoloSplinePoint.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/companion/solo/tlv/mpcc/SoloSplinePoint.java index d07817d407..5cc0fb7831 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/companion/solo/tlv/mpcc/SoloSplinePoint.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/companion/solo/tlv/mpcc/SoloSplinePoint.java @@ -2,6 +2,7 @@ import android.os.Parcel; +import com.o3dr.services.android.lib.coordinate.Frame; import com.o3dr.services.android.lib.coordinate.LatLongAlt; import com.o3dr.services.android.lib.drone.companion.solo.tlv.TLVMessageTypes; import com.o3dr.services.android.lib.drone.companion.solo.tlv.TLVPacket; @@ -100,7 +101,10 @@ public SoloSplinePoint(ByteBuffer dataBuffer){ this(dataBuffer.getShort(), dataBuffer.getFloat(), dataBuffer.getInt(), - new LatLongAlt(dataBuffer.getDouble(), dataBuffer.getDouble(), dataBuffer.getFloat()), + new LatLongAlt(dataBuffer.getDouble(), + dataBuffer.getDouble(), + dataBuffer.getFloat(), + Frame.GLOBAL_RELATIVE), dataBuffer.getFloat(), dataBuffer.getFloat(), dataBuffer.getFloat(), diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/connection/ConnectionParameter.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/connection/ConnectionParameter.java index 2685f519d6..3eddbba81a 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/connection/ConnectionParameter.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/connection/ConnectionParameter.java @@ -277,7 +277,7 @@ public static ConnectionParameter newSoloConnection(String ssid, @Nullable Strin eventsDispatchingPeriod); } - private ConnectionParameter(@ConnectionType.Type int connectionType, Bundle paramsBundle){ + public ConnectionParameter(@ConnectionType.Type int connectionType, Bundle paramsBundle){ this(connectionType, paramsBundle, null); } diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/BaseSpatialItem.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/BaseSpatialItem.java index b04b0de689..7b538dbbb5 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/BaseSpatialItem.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/BaseSpatialItem.java @@ -54,6 +54,8 @@ public String toString() { @Override public LatLongAlt getCoordinate() { + if (coordinate == null) + coordinate = new LatLongAlt(); return coordinate; } diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/Land.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/Land.java index 5f6fb79081..1e3777093d 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/Land.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/Land.java @@ -12,7 +12,7 @@ public class Land extends BaseSpatialItem implements android.os.Parcelable { public Land(){ - super(MissionItemType.LAND, new LatLongAlt(0.0, 0.0, 0.0)); + super(MissionItemType.LAND); } public Land(Land copy){ diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/property/GuidedState.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/property/GuidedState.java index 5cd9e38f26..de8c29e1e3 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/property/GuidedState.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/property/GuidedState.java @@ -5,6 +5,8 @@ import com.o3dr.services.android.lib.coordinate.LatLongAlt; +import org.droidplanner.services.android.impl.core.drone.variables.GuidedPoint; + /** * Created by fhuya on 11/5/14. */ @@ -48,6 +50,28 @@ public void setCoordinate(LatLongAlt coordinate) { this.coordinate = coordinate; } + public static GuidedState getGuidedStateFromPoint(GuidedPoint guidedPoint) { + if (guidedPoint == null) + return new GuidedState(); + + int guidedState; + switch (guidedPoint.getState()) { + default: + case UNINITIALIZED: + guidedState = GuidedState.STATE_UNINITIALIZED; + break; + + case ACTIVE: + guidedState = GuidedState.STATE_ACTIVE; + break; + + case IDLE: + guidedState = GuidedState.STATE_IDLE; + break; + } + return new GuidedState(guidedState, guidedPoint.getCoord()); + } + @Override public int describeContents() { return 0; diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/property/Home.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/property/Home.java index 91a1ea2e3b..9eda0a82a5 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/property/Home.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/property/Home.java @@ -17,10 +17,6 @@ public class Home implements DroneAttribute { public Home(){} - public Home(double latitude, double longitude, double altitude){ - mCoordinate = new LatLongAlt(latitude, longitude, altitude); - } - public Home(LatLongAlt coordinate){ mCoordinate = coordinate; } diff --git a/ClientLib/src/main/java/org/droidplanner/services/android/impl/api/DroneApi.java b/ClientLib/src/main/java/org/droidplanner/services/android/impl/api/DroneApi.java index 0200ad0599..0e8ffbb5e8 100644 --- a/ClientLib/src/main/java/org/droidplanner/services/android/impl/api/DroneApi.java +++ b/ClientLib/src/main/java/org/droidplanner/services/android/impl/api/DroneApi.java @@ -15,6 +15,7 @@ import com.MAVLink.Messages.MAVLinkMessage; import com.MAVLink.ardupilotmega.msg_mag_cal_progress; import com.MAVLink.ardupilotmega.msg_mag_cal_report; +import com.o3dr.services.android.lib.coordinate.Frame; import com.o3dr.services.android.lib.coordinate.LatLongAlt; import com.o3dr.services.android.lib.drone.action.CameraActions; import com.o3dr.services.android.lib.drone.action.ConnectionActions; @@ -211,7 +212,7 @@ public Bundle getAttribute(String type) throws RemoteException { missionItems.remove(i); RegionOfInterest replacement = new RegionOfInterest(); - replacement.setCoordinate(new LatLongAlt(0, 0, 0)); + replacement.setCoordinate(new LatLongAlt(0, 0, 0, Frame.GLOBAL_RELATIVE)); missionItems.add(i, replacement); } } diff --git a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/MAVLink/MavLinkCommands.java b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/MAVLink/MavLinkCommands.java index d05ca227ef..ad75f33c3c 100644 --- a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/MAVLink/MavLinkCommands.java +++ b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/MAVLink/MavLinkCommands.java @@ -38,7 +38,7 @@ public static void setGuidedMode(MavLinkDrone drone, double latitude, double lon msg_mission_item msg = new msg_mission_item(); msg.seq = 0; msg.current = 2; // TODO use guided mode enum - msg.frame = MAV_FRAME.MAV_FRAME_GLOBAL_RELATIVE_ALT; + msg.frame = drone.getMavFrame(); msg.command = MAV_CMD.MAV_CMD_NAV_WAYPOINT; // msg.param1 = 0; // TODO use correct parameter msg.param2 = 0; // TODO use correct parameter @@ -56,7 +56,7 @@ public static void setGuidedMode(MavLinkDrone drone, double latitude, double lon public static void sendGuidedPosition(MavLinkDrone drone, double latitude, double longitude, double altitude){ msg_set_position_target_global_int msg = new msg_set_position_target_global_int(); msg.type_mask = MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE | MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE; - msg.coordinate_frame = MAV_FRAME.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT; + msg.coordinate_frame = drone.getMavFrameInt(); msg.lat_int = (int) (latitude * 1E7); msg.lon_int = (int) (longitude * 1E7); msg.alt = (float) altitude; @@ -68,7 +68,7 @@ public static void sendGuidedPosition(MavLinkDrone drone, double latitude, doubl public static void sendGuidedVelocity(MavLinkDrone drone, double xVel, double yVel, double zVel){ msg_set_position_target_global_int msg = new msg_set_position_target_global_int(); msg.type_mask = MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE | MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE; - msg.coordinate_frame = MAV_FRAME.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT; + msg.coordinate_frame = drone.getMavFrameInt(); msg.vx = (float) xVel; msg.vy = (float) yVel; msg.vz = (float) zVel; @@ -92,7 +92,7 @@ public static void sendGuidedPositionAndVelocity(MavLinkDrone drone, double lati double xVel, double yVel, double zVel){ msg_set_position_target_global_int msg = new msg_set_position_target_global_int(); msg.type_mask = MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE; - msg.coordinate_frame = MAV_FRAME.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT; + msg.coordinate_frame = drone.getMavFrameInt(); msg.lat_int = (int) (latitude * 1E7); msg.lon_int = (int) (longitude * 1E7); msg.alt = (float) altitude; diff --git a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/MAVLink/command/doCmd/MavLinkDoCmds.java b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/MAVLink/command/doCmd/MavLinkDoCmds.java index b1d41d87f8..5e7e492392 100644 --- a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/MAVLink/command/doCmd/MavLinkDoCmds.java +++ b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/MAVLink/command/doCmd/MavLinkDoCmds.java @@ -8,6 +8,8 @@ import com.MAVLink.enums.MAV_CMD; import org.droidplanner.services.android.impl.core.drone.autopilot.MavLinkDrone; + +import com.o3dr.services.android.lib.coordinate.Frame; import com.o3dr.services.android.lib.coordinate.LatLongAlt; import com.o3dr.services.android.lib.model.ICommandListener; @@ -38,9 +40,16 @@ public static void setROI(MavLinkDrone drone, LatLongAlt coord, ICommandListener msg.target_component = drone.getCompid(); msg.command = MAV_CMD.MAV_CMD_DO_SET_ROI; - msg.param5 = (float) coord.getLatitude(); - msg.param6 = (float) coord.getLongitude(); - msg.param7 = (float) coord.getAltitude(); + if (coord != null) { + msg.param5 = (float) coord.getLatitude(); + msg.param6 = (float) coord.getLongitude(); + msg.param7 = (float) coord.getAltitude(); + + } else { + msg.param5 = 0.0f; + msg.param6 = 0.0f; + msg.param7 = 0.0f; + } drone.getMavClient().sendMessage(msg, listener); } @@ -49,7 +58,7 @@ public static void resetROI(MavLinkDrone drone, ICommandListener listener) { if (drone == null) return; - setROI(drone, new LatLongAlt(0, 0, 0), listener); + setROI(drone, null, listener); } public static void triggerCamera(MavLinkDrone drone) { diff --git a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/autopilot/MavLinkDrone.java b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/autopilot/MavLinkDrone.java index 19a35dfc59..e2dcde6d73 100644 --- a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/autopilot/MavLinkDrone.java +++ b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/autopilot/MavLinkDrone.java @@ -15,6 +15,8 @@ import org.droidplanner.services.android.impl.core.firmware.FirmwareType; import org.droidplanner.services.android.impl.core.mission.MissionImpl; +import com.o3dr.services.android.lib.coordinate.*; + public interface MavLinkDrone extends Drone { String PACKAGE_NAME = "org.droidplanner.services.android.core.drone.autopilot"; @@ -27,36 +29,44 @@ public interface MavLinkDrone extends Drone { void onMavLinkMessageReceived(MAVLinkMessage message); - public short getSysid(); + short getSysid(); + + short getCompid(); + + State getState(); + + Frame getFrame(); + + void setFrame(Frame frame); - public short getCompid(); + short getMavFrame(); - public State getState(); + short getMavFrameInt(); - public ParameterManager getParameterManager(); + ParameterManager getParameterManager(); - public int getType(); + int getType(); - public FirmwareType getFirmwareType(); + FirmwareType getFirmwareType(); - public DataLink.DataLinkProvider getMavClient(); + DataLink.DataLinkProvider getMavClient(); - public WaypointManager getWaypointManager(); + WaypointManager getWaypointManager(); - public MissionImpl getMission(); + MissionImpl getMission(); - public StreamRates getStreamRates(); + StreamRates getStreamRates(); - public MissionStats getMissionStats(); + MissionStats getMissionStats(); - public GuidedPoint getGuidedPoint(); + GuidedPoint getGuidedPoint(); - public AccelCalibration getCalibrationSetup(); + AccelCalibration getCalibrationSetup(); - public MagnetometerCalibrationImpl getMagnetometerCalibration(); + MagnetometerCalibrationImpl getMagnetometerCalibration(); - public String getFirmwareVersion(); + String getFirmwareVersion(); - public Camera getCamera(); + Camera getCamera(); } diff --git a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/autopilot/apm/ArduPilot.java b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/autopilot/apm/ArduPilot.java index 71b77987be..fee5195a38 100644 --- a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/autopilot/apm/ArduPilot.java +++ b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/autopilot/apm/ArduPilot.java @@ -55,6 +55,7 @@ import com.o3dr.services.android.lib.drone.attribute.error.CommandExecutionError; import com.o3dr.services.android.lib.drone.mission.action.MissionActions; import com.o3dr.services.android.lib.drone.property.DroneAttribute; +import com.o3dr.services.android.lib.drone.property.GuidedState; import com.o3dr.services.android.lib.drone.property.Parameter; import com.o3dr.services.android.lib.drone.property.VehicleMode; import com.o3dr.services.android.lib.gcs.action.CalibrationActions; @@ -165,7 +166,7 @@ public DroneAttribute getAttribute(String attributeType) { return CommonApiUtils.getMission(this); case AttributeType.GUIDED_STATE: - return CommonApiUtils.getGuidedState(this); + return GuidedState.getGuidedStateFromPoint(guidedPoint); case AttributeType.MAGNETOMETER_CALIBRATION_STATUS: return CommonApiUtils.getMagnetometerCalibrationStatus(this); diff --git a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/autopilot/generic/GenericMavLinkDrone.java b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/autopilot/generic/GenericMavLinkDrone.java index 4f748794bb..aa7bd135b4 100644 --- a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/autopilot/generic/GenericMavLinkDrone.java +++ b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/drone/autopilot/generic/GenericMavLinkDrone.java @@ -19,10 +19,12 @@ import com.MAVLink.common.msg_radio_status; import com.MAVLink.common.msg_sys_status; import com.MAVLink.common.msg_vibration; +import com.MAVLink.enums.MAV_FRAME; import com.MAVLink.enums.MAV_MODE_FLAG; import com.MAVLink.enums.MAV_STATE; import com.o3dr.services.android.lib.coordinate.LatLong; import com.o3dr.services.android.lib.coordinate.LatLongAlt; +import com.o3dr.services.android.lib.coordinate.Frame; import com.o3dr.services.android.lib.drone.action.CapabilityActions; import com.o3dr.services.android.lib.drone.action.ControlActions; import com.o3dr.services.android.lib.drone.action.ExperimentalActions; @@ -102,6 +104,7 @@ public class GenericMavLinkDrone implements MavLinkDrone { private final Gps vehicleGps = new Gps(); private final Parameters parameters = new Parameters(); protected final Altitude altitude = new Altitude(); + protected Frame frame = Frame.GLOBAL_RELATIVE; // Frame in which the vehicle operates. protected final Speed speed = new Speed(); protected final Battery battery = new Battery(); protected final Signal signal = new Signal(); @@ -247,6 +250,42 @@ public short getCompid() { return heartbeat.getCompid(); } + @Override + public Frame getFrame() { + return this.frame; + } + + @Override + public void setFrame(Frame frame){ + this.frame = frame; + } + + @Override + public short getMavFrame() { + switch (frame) { + case GLOBAL_ABS: + return MAV_FRAME.MAV_FRAME_GLOBAL; + case GLOBAL_TERRAIN: + return MAV_FRAME.MAV_FRAME_GLOBAL_TERRAIN_ALT; + case GLOBAL_RELATIVE: + default: + return MAV_FRAME.MAV_FRAME_GLOBAL_RELATIVE_ALT; + } + } + + @Override + public short getMavFrameInt() { + switch (frame) { + case GLOBAL_ABS: + return MAV_FRAME.MAV_FRAME_GLOBAL_INT; + case GLOBAL_TERRAIN: + return MAV_FRAME.MAV_FRAME_GLOBAL_TERRAIN_ALT_INT; + case GLOBAL_RELATIVE: + default: + return MAV_FRAME.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT; + } + } + @Override public int getMavlinkVersion() { return heartbeat.getMavlinkVersion(); @@ -700,6 +739,7 @@ public void processHomeUpdate(msg_mission_item missionItem) { return; } + Frame frame = Frame.getFrame(missionItem.frame); float latitude = missionItem.x; float longitude = missionItem.y; float altitude = missionItem.z; @@ -707,15 +747,17 @@ public void processHomeUpdate(msg_mission_item missionItem) { LatLongAlt homeCoord = vehicleHome.getCoordinate(); if (homeCoord == null) { - vehicleHome.setCoordinate(new LatLongAlt(latitude, longitude, altitude)); + vehicleHome.setCoordinate(new LatLongAlt(latitude, longitude, altitude, frame )); homeUpdated = true; } else { if (homeCoord.getLatitude() != latitude || homeCoord.getLongitude() != longitude - || homeCoord.getAltitude() != altitude) { + || homeCoord.getAltitude() != altitude + || homeCoord.getFrame() != frame) { homeCoord.setLatitude(latitude); homeCoord.setLongitude(longitude); homeCoord.setAltitude(altitude); + homeCoord.setFrame(frame); homeUpdated = true; } } 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..e714051b3c 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 @@ -10,6 +10,7 @@ import org.droidplanner.services.android.impl.core.drone.autopilot.Drone; import org.droidplanner.services.android.impl.core.drone.autopilot.MavLinkDrone; import com.o3dr.services.android.lib.coordinate.LatLong; +import com.o3dr.services.android.lib.coordinate.LatLongAlt; import com.o3dr.services.android.lib.drone.attribute.AttributeType; import com.o3dr.services.android.lib.drone.attribute.error.CommandExecutionError; import com.o3dr.services.android.lib.drone.property.Altitude; @@ -22,8 +23,7 @@ public class GuidedPoint extends DroneVariable implements OnDroneListener { private GuidedStates state = GuidedStates.UNINITIALIZED; - private LatLong coord = new LatLong(0, 0); - private double altitude = 0.0; //altitude in meters + private LatLongAlt latLongAlt; private Runnable mPostInitializationTask; @@ -87,14 +87,14 @@ public void pauseAtCurrentLocation(ICommandListener listener) { if (state == GuidedStates.UNINITIALIZED) { changeToGuidedMode(myDrone, listener); } else { - newGuidedCoord(getGpsPosition()); + newGuidedCoord(getGpsPosition(myDrone)); state = GuidedStates.IDLE; } } - private LatLong getGpsPosition() { - return getGpsPosition(myDrone); - } +// private LatLong getGpsPosition() { +// return getGpsPosition(myDrone); +// } private static LatLong getGpsPosition(Drone drone) { final Gps droneGps = (Gps) drone.getAttribute(AttributeType.GPS); @@ -109,7 +109,7 @@ public static void changeToGuidedMode(MavLinkDrone drone, ICommandListener liste droneState.changeFlightMode(ApmModes.ROTOR_GUIDED, listener); } else if (Type.isPlane(droneType)) { //You have to send a guided point to the plane in order to trigger guided mode. - forceSendGuidedPoint(drone, getGpsPosition(drone), getDroneAltConstrained(drone)); + forceSendGuidedPoint(drone, new LatLongAlt(getGpsPosition(drone), getDroneAltConstrained(drone), drone.getFrame())); } else if (Type.isRover(droneType)) { droneState.changeFlightMode(ApmModes.ROVER_GUIDED, listener); } @@ -117,8 +117,7 @@ public static void changeToGuidedMode(MavLinkDrone drone, ICommandListener liste public void doGuidedTakeoff(final double alt, final ICommandListener listener) { if (Type.isCopter(myDrone.getType())) { - coord = getGpsPosition(); - altitude = alt; + latLongAlt = new LatLongAlt(getGpsPosition(myDrone), alt, myDrone.getFrame()); state = GuidedStates.IDLE; changeToGuidedMode(myDrone, new SimpleCommandListener() { @@ -166,7 +165,7 @@ public void run() { } } - public void newGuidedCoord(LatLong coord) { + public void newGuidedCoord(LatLong coord) { // TODO Where does this call come from? changeCoord(coord); } @@ -234,8 +233,14 @@ public void run() { private void initialize() { if (state == GuidedStates.UNINITIALIZED) { - coord = getGpsPosition(); - altitude = getDroneAltConstrained(myDrone); + latLongAlt = new LatLongAlt(); + LatLong currentPos = getGpsPosition(myDrone); + + if (currentPos != null && currentPos.isValid()) { + latLongAlt.set(currentPos); + } + latLongAlt.setAltitude(getDroneAltConstrained(myDrone)); + latLongAlt.setFrame(myDrone.getFrame()); state = GuidedStates.IDLE; myDrone.notifyDroneEvent(DroneEventsType.GUIDEDPOINT); } @@ -264,7 +269,7 @@ private void changeAlt(double alt) { /** FALL THROUGH **/ case ACTIVE: - altitude = alt; + this.latLongAlt.setAltitude(alt); sendGuidedPoint(); break; } @@ -279,7 +284,7 @@ private void changeCoord(LatLong coord) { state = GuidedStates.ACTIVE; /** FALL THROUGH **/ case ACTIVE: - this.coord = coord; + this.latLongAlt.set(coord); sendGuidedPoint(); break; } @@ -294,7 +299,8 @@ private void changeCoordAndVelocity(LatLong coord, double xVel, double yVel, dou state = GuidedStates.ACTIVE; /** FALL THROUGH **/ case ACTIVE: - this.coord = coord; + this.latLongAlt = new LatLongAlt(coord.getLatitude(), coord.getLongitude(), + latLongAlt.getAltitude(), latLongAlt.getFrame()); sendGuidedPointAndVelocity(xVel, yVel, zVel); break; } @@ -302,20 +308,21 @@ private void changeCoordAndVelocity(LatLong coord, double xVel, double yVel, dou private void sendGuidedPointAndVelocity(double xVel, double yVel, double zVel) { if (state == GuidedStates.ACTIVE) { - forceSendGuidedPointAndVelocity(myDrone, coord, altitude, xVel, yVel, zVel); + forceSendGuidedPointAndVelocity(myDrone, new LatLong(latLongAlt.getLatitude(), latLongAlt.getLongitude()), + latLongAlt.getAltitude(), xVel, yVel, zVel); } } private void sendGuidedPoint() { if (state == GuidedStates.ACTIVE) { - forceSendGuidedPoint(myDrone, coord, altitude); + forceSendGuidedPoint(myDrone, latLongAlt); } } - public static void forceSendGuidedPoint(MavLinkDrone drone, LatLong coord, double altitudeInMeters) { + public static void forceSendGuidedPoint(MavLinkDrone drone, LatLongAlt coord) { drone.notifyDroneEvent(DroneEventsType.GUIDEDPOINT); if (coord != null) { - MavLinkCommands.setGuidedMode(drone, coord.getLatitude(), coord.getLongitude(), altitudeInMeters); + MavLinkCommands.setGuidedMode(drone, coord.getLatitude(), coord.getLongitude(), coord.getAltitude()); } } @@ -334,12 +341,12 @@ private static double getDroneAltConstrained(MavLinkDrone drone) { return Math.max(alt, getDefaultMinAltitude(drone)); } - public LatLong getCoord() { - return coord; + public LatLongAlt getCoord() { + return latLongAlt; } public double getAltitude() { - return this.altitude; + return latLongAlt == null ? 0.0 : latLongAlt.getAltitude(); } public boolean isActive() { diff --git a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/gcs/ReturnToMe.java b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/gcs/ReturnToMe.java index 00e251f5bb..84c8a94a2b 100644 --- a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/gcs/ReturnToMe.java +++ b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/gcs/ReturnToMe.java @@ -2,6 +2,7 @@ import android.os.Bundle; +import com.o3dr.services.android.lib.coordinate.Frame; import com.o3dr.services.android.lib.coordinate.LatLongAlt; import com.o3dr.services.android.lib.drone.attribute.AttributeEvent; import com.o3dr.services.android.lib.drone.attribute.AttributeEventExtra; @@ -132,7 +133,8 @@ public void onLocationUpdate(Location location) { if (displacement >= UPDATE_MINIMAL_DISPLACEMENT) { MavLinkDoCmds.setVehicleHome(droneMgr.getDrone(), - new LatLongAlt(locationCoord.getLatitude(), locationCoord.getLongitude(), homePosition.getAltitude()), + new LatLongAlt(locationCoord.getLatitude(), locationCoord.getLongitude(), + homePosition.getAltitude(), Frame.GLOBAL_RELATIVE), new AbstractCommandListener() { @Override public void onSuccess() { diff --git a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/gcs/follow/FollowGuidedScan.java b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/gcs/follow/FollowGuidedScan.java index 91e6b688d1..8148743ada 100644 --- a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/gcs/follow/FollowGuidedScan.java +++ b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/gcs/follow/FollowGuidedScan.java @@ -6,6 +6,8 @@ import org.droidplanner.services.android.impl.core.drone.autopilot.MavLinkDrone; import org.droidplanner.services.android.impl.core.drone.manager.MavLinkDroneManager; import org.droidplanner.services.android.impl.core.gcs.roi.ROIEstimator; + +import com.o3dr.services.android.lib.coordinate.Frame; import com.o3dr.services.android.lib.coordinate.LatLong; import com.o3dr.services.android.lib.coordinate.LatLongAlt; @@ -45,7 +47,7 @@ public void updateAlgorithmParams(Map params) { if (tempCoord == null || tempCoord instanceof LatLongAlt) { target = (LatLongAlt) tempCoord; } else { - target = new LatLongAlt(tempCoord, sDefaultRoiAltitude); + target = new LatLongAlt(tempCoord, sDefaultRoiAltitude, Frame.GLOBAL_RELATIVE); } getROIEstimator().updateROITarget(target); diff --git a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/gcs/follow/FollowSoloShot.java b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/gcs/follow/FollowSoloShot.java index 50601733ee..c68a8f24f5 100644 --- a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/gcs/follow/FollowSoloShot.java +++ b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/gcs/follow/FollowSoloShot.java @@ -8,6 +8,8 @@ import org.droidplanner.services.android.impl.core.drone.manager.MavLinkDroneManager; import org.droidplanner.services.android.impl.core.gcs.location.Location; import org.droidplanner.services.android.impl.core.gcs.roi.ROIEstimator; + +import com.o3dr.services.android.lib.coordinate.Frame; import com.o3dr.services.android.lib.coordinate.LatLong; import com.o3dr.services.android.lib.coordinate.LatLongAlt; import com.o3dr.services.android.lib.drone.companion.solo.tlv.SoloMessageLocation; @@ -19,7 +21,7 @@ public class FollowSoloShot extends FollowAlgorithm { private final SoloComp soloComp; - private final LatLongAlt locationCoord = new LatLongAlt(0, 0, 0); + private final LatLongAlt locationCoord = new LatLongAlt(0, 0, 0, Frame.GLOBAL_RELATIVE); private final SoloMessageLocation locationSetter = new SoloMessageLocation(locationCoord); public FollowSoloShot(MavLinkDroneManager droneMgr, Handler handler) { @@ -64,7 +66,7 @@ protected ROIEstimator initROIEstimator(MavLinkDrone drone, Handler handler) { protected static class SoloROIEstimator extends ROIEstimator { - private final LatLongAlt locationCoord = new LatLongAlt(0, 0, 0); + private final LatLongAlt locationCoord = new LatLongAlt(0, 0, 0, Frame.GLOBAL_RELATIVE); private final SoloMessageLocation locationSetter = new SoloMessageLocation(locationCoord); private final SoloComp soloComp; diff --git a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/gcs/follow/LocationRelay.java b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/gcs/follow/LocationRelay.java index dc31bd61bf..13d5156ef9 100644 --- a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/gcs/follow/LocationRelay.java +++ b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/gcs/follow/LocationRelay.java @@ -1,5 +1,6 @@ package org.droidplanner.services.android.impl.core.gcs.follow; +import com.o3dr.services.android.lib.coordinate.Frame; import com.o3dr.services.android.lib.coordinate.LatLongAlt; import com.o3dr.services.android.lib.coordinate.LatLong; import org.droidplanner.services.android.impl.core.helpers.geoTools.GeoTools; @@ -85,7 +86,8 @@ public Location toGcsLocation(android.location.Location androidLocation) { new LatLongAlt( androidLocation.getLatitude(), androidLocation.getLongitude(), - androidLocation.getAltitude() + androidLocation.getAltitude(), + Frame.GLOBAL_RELATIVE ), androidLocation.getBearing(), androidLocation.getSpeed(), diff --git a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/gcs/roi/ROIEstimator.java b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/gcs/roi/ROIEstimator.java index c35254a868..f283ce40d1 100644 --- a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/gcs/roi/ROIEstimator.java +++ b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/gcs/roi/ROIEstimator.java @@ -7,6 +7,8 @@ import org.droidplanner.services.android.impl.core.gcs.location.Location; import org.droidplanner.services.android.impl.core.gcs.location.Location.LocationReceiver; import org.droidplanner.services.android.impl.core.helpers.geoTools.GeoTools; + +import com.o3dr.services.android.lib.coordinate.Frame; import com.o3dr.services.android.lib.coordinate.LatLong; import com.o3dr.services.android.lib.coordinate.LatLongAlt; @@ -92,7 +94,8 @@ protected void updateROI() { } protected void sendUpdateROI(LatLong goCoord) { - MavLinkDoCmds.setROI(drone, new LatLongAlt(goCoord.getLatitude(), goCoord.getLongitude(), (0.0)), null); + MavLinkDoCmds.setROI(drone, new LatLongAlt(goCoord.getLatitude(), goCoord.getLongitude(), + (0.0), Frame.GLOBAL_RELATIVE), null); } public boolean isFollowEnabled() { diff --git a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/mission/MissionImpl.java b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/mission/MissionImpl.java index ee1e900079..3cb6c7d267 100644 --- a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/mission/MissionImpl.java +++ b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/mission/MissionImpl.java @@ -6,6 +6,7 @@ import com.MAVLink.common.msg_mission_item; import com.MAVLink.enums.MAV_CMD; import com.MAVLink.enums.MAV_FRAME; +import com.o3dr.services.android.lib.coordinate.Frame; import com.o3dr.services.android.lib.coordinate.LatLong; import com.o3dr.services.android.lib.coordinate.LatLongAlt; import com.o3dr.services.android.lib.drone.attribute.AttributeType; @@ -334,12 +335,15 @@ public List createDronie(LatLong start, LatLong end) { List dronieItems = new ArrayList(); dronieItems.add(new TakeoffImpl(this, startAltitude)); dronieItems.add(new RegionOfInterestImpl(this, - new LatLongAlt(GeoTools.pointAlongTheLine(start, end, roiDistance), (1.0)))); - dronieItems.add(new WaypointImpl(this, new LatLongAlt(end, (startAltitude + GeoTools.getDistance(start, end) / 2.0)))); + new LatLongAlt(GeoTools.pointAlongTheLine(start, end, roiDistance), (1.0), Frame.GLOBAL_RELATIVE))); + dronieItems.add(new WaypointImpl(this, new LatLongAlt(end, + (startAltitude + GeoTools.getDistance(start, end) / 2.0), + Frame.GLOBAL_RELATIVE))); dronieItems.add(new WaypointImpl(this, - new LatLongAlt(slowDownPoint, (startAltitude + GeoTools.getDistance(start, slowDownPoint) / 2.0)))); + new LatLongAlt(slowDownPoint, (startAltitude + GeoTools.getDistance(start, slowDownPoint) / 2.0), + Frame.GLOBAL_RELATIVE))); dronieItems.add(new ChangeSpeedImpl(this, 1.0)); - dronieItems.add(new WaypointImpl(this, new LatLongAlt(start, startAltitude))); + dronieItems.add(new WaypointImpl(this, new LatLongAlt(start, startAltitude, Frame.GLOBAL_RELATIVE))); dronieItems.add(new ChangeSpeedImpl(this, defaultSpeed)); dronieItems.add(new LandImpl(this, start)); return dronieItems; diff --git a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/mission/waypoints/DoLandStartImpl.java b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/mission/waypoints/DoLandStartImpl.java index 332566273b..4a4c5e33bb 100644 --- a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/mission/waypoints/DoLandStartImpl.java +++ b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/mission/waypoints/DoLandStartImpl.java @@ -6,6 +6,8 @@ import org.droidplanner.services.android.impl.core.mission.MissionImpl; import org.droidplanner.services.android.impl.core.mission.MissionItemImpl; import org.droidplanner.services.android.impl.core.mission.MissionItemType; + +import com.o3dr.services.android.lib.coordinate.Frame; import com.o3dr.services.android.lib.coordinate.LatLong; import com.o3dr.services.android.lib.coordinate.LatLongAlt; @@ -23,7 +25,7 @@ public DoLandStartImpl(MissionImpl missionImpl) { } public DoLandStartImpl(MissionImpl mMissionImpl, LatLong coord) { - super(mMissionImpl, new LatLongAlt(coord, (0))); + super(mMissionImpl, new LatLongAlt(coord, (0), Frame.GLOBAL_RELATIVE)); } public DoLandStartImpl(msg_mission_item msg, MissionImpl missionImpl) { diff --git a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/mission/waypoints/LandImpl.java b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/mission/waypoints/LandImpl.java index c7b39acfec..7aafc2253b 100644 --- a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/mission/waypoints/LandImpl.java +++ b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/mission/waypoints/LandImpl.java @@ -6,6 +6,8 @@ import org.droidplanner.services.android.impl.core.mission.MissionImpl; import org.droidplanner.services.android.impl.core.mission.MissionItemImpl; import org.droidplanner.services.android.impl.core.mission.MissionItemType; + +import com.o3dr.services.android.lib.coordinate.Frame; import com.o3dr.services.android.lib.coordinate.LatLong; import com.o3dr.services.android.lib.coordinate.LatLongAlt; @@ -23,7 +25,7 @@ public LandImpl(MissionImpl missionImpl) { } public LandImpl(MissionImpl mMissionImpl, LatLong coord) { - super(mMissionImpl, new LatLongAlt(coord, 0)); + super(mMissionImpl, new LatLongAlt(coord, 0, Frame.GLOBAL_RELATIVE)); } public LandImpl(msg_mission_item msg, MissionImpl missionImpl) { diff --git a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/mission/waypoints/SpatialCoordItem.java b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/mission/waypoints/SpatialCoordItem.java index 84f1b18dad..69604ea91b 100644 --- a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/mission/waypoints/SpatialCoordItem.java +++ b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/mission/waypoints/SpatialCoordItem.java @@ -4,6 +4,8 @@ import org.droidplanner.services.android.impl.core.mission.MissionImpl; import org.droidplanner.services.android.impl.core.mission.MissionItemImpl; + +import com.o3dr.services.android.lib.coordinate.Frame; import com.o3dr.services.android.lib.coordinate.LatLong; import com.o3dr.services.android.lib.coordinate.LatLongAlt; @@ -23,7 +25,7 @@ public SpatialCoordItem(MissionItemImpl item) { if (item instanceof SpatialCoordItem) { coordinate = ((SpatialCoordItem) item).getCoordinate(); } else { - coordinate = new LatLongAlt(0, 0, 0); + coordinate = new LatLongAlt(0, 0, 0, Frame.GLOBAL_RELATIVE); // TODO lets use null to mean no location } } @@ -39,6 +41,7 @@ public LatLongAlt getCoordinate() { public List packMissionItem() { List list = super.packMissionItem(); msg_mission_item mavMsg = list.get(0); + mavMsg.frame = (short) coordinate.getFrame().asInt(); mavMsg.x = (float) coordinate.getLatitude(); mavMsg.y = (float) coordinate.getLongitude(); mavMsg.z = (float) coordinate.getAltitude(); @@ -47,7 +50,7 @@ public List packMissionItem() { @Override public void unpackMAVMessage(msg_mission_item mavMsg) { - setCoordinate(new LatLongAlt(mavMsg.x, mavMsg.y, mavMsg.z)); + setCoordinate(new LatLongAlt(mavMsg.x, mavMsg.y, mavMsg.z, Frame.getFrame(mavMsg.frame))); } public void setAltitude(double altitude) { diff --git a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/mission/waypoints/StructureScannerImpl.java b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/mission/waypoints/StructureScannerImpl.java index 2eecb8024b..78ff604f24 100644 --- a/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/mission/waypoints/StructureScannerImpl.java +++ b/ClientLib/src/main/java/org/droidplanner/services/android/impl/core/mission/waypoints/StructureScannerImpl.java @@ -2,6 +2,7 @@ import com.MAVLink.common.msg_mission_item; import com.MAVLink.enums.MAV_CMD; +import com.o3dr.services.android.lib.coordinate.Frame; import com.o3dr.services.android.lib.coordinate.LatLong; import com.o3dr.services.android.lib.coordinate.LatLongAlt; @@ -45,14 +46,14 @@ public List packMissionItem() { } private void packROI(List list) { - RegionOfInterestImpl roi = new RegionOfInterestImpl(missionImpl, new LatLongAlt(coordinate, (0.0))); + RegionOfInterestImpl roi = new RegionOfInterestImpl(missionImpl, new LatLongAlt(coordinate, (0.0), Frame.GLOBAL_RELATIVE)); list.addAll(roi.packMissionItem()); } private void packCircles(List list) { double altitude = coordinate.getAltitude(); for (int iSteps = 0; iSteps < numberOfSteps; iSteps++) { - CircleImpl circleImpl = new CircleImpl(missionImpl, new LatLongAlt(coordinate, altitude)); + CircleImpl circleImpl = new CircleImpl(missionImpl, new LatLongAlt(coordinate, altitude, Frame.GLOBAL_RELATIVE)); circleImpl.setRadius(radius); list.addAll(circleImpl.packMissionItem()); altitude+= heightStep; 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..a987b63735 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 @@ -429,36 +429,6 @@ public static Type getType(MavLinkDrone drone) { return new Type(CommonApiUtils.getDroneProxyType(drone.getType()), drone.getFirmwareVersion()); } - public static GuidedState getGuidedState(MavLinkDrone drone) { - if (drone == null) - return new GuidedState(); - - GuidedPoint guidedPoint = drone.getGuidedPoint(); - int guidedState; - switch (guidedPoint.getState()) { - default: - case UNINITIALIZED: - guidedState = GuidedState.STATE_UNINITIALIZED; - break; - - case ACTIVE: - guidedState = GuidedState.STATE_ACTIVE; - break; - - case IDLE: - guidedState = GuidedState.STATE_IDLE; - break; - } - - LatLong guidedCoord = guidedPoint.getCoord(); - if (guidedCoord == null) { - guidedCoord = new LatLong(0, 0); - } - - double guidedAlt = guidedPoint.getAltitude(); - return new GuidedState(guidedState, new LatLongAlt(guidedCoord, guidedAlt)); - } - public static void changeVehicleMode(MavLinkDrone drone, VehicleMode newMode, ICommandListener listener) { if (drone == null) return; diff --git a/ClientLib/src/main/java/org/droidplanner/services/android/impl/utils/ProxyUtils.java b/ClientLib/src/main/java/org/droidplanner/services/android/impl/utils/ProxyUtils.java index 568bee34a0..a514ec25b0 100644 --- a/ClientLib/src/main/java/org/droidplanner/services/android/impl/utils/ProxyUtils.java +++ b/ClientLib/src/main/java/org/droidplanner/services/android/impl/utils/ProxyUtils.java @@ -2,6 +2,7 @@ import android.util.Log; +import com.o3dr.services.android.lib.coordinate.Frame; import com.o3dr.services.android.lib.coordinate.LatLongAlt; import com.o3dr.services.android.lib.drone.mission.item.MissionItem; import com.o3dr.services.android.lib.drone.mission.item.command.CameraTrigger; @@ -185,7 +186,7 @@ public static MissionItemImpl getMissionItemImpl(MissionImpl missionImpl, Missio case RESET_ROI: { //Sending a roi with all coordinates set to 0 will reset the current roi. - RegionOfInterestImpl temp = new RegionOfInterestImpl(missionImpl, new LatLongAlt(0, 0, 0)); + RegionOfInterestImpl temp = new RegionOfInterestImpl(missionImpl, new LatLongAlt(0, 0, 0, Frame.GLOBAL_RELATIVE)); missionItemImpl = temp; break; }