diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index e8e3c8cbe6..c93ab99517 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -283,7 +283,7 @@ MissionBlock::is_mission_item_reached_or_completed() // check if within loiter radius around wp, if yes then set altitude sp to mission item if (dist >= 0.0f && dist_xy <= (_navigator->get_acceptance_radius() + fabsf(_mission_item.loiter_radius)) - && dist_z <= _navigator->get_default_altitude_acceptance_radius()) { + && dist_z <= _navigator->get_altitude_acceptance_radius()) { curr_sp->alt = mission_item_altitude_amsl; curr_sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index b0e84064f0..004f8e1f4a 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -200,13 +200,6 @@ public: */ float get_acceptance_radius(); - /** - * Get the default altitude acceptance radius (i.e. from parameters) - * - * @return the distance from the target altitude before considering the waypoint reached - */ - float get_default_altitude_acceptance_radius(); - /** * Get the altitude acceptance radius * diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index b414555c63..565ec28ac1 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -988,10 +988,18 @@ float Navigator::get_default_acceptance_radius() return _param_nav_acc_rad.get(); } -float Navigator::get_default_altitude_acceptance_radius() +float Navigator::get_altitude_acceptance_radius() { if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - return _param_nav_fw_alt_rad.get(); + const position_setpoint_s &next_sp = get_position_setpoint_triplet()->next; + + if (!force_vtol() && next_sp.type == position_setpoint_s::SETPOINT_TYPE_LAND && next_sp.valid) { + // Use separate (tighter) altitude acceptance for clean altitude starting point before FW landing + return _param_nav_fw_altl_rad.get(); + + } else { + return _param_nav_fw_alt_rad.get(); + } } else if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) { return INFINITY; @@ -1010,20 +1018,6 @@ float Navigator::get_default_altitude_acceptance_radius() } } -float Navigator::get_altitude_acceptance_radius() -{ - if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - const position_setpoint_s &next_sp = get_position_setpoint_triplet()->next; - - if (next_sp.type == position_setpoint_s::SETPOINT_TYPE_LAND && next_sp.valid) { - // Use separate (tighter) altitude acceptance for clean altitude starting point before landing - return _param_nav_fw_altl_rad.get(); - } - } - - return get_default_altitude_acceptance_radius(); -} - float Navigator::get_cruising_speed() { /* there are three options: The mission-requested cruise speed, or the current hover / plane speed */