diff --git a/src/lib/npfg/npfg.cpp b/src/lib/npfg/npfg.cpp index be27740bd8..11b2baf15f 100644 --- a/src/lib/npfg/npfg.cpp +++ b/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 { diff --git a/src/lib/npfg/npfg.hpp b/src/lib/npfg/npfg.hpp index 03c8bbde7f..ade25b9de4 100644 --- a/src/lib/npfg/npfg.hpp +++ b/src/lib/npfg/npfg.hpp @@ -409,9 +409,8 @@ private: 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 @@ private: * @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].