diff --git a/ArduPlane/mode_cruise.cpp b/ArduPlane/mode_cruise.cpp index 57d3bcb4be..2d8c4c428c 100644 --- a/ArduPlane/mode_cruise.cpp +++ b/ArduPlane/mode_cruise.cpp @@ -58,11 +58,17 @@ void ModeCruise::navigate() return; } #endif + + // check if we are moving in the direction of the front of the vehicle + const int32_t ground_course_cd = plane.gps.ground_course_cd(); + const bool moving_forwards = fabsf(wrap_PI(radians(ground_course_cd * 0.01) - plane.ahrs.yaw)) < M_PI_2; + if (!locked_heading && plane.channel_roll->get_control_in() == 0 && plane.rudder_input() == 0 && plane.gps.status() >= AP_GPS::GPS_OK_FIX_2D && plane.gps.ground_speed() >= 3 && + moving_forwards && lock_timer_ms == 0) { // user wants to lock the heading - start the timer lock_timer_ms = millis(); @@ -73,7 +79,7 @@ void ModeCruise::navigate() // from user locked_heading = true; lock_timer_ms = 0; - locked_heading_cd = plane.gps.ground_course_cd(); + locked_heading_cd = ground_course_cd; plane.prev_WP_loc = plane.current_loc; } if (locked_heading) {