integrate optional NPFG library for wind-aware fixed-wing guidance

msg/CMakeLists.txt

@@ -112,6 +112,7 @@ set(msg_files
 	mission_result.msg
 	mount_orientation.msg
 	navigator_mission_item.msg
+  npfg_status.msg
 	obstacle_distance.msg
 	offboard_control_mode.msg
 	onboard_computer_status.msg

msg/npfg_status.msg

@@ -0,0 +1,16 @@
+uint64 timestamp                # time since system start (microseconds)
+
+uint8 wind_est_valid            # (boolean) true = wind estimate is valid and/or being used by controller (also indicates if wind est usage is disabled despite being valid)
+float32 lat_accel               # resultant lateral acceleration reference [m/s^2]
+float32 lat_accel_ff            # lateral acceleration demand only for maintaining curvature [m/s^2]
+float32 bearing_feas            # bearing feasibility [0,1]
+float32 bearing_feas_on_track   # on-track bearing feasibility [0,1]
+float32 signed_track_error      # signed track error [m]
+float32 track_error_bound       # track error bound [m]
+float32 airspeed_ref            # (true) airspeed reference [m/s]
+float32 bearing                 # bearing angle [rad]
+float32 heading_ref             # heading angle reference [rad]
+float32 min_ground_speed_ref    # minimum forward ground speed reference [m/s]
+float32 adapted_period          # adapted period (if auto-tuning enabled) [s]
+float32 p_gain                  # controller proportional gain [rad/s]
+float32 time_const              # controller time constant [s]

src/lib/CMakeLists.txt

@@ -54,6 +54,7 @@ add_subdirectory(mathlib)
 add_subdirectory(mixer)
 add_subdirectory(mixer_module)
 add_subdirectory(motion_planning)
+add_subdirectory(npfg)
 add_subdirectory(perf)
 add_subdirectory(pid)
 add_subdirectory(pid_design)

src/lib/npfg/CMakeLists.txt

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2018-2020 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(npfg
+	npfg.cpp
+	npfg.hpp
+)
+
+target_link_libraries(npfg PRIVATE geo)

src/lib/npfg/npfg.cpp

@@ -0,0 +1,694 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file npfg.cpp
+ * Implementation of a lateral-directional nonlinear path following guidance
+ * law with excess wind handling. Commands lateral acceleration and airspeed.
+ *
+ * Authors and acknowledgements in header.
+ */
+
+#include "npfg.hpp"
+#include <lib/geo/geo.h>
+#include <px4_platform_common/defines.h>
+#include <float.h>
+
+using matrix::Vector2d;
+using matrix::Vector2f;
+
+void NPFG::guideToPath(const Vector2f &ground_vel, const Vector2f &wind_vel, const Vector2f &unit_path_tangent,
+		       const float signed_track_error, const float path_curvature)
+{
+	const float ground_speed = ground_vel.norm();
+
+	const Vector2f air_vel = ground_vel - wind_vel;
+	const float airspeed = air_vel.norm();
+
+	const float wind_speed = wind_vel.norm();
+
+	// on-track wind triangle projections
+	const float wind_cross_upt = cross2D(wind_vel, unit_path_tangent);
+	const float wind_dot_upt = wind_vel.dot(unit_path_tangent);
+
+	// calculate the bearing feasibility on the track at the current closest point
+	feas_on_track_ = bearingFeasibility(wind_cross_upt, wind_dot_upt, airspeed, wind_speed);
+
+	const float track_error = fabsf(signed_track_error);
+
+	// update control parameters considering upper and lower stability bounds (if enabled)
+	// must be called before trackErrorBound() as it updates time_const_
+	adapted_period_ = adaptPeriod(ground_speed, airspeed, wind_speed, track_error,
+				      path_curvature, wind_vel, unit_path_tangent, feas_on_track_);
+	p_gain_ = pGain(adapted_period_, damping_);
+	time_const_ = timeConst(adapted_period_, damping_);
+
+	// track error bound is dynamic depending on ground speed
+	track_error_bound_ = trackErrorBound(ground_speed, time_const_);
+	const float normalized_track_error = normalizedTrackError(track_error, track_error_bound_);
+
+	// look ahead angle based solely on track proximity
+	const float look_ahead_ang = lookAheadAngle(normalized_track_error);
+
+	track_proximity_ = trackProximity(look_ahead_ang);
+
+	bearing_vec_ = bearingVec(unit_path_tangent, look_ahead_ang, signed_track_error);
+
+	// wind triangle projections
+	const float wind_cross_bearing = cross2D(wind_vel, bearing_vec_);
+	const float wind_dot_bearing = wind_vel.dot(bearing_vec_);
+
+	// continuous representation of the bearing feasibility
+	feas_ = bearingFeasibility(wind_cross_bearing, wind_dot_bearing, airspeed, wind_speed);
+
+	// we consider feasibility of both the current bearing as well as that on the track at the current closest point
+	const float feas_combined = feas_ * feas_on_track_;
+
+	min_ground_speed_ref_ = minGroundSpeed(normalized_track_error, feas_combined);
+
+	// reference air velocity with directional feedforward effect for following
+	// curvature in wind and magnitude incrementation depending on minimum ground
+	// speed violations and/or high wind conditions in general
+	air_vel_ref_ = refAirVelocity(wind_vel, bearing_vec_, wind_cross_bearing,
+				      wind_dot_bearing, wind_speed, min_ground_speed_ref_);
+	airspeed_ref_ = air_vel_ref_.norm();
+
+	// lateral acceleration demand based on heading error
+	const float lateral_accel = lateralAccel(air_vel, air_vel_ref_, airspeed);
+
+	// lateral acceleration needed to stay on curved track (assuming no heading error)
+	lateral_accel_ff_ = lateralAccelFF(unit_path_tangent, ground_vel, wind_dot_upt,
+					   wind_cross_upt, airspeed, wind_speed, signed_track_error, path_curvature);
+
+	// total lateral acceleration to drive aircaft towards track as well as account
+	// for path curvature. The full effect of the feed-forward acceleration is smoothly
+	// ramped in as the vehicle approaches the track and is further smoothly
+	// zeroed out as the bearing becomes infeasible.
+	lateral_accel_ = lateral_accel + feas_combined * track_proximity_ * lateral_accel_ff_;
+} // guideToPath
+
+void NPFG::guideToPoint(const Vector2f &ground_vel, const Vector2f &wind_vel, const Vector2f &bearing_vec,
+			const float track_error)
+{
+	bearing_vec_ = bearing_vec; // for status output
+
+	const float ground_speed = ground_vel.norm();
+
+	const Vector2f air_vel = ground_vel - wind_vel;
+	const float airspeed = air_vel.norm();
+
+	const float wind_speed = wind_vel.norm();
+
+	// wind triangle projections
+	const float wind_cross_bearing = cross2D(wind_vel, bearing_vec);
+	const float wind_dot_bearing = wind_vel.dot(bearing_vec);
+
+	// continuous representation of the bearing feasibility
+	feas_ = bearingFeasibility(wind_cross_bearing, wind_dot_bearing, airspeed, wind_speed);
+	feas_on_track_ = feas_; // no distinction in point following - set only for recording
+
+	// update control parameters considering upper and lower stability bounds (if enabled)
+	// must be called before trackErrorBound() as it updates time_const_
+	// NOTE: track error input as 0 for the period adaptation as track proximity will
+	//       only ramp in 1) curvature based lower bounding, of which there is none
+	//       for a point, and 2) period upper bounds, which for zero curvature is
+	//       infinite, and thus disregarded in this case.
+	adapted_period_ = adaptPeriod(ground_speed, airspeed, wind_speed, 0.0f, 0.0f,
+				      wind_vel, bearing_vec, feas_);
+	p_gain_ = pGain(adapted_period_, damping_);
+	time_const_ = timeConst(adapted_period_, damping_);
+
+	// track error bound is dynamic depending on ground speed
+	track_error_bound_ = trackErrorBound(ground_speed, time_const_);
+	const float normalized_track_error = normalizedTrackError(track_error, track_error_bound_);
+
+	// look ahead angle based solely on track proximity
+	const float look_ahead_ang = lookAheadAngle(normalized_track_error);
+
+	track_proximity_ = trackProximity(look_ahead_ang);
+
+	min_ground_speed_ref_ = minGroundSpeed(normalized_track_error, feas_);
+
+	// reference air velocity with directional feedforward effect for following
+	// curvature in wind and magnitude incrementation depending on minimum ground
+	// speed violations and/or high wind conditions in general
+	air_vel_ref_ = refAirVelocity(wind_vel, bearing_vec, wind_cross_bearing,
+				      wind_dot_bearing, wind_speed, min_ground_speed_ref_);
+	airspeed_ref_ = air_vel_ref_.norm();
+
+	// lateral acceleration demand based on heading error
+	lateral_accel_ff_ = 0.0f;
+	lateral_accel_ = lateralAccel(air_vel, air_vel_ref_, airspeed);
+} // guideToPoint
+
+float NPFG::adaptPeriod(const float ground_speed, const float airspeed, const float wind_speed,
+			const float track_error, const float path_curvature, const Vector2f &wind_vel,
+			const Vector2f &unit_path_tangent, const float feas_on_track) const
+{
+	float period = period_;
+	const float air_turn_rate = fabsf(path_curvature * airspeed);
+	const float wind_factor = windFactor(airspeed, wind_speed);
+
+	if (en_period_lb_ && roll_time_const_ > NPFG_EPSILON) {
+		// lower bound for period not considering path curvature
+		const float period_lb_zero_curvature = periodLowerBound(0.0f, wind_factor, feas_on_track) * period_safety_factor_;
+
+		// lower bound for period *considering path curvature
+		float period_lb = periodLowerBound(air_turn_rate, wind_factor, feas_on_track) * period_safety_factor_;
+
+		// calculate the time constant and track error bound considering the zero
+		// curvature, lower-bounded period and subsequently recalculate the normalized
+		// track error
+		const float time_const = timeConst(period_lb_zero_curvature, damping_);
+		const float track_error_bound = trackErrorBound(ground_speed, time_const);
+		const float normalized_track_error = normalizedTrackError(track_error, track_error_bound);
+
+		// calculate nominal track proximity with lower bounded time constant
+		// (only a numerical solution can find corresponding track proximity
+		// and adapted gains simultaneously)
+		const float look_ahead_ang = lookAheadAngle(normalized_track_error);
+		const float track_proximity = trackProximity(look_ahead_ang);
+
+		// linearly ramp in curvature dependent lower bound with track proximity
+		period_lb = period_lb * track_proximity + (1.0f - track_proximity) * period_lb_zero_curvature;
+
+		// lower bounded period
+		period = math::max(period_lb, period);
+
+		// only allow upper bounding ONLY if lower bounding is enabled (is otherwise
+		// dangerous to allow period decrements without stability checks).
+		// NOTE: if the roll time constant is not accurately known, lower-bound
+		// checks may be too optimistic and reducing the period can still destabilize
+		// the system! enable this feature at your own risk.
+		if (en_period_ub_) {
+
+			const float period_ub = periodUpperBound(air_turn_rate, wind_factor, feas_on_track);
+
+			if (en_period_ub_ && PX4_ISFINITE(period_ub) && period > period_ub) {
+				// NOTE: if the roll time constant is not accurately known, reducing
+				// the period here can destabilize the system!
+				// enable this feature at your own risk!
+
+				// upper bound the period (for track keeping stability), prefer lower bound if violated
+				const float period_adapted = math::max(period_lb, period_ub);
+
+				// transition from the nominal period to the adapted period as we get
+				// closer to the track
+				period = period_adapted * track_proximity + (1.0f - track_proximity) * period;
+			}
+		}
+	}
+
+	return period;
+} // adaptPeriod
+
+float NPFG::normalizedTrackError(const float track_error, const float track_error_bound) const
+{
+	return math::constrain(track_error / track_error_bound, 0.0f, 1.0f);
+}
+
+float NPFG::windFactor(const float airspeed, const float wind_speed) const
+{
+	// See [TODO: include citation] for definition/elaboration of this approximation.
+	if (wind_speed > airspeed || airspeed < NPFG_EPSILON) {
+		return 2.0f;
+
+	} else {
+		return 2.0f * (1.0f - sqrtf(1.0f - math::min(1.0f, wind_speed / airspeed)));
+	}
+} // windFactor
+
+float NPFG::periodUpperBound(const float air_turn_rate, const float wind_factor, const float feas_on_track) const
+{
+	if (air_turn_rate * wind_factor > NPFG_EPSILON) {
+		// multiply air turn rate by feasibility on track to zero out when we anyway
+		// should not consider the curvature
+		return 4.0f * M_PI_F * damping_ / (air_turn_rate * wind_factor * feas_on_track);
+	}
+
+	return INFINITY;
+} // periodUB
+
+float NPFG::periodLowerBound(const float air_turn_rate, const float wind_factor, const float feas_on_track) const
+{
+	// this method considers a "conservative" lower period bound, i.e. a constant
+	// worst case bound for any wind ratio, airspeed, and path curvature
+
+	// the lower bound for zero curvature and no wind OR damping ratio < 0.5
+	const float period_lb = M_PI_F * roll_time_const_ / damping_;
+
+	if (air_turn_rate * wind_factor < NPFG_EPSILON || damping_ < 0.5f) {
+		return period_lb;
+
+	} else {
+		// the lower bound for tracking a curved path in wind with damping ratio > 0.5
+		const float period_windy_curved_damped = 4.0f * M_PI_F * roll_time_const_ * damping_;
+
+		// blend the two together as the bearing on track becomes less feasible
+		return period_windy_curved_damped * feas_on_track + (1.0f - feas_on_track) * period_lb;
+	}
+} // periodLB
+
+float NPFG::trackProximity(const float look_ahead_ang) const
+{
+	const float sin_look_ahead_ang = sinf(look_ahead_ang);
+	return sin_look_ahead_ang * sin_look_ahead_ang;
+} // trackProximity
+
+float NPFG::trackErrorBound(const float ground_speed, const float time_const) const
+{
+	if (ground_speed > 1.0f) {
+		return ground_speed * time_const;
+
+	} else {
+		// limit bound to some minimum ground speed to avoid singularities in track
+		// error normalization. the following equation assumes ground speed minimum = 1.0
+		return 0.5f * time_const * (ground_speed * ground_speed + 1.0f);
+	}
+} // trackErrorBound
+
+float NPFG::pGain(const float period, const float damping) const
+{
+	return 4.0f * M_PI_F * damping / period;
+} // pGain
+
+float NPFG::timeConst(const float period, const float damping) const
+{
+	return period * damping;
+} // timeConst
+
+float NPFG::lookAheadAngle(const float normalized_track_error) const
+{
+	return M_PI_F * 0.5f * (normalized_track_error - 1.0f) * (normalized_track_error - 1.0f);
+} // lookAheadAngle
+
+Vector2f NPFG::bearingVec(const Vector2f &unit_path_tangent, const float look_ahead_ang,
+			  const float signed_track_error) const
+{
+	const float cos_look_ahead_ang = cosf(look_ahead_ang);
+	const float sin_look_ahead_ang = sinf(look_ahead_ang);
+
+	Vector2f unit_path_normal(-unit_path_tangent(1.0f), unit_path_tangent(0.0f)); // right handed 90 deg (clockwise) turn
+	Vector2f unit_track_error = -((signed_track_error < 0.0f) ? -1.0f : 1.0f) * unit_path_normal;
+
+	return cos_look_ahead_ang * unit_track_error + sin_look_ahead_ang * unit_path_tangent;
+} // bearingVec
+
+float NPFG::minGroundSpeed(const float normalized_track_error, const float feas)
+{
+	// minimum ground speed demand from track keeping logic
+	min_gsp_track_keeping_ = 0.0f;
+
+	if (en_track_keeping_ && en_wind_excess_regulation_) {
+		// zero out track keeping speed increment when bearing is feasible
+		// maximum track keeping speed increment is applied until we are within
+		// a user defined fraction of the normalized track error
+		min_gsp_track_keeping_ = (1.0f - feas) * min_gsp_track_keeping_max_ * math::constrain(
+						 normalized_track_error / NTE_FRACTION, 0.0f,
+						 1.0f);
+	}
+
+	// minimum ground speed demand from minimum forward ground speed user setting
+	float min_gsp_desired = 0.0f;
+
+	if (en_min_ground_speed_ && en_wind_excess_regulation_) {
+		min_gsp_desired = min_gsp_desired_;
+	}
+
+	return math::max(min_gsp_track_keeping_, min_gsp_desired);
+} // minGroundSpeed
+
+Vector2f NPFG::refAirVelocity(const Vector2f &wind_vel, const Vector2f &bearing_vec,
+			      const float wind_cross_bearing, const float wind_dot_bearing, const float wind_speed,
+			      const float min_ground_speed) const
+{
+	Vector2f air_vel_ref;
+
+	if (min_ground_speed > wind_dot_bearing && (en_min_ground_speed_ || en_track_keeping_) && en_wind_excess_regulation_) {
+		// minimum ground speed and/or track keeping
+
+		// airspeed required to achieve minimum ground speed along bearing vector
+		const float airspeed_min = sqrtf((min_ground_speed - wind_dot_bearing) * (min_ground_speed - wind_dot_bearing) +
+						 wind_cross_bearing * wind_cross_bearing);
+
+		if (airspeed_min > airspeed_max_) {
+			if (bearingIsFeasible(wind_cross_bearing, wind_dot_bearing, airspeed_max_, wind_speed)) {
+				// we will not maintain the minimum ground speed, but can still achieve the bearing at maximum airspeed
+				const float airsp_dot_bearing = projectAirspOnBearing(airspeed_max_, wind_cross_bearing);
+				air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vec);
+
+			} else {
+				// bearing is maximally infeasible, employ mitigation law
+				air_vel_ref = infeasibleAirVelRef(wind_vel, bearing_vec, wind_speed, airspeed_max_);
+			}
+
+		} else if (airspeed_min > airspeed_nom_) {
+			// the minimum ground speed is achievable within the nom - max airspeed range
+			// solve wind triangle with for air velocity reference with minimum airspeed
+			const float airsp_dot_bearing = projectAirspOnBearing(airspeed_min, wind_cross_bearing);
+			air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vec);
+
+		} else {
+			// the minimum required airspeed is less than nominal, so we can track the bearing and minimum
+			// ground speed with our nominal airspeed reference
+			const float airsp_dot_bearing = projectAirspOnBearing(airspeed_nom_, wind_cross_bearing);
+			air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vec);
+		}
+
+	} else {
+		// wind excess regulation and/or mitigation
+
+		if (bearingIsFeasible(wind_cross_bearing, wind_dot_bearing, airspeed_nom_, wind_speed)) {
+			// bearing is nominally feasible, solve wind triangle for air velocity reference using nominal airspeed
+			const float airsp_dot_bearing = projectAirspOnBearing(airspeed_nom_, wind_cross_bearing);
+			air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vec);
+
+		} else if (bearingIsFeasible(wind_cross_bearing, wind_dot_bearing, airspeed_max_, wind_speed)
+			   && en_wind_excess_regulation_) {
+			// bearing is maximally feasible
+			if (wind_dot_bearing <= 0.0f) {
+				// we only increment the airspeed to regulate, but not overcome, excess wind
+				// NOTE: in the terminal condition, this will result in a zero ground velocity configuration
+				air_vel_ref = wind_vel;
+
+			} else {
+				// the bearing is achievable within the nom - max airspeed range
+				// solve wind triangle with for air velocity reference with minimum airspeed
+				const float airsp_dot_bearing = 0.0f; // right angle to the bearing line gives minimal airspeed usage
+				air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vec);
+			}
+
+		} else {
+			// bearing is maximally infeasible, employ mitigation law
+			const float airspeed_input = (en_wind_excess_regulation_) ? airspeed_max_ : airspeed_nom_;
+			air_vel_ref = infeasibleAirVelRef(wind_vel, bearing_vec, wind_speed, airspeed_input);
+		}
+	}
+
+	return air_vel_ref;
+} // refAirVelocity
+
+float NPFG::projectAirspOnBearing(const float airspeed, const float wind_cross_bearing) const
+{
+	// NOTE: wind_cross_bearing must be less than airspeed to use this function
+	// it is assumed that bearing feasibility is checked and found feasible (e.g. bearingIsFeasible() = true) prior to entering this method
+	// otherwise the return will be erroneous
+	return sqrtf(math::max(airspeed * airspeed - wind_cross_bearing * wind_cross_bearing, 0.0f));
+} // projectAirspOnBearing
+
+int NPFG::bearingIsFeasible(const float wind_cross_bearing, const float wind_dot_bearing, const float airspeed,
+			    const float wind_speed) const
+{
+	return (fabsf(wind_cross_bearing) < airspeed) && ((wind_dot_bearing > 0.0f) || (wind_speed < airspeed));
+} // bearingIsFeasible
+
+Vector2f NPFG::solveWindTriangle(const float wind_cross_bearing, const float airsp_dot_bearing,
+				 const Vector2f &bearing_vec) const
+{
+	// essentially a 2D rotation with the speeds (magnitudes) baked in
+	return Vector2f{airsp_dot_bearing * bearing_vec(0) - wind_cross_bearing * bearing_vec(1),
+			wind_cross_bearing * bearing_vec(0) + airsp_dot_bearing * bearing_vec(1)};
+} // solveWindTriangle
+
+Vector2f NPFG::infeasibleAirVelRef(const Vector2f &wind_vel, const Vector2f &bearing_vec, const float wind_speed,
+				   const float airspeed) const
+{
+	// NOTE: wind speed must be greater than airspeed, and airspeed must be greater than zero to use this function
+	// it is assumed that bearing feasibility is checked and found infeasible (e.g. bearingIsFeasible() = false) prior to entering this method
+	// otherwise the normalization of the air velocity vector could have a division by zero
+	Vector2f air_vel_ref = sqrtf(math::max(wind_speed * wind_speed - airspeed * airspeed, 0.0f)) * bearing_vec - wind_vel;
+	return air_vel_ref.normalized() * airspeed;
+} // infeasibleAirVelRef
+
+float NPFG::bearingFeasibility(float wind_cross_bearing, const float wind_dot_bearing, const float airspeed,
+			       const float wind_speed) const
+{
+	if (wind_dot_bearing < 0.0f) {
+		wind_cross_bearing = wind_speed;
+
+	} else {
+		wind_cross_bearing = fabsf(wind_cross_bearing);
+	}
+
+	float sin_arg = sinf(M_PI_F * 0.5f * math::constrain((airspeed - wind_cross_bearing) / AIRSPEED_BUFFER, 0.0f, 1.0f));
+	return sin_arg * sin_arg;
+} // bearingFeasibility
+
+float NPFG::lateralAccelFF(const Vector2f &unit_path_tangent, const Vector2f &ground_vel,
+			   const float wind_dot_upt, const float wind_cross_upt, const float airspeed,
+			   const float wind_speed, const float signed_track_error, const float path_curvature) const
+{
+	// NOTE: all calculations within this function take place at the closet point
+	// on the path, as if the aircraft were already tracking the given path at
+	// this point with zero angular error. this allows us to evaluate curvature
+	// induced requirements for lateral acceleration incrementation and ramp them
+	// in with the track proximity and further consider the bearing feasibility
+	// in excess wind conditions (this is done external to this method).
+
+	// path frame curvature is the instantaneous curvature at our current distance
+	// from the actual path (considering e.g. concentric circles emanating outward/inward)
+	const float path_frame_curvature = path_curvature / math::max(1.0f - path_curvature * signed_track_error,
+					   fabsf(path_curvature) * MIN_RADIUS);
+
+	// limit tangent ground speed to along track (forward moving) direction
+	const float tangent_ground_speed = math::max(ground_vel.dot(unit_path_tangent), 0.0f);
+
+	const float path_frame_rate = path_frame_curvature * tangent_ground_speed;
+
+	// speed ratio = projection of ground vel on track / projection of air velocity
+	// on track
+	const float speed_ratio = (1.0f + wind_dot_upt / math::max(projectAirspOnBearing(airspeed, wind_cross_upt),
+				   NPFG_EPSILON));
+
+	// note the use of airspeed * speed_ratio as oppose to ground_speed^2 here --
+	// the prior considers that we command lateral acceleration in the air mass
+	// relative frame while the latter does not
+	return airspeed * speed_ratio * path_frame_rate;
+} // lateralAccelFF
+
+float NPFG::lateralAccel(const Vector2f &air_vel, const Vector2f &air_vel_ref, const float airspeed) const
+{
+	// lateral acceleration demand only from the heading error
+
+	const float dot_air_vel_err = air_vel.dot(air_vel_ref);
+	const float cross_air_vel_err = cross2D(air_vel, air_vel_ref);
+
+	if (dot_air_vel_err < 0.0f) {
+		// hold max lateral acceleration command above 90 deg heading error
+		return p_gain_ * ((cross_air_vel_err < 0.0f) ? -airspeed : airspeed);
+
+	} else {
+		// airspeed/airspeed_ref is used to scale any incremented airspeed reference back to the current airspeed
+		// for acceleration commands in a "feedback" sense (i.e. at the current vehicle airspeed)
+		return p_gain_ * cross_air_vel_err / airspeed_ref_;
+	}
+} // lateralAccel
+
+/*******************************************************************************
+ * PX4 NAVIGATION INTERFACE FUNCTIONS (provide similar functionality to ECL_L1_Pos_Controller)
+ */
+
+void NPFG::navigateWaypoints(const Vector2d &waypoint_A, const Vector2d &waypoint_B,
+			     const Vector2d &vehicle_pos, const Vector2f &ground_vel, const Vector2f &wind_vel)
+{
+	// similar to logic found in ECL_L1_Pos_Controller method of same name
+	// BUT no arbitrary max approach angle, approach entirely determined by generated
+	// bearing vectors
+
+	path_type_loiter_ = false;
+
+	Vector2f vector_A_to_B = getLocalPlanarVector(waypoint_A, waypoint_B);
+	Vector2f vector_A_to_vehicle = getLocalPlanarVector(waypoint_A, vehicle_pos);
+
+	if (vector_A_to_B.norm() < NPFG_EPSILON) {
+		// the waypoints are on top of each other and should be considered as a
+		// single waypoint, fly directly to it
+		unit_path_tangent_ = -vector_A_to_vehicle.normalized();
+		signed_track_error_ = vector_A_to_vehicle.norm();
+		guideToPoint(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_);
+
+	} else if (vector_A_to_B.dot(vector_A_to_vehicle) < 0.0f) {
+		// we are in front of waypoint A, fly directly to it until the bearing generated
+		// to the line segement between A and B is shallower than that from the
+		// bearing to the first waypoint (A).
+
+		// guidance to the line through A and B
+		unit_path_tangent_ = vector_A_to_B.normalized();
+		signed_track_error_ = cross2D(unit_path_tangent_, vector_A_to_vehicle);
+		guideToPath(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, 0.0f);
+
+		const Vector2f bearing_vec_to_point = -vector_A_to_vehicle.normalized();
+
+		if (unit_path_tangent_.dot(bearing_vec_) < unit_path_tangent_.dot(bearing_vec_to_point)) {
+			// we are in front of the first waypoint and the bearing to the point is
+			// shallower than that to the line. reset path params to fly directly to
+			// the first waypoint.
+
+			// TODO: probably better to blend these instead of hard switching (could
+			// affect the adaptive tuning if we switch between these cases with wind
+			// gusts)
+
+			unit_path_tangent_ = bearing_vec_to_point;
+			signed_track_error_ = vector_A_to_vehicle.norm();
+			guideToPoint(ground_vel, wind_vel, bearing_vec_to_point, signed_track_error_);
+		}
+
+	} else {
+		// track the line segment between A and B
+		unit_path_tangent_ = vector_A_to_B.normalized();
+		signed_track_error_ = cross2D(unit_path_tangent_, vector_A_to_vehicle);
+		guideToPath(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, 0.0f);
+	}
+
+	updateRollSetpoint();
+} // navigateWaypoints
+
+void NPFG::navigateLoiter(const Vector2d &loiter_center, const Vector2d &vehicle_pos,
+			  float radius, int8_t loiter_direction, const Vector2f &ground_vel, const Vector2f &wind_vel)
+{
+	path_type_loiter_ = true;
+
+	radius = math::max(radius, MIN_RADIUS);
+
+	Vector2f vector_center_to_vehicle = getLocalPlanarVector(loiter_center, vehicle_pos);
+	const float dist_to_center = vector_center_to_vehicle.norm();
+
+	// find the direction from the circle center to the closest point on its perimeter
+	// from the vehicle position
+	Vector2f unit_vec_center_to_closest_pt;
+
+	if (dist_to_center < 0.1f) {
+		// the logic breaks down at the circle center, employ some mitigation strategies
+		// until we exit this region
+		if (ground_vel.norm() < 0.1f) {
+			// arbitrarily set the point in the northern top of the circle
+			unit_vec_center_to_closest_pt = Vector2f{1.0f, 0.0f};
+
+		} else {
+			// set the point in the direction we are moving
+			unit_vec_center_to_closest_pt = ground_vel.normalized();
+		}
+
+	} else {
+		// set the point in the direction of the aircraft
+		unit_vec_center_to_closest_pt = vector_center_to_vehicle.normalized();
+	}
+
+	// 90 deg clockwise rotation * loiter direction
+	unit_path_tangent_ = float(loiter_direction) * Vector2f{-unit_vec_center_to_closest_pt(1), unit_vec_center_to_closest_pt(0)};
+
+	// positive in direction of path normal
+	signed_track_error_ = -loiter_direction * (dist_to_center - radius);
+
+	float path_curvature = float(loiter_direction) / radius;
+
+	guideToPath(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, path_curvature);
+
+	updateRollSetpoint();
+} // navigateLoiter
+
+void NPFG::navigateHeading(float heading_ref, const Vector2f &ground_vel, const Vector2f &wind_vel)
+{
+	path_type_loiter_ = false;
+
+	Vector2f air_vel = ground_vel - wind_vel;
+	unit_path_tangent_ = Vector2f{cosf(heading_ref), sinf(heading_ref)};
+	signed_track_error_ = 0.0f;
+
+	// use the guidance law to regulate heading error - ignoring wind or inertial position
+	guideToPath(air_vel, Vector2f{0.0f, 0.0f}, unit_path_tangent_, signed_track_error_, 0.0f);
+
+	updateRollSetpoint();
+} // navigateHeading
+
+void NPFG::navigateBearing(float bearing, const Vector2f &ground_vel, const Vector2f &wind_vel)
+{
+	path_type_loiter_ = false;
+
+	unit_path_tangent_ = Vector2f{cosf(bearing), sinf(bearing)};
+
+	signed_track_error_ = 0.0f;
+
+	// no track error or path curvature to consider, just regulate ground velocity
+	// to bearing vector
+	guideToPath(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, 0.0f);
+
+	updateRollSetpoint();
+} // navigateBearing
+
+void NPFG::navigateLevelFlight(const float heading)
+{
+	path_type_loiter_ = false;
+
+	airspeed_ref_ = airspeed_nom_;
+	lateral_accel_ = 0.0f;
+	feas_ = 1.0f;
+	bearing_vec_ = Vector2f{cosf(heading), sinf(heading)};
+	unit_path_tangent_ = bearing_vec_;
+	signed_track_error_ = 0.0f;
+
+	updateRollSetpoint();
+} // navigateLevelFlight
+
+float NPFG::switchDistance(float wp_radius) const
+{
+	return math::min(wp_radius, track_error_bound_ * switch_distance_multiplier_);
+} // switchDistance
+
+Vector2f NPFG::getLocalPlanarVector(const Vector2d &origin, const Vector2d &target) const
+{
+	/* this is an approximation for small angles, proposed by [2] */
+	const double x_angle = math::radians(target(0) - origin(0));
+	const double y_angle = math::radians(target(1) - origin(1));
+	const double x_origin_cos = cos(math::radians(origin(0)));
+
+	return Vector2f{
+		static_cast<float>(x_angle * CONSTANTS_RADIUS_OF_EARTH),
+		static_cast<float>(y_angle *x_origin_cos * CONSTANTS_RADIUS_OF_EARTH),
+	};
+} // getLocalPlanarVector
+
+void NPFG::updateRollSetpoint()
+{
+	float roll_new = atanf(lateral_accel_ * 1.0f / CONSTANTS_ONE_G);
+	roll_new = math::constrain(roll_new, -roll_lim_rad_, roll_lim_rad_);
+
+	if (dt_ > 0.0f && roll_slew_rate_ > 0.0f) {
+		// slew rate limiting active
+		roll_new = math::constrain(roll_new, roll_setpoint_ - roll_slew_rate_ * dt_, roll_setpoint_ + roll_slew_rate_ * dt_);
+	}
+
+	if (PX4_ISFINITE(roll_new)) {
+		roll_setpoint_ = roll_new;
+	}
+} // updateRollSetpoint

