AutoLineSV: Always publish a valid yaw setpoint

In MPC_YAW_MODE 3 (along trajectory), the flight task is asked to
produce a yaw setpoint. If the previous yaw value was NAN (e.g.: during
early takeoff), the setpoint should then be reset to the current
estimate.

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp

@@ -688,7 +688,7 @@ void FlightTaskAuto::_generateHeading()
 {
 	// Generate heading along trajectory if possible, otherwise hold the previous yaw setpoint
 	if (!_generateHeadingAlongTraj()) {
-		_yaw_setpoint = _yaw_sp_prev;
+		_yaw_setpoint = PX4_ISFINITE(_yaw_sp_prev) ? _yaw_sp_prev : _yaw;
 	}
 }