fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 11:38:56 +08:00
Code Issues Packages Projects Releases Wiki Activity

Rearrange npfg use path input (#21071)

Browse Source 
* [npfg]: Remove the guideToPoint function and replace with guideToPath

* [npfg]: remove unused navigateXXX functions

* [npfg]: Move navigateXXX Function into FWPoscontrol

* [FixedwingPositionControl]: Set default flaps and spoilers in attitude setpoint topic, and only change if necessary.
This commit is contained in:
KonradRudin
2023-02-08 08:54:00 +01:00
committed by GitHub
parent 9ac27c9413
commit c5d041a2f7
4 changed files with 213 additions and 426 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/npfg/npfg.cpp
+9 -233
View File
@@ -47,8 +47,9 @@
using matrix::Vector2d;
using matrix::Vector2f;
void NPFG::guideToPath(const Vector2f &ground_vel, const Vector2f &wind_vel, const Vector2f &unit_path_tangent,
const float signed_track_error, const float path_curvature)
void NPFG::guideToPath(const matrix::Vector2f &curr_pos_local, const Vector2f &ground_vel, const Vector2f &wind_vel,
const Vector2f &unit_path_tangent,
const Vector2f &position_on_path, const float path_curvature)
{
const float ground_speed = ground_vel.norm();
@@ -57,6 +58,9 @@ void NPFG::guideToPath(const Vector2f &ground_vel, const Vector2f &wind_vel, con
const float wind_speed = wind_vel.norm();
const Vector2f path_pos_to_vehicle{curr_pos_local - position_on_path};
const float signed_track_error = unit_path_tangent.cross(path_pos_to_vehicle);
// on-track wind triangle projections
const float wind_cross_upt = wind_vel.cross(unit_path_tangent);
const float wind_dot_upt = wind_vel.dot(unit_path_tangent);
@@ -115,62 +119,10 @@ void NPFG::guideToPath(const Vector2f &ground_vel, const Vector2f &wind_vel, con
// ramped in as the vehicle approaches the track and is further smoothly
// zeroed out as the bearing becomes infeasible.
lateral_accel_ = lateral_accel + feas_combined * track_proximity_ * lateral_accel_ff_;
updateRollSetpoint();
} // guideToPath
void NPFG::guideToPoint(const Vector2f &ground_vel, const Vector2f &wind_vel, const Vector2f &bearing_vec,
const float track_error)
{
bearing_vec_ = bearing_vec; // for status output
const float ground_speed = ground_vel.norm();
const Vector2f air_vel = ground_vel - wind_vel;
const float airspeed = air_vel.norm();
const float wind_speed = wind_vel.norm();
// wind triangle projections
const float wind_cross_bearing = wind_vel.cross(bearing_vec);
const float wind_dot_bearing = wind_vel.dot(bearing_vec);
// continuous representation of the bearing feasibility
feas_ = bearingFeasibility(wind_cross_bearing, wind_dot_bearing, airspeed, wind_speed);
feas_on_track_ = feas_; // no distinction in point following - set only for recording
// update control parameters considering upper and lower stability bounds (if enabled)
// must be called before trackErrorBound() as it updates time_const_
// NOTE: track error input as 0 for the period adaptation as track proximity will
// only ramp in 1) curvature based lower bounding, of which there is none
// for a point, and 2) period upper bounds, which for zero curvature is
// infinite, and thus disregarded in this case.
adapted_period_ = adaptPeriod(ground_speed, airspeed, wind_speed, 0.0f, 0.0f,
wind_vel, bearing_vec, feas_);
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_);
const float normalized_track_error = normalizedTrackError(track_error, track_error_bound_);
// look ahead angle based solely on track proximity
const float look_ahead_ang = lookAheadAngle(normalized_track_error);
track_proximity_ = trackProximity(look_ahead_ang);
min_ground_speed_ref_ = minGroundSpeed(normalized_track_error, feas_);
// reference air velocity with directional feedforward effect for following
// curvature in wind and magnitude incrementation depending on minimum ground
// speed violations and/or high wind conditions in general
air_vel_ref_ = refAirVelocity(wind_vel, bearing_vec, wind_cross_bearing,
wind_dot_bearing, wind_speed, min_ground_speed_ref_);
airspeed_ref_ = air_vel_ref_.norm();
// lateral acceleration demand based on heading error
lateral_accel_ff_ = 0.0f;
lateral_accel_ = lateralAccel(air_vel, air_vel_ref_, airspeed);
} // guideToPoint
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
@@ -309,7 +261,7 @@ float NPFG::timeConst(const float period, const float damping) const
float NPFG::lookAheadAngle(const float normalized_track_error) const
{
return M_PI_F * 0.5f * (normalized_track_error - 1.0f) * (normalized_track_error - 1.0f);
return M_PI_2_F * (normalized_track_error - 1.0f) * (normalized_track_error - 1.0f);
} // lookAheadAngle
Vector2f NPFG::bearingVec(const Vector2f &unit_path_tangent, const float look_ahead_ang,
@@ -514,182 +466,6 @@ float NPFG::lateralAccel(const Vector2f &air_vel, const Vector2f &air_vel_ref, c
}
} // lateralAccel
/*******************************************************************************
* PX4 NAVIGATION INTERFACE FUNCTIONS (provide similar functionality to ECL_L1_Pos_Controller)
*/
void NPFG::navigateWaypoints(const Vector2f &waypoint_A, const Vector2f &waypoint_B,
const Vector2f &vehicle_pos, const Vector2f &ground_vel, const Vector2f &wind_vel)
{
// similar to logic found in ECL_L1_Pos_Controller method of same name
// BUT no arbitrary max approach angle, approach entirely determined by generated
// bearing vectors
path_type_loiter_ = false;
Vector2f vector_A_to_B = waypoint_B - waypoint_A;
Vector2f vector_A_to_vehicle = vehicle_pos - waypoint_A;
if (vector_A_to_B.norm() < NPFG_EPSILON) {
// the waypoints are on top of each other and should be considered as a
// single waypoint, fly directly to it
unit_path_tangent_ = -vector_A_to_vehicle.normalized();
signed_track_error_ = vector_A_to_vehicle.norm();
closest_point_on_path_ = waypoint_A;
guideToPoint(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_);
} else if (vector_A_to_B.dot(vector_A_to_vehicle) < 0.0f) {
// we are in front of waypoint A, fly directly to it until the bearing generated
// to the line segement between A and B is shallower than that from the
// bearing to the first waypoint (A).
// guidance to the line through A and B
unit_path_tangent_ = vector_A_to_B.normalized();
signed_track_error_ = unit_path_tangent_.cross(vector_A_to_vehicle);
closest_point_on_path_ = waypoint_A + vector_A_to_vehicle.dot(unit_path_tangent_) * unit_path_tangent_;
guideToPath(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, 0.0f);
const Vector2f bearing_vec_to_point = -vector_A_to_vehicle.normalized();
if (unit_path_tangent_.dot(bearing_vec_) < unit_path_tangent_.dot(bearing_vec_to_point)) {
// we are in front of the first waypoint and the bearing to the point is
// shallower than that to the line. reset path params to fly directly to
// the first waypoint.
// TODO: probably better to blend these instead of hard switching (could
// affect the adaptive tuning if we switch between these cases with wind
// gusts)
unit_path_tangent_ = bearing_vec_to_point;
signed_track_error_ = vector_A_to_vehicle.norm();
closest_point_on_path_ = waypoint_A;
guideToPoint(ground_vel, wind_vel, bearing_vec_to_point, signed_track_error_);
}
} else {
// track the line segment between A and B
unit_path_tangent_ = vector_A_to_B.normalized();
signed_track_error_ = unit_path_tangent_.cross(vector_A_to_vehicle);
closest_point_on_path_ = waypoint_A + vector_A_to_vehicle.dot(unit_path_tangent_) * unit_path_tangent_;
guideToPath(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, 0.0f);
}
updateRollSetpoint();
} // navigateWaypoints
void NPFG::navigateLoiter(const Vector2f &loiter_center, const Vector2f &vehicle_pos,
float radius, bool loiter_direction_counter_clockwise, const Vector2f &ground_vel, const Vector2f &wind_vel)
{
path_type_loiter_ = true;
radius = math::max(radius, MIN_RADIUS);
const float loiter_direction_multiplier = loiter_direction_counter_clockwise ? -1.f : 1.f;
Vector2f vector_center_to_vehicle = vehicle_pos - loiter_center;
const float dist_to_center = vector_center_to_vehicle.norm();
// find the direction from the circle center to the closest point on its perimeter
// from the vehicle position
Vector2f unit_vec_center_to_closest_pt;
if (dist_to_center < 0.1f) {
// the logic breaks down at the circle center, employ some mitigation strategies
// until we exit this region
if (ground_vel.norm() < 0.1f) {
// arbitrarily set the point in the northern top of the circle
unit_vec_center_to_closest_pt = Vector2f{1.0f, 0.0f};
} else {
// set the point in the direction we are moving
unit_vec_center_to_closest_pt = ground_vel.normalized();
}
} else {
// set the point in the direction of the aircraft
unit_vec_center_to_closest_pt = vector_center_to_vehicle.normalized();
}
// 90 deg clockwise rotation * loiter direction
unit_path_tangent_ = loiter_direction_multiplier * Vector2f{-unit_vec_center_to_closest_pt(1), unit_vec_center_to_closest_pt(0)};
// positive in direction of path normal
signed_track_error_ = -loiter_direction_multiplier * (dist_to_center - radius);
closest_point_on_path_ = unit_vec_center_to_closest_pt * radius + loiter_center;
float path_curvature = loiter_direction_multiplier / radius;
guideToPath(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, path_curvature);
updateRollSetpoint();
} // navigateLoiter
void NPFG::navigatePathTangent(const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &position_setpoint,
const matrix::Vector2f &tangent_setpoint,
const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel, const float &curvature)
{
path_type_loiter_ = false;
// set unit tangent directly
unit_path_tangent_ = tangent_setpoint.normalized();
// closest point to vehicle
matrix::Vector2f error_vector = vehicle_pos - position_setpoint;
closest_point_on_path_ = position_setpoint;
signed_track_error_ = unit_path_tangent_.cross(error_vector);
guideToPath(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, curvature);
updateRollSetpoint();
} // navigatePathTangent
void NPFG::navigateHeading(float heading_ref, const Vector2f &ground_vel, const Vector2f &wind_vel)
{
path_type_loiter_ = false;
Vector2f air_vel = ground_vel - wind_vel;
unit_path_tangent_ = Vector2f{cosf(heading_ref), sinf(heading_ref)};
signed_track_error_ = 0.0f;
closest_point_on_path_.setNaN();
// use the guidance law to regulate heading error - ignoring wind or inertial position
guideToPath(air_vel, Vector2f{0.0f, 0.0f}, unit_path_tangent_, signed_track_error_, 0.0f);
updateRollSetpoint();
} // navigateHeading
void NPFG::navigateBearing(float bearing, const Vector2f &ground_vel, const Vector2f &wind_vel)
{
path_type_loiter_ = false;
unit_path_tangent_ = Vector2f{cosf(bearing), sinf(bearing)};
signed_track_error_ = 0.0f;
// no track error or path curvature to consider, just regulate ground velocity
// to bearing vector
guideToPath(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, 0.0f);
updateRollSetpoint();
} // navigateBearing
void NPFG::navigateLevelFlight(const float heading)
{
path_type_loiter_ = false;
airspeed_ref_ = airspeed_nom_;
lateral_accel_ = 0.0f;
feas_ = 1.0f;
bearing_vec_ = Vector2f{cosf(heading), sinf(heading)};
unit_path_tangent_ = bearing_vec_;
signed_track_error_ = 0.0f;
updateRollSetpoint();
} // navigateLevelFlight
float NPFG::switchDistance(float wp_radius) const
{
return math::min(wp_radius, track_error_bound_ * switch_distance_multiplier_);
src/lib/npfg/npfg.hpp
+16 -137
View File
@@ -71,6 +71,22 @@ class NPFG
{
public:
/*
* Computes the lateral acceleration and airspeed references necessary to track
* a path in wind (including excess wind conditions).
*
* @param[in] curr_pos_local Current horizontal vehicle position in local coordinates [m]
* @param[in] ground_vel Vehicle ground velocity vector [m/s]
* @param[in] wind_vel Wind velocity vector [m/s]
* @param[in] unit_path_tangent Unit vector tangent to path at closest point
* in direction of path
* @param[in] position_on_path Horizontal point on the path to track described in local coordinates [m]
* @param[in] path_curvature Path curvature at closest point on track [m^-1]
*/
void guideToPath(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_vel,
const matrix::Vector2f &wind_vel,
const matrix::Vector2f &unit_path_tangent, const matrix::Vector2f &position_on_path,
const float path_curvature);
/*
* Set the nominal controller period [s].
@@ -180,8 +196,6 @@ public:
*/
float getHeadingRef() const { return atan2f(air_vel_ref_(1), air_vel_ref_(0)); }
matrix::Vector2f getClosestPoint() const { return closest_point_on_path_;}
/*
* @return Bearing angle [rad]
*/
@@ -213,91 +227,6 @@ public:
*/
float getMinGroundSpeedRef() const { return min_ground_speed_ref_; }
/*******************************************************************************
* PX4 NAVIGATION INTERFACE FUNCTIONS (provide similar functionality to ECL_L1_Pos_Controller)
*/
/*
* Waypoint handling logic following closely to the ECL_L1_Pos_Controller
* method of the same name. Takes two waypoints and determines the relevant
* parameters for evaluating the NPFG guidance law, then updates control setpoints.
*
* @param[in] waypoint_A Waypoint A (segment start) position in WGS84 coordinates
* (lat,lon) [deg]
* @param[in] waypoint_B Waypoint B (segment end) position in WGS84 coordinates
* (lat,lon) [deg]
* @param[in] vehicle_pos Vehicle position in WGS84 coordinates (lat,lon) [deg]
* @param[in] ground_vel Vehicle ground velocity vector [m/s]
* @param[in] wind_vel Wind velocity vector [m/s]
*/
void navigateWaypoints(const matrix::Vector2f &waypoint_A, const matrix::Vector2f &waypoint_B,
const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &ground_vel,
const matrix::Vector2f &wind_vel);
/*
* Loitering (unlimited) logic. Takes loiter center, radius, and direction and
* determines the relevant parameters for evaluating the NPFG guidance law,
* then updates control setpoints.
*
* @param[in] loiter_center The position of the center of the loiter circle [m]
* @param[in] vehicle_pos Vehicle position in WGS84 coordinates (lat,lon) [deg]
* @param[in] radius Loiter radius [m]
* @param[in] loiter_direction_counter_clockwise Specifies loiter direction
* @param[in] ground_vel Vehicle ground velocity vector [m/s]
* @param[in] wind_vel Wind velocity vector [m/s]
*/
void navigateLoiter(const matrix::Vector2f &loiter_center, const matrix::Vector2f &vehicle_pos,
float radius, bool loiter_direction_counter_clockwise, const matrix::Vector2f &ground_vel,
const matrix::Vector2f &wind_vel);
/*
* Path following logic. Takes poisiton, path tangent, curvature and
* then updates control setpoints to follow a path setpoint.
*
* @param[in] vehicle_pos vehicle_pos Vehicle position in WGS84 coordinates (lat,lon) [deg]
* @param[in] position_setpoint closest point on a path in WGS84 coordinates (lat,lon) [deg]
* @param[in] tangent_setpoint unit tangent vector of the path [m]
* @param[in] ground_vel Vehicle ground velocity vector [m/s]
* @param[in] wind_vel Wind velocity vector [m/s]
* @param[in] curvature of the path setpoint [1/m]
*/
void navigatePathTangent(const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &position_setpoint,
const matrix::Vector2f &tangent_setpoint,
const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel, const float &curvature);
/*
* Navigate on a fixed heading.
*
* This only holds a certain (air mass relative) direction and does not perform
* cross track correction. Helpful for semi-autonomous modes. Introduced
* by in ECL_L1_Pos_Controller, augmented for use with NPFG here.
*
* @param[in] heading_ref Reference heading angle [rad]
* @param[in] ground_vel Vehicle ground velocity vector [m/s]
* @param[in] wind_vel Wind velocity vector [m/s]
*/
void navigateHeading(float heading_ref, const matrix::Vector2f &ground_vel,
const matrix::Vector2f &wind_vel);
/*
* Navigate on a fixed bearing.
*
* This only holds a certain (ground relative) direction and does not perform
* cross track correction. Helpful for semi-autonomous modes. Similar to navigateHeading.
*
* @param[in] bearing Bearing angle [rad]
* @param[in] ground_vel Vehicle ground velocity vector [m/s]
* @param[in] wind_vel Wind velocity vector [m/s]
*/
void navigateBearing(float bearing, const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel);
/*
* Keep the wings level.
*
* @param[in] heading Heading angle [rad]
*/
void navigateLevelFlight(const float heading);
/*
* [Copied directly from ECL_L1_Pos_Controller]
*
@@ -331,25 +260,6 @@ public:
*/
float switchDistance(float wp_radius) const;
/*
* The path bearing
*
* @return bearing angle (-pi..pi, in NED frame) [rad]
*/
float targetBearing() const { return atan2f(unit_path_tangent_(1), unit_path_tangent_(0)); }
/*
* [Copied directly from ECL_L1_Pos_Controller]
*
* Returns true if the loiter waypoint has been reached
*/
bool reachedLoiterTarget() { return circleMode(); }
/*
* Returns true if following a circle (loiter)
*/
bool circleMode() { return path_type_loiter_ && track_proximity_ > NPFG_EPSILON; }
/*
* [Copied directly from ECL_L1_Pos_Controller]
*
@@ -421,10 +331,8 @@ private:
float track_proximity_{0.0f}; // value in [0,1] indicating proximity to track, 0 = at track error boundary or beyond, 1 = on track
// path following states
matrix::Vector2f unit_path_tangent_{matrix::Vector2f{1.0f, 0.0f}}; // unit path tangent vector
float signed_track_error_{0.0f}; // signed track error [m]
matrix::Vector2f bearing_vec_{matrix::Vector2f{1.0f, 0.0f}}; // bearing unit vector
matrix::Vector2f closest_point_on_path_{matrix::Vector2f{NAN, NAN}}; // instantaneous position setpoint [m]
/*
* guidance outputs
@@ -443,35 +351,6 @@ private:
float roll_lim_rad_{math::radians(30.0f)}; // maximum roll angle [rad]
float roll_setpoint_{0.0f}; // current roll angle setpoint [rad]
float roll_slew_rate_{0.0f}; // roll angle setpoint slew rate limit [rad/s]
bool path_type_loiter_{false}; // true if the guidance law is tracking a loiter circle
/*
* Computes the lateral acceleration and airspeed references necessary to track
* a path in wind (including excess wind conditions).
*
* @param[in] ground_vel Vehicle ground velocity vector [m/s]
* @param[in] wind_vel Wind velocity vector [m/s]
* @param[in] unit_path_tangent Unit vector tangent to path at closest point
* in direction of path
* @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]
*/
void guideToPath(const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel,
const matrix::Vector2f &unit_path_tangent, const float signed_track_error,
const float path_curvature);
/*
* Computes the lateral acceleration and airspeed references necessary to track
* a point in wind (including excess wind conditions).
*
* @param[in] ground_vel Vehicle ground velocity vector [m/s]
* @param[in] wind_vel Wind velocity vector [m/s]
* @param[in] bearing_vec Unit vector from vehicle to the target point
* @param[in] track_error Distance from vehicle to the target point [m]
*/
void guideToPoint(const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel,
const matrix::Vector2f &bearing_vec, const float track_error);
/*
* Adapts the controller period considering user defined inputs, current flight
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+123 -56
View File
@@ -536,7 +536,7 @@ FixedwingPositionControl::status_publish()
const float bearing = _npfg.getBearing(); // dont repeat atan2 calc
pos_ctrl_status.nav_bearing = bearing;
pos_ctrl_status.target_bearing = _npfg.targetBearing();
pos_ctrl_status.target_bearing = _target_bearing;
pos_ctrl_status.xtrack_error = _npfg.getTrackError();
pos_ctrl_status.acceptance_radius = _npfg.switchDistance(500.0f);
@@ -895,9 +895,6 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
_att_sp.thrust_body[0] = 0.0f;
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = radians(_param_fw_psp_off.get());
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
break;
case position_setpoint_s::SETPOINT_TYPE_POSITION:
@@ -957,9 +954,6 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i
}
_att_sp.pitch_body = get_tecs_pitch();
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
}
void
@@ -986,9 +980,6 @@ FixedwingPositionControl::control_auto_descend(const float control_interval)
_att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_min.get() : min(get_tecs_thrust(), _param_fw_thr_max.get());
_att_sp.pitch_body = get_tecs_pitch();
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
}
uint8_t
@@ -1138,11 +1129,11 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
matrix::Vector2f velocity_2d(pos_sp_curr.vx, pos_sp_curr.vy);
float curvature = PX4_ISFINITE(_pos_sp_triplet.current.loiter_radius) ? 1 / _pos_sp_triplet.current.loiter_radius :
0.0f;
_npfg.navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), get_nav_speed_2d(ground_speed),
_wind_vel, curvature);
navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), get_nav_speed_2d(ground_speed),
_wind_vel, curvature);
} else {
_npfg.navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, get_nav_speed_2d(ground_speed), _wind_vel);
navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, get_nav_speed_2d(ground_speed), _wind_vel);
}
_att_sp.roll_body = _npfg.getRollSetpoint();
@@ -1155,9 +1146,6 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
tecs_update_pitch_throttle(control_interval,
position_sp_alt,
target_airspeed,
@@ -1199,9 +1187,10 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
_param_fw_airspd_min.get(), ground_speed);
if (_param_fw_use_npfg.get()) {
Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
_npfg.navigateBearing(_target_bearing, ground_speed, _wind_vel);
navigateBearing(curr_pos_local, _target_bearing, ground_speed, _wind_vel);
_att_sp.roll_body = _npfg.getRollSetpoint();
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
@@ -1212,9 +1201,6 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
_att_sp.yaw_body = _yaw;
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
tecs_update_pitch_throttle(control_interval,
position_sp_alt,
target_airspeed,
@@ -1279,10 +1265,14 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
loiter_radius = _param_nav_loiter_rad.get();
}
const bool in_circle_mode = (_param_fw_use_npfg.get()) ? _npfg.circleMode() : _l1_control.circle_mode();
Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
Vector2f curr_wp_local{_global_local_proj_ref.project(curr_wp(0), curr_wp(1))};
Vector2f vehicle_to_loiter_center{curr_wp_local - curr_pos_local};
const bool close_to_circle = vehicle_to_loiter_center.norm() < loiter_radius + _npfg.switchDistance(500);
if (pos_sp_next.type == position_setpoint_s::SETPOINT_TYPE_LAND && _position_setpoint_next_valid
&& in_circle_mode && _param_fw_lnd_earlycfg.get()) {
&& close_to_circle && _param_fw_lnd_earlycfg.get()) {
// We're in a loiter directly before a landing WP. Enable our landing configuration (flaps,
// landing airspeed and potentially tighter altitude control) already such that we don't
// have to do this switch (which can cause significant altitude errors) close to the ground.
@@ -1291,23 +1281,17 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_LAND;
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_LAND;
} else {
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
}
float target_airspeed = adapt_airspeed_setpoint(control_interval, airspeed_sp, _param_fw_airspd_min.get(),
ground_speed);
Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
if (_param_fw_use_npfg.get()) {
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
_npfg.navigateLoiter(curr_wp_local, curr_pos_local, loiter_radius, pos_sp_curr.loiter_direction_counter_clockwise,
get_nav_speed_2d(ground_speed),
_wind_vel);
navigateLoiter(curr_wp_local, curr_pos_local, loiter_radius, pos_sp_curr.loiter_direction_counter_clockwise,
get_nav_speed_2d(ground_speed),
_wind_vel);
_att_sp.roll_body = _npfg.getRollSetpoint();
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
@@ -1426,8 +1410,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
if (_param_fw_use_npfg.get()) {
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
_npfg.navigatePathTangent(local_2D_position, start_pos_local, takeoff_bearing_vector, ground_speed,
_wind_vel, 0.0f);
navigatePathTangent(local_2D_position, start_pos_local, takeoff_bearing_vector, ground_speed,
_wind_vel, 0.0f);
_att_sp.roll_body = _runway_takeoff.getRoll(_npfg.getRollSetpoint());
@@ -1485,7 +1469,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
// apply flaps for takeoff according to the corresponding scale factor set via FW_FLAPS_TO_SCL
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_TAKEOFF;
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
} else {
/* Perform launch detection */
@@ -1532,8 +1515,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
if (_param_fw_use_npfg.get()) {
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
_npfg.navigatePathTangent(local_2D_position, launch_local_position, takeoff_bearing_vector, ground_speed, _wind_vel,
0.0f);
navigatePathTangent(local_2D_position, launch_local_position, takeoff_bearing_vector, ground_speed, _wind_vel,
0.0f);
_att_sp.roll_body = _npfg.getRollSetpoint();
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
@@ -1578,9 +1561,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
_att_sp.pitch_body = radians(_takeoff_pitch_min.get());
}
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
launch_detection_status_s launch_detection_status;
launch_detection_status.timestamp = now;
launch_detection_status.launch_detection_state = _launchDetector.getLaunchDetected();
@@ -1688,7 +1668,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
if (_param_fw_use_npfg.get()) {
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
_npfg.navigateWaypoints(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel);
navigateWaypoints(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel);
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
_att_sp.roll_body = _npfg.getRollSetpoint();
@@ -1785,7 +1765,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
if (_param_fw_use_npfg.get()) {
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
_npfg.navigateWaypoints(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel);
navigateWaypoints(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel);
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
_att_sp.roll_body = _npfg.getRollSetpoint();
@@ -1889,11 +1869,6 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval,
_att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max);
_att_sp.pitch_body = get_tecs_pitch();
// In Manual modes flaps and spoilers are directly controlled in FW Attitude controller and not passed
// through attitdue setpoints
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
}
void
@@ -1965,7 +1940,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
if (_param_fw_use_npfg.get()) {
_npfg.setAirspeedNom(calibrated_airspeed_sp * _eas2tas);
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
_npfg.navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
_att_sp.roll_body = _npfg.getRollSetpoint();
calibrated_airspeed_sp = _npfg.getAirspeedRef() / _eas2tas;
@@ -2012,11 +1987,6 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
_att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max);
_att_sp.pitch_body = get_tecs_pitch();
// In Manual modes flaps and spoilers are directly controlled in FW Attitude controller and not passed
// through attitdue setpoints
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
}
float
@@ -2232,6 +2202,8 @@ FixedwingPositionControl::Run()
_l1_control.set_l1_period(_param_fw_l1_period.get());
_att_sp.reset_integral = false;
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
// by default we don't want yaw to be contoller directly with rudder
_att_sp.fw_control_yaw_wheel = false;
@@ -2288,9 +2260,6 @@ FixedwingPositionControl::Run()
case FW_POSCTRL_MODE_OTHER: {
_att_sp.thrust_body[0] = min(_att_sp.thrust_body[0], _param_fw_thr_max.get());
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas);
break;
@@ -2720,7 +2689,7 @@ void FixedwingPositionControl::publishLocalPositionSetpoint(const position_setpo
}
} else {
current_setpoint = _npfg.getClosestPoint();
current_setpoint = _closest_point_on_path;
}
local_position_setpoint.x = current_setpoint(0);
@@ -2759,6 +2728,104 @@ void FixedwingPositionControl::publishOrbitStatus(const position_setpoint_s pos_
_orbit_status_pub.publish(orbit_status);
}
void FixedwingPositionControl::navigateWaypoints(const Vector2f &waypoint_A, const Vector2f &waypoint_B,
const Vector2f &vehicle_pos, const Vector2f &ground_vel, const Vector2f &wind_vel)
{
// similar to logic found in ECL_L1_Pos_Controller method of same name
// BUT no arbitrary max approach angle, approach entirely determined by generated
// bearing vectors
Vector2f vector_A_to_B = waypoint_B - waypoint_A;
Vector2f vector_A_to_vehicle = vehicle_pos - waypoint_A;
if (vector_A_to_B.norm() < FLT_EPSILON) {
// the waypoints are on top of each other and should be considered as a
// single waypoint, fly directly to it
if (vector_A_to_vehicle.norm() > FLT_EPSILON) {
vector_A_to_B = -vector_A_to_vehicle;
} else {
// Fly to a point and on it. Stay to the current control. Do not update the npfg library to get last output.
return;
}
} else if ((vector_A_to_B.dot(vector_A_to_vehicle) < -FLT_EPSILON)) {
// we are in front of waypoint A, fly directly to it until we are within switch distance.
if (vector_A_to_vehicle.norm() > _npfg.switchDistance(500.0f)) {
vector_A_to_B = -vector_A_to_vehicle;
}
}
// track the line segment
Vector2f unit_path_tangent{vector_A_to_B.normalized()};
_target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0));
_closest_point_on_path = waypoint_A + vector_A_to_vehicle.dot(unit_path_tangent) * unit_path_tangent;
_npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, waypoint_A, 0.0f);
} // navigateWaypoints
void FixedwingPositionControl::navigateLoiter(const Vector2f &loiter_center, const Vector2f &vehicle_pos,
float radius, bool loiter_direction_counter_clockwise, const Vector2f &ground_vel, const Vector2f &wind_vel)
{
const float loiter_direction_multiplier = loiter_direction_counter_clockwise ? -1.f : 1.f;
Vector2f vector_center_to_vehicle = vehicle_pos - loiter_center;
const float dist_to_center = vector_center_to_vehicle.norm();
// find the direction from the circle center to the closest point on its perimeter
// from the vehicle position
Vector2f unit_vec_center_to_closest_pt;
if (dist_to_center < 0.1f) {
// the logic breaks down at the circle center, employ some mitigation strategies
// until we exit this region
if (ground_vel.norm() < 0.1f) {
// arbitrarily set the point in the northern top of the circle
unit_vec_center_to_closest_pt = Vector2f{1.0f, 0.0f};
} else {
// set the point in the direction we are moving
unit_vec_center_to_closest_pt = ground_vel.normalized();
}
} else {
// set the point in the direction of the aircraft
unit_vec_center_to_closest_pt = vector_center_to_vehicle.normalized();
}
// 90 deg clockwise rotation * loiter direction
const Vector2f unit_path_tangent = loiter_direction_multiplier * Vector2f{-unit_vec_center_to_closest_pt(1), unit_vec_center_to_closest_pt(0)};
float path_curvature = loiter_direction_multiplier / radius;
_target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0));
_closest_point_on_path = unit_vec_center_to_closest_pt * radius + loiter_center;
_npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent,
loiter_center + unit_vec_center_to_closest_pt * radius, path_curvature);
} // navigateLoiter
void FixedwingPositionControl::navigatePathTangent(const matrix::Vector2f &vehicle_pos,
const matrix::Vector2f &position_setpoint,
const matrix::Vector2f &tangent_setpoint,
const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel, const float &curvature)
{
const Vector2f unit_path_tangent{tangent_setpoint.normalized()};
_target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0));
_closest_point_on_path = position_setpoint;
_npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, tangent_setpoint.normalized(), position_setpoint, curvature);
} // navigatePathTangent
void FixedwingPositionControl::navigateBearing(const matrix::Vector2f &vehicle_pos, float bearing,
const Vector2f &ground_vel, const Vector2f &wind_vel)
{
const Vector2f unit_path_tangent = Vector2f{cosf(bearing), sinf(bearing)};
_target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0));
_closest_point_on_path = vehicle_pos;
_npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, vehicle_pos, 0.0f);
} // navigateBearing
int FixedwingPositionControl::task_spawn(int argc, char *argv[])
{
bool vtol = false;
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+65
View File
@@ -412,6 +412,9 @@ private:
// LATERAL-DIRECTIONAL GUIDANCE
// CLosest point on path to track
matrix::Vector2f _closest_point_on_path;
// L1 guidance - lateral-directional position control
ECL_L1_Pos_Controller _l1_control;
@@ -773,6 +776,68 @@ private:
void initializeAutoLanding(const hrt_abstime &now, const position_setpoint_s &pos_sp_prev,
const position_setpoint_s &pos_sp_curr, const Vector2f &local_position, const Vector2f &local_land_point);
/*
* Waypoint handling logic following closely to the ECL_L1_Pos_Controller
* method of the same name. Takes two waypoints and determines the relevant
* parameters for evaluating the NPFG guidance law, then updates control setpoints.
*
* @param[in] waypoint_A Waypoint A (segment start) position in WGS84 coordinates
* (lat,lon) [deg]
* @param[in] waypoint_B Waypoint B (segment end) position in WGS84 coordinates
* (lat,lon) [deg]
* @param[in] vehicle_pos Vehicle position in WGS84 coordinates (lat,lon) [deg]
* @param[in] ground_vel Vehicle ground velocity vector [m/s]
* @param[in] wind_vel Wind velocity vector [m/s]
*/
void navigateWaypoints(const matrix::Vector2f &waypoint_A, const matrix::Vector2f &waypoint_B,
const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &ground_vel,
const matrix::Vector2f &wind_vel);
/*
* Loitering (unlimited) logic. Takes loiter center, radius, and direction and
* determines the relevant parameters for evaluating the NPFG guidance law,
* then updates control setpoints.
*
* @param[in] loiter_center The position of the center of the loiter circle [m]
* @param[in] vehicle_pos Vehicle position in WGS84 coordinates (lat,lon) [deg]
* @param[in] radius Loiter radius [m]
* @param[in] loiter_direction_counter_clockwise Specifies loiter direction
* @param[in] ground_vel Vehicle ground velocity vector [m/s]
* @param[in] wind_vel Wind velocity vector [m/s]
*/
void navigateLoiter(const matrix::Vector2f &loiter_center, const matrix::Vector2f &vehicle_pos,
float radius, bool loiter_direction_counter_clockwise, const matrix::Vector2f &ground_vel,
const matrix::Vector2f &wind_vel);
/*
* Path following logic. Takes poisiton, path tangent, curvature and
* then updates control setpoints to follow a path setpoint.
*
* @param[in] vehicle_pos vehicle_pos Vehicle position in WGS84 coordinates (lat,lon) [deg]
* @param[in] position_setpoint closest point on a path in WGS84 coordinates (lat,lon) [deg]
* @param[in] tangent_setpoint unit tangent vector of the path [m]
* @param[in] ground_vel Vehicle ground velocity vector [m/s]
* @param[in] wind_vel Wind velocity vector [m/s]
* @param[in] curvature of the path setpoint [1/m]
*/
void navigatePathTangent(const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &position_setpoint,
const matrix::Vector2f &tangent_setpoint,
const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel, const float &curvature);
/*
* Navigate on a fixed bearing.
*
* This only holds a certain (ground relative) direction and does not perform
* cross track correction. Helpful for semi-autonomous modes. Similar to navigateHeading.
*
* @param[in] vehicle_pos vehicle_pos Vehicle position in WGS84 coordinates (lat,lon) [deg]
* @param[in] bearing Bearing angle [rad]
* @param[in] ground_vel Vehicle ground velocity vector [m/s]
* @param[in] wind_vel Wind velocity vector [m/s]
*/
void navigateBearing(const matrix::Vector2f &vehicle_pos, float bearing, const matrix::Vector2f &ground_vel,
const matrix::Vector2f &wind_vel);
DEFINE_PARAMETERS(
(ParamFloat<px4::params::FW_AIRSPD_MAX>) _param_fw_airspd_max,