src/lib/npfg/npfg.hpp

@@ -0,0 +1,743 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file npfg.hpp
+ * Implementation of a lateral-directional nonlinear path following guidance
+ * law with excess wind handling. Commands lateral acceleration and airspeed.
+ *
+ * @author Thomas Stastny <tstastny@ethz.ch>
+ *
+ * Notes:
+ * - The wind estimate should be dynamic enough to capture ~1-2 second length gusts,
+ *   Otherwise the performance will suffer.
+ *
+ * Acknowledgements and References:
+ *
+ * The logic is mostly based on [1] and Paper III of [2].
+ * TODO: Concise, up to date documentation and stability analysis for the following
+ *       implementation.
+ *
+ * [1] T. Stastny and R. Siegwart. "On Flying Backwards: Preventing Run-away of
+ *     Small, Low-speed, Fixed-wing UAVs in Strong Winds". IEEE International Conference
+ *     on Intelligent Robots and Systems (IROS). 2019.
+ *     https://arxiv.org/pdf/1908.01381.pdf
+ * [2] T. Stastny. "Low-Altitude Control and Local Re-Planning Strategies for Small
+ *     Fixed-Wing UAVs". Doctoral Thesis, ETH Zürich. 2020.
+ *     https://tstastny.github.io/pdf/tstastny_phd_thesis_wcover.pdf
+ */
+
+#ifndef NPFG_H_
+#define NPFG_H_
+
+#include <matrix/math.hpp>
+#include <lib/mathlib/mathlib.h>
+
+/*
+ * NPFG
+ * Lateral-directional nonlinear path following guidance logic with excess wind handling
+ */
+class NPFG
+{
+
+public:
+
+	/*
+	 * Set the nominal controller period [s].
+	 */
+	void setPeriod(float period) { period_ = math::max(period, NPFG_EPSILON); }
+
+	/*
+	 * Set the nominal controller damping ratio.
+	 */
+	void setDamping(float damping) { damping_ = math::constrain(damping, NPFG_EPSILON, 1.0f); }
+
+	/*
+	 * Enable automatic lower bounding of the user set controller period.
+	 */
+	void enablePeriodLB(const bool en) { en_period_lb_ = en; }
+
+	/*
+	 * Enable automatic adaptation of the user set controller period for track keeping
+	 * performance.
+	 */
+	void enablePeriodUB(const bool en) { en_period_ub_ = en; }
+
+	/*
+	 * Enable minimum forward ground speed maintenance logic.
+	 */
+	void enableMinGroundSpeed(const bool en) { en_min_ground_speed_ = en; }
+
+	/*
+	 * Enable track keeping logic in excess wind conditions.
+	 */
+	void enableTrackKeeping(const bool en) { en_track_keeping_ = en; }
+
+	/*
+	 * Enable wind excess regulation. Disabling this param disables all airspeed
+	 * reference incrementaion (airspeed reference will always be nominal).
+	 */
+	void enableWindExcessRegulation(const bool en) { en_wind_excess_regulation_ = en; }
+
+	/*
+	 * Set the minimum allowed forward ground speed [m/s].
+	 */
+	void setMinGroundSpeed(float min_gsp) { min_gsp_desired_ = math::max(min_gsp, 0.0f); }
+
+	/*
+	 * Set the maximum value of the minimum forward ground speed command for track
+	 * keeping (occurs at the track error boundary) [m/s].
+	 */
+	void setMaxTrackKeepingMinGroundSpeed(float min_gsp) { min_gsp_track_keeping_max_ = math::max(min_gsp, 0.0f); }
+
+	/*
+	 * Set the nominal airspeed reference [m/s].
+	 */
+	void setAirspeedNom(float airsp) { airspeed_nom_ = math::max(airsp, 0.1f); }
+
+	/*
+	 * Set the maximum airspeed reference [m/s].
+	 */
+	void setAirspeedMax(float airsp) { airspeed_max_ = math::max(airsp, 0.1f); }
+
+	/*
+	 * Set the autopilot roll response time constant [s].
+	 */
+	void setRollTimeConst(float tc) { roll_time_const_ = tc; }
+
+	/*
+	 * Set the switch distance multiplier.
+	 */
+	void setSwitchDistanceMultiplier(float mult) { switch_distance_multiplier_ = math::max(mult, 0.1f); }
+
+	/*
+	 * Set the period safety factor.
+	 */
+	void setPeriodSafetyFactor(float sf) { period_safety_factor_ = math::max(sf, 1.0f); }
+
+	/*
+	 * @return Controller proportional gain [rad/s]
+	 */
+	float getPGain() const { return p_gain_; }
+
+	/*
+	 * @return Controller time constant [s]
+	 */
+	float getTimeConst() const { return time_const_; }
+
+	/*
+	 * @return Adapted controller period [s]
+	 */
+	float getAdaptedPeriod() const { return adapted_period_; }
+
+	/*
+	 * @return Track error boundary [m]
+	 */
+	float getTrackErrorBound() const { return track_error_bound_; }
+
+	/*
+	 * @return Signed track error [m]
+	 */
+	float getTrackError() const { return signed_track_error_; }
+
+	/*
+	 * @return Airspeed reference [m/s]
+	 */
+	float getAirspeedRef() const { return airspeed_ref_; }
+
+	/*
+	 * @return Heading angle reference [rad]
+	 */
+	float getHeadingRef() const { return atan2f(air_vel_ref_(1), air_vel_ref_(0)); }
+
+	/*
+	 * @return Bearing angle [rad]
+	 */
+	float getBearing() const { return atan2f(bearing_vec_(1), bearing_vec_(0)); }
+
+	/*
+	 * @return Lateral acceleration command [m/s^2]
+	 */
+	float getLateralAccel() const { return lateral_accel_; }
+
+	/*
+	 * @return Feed-forward lateral acceleration command increment for tracking
+	 * path curvature [m/s^2]
+	 */
+	float getLateralAccelFF() const { return lateral_accel_ff_; }
+
+	/*
+	 * @return Bearing feasibility [0, 1]
+	 */
+	float getBearingFeas() const { return feas_; }
+
+	/*
+	 * @return On-track bearing feasibility [0, 1]
+	 */
+	float getOnTrackBearingFeas() const { return feas_on_track_; }
+
+	/*
+	 * @return Minimum forward ground speed reference [m/s]
+	 */
+	float getMinGroundSpeedRef() const { return min_ground_speed_ref_; }
+
+	/*******************************************************************************
+	 * PX4 NAVIGATION INTERFACE FUNCTIONS (provide similar functionality to ECL_L1_Pos_Controller)
+	 */
+
+	/*
+	 * Waypoint handling logic following closely to the ECL_L1_Pos_Controller
+	 * method of the same name. Takes two waypoints and determines the relevant
+	 * parameters for evaluating the NPFG guidance law, then updates control setpoints.
+	 *
+	 * @param[in] waypoint_A Waypoint A (segment start) position in WGS84 coordinates
+	 *            (lat,lon) [deg]
+	 * @param[in] waypoint_B Waypoint B (segment end) position in WGS84 coordinates
+	 *            (lat,lon) [deg]
+	 * @param[in] vehicle_pos Vehicle position in WGS84 coordinates (lat,lon) [deg]
+	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
+	 * @param[in] wind_vel Wind velocity vector [m/s]
+	 */
+	void navigateWaypoints(const matrix::Vector2d &waypoint_A, const matrix::Vector2d &waypoint_B,
+			       const matrix::Vector2d &vehicle_pos, const matrix::Vector2f &ground_vel,
+			       const matrix::Vector2f &wind_vel);
+
+	/*
+	 * Loitering (unlimited) logic. Takes loiter center, radius, and direction and
+	 * determines the relevant parameters for evaluating the NPFG guidance law,
+	 * then updates control setpoints.
+	 *
+	 * @param[in] loiter_center The position of the center of the loiter circle [m]
+	 * @param[in] vehicle_pos Vehicle position in WGS84 coordinates (lat,lon) [deg]
+	 * @param[in] radius Loiter radius [m]
+	 * @param[in] loiter_direction Loiter direction: -1=counter-clockwise, 1=clockwise
+	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
+	 * @param[in] wind_vel Wind velocity vector [m/s]
+	 */
+	void navigateLoiter(const matrix::Vector2d &loiter_center, const matrix::Vector2d &vehicle_pos,
+			    float radius, int8_t loiter_direction, const matrix::Vector2f &ground_vel,
+			    const matrix::Vector2f &wind_vel);
+
+	/*
+	 * Navigate on a fixed heading.
+	 *
+	 * This only holds a certain (air mass relative) direction and does not perform
+	 * cross track correction. Helpful for semi-autonomous modes. Introduced
+	 * by in ECL_L1_Pos_Controller, augmented for use with NPFG here.
+	 *
+	 * @param[in] heading_ref Reference heading angle [rad]
+	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
+	 * @param[in] wind_vel Wind velocity vector [m/s]
+	 */
+	void navigateHeading(float heading_ref, const matrix::Vector2f &ground_vel,
+			     const matrix::Vector2f &wind_vel);
+
+	/*
+	 * Navigate on a fixed bearing.
+	 *
+	 * This only holds a certain (ground relative) direction and does not perform
+	 * cross track correction. Helpful for semi-autonomous modes. Similar to navigateHeading.
+	 *
+	 * @param[in] bearing Bearing angle [rad]
+	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
+	 * @param[in] wind_vel Wind velocity vector [m/s]
+	 */
+	void navigateBearing(float bearing, const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel);
+
+	/*
+	 * Keep the wings level.
+	 *
+	 * @param[in] heading Heading angle [rad]
+	 */
+	void navigateLevelFlight(const float heading);
+
+	/*
+	 * [Copied directly from ECL_L1_Pos_Controller]
+	 *
+	 * Set the maximum roll angle output in radians
+	 */
+	void setRollLimit(float roll_lim_rad) { roll_lim_rad_ = roll_lim_rad; }
+
+	/*
+	 * [Copied directly from ECL_L1_Pos_Controller]
+	 *
+	 * Set roll angle slew rate. Set to zero to deactivate.
+	 */
+	void setRollSlewRate(float roll_slew_rate) { roll_slew_rate_ = roll_slew_rate; }
+
+	/*
+	 * [Copied directly from ECL_L1_Pos_Controller]
+	 *
+	 * Set control loop dt. The value will be used to apply roll angle setpoint slew rate limiting.
+	 */
+	void setDt(const float dt) { dt_ = dt; }
+
+	/*
+	 * [Copied directly from ECL_L1_Pos_Controller]
+	 *
+	 * Get the switch distance
+	 *
+	 * This is the distance at which the system will switch to the next waypoint.
+	 * This depends on the period and damping
+	 *
+	 * @param[in] wp_radius The switching radius the waypoint has set.
+	 */
+	float switchDistance(float wp_radius) const;
+
+	/*
+	 * The path bearing
+	 *
+	 * @return bearing angle (-pi..pi, in NED frame) [rad]
+	 */
+	float targetBearing() const { return atan2f(unit_path_tangent_(1), unit_path_tangent_(0)); }
+
+	/*
+	 * [Copied directly from ECL_L1_Pos_Controller]
+	 *
+	 * Returns true if the loiter waypoint has been reached
+	 */
+	bool reachedLoiterTarget() { return circleMode(); }
+
+	/*
+	 * Returns true if following a circle (loiter)
+	 */
+	bool circleMode() { return path_type_loiter_ && track_proximity_ > NPFG_EPSILON; }
+
+	/*
+	 * [Copied directly from ECL_L1_Pos_Controller]
+	 *
+	 * Get roll angle setpoint for fixed wing.
+	 *
+	 * @return Roll angle (in NED frame)
+	 */
+	float getRollSetpoint() { return roll_setpoint_; }
+
+private:
+
+	static constexpr float NPFG_EPSILON = 1.0e-6; // small number *bigger than machine epsilon
+	static constexpr float MIN_RADIUS = 0.5f; // minimum effective radius (avoid singularities) [m]
+	static constexpr float NTE_FRACTION = 0.5f; // normalized track error fraction (must be > 0)
+	// ^determines at what fraction of the normalized track error the maximum track keeping forward ground speed demand is reached
+	static constexpr float AIRSPEED_BUFFER = 1.5f; // airspeed buffer [m/s] (must be > 0)
+	// ^the size of the feasibility transition region at cross wind angle >= 90 deg.
+	//  This must be non-zero to avoid jumpy airspeed incrementation while using wind
+	//  excess handling logic. Similarly used as buffer region around zero airspeed.
+
+	/*
+	 * tuning
+	 */
+
+	float period_{10.0f}; // nominal (desired) period -- user defined [s]
+	float damping_{0.7071f}; // nominal (desired) damping ratio -- user defined
+	float p_gain_{0.8885f}; // proportional gain (computed from period_ and damping_) [rad/s]
+	float time_const_{7.071f}; // time constant (computed from period_ and damping_) [s]
+	float adapted_period_{10.0f}; // auto-adapted period (if stability bounds enabled) [s]
+
+	/*
+	 * user defined guidance settings
+	 */
+
+	// guidance options
+	bool en_period_lb_{true}; // enables automatic lower bound constraints on controller period
+	bool en_period_ub_{true}; // enables automatic upper bound constraints on controller period (remains disabled if lower bound is disabled)
+	bool en_min_ground_speed_{true}; // the airspeed reference is incremented to sustain a user defined minimum forward ground speed
+	bool en_track_keeping_{false}; // the airspeed reference is incremented to return to the track and sustain zero ground velocity until excess wind subsides
+	bool en_wind_excess_regulation_{true}; // the airspeed reference is incremented to regulate the excess wind, but not overcome it ...
+	// ^disabling this parameter disables all other excess wind handling options, using only the nominal airspeed for reference
+
+	// guidance settings
+	float airspeed_nom_{15.0f}; // nominal (desired) airspeed reference (generally equivalent to cruise optimized airspeed) [m/s]
+	float airspeed_max_{20.0f}; // maximum airspeed reference - the maximum achievable/allowed airspeed reference [m/s]
+	float roll_time_const_{0.0f}; // autopilot roll response time constant [s]
+	float min_gsp_desired_{0.0f}; // user defined miminum desired forward ground speed [m/s]
+	float min_gsp_track_keeping_max_{5.0f}; // maximum, minimum forward ground speed demand from track keeping logic [m/s]
+
+	// guidance parameters
+	float switch_distance_multiplier_{0.32f}; // a value multiplied by the track error boundary resulting in a lower switch distance
+	// ^as the bearing angle changes quadratically (instead of linearly as in L1), the time constant (automatically calculated for on track stability) proportional track error boundary typically over estimates the required switching distance
+	float period_safety_factor_{1.5f}; // multiplied by the minimum period for conservative lower bound
+
+	/*
+	 * internal guidance states
+	 */
+
+	// speeds
+	float min_gsp_track_keeping_{0.0f}; // minimum forward ground speed demand from track keeping logic [m/s]
+	float min_ground_speed_ref_{0.0f}; // resultant minimum forward ground speed reference considering all active guidance logic [m/s]
+
+	//bearing feasibility
+	float feas_{1.0f}; // continous representation of bearing feasibility in [0,1] (0=infeasible, 1=feasible)
+	float feas_on_track_{1.0f}; // continuous bearing feasibility "on track"
+
+	// track proximity
+	float track_error_bound_{212.13f}; // the current ground speed dependent track error bound [m]
+	float track_proximity_{0.0f}; // value in [0,1] indicating proximity to track, 0 = at track error boundary or beyond, 1 = on track
+
+	// path following states
+	matrix::Vector2f unit_path_tangent_{matrix::Vector2f{1.0f, 0.0f}}; // unit path tangent vector
+	float signed_track_error_{0.0f}; // signed track error [m]
+	matrix::Vector2f bearing_vec_{matrix::Vector2f{1.0f, 0.0f}}; // bearing unit vector
+
+	/*
+	 * guidance outputs
+	 */
+
+	float airspeed_ref_{15.0f}; // airspeed reference [m/s]
+	matrix::Vector2f air_vel_ref_{matrix::Vector2f{15.0f, 0.0f}}; // air velocity reference vector [m/s]
+	float lateral_accel_{0.0f}; // lateral acceleration reference [m/s^2]
+	float lateral_accel_ff_{0.0f}; // lateral acceleration demand to maintain path curvature [m/s^2]
+
+	/*
+	 * ECL_L1_Pos_Controller functionality
+	 */
+
+	float dt_{0}; // control loop time [s]
+	float roll_lim_rad_{math::radians(30.0f)}; // maximum roll angle [rad]
+	float roll_setpoint_{0.0f}; // current roll angle setpoint [rad]
+	float roll_slew_rate_{0.0f}; // roll angle setpoint slew rate limit [rad/s]
+	bool path_type_loiter_{false}; // true if the guidance law is tracking a loiter circle
+
+	/*
+	 * Computes the lateral acceleration and airspeed references necessary to track
+	 * a path in wind (including excess wind conditions).
+	 *
+	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
+	 * @param[in] wind_vel Wind velocity vector [m/s]
+	 * @param[in] unit_path_tangent Unit vector tangent to path at closest point
+	 *            in direction of path
+	 * @param[in] signed_track_error Signed error to track at closest point (sign
+	 *            determined by path normal direction) [m]
+	 * @param[in] path_curvature Path curvature at closest point on track [m^-1]
+	 */
+	void guideToPath(const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel,
+			 const matrix::Vector2f &unit_path_tangent, const float signed_track_error,
+			 const float path_curvature);
+
+	/*
+	 * Computes the lateral acceleration and airspeed references necessary to track
+	 * a point in wind (including excess wind conditions).
+	 *
+	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
+	 * @param[in] wind_vel Wind velocity vector [m/s]
+	 * @param[in] bearing_vec Unit vector from vehicle to the target point
+	 * @param[in] track_error Distance from vehicle to the target point [m]
+	 */
+	void guideToPoint(const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel,
+			  const matrix::Vector2f &bearing_vec, const float track_error);
+
+	/*
+	 * Adapts the controller period considering user defined inputs, current flight
+	 * condition, path properties, and stability bounds.
+	 *
+	 * @param[in] ground_speed Vehicle ground speed [m/s]
+	 * @param[in] airspeed Vehicle airspeed [m/s]
+	 * @param[in] wind_speed Wind speed [m/s]
+	 * @param[in] track_error Track error (magnitude) [m]
+	 * @param[in] path_curvature Path curvature at closest point on track [m^-1]
+	 * @param[in] wind_vel Wind velocity vector in inertial frame [m/s]
+	 * @param[in] unit_path_tangent Unit vector tangent to path at closest point
+	 *            in direction of path
+	 * @param[in] feas_on_track Bearing feasibility on track at the closest point
+	 * @return Adapted period [s]
+	 */
+	float adaptPeriod(const float ground_speed, const float airspeed, const float wind_speed,
+			  const float track_error, const float path_curvature, const matrix::Vector2f &wind_vel,
+			  const matrix::Vector2f &unit_path_tangent, const float feas_on_track) const;
+
+	/*
+	 * Returns normalized (unitless) and constrained track error [0,1].
+	 *
+	 * @param[in] track_error Track error (magnitude) [m]
+	 * @param[in] track_error_bound Track error boundary [m]
+	 * @return Normalized track error
+	 */
+	float normalizedTrackError(const float track_error, const float track_error_bound) const;
+
+	/*
+	 * Cacluates an approximation of the wind factor (see [TODO: include citation]).
+	 *
+	 * @param[in] airspeed Vehicle airspeed [m/s]
+	 * @param[in] wind_speed Wind speed [m/s]
+	 * @return Non-dimensional wind factor approximation
+	 */
+	float windFactor(const float airspeed, const float wind_speed) const;
+
+	/*
+	 * Calculates a theoretical upper bound on the user defined period to maintain
+	 * track keeping stability.
+	 *
+	 * @param[in] air_turn_rate The turn rate required to track the current path
+	 *            curvature at the current airspeed, in a no-wind condition [rad/s]
+	 * @param[in] wind_factor Non-dimensional wind factor (see [TODO: include citation])
+	 * @return Period upper bound [s]
+	 */
+	float periodUpperBound(const float air_turn_rate, const float wind_factor, const float feas_on_track) const;
+
+	/*
+	 * Calculates a theoretical lower bound on the user defined period to avoid
+	 * limit cycle oscillations considering an acceleration actuation delay (e.g.
+	 * roll response delay). Note this lower bound defines *marginal stability,
+	 * and a safety factor should be applied in addition to the returned value.
+	 *
+	 * @param[in] air_turn_rate The turn rate required to track the current path
+	 *            curvature at the current airspeed, in a no-wind condition [rad/s]
+	 * @param[in] wind_factor Non-dimensional wind factor (see [TODO: include citation])
+	 * @return Period lower bound [s]
+	 */
+	float periodLowerBound(const float air_turn_rate, const float wind_factor, const float feas_on_track) const;
+
+	/*
+	 * Computes a continous non-dimensional track proximity [0,1] - 0 when the
+	 * vehicle is at the track error boundary, and 1 when on track.
+	 *
+	 * @param[in] look_ahead_ang The angle at which the bearing vector deviates
+	 *            from the unit track error vector [rad]
+	 * @return Track proximity
+	 */
+	float trackProximity(const float look_ahead_ang) const;
+
+	/*
+	 * Calculates a ground speed modulated track error bound under which the
+	 * look ahead angle is quadratically transitioned from alignment with the
+	 * track error vector to that of the path tangent vector.
+	 *
+	 * @param[in] ground_speed Vehicle ground speed [m/s]
+	 * @param[in] time_const Controller time constant [s]
+	 * @return Track error boundary [m]
+	 */
+	float trackErrorBound(const float ground_speed, const float time_const) const;
+
+	/*
+	 * Calculates the required controller proportional gain to achieve the desired
+	 * system period and damping ratio. NOTE: actual period and damping will vary
+	 * when following paths with curvature in wind.
+	 *
+	 * @param[in] period Desired system period [s]
+	 * @param[in] damping Desired system damping ratio
+	 * @return Proportional gain [rad/s]
+	 */
+	float pGain(const float period, const float damping) const;
+
+	/*
+	 * Calculates the required controller time constant to achieve the desired
+	 * system period and damping ratio. NOTE: actual period and damping will vary
+	 * when following paths with curvature in wind.
+	 *
+	 * @param[in] period Desired system period [s]
+	 * @param[in] damping Desired system damping ratio
+	 * @return Time constant [s]
+	 */
+	float timeConst(const float period, const float damping) const;
+
+	/*
+	 * Cacluates the look ahead angle as a quadratic function of the normalized
+	 * track error.
+	 *
+	 * @param[in] normalized_track_error Normalized track error (track error / track error boundary)
+	 * @return Look ahead angle [rad]
+	 */
+	float lookAheadAngle(const float normalized_track_error) const;
+
+	/*
+	 * Calculates the bearing vector and track proximity transitioning variable
+	 * from the look-ahead angle mapping.
+	 *
+	 * @param[in] unit_path_tangent Unit vector tangent to path at closest point
+	 *            in direction of path
+	 * @param[in] look_ahead_ang The bearing vector lies at this angle from
+	 *            the path normal vector [rad]
+	 * @param[in] signed_track_error Signed error to track at closest point (sign
+	 *            determined by path normal direction) [m]
+	 * @return Unit bearing vector
+	 */
+	matrix::Vector2f bearingVec(const matrix::Vector2f &unit_path_tangent, const float look_ahead_ang,
+				    const float signed_track_error) const;
+
+	/*
+	 * Calculates the minimum forward ground speed demand for minimum forward
+	 * ground speed maintanence as well as track keeping logic.
+	 *
+	 * @param[in] normalized_track_error Normalized track error (track error / track error boundary)
+	 * @param[in] feas Bearing feasibility
+	 * @return Minimum forward ground speed demand [m/s]
+	 */
+	float minGroundSpeed(const float normalized_track_error, const float feas);
+
+	/*
+	 * Determines a reference air velocity *without curvature compensation, but
+	 * including "optimal" airspeed reference compensation in excess wind conditions.
+	 * Nominal and maximum airspeed member variables must be set before using this method.
+	 *
+	 * @param[in] wind_vel Wind velocity vector [m/s]
+	 * @param[in] bearing_vec Bearing vector
+	 * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s]
+	 * @param[in] wind_dot_bearing 2D dot product of wind velocity and bearing vector [m/s]
+	 * @param[in] wind_speed Wind speed [m/s]
+	 * @param[in] min_ground_speed Minimum commanded forward ground speed [m/s]
+	 * @return Air velocity vector [m/s]
+	 */
+	matrix::Vector2f refAirVelocity(const matrix::Vector2f &wind_vel, const matrix::Vector2f &bearing_vec,
+					const float wind_cross_bearing, const float wind_dot_bearing, const float wind_speed,
+					const float min_ground_speed) const;
+
+	/*
+	 * Projection of the air velocity vector onto the bearing line considering
+	 * a connected wind triangle.
+	 *
+	 * @param[in] airspeed Vehicle airspeed [m/s]
+	 * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s]
+	 * @return Projection of air velocity vector on bearing vector [m/s]
+	 */
+	float projectAirspOnBearing(const float airspeed, const float wind_cross_bearing) const;
+
+	/*
+	 * Check for binary bearing feasibility.
+	 *
+	 * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s]
+	 * @param[in] wind_dot_bearing 2D dot product of wind velocity and bearing vector [m/s]
+	 * @param[in] airspeed Vehicle airspeed [m/s]
+	 * @param[in] wind_speed Wind speed [m/s]
+	 * @return Binary bearing feasibility: 1 if feasible, 0 if infeasible
+	 */
+	int bearingIsFeasible(const float wind_cross_bearing, const float wind_dot_bearing, const float airspeed,
+			      const float wind_speed) const;
+
+	/*
+	 * Air velocity solution for a given wind velocity and bearing vector assuming
+	 * a "high speed" (not backwards) solution in the excess wind case.
+	 *
+	 * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s]
+	 * @param[in] airsp_dot_bearing 2D dot product of air velocity (solution) and bearing vector [m/s]
+	 * @param[in] bearing_vec Bearing vector
+	 * @return Air velocity vector [m/s]
+	 */
+	matrix::Vector2f solveWindTriangle(const float wind_cross_bearing, const float airsp_dot_bearing,
+					   const matrix::Vector2f &bearing_vec) const;
+
+
+	/*
+	 * Air velocity solution for an infeasible bearing.
+	 *
+	 * @param[in] wind_vel Wind velocity vector [m/s]
+	 * @param[in] bearing_vec Bearing vector
+	 * @param[in] wind_speed Wind speed [m/s]
+	 * @param[in] airspeed Vehicle airspeed [m/s]
+	 * @return Air velocity vector [m/s]
+	 */
+	matrix::Vector2f infeasibleAirVelRef(const matrix::Vector2f &wind_vel, const matrix::Vector2f &bearing_vec,
+					     const float wind_speed, const float airspeed) const;
+
+
+	/*
+	 * Cacluates a continuous representation of the bearing feasibility from [0,1].
+	 * 0 = infeasible, 1 = fully feasible, partial feasibility in between.
+	 *
+	 * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s]
+	 * @param[in] wind_dot_bearing 2D dot product of wind velocity and bearing vector [m/s]
+	 * @param[in] airspeed Vehicle airspeed [m/s]
+	 * @param[in] wind_speed Wind speed [m/s]
+	 * @return bearing feasibility
+	 */
+	float bearingFeasibility(float wind_cross_bearing, const float wind_dot_bearing, const float airspeed,
+				 const float wind_speed) const;
+
+	/*
+	 * Calculates an additional feed-forward lateral acceleration demand considering
+	 * the path curvature.
+	 *
+	 * @param[in] unit_path_tangent Unit vector tangent to path at closest point
+	 *            in direction of path
+	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
+	 * @param[in] wind_vel Wind velocity vector [m/s]
+	 * @param[in] airspeed Vehicle airspeed [m/s]
+	 * @param[in] wind_speed Wind speed [m/s]
+	 * @param[in] signed_track_error Signed error to track at closest point (sign
+	 *             determined by path normal direction) [m]
+	 * @param[in] path_curvature Path curvature at closest point on track [m^-1]
+	 * @return Feed-forward lateral acceleration command [m/s^2]
+	 */
+	float lateralAccelFF(const matrix::Vector2f &unit_path_tangent, const matrix::Vector2f &ground_vel,
+			     const float wind_dot_upt, const float wind_cross_upt, const float airspeed,
+			     const float wind_speed, const float signed_track_error, const float path_curvature) const;
+
+	/*
+	 * Calculates a lateral acceleration demand from the heading error.
+	 * (does not consider path curvature)
+	 *
+	 * @param[in] air_vel Vechile air velocity vector [m/s]
+	 * @param[in] air_vel_ref Reference air velocity vector [m/s]
+	 * @param[in] airspeed Vehicle airspeed [m/s]
+	 * @return Lateral acceleration demand [m/s^2]
+	 */
+	float lateralAccel(const matrix::Vector2f &air_vel, const matrix::Vector2f &air_vel_ref,
+			   const float airspeed) const;
+
+	/*
+	 * Calculates two-dimensional "cross product" of two vectors.
+	 * TODO: move to matrix lib (Vector2 operation)
+	 *
+	 * @param[in] vec_1 Vector 1
+	 * @param[in] vec_2 Vector 2
+	 * @return 2D cross product
+	 */
+	float cross2D(const matrix::Vector2f &vec_1, const matrix::Vector2f &vec_2) const { return vec_1(0) * vec_2(1) - vec_1(1) * vec_2(0); }
+
+	/*******************************************************************************
+	 * PX4 POSITION SETPOINT INTERFACE FUNCTIONS
+	 */
+
+	/**
+	 * [Copied directly from ECL_L1_Pos_Controller]
+	 *
+	 * Convert a 2D vector from WGS84 to planar coordinates.
+	 *
+	 * This converts from latitude and longitude to planar
+	 * coordinates with (0,0) being at the position of ref and
+	 * returns a vector in meters towards wp.
+	 *
+	 * @param ref The reference position in WGS84 coordinates
+	 * @param wp The point to convert to into the local coordinates, in WGS84 coordinates
+	 * @return The vector in meters pointing from the reference position to the coordinates
+	 */
+	matrix::Vector2f getLocalPlanarVector(const matrix::Vector2d &origin, const matrix::Vector2d &target) const;
+
+	/**
+	 * [Copied directly from ECL_L1_Pos_Controller]
+	 *
+	 * Update roll angle setpoint. This will also apply slew rate limits if set.
+	 */
+	void updateRollSetpoint();
+
+}; // class NPFG
+
+#endif // NPFG_H_

