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

fw_pos_control_l1 project virtual waypoint to handle landing overshoot

Browse Source
This commit is contained in:
Daniel Agar
2018-03-14 16:39:19 -04:00
parent d1dca4e74c
commit 7607c71ca8
1 changed files with 20 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+20 -3
View File
@@ -1269,6 +1269,21 @@ FixedwingPositionControl::control_landing(const math::Vector<2> &curr_pos, const
wp_distance_save = 0.0f;
}
// create virtual waypoint which is on the desired flight path but
// some distance behind landing waypoint. This will make sure that the plane
// will always follow the desired flight path even if we get close or past
// the landing waypoint
if (pos_sp_prev.valid) {
double lat = pos_sp_curr.lat;
double lon = pos_sp_curr.lon;
create_waypoint_from_line_and_dist(pos_sp_curr.lat, pos_sp_curr.lon,
pos_sp_prev.lat, pos_sp_prev.lon, -1000.0f, &lat, &lon);
curr_wp(0) = (float)lat;
curr_wp(1) = (float)lon;
}
// we want the plane to keep tracking the desired flight path until we start flaring
// if we go into heading hold mode earlier then we risk to be pushed away from the runway by cross winds
if ((_parameters.land_heading_hold_horizontal_distance > 0.0f) && !_land_noreturn_horizontal &&
@@ -1305,9 +1320,6 @@ FixedwingPositionControl::control_landing(const math::Vector<2> &curr_pos, const
/* Vertical landing control */
/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
float airspeed_land = _parameters.land_airspeed_scale * _parameters.airspeed_min;
float airspeed_approach = _parameters.land_airspeed_scale * _parameters.airspeed_min;
// default to no terrain estimation, just use landing waypoint altitude
float terrain_alt = pos_sp_curr.alt;
@@ -1396,6 +1408,9 @@ FixedwingPositionControl::control_landing(const math::Vector<2> &curr_pos, const
_land_stayonground = true;
}
const float airspeed_land = _parameters.land_airspeed_scale * _parameters.airspeed_min;
const float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel,
calculate_target_airspeed(airspeed_land),
radians(_parameters.land_flare_pitch_min_deg),
@@ -1457,6 +1472,8 @@ FixedwingPositionControl::control_landing(const math::Vector<2> &curr_pos, const
}
}
const float airspeed_approach = _parameters.land_airspeed_scale * _parameters.airspeed_min;
tecs_update_pitch_throttle(terrain_alt + altitude_desired_rel,
calculate_target_airspeed(airspeed_approach),
radians(_parameters.pitch_limit_min),