Merge pull request #420 from kellyschrock/locationrelay_fix

LocationRelay improvement
dronekit · Jul 27, 2016 · b3101db · b3101db
2 parents 9fa9d69 + 80d5285
commit b3101db
Showing 1 changed file with 20 additions and 8 deletions.
diff --git a/ServiceApp/src/org/droidplanner/services/android/core/gcs/follow/LocationRelay.java b/ServiceApp/src/org/droidplanner/services/android/core/gcs/follow/LocationRelay.java
@@ -1,7 +1,8 @@
 package org.droidplanner.services.android.core.gcs.follow;
 
 import com.o3dr.services.android.lib.coordinate.LatLongAlt;
-
+import com.o3dr.services.android.lib.coordinate.LatLong;
+import org.droidplanner.services.android.core.helpers.geoTools.GeoTools;
 import org.droidplanner.services.android.core.gcs.location.Location;
 
 import timber.log.Timber;
@@ -40,26 +41,37 @@ public void onFollowStart() {
      */
     public Location toGcsLocation(android.location.Location androidLocation) {
         Location gcsLocation = null;
-        if(VERBOSE) Timber.d("toLocation(): followLoc=" + androidLocation);
+        if(VERBOSE) Timber.d("toGcsLocation(): followLoc=" + androidLocation);
+
+        // If location has no bearing, set one based on its heading from the
+        // previous location (or 0 if no previous location).
+        if(!androidLocation.hasBearing()) {
+            if(mLastLocation != null) {
+                LatLong last = new LatLong(mLastLocation.getLatitude(), mLastLocation.getLongitude());
+                LatLong newLoc = new LatLong(androidLocation.getLatitude(), androidLocation.getLongitude());
+                androidLocation.setBearing((float)GeoTools.getHeadingFromCoordinates(last, newLoc));
+            } else {
+                androidLocation.setBearing(0);
+            }
+        }
 
         boolean ok = (androidLocation.hasAccuracy() && androidLocation.hasBearing() && androidLocation.getTime() > 0);
 
         if(!ok) {
-            Timber.w("toLocation(): Location needs accuracy, heading, and time");
-        }
-
-        if(ok) {
+            Timber.w("toGcsLocation(): Location needs accuracy, bearing, and time.");
+        } else {
             float distanceToLast = -1.0f;
             long timeSinceLast = -1L;
 
             final long androidLocationTime = androidLocation.getTime();
             if (mLastLocation != null) {
                 distanceToLast = androidLocation.distanceTo(mLastLocation);
-                timeSinceLast = (androidLocationTime - mLastLocation.getTime()) / 1000;
+                timeSinceLast = (androidLocationTime - mLastLocation.getTime());
             }
 
+            // mm/ms (does a better job calculating for locations that arrive at < 1-second intervals)
             final float currentSpeed = (distanceToLast > 0f && timeSinceLast > 0) ?
-                    (distanceToLast / timeSinceLast) : 0f;
+                    ((distanceToLast * 1000) / timeSinceLast) : 0f;
 
             final boolean isAccurate = isLocationAccurate(androidLocation.getAccuracy(), currentSpeed);