src/modules/fw_pos_control_l1/CMakeLists.txt

@@ -44,6 +44,7 @@ px4_add_module(
 		l1
 		launchdetection
 		landing_slope
+    npfg
 		runway_takeoff
 		SlewRate
 		tecs

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -110,6 +110,22 @@ FixedwingPositionControl::parameters_update()
 	_l1_control.set_l1_roll_limit(radians(_param_fw_r_lim.get()));
 	_l1_control.set_roll_slew_rate(radians(_param_fw_l1_r_slew_max.get()));
 
+	// NPFG parameters
+	_npfg.setPeriod(_param_npfg_period.get());
+	_npfg.setDamping(_param_npfg_damping.get());
+	_npfg.enablePeriodLB(_param_npfg_en_period_lb.get());
+	_npfg.enablePeriodUB(_param_npfg_en_period_ub.get());
+	_npfg.enableMinGroundSpeed(_param_npfg_en_min_gsp.get());
+	_npfg.enableTrackKeeping(_param_npfg_en_track_keeping.get());
+	_npfg.enableWindExcessRegulation(_param_npfg_en_wind_reg.get());
+	_npfg.setMinGroundSpeed(_param_fw_gnd_spd_min.get());
+	_npfg.setMaxTrackKeepingMinGroundSpeed(_param_npfg_track_keeping_gsp_max.get());
+	_npfg.setRollTimeConst(_param_npfg_roll_time_const.get());
+	_npfg.setSwitchDistanceMultiplier(_param_npfg_switch_distance_multiplier.get());
+	_npfg.setRollLimit(radians(_param_fw_r_lim.get()));
+	_npfg.setRollSlewRate(radians(_param_fw_l1_r_slew_max.get()));
+	_npfg.setPeriodSafetyFactor(_param_npfg_period_safety_factor.get());
+
 	// TECS parameters
 	_tecs.set_max_climb_rate(_param_fw_t_clmb_max.get());
 	_tecs.set_max_sink_rate(_param_fw_t_sink_max.get());
@@ -279,6 +295,28 @@ FixedwingPositionControl::airspeed_poll()
 	}
 }
 
