Copter: rtl initialises wp controller earlier

We need to initialise wp controller or the build_path could use the wrong speeds and accelerations when calculating the origin (aka stopping point)
This commit is contained in:
Randy Mackay 2017-04-27 15:53:05 +09:00
parent 22ab8de4d2
commit 65e97a9503
1 changed files with 2 additions and 3 deletions

View File

@ -11,6 +11,8 @@
bool Copter::rtl_init(bool ignore_checks)
{
if (position_ok() || ignore_checks) {
// initialise waypoint and spline controller
wp_nav->wp_and_spline_init();
rtl_build_path(!failsafe.terrain);
rtl_climb_start();
return true;
@ -91,9 +93,6 @@ void Copter::rtl_climb_start()
rtl_state = RTL_InitialClimb;
rtl_state_complete = false;
// initialise waypoint and spline controller
wp_nav->wp_and_spline_init();
// RTL_SPEED == 0 means use WPNAV_SPEED
if (g.rtl_speed_cms != 0) {
wp_nav->set_speed_xy(g.rtl_speed_cms);