diff options
Diffstat (limited to 'src/modules/navigator/navigator_main.cpp')
| -rw-r--r-- | src/modules/navigator/navigator_main.cpp | 10 |
1 files changed, 8 insertions, 2 deletions
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 170e5df05..73b7c13d0 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -685,7 +685,8 @@ Navigator::task_main() /* RC signal available, use control switches to set mode */ /* RETURN switch, overrides MISSION switch */ if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + /* switch to RTL if not already landed after RTL and home position set */ + if ((myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) && _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } @@ -742,7 +743,7 @@ Navigator::task_main() break; case NAV_STATE_RTL: - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + if ((myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) && _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } @@ -815,6 +816,11 @@ 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) { + _pos_sp_triplet_updated = true; + } + /* only check if waypoint has been reached in MISSION and RTL modes */ if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { if (check_mission_item_reached()) { |