+void
+FixedwingPositionControl::wind_poll()
+{
+	if (_wind_sub.updated()) {
+		wind_s wind;
+		_wind_sub.update(&wind);
+
+		// assumes wind is valid if finite
+		_wind_valid = PX4_ISFINITE(wind.windspeed_north)
+			      && PX4_ISFINITE(wind.windspeed_east);
+
+		_time_wind_last_received = hrt_absolute_time();
+
+		_wind_vel(0) = wind.windspeed_north;
+		_wind_vel(1) = wind.windspeed_east;
+
+	} else {
+		// invalidate wind estimate usage (and correspondingly NPFG, if enabled) after subscription timeout
+		_wind_valid = _wind_valid && (hrt_absolute_time() - _time_wind_last_received) < T_WIND_EST_TIMEOUT;
+	}
+}
+
 void
 FixedwingPositionControl::manual_control_setpoint_poll()
 {
@@ -369,7 +407,9 @@ FixedwingPositionControl::get_auto_airspeed_setpoint(const hrt_abstime &now, con
 	}
 
 	// Adapt cruise airspeed when otherwise the min groundspeed couldn't be maintained
-	if (!_l1_control.circle_mode()) {
+	const bool not_using_wind_est = !_param_fw_use_npfg.get() || !_wind_valid;
+
+	if (!_l1_control.circle_mode() && not_using_wind_est) {
 		/*
 		 * This error value ensures that a plane (as long as its throttle capability is
 		 * not exceeded) travels towards a waypoint (and is not pushed more and more away
@@ -417,7 +457,6 @@ FixedwingPositionControl::get_auto_airspeed_setpoint(const hrt_abstime &now, con
 	return airspeed_setpoint;
 }
 
-
 void
 FixedwingPositionControl::tecs_status_publish()
 {
@@ -504,11 +543,42 @@ FixedwingPositionControl::status_publish()
 
 	}
 
+	if (_param_fw_use_npfg.get() && _wind_valid) {
+		npfg_status_s npfg_status = {};
+
+		npfg_status.wind_est_valid = _wind_valid;
+
+		const float bearing = _npfg.getBearing(); // dont repeat atan2 calc
+
+		pos_ctrl_status.nav_bearing = bearing;
+		pos_ctrl_status.target_bearing = _npfg.targetBearing();
+		pos_ctrl_status.xtrack_error = _npfg.getTrackError();
+		pos_ctrl_status.acceptance_radius = _npfg.switchDistance(500.0f);
+
+		npfg_status.lat_accel = _npfg.getLateralAccel();
+		npfg_status.lat_accel_ff = _npfg.getLateralAccelFF();
+		npfg_status.heading_ref = _npfg.getHeadingRef();
+		npfg_status.bearing = bearing;
+		npfg_status.bearing_feas = _npfg.getBearingFeas();
+		npfg_status.bearing_feas_on_track = _npfg.getOnTrackBearingFeas();
+		npfg_status.signed_track_error = _npfg.getTrackError();
+		npfg_status.track_error_bound = _npfg.getTrackErrorBound();
+		npfg_status.airspeed_ref = _npfg.getAirspeedRef();
+		npfg_status.min_ground_speed_ref = _npfg.getMinGroundSpeedRef();
+		npfg_status.adapted_period = _npfg.getAdaptedPeriod();
+		npfg_status.p_gain = _npfg.getPGain();
+		npfg_status.time_const = _npfg.getTimeConst();
+		npfg_status.timestamp = hrt_absolute_time();
+
+		_npfg_status_pub.publish(npfg_status);
+
+	} else {
+		pos_ctrl_status.acceptance_radius = _l1_control.switch_distance(500.0f);
+	}
+
 	pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(_current_latitude, _current_longitude,
 				  _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon);
 
-	pos_ctrl_status.acceptance_radius = _l1_control.switch_distance(500.0f);
-
 	pos_ctrl_status.yaw_acceptance = NAN;
 
 	pos_ctrl_status.timestamp = hrt_absolute_time();
@@ -724,7 +794,12 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
 	const float dt = math::constrain((now - _control_position_last_called) * 1e-6f, 0.01f, 0.05f);
 	_control_position_last_called = now;
 
-	_l1_control.set_dt(dt);
+	if (_param_fw_use_npfg.get() && _wind_valid) {
+		_npfg.setDt(dt);
+
+	} else {
+		_l1_control.set_dt(dt);
+	}
 
 	_att_sp.fw_control_yaw = false;		// by default we don't want yaw to be contoller directly with rudder
 	_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;		// by default we don't use flaps
@@ -751,7 +826,7 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
 	_hdg_hold_yaw = _yaw;
 
 	/* get circle mode */
-	const bool was_circle_mode = _l1_control.circle_mode();
+	const bool was_circle_mode = (_param_fw_use_npfg.get() && _wind_valid) ? _npfg.circleMode() : _l1_control.circle_mode();
 
 	/* restore TECS parameters, in case changed intermittently (e.g. in landing handling) */
 	_tecs.set_speed_weight(_param_fw_t_spdweight.get());
@@ -962,7 +1037,8 @@ FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, cons
 	/* current waypoint (the one currently heading for) */
 	curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon);
 
-	const float acc_rad = _l1_control.switch_distance(500.0f);
+	const float acc_rad = (_param_fw_use_npfg.get()
+			       && _wind_valid) ? _npfg.switchDistance(500.0f) : _l1_control.switch_distance(500.0f);
 
 	uint8_t position_sp_type = setpoint_type;
 
@@ -1003,7 +1079,8 @@ FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, cons
 		} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
 			// LOITER: use SETPOINT_TYPE_POSITION to get to SETPOINT_TYPE_LOITER
 			if ((dist >= 0.f)
-			    && (dist_xy > 2.f * math::max(acc_rad, loiter_radius_abs))) {
+			    && (dist_xy > 2.f * math::max(acc_rad, loiter_radius_abs))
+			    && !(_param_fw_use_npfg.get() && _wind_valid)) {
 				// SETPOINT_TYPE_LOITER -> SETPOINT_TYPE_POSITION
 				position_sp_type = position_setpoint_s::SETPOINT_TYPE_POSITION;
 			}
@@ -1022,7 +1099,8 @@ void
 FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
 		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
 {
-	const float acc_rad = _l1_control.switch_distance(500.0f);
+	const float acc_rad = (_param_fw_use_npfg.get()
+			       && _wind_valid) ? _npfg.switchDistance(500.0f) : _l1_control.switch_distance(500.0f);
 	Vector2d curr_wp{0, 0};
 	Vector2d prev_wp{0, 0};
 
@@ -1103,12 +1181,24 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const fl
 		}
 	}
 
