From beb194b4a1664afc7baa17a152fed2902d5fcba0 Mon Sep 17 00:00:00 2001 From: xianglunkai <1322099375@qq.com> Date: Tue, 15 Feb 2022 20:34:28 +0900 Subject: [PATCH] AP_L1_Control: update_waypoint wrap added to nav_bearing Co-authored-by: Iampete1 --- libraries/AP_L1_Control/AP_L1_Control.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_L1_Control/AP_L1_Control.cpp b/libraries/AP_L1_Control/AP_L1_Control.cpp index b685895194..2ff3696c53 100644 --- a/libraries/AP_L1_Control/AP_L1_Control.cpp +++ b/libraries/AP_L1_Control/AP_L1_Control.cpp @@ -313,7 +313,7 @@ void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct Nu1 += _L1_xtrack_i; Nu = Nu1 + Nu2; - _nav_bearing = atan2f(AB.y, AB.x) + Nu1; // bearing (radians) from AC to L1 point + _nav_bearing = wrap_PI(atan2f(AB.y, AB.x) + Nu1); // bearing (radians) from AC to L1 point } _prevent_indecision(Nu);