forked from Archive/PX4-Autopilot
FWPositionController: add support for circular landings
Add method for circular landing, that is used instead of the straight fixed-wing landings in case the landing is not part of a mission landing. Use straight landing if previous WP is valid, and the ciruclar otherwise. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
parent
50d75c537e
commit
c47210fc77
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue