Fixed the issue causing lack of roi update when in follow mode on the…

… nexus 6: the location object returned by the nexus 6 location provider is missing the speed value, which the roi estimator was relying on to update its state.

Possible fix for the 'copter pointed east' reported issue.

ServiceApp/src/org/droidplanner/services/android/location/FusedLocation.java

@@ -90,9 +90,10 @@ public void onLocationChanged(Location androidLocation) {
             float distanceToLast = -1.0f;
             long timeSinceLast = -1L;
 
+            final long androidLocationTime = androidLocation.getTime();
             if (mLastLocation != null) {
                 distanceToLast = androidLocation.distanceTo(mLastLocation);
-                timeSinceLast = (androidLocation.getTime() - mLastLocation.getTime()) / 1000;
+                timeSinceLast = (androidLocationTime - mLastLocation.getTime()) / 1000;
             }
 
             final float currentSpeed = distanceToLast > 0f && timeSinceLast > 0
@@ -101,9 +102,14 @@ public void onLocationChanged(Location androidLocation) {
             final boolean isLocationAccurate = isLocationAccurate(androidLocation.getAccuracy(), currentSpeed);
 
             org.droidplanner.core.gcs.location.Location location = new org.droidplanner.core.gcs.location.Location(
-                    new Coord3D(androidLocation.getLatitude(), androidLocation.getLongitude(),
-                            new Altitude(androidLocation.getAltitude())), androidLocation.getBearing(),
-                    androidLocation.getSpeed(), isLocationAccurate, androidLocation.getTime());
+                    new Coord3D(
+                            androidLocation.getLatitude(),
+                            androidLocation.getLongitude(),
+                            new Altitude(androidLocation.getAltitude())),
+                    androidLocation.getBearing(),
+                    androidLocation.hasSpeed() ? androidLocation.getSpeed() : currentSpeed,
+                    isLocationAccurate,
+                    androidLocationTime);
 
             mLastLocation = androidLocation;
             receiver.onLocationUpdate(location);
@@ -141,15 +147,15 @@ public void setLocationListener(LocationReceiver receiver) {
 
     @Override
     public void onGoogleApiConnectionError(ConnectionResult result) {
-        if(receiver != null)
+        if (receiver != null)
             receiver.onLocationUnavailable();
 
         GooglePlayServicesUtil.showErrorNotification(result.getErrorCode(), this.context);
     }
 
     @Override
     public void onUnavailableGooglePlayServices(int status) {
-        if(receiver != null)
+        if (receiver != null)
             receiver.onLocationUnavailable();
 
         GooglePlayServicesUtil.showErrorNotification(status, this.context);
@@ -161,5 +167,6 @@ public void onManagerStarted() {
     }
 
     @Override
-    public void onManagerStopped() {}
+    public void onManagerStopped() {
+    }
 }

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

@@ -62,11 +62,11 @@ public void toggleFollowMeState() {
 
     private void enableFollowMe() {
         lastLocation = null;
+        state = FollowStates.FOLLOW_START;
 
         locationFinder.enableLocationUpdates();
         followAlgorithm.enableFollow();
 
-        state = FollowStates.FOLLOW_START;
         drone.notifyDroneEvent(DroneEventsType.FOLLOW_START);
     }
 
@@ -90,14 +90,16 @@ public boolean isEnabled() {
     public void onDroneEvent(DroneEventsType event, Drone drone) {
         switch (event) {
             case MODE:
-                if (!GuidedPoint.isGuidedMode(drone)) {
+                if (isEnabled() && !GuidedPoint.isGuidedMode(drone)) {
                     disableFollowMe();
                 }
                 break;
 
             case HEARTBEAT_TIMEOUT:
             case DISCONNECTED:
-                disableFollowMe();
+                if(isEnabled()) {
+                    disableFollowMe();
+                }
                 break;
         }
     }

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

@@ -31,12 +31,13 @@ public void enableFollow() {
     }
 
     public void disableFollow() {
-        isFollowEnabled.set(false);
-        if (GuidedPoint.isGuidedMode(drone)) {
-            drone.getGuidedPoint().pauseAtCurrentLocation();
-        }
+        if(isFollowEnabled.compareAndSet(true, false)) {
+            if (GuidedPoint.isGuidedMode(drone)) {
+                drone.getGuidedPoint().pauseAtCurrentLocation();
+            }
 
-        roiEstimator.disableFollow();
+            roiEstimator.disableFollow();
+        }
     }
 
     public void updateAlgorithmParams(Map<String, ?> paramsMap) {

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

@@ -13,6 +13,8 @@ public class FollowSplineAbove extends FollowAlgorithm {
     @Override
     public void processNewLocation(Location location) {
         Coord2D gcsLoc = new Coord2D(location.getCoord().getLat(), location.getCoord().getLng());
+
+        //TODO: some device (nexus 6) do not report the speed (always 0).. figure out workaround.
         double speed = location.getSpeed();
         double bearing = location.getBearing();
         double bearingInRad = Math.toRadians(bearing);

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

@@ -25,6 +25,7 @@ public void processNewLocation(Location location) {
             double headingGCSToDrone = GeoTools.getHeadingFromCoordinates(userLoc, droneLoc);
             Coord2D goCoord = GeoTools.newCoordFromBearingAndDistance(userLoc, headingGCSToDrone, radius);
 
+            //TODO: some device (nexus 6) do not report the speed (always 0).. figure out workaround.
             double speed = location.getSpeed();
             double bearing = location.getBearing();
             double bearingInRad = Math.toRadians(bearing);

dependencyLibs/Core/src/org/droidplanner/core/gcs/roi/ROIEstimator.java

@@ -45,14 +45,16 @@ public void enableFollow() {
     }
 
     public void disableFollow() {
-        disableWatchdog();
-        isFollowEnabled.set(false);
-        MavLinkDoCmds.resetROI(drone);
+        if(isFollowEnabled.compareAndSet(true, false)){
+            realLocation = null;
+            MavLinkDoCmds.resetROI(drone);
+            disableWatchdog();
+        }
     }
 
     @Override
     public final void onLocationUpdate(Location location) {
-        if(!isFollowEnabled.get())
+        if (!isFollowEnabled.get())
             return;
 
         realLocation = location;
@@ -67,7 +69,7 @@ public void onLocationUnavailable() {
         disableWatchdog();
     }
 
-    protected void disableWatchdog(){
+    protected void disableWatchdog() {
         watchdog.removeCallbacks(watchdogCallback);
     }
 
@@ -76,17 +78,17 @@ protected void updateROI() {
             return;
         }
 
-        Coord2D gcsCoord = new Coord2D(realLocation.getCoord().getLat(), realLocation.getCoord().getLng());
+        Coord2D gcsCoord = realLocation.getCoord();
 
         double bearing = realLocation.getBearing();
         double distanceTraveledSinceLastPoint = realLocation.getSpeed()
                 * (System.currentTimeMillis() - timeOfLastLocation) / 1000f;
         Coord2D goCoord = GeoTools.newCoordFromBearingAndDistance(gcsCoord, bearing, distanceTraveledSinceLastPoint);
-        if (distanceTraveledSinceLastPoint > 0.0) {
-            MavLinkDoCmds.setROI(drone, new Coord3D(goCoord.getLat(), goCoord.getLng(), new Altitude(0.0)));
-        }
 
-        watchdog.postDelayed(watchdogCallback, TIMEOUT);
+        MavLinkDoCmds.setROI(drone, new Coord3D(goCoord.getLat(), goCoord.getLng(), new Altitude(0.0)));
+
+        if (realLocation.getSpeed() > 0)
+            watchdog.postDelayed(watchdogCallback, TIMEOUT);
     }
 
     public boolean isFollowEnabled() {

dependencyLibs/Core/src/org/droidplanner/core/helpers/geoTools/GeoTools.java

@@ -54,8 +54,7 @@ public static Double latToMeters(double lat) {
 	 *            distance to be added
 	 * @return New point with the added distance
 	 */
-	public static Coord2D newCoordFromBearingAndDistance(Coord2D origin, double bearing,
-			double distance) {
+	public static Coord2D newCoordFromBearingAndDistance(Coord2D origin, double bearing, double distance) {
 
 		double lat = origin.getLat();
 		double lon = origin.getLng();