navigator: use switches to select nav state

1 files changed, 68 insertions, 28 deletions

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 0a0ee2541..4176cee1b 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -526,6 +526,8 @@ Navigator::task_main()
 	/* rate limit position updates to 50 Hz */
 	orb_set_interval(_global_pos_sub, 20);
 
+	unsigned prevState = 0;
+
 	/* wakeup source(s) */
 	struct pollfd fds[7];
 
@@ -545,7 +547,6 @@ Navigator::task_main()
 	fds[6].fd = _vstatus_sub;
 	fds[6].events = POLLIN;
 
-
 	while (!_task_should_exit) {
 
 		/* wait for up to 100ms for data */
@@ -571,41 +572,75 @@ Navigator::task_main()
 
 			/* Evaluate state machine from commander and set the navigator mode accordingly */
 			if (_vstatus.main_state == MAIN_STATE_AUTO) {
-				if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) {
-					/* commander requested new navigation mode, try to set it */
-					_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
+				bool stick_mode = false;
+				if (!_vstatus.rc_signal_lost) {
+					/* RC signal available, use control switches to set mode */
+					/* RETURN switch, overrides MISSION switch */
+					if (_vstatus.return_switch == RETURN_SWITCH_RETURN) {
+						dispatch(EVENT_RTL_REQUESTED);
+						stick_mode = true;
+					} else {
+						/* MISSION switch */
+						if (!stick_mode) {
+							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;
+							}
+						}
+						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 */
+							dispatch(EVENT_LOITER_REQUESTED);
+							stick_mode = true;
+						}
+					}
+				}
 
-					switch (_vstatus.set_nav_state) {
-					case NAV_STATE_INIT:
-					case NAV_STATE_NONE:
-						/* nothing to do */
-						break;
+				if (!stick_mode) {
+					if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) {
+						/* commander requested new navigation mode, try to set it */
+						_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
 
-					case NAV_STATE_LOITER:
-						dispatch(EVENT_LOITER_REQUESTED);
-						break;
+						switch (_vstatus.set_nav_state) {
+						case NAV_STATE_INIT:
+						case NAV_STATE_NONE:
+							/* nothing to do */
+							break;
 
-					case NAV_STATE_MISSION:
-						dispatch(EVENT_MISSION_REQUESTED);
-						break;
+						case NAV_STATE_LOITER:
+							dispatch(EVENT_LOITER_REQUESTED);
+							break;
 
-					case NAV_STATE_RTL:
-						dispatch(EVENT_RTL_REQUESTED);
-						break;
+						case NAV_STATE_MISSION:
+							dispatch(EVENT_MISSION_REQUESTED);
+							break;
 
-					default:
-						warnx("ERROR: Requested navigation state not supported");
-						break;
-					}
+						case NAV_STATE_RTL:
+							dispatch(EVENT_RTL_REQUESTED);
+							break;
+
+						default:
+							warnx("ERROR: Requested navigation state not supported");
+							break;
+						}
 
-				} else {
-					/* try mission, if none is available fallback to loiter instead */
-					if (_mission.current_mission_available()) {
-						dispatch(EVENT_MISSION_REQUESTED);
 					} else {
-						dispatch(EVENT_LOITER_REQUESTED);
+						/* on first switch to AUTO try mission, if none is available fallback to loiter instead */
+						if (myState == NAV_STATE_INIT || myState == NAV_STATE_NONE) {
+							if (_mission.current_mission_available()) {
+								dispatch(EVENT_MISSION_REQUESTED);
+							} else {
+								dispatch(EVENT_LOITER_REQUESTED);
+							}
+						}
 					}
-					break;
 				}
 
 			} else {
@@ -673,6 +708,11 @@ Navigator::task_main()
 			}
 		}
 
+		if (myState != prevState) {
+			mavlink_log_info(_mavlink_fd, "[navigator] nav state %d -> %d", prevState, myState);
+			prevState = myState;
+		}
+
 		publish_control_mode();
 
 		perf_end(_loop_perf);