commander: WIP, SEATBELT renamed to ASSISTED, added SIMPLE mode, added ASSISTED switch, some cleanup and reorganizing

5 files changed, 178 insertions, 108 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 253b6295f..25c5a84ea 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1005,17 +1005,15 @@ int commander_thread_main(int argc, char *argv[])
 
 
 		/* check for global or local position updates, set a timeout of 2s */
-		if (hrt_absolute_time() - last_global_position_time < 2000000 && hrt_absolute_time() > 2000000) {
+		if (hrt_absolute_time() - last_global_position_time < 2000000 && hrt_absolute_time() > 2000000 && global_position.valid) {
 			current_status.condition_global_position_valid = true;
-			// XXX check for controller status and home position as well
 
 		} else {
 			current_status.condition_global_position_valid = false;
 		}
 
-		if (hrt_absolute_time() - last_local_position_time < 2000000 && hrt_absolute_time() > 2000000) {
+		if (hrt_absolute_time() - last_local_position_time < 2000000 && hrt_absolute_time() > 2000000 && local_position.valid) {
 			current_status.condition_local_position_valid = true;
-			// XXX check for controller status and home position as well
 
 		} else {
 			current_status.condition_local_position_valid = false;
@@ -1154,7 +1152,7 @@ int commander_thread_main(int argc, char *argv[])
 				} else {
 
 					/* center stick position, set seatbelt/simple control */
-					current_status.mode_switch = MODE_SWITCH_SEATBELT;
+					current_status.mode_switch = MODE_SWITCH_ASSISTED;
 				}
 
 				// warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode);
@@ -1167,21 +1165,32 @@ int commander_thread_main(int argc, char *argv[])
 					/* this switch is not properly mapped, set default */
 					current_status.return_switch = RETURN_SWITCH_NONE;
 
-				} else if (sp_man.return_switch < -STICK_ON_OFF_LIMIT) {
-
-					/* bottom stick position, set altitude hold */
-					current_status.return_switch = RETURN_SWITCH_NONE;
-
 				} else if (sp_man.return_switch > STICK_ON_OFF_LIMIT) {
 
 					/* top stick position */
 					current_status.return_switch = RETURN_SWITCH_RETURN;
 
 				} else {
-					/* center stick position, set default */
+					/* center or bottom stick position, set default */
 					current_status.return_switch = RETURN_SWITCH_NONE;
 				}
 
+				/* check assisted switch  */
+				if (!isfinite(sp_man.assisted_switch)) {
+
+					/* this switch is not properly mapped, set default */
+					current_status.assisted_switch = ASSISTED_SWITCH_SEATBELT;
+
+				} else if (sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
+
+					/* top stick position */
+					current_status.assisted_switch = ASSISTED_SWITCH_SIMPLE;
+
+				} else {
+					/* center or bottom stick position, set default */
+					current_status.assisted_switch = ASSISTED_SWITCH_SEATBELT;
+				}
+
 				/* check mission switch  */
 				if (!isfinite(sp_man.mission_switch)) {
 
@@ -1219,10 +1228,10 @@ int commander_thread_main(int argc, char *argv[])
 								warnx("ERROR: Navigation state MANUAL_STANDBY rejected");
 							}
 			
-						/* Try seatbelt or fallback to manual */
-						} else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) {
+						/* Try assisted or fallback to manual */
+						} else if (current_status.mode_switch == MODE_SWITCH_ASSISTED) {
 			
-							if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+							if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_ASSISTED_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) {
 								// fallback to MANUAL_STANDBY
 								if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) {
 								// These is not supposed to happen
@@ -1235,7 +1244,7 @@ int commander_thread_main(int argc, char *argv[])
 							
 							if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) {
 								// first fallback to SEATBELT_STANDY
-								if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+								if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_ASSISTED_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) {
 									// or fallback to MANUAL_STANDBY
 									if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) {
 									// These is not supposed to happen
@@ -1250,89 +1259,104 @@ int commander_thread_main(int argc, char *argv[])
 					/* evaluate the navigation state when armed */
 					case ARMING_STATE_ARMED:
 			
-						/* Always accept manual mode */
 						if (current_status.mode_switch == MODE_SWITCH_MANUAL) {
-
+							/* MANUAL */
 							if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
 								// These is not supposed to happen
 								warnx("ERROR: Navigation state MANUAL rejected");
 							}
 			
-						/* SEATBELT_STANDBY (fallback: MANUAL) */
-						} else if (current_status.mode_switch == MODE_SWITCH_SEATBELT
-								&& current_status.return_switch == RETURN_SWITCH_NONE) {
-			
-							if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) {
-								// fallback to MANUAL_STANDBY
-								if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
-								// These is not supposed to happen
-								warnx("ERROR: Navigation state MANUAL rejected");
-								}
-							}
-			
-						/* SEATBELT_DESCENT (fallback: MANUAL) */
-						} else if (current_status.mode_switch == MODE_SWITCH_SEATBELT
-								&& current_status.return_switch == RETURN_SWITCH_RETURN) {
-			
-							if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) {
-								// fallback to MANUAL_STANDBY
-								if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
-								// These is not supposed to happen
-								warnx("ERROR: Navigation state MANUAL rejected");
-								}
-							}
-			
-						/* AUTO_LOITER (fallback: SEATBELT, MANUAL) */
-						} else if (current_status.mode_switch == MODE_SWITCH_AUTO
-								&& current_status.return_switch == RETURN_SWITCH_NONE
-								&& current_status.mission_switch == MISSION_SWITCH_NONE) {
-			
-							/* we might come from the disarmed state AUTO_STANDBY */
-							if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_READY, control_mode_pub, &control_mode, mavlink_fd) != OK) {
-								if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) {
-									// fallback to MANUAL_STANDBY
+						} else if (current_status.mode_switch == MODE_SWITCH_ASSISTED) {
+							/* ASSISTED */
+							if (current_status.return_switch == RETURN_SWITCH_RETURN) {
+								/* ASSISTED_DESCENT */
+								if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_ASSISTED_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+									// fallback to MANUAL
 									if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
-									// These is not supposed to happen
-									warnx("ERROR: Navigation state MANUAL rejected");
+										// These is not supposed to happen
+										warnx("ERROR: Navigation state MANUAL rejected");
 									}
 								}