-	_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, get_nav_speed_2d(ground_speed));
-	_att_sp.roll_body = _l1_control.get_roll_setpoint();
+	float target_airspeed = get_auto_airspeed_setpoint(now, pos_sp_curr.cruising_speed, ground_speed, dt);
+
+	if (_param_fw_use_npfg.get() && _wind_valid) {
+		_npfg.setAirspeedNom(target_airspeed * _eas2tas);
+		_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+		_npfg.navigateWaypoints(prev_wp, curr_wp, curr_pos, ground_speed, _wind_vel);
+		_att_sp.roll_body = _npfg.getRollSetpoint();
+		target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
+
+	} else {
+		_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, get_nav_speed_2d(ground_speed));
+		_att_sp.roll_body = _l1_control.get_roll_setpoint();
+	}
+
 	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
 
 	tecs_update_pitch_throttle(now, position_sp_alt,
-				   get_auto_airspeed_setpoint(now, pos_sp_curr.cruising_speed, ground_speed, dt),
+				   target_airspeed,
 				   radians(_param_fw_p_lim_min.get()),
 				   radians(_param_fw_p_lim_max.get()),
 				   tecs_fw_thr_min,
@@ -1153,13 +1243,25 @@ FixedwingPositionControl::control_auto_velocity(const hrt_abstime &now, const fl
 	//Offboard velocity control
 	Vector2f target_velocity{pos_sp_curr.vx, pos_sp_curr.vy};
 	_target_bearing = wrap_pi(atan2f(target_velocity(1), target_velocity(0)));
-	_l1_control.navigate_heading(_target_bearing, _yaw, ground_speed);
 
-	_att_sp.roll_body = _l1_control.get_roll_setpoint();
+	float target_airspeed = get_auto_airspeed_setpoint(now, pos_sp_curr.cruising_speed, ground_speed, dt);
+
+	if (_param_fw_use_npfg.get() && _wind_valid) {
+		_npfg.setAirspeedNom(target_airspeed * _eas2tas);
+		_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+		_npfg.navigateBearing(_target_bearing, ground_speed, _wind_vel);
+		_att_sp.roll_body = _npfg.getRollSetpoint();
+		target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
+
+	} else {
+		_l1_control.navigate_heading(_target_bearing, _yaw, ground_speed);
+		_att_sp.roll_body = _l1_control.get_roll_setpoint();
+	}
+
 	_att_sp.yaw_body = _yaw;
 
 	tecs_update_pitch_throttle(now, position_sp_alt,
-				   get_auto_airspeed_setpoint(now, pos_sp_curr.cruising_speed, ground_speed, dt),
+				   target_airspeed,
 				   radians(_param_fw_p_lim_min.get()),
 				   radians(_param_fw_p_lim_max.get()),
 				   tecs_fw_thr_min,
@@ -1236,15 +1338,10 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const floa
 		loiter_direction = (loiter_radius > 0) ? 1 : -1;
 	}
 
-	_l1_control.navigate_loiter(curr_wp, curr_pos, loiter_radius, loiter_direction, get_nav_speed_2d(ground_speed));
-
-	_att_sp.roll_body = _l1_control.get_roll_setpoint();
-	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
-
-	float alt_sp = pos_sp_curr.alt;
+	const bool in_circle_mode = (_param_fw_use_npfg.get() && _wind_valid) ? _npfg.circleMode() : _l1_control.circle_mode();
 
 	if (pos_sp_next.type == position_setpoint_s::SETPOINT_TYPE_LAND && pos_sp_next.valid
-	    && _l1_control.circle_mode() && _param_fw_lnd_earlycfg.get()) {
+	    && in_circle_mode && _param_fw_lnd_earlycfg.get()) {
 		// We're in a loiter directly before a landing WP. Enable our landing configuration (flaps,
 		// landing airspeed and potentially tighter altitude control) already such that we don't
 		// have to do this switch (which can cause significant altitude errors) close to the ground.
@@ -1253,6 +1350,24 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const floa
 		_att_sp.apply_flaps = true;
 	}
 
+	float target_airspeed = get_auto_airspeed_setpoint(now, airspeed_sp, ground_speed, dt);
+
+	if (_param_fw_use_npfg.get() && _wind_valid) {
+		_npfg.setAirspeedNom(target_airspeed * _eas2tas);
+		_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+		_npfg.navigateLoiter(curr_wp, curr_pos, loiter_radius, loiter_direction, ground_speed, _wind_vel);
+		_att_sp.roll_body = _npfg.getRollSetpoint();
+		target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
+
+	} else {
+		_l1_control.navigate_loiter(curr_wp, curr_pos, loiter_radius, loiter_direction, get_nav_speed_2d(ground_speed));
+		_att_sp.roll_body = _l1_control.get_roll_setpoint();
+	}
+
+	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
+
+	float alt_sp = pos_sp_curr.alt;
+
 	if (in_takeoff_situation()) {
 		alt_sp = max(alt_sp, _takeoff_ground_alt + _param_fw_clmbout_diff.get());
 		_att_sp.roll_body = constrain(_att_sp.roll_body, radians(-5.0f), radians(5.0f));
@@ -1272,7 +1387,7 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const floa
 	}
 
 	tecs_update_pitch_throttle(now, alt_sp,
-				   get_auto_airspeed_setpoint(now, airspeed_sp, ground_speed, dt),
+				   target_airspeed,
 				   radians(_param_fw_p_lim_min.get()),
 				   radians(_param_fw_p_lim_max.get()),
 				   tecs_fw_thr_min,
@@ -1332,18 +1447,34 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 		_runway_takeoff.update(now, _airspeed, _current_altitude - terrain_alt,
 				       _current_latitude, _current_longitude, &_mavlink_log_pub);
 
+		float target_airspeed = get_auto_airspeed_setpoint(now,
+					_runway_takeoff.getMinAirspeedScaling() * _param_fw_airspd_min.get(), ground_speed,
+					dt);
+
 		/*
 		 * Update navigation: _runway_takeoff returns the start WP according to mode and phase.
 		 * If we use the navigator heading or not is decided later.
 		 */
-		_l1_control.navigate_waypoints(_runway_takeoff.getStartWP(), curr_wp, curr_pos, ground_speed);
+		if (_param_fw_use_npfg.get() && _wind_valid) {
+			_npfg.setAirspeedNom(target_airspeed * _eas2tas);
+			_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+			_npfg.navigateWaypoints(_runway_takeoff.getStartWP(), curr_wp, curr_pos, ground_speed, _wind_vel);
+			_att_sp.roll_body = _runway_takeoff.getRoll(_npfg.getRollSetpoint());
+			target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
+
+		} else {
+			_l1_control.navigate_waypoints(_runway_takeoff.getStartWP(), curr_wp, curr_pos, ground_speed);
+			_att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.get_roll_setpoint());
+		}
+
+		_att_sp.yaw_body = _runway_takeoff.getYaw(_yaw);
+
 
 		// update tecs
 		const float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_param_fw_p_lim_max.get());
 
 		tecs_update_pitch_throttle(now, pos_sp_curr.alt,
-					   get_auto_airspeed_setpoint(now, _runway_takeoff.getMinAirspeedScaling() * _param_fw_airspd_min.get(), ground_speed,
-							   dt),
+					   target_airspeed,
 					   radians(_param_fw_p_lim_min.get()),
 					   radians(takeoff_pitch_max_deg),
 					   _param_fw_thr_min.get(),
@@ -1353,8 +1484,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 					   radians(_runway_takeoff.getMinPitch(_takeoff_pitch_min.get(), _param_fw_p_lim_min.get())));
 
 		// assign values
-		_att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.get_roll_setpoint());
-		_att_sp.yaw_body = _runway_takeoff.getYaw(_yaw);
 		_att_sp.fw_control_yaw = _runway_takeoff.controlYaw();
 		_att_sp.pitch_body = _runway_takeoff.getPitch(get_tecs_pitch());
 
