From 07c57ba973cd9c80fee9c9e7c3530e54d28ae9ae Mon Sep 17 00:00:00 2001 From: Grant Morphett Date: Mon, 27 Jul 2015 21:39:25 +1000 Subject: [PATCH] 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 | 32 ++++++++++++++------------------ 1 file changed, 14 insertions(+), 18 deletions(-) diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp index 95032b5d66..8deaf49814 100644 --- a/APMrover2/APMrover2.cpp +++ b/APMrover2/APMrover2.cpp @@ -376,17 +376,15 @@ void Rover::update_current_mode(void) case GUIDED: set_reverse(false); - if (!rtl_complete) { - if (verify_RTL()) { - // we have reached destination so stop where we are - channel_throttle->servo_out = g.throttle_min.get(); - channel_steer->servo_out = 0; - lateral_acceleration = 0; - } else { - calc_lateral_acceleration(); - calc_nav_steer(); - calc_throttle(g.speed_cruise); - } + if (rtl_complete || verify_RTL()) { + // we have reached destination so stop where we are + channel_throttle->servo_out = g.throttle_min.get(); + channel_steer->servo_out = 0; + lateral_acceleration = 0; + } else { + calc_lateral_acceleration(); + calc_nav_steer(); + 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 (verify_RTL()) { - // we have reached destination so stop where we are - channel_throttle->servo_out = g.throttle_min.get(); - channel_steer->servo_out = 0; - lateral_acceleration = 0; - } + if (rtl_complete || verify_RTL()) { + // we have reached destination so stop where we are + channel_throttle->servo_out = g.throttle_min.get(); + channel_steer->servo_out = 0; + lateral_acceleration = 0; } break; }