yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00:00
Code Issues 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

37
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
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
// 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> &current_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);