FlightTaskAuto: lock yaw once within acceptance radius

src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp

@@ -256,13 +256,20 @@ void FlightTaskAuto::_set_heading_from_mode()
 	}
 
 	if (PX4_ISFINITE(v.length())) {
-		// We only adjust yaw if vehicle is outside of acceptance radius.
+		// We only adjust yaw if vehicle is outside of acceptance radius. Once we enter acceptance
+		// radius, lock yaw to current yaw.
 		// This prevents excessive yawing.
 		if (v.length() > NAV_ACC_RAD.get()) {
 			_compute_heading_from_2D_vector(_yaw_setpoint, v);
+			_yaw_lock = false;
+		} else {
+			if (!_yaw_lock) {
+				_yaw_setpoint = _yaw;
+				_yaw_lock = true;
 		}
 
 	} else {
+		_yaw_lock = false;
 		_yaw_setpoint = NAN;
 	}
 }

src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp

@@ -107,7 +107,7 @@ protected:
 
 private:
 	matrix::Vector2f _lock_position_xy{NAN, NAN}; /**< if no valid triplet is received, lock positition to current position */
-
+	bool _yaw_lock = false; /**< if within acceptance radius, lock yaw to current yaw */
 	uORB::Subscription<position_setpoint_triplet_s> *_sub_triplet_setpoint{nullptr};
 
 	matrix::Vector3f