-							/* or from some other armed state like SEATBELT or MANUAL */
-							} else if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode_pub, &control_mode, mavlink_fd) != OK) {
-								if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) {
-									// fallback to MANUAL_STANDBY
-									if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
-									// These is not supposed to happen
-									warnx("ERROR: Navigation state MANUAL rejected");
+
+							} else {
+								if (current_status.assisted_switch == ASSISTED_SWITCH_SIMPLE) {
+									/* ASSISTED_SIMPLE */
+									if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_ASSISTED_SIMPLE, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+										// fallback to ASSISTED_SEATBELT
+										if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+											// fallback to MANUAL
+											if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+												// These is not supposed to happen
+												warnx("ERROR: Navigation state MANUAL rejected");
+											}
+										}
+									}
+								} else {
+									/* ASSISTED_SEATBELT */
+									if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+										// fallback to MANUAL
+										if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+											// These is not supposed to happen
+											warnx("ERROR: Navigation state MANUAL rejected");
+										}
 									}
 								}
 							}
 			
-						/* AUTO_MISSION (fallback: SEATBELT, MANUAL) */
-						} else if (current_status.mode_switch == MODE_SWITCH_AUTO
-								&& current_status.return_switch == RETURN_SWITCH_NONE
-								&& current_status.mission_switch == MISSION_SWITCH_MISSION) {
-							
-							if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode_pub, &control_mode, mavlink_fd) != OK) {
-								if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) {
-									// fallback to MANUAL_STANDBY
-									if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
-									// These is not supposed to happen
-									warnx("ERROR: Navigation state MANUAL rejected");
+						} else if (current_status.mode_switch == MODE_SWITCH_AUTO) {
+							/* AUTO */
+							if (current_status.return_switch == RETURN_SWITCH_RETURN) {
+								/* AUTO_RTL */
+								if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_RTL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+									// fallback to ASSISTED_DESCENT
+									if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_ASSISTED_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+										// fallback to MANUAL
+										if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+											// These is not supposed to happen
+											warnx("ERROR: Navigation state MANUAL rejected");
+										}
 									}
 								}
