npfg: create const adaptPeriod method and update control params outside

Author: Thomas Stastny

src/lib/npfg/npfg.cpp

@@ -78,8 +78,10 @@ void NPFG::evaluate(const Vector2f &ground_vel, const Vector2f &wind_vel,
 
 	// 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_);
+	adapted_period_ = adaptPeriod(ground_speed, airspeed, wind_ratio, 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_);
@@ -120,9 +122,9 @@ void NPFG::evaluate(const Vector2f &ground_vel, const Vector2f &wind_vel,
 	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 NPFG::adaptPeriod(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) const
 {
 	float period = period_;
 	const float air_turn_rate = fabsf(path_curvature * airspeed);
@@ -164,11 +166,8 @@ void NPFG::updateControlParams(const float ground_speed, const float airspeed, c
 		}
 	}
 
-	// update the control parameters / output the adapted period
-	adapted_period_ = period;
-	p_gain_ = pGain(period, damping_);
-	time_const_ = timeConst(period, damping_);
-} // updateControlParams
+	return period;
+} // adaptPeriod
 
 float NPFG::normalizedTrackError(const float track_error, const float track_error_bound) const
 {

src/lib/npfg/npfg.hpp

@@ -409,9 +409,8 @@ class NPFG
 		      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.
+	 * 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]
@@ -421,11 +420,12 @@ class NPFG
 	 * @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]
 	 */
-	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);
+	float adaptPeriod(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) const;
 
 	/*
 	 * Returns normalized (unitless) and constrained track error [0,1].