add npfg lib

src/lib/CMakeLists.txt

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

src/lib/npfg/CMakeLists.txt

@@ -0,0 +1,40 @@
+############################################################################
+#
+#   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
+)
+
+add_dependencies(npfg git_ecl)
+target_link_libraries(npfg PRIVATE ecl_geo)

src/lib/npfg/npfg.cpp

@@ -0,0 +1,632 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2021 Autonomous Systems Lab, ETH Zurich. 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.
+ *
+ * Authors and acknowledgements in header.
+ */
+
+#include "npfg.hpp"
+#include <lib/ecl/geo/geo.h>
+#include <px4_platform_common/defines.h>
+#include <float.h>
+
+using matrix::Vector2d;
+using matrix::Vector2f;
+
+void NPFG::evaluate(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();
+
+	Vector2f air_vel = ground_vel - wind_vel;
+	const float airspeed = air_vel.norm();
+
+	if (airspeed < MIN_AIRSPEED) {
+		// this case should only ever happen if we have not launched, the wind
+		// estimator has failed, or the aircraft is legitimately in a very sad
+		// situation
+		airspeed_ref_ = airspeed_nom_;
+		lateral_accel_ = 0.0f;
+		feas_ = 0.0f;
+		return;
+	}
+
+	const float wind_speed = wind_vel.norm();
+	const float wind_ratio = wind_speed / airspeed;
+
+	const float track_error = fabsf(signed_track_error);
+
+	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, wind_speed, wind_ratio);
+
+	// update control parameters considering upper and lower stability bounds (if enabled)
+	// must be called before trackErrorBound() as it updates time_const_
+	updateControlParams(ground_speed, airspeed, wind_ratio, track_error, path_curvature,
+			    wind_vel, unit_path_tangent, feas_on_track_);
+
+	// 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 purely on track proximity
+	const float look_ahead_ang = lookAheadAngle(normalized_track_error);
+
+	bearing_vec_ = bearingVec(unit_path_tangent, look_ahead_ang, signed_track_error);
+
+	float wind_cross_bearing = cross2D(wind_vel, bearing_vec_);
+	float wind_dot_bearing = wind_vel.dot(bearing_vec_);
+
+	// continuous representation of the bearing feasibility
+	feas_ = bearingFeasibility(wind_cross_bearing, wind_dot_bearing, wind_speed, wind_ratio);
+
+	// we consider feasibility of both the current bearing as well as that on the track at the current closest point
+	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();
+
+	track_proximity_ = trackProximity(look_ahead_ang);
+
+	// 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, wind_ratio,
+					   signed_track_error, path_curvature, track_proximity_, feas_combined);
+
+	// total lateral acceleration to drive aircaft towards track as well as account
+	// for path curvature
+	lateral_accel_ = lateralAccel(air_vel, air_vel_ref_, airspeed) + lateral_accel_ff_;
+} // evaluate
+
+void NPFG::updateControlParams(const float ground_speed, const float airspeed, const float wind_ratio,
+			       const float track_error, const float path_curvature, const Vector2f &wind_vel,
+			       const Vector2f &unit_path_tangent, const float feas_on_track)
+{
+	float period = period_;
+	const float air_turn_rate = fabsf(path_curvature * airspeed);
+	const float wind_factor = windFactor(wind_ratio);
+
+	if (en_period_lb_) {
+		// lower bound the period for stability w.r.t. roll time constant and current flight condition
+		const float period_lb = periodLB(air_turn_rate, wind_factor, feas_on_track);
+		period = math::max(period_lb * PERIOD_SAFETY_FACTOR, period);
+
+		// only allow upper bounding ONLY if lower bounding is enabled (is otherwise
+		// dangerous to allow period decrements without stability checks)
+		const float period_ub = periodUB(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_SAFETY_FACTOR, period_ub);
+
+			// recalculate time constant and track error bound for lower-bounded
+			// period for normalized track error calculation
+			const float time_const = timeConst(period, 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);
+
+			// transition from the nominal period to the adapted period as we get
+			// closer to the track
+			period = (ramp_in_adapted_period_) ? period_adapted * track_proximity + (1.0f - track_proximity) * period :
+				 period_adapted;
+		}
+	}
+
+	// update the control parameters / output the adapted period
+	adapted_period_ = period;
+	p_gain_ = pGain(period, damping_);
+	time_const_ = timeConst(period, damping_);
+} // updateControlParams
+
+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 wind_ratio) const
+{
+	// See [TODO: include citation] for definition/elaboration of this approximation.
+	return 2.0f * (1.0f - sqrtf(1.0f - math::min(1.0f, wind_ratio)));
+} // windFactor
+
+float NPFG::periodUB(const float air_turn_rate, const float wind_factor, const float feas_on_track) const
+{
+	if (air_turn_rate * wind_factor > 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::periodLB(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 < 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
+		min_gsp_track_keeping_ = (1.0f - feas) * min_gsp_track_keeping_max_ * math::constrain(
+						 normalized_track_error * inv_nte_fraction_, 0.0f,
+						 1.0f);
+	}
+
+	// minimum ground speed demand from minimum forward ground speed user setting
+	float min_gsp_cmd = 0.0f;
+
+	if (en_min_ground_speed_ && en_wind_excess_regulation_) {
+		min_gsp_cmd = min_gsp_cmd_;
+	}
+
+	return math::max(min_gsp_track_keeping_, min_gsp_cmd);
+} // 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(const float wind_cross_bearing, const float wind_dot_bearing, const float wind_speed,
+			       const float wind_ratio) const
+{
+	float sin_cross_wind_ang; // in [0, 1] (constant after 90 deg)
+
+	if (wind_dot_bearing <= 0.0f) {
+		sin_cross_wind_ang = 1.0f;
+
+	} else {
+		sin_cross_wind_ang = fabsf(wind_cross_bearing / wind_speed);
+	}
+
+	// upper and lower feasibility barriers
+	float wind_ratio_ub, wind_ratio_lb;
+
+	if (sin_cross_wind_ang < CROSS_WIND_ANG_CO) { // small angle approx.
+		// linear feasibility function (avoid singularity)
+
+		const float wind_ratio_ub_co = ONE_DIV_SIN_CROSS_WIND_ANG_CO;
+		wind_ratio_ub = wind_ratio_ub_co + CO_SLOPE * (CROSS_WIND_ANG_CO - sin_cross_wind_ang);
+
+		const float wind_ratio_lb_co = (ONE_DIV_SIN_CROSS_WIND_ANG_CO - 2.0f) * wind_ratio_buffer_ + 1.0f;
+		wind_ratio_lb = wind_ratio_lb_co + wind_ratio_buffer_ * CO_SLOPE * (CROSS_WIND_ANG_CO - sin_cross_wind_ang);
+
+	} else {
+		const float one_div_sin_cross_wind_ang = 1.0f / sin_cross_wind_ang;
+		wind_ratio_ub = one_div_sin_cross_wind_ang;
+		wind_ratio_lb = (one_div_sin_cross_wind_ang - 2.0f) * wind_ratio_buffer_ + 1.0f;
+	}
+
+	// calculate bearing feasibility
+	float feas = 1.0f; // feasible
+
+	if (wind_ratio > wind_ratio_ub) {
+		// infeasible
+		feas = 0.0f;
+
+	} else if (wind_ratio > wind_ratio_lb) {
+		// partially feasible
+		// smoothly transition from fully feasible to fully infeasible
+		feas = cosf(M_PI_F * 0.5f * math::constrain((wind_ratio - wind_ratio_lb) / (wind_ratio_ub - wind_ratio_lb), 0.0f,
+				1.0f));
+		feas *= feas;
+	}
+
+	return feas;
+} // 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 wind_ratio, const float signed_track_error,
+			   const float path_curvature, const float track_proximity, const float feas) 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. further the bearing feasibility is considered
+	// in excess wind conditions.
+
+	// 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,
+					   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), 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 * track_proximity * feas * 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 : 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 / 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
+
+	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() < EPSILON || vector_A_to_B.dot(vector_A_to_vehicle) < 0.0f) {
+		// the waypoints are either on top of each other and should be considered
+		// as single waypoint, or we are in front of waypoint A. in either case,
+		// fly directly to A.
+		unit_path_tangent_ = -vector_A_to_vehicle.normalized();
+		signed_track_error_ = 0.0f;
+
+	} 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);
+	}
+
+	evaluate(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;
+
+	evaluate(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 regular heading error - ignoring wind or inertial position
+	evaluate(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
+	evaluate(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_);
+} // 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,703 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2021 Autonomous Systems Lab, ETH Zurich. 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.
+ *
+ * Acknowledgements and References:
+ *
+ * TODO
+ *
+ */
+
+#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, EPSILON); }
+
+	/*
+	 * Set the nominal controller damping ratio.
+	 */
+	void setDamping(float damping) { damping_ = math::constrain(damping, 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; }
+
+	/*
+	 * Ramp in any automatic period adaptations with the track proximity.
+	 */
+	void rampInAdaptedPeriod(const bool en) { ramp_in_adapted_period_ = 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_cmd_ = 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 normalized track error fraction.
+	 */
+	void setNormalizedTrackErrorFraction(float nte_fraction) { inv_nte_fraction_ = 1.0f / math::max(nte_fraction, 0.1f); }
+
+	/*
+	 * Set the nominal airspeed reference [m/s].
+	 */
+	void setAirspeedNom(float airsp) { airspeed_nom_ = math::constrain(airsp, MIN_AIRSPEED, airspeed_max_); }
+
+	/*
+	 * Set the maximum airspeed reference [m/s].
+	 */
+	void setAirspeedMax(float airsp) { airspeed_max_ = math::max(airsp, airspeed_nom_); }
+
+	/*
+	 * Set the autopilot roll response time constant [s].
+	 */
+	void setRollTimeConst(float tc) { roll_time_const_ = math::max(tc, 0.1f); }
+
+	/*
+	 * Set the wind ratio buffer size.
+	 */
+	void setWindRatioBuffer(float buf) { wind_ratio_buffer_ = math::constrain(buf, 0.01f, 0.2f); }
+
+	/*
+	 * @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_ > 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 EPSILON = 1.0e-4;
+	static constexpr float MIN_AIRSPEED = 1.0f; // constrain airspeed to avoid singularities [m/s]
+	static constexpr float MIN_RADIUS = 0.5f; // minimum effective radius (avoid singularities) [m]
+	static constexpr float PERIOD_SAFETY_FACTOR = 4.0f; // multiplier for period lower bound
+
+	/* pre-computed constants for linear cut-off function for bearing feasibility calculation */
+	static constexpr float CROSS_WIND_ANG_CO =
+		0.02; // cross wind angle cut-off below which the feasibility barrier function is finite and linear [rad] (= approx. 1 deg)
+	static constexpr float ONE_DIV_SIN_CROSS_WIND_ANG_CO = 50.003333488895450; // 1/sin(CROSS_WIND_ANG_CO)
+	static constexpr float CO_SLOPE =
+		2499.833309998360; // cross wind angle cut-off slope = cos(CROSS_WIND_ANG_CO)/sin(CROSS_WIND_ANG_CO)^2
+
+	float period_{30.0f}; // nominal (desired) period -- user defined [s]
+	float damping_{0.25f}; // nominal (desired) damping ratio -- user defined
+	float p_gain_{0.12566f}; // proportional gain (computed from period_ and damping_) [rad/s]
+	float time_const_{9.0f}; // time constant (computed from period_ and damping_) [s]
+	float adapted_period_{30.0f}; // auto-adapted period (if stability bounds enabled) [s]
+
+	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 ramp_in_adapted_period_{true}; // linearly ramps in upper bounded period adaptations from the nominal user setting according to track proximity
+
+	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
+	float min_gsp_cmd_{0.0f}; // user defined miminum forward ground speed [m/s]
+	float min_gsp_track_keeping_{0.0f}; // minimum forward ground speed demand from track keeping logic [m/s]
+	float min_gsp_track_keeping_max_{5.0f}; // maximum, 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]
+	float inv_nte_fraction_{0.5f}; // inverse normalized track error fraction ...
+	// ^determines at what fraction of the normalized track error the maximum track keeping forward ground speed demand is reached
+	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"
+	float wind_ratio_buffer_{0.1f}; // a buffer region below unity wind ratio allowing continuous transition between feasible and infeasible conditions/commands
+
+	float track_error_bound_{135.0f}; // 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
+
+	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.5f}; // autopilot roll response time constant [s]
+
+	matrix::Vector2f bearing_vec_{matrix::Vector2f{1.0f, 0.0f}}; // bearing unit vector
+	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 circle_mode_{false}; // true if following circle
+	bool path_type_loiter_{false}; // true if the guidance law is tracking a loiter circle
+	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]
+
+	/*
+	 * 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 evaluate(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);
+
+	/*
+	 * Updates the proportional gain and time constant of the controller 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_ratio Wind speed to airspeed ratio
+	 * @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
+	 */
+	void updateControlParams(const float ground_speed, const float airspeed,
+				 const float wind_ratio, const float track_error, const float path_curvature,
+				 const matrix::Vector2f &wind_vel, const matrix::Vector2f &unit_path_tangent,
+				 const float feas_on_track);
+
+	/*
+	 * 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] wind_ratio Wind speed to airspeed ratio
+	 * @return Non-dimensional wind factor approximation
+	 */
+	float windFactor(const float wind_ratio) 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 periodUB(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 periodLB(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]
+	* ADSFASDFSAFDSF
+	 * @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 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[out] track_proximity Smoothing parameter based on vehicle proximity
+	 *             to track with values between 0 (at track error boundary) and 1 (on track)
+	 * @param[in] unit_track_error Unit vector in direction from vehicle to
+	 *            closest point on path
+	 * @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]
+	 * @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] wind_speed Wind speed [m/s]
+	 * @param[in] wind_ratio Wind speed to airspeed ratio
+	 * @return bearing feasibility
+	 */
+	float bearingFeasibility(const float wind_cross_bearing, const float wind_dot_bearing, const float wind_speed,
+				 const float wind_ratio) const;
+
+	/*
+	 * Calculates an additional feed-forward lateral acceleration demand considering
+	* the path curvature. The full effect of the acceleration increment is smoothly
+	 * ramped in as the vehicle approaches the track and is further smoothly
+	 * zeroed out as the bearing becomes infeasible.
+	 *
+	 * @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] wind_ratio Wind speed to airspeed ratio
+	 * @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]
+	 * @param[in] track_proximity Smoothing parameter based on vehicle proximity
+	 *             to track with values between 0 (at track error boundary) and 1 (on track)
+	 * @param[in] feas Bearing feasibility
+	 * @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 wind_ratio, const float signed_track_error,
+			     const float path_curvature, const float track_proximity, const float feas) 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_