diff --git a/src/modules/fw_path_navigation/FixedwingPositionControl.cpp b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp index b680102799..b31ea75c1b 100644 --- a/src/modules/fw_path_navigation/FixedwingPositionControl.cpp +++ b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -734,7 +734,16 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now) } else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { if (!_vehicle_status.in_transition_mode) { - _control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING; + + // 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 { // in this case we want the waypoint handled as a position setpoint -- a submode in control_auto() @@ -1498,7 +1507,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo } void -FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const float control_interval, +FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, const float control_interval, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr) { // first handle non-position things like airspeed and tecs settings @@ -1573,7 +1582,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo } // ramp in flare limits and setpoints with the flare time, command a soft touchdown - const float seconds_since_flare_start = hrt_elapsed_time(&_flare_states.start_time) / float(1_s); + const float seconds_since_flare_start = hrt_elapsed_time(&_flare_states.start_time) * 1.e-6f; const float flare_ramp_interpolator = math::constrain(seconds_since_flare_start / _param_fw_lnd_fl_time.get(), 0.0f, 1.0f); @@ -1691,7 +1700,207 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo /* set the attitude and throttle commands */ - // TECS has authority (though constrained) over pitch during flare, throttle is killed + _att_sp.pitch_body = get_tecs_pitch(); + + // yaw is not controlled in nominal flight + _att_sp.yaw_body = _yaw; + + // enable direct yaw control using rudder/wheel + _att_sp.fw_control_yaw_wheel = false; + + _att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_idle.get() : get_tecs_thrust(); + } + + _tecs.set_equivalent_airspeed_min(_param_fw_airspd_min.get()); // reset after TECS calculation + + _att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, terrain_alt); + + // Apply flaps and spoilers for landing. Amount of deflection is handled in the FW attitdue controller + _att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_LAND; + _att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_LAND; + + if (!_vehicle_status.in_transition_to_fw) { + publishLocalPositionSetpoint(pos_sp_curr); + } + + landing_status_publish(); +} + +void +FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, const float control_interval, + const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr) +{ + // first handle non-position things like airspeed and tecs settings + const float airspeed_land = (_param_fw_lnd_airspd.get() > FLT_EPSILON) ? _param_fw_lnd_airspd.get() : + _param_fw_airspd_min.get(); + float adjusted_min_airspeed = _param_fw_airspd_min.get(); + + if (airspeed_land < _param_fw_airspd_min.get()) { + // adjust underspeed detection bounds for landing airspeed + _tecs.set_equivalent_airspeed_min(airspeed_land); + adjusted_min_airspeed = airspeed_land; + } + + float target_airspeed = adapt_airspeed_setpoint(control_interval, airspeed_land, adjusted_min_airspeed, + ground_speed); + + // Enable tighter altitude control for landings + _tecs.set_altitude_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get()); + + const Vector2f local_position{_local_pos.x, _local_pos.y}; + Vector2f local_landing_orbit_center = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon); + + if (_time_started_landing == 0) { + // save time at which we started landing and reset landing abort status + reset_landing_state(); + _time_started_landing = now; + } + + const float terrain_alt = getLandingTerrainAltitudeEstimate(now, pos_sp_curr.alt); + + // flare at the maximum of the altitude determined by the time before touchdown and a minimum flare altitude + const float flare_rel_alt = math::max(_param_fw_lnd_fl_time.get() * _local_pos.vz, _param_fw_lnd_flalt.get()); + + float loiter_radius = pos_sp_curr.loiter_radius; + + if (fabsf(pos_sp_curr.loiter_radius) < FLT_EPSILON) { + loiter_radius = _param_nav_loiter_rad.get(); + } + + // the terrain estimate (if enabled) is always used to determine the flaring altitude + if ((_current_altitude < terrain_alt + flare_rel_alt) || _flare_states.flaring) { + // flare and land with minimal speed + + // flaring is a "point of no return" + if (!_flare_states.flaring) { + _flare_states.flaring = true; + _flare_states.start_time = now; + _flare_states.initial_height_rate_setpoint = -_local_pos.vz; + _flare_states.initial_throttle_setpoint = _att_sp.thrust_body[0]; + events::send(events::ID("fixedwing_position_control_landing_circle_flaring"), events::Log::Info, + "Landing, flaring"); + } + + // ramp in flare limits and setpoints with the flare time, command a soft touchdown + const float seconds_since_flare_start = hrt_elapsed_time(&_flare_states.start_time) * 1.e-6f; + const float flare_ramp_interpolator = math::constrain(seconds_since_flare_start / _param_fw_lnd_fl_time.get(), 0.0f, + 1.0f); + + /* lateral guidance first, because npfg will adjust the airspeed setpoint if necessary */ + + // tune up the lateral position control guidance when on the ground + const float ground_roll_npfg_period = flare_ramp_interpolator * _param_rwto_npfg_period.get() + + (1.0f - flare_ramp_interpolator) * _param_npfg_period.get(); + + _npfg.setPeriod(ground_roll_npfg_period); + _npfg.setAirspeedNom(target_airspeed * _eas2tas); + _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); + + navigateLoiter(local_landing_orbit_center, local_position, loiter_radius, + pos_sp_curr.loiter_direction_counter_clockwise, + ground_speed, _wind_vel); + target_airspeed = _npfg.getAirspeedRef() / _eas2tas; + _att_sp.roll_body = _npfg.getRollSetpoint(); + + _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + + /* longitudinal guidance */ + + const float flare_ramp_interpolator_sqrt = sqrtf(flare_ramp_interpolator); + + const float height_rate_setpoint = flare_ramp_interpolator_sqrt * (-_param_fw_lnd_fl_sink.get()) + + (1.0f - flare_ramp_interpolator_sqrt) * _flare_states.initial_height_rate_setpoint; + + float pitch_min_rad = flare_ramp_interpolator_sqrt * radians(_param_fw_lnd_fl_pmin.get()) + + (1.0f - flare_ramp_interpolator_sqrt) * radians(_param_fw_p_lim_min.get()); + float pitch_max_rad = flare_ramp_interpolator_sqrt * radians(_param_fw_lnd_fl_pmax.get()) + + (1.0f - flare_ramp_interpolator_sqrt) * radians(_param_fw_p_lim_max.get()); + + if (_param_fw_lnd_td_time.get() > FLT_EPSILON) { + const float touchdown_time = math::max(_param_fw_lnd_td_time.get(), _param_fw_lnd_fl_time.get()); + + const float touchdown_interpolator = math::constrain((seconds_since_flare_start - touchdown_time) / + POST_TOUCHDOWN_CLAMP_TIME, 0.0f, 1.0f); + + pitch_max_rad = touchdown_interpolator * _param_rwto_psp.get() + (1.0f - touchdown_interpolator) * pitch_max_rad; + pitch_min_rad = touchdown_interpolator * _param_rwto_psp.get() + (1.0f - touchdown_interpolator) * pitch_min_rad; + } + + // idle throttle may be >0 for internal combustion engines + // normally set to zero for electric motors + const float throttle_max = flare_ramp_interpolator_sqrt * _param_fw_thr_idle.get() + + (1.0f - flare_ramp_interpolator_sqrt) * + _param_fw_thr_max.get(); + + tecs_update_pitch_throttle(control_interval, + _current_altitude, // is not controlled, control descend rate + target_airspeed, + pitch_min_rad, + pitch_max_rad, + _param_fw_thr_idle.get(), + throttle_max, + _param_sinkrate_target.get(), + _param_climbrate_target.get(), + true, + height_rate_setpoint); + + /* set the attitude and throttle commands */ + + // TECS has authority (though constrained) over pitch during flare, throttle is hard set to idle + _att_sp.pitch_body = get_tecs_pitch(); + + // enable direct yaw control using rudder/wheel + _att_sp.fw_control_yaw_wheel = true; + + // XXX: hacky way to pass through manual nose-wheel incrementing. need to clean this interface. + if (_param_fw_lnd_nudge.get() > LandingNudgingOption::kNudgingDisabled) { + _att_sp.yaw_sp_move_rate = _manual_control_setpoint.yaw; + } + + // blend the height rate controlled throttle setpoints with initial throttle setting over the flare + // ramp time period to maintain throttle command continuity when switching from altitude to height rate + // control + const float blended_throttle = flare_ramp_interpolator * get_tecs_thrust() + (1.0f - flare_ramp_interpolator) * + _flare_states.initial_throttle_setpoint; + + _att_sp.thrust_body[0] = blended_throttle; + + } else { + + // follow the glide slope + + /* lateral guidance */ + _npfg.setAirspeedNom(target_airspeed * _eas2tas); + _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); + + navigateLoiter(local_landing_orbit_center, local_position, loiter_radius, + pos_sp_curr.loiter_direction_counter_clockwise, + ground_speed, _wind_vel); + target_airspeed = _npfg.getAirspeedRef() / _eas2tas; + _att_sp.roll_body = _npfg.getRollSetpoint(); + + /* longitudinal guidance */ + + // open the desired max sink rate to encompass the glide slope if within the aircraft's performance limits + // x/sqrt(x^2+1) = sin(arctan(x)) + const float glide_slope = math::radians(_param_fw_lnd_ang.get()); + const float glide_slope_sink_rate = airspeed_land * glide_slope / sqrtf(glide_slope * glide_slope + 1.0f); + const float desired_max_sinkrate = math::min(math::max(glide_slope_sink_rate, _param_sinkrate_target.get()), + _param_fw_t_sink_max.get()); + tecs_update_pitch_throttle(control_interval, + _current_altitude, // is not controlled, control descend rate + target_airspeed, + radians(_param_fw_p_lim_min.get()), + radians(_param_fw_p_lim_max.get()), + _param_fw_thr_min.get(), + _param_fw_thr_max.get(), + desired_max_sinkrate, + _param_climbrate_target.get(), + false, + -glide_slope_sink_rate); // heightrate = -sinkrate + + /* set the attitude and throttle commands */ + _att_sp.pitch_body = get_tecs_pitch(); // yaw is not controlled in nominal flight @@ -2086,7 +2295,8 @@ FixedwingPositionControl::Run() // default to zero - is used (IN A HACKY WAY) to pass direct nose wheel steering via yaw stick to the actuators during auto takeoff _att_sp.yaw_sp_move_rate = 0.0f; - if (_control_mode_current != FW_POSCTRL_MODE_AUTO_LANDING) { + if (_control_mode_current != FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT + && _control_mode_current != FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR) { reset_landing_state(); } @@ -2111,9 +2321,14 @@ FixedwingPositionControl::Run() break; } - case FW_POSCTRL_MODE_AUTO_LANDING: { - control_auto_landing(_local_pos.timestamp, control_interval, ground_speed, _pos_sp_triplet.previous, - _pos_sp_triplet.current); + case FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT: { + control_auto_landing_straight(_local_pos.timestamp, control_interval, ground_speed, _pos_sp_triplet.previous, + _pos_sp_triplet.current); + break; + } + + case FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR: { + control_auto_landing_circular(_local_pos.timestamp, control_interval, ground_speed, _pos_sp_triplet.current); break; } @@ -2361,7 +2576,7 @@ FixedwingPositionControl::calculateTakeoffBearingVector(const Vector2f &launch_p void FixedwingPositionControl::initializeAutoLanding(const hrt_abstime &now, const position_setpoint_s &pos_sp_prev, - const float land_point_altitdue, const Vector2f &local_position, const Vector2f &local_land_point) + const float land_point_altitude, const Vector2f &local_position, const Vector2f &local_land_point) { if (_time_started_landing == 0) { @@ -2372,7 +2587,7 @@ FixedwingPositionControl::initializeAutoLanding(const hrt_abstime &now, const po // 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_altitdue; + 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 { @@ -2385,7 +2600,7 @@ FixedwingPositionControl::initializeAutoLanding(const hrt_abstime &now, const po // 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_altitdue; + height_above_land_point = _current_altitude - land_point_altitude; local_approach_entrance = local_position; } @@ -2400,7 +2615,7 @@ FixedwingPositionControl::initializeAutoLanding(const hrt_abstime &now, const po 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() method inappropriately + // 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; } diff --git a/src/modules/fw_path_navigation/FixedwingPositionControl.hpp b/src/modules/fw_path_navigation/FixedwingPositionControl.hpp index a37512165c..380ff88d71 100644 --- a/src/modules/fw_path_navigation/FixedwingPositionControl.hpp +++ b/src/modules/fw_path_navigation/FixedwingPositionControl.hpp @@ -236,7 +236,8 @@ private: FW_POSCTRL_MODE_AUTO_ALTITUDE, FW_POSCTRL_MODE_AUTO_CLIMBRATE, FW_POSCTRL_MODE_AUTO_TAKEOFF, - FW_POSCTRL_MODE_AUTO_LANDING, + FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT, + FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR, FW_POSCTRL_MODE_MANUAL_POSITION, FW_POSCTRL_MODE_MANUAL_ALTITUDE, FW_POSCTRL_MODE_OTHER @@ -596,7 +597,9 @@ private: const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr); /** - * @brief Controls automatic landing. + * @brief Controls automatic landing with straight approach. + * + * To be used in Missions that contain a loiter down followed by a land waypoint. * * @param now Current system time [us] * @param control_interval Time since last position control call [s] @@ -605,8 +608,22 @@ private: * @param pos_sp_prev previous position setpoint * @param pos_sp_curr current position setpoint */ - void control_auto_landing(const hrt_abstime &now, const float control_interval, const Vector2f &ground_speed, - const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr); + void control_auto_landing_straight(const hrt_abstime &now, const float control_interval, const Vector2f &ground_speed, + const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr); + + /** + * @brief Controls automatic landing with circular final appraoch. + * + * To be used outside of Mission landings. Vehicle will orbit down around the landing position setpoint until flaring. + * + * @param now Current system time [us] + * @param control_interval Time since last position control call [s] + * @param control_interval Time since the last position control update [s] + * @param ground_speed Local 2D ground speed of vehicle [m/s] + * @param pos_sp_curr current position setpoint + */ + void control_auto_landing_circular(const hrt_abstime &now, const float control_interval, const Vector2f &ground_speed, + const position_setpoint_s &pos_sp_curr); /* manual control methods */ diff --git a/src/modules/fw_path_navigation/runway_takeoff/RunwayTakeoff.cpp b/src/modules/fw_path_navigation/runway_takeoff/RunwayTakeoff.cpp index 9b67f3a3c3..7cfc23b438 100644 --- a/src/modules/fw_path_navigation/runway_takeoff/RunwayTakeoff.cpp +++ b/src/modules/fw_path_navigation/runway_takeoff/RunwayTakeoff.cpp @@ -214,7 +214,7 @@ void RunwayTakeoff::reset() float RunwayTakeoff::interpolateValuesOverAbsoluteTime(const float start_value, const float end_value, const hrt_abstime &start_time, const float interpolation_time) const { - const float seconds_since_start = hrt_elapsed_time(&start_time) / float(1_s); + const float seconds_since_start = hrt_elapsed_time(&start_time) * 1.e-6f; const float interpolator = math::constrain(seconds_since_start / interpolation_time, 0.0f, 1.0f); return interpolator * end_value + (1.0f - interpolator) * start_value; diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index 0c5fb3a50c..ca6b6da426 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -94,4 +94,20 @@ Land::on_active() mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); _navigator->set_position_setpoint_triplet_updated(); } + + /* check if landing needs to be aborted */ + if (_navigator->abort_landing()) { + + // send reposition cmd to get out of land mode (will loiter at current position and altitude) + vehicle_command_s vcmd = {}; + + vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_REPOSITION; + vcmd.param1 = -1; + vcmd.param2 = 1; + vcmd.param5 = _navigator->get_global_position()->lat; + vcmd.param6 = _navigator->get_global_position()->lon; + vcmd.param7 = _navigator->get_global_position()->alt; + + _navigator->publish_vehicle_cmd(&vcmd); + } }