From ff68d63143d93ab1916b4a70db6cdf2979fb4fea Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 7 Feb 2017 01:09:53 -0500 Subject: [PATCH] fw_pos_control_l1 force heading hold at flare --- .../fw_pos_control_l1_main.cpp | 37 +++++++++---------- 1 file changed, 17 insertions(+), 20 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 6be83db41c..e0e94480f0 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1509,32 +1509,27 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // 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 (land_noreturn_vertical) { - if (wp_distance < _parameters.land_heading_hold_horizontal_distance || _land_noreturn_horizontal) { + if (!_land_noreturn_horizontal && + ((wp_distance < _parameters.land_heading_hold_horizontal_distance) || _land_noreturn_vertical)) { - /* heading hold, along the line connecting this and the last waypoint */ + if (pos_sp_triplet.previous.valid) { + /* heading hold, along the line connecting this and the last waypoint */ + _target_bearing = bearing_lastwp_currwp; - if (!_land_noreturn_horizontal) { - // set target_bearing in first occurrence - if (pos_sp_triplet.previous.valid) { - _target_bearing = bearing_lastwp_currwp; - - } else { - _target_bearing = _yaw; - } - - mavlink_log_info(&_mavlink_log_pub, "#Landing, heading hold"); + } else { + _target_bearing = _yaw; } -// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_yaw)); + _land_noreturn_horizontal = true; + mavlink_log_info(&_mavlink_log_pub, "#Landing, heading hold"); + } + if (_land_noreturn_horizontal) { + // heading hold _l1_control.navigate_heading(_target_bearing, _yaw, nav_speed_2d); - _land_noreturn_horizontal = true; - } else { - - /* normal navigation */ + // normal navigation _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, nav_speed_2d); } @@ -1622,8 +1617,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi throttle_max = _parameters.throttle_max; /* enable direct yaw control using rudder/wheel */ - _att_sp.yaw_body = _target_bearing; - _att_sp.fw_control_yaw = true; + if (_land_noreturn_horizontal) { + _att_sp.yaw_body = _target_bearing; + _att_sp.fw_control_yaw = true; + } if (_global_pos.alt < terrain_alt + _landingslope.motor_lim_relative_alt() || _land_motor_lim) { throttle_max = math::min(throttle_max, _parameters.throttle_land_max);