diff --git a/libraries/APO/AP_Guide.cpp b/libraries/APO/AP_Guide.cpp
index 5ad372bc83..86c24338aa 100644
--- a/libraries/APO/AP_Guide.cpp
+++ b/libraries/APO/AP_Guide.cpp
@@ -170,7 +170,6 @@ void MavlinkGuide::handleCommand() {
 
         // check for along track next waypoint requirement
         float alongTrack = _command.alongTrack(_previousCommand,
-                                               _command,
                                                _nav->getLat_degInt(),
                                                _nav->getLon_degInt());
         float segmentLength = _previousCommand.distanceTo(_command);
diff --git a/libraries/APO/AP_MavlinkCommand.cpp b/libraries/APO/AP_MavlinkCommand.cpp
index e1b44b10d8..d659048c20 100644
--- a/libraries/APO/AP_MavlinkCommand.cpp
+++ b/libraries/APO/AP_MavlinkCommand.cpp
@@ -194,16 +194,12 @@ float AP_MavlinkCommand::crossTrack(const AP_MavlinkCommand & previous,
 }
 
 // calculates along  track distance of a current location
-float AP_MavlinkCommand::alongTrack(const AP_MavlinkCommand & previous, const AP_MavlinkCommand & next,
+float AP_MavlinkCommand::alongTrack(const AP_MavlinkCommand & previous,
                                     int32_t lat_degInt, int32_t lon_degInt) const {
     // ignores lat/lon since single prec.
-    float t1N = previous.getPN(lat_degInt, lon_degInt);
-    float t1E = previous.getPE(lat_degInt, lon_degInt);
-    float t2N = previous.getPN(next.getLat_degInt(), next.getLon_degInt());
-    float t2E = previous.getPE(next.getLat_degInt(), next.getLon_degInt());
-    float segmentLength = previous.distanceTo(next);
-    if (segmentLength == 0) return 0;
-    return (t1N*t2N + t1E*t2E)/segmentLength;
+    float dXt = crossTrack(previous,lat_degInt,lon_degInt);
+    float d = previous.distanceTo(getLat_degInt(),getLon_degInt());
+    return acos(cos(d/rEarth)/cos(dXt/rEarth))*rEarth;
 }
 
 
diff --git a/libraries/APO/AP_MavlinkCommand.h b/libraries/APO/AP_MavlinkCommand.h
index 9c541026f0..c6d4ee2e46 100644
--- a/libraries/APO/AP_MavlinkCommand.h
+++ b/libraries/APO/AP_MavlinkCommand.h
@@ -370,7 +370,7 @@ public:
     float crossTrack(const AP_MavlinkCommand & previous, int32_t lat_degInt, int32_t lon_degInt) const;
 
     // calculates along  track distance of a current location
-    float alongTrack(const AP_MavlinkCommand & previous, const AP_MavlinkCommand & next, int32_t lat_degInt, int32_t lon_degInt) const;
+    float alongTrack(const AP_MavlinkCommand & previous, int32_t lat_degInt, int32_t lon_degInt) const;
 };
 
 } // namespace apo