From ea64438ef93d0e8e24d797b36d9a84170ce46b3a Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 24 Apr 2014 18:52:21 +0900 Subject: [PATCH] Copter: RTL ensures heading is back to initial heading This fixes the issue in which a short RTL_LOIT_TIME could cause the vehicle's heading to be caught between it's heading when it arrived at home and the initial armed heading. With this fix it now waits above home until the timer has run out AND the heading is within 2degrees of the initial armed heading. --- ArduCopter/control_rtl.pde | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/ArduCopter/control_rtl.pde b/ArduCopter/control_rtl.pde index 0e7f45e843..9c1e2640ff 100644 --- a/ArduCopter/control_rtl.pde +++ b/ArduCopter/control_rtl.pde @@ -225,8 +225,17 @@ static void rtl_loiterathome_run() } // check if we've completed this stage of RTL - // To-Do: add extra check that we've reached the target yaw - rtl_state_complete = ((millis() - rtl_loiter_start_time) > (uint32_t)g.rtl_loiter_time.get()); + if ((millis() - rtl_loiter_start_time) >= (uint32_t)g.rtl_loiter_time.get()) { + if (auto_yaw_mode == AUTO_YAW_RESETTOARMEDYAW) { + // check if heading is within 2 degrees of heading when vehicle was armed + if (labs(wrap_180_cd(ahrs.yaw_sensor-initial_armed_bearing)) <= 200) { + rtl_state_complete = true; + } + } else { + // we have loitered long enough + rtl_state_complete = true; + } + } } // rtl_loiterathome_start - initialise controllers to loiter over home