fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-17 03:07:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

fw landing: horiz flare check: fix wp distance

Browse Source 
better calculation of wp distance when behind wp
This commit is contained in:
Thomas Gubler
2014-09-04 11:11:03 +02:00
parent 611f2b3633
commit bf3f8861dd
1 changed files with 11 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+11 -3
View File
@@ -909,10 +909,17 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
/* Horizontal landing control */
/* switch to heading hold for the last meters, continue heading hold after */
float wp_distance = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
/* calculate a waypoint distance value which is 0 when the aircraft is behind the waypoint */
float wp_distance_save = wp_distance;
if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) >= math::radians(90.0f)) {
wp_distance_save = 0.0f;
}
//warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO");
if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) {
@@ -964,13 +971,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
float L_altitude_rel = _pos_sp_triplet.previous.valid ?
_pos_sp_triplet.previous.alt - terrain_alt : 0.0f;
float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp);
/* Check if we should start flaring with a vertical and a
* horizontal limit (with some tolerance) */
* horizontal limit (with some tolerance)
* The horizontal limit is only applied when we are in front of the wp
*/
if (((_global_pos.alt < terrain_alt + landingslope.flare_relative_alt()) &&
(wp_distance < landingslope.flare_length() + 5.0f)) ||
(wp_distance_save < landingslope.flare_length() + 5.0f)) ||
land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out
/* land with minimal speed */