diff --git a/src/lib/npfg/npfg.cpp b/src/lib/npfg/npfg.cpp index 03ba6939fb..a4308fb150 100644 --- a/src/lib/npfg/npfg.cpp +++ b/src/lib/npfg/npfg.cpp @@ -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_); diff --git a/src/lib/npfg/npfg.hpp b/src/lib/npfg/npfg.hpp index decce96f20..f9c6bf2c42 100644 --- a/src/lib/npfg/npfg.hpp +++ b/src/lib/npfg/npfg.hpp @@ -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 diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 29c456cc35..e97d669af3 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -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; diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index d7f59b5a56..e2d42b7f0a 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -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) _param_fw_airspd_max,