-							}
-			
-						/* AUTO_RTL (fallback: SEATBELT_DESCENT, MANUAL) */
-						} else if (current_status.mode_switch == MODE_SWITCH_AUTO
-								&& current_status.return_switch == RETURN_SWITCH_RETURN
-								&& (current_status.mission_switch == MISSION_SWITCH_NONE || current_status.mission_switch == MISSION_SWITCH_MISSION)) {
-			
-							if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_RTL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
-								if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_SEATBELT_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) {
-									// fallback to MANUAL_STANDBY
-									if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
-									// These is not supposed to happen
-									warnx("ERROR: Navigation state MANUAL rejected");
+							} else {
+								if (current_status.mission_switch == MISSION_SWITCH_MISSION) {
+									/* AUTO_MISSION */
+									if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+										// fallback to ASSISTED_SEATBELT
+										if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+											// fallback to MANUAL
+											if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+												// These is not supposed to happen
+												warnx("ERROR: Navigation state MANUAL rejected");
+											}
+										}
+									}
+								} else {
+									// TODO check this
+									if (current_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY
+									 || current_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) {
+										/* AUTO_READY */
+										if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_READY, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+											/* AUTO_READY */
+											if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+												// fallback to MANUAL
+												if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+													// These is not supposed to happen
+													warnx("ERROR: Navigation state MANUAL rejected");
+												}
+											}
+										}
+									} else {
+										/* AUTO_LOITER */
+										if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+											// fallback to ASSISTED_SEATBELT
+											if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+												// fallback to MANUAL
+												if (navigation_state_transition(status_pub, &current_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) {
+													// These is not supposed to happen
+													warnx("ERROR: Navigation state MANUAL rejected");
+												}
+											}
+										}
 									}
 								}
 							}
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 3cf60a99a..60ab01536 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -184,7 +184,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
 
 				/* transitions back to INIT are possible for calibration */
 				if (current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY
-				 || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY
+				 || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY
 				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY) {
 
 					ret = OK;
@@ -200,7 +200,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
 
 				/* transitions from INIT and other STANDBY states as well as MANUAL are possible */
 				if (current_state->navigation_state == NAVIGATION_STATE_INIT
-				 || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY
+				 || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY
 				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY
 				 || current_state->navigation_state == NAVIGATION_STATE_MANUAL) {
 
@@ -235,14 +235,15 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
 				}
 				break;
 
-			case NAVIGATION_STATE_SEATBELT_STANDBY:
+			case NAVIGATION_STATE_ASSISTED_STANDBY:
 
 				/* transitions from INIT and other STANDBY states as well as SEATBELT and SEATBELT_DESCENT are possible */
 				if (current_state->navigation_state == NAVIGATION_STATE_INIT
 				 || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY
 				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY
-				 || current_state->navigation_state == NAVIGATION_STATE_SEATBELT
-				 || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT) {
+				 || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT
+				 || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE
+				 || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT) {
 
 					/* need to be disarmed and have a position estimate */
 					if (current_state->arming_state != ARMING_STATE_STANDBY) {
@@ -262,11 +263,12 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
 				}
 				break;
 
-			case NAVIGATION_STATE_SEATBELT:
+			case NAVIGATION_STATE_ASSISTED_SEATBELT:
 
 				/* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/
-				if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY
-				 || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT
+				if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY
+				 || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE
+				 || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT
 				 || current_state->navigation_state == NAVIGATION_STATE_MANUAL
 				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND
 				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER
@@ -293,10 +295,43 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
 				}
 				break;
 
-			case NAVIGATION_STATE_SEATBELT_DESCENT:
+			case NAVIGATION_STATE_ASSISTED_SIMPLE:
+
+				/* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/
+				if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY
+				 || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT
+				 || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT
+				 || current_state->navigation_state == NAVIGATION_STATE_MANUAL
+				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND
+				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER
+				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION
+				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY
+				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL
+				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
+
+					/* need to be armed and have a position estimate */
+					if (current_state->arming_state != ARMING_STATE_ARMED) {
+						mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed");
+						tune_negative();
+					} else if (!current_state->condition_local_position_valid) {
+						mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate");
+						tune_negative();
+					} else {
+						ret = OK;
+						control_mode->flag_control_rates_enabled = true;
+						control_mode->flag_control_attitude_enabled = true;
+						control_mode->flag_control_velocity_enabled = true;
+						control_mode->flag_control_position_enabled = false;
+						control_mode->flag_control_manual_enabled = false;
+					}
+				}
+				break;
+
+			case NAVIGATION_STATE_ASSISTED_DESCENT:
 
 				/* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT and SEATBELT_STANDBY */
-				if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT
+				if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT
+				 || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE
 				 || current_state->navigation_state == NAVIGATION_STATE_MANUAL
 				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND
 				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER
@@ -328,7 +363,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
 				/* transitions from INIT or from other STANDBY modes or from AUTO READY */
 				if (current_state->navigation_state == NAVIGATION_STATE_INIT
 				 || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY
-				 || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY
+				 || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY
 				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) {
 
 					/* need to be disarmed and have a position and home lock */
@@ -395,7 +430,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
 				if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF
 				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION
 				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL
-				 || current_state->navigation_state == NAVIGATION_STATE_SEATBELT
+				 || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT
+				 || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE
 				 || current_state->navigation_state == NAVIGATION_STATE_MANUAL) {
 
 					/* need to have a position and home lock */
@@ -422,7 +458,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
 				if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF
 				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER
 				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL
-				 || current_state->navigation_state == NAVIGATION_STATE_SEATBELT
+				 || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT
+				 || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE
 				 || current_state->navigation_state == NAVIGATION_STATE_MANUAL) {
 
 					/* need to have a mission ready */
@@ -446,7 +483,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
 				if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF
 				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION
 				 || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER
-				 || current_state->navigation_state == NAVIGATION_STATE_SEATBELT
+				 || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT
+				 || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE
 				 || current_state->navigation_state == NAVIGATION_STATE_MANUAL) {
 
 					/* need to have a position and home lock */
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index db8ee9067..ae8e168e1 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -221,9 +221,9 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
 		*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
 	}
 
-	if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT
-		|| v_status.navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT
-		|| v_status.navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY) {
+	if (v_status.navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT
+		|| v_status.navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT
+		|| v_status.navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY) {
 
 		*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
 	}
@@ -248,15 +248,15 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
 			|| v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION
 			|| v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL
 			|| v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF
-			|| v_status.navigation_state == NAVIGATION_STATE_SEATBELT
-			|| v_status.navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT
+			|| v_status.navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT
+			|| v_status.navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT
 			|| v_status.navigation_state == NAVIGATION_STATE_MANUAL) {
 
 		*mavlink_state = MAV_STATE_ACTIVE;
 
 	} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY
 			|| v_status.navigation_state == NAVIGATION_STATE_AUTO_READY // XXX correct?
