From 176945352044fc61a48fe3590518ea3d8a2168cb Mon Sep 17 00:00:00 2001 From: Mark Agate Date: Fri, 1 Sep 2023 07:56:10 -0700 Subject: [PATCH] AP_L1_Control: check vehicle yaw (heading) and ground track agree --- libraries/AP_L1_Control/AP_L1_Control.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/libraries/AP_L1_Control/AP_L1_Control.cpp b/libraries/AP_L1_Control/AP_L1_Control.cpp index b15bdc9122..816637af45 100644 --- a/libraries/AP_L1_Control/AP_L1_Control.cpp +++ b/libraries/AP_L1_Control/AP_L1_Control.cpp @@ -240,7 +240,11 @@ void AP_L1_Control::update_waypoint(const Location &prev_WP, const Location &nex //Calculate groundspeed float groundSpeed = _groundspeed_vector.length(); - if (groundSpeed < 0.1f) { + + // check if we are moving in the direction of the front of the vehicle + const bool moving_forwards = fabsf(wrap_PI(_groundspeed_vector.angle() - get_yaw())) < M_PI_2; + + if (groundSpeed < 0.1f || !moving_forwards) { // use a small ground speed vector in the right direction, // allowing us to use the compass heading at zero GPS velocity groundSpeed = 0.1f;