diff --git a/src/lib/npfg/npfg.cpp b/src/lib/npfg/npfg.cpp
index 870a0b4065..87e005f562 100644
--- a/src/lib/npfg/npfg.cpp
+++ b/src/lib/npfg/npfg.cpp
@@ -617,6 +617,25 @@ void NPFG::navigateLoiter(const Vector2d &loiter_center, const Vector2d &vehicle
 	updateRollSetpoint();
 } // navigateLoiter
 
+
+void NPFG::navigatePathTangent(const matrix::Vector2d &vehicle_pos, const matrix::Vector2d &position_setpoint,
+			       const matrix::Vector2f &tangent_setpoint,
+			       const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel, const float &curvature)
+{
+	path_type_loiter_ = false;
+
+	// set unit tangent directly
+	unit_path_tangent_ = tangent_setpoint.normalized();
+
+	// closest point to vehicle
+	matrix::Vector2f error_vector = getLocalPlanarVector(position_setpoint, vehicle_pos);
+	signed_track_error_ = cross2D(unit_path_tangent_, error_vector);
+
+	guideToPath(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, curvature);
+
+	updateRollSetpoint();
+} // navigatePathTangent
+
 void NPFG::navigateHeading(float heading_ref, const Vector2f &ground_vel, const Vector2f &wind_vel)
 {
 	path_type_loiter_ = false;
diff --git a/src/lib/npfg/npfg.hpp b/src/lib/npfg/npfg.hpp
index 61c0cf4f3f..932105f83d 100644
--- a/src/lib/npfg/npfg.hpp
+++ b/src/lib/npfg/npfg.hpp
@@ -248,6 +248,21 @@ public:
 			    float radius, int8_t loiter_direction, const matrix::Vector2f &ground_vel,
 			    const matrix::Vector2f &wind_vel);
 
