FW L1 controller: when loitering, set the yaw setpoint to be tangentially to loiter circle

Signed-off-by: Silvan Fuhrer <[email protected]>

Author: sfuhrer

src/lib/l1/ECL_L1_Pos_Controller.cpp

@@ -306,8 +306,9 @@ ECL_L1_Pos_Controller::navigate_loiter(const Vector2d &vector_A, const Vector2d
 		_lateral_accel = lateral_accel_sp_circle;
 		_circle_mode = true;
 		_bearing_error = 0.0f;
-		/* bearing from current position to L1 point */
-		_nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0));
+		// 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;
 	}
 
 	update_roll_setpoint();