diff --git a/ArduPlane/commands.cpp b/ArduPlane/commands.cpp index de75ebdc20..1cbacd3968 100644 --- a/ArduPlane/commands.cpp +++ b/ArduPlane/commands.cpp @@ -91,6 +91,9 @@ void Plane::set_guided_WP(void) setup_glide_slope(); setup_turn_angle(); + // disable crosstrack, head directly to the point + auto_state.crosstrack = false; + // reset loiter start time. loiter.start_time_ms = 0; diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 1a26dfe9a9..7a6975ddb0 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -190,7 +190,7 @@ void Plane::update_loiter(uint16_t radius) quadplane.guided_start(); } } else if ((loiter.start_time_ms == 0 && - control_mode == AUTO && + (control_mode == AUTO || control_mode == GUIDED) && auto_state.crosstrack && get_distance(current_loc, next_WP_loc) > radius*3) || (control_mode == RTL && quadplane.available() && quadplane.rtl_mode == 1)) {