From 44be415e0e9cce211941489ef9b8e31f39073e7e Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 16 May 2022 11:42:37 +0200 Subject: [PATCH] mission_block: make "Delay until" item work while landed To enable landing, waiting and taking off again e.g. for delivery. --- src/modules/navigator/mission_block.cpp | 23 +++++++++++++---------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 03be49bcc5..a3c8baf544 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -74,6 +74,8 @@ MissionBlock::is_mission_item_reached() { /* handle non-navigation or indefinite waypoints */ + hrt_abstime now = hrt_absolute_time(); + switch (_mission_item.nav_cmd) { case NAV_CMD_DO_SET_SERVO: return true; @@ -108,6 +110,8 @@ MissionBlock::is_mission_item_reached() case NAV_CMD_SET_CAMERA_MODE: case NAV_CMD_SET_CAMERA_ZOOM: case NAV_CMD_SET_CAMERA_FOCUS: + case NAV_CMD_DO_CHANGE_SPEED: + case NAV_CMD_DO_SET_HOME: return true; case NAV_CMD_DO_VTOL_TRANSITION: @@ -125,17 +129,21 @@ MissionBlock::is_mission_item_reached() return false; } - case NAV_CMD_DO_CHANGE_SPEED: - case NAV_CMD_DO_SET_HOME: - return true; + case NAV_CMD_DELAY: + // Set reached flags directly such that only the delay time is considered + _waypoint_position_reached = true; + _waypoint_yaw_reached = true; + + // Set timestamp when entering only (it's reset to 0 for every waypoint) + if (_time_wp_reached == 0) { + _time_wp_reached = now; + } default: /* do nothing, this is a 3D waypoint */ break; } - hrt_abstime now = hrt_absolute_time(); - if (!_navigator->get_land_detected()->landed && !_waypoint_position_reached) { float dist = -1.0f; @@ -268,11 +276,6 @@ MissionBlock::is_mission_item_reached() } } - } else if (_mission_item.nav_cmd == NAV_CMD_DELAY) { - _waypoint_position_reached = true; - _waypoint_yaw_reached = true; - _time_wp_reached = now; - } else { float acceptance_radius = _navigator->get_acceptance_radius();