From ec4ccc1fcddba14f8be66bbce0a907555c622ca1 Mon Sep 17 00:00:00 2001 From: Philipp Oettershagen Date: Sat, 16 Jun 2018 12:28:45 +0200 Subject: [PATCH] Fixed-wing autoland: Use a more appropriate (i.e. mostly tighter) altitude acceptance radius than just the standard altitude acceptance parameter (which may be too large to allow a precise autoland) --- src/modules/navigator/mission_block.cpp | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 97f4c04420..b0860b9575 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -251,15 +251,26 @@ MissionBlock::is_mission_item_reached() } } else { + // Determine altitude acceptance radius. By default, this is taken from the respective parameter. + // However, before a landing approach the acceptance radius needs to be tighter: Assume a 30% error + // w.r.t. the remaining descent altitude is OK, but enforce min/max values (e.g. min=3m to make + // sure that the waypoint can still be reached in case of wrong user input). + float alt_accept_rad = _navigator->get_altitude_acceptance_radius(); + struct position_setpoint_s next_sp = _navigator->get_position_setpoint_triplet()->next; + + if (next_sp.type == position_setpoint_s::SETPOINT_TYPE_LAND && next_sp.valid) { + alt_accept_rad = math::constrain(0.3f * (curr_sp->alt - next_sp.alt), 3.0f, + _navigator->get_altitude_acceptance_radius()); + PX4_INFO("Accept:%5.3f", double(alt_accept_rad)); + } + if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f) - && dist_z <= _navigator->get_altitude_acceptance_radius()) { + && dist_z <= alt_accept_rad) { _waypoint_position_reached = true; // set required yaw from bearing to the next mission item if (_mission_item.force_heading) { - struct position_setpoint_s next_sp = _navigator->get_position_setpoint_triplet()->next; - if (next_sp.valid) { _mission_item.yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,