new bearing feasibility for handling zero airspeed

- simpler bearing feasibility function
- get rid of wind ratio
- replace wind ratio buffer with airspeed buffer

Author: Thomas Stastny

src/lib/npfg/npfg.cpp

@@ -56,45 +56,33 @@ void NPFG::evaluate(const Vector2f &ground_vel, const Vector2f &wind_vel, Vector
 	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;
-		feas_on_track_ = 0.0f;
-		return;
-	}
-
 	const float wind_speed = wind_vel.norm();
-	const float wind_ratio = wind_speed / airspeed;
 
 	float track_error = fabsf(signed_track_error);
 
+	// track error bound is dynamic depending on ground speed
+	track_error_bound_ = trackErrorBound(ground_speed, time_const_);
+	float normalized_track_error = normalizedTrackError(track_error, track_error_bound_);
+
+	// look ahead angle based purely on track proximity
+	float look_ahead_ang = lookAheadAngle(normalized_track_error);
+
+	bearing_vec_ = bearingVec(unit_path_tangent, look_ahead_ang, signed_track_error);
+
 	// on-track wind triangle projections
 	float wind_cross_upt = cross2D(wind_vel, unit_path_tangent);
 	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);
+	feas_on_track_ = bearingFeasibility(wind_cross_upt, wind_dot_upt, airspeed, wind_speed);
 
 	// 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_ratio, track_error,
+	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_);
-	float normalized_track_error = normalizedTrackError(track_error, track_error_bound_);
-
-	// look ahead angle based purely on track proximity
-	float look_ahead_ang = lookAheadAngle(normalized_track_error);
-
-	bearing_vec_ = bearingVec(unit_path_tangent, look_ahead_ang, signed_track_error);
-
 	// specific waypoint logic complications... handles case where we are following
 	// waypoints and are in front of the first of the segment.
 	// TODO: find a way to get this out of the main eval method!
@@ -118,9 +106,9 @@ void NPFG::evaluate(const Vector2f &ground_vel, const Vector2f &wind_vel, Vector
 
 		wind_cross_upt = cross2D(wind_vel, unit_path_tangent);
 		wind_dot_upt = wind_vel.dot(unit_path_tangent);
-		feas_on_track_ = bearingFeasibility(wind_cross_upt, wind_dot_upt, wind_speed, wind_ratio);
+		feas_on_track_ = bearingFeasibility(wind_cross_upt, wind_dot_upt, airspeed, wind_speed);
 
-		adapted_period_ = adaptPeriod(ground_speed, airspeed, wind_ratio, track_error,
+		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_);
@@ -133,7 +121,7 @@ void NPFG::evaluate(const Vector2f &ground_vel, const Vector2f &wind_vel, Vector
 	const 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);
+	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_;
@@ -152,7 +140,7 @@ void NPFG::evaluate(const Vector2f &ground_vel, const Vector2f &wind_vel, Vector
 
 	// 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);
+					   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
@@ -161,13 +149,13 @@ void NPFG::evaluate(const Vector2f &ground_vel, const Vector2f &wind_vel, Vector
 	lateral_accel_ = lateral_accel + feas_combined * track_proximity_ * lateral_accel_ff_;
 } // evaluate
 
-float NPFG::adaptPeriod(const float ground_speed, const float airspeed, const float wind_ratio,
+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(wind_ratio);
+	const float wind_factor = windFactor(airspeed, wind_speed);
 
 	if (en_period_lb_) {
 		// lower bound the period for stability w.r.t. roll time constant and current flight condition
@@ -213,10 +201,15 @@ float NPFG::normalizedTrackError(const float track_error, const float track_erro
 	return math::constrain(track_error / track_error_bound, 0.0f, 1.0f);
 }
 
-float NPFG::windFactor(const float wind_ratio) const
+float NPFG::windFactor(const float airspeed, const float wind_speed) 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)));
+	if (wind_speed > airspeed || airspeed < EPSILON) {
+		return 2.0f;
+
+	} else {
+		return 2.0f * (1.0f - sqrtf(1.0f - math::min(1.0f, wind_speed / airspeed)));
+	}
 } // windFactor
 
 float NPFG::periodUB(const float air_turn_rate, const float wind_factor, const float feas_on_track) const
@@ -419,58 +412,23 @@ Vector2f NPFG::infeasibleAirVelRef(const Vector2f &wind_vel, const Vector2f &bea
 	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 NPFG::bearingFeasibility(float wind_cross_bearing, const float wind_dot_bearing, const float airspeed,
+			       const float wind_speed) 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;
+	if (wind_dot_bearing < 0.0f) {
+		wind_cross_bearing = wind_speed;
 
 	} 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;
+		wind_cross_bearing = fabsf(wind_cross_bearing);
 	}
 
-	return feas;
+	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 wind_ratio, const float signed_track_error,
-			   const float path_curvature) const
+			   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

src/lib/npfg/npfg.hpp

@@ -118,22 +118,22 @@ class NPFG
 	/*
 	 * Set the nominal airspeed reference [m/s].
 	 */
