From 87574986d5328a1a1ad627a9c345e41f0f2ceb4e Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Sat, 10 Jan 2026 06:41:06 -0800 Subject: [PATCH] Remove current mode Remove landing logic Remove waypoint navigtion logic --- .../FixedWingGuidanceControl.cpp | 800 +----------------- .../FixedWingGuidanceControl.hpp | 266 +----- 2 files changed, 14 insertions(+), 1052 deletions(-) diff --git a/src/modules/fw_guidance_control/FixedWingGuidanceControl.cpp b/src/modules/fw_guidance_control/FixedWingGuidanceControl.cpp index 47c6f8ec7a..56ae1c5dd2 100644 --- a/src/modules/fw_guidance_control/FixedWingGuidanceControl.cpp +++ b/src/modules/fw_guidance_control/FixedWingGuidanceControl.cpp @@ -60,13 +60,8 @@ FixedWingGuidanceControl::FixedWingGuidanceControl() : // limit to 50 Hz _local_pos_sub.set_interval_ms(20); - _pos_ctrl_landing_status_pub.advertise(); _launch_detection_status_pub.advertise(); - _landing_gear_pub.advertise(); - _flaps_setpoint_pub.advertise(); - _spoilers_setpoint_pub.advertise(); _fixed_wing_lateral_guidance_status_pub.advertise(); - _fixed_wing_runway_control_pub.advertise(); parameters_update(); } @@ -104,18 +99,6 @@ FixedWingGuidanceControl::parameters_update() void FixedWingGuidanceControl::vehicle_control_mode_poll() { - if (_control_mode_sub.updated()) { - const bool was_armed = _control_mode.flag_armed; - - if (_control_mode_sub.copy(&_control_mode)) { - - // reset state when arming - if (!was_armed && _control_mode.flag_armed) { - reset_takeoff_state(); - reset_landing_state(); - } - } - } } void @@ -124,16 +107,7 @@ FixedWingGuidanceControl::vehicle_command_poll() vehicle_command_s vehicle_command; while (_vehicle_command_sub.update(&vehicle_command)) { - if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND) { - // only abort landing before point of no return (horizontal and vertical) - if (_control_mode.flag_control_auto_enabled && - _position_setpoint_current_valid && - (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND)) { - - updateLandingAbortStatus(position_controller_landing_status_s::ABORTED_BY_OPERATOR); - } - - } else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED) { + if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED) { if ((static_cast(vehicle_command.param1 + .5f) == vehicle_command_s::SPEED_TYPE_AIRSPEED)) { if (vehicle_command.param2 > FLT_EPSILON) { // param2 is an equivalent airspeed setpoint @@ -252,237 +226,6 @@ FixedWingGuidanceControl::vehicle_attitude_poll() } } -float -FixedWingGuidanceControl::get_manual_airspeed_setpoint() -{ - float manual_airspeed_setpoint = NAN; - - if (_param_fw_pos_stk_conf.get() & STICK_CONFIG_ENABLE_AIRSPEED_SP_MANUAL_BIT) { - // neutral throttle corresponds to trim airspeed - manual_airspeed_setpoint = math::interpolateNXY(_manual_control_setpoint_for_airspeed, - {-1.f, 0.f, 1.f}, - {_param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), _param_fw_airspd_max.get()}); - - } else if (PX4_ISFINITE(_commanded_manual_airspeed_setpoint)) { - // override stick by commanded airspeed - manual_airspeed_setpoint = _commanded_manual_airspeed_setpoint; - } - - return manual_airspeed_setpoint; -} - -void -FixedWingGuidanceControl::landing_status_publish() -{ - position_controller_landing_status_s pos_ctrl_landing_status = {}; - - pos_ctrl_landing_status.lateral_touchdown_offset = _lateral_touchdown_position_offset; - pos_ctrl_landing_status.flaring = _flare_states.flaring; - pos_ctrl_landing_status.abort_status = _landing_abort_status; - pos_ctrl_landing_status.timestamp = hrt_absolute_time(); - - _pos_ctrl_landing_status_pub.publish(pos_ctrl_landing_status); -} - -void -FixedWingGuidanceControl::updateLandingAbortStatus(const uint8_t new_abort_status) -{ - // prevent automatic aborts if already flaring, but allow manual aborts - if (!_flare_states.flaring || new_abort_status == position_controller_landing_status_s::ABORTED_BY_OPERATOR) { - - // only announce changes - // if (new_abort_status > 0 && _landing_abort_status != new_abort_status) { - - // switch (new_abort_status) { - // case (position_controller_landing_status_s::ABORTED_BY_OPERATOR): { - // events::send(events::ID("fixedwing_position_control_landing_abort_status_operator_abort"), events::Log::Critical, - // "Landing aborted by operator"); - // break; - // } - - // case (position_controller_landing_status_s::TERRAIN_NOT_FOUND): { - // events::send(events::ID("fixedwing_position_control_landing_abort_status_terrain_not_found"), events::Log::Critical, - // "Landing aborted: terrain measurement not found"); - // break; - // } - - // case (position_controller_landing_status_s::TERRAIN_TIMEOUT): { - // events::send(events::ID("fixedwing_position_control_landing_abort_status_terrain_timeout"), events::Log::Critical, - // "Landing aborted: terrain estimate timed out"); - // break; - // } - - // default: { - // events::send(events::ID("fixedwing_position_control_landing_abort_status_unknown_criterion"), events::Log::Critical, - // "Landing aborted: unknown criterion"); - // } - // } - // } - - _landing_abort_status = (new_abort_status >= position_controller_landing_status_s::UNKNOWN_ABORT_CRITERION) ? - position_controller_landing_status_s::UNKNOWN_ABORT_CRITERION : new_abort_status; - landing_status_publish(); - } -} - -float -FixedWingGuidanceControl::getManualHeightRateSetpoint() -{ - float height_rate_setpoint = 0.f; - - if (_manual_control_setpoint_for_height_rate >= FLT_EPSILON) { - height_rate_setpoint = math::interpolate(math::deadzone(_manual_control_setpoint_for_height_rate, - kStickDeadBand), 0, 1.f, 0.f, -_param_sinkrate_target.get()); - - } else { - height_rate_setpoint = math::interpolate(math::deadzone(_manual_control_setpoint_for_height_rate, - kStickDeadBand), -1., 0.f, _param_climbrate_target.get(), 0.f); - } - - return height_rate_setpoint; -} - -void -FixedWingGuidanceControl::updateManualTakeoffStatus() -{ - if (!_completed_manual_takeoff) { - const bool at_controllable_airspeed = _airspeed_eas > _param_fw_airspd_min.get() - || !_airspeed_valid; - const bool is_hovering = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING - && _control_mode.flag_armed; - _completed_manual_takeoff = (!_landed && at_controllable_airspeed) || is_hovering; - } -} - -// void -// FixedWingGuidanceControl::set_control_mode_current(const hrt_abstime &now) -// { -// /* only run position controller in fixed-wing mode and during transitions for VTOL */ -// if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.in_transition_mode) { -// _control_mode_current = FW_POSCTRL_MODE_OTHER; -// return; // do not publish the setpoint -// } - -// const FW_POSCTRL_MODE previous_position_control_mode = _control_mode_current; - -// _skipping_takeoff_detection = false; -// const bool doing_backtransition = _vehicle_status.in_transition_mode && !_vehicle_status.in_transition_to_fw; - -// if (_control_mode.flag_control_offboard_enabled && _position_setpoint_current_valid -// && _control_mode.flag_control_position_enabled) { -// if (PX4_ISFINITE(_pos_sp_triplet.current.vx) && PX4_ISFINITE(_pos_sp_triplet.current.vy) -// && PX4_ISFINITE(_pos_sp_triplet.current.vz)) { -// // Offboard position with velocity setpoints -// _control_mode_current = FW_POSCTRL_MODE_AUTO_PATH; -// return; - -// } else { -// // Offboard position setpoint only -// _control_mode_current = FW_POSCTRL_MODE_AUTO; -// return; -// } - -// } else if ((_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled) -// && (_position_setpoint_current_valid -// || _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE)) { - -// // Enter this mode only if the current waypoint has valid 3D position setpoints or is of type IDLE. -// // A setpoint of type IDLE can be published by Navigator without a valid position, and is handled here in FW_POSCTRL_MODE_AUTO. - -// if (doing_backtransition) { -// _control_mode_current = FW_POSCTRL_MODE_TRANSITION_TO_HOVER_LINE_FOLLOW; - -// } else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { - -// if (_vehicle_status.is_vtol && _vehicle_status.in_transition_mode) { -// _control_mode_current = FW_POSCTRL_MODE_AUTO; - -// // in this case we want the waypoint handled as a position setpoint -- a submode in control_auto() -// _pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; - -// } else { -// _control_mode_current = _local_pos.xy_valid ? FW_POSCTRL_MODE_AUTO_TAKEOFF : FW_POSCTRL_MODE_AUTO_TAKEOFF_NO_NAV; - -// if (previous_position_control_mode != FW_POSCTRL_MODE_AUTO_TAKEOFF_NO_NAV -// && previous_position_control_mode != FW_POSCTRL_MODE_AUTO_TAKEOFF && !_landed) { -// // skip takeoff detection when switching from any other mode, auto or manual, -// // while already in air. -// // TODO: find a better place for / way of doing this -// _skipping_takeoff_detection = true; -// } -// } - -// } else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { - -// // Use _position_setpoint_previous_valid to determine if landing should be straight or circular. -// // Straight landings are currently only possible in Missions, and there the previous WP -// // is valid, and circular ones are used outside of Missions, as the land mode sets prev_valid=false. -// if (_position_setpoint_previous_valid) { -// _control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT; - -// } else { -// _control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR; -// } - -// } else { -// _control_mode_current = FW_POSCTRL_MODE_AUTO; -// } - -// } else if (_control_mode.flag_control_auto_enabled -// && _control_mode.flag_control_climb_rate_enabled -// && _control_mode.flag_armed // only enter this modes if armed, as pure failsafe modes -// && !_control_mode.flag_control_position_enabled) { - -// // failsafe modes engaged if position estimate is invalidated - -// if (previous_position_control_mode != FW_POSCTRL_MODE_AUTO_ALTITUDE -// && previous_position_control_mode != FW_POSCTRL_MODE_AUTO_CLIMBRATE) { -// // reset timer the first time we switch into this mode -// _time_in_fixed_bank_loiter = now; -// } - -// if (doing_backtransition) { -// // we handle loss of position control during backtransition as a special case -// _control_mode_current = FW_POSCTRL_MODE_TRANSITION_TO_HOVER_HEADING_HOLD; - -// } else if (hrt_elapsed_time(&_time_in_fixed_bank_loiter) < (_param_nav_gpsf_lt.get() * 1_s) -// && !_vehicle_status.in_transition_mode) { -// if (previous_position_control_mode != FW_POSCTRL_MODE_AUTO_ALTITUDE) { -// // Need to init because last loop iteration was in a different mode -// events::send(events::ID("fixedwing_position_control_fb_loiter"), events::Log::Critical, -// "Start loiter with fixed bank angle"); -// } - -// _control_mode_current = FW_POSCTRL_MODE_AUTO_ALTITUDE; - -// } else { -// if (previous_position_control_mode != FW_POSCTRL_MODE_AUTO_CLIMBRATE && !_vehicle_status.in_transition_mode) { -// events::send(events::ID("fixedwing_position_control_descend"), events::Log::Critical, "Start descending"); -// } - -// _control_mode_current = FW_POSCTRL_MODE_AUTO_CLIMBRATE; -// } - - -// } else if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_position_enabled) { -// if (previous_position_control_mode != FW_POSCTRL_MODE_MANUAL_POSITION) { -// /* Need to init because last loop iteration was in a different mode */ -// _hdg_hold_yaw = _yaw; // yaw is not controlled, so set setpoint to current yaw -// _hdg_hold_enabled = false; // this makes sure the waypoints are reset below -// _yaw_lock_engaged = false; -// } - -// _control_mode_current = FW_POSCTRL_MODE_MANUAL_POSITION; - -// } else if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_altitude_enabled) { - -// _control_mode_current = FW_POSCTRL_MODE_MANUAL_ALTITUDE; - -// } else { -// _control_mode_current = FW_POSCTRL_MODE_OTHER; -// } -// } - void FixedWingGuidanceControl::update_in_air_states(const hrt_abstime now) { @@ -492,108 +235,6 @@ FixedWingGuidanceControl::update_in_air_states(const hrt_abstime now) } } -void -FixedWingGuidanceControl::move_position_setpoint_for_vtol_transition(position_setpoint_s ¤t_sp) -{ - // TODO: velocity, altitude, or just a heading hold position mode should be used for this, not position - // shifting hacks - - if (_vehicle_status.in_transition_to_fw) { - - if (!PX4_ISFINITE(_transition_waypoint(0))) { - double lat_transition, lon_transition; - - // Create a virtual waypoint HDG_HOLD_DIST_NEXT meters in front of the vehicle which the path navigation controller can track - // during the transition. Use the current yaw setpoint to determine the transition heading, as that one in turn - // is set to the transition heading by Navigator, or current yaw if setpoint is not valid. - const float transition_heading = PX4_ISFINITE(current_sp.yaw) ? current_sp.yaw : _yaw; - waypoint_from_heading_and_distance(_current_latitude, _current_longitude, transition_heading, HDG_HOLD_DIST_NEXT, - &lat_transition, - &lon_transition); - - _transition_waypoint(0) = lat_transition; - _transition_waypoint(1) = lon_transition; - } - - current_sp.lat = _transition_waypoint(0); - current_sp.lon = _transition_waypoint(1); - - } else { - /* reset transition waypoint, will be set upon entering front transition */ - _transition_waypoint(0) = static_cast(NAN); - _transition_waypoint(1) = static_cast(NAN); - } -} - -void FixedWingGuidanceControl::control_idle() -{ - const hrt_abstime now = hrt_absolute_time(); - fixed_wing_lateral_setpoint_s lateral_ctrl_sp {empty_lateral_control_setpoint}; - lateral_ctrl_sp.timestamp = now; - lateral_ctrl_sp.lateral_acceleration = 0.0f; - _lateral_ctrl_sp_pub.publish(lateral_ctrl_sp); - - fixed_wing_longitudinal_setpoint_s long_contrl_sp {empty_longitudinal_control_setpoint}; - long_contrl_sp.timestamp = now; - long_contrl_sp.pitch_direct = 0.f; - long_contrl_sp.throttle_direct = 0.0f; - _longitudinal_ctrl_sp_pub.publish(long_contrl_sp); - - _ctrl_configuration_handler.setThrottleMax(0.0f); - _ctrl_configuration_handler.setThrottleMin(0.0f); -} - -uint8_t -FixedWingGuidanceControl::handle_setpoint_type(const position_setpoint_s &pos_sp_curr, - const position_setpoint_s &pos_sp_next) -{ - uint8_t position_sp_type = pos_sp_curr.type; - - if (!_control_mode.flag_control_position_enabled && _control_mode.flag_control_velocity_enabled) { - return position_setpoint_s::SETPOINT_TYPE_VELOCITY; - } - - Vector2d curr_wp{0, 0}; - - /* current waypoint (the one currently heading for) */ - curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon); - - const float acc_rad = _directional_guidance.switchDistance(500.0f); - - const bool approaching_vtol_backtransition = _vehicle_status.is_vtol - && pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION && _position_setpoint_current_valid - && pos_sp_next.type == position_setpoint_s::SETPOINT_TYPE_LAND && _position_setpoint_next_valid; - - // check if we should switch to loiter but only if we are not expecting a backtransition to happen - if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION && !approaching_vtol_backtransition) { - - float dist_xy = -1.f; - float dist_z = -1.f; - - const float dist = get_distance_to_point_global_wgs84( - (double)curr_wp(0), (double)curr_wp(1), pos_sp_curr.alt, - _current_latitude, _current_longitude, _current_altitude, - &dist_xy, &dist_z); - - const float acc_rad_z = (PX4_ISFINITE(pos_sp_curr.alt_acceptance_radius) - && pos_sp_curr.alt_acceptance_radius > FLT_EPSILON) ? pos_sp_curr.alt_acceptance_radius : - _param_nav_fw_alt_rad.get(); - - // Achieve position setpoint altitude via loiter when laterally close to WP. - // Detect if system has switchted into a Loiter before (check _position_sp_type), and in that - // case remove the dist_xy check (not switch out of Loiter until altitude is reached). - if ((!_vehicle_status.in_transition_mode) && (dist >= 0.f) - && (dist_z > acc_rad_z) - && (dist_xy < acc_rad || _position_sp_type == position_setpoint_s::SETPOINT_TYPE_LOITER)) { - - // SETPOINT_TYPE_POSITION -> SETPOINT_TYPE_LOITER - position_sp_type = position_setpoint_s::SETPOINT_TYPE_LOITER; - } - } - - return position_sp_type; -} - void FixedWingGuidanceControl::control_auto_path(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr) @@ -635,11 +276,6 @@ FixedWingGuidanceControl::control_auto_path(const float control_interval, const } } -float FixedWingGuidanceControl::rollAngleToLateralAccel(float roll_body) const -{ - return tanf(roll_body) * CONSTANTS_ONE_G; -} - void FixedWingGuidanceControl::Run() { @@ -808,8 +444,6 @@ FixedWingGuidanceControl::Run() Vector2d curr_pos(_current_latitude, _current_longitude); Vector2f ground_speed(_local_pos.vx, _local_pos.vy); - // set_control_mode_current(now); - update_in_air_states(now); // restore nominal TECS parameters in case changed intermittently (e.g. in landing handling) @@ -817,33 +451,13 @@ FixedWingGuidanceControl::Run() // restore lateral-directional guidance parameters (changed in takeoff mode) _directional_guidance.setPeriod(_param_npfg_period.get()); - // by default no flaps/spoilers, is overwritten below in certain modes - _flaps_setpoint = 0.f; - _spoilers_setpoint = 0.f; - // by default set speed weight to the param value, can be overwritten inside the methods below _ctrl_configuration_handler.setSpeedWeight(_param_t_spdweight.get()); - if (_control_mode_current != FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT - && _control_mode_current != FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR) { - reset_landing_state(); - } - if (_control_mode_current != FW_POSCTRL_MODE_AUTO_TAKEOFF - && _control_mode_current != FW_POSCTRL_MODE_AUTO_TAKEOFF_NO_NAV) { - reset_takeoff_state(); - } + control_auto_path(control_interval, curr_pos, ground_speed, _pos_sp_triplet.current); - int8_t old_landing_gear_position = _new_landing_gear_position; - _new_landing_gear_position = landing_gear_s::GEAR_KEEP; // is overwritten in Takeoff and Land - - if (_control_mode_current == FW_POSCTRL_MODE_AUTO_PATH) { - control_auto_path(control_interval, curr_pos, ground_speed, _pos_sp_triplet.current); - } - - if (_control_mode_current != FW_POSCTRL_MODE_OTHER) { - _ctrl_configuration_handler.update(now); - } + _ctrl_configuration_handler.update(now); // only publish status in full FW mode if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING @@ -852,412 +466,12 @@ FixedWingGuidanceControl::Run() } - // if there's any change in landing gear setpoint publish it - if (_new_landing_gear_position != old_landing_gear_position - && _new_landing_gear_position != landing_gear_s::GEAR_KEEP) { - - landing_gear_s landing_gear = {}; - landing_gear.landing_gear = _new_landing_gear_position; - landing_gear.timestamp = now; - _landing_gear_pub.publish(landing_gear); - } - - // In Manual modes flaps and spoilers are directly controlled in the Attitude controller and not published here - if (_control_mode.flag_control_auto_enabled - && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - normalized_unsigned_setpoint_s flaps_setpoint; - flaps_setpoint.normalized_setpoint = _flaps_setpoint; - flaps_setpoint.timestamp = now; - _flaps_setpoint_pub.publish(flaps_setpoint); - - normalized_unsigned_setpoint_s spoilers_setpoint; - spoilers_setpoint.normalized_setpoint = _spoilers_setpoint; - spoilers_setpoint.timestamp = now; - _spoilers_setpoint_pub.publish(spoilers_setpoint); - } - _xy_reset_counter = _local_pos.xy_reset_counter; perf_end(_loop_perf); } } -void -FixedWingGuidanceControl::reset_takeoff_state() -{ - _launch_detected = false; - - _takeoff_ground_alt = _current_altitude; -} - -void -FixedWingGuidanceControl::reset_landing_state() -{ - _time_started_landing = 0; - - _flare_states = FlareStates{}; - - _lateral_touchdown_position_offset = 0.0f; - - _last_time_terrain_alt_was_valid = 0; - - // reset abort land, unless loitering after an abort - if ((_landing_abort_status && (_pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_LOITER)) || - (_landing_abort_status && _param_fw_lnd_abort.get() == 0)) { - - updateLandingAbortStatus(position_controller_landing_status_s::NOT_ABORTED); - } -} - -float FixedWingGuidanceControl::getMaxRollAngleNearGround(const float altitude, const float terrain_altitude) const -{ - // we want the wings level when at the wing height above ground - const float height_above_ground = math::max(altitude - (terrain_altitude + _param_fw_wing_height.get()), 0.0f); - - // this is a conservative (linear) approximation of the roll angle that would cause wing tip strike - // roll strike = arcsin( 2 * height / span ) - // d(roll strike)/d(height) = 2 / span / cos(2 * height / span) - // d(roll strike)/d(height) (@height=0) = 2 / span - // roll strike ~= 2 * height / span - - return math::constrain(2.f * height_above_ground / _param_fw_wing_span.get(), 0.f, - math::radians(_param_fw_r_lim.get())); -} - - -void -FixedWingGuidanceControl::initializeAutoLanding(const hrt_abstime &now, const position_setpoint_s &pos_sp_prev, - const float land_point_altitude, const Vector2f &local_position, const Vector2f &local_land_point) -{ - if (_time_started_landing == 0) { - - float height_above_land_point; - Vector2f local_approach_entrance; - - // set the landing approach entrance location when we have just started the landing and store it - // NOTE: the landing approach vector is relative to the land point. ekf resets may cause a local frame - // jump, so we reference to the land point, which is globally referenced and will update - if (_position_setpoint_previous_valid) { - height_above_land_point = pos_sp_prev.alt - land_point_altitude; - local_approach_entrance = _global_local_proj_ref.project(pos_sp_prev.lat, pos_sp_prev.lon); - - } else { - // no valid previous waypoint, construct one from the glide slope and direction from current - // position to land point - - // NOTE: this is not really a supported use case at the moment, this is just bandaiding any - // ill-advised usage of the current implementation - - // TODO: proper handling of on-the-fly landing points would need to involve some more sophisticated - // landing pattern generation and corresponding logic - - height_above_land_point = _current_altitude - land_point_altitude; - local_approach_entrance = local_position; - } - - _landing_approach_entrance_rel_alt = math::max(height_above_land_point, FLT_EPSILON); - - const Vector2f landing_approach_vector = local_land_point - local_approach_entrance; - float landing_approach_distance = landing_approach_vector.norm(); - - const float max_glide_slope = tanf(math::radians(_param_fw_lnd_ang.get())); - const float glide_slope = _landing_approach_entrance_rel_alt / landing_approach_distance; - - if (glide_slope > max_glide_slope) { - // rescale the landing distance - this will have the same effect as dropping down the approach - // entrance altitude on the vehicle's behavior. if we reach here.. it means the navigator checks - // didn't work, or something is using the control_auto_landing_straight() method inappropriately - landing_approach_distance = _landing_approach_entrance_rel_alt / max_glide_slope; - } - - if (landing_approach_vector.norm_squared() > FLT_EPSILON) { - _landing_approach_entrance_offset_vector = -landing_approach_vector.unit_or_zero() * landing_approach_distance; - - } else { - // land in direction of airframe - _landing_approach_entrance_offset_vector = Vector2f({cosf(_yaw), sinf(_yaw)}) * landing_approach_distance; - } - - // save time at which we started landing and reset landing abort status - reset_landing_state(); - _time_started_landing = now; - } -} - -Vector2f -FixedWingGuidanceControl::calculateTouchdownPosition(const float control_interval, const Vector2f &local_land_position) -{ - if (fabsf(_sticks.getYaw()) > MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE - && _param_fw_lnd_nudge.get() > LandingNudgingOption::kNudgingDisabled - && !_flare_states.flaring) { - // laterally nudge touchdown location with yaw stick - // positive is defined in the direction of a right hand turn starting from the approach vector direction - const float signed_deadzone_threshold = MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE * math::signNoZero( - _sticks.getYaw()); - _lateral_touchdown_position_offset += (_sticks.getYaw() - signed_deadzone_threshold) * - MAX_TOUCHDOWN_POSITION_NUDGE_RATE * control_interval; - _lateral_touchdown_position_offset = math::constrain(_lateral_touchdown_position_offset, -_param_fw_lnd_td_off.get(), - _param_fw_lnd_td_off.get()); - } - - const Vector2f approach_unit_vector = -_landing_approach_entrance_offset_vector.unit_or_zero(); - const Vector2f approach_unit_normal_vector{-approach_unit_vector(1), approach_unit_vector(0)}; - - return local_land_position + approach_unit_normal_vector * _lateral_touchdown_position_offset; -} - -Vector2f -FixedWingGuidanceControl::calculateLandingApproachVector() const -{ - Vector2f landing_approach_vector = -_landing_approach_entrance_offset_vector; - const Vector2f approach_unit_vector = landing_approach_vector.unit_or_zero(); - const Vector2f approach_unit_normal_vector{-approach_unit_vector(1), approach_unit_vector(0)}; - - if (_param_fw_lnd_nudge.get() == LandingNudgingOption::kNudgeApproachAngle) { - // nudge the approach angle -- i.e. we adjust the approach vector to reach from the original approach - // entrance position to the newly nudged touchdown point - // NOTE: this lengthens the landing distance.. which will adjust the glideslope height slightly - landing_approach_vector += approach_unit_normal_vector * _lateral_touchdown_position_offset; - } - - // if _param_fw_lnd_nudge.get() == LandingNudgingOption::kNudgingDisabled, no nudging - - // if _param_fw_lnd_nudge.get() == LandingNudgingOption::kNudgeApproachPath, the full path (including approach - // entrance point) is nudged with the touchdown point, which does not require any additions to the approach vector - - return landing_approach_vector; -} - -float -FixedWingGuidanceControl::getLandingTerrainAltitudeEstimate(const hrt_abstime &now, const float land_point_altitude, - const bool abort_on_terrain_measurement_timeout, const bool abort_on_terrain_timeout) -{ - if (_param_fw_lnd_useter.get() > TerrainEstimateUseOnLanding::kDisableTerrainEstimation) { - - if (_local_pos.dist_bottom_valid) { - - const float terrain_estimate = _local_pos.ref_alt + -_local_pos.z - _local_pos.dist_bottom; - _last_valid_terrain_alt_estimate = terrain_estimate; - _last_time_terrain_alt_was_valid = now; - - return terrain_estimate; - } - - if (_last_time_terrain_alt_was_valid == 0) { - - const bool terrain_first_measurement_timed_out = (now - _time_started_landing) > TERRAIN_ALT_FIRST_MEASUREMENT_TIMEOUT; - - if (terrain_first_measurement_timed_out && abort_on_terrain_measurement_timeout) { - updateLandingAbortStatus(position_controller_landing_status_s::TERRAIN_NOT_FOUND); - } - - return land_point_altitude; - } - - if (!_local_pos.dist_bottom_valid) { - - const bool terrain_timed_out = (now - _last_time_terrain_alt_was_valid) > TERRAIN_ALT_TIMEOUT; - - if (terrain_timed_out && abort_on_terrain_timeout) { - updateLandingAbortStatus(position_controller_landing_status_s::TERRAIN_TIMEOUT); - } - - return _last_valid_terrain_alt_estimate; - } - } - - return land_point_altitude; -} - -bool FixedWingGuidanceControl::checkLandingAbortBitMask(const uint8_t automatic_abort_criteria_bitmask, - uint8_t landing_abort_criterion) -{ - // landing abort status contains a manual criterion at abort_status==1, need to subtract 2 to directly compare - // to automatic criteria bits from the parameter FW_LND_ABORT - if (landing_abort_criterion <= 1) { - return false; - } - - landing_abort_criterion -= 2; - - return ((1 << landing_abort_criterion) & automatic_abort_criteria_bitmask) == (1 << landing_abort_criterion); -} - -void FixedWingGuidanceControl::publishLocalPositionSetpoint(const position_setpoint_s ¤t_waypoint) -{ - vehicle_local_position_setpoint_s local_position_setpoint{}; - local_position_setpoint.timestamp = hrt_absolute_time(); - - Vector2f current_setpoint; - - current_setpoint = _closest_point_on_path; - - local_position_setpoint.x = current_setpoint(0); - local_position_setpoint.y = current_setpoint(1); - local_position_setpoint.z = _reference_altitude - current_waypoint.alt; - local_position_setpoint.yaw = NAN; - local_position_setpoint.yawspeed = NAN; - local_position_setpoint.vx = NAN; - local_position_setpoint.vy = NAN; - local_position_setpoint.vz = NAN; - local_position_setpoint.acceleration[0] = NAN; - local_position_setpoint.acceleration[1] = NAN; - local_position_setpoint.acceleration[2] = NAN; - _local_pos_sp_pub.publish(local_position_setpoint); -} - -void FixedWingGuidanceControl::publishOrbitStatus(const position_setpoint_s pos_sp) -{ - orbit_status_s orbit_status{}; - orbit_status.timestamp = hrt_absolute_time(); - float loiter_radius = pos_sp.loiter_radius * (pos_sp.loiter_direction_counter_clockwise ? -1.f : 1.f); - - if (fabsf(loiter_radius) < FLT_EPSILON) { - loiter_radius = _param_nav_loiter_rad.get(); - } - - orbit_status.radius = loiter_radius; - orbit_status.frame = 0; // MAV_FRAME::MAV_FRAME_GLOBAL - orbit_status.x = static_cast(pos_sp.lat); - orbit_status.y = static_cast(pos_sp.lon); - orbit_status.z = pos_sp.alt; - orbit_status.yaw_behaviour = orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE; - _orbit_status_pub.publish(orbit_status); -} - -DirectionalGuidanceOutput FixedWingGuidanceControl::navigateWaypoints(const Vector2f &start_waypoint, - const Vector2f &end_waypoint, - const Vector2f &vehicle_pos, const Vector2f &ground_vel, const Vector2f &wind_vel) -{ - const Vector2f start_waypoint_to_end_waypoint = end_waypoint - start_waypoint; - const Vector2f start_waypoint_to_vehicle = vehicle_pos - start_waypoint; - const Vector2f end_waypoint_to_vehicle = vehicle_pos - end_waypoint; - - if (start_waypoint_to_end_waypoint.norm() < FLT_EPSILON) { - // degenerate case: the waypoints are on top of each other, this should only happen when someone uses this - // method incorrectly. just as a safe guard, call the singular waypoint navigation method. - return navigateWaypoint(end_waypoint, vehicle_pos, ground_vel, wind_vel); - } - - if ((start_waypoint_to_end_waypoint.dot(start_waypoint_to_vehicle) < -FLT_EPSILON) - && (start_waypoint_to_vehicle.norm() > _directional_guidance.switchDistance(500.0f))) { - // we are in front of the start waypoint, fly directly to it until we are within switch distance - return navigateWaypoint(start_waypoint, vehicle_pos, ground_vel, wind_vel); - } - - if (start_waypoint_to_end_waypoint.dot(end_waypoint_to_vehicle) > FLT_EPSILON) { - // we are beyond the end waypoint, fly back to it - // NOTE: this logic ideally never gets executed, as a waypoint switch should happen before passing the - // end waypoint. however this included here as a safety precaution if any navigator (module) switch condition - // is missed for any reason. in the future this logic should all be handled in one place in a dedicated - // flight mode state machine. - return navigateWaypoint(end_waypoint, vehicle_pos, ground_vel, wind_vel); - } - - // follow the line segment between the start and end waypoints - return navigateLine(start_waypoint, end_waypoint, vehicle_pos, ground_vel, wind_vel); -} - -DirectionalGuidanceOutput FixedWingGuidanceControl::navigateWaypoint(const Vector2f &waypoint_pos, - const Vector2f &vehicle_pos, - const Vector2f &ground_vel, const Vector2f &wind_vel) -{ - const Vector2f vehicle_to_waypoint = waypoint_pos - vehicle_pos; - - if (vehicle_to_waypoint.norm() < FLT_EPSILON) { - // degenerate case: the vehicle is on top of the single waypoint. (can happen). maintain the last npfg command. - return DirectionalGuidanceOutput{}; - } - - const Vector2f unit_path_tangent = vehicle_to_waypoint.normalized(); - _closest_point_on_path = waypoint_pos; - - const float path_curvature = 0.f; - DirectionalGuidanceOutput sp = _directional_guidance.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, - _closest_point_on_path, path_curvature); - - return sp; -} - -DirectionalGuidanceOutput FixedWingGuidanceControl::navigateLine(const Vector2f &point_on_line_1, - const Vector2f &point_on_line_2, - const Vector2f &vehicle_pos, const Vector2f &ground_vel, const Vector2f &wind_vel) -{ - const Vector2f line_segment = point_on_line_2 - point_on_line_1; - - if (line_segment.norm() <= FLT_EPSILON) { - // degenerate case: line segment has zero length. maintain the last npfg command. - return DirectionalGuidanceOutput{}; - } - - const Vector2f unit_path_tangent = line_segment.normalized(); - - const Vector2f point_1_to_vehicle = vehicle_pos - point_on_line_1; - _closest_point_on_path = point_on_line_1 + point_1_to_vehicle.dot(unit_path_tangent) * unit_path_tangent; - - const float path_curvature = 0.f; - const DirectionalGuidanceOutput sp = _directional_guidance.guideToPath(vehicle_pos, ground_vel, wind_vel, - unit_path_tangent, - _closest_point_on_path, path_curvature); - - return sp; -} - -DirectionalGuidanceOutput FixedWingGuidanceControl::navigateLine(const Vector2f &point_on_line, - const float line_bearing, - const Vector2f &vehicle_pos, const Vector2f &ground_vel, const Vector2f &wind_vel) -{ - const Vector2f unit_path_tangent{cosf(line_bearing), sinf(line_bearing)}; - - const Vector2f point_on_line_to_vehicle = vehicle_pos - point_on_line; - _closest_point_on_path = point_on_line + point_on_line_to_vehicle.dot(unit_path_tangent) * unit_path_tangent; - - const float path_curvature = 0.f; - const DirectionalGuidanceOutput sp = _directional_guidance.guideToPath(vehicle_pos, ground_vel, wind_vel, - unit_path_tangent, - _closest_point_on_path, path_curvature); - - return sp; -} - -DirectionalGuidanceOutput FixedWingGuidanceControl::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)}; - - const float path_curvature = loiter_direction_multiplier / radius; - _closest_point_on_path = unit_vec_center_to_closest_pt * radius + loiter_center; - return _directional_guidance.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, - loiter_center + unit_vec_center_to_closest_pt * radius, path_curvature); -} DirectionalGuidanceOutput FixedWingGuidanceControl::navigatePathTangent(const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &position_setpoint, @@ -1276,14 +490,6 @@ DirectionalGuidanceOutput FixedWingGuidanceControl::navigatePathTangent(const ma curvature); } -DirectionalGuidanceOutput FixedWingGuidanceControl::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)}; - _closest_point_on_path = vehicle_pos; - return _directional_guidance.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, vehicle_pos, 0.0f); -} - void FixedWingGuidanceControl::publish_lateral_guidance_status(const hrt_abstime now) { fixed_wing_lateral_guidance_status_s fixed_wing_lateral_guidance_status{}; diff --git a/src/modules/fw_guidance_control/FixedWingGuidanceControl.hpp b/src/modules/fw_guidance_control/FixedWingGuidanceControl.hpp index 6489cc6af3..202dab5d93 100644 --- a/src/modules/fw_guidance_control/FixedWingGuidanceControl.hpp +++ b/src/modules/fw_guidance_control/FixedWingGuidanceControl.hpp @@ -68,11 +68,9 @@ #include #include #include -#include #include #include #include -#include #include #include #include @@ -180,16 +178,11 @@ private: uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; uORB::Publication _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; - uORB::Publication _pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)}; uORB::Publication _launch_detection_status_pub{ORB_ID(launch_detection_status)}; uORB::PublicationMulti _orbit_status_pub{ORB_ID(orbit_status)}; - uORB::Publication _landing_gear_pub {ORB_ID(landing_gear)}; - uORB::Publication _flaps_setpoint_pub{ORB_ID(flaps_setpoint)}; - uORB::Publication _spoilers_setpoint_pub{ORB_ID(spoilers_setpoint)}; uORB::PublicationData _lateral_ctrl_sp_pub{ORB_ID(fixed_wing_lateral_setpoint)}; uORB::PublicationData _longitudinal_ctrl_sp_pub{ORB_ID(fixed_wing_longitudinal_setpoint)}; uORB::Publication _fixed_wing_lateral_guidance_status_pub{ORB_ID(fixed_wing_lateral_guidance_status)}; - uORB::Publication _fixed_wing_runway_control_pub{ORB_ID(fixed_wing_runway_control)}; position_setpoint_triplet_s _pos_sp_triplet{}; vehicle_control_mode_s _control_mode{}; @@ -292,48 +285,15 @@ private: bool _skipping_takeoff_detection{false}; // AUTO LANDING - - // corresponds to param FW_LND_NUDGE - enum LandingNudgingOption { - kNudgingDisabled = 0, - kNudgeApproachAngle, - kNudgeApproachPath - }; - - // [us] Start time of the landing approach. If a fixed-wing landing pattern is used, this timer starts *after any - // orbit to altitude only when the aircraft has entered the final *straight approach. - hrt_abstime _time_started_landing{0}; - // [m] lateral touchdown position offset manually commanded during landing float _lateral_touchdown_position_offset{0.0f}; - // [m] relative vector from land point to approach entrance (NE) - Vector2f _landing_approach_entrance_offset_vector{}; - - // [m] relative height above land point - float _landing_approach_entrance_rel_alt{0.0f}; - - uint8_t _landing_abort_status{position_controller_landing_status_s::NOT_ABORTED}; - - // organize flare states XXX: need to split into a separate class at some point! - struct FlareStates { - bool flaring{false}; - hrt_abstime start_time{0}; // [us] - float initial_height_rate_setpoint{0.0f}; // [m/s] - } _flare_states; - // [m] last terrain estimate which was valid float _last_valid_terrain_alt_estimate{0.0f}; // [us] time at which we had last valid terrain alt hrt_abstime _last_time_terrain_alt_was_valid{0}; - enum TerrainEstimateUseOnLanding { - kDisableTerrainEstimation = 0, - kTriggerFlareWithTerrainEstimate, - kFollowTerrainRelativeLandingGlideSlope - }; - // AIRSPEED float _airspeed_eas{0.f}; @@ -367,13 +327,6 @@ private: // nonlinear path following guidance - lateral-directional position control DirectionalGuidance _directional_guidance; - // LANDING GEAR - int8_t _new_landing_gear_position{landing_gear_s::GEAR_KEEP}; - - // FLAPS/SPOILERS - float _flaps_setpoint{0.f}; - float _spoilers_setpoint{0.f}; - hrt_abstime _time_in_fixed_bank_loiter{0}; // [us] float _min_current_sp_distance_xy{FLT_MAX}; @@ -390,40 +343,20 @@ private: void wind_poll(const hrt_abstime now); - void landing_status_publish(); + /** + * @brief Vehicle control for following a path. + * + * @param control_interval Time since last position control call [s] + * @param curr_pos Current 2D local position vector of vehicle [m] + * @param ground_speed Local 2D ground speed of vehicle [m/s] + * @param pos_sp_prev previous position setpoint + * @param pos_sp_curr current position setpoint + */ + void control_auto_path(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed, + const position_setpoint_s &pos_sp_curr); void publishLocalPositionSetpoint(const position_setpoint_s ¤t_waypoint); - /** - * @brief Sets the landing abort status and publishes landing status. - * - * @param new_abort_status Either 0 (not aborted) or the singular bit >0 which triggered the abort - */ - void updateLandingAbortStatus(const uint8_t new_abort_status = position_controller_landing_status_s::NOT_ABORTED); - - /** - * @brief Checks if the automatic abort bitmask (from FW_LND_ABORT) contains the given abort criterion. - * - * @param automatic_abort_criteria_bitmask Bitmask containing all active abort criteria - * @param landing_abort_criterion The specifc criterion we are checking for - * @return true if the bitmask contains the criterion - */ - bool checkLandingAbortBitMask(const uint8_t automatic_abort_criteria_bitmask, uint8_t landing_abort_criterion); - - /** - * @brief Maps the manual control setpoint (pilot sticks) to height rate commands - * - * @return Manual height rate setpoint [m/s] - */ - float getManualHeightRateSetpoint(); - - /** - * @brief Updates a state indicating whether a manual takeoff has been completed. - * - * Criteria include passing an airspeed threshold and not being in a landed state. VTOL airframes always pass. - */ - void updateManualTakeoffStatus(); - /** * @brief Updates timing information for landed and in-air states. * @@ -431,165 +364,6 @@ private: */ void update_in_air_states(const hrt_abstime now); - /** - * @brief Moves the current position setpoint to a value far ahead of the current vehicle yaw when in a VTOL - * transition. - * - * @param[in,out] current_sp Current position setpoint - */ - void move_position_setpoint_for_vtol_transition(position_setpoint_s ¤t_sp); - - /** - * @brief Changes the position setpoint type to achieve the desired behavior in some instances. - * - * @param pos_sp_curr Current position setpoint - * @return Adjusted position setpoint type - */ - uint8_t handle_setpoint_type(const position_setpoint_s &pos_sp_curr, - const position_setpoint_s &pos_sp_next); - - /* automatic control methods */ - - float get_manual_airspeed_setpoint(); - - void reset_takeoff_state(); - void reset_landing_state(); - - /** - * @brief Decides which control mode to execute. - * - * May also change the position setpoint type depending on the desired behavior. - * - * @param now Current system time [us] - */ - // void set_control_mode_current(const hrt_abstime &now); - - void publishOrbitStatus(const position_setpoint_s pos_sp); - - float getMaxRollAngleNearGround(const float altitude, const float terrain_altitude) const; - - /** - * @brief Calculates the touchdown position for landing with optional manual lateral adjustments. - * - * Manual inputs (from the remote) are used to command a rate at which the position moves and the integrated - * position is bounded. This is useful for manually adjusting the landing point in real time when map or GNSS - * errors cause an offset from the desired landing vector. - * - * @param control_interval Time since the last position control update [s] - * @param local_land_position Originally commanded local land position (NE) [m] - * @return (Nudged) Local touchdown position (NE) [m] - */ - Vector2f calculateTouchdownPosition(const float control_interval, const Vector2f &local_land_position); - - /** - * @brief Calculates the vector from landing approach entrance to touchdown point - * - * NOTE: calculateTouchdownPosition() MUST be called before this method - * - * @return Landing approach vector [m] - */ - Vector2f calculateLandingApproachVector() const; - - /** - * @brief Returns a terrain altitude estimate with consideration of altimeter measurements. - * - * @param now Current system time [us] - * @param land_point_altitude Altitude (AMSL) of the land point [m] - * @param abort_on_terrain_measurement_timeout Abort if distance to ground estimation doesn't get valid when we expect it to - * @param abort_on_terrain_timeout Abort if distance to ground estimation is invalid after being valid before - * @return Terrain altitude (AMSL) [m] - */ - float getLandingTerrainAltitudeEstimate(const hrt_abstime &now, const float land_point_altitude, - const bool abort_on_terrain_measurement_timeout, const bool abort_on_terrain_timeout); - - /** - * @brief Initializes landing states - * - * @param now Current system time [us] - * @param pos_sp_prev Previous position setpoint - * @param land_point_alt Landing point altitude setpoint AMSL [m] - * @param local_position Local aircraft position (NE) [m] - * @param local_land_point Local land point (NE) [m] - */ - void initializeAutoLanding(const hrt_abstime &now, const position_setpoint_s &pos_sp_prev, - const float land_point_alt, 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, steering the vehicle to track - * the line segment between them. - * - * @param[in] start_waypoint Segment starting position in local coordinates. (N,E) [m] - * @param[in] end_waypoint Segment end position in local coordinates. (N,E) [m] - * @param[in] vehicle_pos Vehicle position in local coordinates. (N,E) [m] - * @param[in] ground_vel Vehicle ground velocity vector [m/s] - * @param[in] wind_vel Wind velocity vector [m/s] - */ - DirectionalGuidanceOutput navigateWaypoints(const matrix::Vector2f &start_waypoint, - const matrix::Vector2f &end_waypoint, - const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &ground_vel, - const matrix::Vector2f &wind_vel); - - /* - * Takes one waypoint and steers the vehicle towards this. - * - * NOTE: this *will lead to "flowering" behavior if no higher level state machine or - * switching condition changes the waypoint. - * - * @param[in] waypoint_pos Waypoint position in local coordinates. (N,E) [m] - * @param[in] vehicle_pos Vehicle position in local coordinates. (N,E) [m] - * @param[in] ground_vel Vehicle ground velocity vector [m/s] - * @param[in] wind_vel Wind velocity vector [m/s] - */ - DirectionalGuidanceOutput navigateWaypoint(const matrix::Vector2f &waypoint_pos, const matrix::Vector2f &vehicle_pos, - const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel); - - /* - * Line (infinite) following logic. Two points on the line are used to define the - * line in 2D space (first to second point determines the direction). Determines the - * relevant parameters for evaluating the NPFG guidance law, then updates control setpoints. - * - * @param[in] point_on_line_1 Arbitrary first position on line in local coordinates. (N,E) [m] - * @param[in] point_on_line_2 Arbitrary second position on line in local coordinates. (N,E) [m] - * @param[in] vehicle_pos Vehicle position in local coordinates. (N,E) [m] - * @param[in] ground_vel Vehicle ground velocity vector [m/s] - * @param[in] wind_vel Wind velocity vector [m/s] - */ - DirectionalGuidanceOutput navigateLine(const Vector2f &point_on_line_1, const Vector2f &point_on_line_2, - const Vector2f &vehicle_pos, - const Vector2f &ground_vel, const Vector2f &wind_vel); - - /* - * Line (infinite) following logic. One point on the line and a line bearing are used to define - * the line in 2D space. Determines the relevant parameters for evaluating the NPFG guidance law, - * then updates control setpoints. - * - * @param[in] point_on_line Arbitrary position on line in local coordinates. (N,E) [m] - * @param[in] line_bearing Line bearing [rad] (from north) - * @param[in] vehicle_pos Vehicle position in local coordinates. (N,E) [m] - * @param[in] ground_vel Vehicle ground velocity vector [m/s] - * @param[in] wind_vel Wind velocity vector [m/s] - */ - DirectionalGuidanceOutput navigateLine(const Vector2f &point_on_line, const float line_bearing, - const Vector2f &vehicle_pos, - const Vector2f &ground_vel, const 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 local coordinates. (N,E) [m] - * @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] - */ - DirectionalGuidanceOutput 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. @@ -608,26 +382,8 @@ private: 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. - * - * @param[in] vehicle_pos vehicle_pos Vehicle position in local coordinates. (N,E) [m] - * @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] - */ - DirectionalGuidanceOutput navigateBearing(const matrix::Vector2f &vehicle_pos, float bearing, - const matrix::Vector2f &ground_vel, - const matrix::Vector2f &wind_vel); - - void control_idle(); void publish_lateral_guidance_status(const hrt_abstime now); - float rollAngleToLateralAccel(float roll_body) const; - DEFINE_PARAMETERS( (ParamFloat) _param_fw_r_lim,