@@ -1393,8 +1522,21 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 		if (_launch_detection_state != LAUNCHDETECTION_RES_NONE) {
 			/* Launch has been detected, hence we have to control the plane. */
 
-			_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
-			_att_sp.roll_body = _l1_control.get_roll_setpoint();
+			float target_airspeed = get_auto_airspeed_setpoint(now, _param_fw_airspd_trim.get(), ground_speed,
+						dt);
+
+			if (_param_fw_use_npfg.get() && _wind_valid) {
+				_npfg.setAirspeedNom(target_airspeed * _eas2tas);
+				_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+				_npfg.navigateWaypoints(prev_wp, curr_wp, curr_pos, ground_speed, _wind_vel);
+				_att_sp.roll_body = _npfg.getRollSetpoint();
+				target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
+
+			} else {
+				_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
+				_att_sp.roll_body = _l1_control.get_roll_setpoint();
+			}
+
 			_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
 
 			/* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use
@@ -1414,7 +1556,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 			if (_param_fw_clmbout_diff.get() > 0.0f && altitude_error > _param_fw_clmbout_diff.get()) {
 				/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
 				tecs_update_pitch_throttle(now, pos_sp_curr.alt,
-							   _param_fw_airspd_trim.get(),
+							   target_airspeed,
 							   radians(_param_fw_p_lim_min.get()),
 							   radians(takeoff_pitch_max_deg),
 							   _param_fw_thr_min.get(),
@@ -1428,7 +1570,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 
 			} else {
 				tecs_update_pitch_throttle(now, pos_sp_curr.alt,
-							   get_auto_airspeed_setpoint(now, _param_fw_airspd_trim.get(), ground_speed, dt),
+							   target_airspeed,
 							   radians(_param_fw_p_lim_min.get()),
 							   radians(_param_fw_p_lim_max.get()),
 							   _param_fw_thr_min.get(),
@@ -1541,23 +1683,6 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
 		events::send(events::ID("fixedwing_position_control_landing"), events::Log::Info, "Landing, heading hold");
 	}
 
-	if (_land_noreturn_horizontal) {
-		// heading hold
-		_l1_control.navigate_heading(_target_bearing, _yaw, ground_speed);
-
-	} else {
-		// normal navigation
-		_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
-	}
-
-	_att_sp.roll_body = _l1_control.get_roll_setpoint();
-	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
-
-	if (_land_noreturn_horizontal) {
-		/* limit roll motion to prevent wings from touching the ground first */
-		_att_sp.roll_body = constrain(_att_sp.roll_body, radians(-10.0f), radians(10.0f));
-	}
-
 	/* Vertical landing control */
 	/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
 