+	/*
+	 * Path following logic. Takes poisiton, path tangent, curvature and
+	 * then updates control setpoints to follow a path setpoint.
+	 *
+	 * @param[in] vehicle_pos vehicle_pos Vehicle position in WGS84 coordinates (lat,lon) [deg]
+	 * @param[in] position_setpoint closest point on a path in WGS84 coordinates (lat,lon) [deg]
+	 * @param[in] tangent_setpoint unit tangent vector of the path [m]
+	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
+	 * @param[in] wind_vel Wind velocity vector [m/s]
+	 * @param[in] curvature of the path setpoint [1/m]
+	 */
+	void navigatePathTangent(const matrix::Vector2d &vehicle_pos, const matrix::Vector2d &position_setpoint,
+				 const matrix::Vector2f &tangent_setpoint,
+				 const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel, const float &curvature);
+
 	/*
 	 * Navigate on a fixed heading.
 	 *
diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
index 92e24042c1..e69499a2df 100644
--- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
@@ -1191,6 +1191,16 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const fl
 	if (_param_fw_use_npfg.get()) {
 		_npfg.setAirspeedNom(target_airspeed * _eas2tas);
 		_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+
+		if (_control_mode.flag_control_offboard_enabled && PX4_ISFINITE(pos_sp_curr.vx) && PX4_ISFINITE(pos_sp_curr.vy)) {
+			// Navigate directly on position setpoint and path tangent
+			matrix::Vector2f velocity_2d(pos_sp_curr.vx, pos_sp_curr.vy);
+			_npfg.navigatePathTangent(curr_pos, curr_wp, velocity_2d.normalized(), ground_speed, _wind_vel, pos_sp_curr.yawspeed);
+
+		} else {
+			_npfg.navigateWaypoints(prev_wp, curr_wp, curr_pos, ground_speed, _wind_vel);
+		}
+
 		_npfg.navigateWaypoints(prev_wp, curr_wp, curr_pos, get_nav_speed_2d(ground_speed), _wind_vel);
 		_att_sp.roll_body = _npfg.getRollSetpoint();
 		target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
@@ -2219,44 +2229,49 @@ FixedwingPositionControl::Run()
 			vehicle_local_position_setpoint_s trajectory_setpoint;
 
 			if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) {
+				bool valid_setpoint = false;
+				_pos_sp_triplet = {}; // clear any existing
+				_pos_sp_triplet.timestamp = trajectory_setpoint.timestamp;
+				_pos_sp_triplet.current.timestamp = trajectory_setpoint.timestamp;
+				_pos_sp_triplet.current.cruising_speed = NAN; // ignored
+				_pos_sp_triplet.current.cruising_throttle = NAN; // ignored
+				_pos_sp_triplet.current.vx = NAN;
+				_pos_sp_triplet.current.vy = NAN;
+				_pos_sp_triplet.current.vz = NAN;
+				_pos_sp_triplet.current.lat = static_cast<double>(NAN);
+				_pos_sp_triplet.current.lon = static_cast<double>(NAN);
+				_pos_sp_triplet.current.alt = NAN;
+
 				if (PX4_ISFINITE(trajectory_setpoint.x) && PX4_ISFINITE(trajectory_setpoint.y) && PX4_ISFINITE(trajectory_setpoint.z)) {
-					double lat;
-					double lon;
-
 					if (_global_local_proj_ref.isInitialized()) {
+						double lat;
+						double lon;
 						_global_local_proj_ref.reproject(trajectory_setpoint.x, trajectory_setpoint.y, lat, lon);
-						_pos_sp_triplet = {}; // clear any existing
-
-						_pos_sp_triplet.timestamp = trajectory_setpoint.timestamp;
-						_pos_sp_triplet.current.timestamp = trajectory_setpoint.timestamp;
-						_pos_sp_triplet.current.valid = true;
+						valid_setpoint = true;
 						_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
 						_pos_sp_triplet.current.lat = lat;
 						_pos_sp_triplet.current.lon = lon;
 						_pos_sp_triplet.current.alt = _global_local_alt0 - trajectory_setpoint.z;
-						_pos_sp_triplet.current.cruising_speed = NAN; // ignored
-						_pos_sp_triplet.current.cruising_throttle = NAN; // ignored
 					}
 
-				} else if (PX4_ISFINITE(trajectory_setpoint.vx) && PX4_ISFINITE(trajectory_setpoint.vx)
-					   && PX4_ISFINITE(trajectory_setpoint.vz)) {
-					_pos_sp_triplet = {}; // clear any existing
+				}
 
-					_pos_sp_triplet.timestamp = trajectory_setpoint.timestamp;
-					_pos_sp_triplet.current.timestamp = trajectory_setpoint.timestamp;
-					_pos_sp_triplet.current.valid = true;
+				if (PX4_ISFINITE(trajectory_setpoint.vx) && PX4_ISFINITE(trajectory_setpoint.vx)
+				    && PX4_ISFINITE(trajectory_setpoint.vz)) {
+					valid_setpoint = true;
 					_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
 					_pos_sp_triplet.current.vx = trajectory_setpoint.vx;
 					_pos_sp_triplet.current.vy = trajectory_setpoint.vy;
 					_pos_sp_triplet.current.vz = trajectory_setpoint.vz;
-					_pos_sp_triplet.current.alt = NAN;
-					_pos_sp_triplet.current.cruising_speed = NAN; // ignored
-					_pos_sp_triplet.current.cruising_throttle = NAN; // ignored
+				}
 
-				} else {
+				if (!valid_setpoint) {
 					mavlink_log_critical(&_mavlink_log_pub, "Invalid offboard setpoint\t");
 					events::send(events::ID("fixedwing_position_control_invalid_offboard_sp"), events::Log::Error,
 						     "Invalid offboard setpoint");
+
+				} else {
+					_pos_sp_triplet.current.valid = valid_setpoint;
 				}
 			}