fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

npfg: create const adaptPeriod method and update control params outside

Browse Source
This commit is contained in:
Thomas Stastny 2021-08-24 22:03:12 +02:00 committed by JaeyoungLim
parent 0b705c6c6d
commit e9b0a3eb66
2 changed files with 16 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
src/lib/npfg/npfg.cpp
View File

@ -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
{

14
src/lib/npfg/npfg.hpp
View File

@ -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].