Rover: fixed a bug going into guided and rover still moving

When the rover goes into guided mode it sets the current location as
the guided point to goto.  If the rover is stationary when this
happens no problem.  If however the rover is still rolling (say going
from AUTO to GUIDED) then the rover would go past its guided position
and get confused and begin to circle it.  This change resolves that issue.

APMrover2/APMrover2.cpp

@@ -376,17 +376,15 @@ void Rover::update_current_mode(void)
 
     case GUIDED:
         set_reverse(false);
-        if (!rtl_complete) {
+        if (rtl_complete || verify_RTL()) {
-            if (verify_RTL()) {
+            // we have reached destination so stop where we are
-                // we have reached destination so stop where we are
+            channel_throttle->servo_out = g.throttle_min.get();
-                channel_throttle->servo_out = g.throttle_min.get();
+            channel_steer->servo_out = 0;
-                channel_steer->servo_out = 0;
+            lateral_acceleration = 0;
-                lateral_acceleration = 0;
+        } else {
-            } else {
+            calc_lateral_acceleration();
-                calc_lateral_acceleration();
+            calc_nav_steer();
-                calc_nav_steer();
+            calc_throttle(g.speed_cruise);
-                calc_throttle(g.speed_cruise);
-            }
         }
         break;
 
@@ -475,13 +473,11 @@ void Rover::update_navigation()
         // no loitering around the wp with the rover, goes direct to the wp position
         calc_lateral_acceleration();
         calc_nav_steer();
-        if (!rtl_complete) {
+        if (rtl_complete || verify_RTL()) {
-            if (verify_RTL()) {
+            // we have reached destination so stop where we are
-                // we have reached destination so stop where we are
+            channel_throttle->servo_out = g.throttle_min.get();
-                channel_throttle->servo_out = g.throttle_min.get();
+            channel_steer->servo_out = 0;
-                channel_steer->servo_out = 0;
+            lateral_acceleration = 0;
-                lateral_acceleration = 0;
-            }
         }
         break;
     }