Added ability to modify the GUIDED SCAN roi altitude.

Author: m4gr3d

ServiceApp/build.gradle

@@ -27,7 +27,7 @@ android {
         applicationId 'org.droidplanner.services.android'
         minSdkVersion 14
         targetSdkVersion 21
-        versionCode 10109
+        versionCode 10110
         versionName getGitVersion()
     }
 

ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java

@@ -70,6 +70,7 @@
 import org.droidplanner.core.gcs.follow.Follow;
 import org.droidplanner.core.gcs.follow.FollowAlgorithm;
 import org.droidplanner.core.helpers.coordinates.Coord2D;
+import org.droidplanner.core.helpers.coordinates.Coord3D;
 import org.droidplanner.core.model.Drone;
 import org.droidplanner.core.parameters.Parameter;
 import org.droidplanner.core.survey.CameraInfo;
@@ -730,9 +731,10 @@ private FollowState getFollowState() {
         for(Map.Entry<String, Object> entry : modeParams.entrySet()){
             switch(entry.getKey()){
                 case FollowType.EXTRA_FOLLOW_ROI_TARGET:
-                    Coord2D target = (Coord2D) entry.getValue();
+                    Coord3D target = (Coord3D) entry.getValue();
                     if(target != null){
-                        params.putParcelable(entry.getKey(), new LatLong(target.getLat(), target.getLng()));
+                        params.putParcelable(entry.getKey(), new LatLongAlt(target.getLat(), target.getLng(),
+                                target.getAltitude().valueInMeters()));
                     }
                     break;
 
@@ -999,11 +1001,20 @@ public void performAction(Action action) throws RemoteException {
                 if(followAlgorithm != null){
                     Map<String, Object> paramsMap = new HashMap<>();
                     Set<String> dataKeys = data.keySet();
+
                     for(String key: dataKeys){
                         if(FollowType.EXTRA_FOLLOW_ROI_TARGET.equals(key)){
                             LatLong target = data.getParcelable(key);
                             if(target != null) {
-                                Coord2D roiTarget = new Coord2D(target.getLatitude(), target.getLongitude());
+                                final Coord2D roiTarget;
+                                if(target instanceof LatLongAlt) {
+                                    roiTarget = new Coord3D(target.getLatitude(), target.getLongitude(),
+                                            new org.droidplanner.core.helpers.units.Altitude(((LatLongAlt) target)
+                                                    .getAltitude()));
+                                }
+                                else{
+                                    roiTarget = new Coord2D(target.getLatitude(), target.getLongitude());
+                                }
                                 paramsMap.put(key, roiTarget);
                             }
                         }

dependencyLibs/Core/src/org/droidplanner/core/gcs/follow/FollowGuidedScan.java

@@ -16,8 +16,13 @@
  */
 public class FollowGuidedScan extends FollowAbove {
 
+    private static final String TAG = FollowGuidedScan.class.getSimpleName();
+
     public static final String EXTRA_FOLLOW_ROI_TARGET = "extra_follow_roi_target";
 
+    public static final double DEFAULT_FOLLOW_ROI_ALTITUDE = 10; //meters
+    private static final Altitude sDefaultRoiAltitude = new Altitude(DEFAULT_FOLLOW_ROI_ALTITUDE);
+
     @Override
     public FollowModes getType() {
         return FollowModes.GUIDED_SCAN;
@@ -31,7 +36,16 @@ public FollowGuidedScan(Drone drone, DroneInterfaces.Handler handler) {
     public void updateAlgorithmParams(Map<String, ?> params){
         super.updateAlgorithmParams(params);
 
-        Coord2D target = (Coord2D) params.get(EXTRA_FOLLOW_ROI_TARGET);
+        final Coord3D target;
+
+        Coord2D tempCoord = (Coord2D) params.get(EXTRA_FOLLOW_ROI_TARGET);
+        if(tempCoord == null || tempCoord instanceof Coord3D){
+            target = (Coord3D) tempCoord;
+        }
+        else{
+            target = new Coord3D(tempCoord, sDefaultRoiAltitude);
+        }
+
         getROIEstimator().updateROITarget(target);
     }
 
@@ -54,26 +68,29 @@ protected GuidedROIEstimator getROIEstimator(){
 
     private static class GuidedROIEstimator extends ROIEstimator {
 
-        private Coord2D roiTarget;
+        private Coord3D roiTarget;
 
         public GuidedROIEstimator(Drone drone, DroneInterfaces.Handler handler) {
             super(drone, handler);
         }
 
-        void updateROITarget(Coord2D roiTarget){
+        void updateROITarget(Coord3D roiTarget){
             this.roiTarget = roiTarget;
             onLocationChanged(null);
         }
 
         @Override
         protected void updateROI(){
             if(roiTarget == null){
+                System.out.println("Cancelling ROI lock.");
                 //Fallback to the default behavior
                 super.updateROI();
             }
             else{
+                System.out.println("ROI Target: " + roiTarget.toString());
+
                 //Track the target until told otherwise.
-                MavLinkROI.setROI(drone, new Coord3D(roiTarget, new Altitude(1.0)));
+                MavLinkROI.setROI(drone, roiTarget);
             }
         }
     }

dependencyLibs/Core/src/org/droidplanner/core/helpers/coordinates/Coord3D.java

@@ -26,4 +26,9 @@ public void set(double lat, double lon, Altitude alt) {
 	public Altitude getAltitude() {
 		return alt;
 	}
+
+    @Override
+    public String toString() {
+        return "lat/lon/alt: " + getLat() + "/" + getLng() + "/" + alt.toString();
+    }
 }