diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2014-01-28 21:44:46 +0100 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2014-01-28 21:44:46 +0100 |
| commit | 23a87f5a5204580b4fad435a75b38efc9d15c05c (patch) | |
| tree | fbd3586a967e8b11bc4e9b9f61f04a75edd2c419 /src/modules/navigator | |
| parent | 6a1a29f77ecc9ded341bfbca037c9a6768ed3fb4 (diff) | |
| download | px4-firmware-23a87f5a5204580b4fad435a75b38efc9d15c05c.tar.gz px4-firmware-23a87f5a5204580b4fad435a75b38efc9d15c05c.tar.bz2 px4-firmware-23a87f5a5204580b4fad435a75b38efc9d15c05c.zip | |
navigator: add home position check for RTL state
Diffstat (limited to 'src/modules/navigator')
| -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()) { |