Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 11:06:04 +08:00
Code Issues Actions 16 Packages Projects Releases Wiki Activity

fw_pos_control_l1 force heading hold at flare

Browse Source
This commit is contained in:
Daniel Agar
2017-02-07 01:09:53 -05:00
committed by Lorenz Meier
parent 94d6a92f41
commit ff68d63143
1 changed files with 17 additions and 20 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
+9 -12
View File
@@ -1509,32 +1509,27 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
// we want the plane to keep tracking the desired flight path until we start flaring // 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 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 (!_land_noreturn_horizontal &&
if (wp_distance < _parameters.land_heading_hold_horizontal_distance || _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 (!_land_noreturn_horizontal) {
// set target_bearing in first occurrence
if (pos_sp_triplet.previous.valid) { if (pos_sp_triplet.previous.valid) {
/* heading hold, along the line connecting this and the last waypoint */
_target_bearing = bearing_lastwp_currwp; _target_bearing = bearing_lastwp_currwp;
} else { } else {
_target_bearing = _yaw; _target_bearing = _yaw;
} }
_land_noreturn_horizontal = true;
mavlink_log_info(&_mavlink_log_pub, "#Landing, heading hold"); mavlink_log_info(&_mavlink_log_pub, "#Landing, heading hold");
} }
// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_yaw)); if (_land_noreturn_horizontal) {
// heading hold
_l1_control.navigate_heading(_target_bearing, _yaw, nav_speed_2d); _l1_control.navigate_heading(_target_bearing, _yaw, nav_speed_2d);
_land_noreturn_horizontal = true;
} else { } else {
// normal navigation
/* normal navigation */
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, nav_speed_2d); _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, nav_speed_2d);
} }
@@ -1622,8 +1617,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
throttle_max = _parameters.throttle_max; throttle_max = _parameters.throttle_max;
/* enable direct yaw control using rudder/wheel */ /* enable direct yaw control using rudder/wheel */
if (_land_noreturn_horizontal) {
_att_sp.yaw_body = _target_bearing; _att_sp.yaw_body = _target_bearing;
_att_sp.fw_control_yaw = true; _att_sp.fw_control_yaw = true;
}
if (_global_pos.alt < terrain_alt + _landingslope.motor_lim_relative_alt() || _land_motor_lim) { if (_global_pos.alt < terrain_alt + _landingslope.motor_lim_relative_alt() || _land_motor_lim) {
throttle_max = math::min(throttle_max, _parameters.throttle_land_max); throttle_max = math::min(throttle_max, _parameters.throttle_land_max);