From d76a9b6c2418b8389d54267a72d0dbd0da2adf94 Mon Sep 17 00:00:00 2001 From: ne0fhyk Date: Tue, 24 Feb 2015 01:03:41 -0800 Subject: [PATCH] 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. --- .../android/location/FusedLocation.java | 21 ++++++++++++------ .../droidplanner/core/gcs/follow/Follow.java | 8 ++++--- .../core/gcs/follow/FollowAlgorithm.java | 11 +++++----- .../core/gcs/follow/FollowSplineAbove.java | 2 ++ .../core/gcs/follow/FollowSplineLeash.java | 1 + .../core/gcs/roi/ROIEstimator.java | 22 ++++++++++--------- .../core/helpers/geoTools/GeoTools.java | 3 +-- 7 files changed, 41 insertions(+), 27 deletions(-) diff --git a/ServiceApp/src/org/droidplanner/services/android/location/FusedLocation.java b/ServiceApp/src/org/droidplanner/services/android/location/FusedLocation.java index fc24bb98fd..b255bd9ffd 100644 --- a/ServiceApp/src/org/droidplanner/services/android/location/FusedLocation.java +++ b/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,7 +147,7 @@ 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); @@ -149,7 +155,7 @@ public void onGoogleApiConnectionError(ConnectionResult result) { @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() { + } } diff --git a/dependencyLibs/Core/src/org/droidplanner/core/gcs/follow/Follow.java b/dependencyLibs/Core/src/org/droidplanner/core/gcs/follow/Follow.java index f6bf18e39f..e9d1e5b0b8 100644 --- a/dependencyLibs/Core/src/org/droidplanner/core/gcs/follow/Follow.java +++ b/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; } } diff --git a/dependencyLibs/Core/src/org/droidplanner/core/gcs/follow/FollowAlgorithm.java b/dependencyLibs/Core/src/org/droidplanner/core/gcs/follow/FollowAlgorithm.java index e10fd9d0f0..7c6006cc43 100644 --- a/dependencyLibs/Core/src/org/droidplanner/core/gcs/follow/FollowAlgorithm.java +++ b/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 paramsMap) { diff --git a/dependencyLibs/Core/src/org/droidplanner/core/gcs/follow/FollowSplineAbove.java b/dependencyLibs/Core/src/org/droidplanner/core/gcs/follow/FollowSplineAbove.java index e9b26d804f..35674066f2 100644 --- a/dependencyLibs/Core/src/org/droidplanner/core/gcs/follow/FollowSplineAbove.java +++ b/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); diff --git a/dependencyLibs/Core/src/org/droidplanner/core/gcs/follow/FollowSplineLeash.java b/dependencyLibs/Core/src/org/droidplanner/core/gcs/follow/FollowSplineLeash.java index 62b721d849..6e78882520 100644 --- a/dependencyLibs/Core/src/org/droidplanner/core/gcs/follow/FollowSplineLeash.java +++ b/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); diff --git a/dependencyLibs/Core/src/org/droidplanner/core/gcs/roi/ROIEstimator.java b/dependencyLibs/Core/src/org/droidplanner/core/gcs/roi/ROIEstimator.java index 5f2dd1fc44..f9aeed0185 100644 --- a/dependencyLibs/Core/src/org/droidplanner/core/gcs/roi/ROIEstimator.java +++ b/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() { diff --git a/dependencyLibs/Core/src/org/droidplanner/core/helpers/geoTools/GeoTools.java b/dependencyLibs/Core/src/org/droidplanner/core/helpers/geoTools/GeoTools.java index 276abed3ca..4525e1deac 100644 --- a/dependencyLibs/Core/src/org/droidplanner/core/helpers/geoTools/GeoTools.java +++ b/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();