fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 03:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

mission_block: make "Delay until" item work while landed

Browse Source 
To enable landing, waiting and taking off again e.g. for delivery.
This commit is contained in:
Matthias Grob
2022-05-24 13:42:03 +02:00
parent 0be474430c
commit 44be415e0e
1 changed files with 13 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_block.cpp
+13 -10
View File
@@ -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();