diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2013-12-31 15:44:52 +0400 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2013-12-31 15:44:52 +0400 |
| commit | f6ee354c10026488c937568033a640d72a07cd14 (patch) | |
| tree | 65a2ab78593a69a13dfbae5f62310b760dea31c0 /src/modules/navigator | |
| parent | 2e6cd186155d20f53fc45ab2f014d7ebb339c66c (diff) | |
| download | px4-firmware-f6ee354c10026488c937568033a640d72a07cd14.tar.gz px4-firmware-f6ee354c10026488c937568033a640d72a07cd14.tar.bz2 px4-firmware-f6ee354c10026488c937568033a640d72a07cd14.zip | |
navigator: mode switching fixes
Diffstat (limited to 'src/modules/navigator')
| -rw-r--r-- | src/modules/navigator/navigator_main.cpp | 31 |
1 files changed, 16 insertions, 15 deletions
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 12f761b05..34dcf8904 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -594,19 +594,17 @@ Navigator::task_main() stick_mode = true; } else { /* MISSION switch */ - if (!stick_mode) { - if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { + if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { + dispatch(EVENT_LOITER_REQUESTED); + stick_mode = true; + } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { + /* switch to mission only if available */ + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + } else { dispatch(EVENT_LOITER_REQUESTED); - stick_mode = true; - } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { - /* switch to mission only if available */ - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - } else { - dispatch(EVENT_LOITER_REQUESTED); - } - stick_mode = true; } + stick_mode = true; } if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ @@ -631,7 +629,11 @@ Navigator::task_main() break; case NAV_STATE_MISSION: - dispatch(EVENT_MISSION_REQUESTED); + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + } else { + dispatch(EVENT_LOITER_REQUESTED); + } break; case NAV_STATE_RTL: @@ -997,8 +999,7 @@ Navigator::start_loiter() void Navigator::start_mission() { - /* leave previous mission item as is as is */ - + /* leave previous mission item as is */ int ret; bool onboard; unsigned index; @@ -1110,7 +1111,7 @@ Navigator::start_rtl() _mission_item_triplet.current.yaw = 0.0f; // TODO use heading to waypoint? _mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH; _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number + _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; _mission_item_triplet.current.radius = 50.0f; // TODO: get rid of magic number _mission_item_triplet.current.autocontinue = false; _mission_item_triplet.current_valid = true; |