diff options
Diffstat (limited to 'src/modules/navigator')
| -rw-r--r-- | src/modules/navigator/navigator_main.cpp | 12 |
1 files changed, 11 insertions, 1 deletions
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index ca71aab70..12fd35a0a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -692,6 +692,9 @@ Navigator::task_main() /* evaluate state requested by commander */ if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { + /* publish position setpoint triplet on each status update if navigator active */ + _pos_sp_triplet_updated = true; + if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { /* commander requested new navigation mode, try to set it */ switch (_vstatus.set_nav_state) { @@ -733,6 +736,13 @@ Navigator::task_main() } } + /* check if waypoint has been reached in MISSION, RTL and LAND modes */ + if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) { + if (check_mission_item_reached()) { + on_mission_item_reached(); + } + } + } else { /* navigator shouldn't act */ dispatch(EVENT_NONE_REQUESTED); @@ -777,8 +787,8 @@ Navigator::task_main() if (fds[1].revents & POLLIN) { global_position_update(); - /* publish position setpoint triplet on each position update if navigator active */ if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { + /* publish position setpoint triplet on each position update if navigator active */ _pos_sp_triplet_updated = true; if (myState == NAV_STATE_LAND && !_global_pos_valid) { |