From 59c25aa26ba510a54a6c23a59995b0dc92a6d023 Mon Sep 17 00:00:00 2001 From: Tomas Twardzik Date: Tue, 14 Jan 2025 11:52:57 +0100 Subject: [PATCH] [fix] Making altitude acceptance radius equal to infinity [fix] Adding boat condition for NAV_CMD_TAKEOFF --- src/modules/navigator/mission_block.cpp | 5 +++-- src/modules/navigator/navigator_main.cpp | 3 ++- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 1b05628a6c..db881b731a 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -206,8 +206,9 @@ MissionBlock::is_mission_item_reached_or_completed() _waypoint_position_reached = true; } - } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF - && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) { + } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && + (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER || + _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_BOAT)) { // Accept takeoff waypoint to be reached if the distance in 2D plane is within acceptance radius if (dist_xy >= 0.0f && dist_xy <= _navigator->get_acceptance_radius()) { _waypoint_position_reached = true; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 201eeb6123..06d38c4503 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1139,7 +1139,8 @@ float Navigator::get_altitude_acceptance_radius() } else if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) { return INFINITY; - + } else if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_BOAT) { + return INFINITY; } else { float alt_acceptance_radius = _param_nav_mc_alt_rad.get();