-	void setAirspeedNom(float airsp) { airspeed_nom_ = math::constrain(airsp, MIN_AIRSPEED, airspeed_max_); }
+	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, airspeed_nom_); }
+	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_ = math::max(tc, 0.1f); }
 
 	/*
-	 * Set the wind ratio buffer size.
+	 * Set the airspeed buffer size.
 	 */
-	void setWindRatioBuffer(float buf) { wind_ratio_buffer_ = math::constrain(buf, 0.01f, 0.2f); }
+	void setAirspeedBuffer(float buf) { airspeed_buffer_ = math::max(buf, 0.1f); }
 
 	/*
 	 * @return Controller proportional gain [rad/s]
@@ -335,22 +335,14 @@ class NPFG
 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
+	static constexpr float PERIOD_SAFETY_FACTOR = 4.0f; // multiplier for period lower bound [s]
 
-	/* 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]
+	float period_{20.0f}; // nominal (desired) period -- user defined [s]
+	float damping_{0.7071f}; // nominal (desired) damping ratio -- user defined
+	float p_gain_{0.4442}; // proportional gain (computed from period_ and damping_) [rad/s]
+	float time_const_{14.142f}; // time constant (computed from period_ and damping_) [s]
+	float adapted_period_{20.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)
@@ -368,7 +360,7 @@ class NPFG
 	// ^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 airspeed_buffer_{1.5f}; // size of the region above the feasibility boundary (into feasible space) where a continuous transition from feasible to infeasible is imposed [m/s]
 
 	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
@@ -420,7 +412,7 @@ class NPFG
 	 *
 	 * @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] 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]
@@ -429,7 +421,7 @@ class NPFG
 	 * @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_ratio,
+	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;
 
@@ -445,10 +437,11 @@ class NPFG
 	/*
 	 * Cacluates an approximation of the wind factor (see [TODO: include citation]).
 	 *
-	 * @param[in] wind_ratio Wind speed to airspeed ratio
+	 * @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 wind_ratio) const;
+	float windFactor(const float airspeed, const float wind_speed) const;
 
 	/*
 	 * Calculates a theoretical upper bound on the user defined period to maintain
@@ -622,12 +615,12 @@ class NPFG
 	 *
 	 * @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]
-	 * @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;
+	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
@@ -639,16 +632,14 @@ class NPFG
 	 * @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]
 	 * @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;
+			     const float wind_speed, const float signed_track_error, const float path_curvature) const;
 
 	/*
 	 * Calculates a lateral acceleration demand from the heading error.

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -118,7 +118,7 @@ FixedwingPositionControl::parameters_update()
 	_npfg.setMaxTrackKeepingMinGroundSpeed(_param_npfg_track_keeping_gsp_max.get());
 	_npfg.setNormalizedTrackErrorFraction(_param_npfg_nte_fraction.get());
 	_npfg.setRollTimeConst(_param_npfg_roll_time_const.get());
-	_npfg.setWindRatioBuffer(_param_npfg_wind_ratio_buf.get());
+	_npfg.setAirspeedBuffer(_param_npfg_airspeed_buffer.get());
 	_npfg.setRollLimit(radians(_param_fw_r_lim.get()));
 	_npfg.setRollSlewRate(radians(_param_fw_l1_r_slew_max.get()));
 

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -387,7 +387,7 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 		(ParamFloat<px4::params::NPFG_GSP_MAX_TK>) _param_npfg_track_keeping_gsp_max,
 		(ParamFloat<px4::params::NPFG_NTE_FRAC>) _param_npfg_nte_fraction,
 		(ParamFloat<px4::params::NPFG_ROLL_TC>) _param_npfg_roll_time_const,
-		(ParamFloat<px4::params::NPFG_WR_BUF>) _param_npfg_wind_ratio_buf,
+		(ParamFloat<px4::params::NPFG_ASPD_BUF>) _param_npfg_airspeed_buffer,
 
 		(ParamFloat<px4::params::FW_LND_AIRSPD_SC>) _param_fw_lnd_airspd_sc,
 		(ParamFloat<px4::params::FW_LND_ANG>) _param_fw_lnd_ang,

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -222,19 +222,20 @@ PARAM_DEFINE_FLOAT(NPFG_NTE_FRAC, 0.5f);
 PARAM_DEFINE_FLOAT(NPFG_ROLL_TC, 0.5f);
 
 /**
- * NPFG wind ratio buffer
+ * NPFG airspeed buffer
  *
  * 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.
+ * excess handling logic. Similarly used as buffer region around zero airspeed.
  *
- * @min 0.01
- * @max 0.30
- * @decimal 2
- * @increment 0.01
+ * @unit m/s
+ * @min 0.1
+ * @max 5.0
+ * @decimal 1
+ * @increment 0.1
  * @group FW NPFG Control
  */
-PARAM_DEFINE_FLOAT(NPFG_WR_BUF, 0.1f);
+PARAM_DEFINE_FLOAT(NPFG_ASPD_BUF, 1.5f);
 
 /**
  * Cruise throttle