aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: RTL ensures heading is back to initial heading

Browse Source 
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.
This commit is contained in:
Randy Mackay 2014-04-24 18:52:21 +09:00
parent e23115516d
commit ea64438ef9
1 changed files with 11 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
ArduCopter/control_rtl.pde
View File

@ -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