-			|| v_status.navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY
+			|| v_status.navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY
 			|| v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) {
 
 		*mavlink_state = MAV_STATE_STANDBY;
diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h
index cfee81ea2..eac6d6e98 100644
--- a/src/modules/uORB/topics/manual_control_setpoint.h
+++ b/src/modules/uORB/topics/manual_control_setpoint.h
@@ -58,6 +58,7 @@ struct manual_control_setpoint_s {
 
 	float mode_switch;			/**< mode 3 position switch (mandatory): manual, assisted, auto */
 	float return_switch;			/**< land 2 position switch (mandatory): land, no effect */
+	float assisted_switch;			/**< assisted 2 position switch (optional): seatbelt, simple */
 	float mission_switch;		/**< mission 2 position switch (optional): mission, loiter */
 
 	/**
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index ec08a6c13..9b91e834e 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -60,12 +60,13 @@
 
 /* State Machine */
 typedef enum {
-	NAVIGATION_STATE_INIT=0,
+	NAVIGATION_STATE_INIT = 0,
 	NAVIGATION_STATE_MANUAL_STANDBY,
 	NAVIGATION_STATE_MANUAL,
-	NAVIGATION_STATE_SEATBELT_STANDBY,
-	NAVIGATION_STATE_SEATBELT,
-	NAVIGATION_STATE_SEATBELT_DESCENT,
+	NAVIGATION_STATE_ASSISTED_STANDBY,
+	NAVIGATION_STATE_ASSISTED_SEATBELT,
+	NAVIGATION_STATE_ASSISTED_SIMPLE,
+	NAVIGATION_STATE_ASSISTED_DESCENT,
 	NAVIGATION_STATE_AUTO_STANDBY,
 	NAVIGATION_STATE_AUTO_READY,
 	NAVIGATION_STATE_AUTO_TAKEOFF,
@@ -98,7 +99,7 @@ typedef enum {
 
 typedef enum {
 	MODE_SWITCH_MANUAL = 0,
-	MODE_SWITCH_SEATBELT,
+	MODE_SWITCH_ASSISTED,
 	MODE_SWITCH_AUTO
 } mode_switch_pos_t;
 
@@ -108,6 +109,11 @@ typedef enum {
 } return_switch_pos_t;
 
 typedef enum {
+	ASSISTED_SWITCH_SEATBELT = 0,
+	ASSISTED_SWITCH_SIMPLE
+} assisted_switch_pos_t;
+
+typedef enum {
 	MISSION_SWITCH_NONE = 0,
 	MISSION_SWITCH_MISSION
 } mission_switch_pos_t;
@@ -179,6 +185,7 @@ struct vehicle_status_s
 
 	mode_switch_pos_t mode_switch;
 	return_switch_pos_t return_switch;
+	assisted_switch_pos_t assisted_switch;
 	mission_switch_pos_t mission_switch;
 
 	bool condition_battery_voltage_valid;