multiply track error boundary by scale factor in calculation of waypoint switch distance

Author: Thomas Stastny

src/lib/npfg/npfg.cpp

@@ -605,7 +605,7 @@ void NPFG::navigateLevelFlight(const float heading)
 
 float NPFG::switchDistance(float wp_radius) const
 {
-	return math::min(wp_radius, track_error_bound_);
+	return math::min(wp_radius, track_error_bound_ * switch_distance_multiplier_);
 } // switchDistance
 
 Vector2f NPFG::getLocalPlanarVector(const Vector2d &origin, const Vector2d &target) const

src/lib/npfg/npfg.hpp

@@ -135,6 +135,11 @@ class NPFG
 	 */
 	void setAirspeedBuffer(float buf) { airspeed_buffer_ = math::max(buf, 0.1f); }
 
+	/*
+	 * Set the switch distance multiplier.
+	 */
+	void setSwitchDistanceMultiplier(float mult) { switch_distance_multiplier_ = math::max(mult, 0.1f); }
+
 	/*
 	 * @return Controller proportional gain [rad/s]
 	 */
@@ -369,6 +374,8 @@ class NPFG
 	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.318f}; // 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 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 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

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -119,6 +119,7 @@ FixedwingPositionControl::parameters_update()
 	_npfg.setNormalizedTrackErrorFraction(_param_npfg_nte_fraction.get());
 	_npfg.setRollTimeConst(_param_npfg_roll_time_const.get());
 	_npfg.setAirspeedBuffer(_param_npfg_airspeed_buffer.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()));
 

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -388,6 +388,7 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 		(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_ASPD_BUF>) _param_npfg_airspeed_buffer,
+		(ParamFloat<px4::params::NPFG_SW_DST_MLT>) _param_npfg_switch_distance_multiplier,
 
 		(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

@@ -237,6 +237,21 @@ PARAM_DEFINE_FLOAT(NPFG_ROLL_TC, 0.5f);
  */
 PARAM_DEFINE_FLOAT(NPFG_ASPD_BUF, 1.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);
+
 /**
  * Cruise throttle
  *