diff --git a/src/lib/npfg/npfg.cpp b/src/lib/npfg/npfg.cpp index c6394f03b9..49fbea8f56 100644 --- a/src/lib/npfg/npfg.cpp +++ b/src/lib/npfg/npfg.cpp @@ -56,36 +56,10 @@ 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); - // 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); - - // 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, - 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_); @@ -95,6 +69,20 @@ void NPFG::evaluate(const Vector2f &ground_vel, const Vector2f &wind_vel, Vector 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, 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_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_); + // 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); + wind_cross_bearing = fabsf(wind_cross_bearing); } - // 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; + 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 diff --git a/src/lib/npfg/npfg.hpp b/src/lib/npfg/npfg.hpp index 5fc118d6b4..21b61b5abe 100644 --- a/src/lib/npfg/npfg.hpp +++ b/src/lib/npfg/npfg.hpp @@ -118,12 +118,12 @@ public: /* * 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]. @@ -131,9 +131,9 @@ public: 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 @@ public: 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 @@ private: // ^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 @@ private: * * @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 @@ private: * @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 @@ private: /* * 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 @@ private: * * @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,7 +632,6 @@ private: * @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] @@ -647,8 +639,7 @@ private: */ 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. diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 87b6779ce7..4dfc7010b9 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/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())); diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index dccfbd2e0e..ebbba36bb5 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -387,7 +387,7 @@ private: (ParamFloat) _param_npfg_track_keeping_gsp_max, (ParamFloat) _param_npfg_nte_fraction, (ParamFloat) _param_npfg_roll_time_const, - (ParamFloat) _param_npfg_wind_ratio_buf, + (ParamFloat) _param_npfg_airspeed_buffer, (ParamFloat) _param_fw_lnd_airspd_sc, (ParamFloat) _param_fw_lnd_ang, diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 35a8ed4c77..d8a624e06a 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/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