From 3c03d488765f06146420f6d5a7c28c678594b21c Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 20 Oct 2021 09:45:22 +0200 Subject: [PATCH] FW L1 controller: also handle negative loiter direction case Signed-off-by: Silvan Fuhrer --- src/lib/l1/ECL_L1_Pos_Controller.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/lib/l1/ECL_L1_Pos_Controller.cpp b/src/lib/l1/ECL_L1_Pos_Controller.cpp index 042496956c..89e67e1c32 100644 --- a/src/lib/l1/ECL_L1_Pos_Controller.cpp +++ b/src/lib/l1/ECL_L1_Pos_Controller.cpp @@ -307,8 +307,9 @@ ECL_L1_Pos_Controller::navigate_loiter(const Vector2d &vector_A, const Vector2d _circle_mode = true; _bearing_error = 0.0f; // once loitering, the bearing is tangentially to the loiter, - // so subtract PI/2 from the bearing from current position to loiter center - _nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0)) - M_PI_2_F; + // so subtract/add PI/2 from the bearing from current position to loiter center depending on loiter direction + const float center_to_tangent = M_PI_2_F * loiter_direction * -1.f; + _nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0)) + center_to_tangent; } update_roll_setpoint();