@@ -1614,12 +1739,6 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
 		/* kill the throttle if param requests it */
 		float throttle_max = _param_fw_thr_max.get();
 
-		/* enable direct yaw control using rudder/wheel */
-		if (_land_noreturn_horizontal) {
-			_att_sp.yaw_body = _target_bearing;
-			_att_sp.fw_control_yaw = true;
-		}
-
 		if (((_current_altitude < terrain_alt + _landingslope.motor_lim_relative_alt()) &&
 		     (wp_distance_save < _landingslope.flare_length() + 5.0f)) || // Only kill throttle when close to WP
 		    _land_motor_lim) {
@@ -1643,10 +1762,56 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
 		}
 
 		const float airspeed_land = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
+		float target_airspeed = get_auto_airspeed_setpoint(now, airspeed_land, ground_speed, dt);
+
 		const float throttle_land = _param_fw_thr_min.get() + (_param_fw_thr_max.get() - _param_fw_thr_min.get()) * 0.1f;
 
+		/* lateral guidance */
+		if (_param_fw_use_npfg.get() && _wind_valid) {
+			_npfg.setAirspeedNom(target_airspeed * _eas2tas);
+			_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+
+			if (_land_noreturn_horizontal) {
+				// heading hold
+				_npfg.navigateHeading(_target_bearing, ground_speed, _wind_vel);
+
+			} else {
+				// normal navigation
+				_npfg.navigateWaypoints(prev_wp, curr_wp, curr_pos, ground_speed, _wind_vel);
+			}
+
+			target_airspeed = _npfg.getAirspeedRef() /  _eas2tas;
+			_att_sp.roll_body = _npfg.getRollSetpoint();
+
+		} else {
+			if (_land_noreturn_horizontal) {
+				// heading hold
+				_l1_control.navigate_heading(_target_bearing, _yaw, ground_speed);
+
+			} else {
+				// normal navigation
+				_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
+			}
+
+			_att_sp.roll_body = _l1_control.get_roll_setpoint();
+		}
+
+		if (_land_noreturn_horizontal) {
+			/* limit roll motion to prevent wings from touching the ground first */
+			_att_sp.roll_body = constrain(_att_sp.roll_body, radians(-10.0f), radians(10.0f));
+		}
+
+		/* enable direct yaw control using rudder/wheel */
+		if (_land_noreturn_horizontal) {
+			_att_sp.yaw_body = _target_bearing;
+			_att_sp.fw_control_yaw = true;
+
+		} else {
+			_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
+		}
+
 		tecs_update_pitch_throttle(now, terrain_alt + flare_curve_alt_rel,
-					   get_auto_airspeed_setpoint(now, airspeed_land, ground_speed, dt),
+					   target_airspeed,
 					   radians(_param_fw_lnd_fl_pmin.get()),
 					   radians(_param_fw_lnd_fl_pmax.get()),
 					   0.0f,
@@ -1714,9 +1879,42 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
 		}
 
 		const float airspeed_approach = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
+		float target_airspeed = get_auto_airspeed_setpoint(now, airspeed_approach, ground_speed, dt);
+
+		/* lateral guidance */
+		if (_param_fw_use_npfg.get() && _wind_valid) {
+			_npfg.setAirspeedNom(target_airspeed * _eas2tas);
+			_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+
+			if (_land_noreturn_horizontal) {
+				// heading hold
+				_npfg.navigateHeading(_target_bearing, ground_speed, _wind_vel);
+
+			} else {
+				// normal navigation
+				_npfg.navigateWaypoints(prev_wp, curr_wp, curr_pos, ground_speed, _wind_vel);
+			}
+
+			target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
+			_att_sp.roll_body = _npfg.getRollSetpoint();
+
+		} else {
+			if (_land_noreturn_horizontal) {
+				// heading hold
+				_l1_control.navigate_heading(_target_bearing, _yaw, ground_speed);
+
+			} else {
+				// normal navigation
+				_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
+			}
+
+			_att_sp.roll_body = _l1_control.get_roll_setpoint();
+		}
+
+		_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
 
 		tecs_update_pitch_throttle(now, altitude_desired,
-					   get_auto_airspeed_setpoint(now, airspeed_approach, ground_speed, dt),
+					   target_airspeed,
 					   radians(_param_fw_p_lim_min.get()),
 					   radians(_param_fw_p_lim_max.get()),
 					   _param_fw_thr_min.get(),
@@ -1821,17 +2019,7 @@ FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const
 		throttle_max = 0.0f;
 	}
 
