Merge pull request #1145 from kellyschrock/flyaway_avg_speed_upstream

Fly-away Follow-Me fix
DroidPlanner · Oct 7, 2014 · 634e4ea · 634e4ea
2 parents 9692138 + bea8275
commit 634e4ea
Show file tree

Hide file tree

Showing 3 changed files with 78 additions and 9 deletions.
diff --git a/Android/src/org/droidplanner/android/gcs/location/FusedLocation.java b/Android/src/org/droidplanner/android/gcs/location/FusedLocation.java
@@ -26,6 +26,8 @@ public class FusedLocation implements LocationFinder, GooglePlayServicesClient.C
 	private LocationClient mLocationClient;
 	private LocationReceiver receiver;
 
+	private Location mLastLocation;
+
 	public FusedLocation(Context context) {
 		mLocationClient = new LocationClient(context, this, this);
 		mLocationClient.connect();
@@ -39,7 +41,6 @@ public void enableLocationUpdates() {
 		mLocationRequest.setFastestInterval(MIN_TIME_MS);
 		mLocationRequest.setSmallestDisplacement(MIN_DISTANCE_M);
 		mLocationClient.requestLocationUpdates(mLocationRequest, this);
-
 	}
 
 	@Override
@@ -67,10 +68,20 @@ public void onDisconnected() {
 	@Override
 	public void onLocationChanged(Location androidLocation) {
 		if (receiver != null) {
+			double distanceToLast = -1.0;
+			long timeSinceLast = -1L;
+
+			if(mLastLocation != null) {
+				distanceToLast = androidLocation.distanceTo(mLastLocation);
+				timeSinceLast = (androidLocation.getTime() - mLastLocation.getTime());
+			}
+
 			org.droidplanner.core.gcs.location.Location location = new org.droidplanner.core.gcs.location.Location(
 					new Coord2D(androidLocation.getLatitude(), androidLocation.getLongitude()),
 					androidLocation.getAccuracy(), androidLocation.getBearing(),
-					androidLocation.getSpeed());
+					androidLocation.getSpeed(), distanceToLast, timeSinceLast);
+
+			mLastLocation = androidLocation;
 			receiver.onLocationChanged(location);
 		}
 	}

diff --git a/Core/src/org/droidplanner/core/gcs/follow/Follow.java b/Core/src/org/droidplanner/core/gcs/follow/Follow.java
@@ -16,18 +16,21 @@
 import com.MAVLink.Messages.ApmModes;
 
 public class Follow implements OnDroneListener, LocationReceiver {
+	private static final double JUMP_FACTOR = 4.0;
 
 	/** Set of return value for the 'toggleFollowMeState' method.*/
 	public enum FollowStates {
-		FOLLOW_INVALID_STATE, FOLLOW_DRONE_NOT_ARMED, FOLLOW_DRONE_DISCONNECTED, FOLLOW_START, FOLLOW_RUNNING, FOLLOW_END,
-	};
+		FOLLOW_INVALID_STATE, FOLLOW_DRONE_NOT_ARMED, FOLLOW_DRONE_DISCONNECTED, FOLLOW_START, FOLLOW_RUNNING, FOLLOW_END
+	}
 
 	private FollowStates state = FollowStates.FOLLOW_INVALID_STATE;
 	private Drone drone;
 
 	private ROIEstimator roiEstimator;
 	private LocationFinder locationFinder;
 	private FollowAlgorithm followAlgorithm;
+	private long speedReadings = 0;
+	private double totalSpeed = 0.0;
 
 	public Follow(Drone drone, Handler handler, LocationFinder locationFinder) {
 		this.drone = drone;
@@ -67,6 +70,8 @@ private void enableFollowMe() {
 		locationFinder.enableLocationUpdates();
 		state = FollowStates.FOLLOW_START;
 		drone.notifyDroneEvent(DroneEventsType.FOLLOW_START);
+		speedReadings = 0;
+		totalSpeed = 0.0;
 	}
 
 	private void disableFollowMe() {
@@ -104,14 +109,43 @@ public Length getRadius() {
 
 	@Override
 	public void onLocationChanged(Location location) {
+		boolean follow = false;
+
 		if (location.getAccuracy() < 10.0) {
 			state = FollowStates.FOLLOW_RUNNING;
-			followAlgorithm.processNewLocation(location);
-		}else{
+
+			double mps = location.getCurrentSpeed();
+			totalSpeed += mps;
+			double avg = (totalSpeed / ++speedReadings);
+
+			// If moving:
+			if(mps > 0) {
+				// If average indicates some movement
+				if(avg >= 1.0) {
+					// Only accept reasonable updates.
+					if(mps < (avg * JUMP_FACTOR)) {
+						follow = true;
+					}
+				}
+				else {
+					// Get moving
+					follow = true;
+				}
+			}
+			else {
+				// Get moving
+				follow = true;
+			}
+		}
+		else {
 			state = FollowStates.FOLLOW_START;
 		}
-		drone.notifyDroneEvent(DroneEventsType.FOLLOW_UPDATE);
-		roiEstimator.onLocationChanged(location);
+
+		if(follow) {
+			followAlgorithm.processNewLocation(location);
+			drone.notifyDroneEvent(DroneEventsType.FOLLOW_UPDATE);
+			roiEstimator.onLocationChanged(location);
+		}
 	}
 
 	public void setType(FollowModes item) {

diff --git a/Core/src/org/droidplanner/core/gcs/location/Location.java b/Core/src/org/droidplanner/core/gcs/location/Location.java
@@ -20,16 +20,20 @@ public interface LocationFinder {
 	private double accuracy = 0.0;
 	private double heading = 0.0;
 	private double speed = 0.0;
+	private double distanceToPrevious = 0.0;
+	private long timeSinceLast = 0L;
 
 	public Location(Coord2D coord2d) {
 		coordinate = coord2d;
 	}
 
-	public Location(Coord2D coord2d, double accuracy, float heading, float speed) {
+	public Location(Coord2D coord2d, double accuracy, float heading, float speed, double distance, long since) {
 		coordinate = coord2d;
 		this.accuracy = accuracy;
 		this.heading = heading;
 		this.speed = speed;
+		this.distanceToPrevious = distance;
+		this.timeSinceLast = since;
 	}
 
 	public Coord2D getCoord() {
@@ -48,4 +52,24 @@ public double getSpeed() {
 		return speed;
 	}
 
+	public double getDistanceToPrevious() {
+		return distanceToPrevious;
+	}
+
+	public long getTimeSinceLast() {
+		return timeSinceLast;
+	}
+
+	public boolean hasTimeSinceLast() {
+		return (timeSinceLast > 0);
+	}
+
+	public boolean hasDistanceToPrevious() {
+		return (distanceToPrevious > 0.0);
+	}
+
+	public double getCurrentSpeed() {
+		return (hasDistanceToPrevious() && hasTimeSinceLast())?
+			(distanceToPrevious / (timeSinceLast / 1000)): 0.0;
+	}
 }