-	tecs_update_pitch_throttle(now, altitude_sp_amsl,
-				   get_manual_airspeed_setpoint(),
-				   radians(_param_fw_p_lim_min.get()),
-				   radians(_param_fw_p_lim_max.get()),
-				   _param_fw_thr_min.get(),
-				   throttle_max,
-				   _param_fw_thr_cruise.get(),
-				   false,
-				   pitch_limit_min,
-				   false,
-				   height_rate_sp);
+	float target_airspeed = get_manual_airspeed_setpoint();
 
 	/* heading control */
 	if (fabsf(_manual_control_setpoint.y) < HDG_HOLD_MAN_INPUT_THRESH &&
@@ -1872,10 +2060,19 @@ FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const
 			Vector2d prev_wp{_hdg_hold_prev_wp.lat, _hdg_hold_prev_wp.lon};
 			Vector2d curr_wp{_hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon};
 
-			/* populate l1 control setpoint */
-			_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
+			if (_param_fw_use_npfg.get() && _wind_valid) {
+				_npfg.setAirspeedNom(target_airspeed * _eas2tas);
+				_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+				_npfg.navigateWaypoints(prev_wp, curr_wp, curr_pos, ground_speed, _wind_vel);
+				_att_sp.roll_body = _npfg.getRollSetpoint();
+				target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
+
+			} else {
+				/* populate l1 control setpoint */
+				_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
+				_att_sp.roll_body = _l1_control.get_roll_setpoint();
+			}
 
-			_att_sp.roll_body = _l1_control.get_roll_setpoint();
 			_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
 
 			if (in_takeoff_situation()) {
@@ -1885,6 +2082,18 @@ FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const
 		}
 	}
 
+	tecs_update_pitch_throttle(now, altitude_sp_amsl,
+				   target_airspeed,
+				   radians(_param_fw_p_lim_min.get()),
+				   radians(_param_fw_p_lim_max.get()),
+				   _param_fw_thr_min.get(),
+				   throttle_max,
+				   _param_fw_thr_cruise.get(),
+				   false,
+				   pitch_limit_min,
+				   false,
+				   height_rate_sp);
+
 	if (!_yaw_lock_engaged || fabsf(_manual_control_setpoint.y) >= HDG_HOLD_MAN_INPUT_THRESH ||
 	    fabsf(_manual_control_setpoint.r) >= HDG_HOLD_MAN_INPUT_THRESH) {
 
@@ -2058,6 +2267,7 @@ FixedwingPositionControl::Run()
 		vehicle_attitude_poll();
 		vehicle_command_poll();
 		vehicle_control_mode_poll();
+		wind_poll();
 
 		if (_vehicle_land_detected_sub.updated()) {
 			vehicle_land_detected_s vehicle_land_detected;

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -56,6 +56,7 @@
 #include <drivers/drv_hrt.h>
 #include <lib/geo/geo.h>
 #include <lib/l1/ECL_L1_Pos_Controller.hpp>
+#include <lib/npfg/npfg.hpp>
 #include <lib/tecs/TECS.hpp>
 #include <lib/landing_slope/Landingslope.hpp>
 #include <lib/mathlib/mathlib.h>
@@ -73,6 +74,7 @@
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/airspeed_validated.h>
 #include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/npfg_status.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/position_controller_landing_status.h>
 #include <uORB/topics/position_controller_status.h>
@@ -89,6 +91,7 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/wind.h>
 #include <uORB/topics/orbit_status.h>
 #include <uORB/uORB.h>
 #include <vtol_att_control/vtol_type.h>
@@ -114,6 +117,8 @@ static constexpr hrt_abstime T_ALT_TIMEOUT = 1_s; // time after which we abort l
 static constexpr float THROTTLE_THRESH =
 	0.05f;	///< max throttle from user which will not lead to motors spinning up in altitude controlled modes
 static constexpr float ASPD_SP_SLEW_RATE = 1.f; // slew rate limit for airspeed setpoint changes [m/s/S]
+static constexpr hrt_abstime T_WIND_EST_TIMEOUT =
+	10_s; // time after which the wind estimate is disabled if no longer updating
 
 class FixedwingPositionControl final : public ModuleBase<FixedwingPositionControl>, public ModuleParams,
 	public px4::WorkItem
@@ -143,6 +148,7 @@ private:
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
 	uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
+	uORB::Subscription _wind_sub{ORB_ID(wind)};
 	uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)};
 	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
@@ -157,6 +163,7 @@ private:
 
 	uORB::Publication<vehicle_attitude_setpoint_s>		_attitude_sp_pub;
 	uORB::Publication<vehicle_local_position_setpoint_s> 	_local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)};	///< vehicle local position setpoint publication
+	uORB::Publication<npfg_status_s> _npfg_status_pub{ORB_ID(npfg_status)}; ///< NPFG status publication
 	uORB::Publication<position_controller_status_s>		_pos_ctrl_status_pub{ORB_ID(position_controller_status)};			///< navigation capabilities publication
 	uORB::Publication<position_controller_landing_status_s>	_pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)};	///< landing status publication
 	uORB::Publication<tecs_status_s>			_tecs_status_pub{ORB_ID(tecs_status)};						///< TECS status publication
@@ -230,6 +237,11 @@ private:
 	float _airspeed{0.0f};
 	float _eas2tas{1.0f};
 
+	/* wind estimates */
+	Vector2f _wind_vel{0.0f, 0.0f}; ///< wind velocity vector [m/s]
+	bool _wind_valid{false}; ///< flag if a valid wind estimate exists
+	hrt_abstime _time_wind_last_received{0}; ///< last time wind estimate was received in microseconds. Used to detect timeouts.
+
 	float _pitch{0.0f};
 	float _yaw{0.0f};
 	float _yawrate{0.0f};
@@ -258,6 +270,7 @@ private:
 	hrt_abstime _time_in_fixed_bank_loiter{0};
 
 	ECL_L1_Pos_Controller	_l1_control;
+	NPFG _npfg;
 	TECS			_tecs;
 
 	uint8_t _position_sp_type{0};
@@ -284,6 +297,7 @@ private:
 	void		vehicle_command_poll();
 	void		vehicle_control_mode_poll();
 	void		vehicle_status_poll();
+	void        wind_poll();
 
 	void		status_publish();
 	void		landing_status_publish();
@@ -390,6 +404,19 @@ private:
 		(ParamFloat<px4::params::FW_L1_R_SLEW_MAX>) _param_fw_l1_r_slew_max,
 		(ParamFloat<px4::params::FW_R_LIM>) _param_fw_r_lim,
 
+		(ParamBool<px4::params::FW_USE_NPFG>) _param_fw_use_npfg,
+		(ParamFloat<px4::params::NPFG_PERIOD>) _param_npfg_period,
+		(ParamFloat<px4::params::NPFG_DAMPING>) _param_npfg_damping,
+		(ParamBool<px4::params::NPFG_LB_PERIOD>) _param_npfg_en_period_lb,
+		(ParamBool<px4::params::NPFG_UB_PERIOD>) _param_npfg_en_period_ub,
+		(ParamBool<px4::params::NPFG_TRACK_KEEP>) _param_npfg_en_track_keeping,
+		(ParamBool<px4::params::NPFG_EN_MIN_GSP>) _param_npfg_en_min_gsp,
+		(ParamBool<px4::params::NPFG_WIND_REG>) _param_npfg_en_wind_reg,
+		(ParamFloat<px4::params::NPFG_GSP_MAX_TK>) _param_npfg_track_keeping_gsp_max,
+		(ParamFloat<px4::params::NPFG_ROLL_TC>) _param_npfg_roll_time_const,
+		(ParamFloat<px4::params::NPFG_SW_DST_MLT>) _param_npfg_switch_distance_multiplier,
+		(ParamFloat<px4::params::NPFG_PERIOD_SF>) _param_npfg_period_safety_factor,
+
 		(ParamFloat<px4::params::FW_LND_AIRSPD_SC>) _param_fw_lnd_airspd_sc,
 		(ParamFloat<px4::params::FW_LND_ANG>) _param_fw_lnd_ang,
 		(ParamFloat<px4::params::FW_LND_FL_PMAX>) _param_fw_lnd_fl_pmax,

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -85,6 +85,150 @@ PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f);
  */
 PARAM_DEFINE_FLOAT(FW_L1_R_SLEW_MAX, 90.0f);
 
+/**
+ * Use NPFG as lateral-directional guidance law for fixed-wing vehicles
+ *
+ * Replaces L1.
+ *
+ * @boolean
+ * @group FW NPFG Control
+ */
+PARAM_DEFINE_INT32(FW_USE_NPFG, 0);
+
+/**
+ * NPFG period
+ *
+ * Period of the NPFG control law.
+ *
+ * @unit s
+ * @min 1.0
+ * @max 100.0
+ * @decimal 1
+ * @increment 0.1
+ * @group FW NPFG Control
+ */
+PARAM_DEFINE_FLOAT(NPFG_PERIOD, 10.0f);
+
+/**
+ * NPFG damping ratio
+ *
+ * Damping ratio of the NPFG control law.
+ *
+ * @min 0.10
+ * @max 1.00
+ * @decimal 2
+ * @increment 0.01
+ * @group FW NPFG Control
+ */
+PARAM_DEFINE_FLOAT(NPFG_DAMPING, 0.7f);
+
+/**
+ * Enable automatic lower bound on the NPFG period
+ *
+ * Avoids limit cycling from a too aggressively tuned period/damping combination.
+ * If set to false, also disables the upper bound NPFG_PERIOD_UB.
+ *
+ * @boolean
+ * @group FW NPFG Control
+ */
+PARAM_DEFINE_INT32(NPFG_LB_PERIOD, 1);
+
+/**
+ * Enable automatic upper bound on the NPFG period
+ *
+ * Adapts period to maintain track keeping in variable winds and path curvature.
+ *
+ * @boolean
+ * @group FW NPFG Control
+ */
+PARAM_DEFINE_INT32(NPFG_UB_PERIOD, 1);
+
+/**
+ * Enable track keeping excess wind handling logic.
+ *
+ * @boolean
+ * @group FW NPFG Control
+ */
+PARAM_DEFINE_INT32(NPFG_TRACK_KEEP, 1);
+
+/**
+ * Enable minimum forward ground speed maintaining excess wind handling logic
+ *
+ * @boolean
+ * @group FW NPFG Control
+ */
+PARAM_DEFINE_INT32(NPFG_EN_MIN_GSP, 1);
+
+/**
+ * Enable wind excess regulation.
+ *
+ * Disabling this parameter further disables all other airspeed incrementation options.
+ *
+ * @boolean
+ * @group FW NPFG Control
+ */
+PARAM_DEFINE_INT32(NPFG_WIND_REG, 1);
+
+/**
+ * Maximum, minimum forward ground speed for track keeping in excess wind
+ *
+ * The maximum value of the minimum forward ground speed that may be commanded
+ * by the track keeping excess wind handling logic. Commanded in full at the normalized
+ * track error fraction of the track error boundary and reduced to zero on track.
+ *
+ * @unit m/s
+ * @min 0.0
+ * @max 10.0
+ * @decimal 1
+ * @increment 0.5
+ * @group FW NPFG Control
+ */
+PARAM_DEFINE_FLOAT(NPFG_GSP_MAX_TK, 5.0f);
+
+/**
+ * Roll time constant
+ *
+ * Time constant of roll controller command / response, modeled as first order delay.
+ * Used to determine lower period bound. Setting zero disables automatic period bounding.
+ *
+ * @unit s
+ * @min 0.00
+ * @max 2.00
+ * @decimal 2
+ * @increment 0.05
+ * @group FW NPFG Control
+ */
+PARAM_DEFINE_FLOAT(NPFG_ROLL_TC, 0.5f);
+
+/**
+ * NPFG switch distance multiplier
+ *
+ * Multiplied by the track error boundary to determine when the aircraft switches
+ * to the next waypoint and/or path segment. Should be less than 1. 1/pi (0.32)
+ * sets the switch distance equivalent to that of the L1 controller.
+ *
+ * @min 0.1
+ * @max 1.0
+ * @decimal 2
+ * @increment 0.01
+ * @group FW NPFG Control
+ */
+PARAM_DEFINE_FLOAT(NPFG_SW_DST_MLT, 0.32f);
+
+/**
+ * Period safety factor
+ *
+ * Multiplied by period for conservative minimum period bounding (when period lower
+ * bounding is enabled). 1.0 bounds at marginal stability.
+ *
+ * @min 1.0
+ * @max 10.0
+ * @decimal 1
+ * @increment 0.1
+ * @group FW NPFG Control
+ */
+PARAM_DEFINE_FLOAT(NPFG_PERIOD_SF, 1.5f);
+
 /**
  * Cruise throttle
  *

src/modules/logger/logged_topics.cpp

@@ -75,6 +75,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("manual_control_switches");
 	add_topic("mission_result");
 	add_topic("navigator_mission_item");
+	add_topic("npfg_status", 100);
 	add_topic("offboard_control_mode", 100);
 	add_topic("onboard_computer_status", 10);
 	add_topic("parameter_update");