Cleaned up control mode state machine / flight mode / navigation state machine still needs a bit cleaning up

10 files changed, 636 insertions, 311 deletions

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index a088ba1ba..b5df8a8b3 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -1268,6 +1268,8 @@ int commander_thread_main(int argc, char *argv[])
 	current_status.offboard_control_signal_lost = true;
 	/* allow manual override initially */
 	current_status.flag_external_manual_override_ok = true;
+	/* flag position info as bad, do not allow auto mode */
+	current_status.flag_vector_flight_mode_ok = false;
 
 	/* advertise to ORB */
 	stat_pub = orb_advertise(ORB_ID(vehicle_status), &current_status);
@@ -1655,6 +1657,72 @@ int commander_thread_main(int argc, char *argv[])
 			if ((hrt_absolute_time() - sp_man.timestamp) < 100000) {
 
 				/*
+				 * Check if manual control modes have to be switched
+				 */
+				if (!isfinite(sp_man.manual_mode_switch)) {
+
+					/* this switch is not properly mapped, set default */
+					if ((current_status.system_type == MAV_TYPE_QUADROTOR) ||
+						(current_status.system_type == MAV_TYPE_HEXAROTOR) ||
+						(current_status.system_type == MAV_TYPE_OCTOROTOR)) {
+
+						/* assuming a rotary wing, fall back to SAS */
+						current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
+					} else {
+
+						/* assuming a fixed wing, fall back to direct pass-through */
+						current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
+					}
+
+				} else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) {
+
+					/* bottom stick position, set direct mode for vehicles supporting it */
+					if ((current_status.system_type == MAV_TYPE_QUADROTOR) ||
+						(current_status.system_type == MAV_TYPE_HEXAROTOR) ||
+						(current_status.system_type == MAV_TYPE_OCTOROTOR)) {
+
+						/* assuming a rotary wing, fall back to SAS */
+						current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
+					} else {
+
+						/* assuming a fixed wing, fall back to direct pass-through */
+						current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
+					}
+
+				} else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) {
+
+					/* top stick position, set SAS for all vehicle types */
+					current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
+
+				} else {
+					/* center stick position, set rate control */
+					current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_RATES;
+				}
+
+				/*
+				 * Check if manual stability control modes have to be switched
+				 */
+				if (!isfinite(sp_man.manual_sas_switch)) {
+
+					/* this switch is not properly mapped, set default */
+					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS;
+
+				} else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) {
+
+					/* bottom stick position, set altitude hold */
+					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE;
+
+				} else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) {
+
+					/* top stick position */
+					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE;
+
+				} else {
+					/* center stick position, set default */
+					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS;
+				}
+
+				/*
 				 * Check if left stick is in lower left position --> switch to standby state.
 				 * Do this only for multirotors, not for fixed wing aircraft.
 				 */
@@ -1686,19 +1754,21 @@ int commander_thread_main(int argc, char *argv[])
 					}
 				}
 
+				/* check manual override switch - switch to manual or auto mode */
 				if (sp_man.manual_override_switch > STICK_ON_OFF_LIMIT) {
+					/* enable manual override */
 					update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd);
 				} else {
+					/* check auto mode switch for correct mode */
 					if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) {
-						//update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd);
-						// XXX hack
+						/* enable stabilized mode */
 						update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
 
 					} else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) {
 						update_state_machine_mode_auto(stat_pub, &current_status, mavlink_fd);
 
 					} else {
-						update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
+						update_state_machine_mode_hold(stat_pub, &current_status, mavlink_fd);
 					}
 				}
 
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c
index c4b9fbb6a..4152142df 100644
--- a/apps/commander/state_machine_helper.c
+++ b/apps/commander/state_machine_helper.c
@@ -93,8 +93,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 		/* set system flags according to state */
 		current_status->flag_system_armed = true;
 
-		fprintf(stderr, "[commander] EMERGENCY LANDING!\n");
-		mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY LANDING!");
+		fprintf(stderr, "[cmd] EMERGENCY LANDING!\n");
+		mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY LANDING!");
 		break;
 
 	case SYSTEM_STATE_EMCY_CUTOFF:
@@ -103,8 +103,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 		/* set system flags according to state */
 		current_status->flag_system_armed = false;
 
-		fprintf(stderr, "[commander] EMERGENCY MOTOR CUTOFF!\n");
-		mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY MOTOR CUTOFF!");
+		fprintf(stderr, "[cmd] EMERGENCY MOTOR CUTOFF!\n");
+		mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY MOTOR CUTOFF!");
 		break;
 
 	case SYSTEM_STATE_GROUND_ERROR:
@@ -114,8 +114,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 		/* prevent actuators from arming */
 		current_status->flag_system_armed = false;
 
-		fprintf(stderr, "[commander] GROUND ERROR, locking down propulsion system\n");
-		mavlink_log_critical(mavlink_fd, "[commander] GROUND ERROR, locking down propulsion system");
+		fprintf(stderr, "[cmd] GROUND ERROR, locking down propulsion system\n");
+		mavlink_log_critical(mavlink_fd, "[cmd] GROUND ERROR, locking down propulsion system");
 		break;
 
 	case SYSTEM_STATE_PREFLIGHT:
@@ -123,10 +123,10 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 		 || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
 			/* set system flags according to state */
 			current_status->flag_system_armed = false;
-			mavlink_log_critical(mavlink_fd, "[commander] Switched to PREFLIGHT state");
+			mavlink_log_critical(mavlink_fd, "[cmd] Switched to PREFLIGHT state");
 		} else {
 			invalid_state = true;
-			mavlink_log_critical(mavlink_fd, "[commander] REFUSED to switch to PREFLIGHT state");
+			mavlink_log_critical(mavlink_fd, "[cmd] REFUSED to switch to PREFLIGHT state");
 		}
 		break;
 
@@ -136,13 +136,13 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 			invalid_state = false;
 			/* set system flags according to state */
 			current_status->flag_system_armed = false;
-			mavlink_log_critical(mavlink_fd, "[commander] REBOOTING SYSTEM");
+			mavlink_log_critical(mavlink_fd, "[cmd] REBOOTING SYSTEM");
 			usleep(500000);
 			up_systemreset();
 			/* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
 		} else {
 			invalid_state = true;
-			mavlink_log_critical(mavlink_fd, "[commander] REFUSED to REBOOT");
+			mavlink_log_critical(mavlink_fd, "[cmd] REFUSED to REBOOT");
 		}
 		break;
 
@@ -152,7 +152,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 		/* standby enforces disarmed */
 		current_status->flag_system_armed = false;
 
-		mavlink_log_critical(mavlink_fd, "[commander] Switched to STANDBY state");
+		mavlink_log_critical(mavlink_fd, "[cmd] Switched to STANDBY state");
 		break;
 
 	case SYSTEM_STATE_GROUND_READY:
@@ -162,7 +162,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 		/* ground ready has motors / actuators armed */
 		current_status->flag_system_armed = true;
 
-		mavlink_log_critical(mavlink_fd, "[commander] Switched to GROUND READY state");
+		mavlink_log_critical(mavlink_fd, "[cmd] Switched to GROUND READY state");
 		break;
 
 	case SYSTEM_STATE_AUTO:
@@ -172,7 +172,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 		/* auto is airborne and in auto mode, motors armed */
 		current_status->flag_system_armed = true;
 
-		mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / AUTO mode");
+		mavlink_log_critical(mavlink_fd, "[cmd] Switched to FLYING / AUTO mode");
 		break;
 
 	case SYSTEM_STATE_STABILIZED:
@@ -180,7 +180,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 		/* set system flags according to state */
 		current_status->flag_system_armed = true;
 
-		mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / STABILIZED mode");
+		mavlink_log_critical(mavlink_fd, "[cmd] Switched to FLYING / STABILIZED mode");
 		break;
 
 	case SYSTEM_STATE_MANUAL:
@@ -188,7 +188,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 		/* set system flags according to state */
 		current_status->flag_system_armed = true;
 
-		mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / MANUAL mode");
+		mavlink_log_critical(mavlink_fd, "[cmd] Switched to FLYING / MANUAL mode");
 		break;
 
 	default:
@@ -203,7 +203,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 		ret = OK;
 	}
 	if (invalid_state) {
-		mavlink_log_critical(mavlink_fd, "[commander] REJECTING invalid state transition");
+		mavlink_log_critical(mavlink_fd, "[cmd] REJECTING invalid state transition");
 		ret = ERROR;
 	}
 	return ret;
@@ -232,7 +232,7 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
 	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0;
 
 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-	printf("[commander] new state: %s\n", system_state_txt[current_status->state_machine]);
+	printf("[cmd] new state: %s\n", system_state_txt[current_status->state_machine]);
 }
 
 void publish_armed_status(const struct vehicle_status_s *current_status) {
@@ -250,7 +250,7 @@ void publish_armed_status(const struct vehicle_status_s *current_status) {
  */
 void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
 {
-	fprintf(stderr, "[commander] EMERGENCY HANDLER\n");
+	fprintf(stderr, "[cmd] EMERGENCY HANDLER\n");
 	/* Depending on the current state go to one of the error states */
 
 	if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
@@ -262,7 +262,7 @@ void state_machine_emergency_always_critical(int status_pub, struct vehicle_stat
 		//do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT);
 
 	} else {
-		fprintf(stderr, "[commander] Unknown system state: #%d\n", current_status->state_machine);
+		fprintf(stderr, "[cmd] Unknown system state: #%d\n", current_status->state_machine);
 	}
 }
 
@@ -272,7 +272,7 @@ void state_machine_emergency(int status_pub, struct vehicle_status_s *current_st
 		state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd);
 
 	} else {
-		//global_data_send_mavlink_statustext_message_out("[commander] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL);
+		//global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL);
 	}
 
 }
@@ -497,7 +497,7 @@ void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_
 void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
 {
 	if (current_status->state_machine == SYSTEM_STATE_STANDBY) {
-		printf("[commander] arming\n");
+		printf("[cmd] arming\n");
 		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
 	}
 }
@@ -505,11 +505,11 @@ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_s
 void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
 {
 	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
-		printf("[commander] going standby\n");
+		printf("[cmd] going standby\n");
 		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
 
 	} else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
-		printf("[commander] MISSION ABORT!\n");
+		printf("[cmd] MISSION ABORT!\n");
 		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
 	}
 }
@@ -518,6 +518,7 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c
 {
 	int old_mode = current_status->flight_mode;
 	current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
+
 	current_status->flag_control_manual_enabled = true;
 	/* enable attitude control per default */
 	current_status->flag_control_attitude_enabled = true;
@@ -525,58 +526,91 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c
 	if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
 
 	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
-		printf("[commander] manual mode\n");
+		printf("[cmd] manual mode\n");
 		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
 	}
 }
 
 void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
 {
-	int old_mode = current_status->flight_mode;
-	current_status->flight_mode = VEHICLE_FLIGHT_MODE_STABILIZED;
-	current_status->flag_control_manual_enabled = true;
-	current_status->flag_control_attitude_enabled = true;
-	current_status->flag_control_rates_enabled = true;
-	if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
+	if (!current_status->flag_vector_flight_mode_ok) {
+		mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. STAB MODE");
+		return;
+	}
+	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
+		printf("[cmd] stabilized mode\n");
+		int old_mode = current_status->flight_mode;
+		current_status->flight_mode = VEHICLE_FLIGHT_MODE_STAB;
+		current_status->flag_control_manual_enabled = false;
+		current_status->flag_control_attitude_enabled = true;
+		current_status->flag_control_rates_enabled = true;
+		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED);
+		if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
+
+	}
+}
 
+void update_state_machine_mode_hold(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
+{
+	if (!current_status->flag_vector_flight_mode_ok) {
+		mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. HOLD MODE");
+		return;
+	}
+	
 	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
-		printf("[commander] stabilized mode\n");
+		printf("[cmd] stabilized mode\n");
+		int old_mode = current_status->flight_mode;
+		current_status->flight_mode = VEHICLE_FLIGHT_MODE_HOLD;
+		current_status->flag_control_manual_enabled = false;
+		current_status->flag_control_attitude_enabled = true;
+		current_status->flag_control_rates_enabled = true;
 		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED);
+		if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
 	}
 }
 
 void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
 {
-	int old_mode = current_status->flight_mode;
-	current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO;
-	current_status->flag_control_manual_enabled = true;
-	current_status->flag_control_attitude_enabled = true;
-	current_status->flag_control_rates_enabled = true;
-	if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
-
+	if (!current_status->flag_vector_flight_mode_ok) {
+		mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. AUTO MODE");
+		return;
+	}
+	
 	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) {
-		printf("[commander] auto mode\n");
+		printf("[cmd] auto mode\n");
+		int old_mode = current_status->flight_mode;
+		current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO;
+		current_status->flag_control_manual_enabled = false;
+		current_status->flag_control_attitude_enabled = true;
+		current_status->flag_control_rates_enabled = true;
 		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO);
+		if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
 	}
 }
 
 
 uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
 {
-	printf("[commander] Requested new mode: %d\n", (int)mode);
 	uint8_t ret = 1;
     
-    /* Switch on HIL if in standby and not already in HIL mode */
-	if ((current_status->state_machine == SYSTEM_STATE_STANDBY) && (mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
+	/* Switch on HIL if in standby and not already in HIL mode */
+	if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
 		&& !current_status->flag_hil_enabled) {
-		/* Enable HIL on request */
-		current_status->flag_hil_enabled = true;
-		ret = OK;
-		state_machine_publish(status_pub, current_status, mavlink_fd);
-		publish_armed_status(current_status);
-		printf("[commander] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
-	} else if (current_status->state_machine != SYSTEM_STATE_STANDBY) {
-		mavlink_log_critical(mavlink_fd, "[commander] REJECTING HIL, not in standby.")
+		if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) {
+			/* Enable HIL on request */
+			current_status->flag_hil_enabled = true;
+			ret = OK;
+			state_machine_publish(status_pub, current_status, mavlink_fd);
+			publish_armed_status(current_status);
+			printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
+		} else if (current_status->state_machine != SYSTEM_STATE_STANDBY &&
+			current_status->flag_system_armed) {
+
+			mavlink_log_critical(mavlink_fd, "[cmd] REJECTING HIL, disarm first!")
+		} else {
+
+			mavlink_log_critical(mavlink_fd, "[cmd] REJECTING HIL, not in standby.")
+		}
 	}
     
 	/* switch manual / auto */
@@ -595,7 +629,7 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
 		if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
 			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
 			ret = OK;
-			printf("[commander] arming due to command request\n");
+			printf("[cmd] arming due to command request\n");
 		}
 	}
 
@@ -605,13 +639,14 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
 		if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
 			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
 			ret = OK;
-			printf("[commander] disarming due to command request\n");
+			printf("[cmd] disarming due to command request\n");
 		}
 	}
 
 	/* NEVER actually switch off HIL without reboot */
 	if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
-		fprintf(stderr, "[commander] DENYING request to switch off HIL. Please power cycle (safety reasons)\n");
+		fprintf(stderr, "[cmd] DENYING request to switch off HIL. Please power cycle (safety reasons)\n");
+		mavlink_log_critical(mavlink_fd, "[cmd] Power-cycle to exit HIL");
 		ret = ERROR;
 	}
 
@@ -636,7 +671,8 @@ uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_
 
 		if (current_system_state == SYSTEM_STATE_STANDBY || current_system_state == SYSTEM_STATE_PREFLIGHT) {
 			printf("system will reboot\n");
-			//global_data_send_mavlink_statustext_message_out("Rebooting autopilot.. ", MAV_SEVERITY_INFO);
+			mavlink_log_critical(mavlink_fd, "[cmd] Rebooting..");
+			usleep(200000);
 			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT);
 			ret = 0;
 		}
diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h
index c4d1b78a5..815645089 100644
--- a/apps/commander/state_machine_helper.h
+++ b/apps/commander/state_machine_helper.h
@@ -128,6 +128,15 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c
 void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
 
 /**
+ * Handle state machine if mode switch is hold
+ *
+ * @param status_pub file descriptor for state update topic publication
+ * @param current_status pointer to the current state machine to operate on
+ * @param mavlink_fd file descriptor for MAVLink statustext messages
+ */
+void update_state_machine_mode_hold(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
+
+/**
  * Handle state machine if mode switch is auto
  *
  * @param status_pub file descriptor for state update topic publication
diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c
index 3e1fc4952..9c450e68e 100644
--- a/apps/fixedwing_att_control/fixedwing_att_control_main.c
+++ b/apps/fixedwing_att_control/fixedwing_att_control_main.c
@@ -195,44 +195,61 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 				/* pass through throttle */
 				actuators.control[3] = att_sp.thrust;
 
-			} else if (vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
-
-				/* if the RC signal is lost, try to stay level and go slowly back down to ground */
-				if(vstatus.rc_signal_lost) {
-					
-					// XXX define failsafe throttle param
-					//param_get(failsafe_throttle_handle, &failsafe_throttle);
-					att_sp.roll_body = 0.3f;
-					att_sp.pitch_body = 0.0f;
-					att_sp.thrust = 0.5f;
-
-					// XXX disable yaw control, loiter
-
-				} else {
-					
-					att_sp.roll_body = manual_sp.roll;
-					att_sp.pitch_body = manual_sp.pitch;
-					att_sp.yaw_body = 0;
-					att_sp.thrust = manual_sp.throttle;
-					att_sp.timestamp = hrt_absolute_time();
+				/* set flaps to zero */
+				actuators.control[4] = 0.0f;
+
+			} else {
+				if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
+
+					/* if the RC signal is lost, try to stay level and go slowly back down to ground */
+					if(vstatus.rc_signal_lost) {
+						
+						// XXX define failsafe throttle param
+						//param_get(failsafe_throttle_handle, &failsafe_throttle);
+						att_sp.roll_body = 0.3f;
+						att_sp.pitch_body = 0.0f;
+						att_sp.thrust = 0.5f;
+
+						// XXX disable yaw control, loiter
+
+					} else {
+						
+						att_sp.roll_body = manual_sp.roll;
+						att_sp.pitch_body = manual_sp.pitch;
+						att_sp.yaw_body = 0;
+						att_sp.thrust = manual_sp.throttle;
+						att_sp.timestamp = hrt_absolute_time();
+					}
+
+					/* attitude control */
+					fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
+
+					/* angular rate control */
+					fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
+
+					/* pass through throttle */
+					actuators.control[3] = att_sp.thrust;
+
+					/* pass through flaps */
+					if (isfinite(manual_sp.flaps)) {
+						actuators.control[4] = manual_sp.flaps;
+					} else {
+						actuators.control[4] = 0.0f;
+					}
+
+				} else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
+					/* directly pass through values */
+					actuators.control[0] = manual_sp.roll;
+					/* positive pitch means negative actuator -> pull up */
+					actuators.control[1] = manual_sp.pitch;
+					actuators.control[2] = manual_sp.yaw;
+					actuators.control[3] = manual_sp.throttle;
+					if (isfinite(manual_sp.flaps)) {
+						actuators.control[4] = manual_sp.flaps;
+					} else {
+						actuators.control[4] = 0.0f;
+					}
 				}
-
-				/* attitude control */
-				fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
-
-				/* angular rate control */
-				fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
-
-				/* pass through throttle */
-				actuators.control[3] = att_sp.thrust;
-
-			} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
-				/* directly pass through values */
-				actuators.control[0] = manual_sp.roll;
-				/* positive pitch means negative actuator -> pull up */
-				actuators.control[1] = manual_sp.pitch;
-				actuators.control[2] = manual_sp.yaw;
-				actuators.control[3] = manual_sp.throttle;
 			}
 
 			/* publish rates */
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c
index ce1e52c1b..f184859a3 100644
--- a/apps/multirotor_att_control/multirotor_att_control_main.c
+++ b/apps/multirotor_att_control/multirotor_att_control_main.c
@@ -232,19 +232,9 @@ mc_thread_main(int argc, char *argv[])
 						/* STEP 2: publish the result to the vehicle actuators */
 						orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
 					}
-					/* decide wether we want rate or position input */
-				}
-				else if (state.flag_control_manual_enabled) {
-
-					/* manual inputs, from RC control or joystick */
-					if (state.flag_control_rates_enabled && !state.flag_control_attitude_enabled) {
-						rates_sp.roll = manual.roll;
+					
 
-						rates_sp.pitch = manual.pitch;
-						rates_sp.yaw = manual.yaw;
-						rates_sp.thrust = manual.throttle;
-						rates_sp.timestamp = hrt_absolute_time();
-					}
+				} else if (state.flag_control_manual_enabled) {
 
 					if (state.flag_control_attitude_enabled) {
 
@@ -258,7 +248,7 @@ mc_thread_main(int argc, char *argv[])
 						static bool rc_loss_first_time = true;
 
 						/* if the RC signal is lost, try to stay level and go slowly back down to ground */
-						if(state.rc_signal_lost) {
+						if (state.rc_signal_lost) {
 							/* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */
 							param_get(failsafe_throttle_handle, &failsafe_throttle);
 							att_sp.roll_body = 0.0f;
@@ -285,41 +275,66 @@ mc_thread_main(int argc, char *argv[])
 								att_sp.yaw_body = att.yaw;
 							}
 
-							/* only move setpoint if manual input is != 0 */
+							/* act if stabilization is active or if the (nonsense) direct pass through mode is set */
+							if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS ||
+								state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
 
-							if(manual.mode == MANUAL_CONTROL_MODE_ATT_YAW_POS) {
-								if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) {
+								if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) {
 									rates_sp.yaw = manual.yaw;
 									control_yaw_position = false;
-									first_time_after_yaw_speed_control = true;
 								} else {
-									if (first_time_after_yaw_speed_control) {
-										att_sp.yaw_body = att.yaw;
-										first_time_after_yaw_speed_control = false;
+									/*
+									 * This mode SHOULD be the default mode, which is:
+									 * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS
+									 *
+									 * However, we fall back to this setting for all other (nonsense)
+									 * settings as well.
+									 */
+
+									/* only move setpoint if manual input is != 0 */
+									if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) {
+										rates_sp.yaw = manual.yaw;
+										control_yaw_position = false;
+										first_time_after_yaw_speed_control = true;
+									} else {
+										if (first_time_after_yaw_speed_control) {
+											att_sp.yaw_body = att.yaw;
+											first_time_after_yaw_speed_control = false;
+										}
+										control_yaw_position = true;
 									}
-									control_yaw_position = true;
 								}
-							} else if (manual.mode == MANUAL_CONTROL_MODE_ATT_YAW_RATE) {
-								rates_sp.yaw = manual.yaw;
-								control_yaw_position = false;
 							}
 
 							att_sp.thrust = manual.throttle;
 							att_sp.timestamp = hrt_absolute_time();
 						}
-					}
-					/* STEP 2: publish the result to the vehicle actuators */
-					orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-
-					if (motor_test_mode) {
-						printf("testmode");
-						att_sp.roll_body = 0.0f;
-						att_sp.pitch_body = 0.0f;
-						att_sp.yaw_body = 0.0f;
-						att_sp.thrust = 0.1f;
-						att_sp.timestamp = hrt_absolute_time();
-						/* STEP 2: publish the result to the vehicle actuators */
+
+						/* STEP 2: publish the controller output */
 						orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
+
+						if (motor_test_mode) {
+							printf("testmode");
+							att_sp.roll_body = 0.0f;
+							att_sp.pitch_body = 0.0f;
+							att_sp.yaw_body = 0.0f;
+							att_sp.thrust = 0.1f;
+							att_sp.timestamp = hrt_absolute_time();
+							/* STEP 2: publish the result to the vehicle actuators */
+							orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
+						}
+
+					} else {
+						/* manual rate inputs, from RC control or joystick */
+						if (state.flag_control_rates_enabled &&
+							state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) {
+							rates_sp.roll = manual.roll;
+
+							rates_sp.pitch = manual.pitch;
+							rates_sp.yaw = manual.yaw;
+							rates_sp.thrust = manual.throttle;
+							rates_sp.timestamp = hrt_absolute_time();
+						}
 					}
 
 				}
diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c
index 37ba0ec3e..1546fb56d 100644
--- a/apps/sensors/sensor_params.c
+++ b/apps/sensors/sensor_params.c
@@ -69,60 +69,100 @@ PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
 PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f);
 PARAM_DEFINE_FLOAT(RC1_REV, 1.0f);
 PARAM_DEFINE_FLOAT(RC1_DZ, 0.0f);
-PARAM_DEFINE_FLOAT(RC1_EXP, 0.0f);
+// PARAM_DEFINE_FLOAT(RC1_EXP, 0.0f);
 
 PARAM_DEFINE_FLOAT(RC2_MIN, 1000);
 PARAM_DEFINE_FLOAT(RC2_TRIM, 1500);
 PARAM_DEFINE_FLOAT(RC2_MAX, 2000);
 PARAM_DEFINE_FLOAT(RC2_REV, 1.0f);
 PARAM_DEFINE_FLOAT(RC2_DZ, 0.0f);
-PARAM_DEFINE_FLOAT(RC2_EXP, 0.0f);
+// PARAM_DEFINE_FLOAT(RC2_EXP, 0.0f);
 
 PARAM_DEFINE_FLOAT(RC3_MIN, 1000);
 PARAM_DEFINE_FLOAT(RC3_TRIM, 1500);
 PARAM_DEFINE_FLOAT(RC3_MAX, 2000);
 PARAM_DEFINE_FLOAT(RC3_REV, 1.0f);
 PARAM_DEFINE_FLOAT(RC3_DZ, 0.0f);
-PARAM_DEFINE_FLOAT(RC3_EXP, 0.0f);
+// PARAM_DEFINE_FLOAT(RC3_EXP, 0.0f);
 
 PARAM_DEFINE_FLOAT(RC4_MIN, 1000);
 PARAM_DEFINE_FLOAT(RC4_TRIM, 1500);
 PARAM_DEFINE_FLOAT(RC4_MAX, 2000);
 PARAM_DEFINE_FLOAT(RC4_REV, 1.0f);
 PARAM_DEFINE_FLOAT(RC4_DZ, 0.0f);
-PARAM_DEFINE_FLOAT(RC4_EXP, 0.0f);
+// PARAM_DEFINE_FLOAT(RC4_EXP, 0.0f);
 
 PARAM_DEFINE_FLOAT(RC5_MIN, 1000);
 PARAM_DEFINE_FLOAT(RC5_TRIM, 1500);
 PARAM_DEFINE_FLOAT(RC5_MAX, 2000);
 PARAM_DEFINE_FLOAT(RC5_REV, 1.0f);
 PARAM_DEFINE_FLOAT(RC5_DZ,  0.0f);
-PARAM_DEFINE_FLOAT(RC5_EXP, 0.0f);
+// PARAM_DEFINE_FLOAT(RC5_EXP, 0.0f);
 
 PARAM_DEFINE_FLOAT(RC6_MIN, 1000);
 PARAM_DEFINE_FLOAT(RC6_TRIM, 1500);
 PARAM_DEFINE_FLOAT(RC6_MAX, 2000);
 PARAM_DEFINE_FLOAT(RC6_REV, 1.0f);
 PARAM_DEFINE_FLOAT(RC6_DZ, 0.0f);
-PARAM_DEFINE_FLOAT(RC6_EXP, 0.0f);
+// PARAM_DEFINE_FLOAT(RC6_EXP, 0.0f);
 
 PARAM_DEFINE_FLOAT(RC7_MIN, 1000);
 PARAM_DEFINE_FLOAT(RC7_TRIM, 1500);
 PARAM_DEFINE_FLOAT(RC7_MAX, 2000);
 PARAM_DEFINE_FLOAT(RC7_REV, 1.0f);
 PARAM_DEFINE_FLOAT(RC7_DZ, 0.0f);
-PARAM_DEFINE_FLOAT(RC7_EXP, 0.0f);
+// PARAM_DEFINE_FLOAT(RC7_EXP, 0.0f);
 
 PARAM_DEFINE_FLOAT(RC8_MIN, 1000);
 PARAM_DEFINE_FLOAT(RC8_TRIM, 1500);
 PARAM_DEFINE_FLOAT(RC8_MAX, 2000);
 PARAM_DEFINE_FLOAT(RC8_REV, 1.0f);
 PARAM_DEFINE_FLOAT(RC8_DZ, 0.0f);
-PARAM_DEFINE_FLOAT(RC8_EXP, 0.0f);
-
-PARAM_DEFINE_INT32(RC_TYPE, 1); // 1 = FUTABA
-
-PARAM_DEFINE_INT32(RC_DEMIX, 0); /**< 0 = off, 1 = auto, 2 = delta */
+// PARAM_DEFINE_FLOAT(RC8_EXP, 0.0f);
+
+PARAM_DEFINE_FLOAT(RC9_MIN, 1000);
+PARAM_DEFINE_FLOAT(RC9_TRIM, 1500);
+PARAM_DEFINE_FLOAT(RC9_MAX, 2000);
+PARAM_DEFINE_FLOAT(RC9_REV, 1.0f);
+PARAM_DEFINE_FLOAT(RC9_DZ, 0.0f);
+// PARAM_DEFINE_FLOAT(RC9_EXP, 0.0f);
+
+PARAM_DEFINE_FLOAT(RC10_MIN, 1000);
+PARAM_DEFINE_FLOAT(RC10_TRIM, 1500);
+PARAM_DEFINE_FLOAT(RC10_MAX, 2000);
+PARAM_DEFINE_FLOAT(RC10_REV, 1.0f);
+PARAM_DEFINE_FLOAT(RC10_DZ, 0.0f);
+// PARAM_DEFINE_FLOAT(RC10_EXP, 0.0f);
+
+PARAM_DEFINE_FLOAT(RC11_MIN, 1000);
+PARAM_DEFINE_FLOAT(RC11_TRIM, 1500);
+PARAM_DEFINE_FLOAT(RC11_MAX, 2000);
+PARAM_DEFINE_FLOAT(RC11_REV, 1.0f);
+PARAM_DEFINE_FLOAT(RC11_DZ, 0.0f);
+// PARAM_DEFINE_FLOAT(RC11_EXP, 0.0f);
+
+PARAM_DEFINE_FLOAT(RC12_MIN, 1000);
+PARAM_DEFINE_FLOAT(RC12_TRIM, 1500);
+PARAM_DEFINE_FLOAT(RC12_MAX, 2000);
+PARAM_DEFINE_FLOAT(RC12_REV, 1.0f);
+PARAM_DEFINE_FLOAT(RC12_DZ, 0.0f);
+// PARAM_DEFINE_FLOAT(RC12_EXP, 0.0f);
+
+PARAM_DEFINE_FLOAT(RC13_MIN, 1000);
+PARAM_DEFINE_FLOAT(RC13_TRIM, 1500);
+PARAM_DEFINE_FLOAT(RC13_MAX, 2000);
+PARAM_DEFINE_FLOAT(RC13_REV, 1.0f);
+PARAM_DEFINE_FLOAT(RC13_DZ, 0.0f);
+// PARAM_DEFINE_FLOAT(RC13_EXP, 0.0f);
+
+PARAM_DEFINE_FLOAT(RC14_MIN, 1000);
+PARAM_DEFINE_FLOAT(RC14_TRIM, 1500);
+PARAM_DEFINE_FLOAT(RC14_MAX, 2000);
+PARAM_DEFINE_FLOAT(RC14_REV, 1.0f);
+PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f);
+// PARAM_DEFINE_FLOAT(RC12_EXP, 0.0f);
+
+PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */
 
 /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
 PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f));
@@ -131,12 +171,23 @@ PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);
 PARAM_DEFINE_INT32(RC_MAP_PITCH, 2);
 PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3);
 PARAM_DEFINE_INT32(RC_MAP_YAW, 4);
-PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5);
-PARAM_DEFINE_INT32(RC_MAP_AUX1, 6);
-PARAM_DEFINE_INT32(RC_MAP_AUX2, 7);
-PARAM_DEFINE_INT32(RC_MAP_AUX3, 8);
+
+PARAM_DEFINE_INT32(RC_MAP_OVER_SW, 5);
+PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 6);
+
+PARAM_DEFINE_INT32(RC_MAP_MAN_SW, 0);
+PARAM_DEFINE_INT32(RC_MAP_SAS_SW, 0);
+PARAM_DEFINE_INT32(RC_MAP_RTL_SW, 0);
+PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
+
+PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0);
+
+PARAM_DEFINE_INT32(RC_MAP_AUX1, 0);	/**< default function: camera yaw / azimuth */
+PARAM_DEFINE_INT32(RC_MAP_AUX2, 0);	/**< default function: camera pitch / tilt */
+PARAM_DEFINE_INT32(RC_MAP_AUX3, 0);	/**< default function: camera trigger */
+PARAM_DEFINE_INT32(RC_MAP_AUX4, 0);	/**< default function: camera roll */
+PARAM_DEFINE_INT32(RC_MAP_AUX5, 0);	/**< default function: payload drop */
 
 PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.4f);
 PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.4f);
 PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 1.0f);
-
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index bcc383330..10d25d347 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -95,11 +95,7 @@
 
 #define PPM_INPUT_TIMEOUT_INTERVAL	50000 /**< 50 ms timeout / 20 Hz */
 
-enum RC_DEMIX {
-	RC_DEMIX_NONE  = 0,
-	RC_DEMIX_AUTO  = 1,
-	RC_DEMIX_DELTA = 2
-};
+#define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg)
 
 /**
  * Sensor app start / stop handling function
@@ -129,7 +125,7 @@ public:
 	int		start();
 
 private:	
-	static const unsigned _rc_max_chan_count = 8;	/**< maximum number of r/c channels we handle */
+	static const unsigned _rc_max_chan_count = RC_CHANNELS_MAX;	/**< maximum number of r/c channels we handle */
 
 #if CONFIG_HRT_PPM
 	hrt_abstime	_ppm_last_valid;		/**< last time we got a valid ppm signal */
@@ -173,7 +169,7 @@ private:
 		float max[_rc_max_chan_count];
 		float rev[_rc_max_chan_count];
 		float dz[_rc_max_chan_count];
-		float ex[_rc_max_chan_count];
+		// float ex[_rc_max_chan_count];
 		float scaling_factor[_rc_max_chan_count];
 
 		float gyro_offset[3];
@@ -184,21 +180,31 @@ private:
 
 		int rc_type;
 
-		int rc_demix;					/**< enabled de-mixing of RC mixers */
-
 		int rc_map_roll;
 		int rc_map_pitch;
 		int rc_map_yaw;
 		int rc_map_throttle;
-		int rc_map_mode_sw;
+
+		int rc_map_manual_override_sw;
+		int rc_map_auto_mode_sw;
+
+		int rc_map_manual_mode_sw;
+		int rc_map_sas_mode_sw;
+		int rc_map_rtl_sw;
+		int rc_map_offboard_ctrl_mode_sw;
+
+		int rc_map_flaps;
 
 		int rc_map_aux1;
 		int rc_map_aux2;
 		int rc_map_aux3;
+		int rc_map_aux4;
+		int rc_map_aux5;
 
 		float rc_scale_roll;
 		float rc_scale_pitch;
 		float rc_scale_yaw;
+		float rc_scale_flaps;
 
 		float battery_voltage_scaling;
 	}		_parameters;			/**< local copies of interesting parameters */
@@ -209,7 +215,7 @@ private:
 		param_t max[_rc_max_chan_count];
 		param_t rev[_rc_max_chan_count];
 		param_t dz[_rc_max_chan_count];
-		param_t ex[_rc_max_chan_count];
+		// param_t ex[_rc_max_chan_count];
 		param_t rc_type;
 
 		param_t rc_demix;
@@ -224,15 +230,27 @@ private:
 		param_t rc_map_pitch;
 		param_t rc_map_yaw;
 		param_t rc_map_throttle;
-		param_t rc_map_mode_sw;
+		
+		param_t rc_map_manual_override_sw;
+		param_t rc_map_auto_mode_sw;
+
+		param_t rc_map_manual_mode_sw;
+		param_t rc_map_sas_mode_sw;
+		param_t rc_map_rtl_sw;
+		param_t rc_map_offboard_ctrl_mode_sw;
+
+		param_t rc_map_flaps;
 
 		param_t rc_map_aux1;
 		param_t rc_map_aux2;
 		param_t rc_map_aux3;
+		param_t rc_map_aux4;
+		param_t rc_map_aux5;
 
 		param_t rc_scale_roll;
 		param_t rc_scale_pitch;
 		param_t rc_scale_yaw;
+		param_t rc_scale_flaps;
 
 		param_t battery_voltage_scaling;
 	}		_parameter_handles;		/**< handles for interesting parameters */
@@ -395,27 +413,43 @@ Sensors::Sensors() :
 		sprintf(nbuf, "RC%d_DZ", i + 1);
 		_parameter_handles.dz[i] = param_find(nbuf);
 
-		/* channel exponential gain */
-		sprintf(nbuf, "RC%d_EXP", i + 1);
-		_parameter_handles.ex[i] = param_find(nbuf);
+		// /* channel exponential gain */
+		// sprintf(nbuf, "RC%d_EXP", i + 1);
+		// _parameter_handles.ex[i] = param_find(nbuf);
 	}
 
 	_parameter_handles.rc_type = param_find("RC_TYPE");
 
-	_parameter_handles.rc_demix = param_find("RC_DEMIX");
+	// _parameter_handles.rc_demix = param_find("RC_DEMIX");
 
+	/* mandatory input switched, mapped to channels 1-4 per default */
 	_parameter_handles.rc_map_roll 	= param_find("RC_MAP_ROLL");
 	_parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH");
 	_parameter_handles.rc_map_yaw 	= param_find("RC_MAP_YAW");
 	_parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE");
-	_parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW");
+
+	/* mandatory mode switches, mapped to channel 5 and 6 per default */
+	_parameter_handles.rc_map_manual_override_sw = param_find("RC_MAP_OVER_SW");
+	_parameter_handles.rc_map_auto_mode_sw = param_find("RC_MAP_MODE_SW");
+
+	_parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS");
+
+	/* optional mode switches, not mapped per default */
+	_parameter_handles.rc_map_manual_mode_sw = param_find("RC_MAP_MAN_SW");
+	_parameter_handles.rc_map_sas_mode_sw = param_find("RC_MAP_SAS_SW");
+	_parameter_handles.rc_map_rtl_sw = param_find("RC_MAP_RTL_SW");
+	_parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW");
+
 	_parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1");
 	_parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2");
 	_parameter_handles.rc_map_aux3 = param_find("RC_MAP_AUX3");
+	_parameter_handles.rc_map_aux4 = param_find("RC_MAP_AUX4");
+	_parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5");
 
 	_parameter_handles.rc_scale_roll = param_find("RC_SCALE_ROLL");
 	_parameter_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH");
 	_parameter_handles.rc_scale_yaw = param_find("RC_SCALE_YAW");
+	_parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS");
 
 	/* gyro offsets */
 	_parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF");
@@ -472,10 +506,10 @@ Sensors::~Sensors()
 int
 Sensors::parameters_update()
 {
-	const unsigned int nchans = 8;
+	bool rc_valid = true;
 
 	/* rc values */
-	for (unsigned int i = 0; i < nchans; i++) {
+	for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) {
 
 		if (param_get(_parameter_handles.min[i], &(_parameters.min[i])) != OK) {
 			warnx("Failed getting min for chan %d", i);
@@ -492,32 +526,33 @@ Sensors::parameters_update()
 		if (param_get(_parameter_handles.dz[i], &(_parameters.dz[i])) != OK) {
 			warnx("Failed getting dead zone for chan %d", i);
 		}
-		if (param_get(_parameter_handles.ex[i], &(_parameters.ex[i])) != OK) {
-			warnx("Failed getting exponential gain for chan %d", i);
-		}
+		// if (param_get(_parameter_handles.ex[i], &(_parameters.ex[i])) != OK) {
+		// 	warnx("Failed getting exponential gain for chan %d", i);
+		// }
 
 		_parameters.scaling_factor[i] = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]);
 
 		/* handle blowup in the scaling factor calculation */
-		if (isnan(_parameters.scaling_factor[i]) || isinf(_parameters.scaling_factor[i])) {
+		if (!isfinite(_parameters.scaling_factor[i]) ||
+			_parameters.scaling_factor[i] * _parameters.rev[i] < 0.000001f ||
+			_parameters.scaling_factor[i] * _parameters.rev[i] > 0.2f) {
+
+			/* scaling factors do not make sense, lock them down */
 			_parameters.scaling_factor[i] = 0;
+			rc_valid = false;
 		}
 
-		/* handle wrong values */
-		// XXX TODO
-
 	}
 
+	/* handle wrong values */
+	if (!rc_valid)
+		warnx("WARNING     WARNING     WARNING\n\nRC CALIBRATION NOT SANE!\n\n");
+
 	/* remote control type */
 	if (param_get(_parameter_handles.rc_type, &(_parameters.rc_type)) != OK) {
 		warnx("Failed getting remote control type");
 	}
 
-	/* de-mixing */
-	if (param_get(_parameter_handles.rc_demix, &(_parameters.rc_demix)) != OK) {
-		warnx("Failed getting demix setting");
-	}
-
 	/* channel mapping */
 	if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) {
 		warnx("Failed getting roll chan index");
@@ -531,9 +566,30 @@ Sensors::parameters_update()
 	if (param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)) != OK) {
 		warnx("Failed getting throttle chan index");
 	}
-	if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) {
-		warnx("Failed getting mode sw chan index");
+	if (param_get(_parameter_handles.rc_map_manual_override_sw, &(_parameters.rc_map_manual_override_sw)) != OK) {
+		warnx("Failed getting override sw chan index");
+	}
+	if (param_get(_parameter_handles.rc_map_auto_mode_sw, &(_parameters.rc_map_auto_mode_sw)) != OK) {
+		warnx("Failed getting auto mode sw chan index");
+	}
+
+	if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
+		warnx("Failed getting flaps chan index");
+	}
+
+	if (param_get(_parameter_handles.rc_map_manual_mode_sw, &(_parameters.rc_map_manual_mode_sw)) != OK) {
+		warnx("Failed getting manual mode sw chan index");
+	}
+	if (param_get(_parameter_handles.rc_map_rtl_sw, &(_parameters.rc_map_rtl_sw)) != OK) {
+		warnx("Failed getting rtl sw chan index");
+	}
+	if (param_get(_parameter_handles.rc_map_sas_mode_sw, &(_parameters.rc_map_sas_mode_sw)) != OK) {
+		warnx("Failed getting sas mode sw chan index");
 	}
+	if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) {
+		warnx("Failed getting offboard control mode sw chan index");
+	}
+
 	if (param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)) != OK) {
 		warnx("Failed getting mode aux 1 index");
 	}
@@ -543,6 +599,12 @@ Sensors::parameters_update()
 	if (param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)) != OK) {
 		warnx("Failed getting mode aux 3 index");
 	}
+	if (param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)) != OK) {
+		warnx("Failed getting mode aux 4 index");
+	}
+	if (param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)) != OK) {
+		warnx("Failed getting mode aux 5 index");
+	}
 
 	if (param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll)) != OK) {
 		warnx("Failed getting rc scaling for roll");
@@ -553,16 +615,31 @@ Sensors::parameters_update()
 	if (param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)) != OK) {
 		warnx("Failed getting rc scaling for yaw");
 	}
+	if (param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps)) != OK) {
+		warnx("Failed getting rc scaling for flaps");
+	}
 
 	/* update RC function mappings */
 	_rc.function[THROTTLE] = _parameters.rc_map_throttle - 1;
 	_rc.function[ROLL] = _parameters.rc_map_roll - 1;
 	_rc.function[PITCH] = _parameters.rc_map_pitch - 1;
 	_rc.function[YAW] = _parameters.rc_map_yaw - 1;
-	_rc.function[OVERRIDE] = _parameters.rc_map_mode_sw - 1;
-	_rc.function[FUNC_0] = _parameters.rc_map_aux1 - 1;
-	_rc.function[FUNC_1] = _parameters.rc_map_aux2 - 1;
-	_rc.function[FUNC_2] = _parameters.rc_map_aux3 - 1;
+
+	_rc.function[OVERRIDE] = _parameters.rc_map_manual_override_sw - 1;
+	_rc.function[AUTO_MODE] = _parameters.rc_map_auto_mode_sw - 1;
+
+	_rc.function[FLAPS] = _parameters.rc_map_flaps - 1;
+
+	_rc.function[MANUAL_MODE] = _parameters.rc_map_manual_mode_sw - 1;
+	_rc.function[RTL] = _parameters.rc_map_rtl_sw - 1;
+	_rc.function[SAS_MODE] = _parameters.rc_map_sas_mode_sw - 1;
+	_rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1;
+
+	_rc.function[AUX_1] = _parameters.rc_map_aux1 - 1;
+	_rc.function[AUX_2] = _parameters.rc_map_aux2 - 1;
+	_rc.function[AUX_3] = _parameters.rc_map_aux3 - 1;
+	_rc.function[AUX_4] = _parameters.rc_map_aux4 - 1;
+	_rc.function[AUX_5] = _parameters.rc_map_aux5 - 1;
 
 	/* gyro offsets */
 	param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0]));
@@ -948,8 +1025,23 @@ Sensors::ppm_poll()
 
 		struct manual_control_setpoint_s manual_control;
 
-		/* get a copy first, to prevent altering values */
-		orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &manual_control);
+		/* initialize to default values */
+		manual_control.roll = NAN;
+		manual_control.pitch = NAN;
+		manual_control.yaw = NAN;
+		manual_control.throttle = NAN;
+
+		manual_control.manual_mode_switch = NAN;
+		manual_control.manual_sas_switch = NAN;
+		manual_control.return_to_launch_switch = NAN;
+		manual_control.auto_offboard_input_switch = NAN;
+
+		manual_control.flaps = NAN;
+		manual_control.aux1 = NAN;
+		manual_control.aux2 = NAN;
+		manual_control.aux3 = NAN;
+		manual_control.aux4 = NAN;
+		manual_control.aux5 = NAN;
 
 		/* require at least four channels to consider the signal valid */
 		if (rc_input.channel_count < 4)
@@ -996,79 +1088,116 @@ Sensors::ppm_poll()
 
 		manual_control.timestamp = rc_input.timestamp;
 
-		/* check if input needs to be de-mixed */
-		if (_parameters.rc_demix == (int)RC_DEMIX_DELTA) {
-			// XXX hardcoded for testing purposes, replace with inverted delta mixer from ROMFS
-
-			/* roll input - mixed roll and pitch channels */
-			manual_control.roll = 0.5f * (_rc.chan[_rc.function[ROLL]].scaled - _rc.chan[_rc.function[PITCH]].scaled);
-			/* pitch input - mixed roll and pitch channels */
-			manual_control.pitch = 0.5f * (_rc.chan[_rc.function[ROLL]].scaled + _rc.chan[_rc.function[PITCH]].scaled);
-			/* yaw input - stick right is positive and positive rotation */
-			manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled;
-			/* throttle input */
-			manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled;
-
-		/* direct pass-through of manual input */
-		} else {
-			/* roll input - rolling right is stick-wise and rotation-wise positive */
-			manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled;
-			/*
-			 * pitch input - stick down is negative, but stick down is pitching up (pos) in NED,
-			 * so reverse sign.
-			 */
-			manual_control.pitch = -1.0f * _rc.chan[_rc.function[PITCH]].scaled;
-			/* yaw input - stick right is positive and positive rotation */
-			manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled;
-			/* throttle input */
-			manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled;
-		}
+		// /* check if input needs to be de-mixed */
+		// if (_parameters.rc_demix == (int)RC_DEMIX_DELTA) {
+		// 	// XXX hardcoded for testing purposes, replace with inverted delta mixer from ROMFS
+
+		// 	/* roll input - mixed roll and pitch channels */
+		// 	manual_control.roll = 0.5f * (_rc.chan[_rc.function[ROLL]].scaled - _rc.chan[_rc.function[PITCH]].scaled);
+		// 	 pitch input - mixed roll and pitch channels 
+		// 	manual_control.pitch = 0.5f * (_rc.chan[_rc.function[ROLL]].scaled + _rc.chan[_rc.function[PITCH]].scaled);
+		// 	/* yaw input - stick right is positive and positive rotation */
+		// 	manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled;
+		// 	/* throttle input */
+		// 	manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled;
+
+		// /* direct pass-through of manual input */
+		// } else {
+
+		/* roll input - rolling right is stick-wise and rotation-wise positive */
+		manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled;
+		/*
+		 * pitch input - stick down is negative, but stick down is pitching up (pos) in NED,
+		 * so reverse sign.
+		 */
+		manual_control.pitch = limit_minus_one_to_one(-1.0f * _rc.chan[_rc.function[PITCH]].scaled);
+		/* yaw input - stick right is positive and positive rotation */
+		manual_control.yaw = limit_minus_one_to_one(_rc.chan[_rc.function[YAW]].scaled);
+		/* throttle input */
+		manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled;
+		if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f;
+		if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f;
 
-		/* limit outputs */
-		if (manual_control.roll < -1.0f) manual_control.roll = -1.0f;
-		if (manual_control.roll >  1.0f) manual_control.roll =  1.0f;
+		/* scale output */
 		if (isfinite(_parameters.rc_scale_roll) && _parameters.rc_scale_roll > 0.0f) {
 			manual_control.roll *= _parameters.rc_scale_roll;
 		}
 
-		
-		if (manual_control.pitch < -1.0f) manual_control.pitch = -1.0f;
-		if (manual_control.pitch >  1.0f) manual_control.pitch =  1.0f;
 		if (isfinite(_parameters.rc_scale_pitch) && _parameters.rc_scale_pitch > 0.0f) {
 			manual_control.pitch *= _parameters.rc_scale_pitch;
 		}
 
-		if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f;
-		if (manual_control.yaw >  1.0f) manual_control.yaw =  1.0f;
 		if (isfinite(_parameters.rc_scale_yaw) && _parameters.rc_scale_yaw > 0.0f) {
 			manual_control.yaw *= _parameters.rc_scale_yaw;
 		}
-		
-		if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f;
-		if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f;
+
+		/* override switch input */
+		manual_control.manual_override_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OVERRIDE]].scaled);
 
 		/* mode switch input */
-		manual_control.override_mode_switch = _rc.chan[_rc.function[OVERRIDE]].scaled;
-		if (manual_control.override_mode_switch < -1.0f) manual_control.override_mode_switch = -1.0f;
-		if (manual_control.override_mode_switch >  1.0f) manual_control.override_mode_switch =  1.0f;
-
-		/* aux functions */
-		manual_control.aux1_cam_pan_flaps = _rc.chan[_rc.function[FUNC_0]].scaled;
-		if (manual_control.aux1_cam_pan_flaps < -1.0f) manual_control.aux1_cam_pan_flaps = -1.0f;
-		if (manual_control.aux1_cam_pan_flaps >  1.0f) manual_control.aux1_cam_pan_flaps =  1.0f;
-
-		/* aux inputs */
-		manual_control.aux2_cam_tilt = _rc.chan[_rc.function[FUNC_1]].scaled;
-		if (manual_control.aux2_cam_tilt < -1.0f) manual_control.aux2_cam_tilt = -1.0f;
-		if (manual_control.aux2_cam_tilt >  1.0f) manual_control.aux2_cam_tilt =  1.0f;
-
-		/* aux inputs */
-		manual_control.aux3_cam_zoom = _rc.chan[_rc.function[FUNC_2]].scaled;
-		if (manual_control.aux3_cam_zoom < -1.0f) manual_control.aux3_cam_zoom = -1.0f;
-		if (manual_control.aux3_cam_zoom >  1.0f) manual_control.aux3_cam_zoom =  1.0f;
-
-		orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc);
-		orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control);
+		manual_control.auto_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[AUTO_MODE]].scaled);
+
+		/* flaps */
+		if (_rc.function[FLAPS] >= 0) {
+
+			manual_control.flaps = limit_minus_one_to_one(_rc.chan[_rc.function[FLAPS]].scaled);
+
+			if (isfinite(_parameters.rc_scale_flaps) && _parameters.rc_scale_flaps > 0.0f) {
+				manual_control.flaps *= _parameters.rc_scale_flaps;
+			}
+		}
+
+		if (_rc.function[MANUAL_MODE] >= 0) {
+			manual_control.manual_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MANUAL_MODE]].scaled);
+		}
+
+		if (_rc.function[SAS_MODE] >= 0) {
+			manual_control.manual_sas_switch = limit_minus_one_to_one(_rc.chan[_rc.function[SAS_MODE]].scaled);
+		}
+
+		if (_rc.function[RTL] >= 0) {
+			manual_control.return_to_launch_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RTL]].scaled);
+		}
+
+		if (_rc.function[OFFBOARD_MODE] >= 0) {
+			manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled);
+		}
+
+		/* aux functions, only assign if valid mapping is present */
+		if (_rc.function[AUX_1] >= 0) {
+			manual_control.aux1 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_1]].scaled);
+		}
+
+		if (_rc.function[AUX_2] >= 0) {
+			manual_control.aux2 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_2]].scaled);
+		}
+
+		if (_rc.function[AUX_3] >= 0) {
+			manual_control.aux3 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_3]].scaled);
+		}
+
+		if (_rc.function[AUX_4] >= 0) {
+			manual_control.aux4 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_4]].scaled);
+		}
+
+		if (_rc.function[AUX_5] >= 0) {
+			manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled);
+		}
+
+		/* check if ready for publishing */
+		if (_rc_pub > 0) {
+			orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc);
+		} else {
+			/* advertise the rc topic */
+			_rc_pub = orb_advertise(ORB_ID(rc_channels), &_rc);
+		}
+
+		/* check if ready for publishing */
+		if (_manual_control_pub > 0) {
+			orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control);
+		} else {
+			_manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control);
+		}
 	}
 
 }
@@ -1117,7 +1246,7 @@ Sensors::task_main()
 	memset(&raw, 0, sizeof(raw));
 	raw.timestamp = hrt_absolute_time();
 	raw.battery_voltage_v = BAT_VOL_INITIAL;
-	raw.adc_voltage_v[0] = 0.9f;
+	raw.adc_voltage_v[0] = 0.0f;
 	raw.adc_voltage_v[1] = 0.0f;
 	raw.adc_voltage_v[2] = 0.0f;
 	raw.battery_voltage_counter = 0;
@@ -1134,27 +1263,6 @@ Sensors::task_main()
 	/* advertise the sensor_combined topic and make the initial publication */
 	_sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
 
-	/* advertise the manual_control topic */
-	struct manual_control_setpoint_s manual_control;
-	manual_control.mode = MANUAL_CONTROL_MODE_ATT_YAW_POS;
-	manual_control.roll = 0.0f;
-	manual_control.pitch = 0.0f;
-	manual_control.yaw = 0.0f;
-	manual_control.throttle = 0.0f;
-	manual_control.aux1_cam_pan_flaps = 0.0f;
-	manual_control.aux2_cam_tilt = 0.0f;
-	manual_control.aux3_cam_zoom = 0.0f;
-	manual_control.aux4_cam_roll = 0.0f;
-
-	_manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control);
-
-	/* advertise the rc topic */
-	{
-		struct rc_channels_s rc;
-		memset(&rc, 0, sizeof(rc));
-		_rc_pub = orb_advertise(ORB_ID(rc_channels), &rc);
-	}
-
 	/* wakeup source(s) */
 	struct pollfd fds[1];
 
diff --git a/apps/uORB/topics/manual_control_setpoint.h b/apps/uORB/topics/manual_control_setpoint.h
index 1368cb716..261a8a4ad 100644
--- a/apps/uORB/topics/manual_control_setpoint.h
+++ b/apps/uORB/topics/manual_control_setpoint.h
@@ -48,29 +48,33 @@
  * @{
  */
 
-enum MANUAL_CONTROL_MODE
-{
-	MANUAL_CONTROL_MODE_DIRECT = 0,
-	MANUAL_CONTROL_MODE_ATT_YAW_RATE = 1,
-	MANUAL_CONTROL_MODE_ATT_YAW_POS = 2,
-	MANUAL_CONTROL_MODE_MULTIROTOR_SIMPLE = 3 /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */
-};
-
 struct manual_control_setpoint_s {
 	uint64_t timestamp;
 
-	enum MANUAL_CONTROL_MODE mode;		 /**< The current control inputs mode */
-	float roll;			 /**< ailerons roll / roll rate input */
-	float pitch;     /**< elevator / pitch / pitch rate */
-	float yaw;       /**< rudder / yaw rate / yaw */
-	float throttle;  /**< throttle / collective thrust / altitude */
+	float roll;			 	/**< ailerons roll / roll rate input */
+	float pitch;				/**< elevator / pitch / pitch rate */
+	float yaw;				/**< rudder / yaw rate / yaw */
+	float throttle;				/**< throttle / collective thrust / altitude */
+
+	float manual_override_switch;		/**< manual override mode (mandatory) */
+	float auto_mode_switch;			/**< auto mode switch (mandatory) */
+
+	/**
+	 * Any of the channels below may not be available and be set to NaN
+	 * to indicate that it does not contain valid data.
+	 */
+	float manual_mode_switch;		/**< manual mode (man, sas, alt) switch (optional) */
+	float manual_sas_switch;		/**< sas mode (rates / attitude) switch (optional) */
+	float return_to_launch_switch;		/**< return to launch switch (0 = disabled, 1 = enabled) */
+	float auto_offboard_input_switch;	/**< controller setpoint source (0 = onboard, 1 = offboard) */
 
-	float override_mode_switch;
+	float flaps;				/**< flap position */
 
-	float aux1_cam_pan_flaps;
-	float aux2_cam_tilt;
-	float aux3_cam_zoom;
-	float aux4_cam_roll;
+	float aux1;				/**< default function: camera yaw / azimuth */
+	float aux2;				/**< default function: camera pitch / tilt */
+	float aux3;				/**< default function: camera trigger */
+	float aux4;				/**< default function: camera roll */
+	float aux5;				/**< default function: payload drop */
 
 }; /**< manual control inputs */
 
diff --git a/apps/uORB/topics/rc_channels.h b/apps/uORB/topics/rc_channels.h
index fef6ef2b3..9dd54df91 100644
--- a/apps/uORB/topics/rc_channels.h
+++ b/apps/uORB/topics/rc_channels.h
@@ -50,6 +50,13 @@
  * @{
  */
 
+/**
+ * The number of RC channel inputs supported.
+ * Current (Q1/2013) radios support up to 18 channels,
+ * leaving at a sane value of 14.
+ */
+#define RC_CHANNELS_MAX   14
+
 /** 
  * This defines the mapping of the RC functions.
  * The value assigned to the specific function corresponds to the entry of
@@ -62,14 +69,18 @@ enum RC_CHANNELS_FUNCTION
   PITCH    = 2,
   YAW      = 3,
   OVERRIDE = 4,
-  FUNC_0   = 5,
-  FUNC_1   = 6,
-  FUNC_2   = 7,
-  FUNC_3   = 8,
-  FUNC_4   = 9,
-  FUNC_5   = 10,
-  FUNC_6   = 11,
-  RC_CHANNELS_FUNCTION_MAX = 12
+  AUTO_MODE = 5,
+  MANUAL_MODE = 6,
+  SAS_MODE = 7,
+  RTL = 8,
+  OFFBOARD_MODE = 9,
+  FLAPS   = 10,
+  AUX_1   = 11,
+  AUX_2   = 12,
+  AUX_3   = 13,
+  AUX_4   = 14,
+  AUX_5   = 15,
+  RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */
 };
 
 struct rc_channels_s {
@@ -78,14 +89,13 @@ struct rc_channels_s {
   uint64_t timestamp_last_valid;      /**< timestamp of last valid RC signal. */
   struct {
     float scaled;                     /**< Scaled to -1..1 (throttle: 0..1) */
-  } chan[RC_CHANNELS_FUNCTION_MAX];
-  uint8_t chan_count;                 /**< maximum number of valid channels */
+  } chan[RC_CHANNELS_MAX];
+  uint8_t chan_count;                 /**< number of valid channels */
 
   /*String array to store the names of the functions*/
   char function_name[RC_CHANNELS_FUNCTION_MAX][20];
-  uint8_t function[RC_CHANNELS_FUNCTION_MAX];
+  int8_t function[RC_CHANNELS_FUNCTION_MAX];
   uint8_t rssi;                       /**< Overall receive signal strength */
-  bool is_valid;                      /**< Inputs are valid, no timeout */
 }; /**< radio control channels. */
 
 /**
diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h
index 738ca644f..ed3fed1ab 100644
--- a/apps/uORB/topics/vehicle_status.h
+++ b/apps/uORB/topics/vehicle_status.h
@@ -87,16 +87,23 @@ enum VEHICLE_MODE_FLAG {
 }; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */
 
 enum VEHICLE_FLIGHT_MODE {
-	VEHICLE_FLIGHT_MODE_MANUAL = 0,	/**< direct manual control, same as VEHICLE_FLIGHT_MODE_ATTITUDE for multirotors */
-	VEHICLE_FLIGHT_MODE_ATTITUDE,	/**< attitude or rate stabilization, as defined by VEHICLE_ATTITUDE_MODE */
-	VEHICLE_FLIGHT_MODE_STABILIZED,	/**< attitude or rate stabilization plus velocity or position stabilization */
-	VEHICLE_FLIGHT_MODE_AUTO	/**< attitude or rate stabilization plus absolute position control and waypoints */
+	VEHICLE_FLIGHT_MODE_MANUAL = 0,		/**< direct manual control, exact mode determined by VEHICLE_MANUAL_CONTROL_MODE */
+	VEHICLE_FLIGHT_MODE_STAB,		/**< attitude or rate stabilization plus velocity or position stabilization */
+	VEHICLE_FLIGHT_MODE_HOLD,		/**< hold current position (hover or loiter around position when switched) */
+	VEHICLE_FLIGHT_MODE_AUTO		/**< attitude or rate stabilization plus absolute position control and waypoints */
 };
 
-enum VEHICLE_ATTITUDE_MODE {
-	VEHICLE_ATTITUDE_MODE_DIRECT,	/**< no attitude control, direct stick input mixing (only fixed wing) */
-	VEHICLE_ATTITUDE_MODE_RATES,	/**< body rates control mode */
-	VEHICLE_ATTITUDE_MODE_ATTITUDE	/**< tait-bryan attitude control mode */
+enum VEHICLE_MANUAL_CONTROL_MODE {
+	VEHICLE_MANUAL_CONTROL_MODE_DIRECT = 0,	/**< no attitude control, direct stick input mixing (only fixed wing) */
+	VEHICLE_MANUAL_CONTROL_MODE_RATES,	/**< body rates control mode */
+	VEHICLE_MANUAL_CONTROL_MODE_SAS		/**< stability augmented system (SAS) mode */
+};
+
+enum VEHICLE_MANUAL_SAS_MODE {
+	VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS = 0,	/**< roll, pitch and yaw absolute */
+	VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE,	/**< roll and pitch absolute, yaw rate */
+	VEHICLE_MANUAL_SAS_MODE_SIMPLE,				/**< simple mode (includes altitude hold) */
+	VEHICLE_MANUAL_SAS_MODE_ALTITUDE			/**< altitude hold */
 };
 
 /**
@@ -115,12 +122,10 @@ struct vehicle_status_s
 
 	commander_state_machine_t state_machine;	/**< current flight state, main state machine */
 	enum VEHICLE_FLIGHT_MODE flight_mode;		/**< current flight mode, as defined by mode switch */
-	enum VEHICLE_ATTITUDE_MODE attitute_mode;	/**< current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */
+	enum VEHICLE_MANUAL_CONTROL_MODE manual_control_mode;	/**< current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */
+	enum VEHICLE_MANUAL_SAS_MODE	manual_sas_mode;	/**< current stabilization mode */
 	int32_t system_type;				/**< system type, inspired by MAVLinks MAV_TYPE enum */
 
-	// uint8_t mode;
-
-
 	/* system flags - these represent the state predicates */
 
 	bool flag_system_armed;				/**< true is motors / actuators are armed */
@@ -165,11 +170,11 @@ struct vehicle_status_s
 	uint16_t errors_count3;
 	uint16_t errors_count4;
 
-//	bool remote_manual; /**< set to true by the commander when the manual-switch on the remote is set to manual */
 	bool flag_global_position_valid;     /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */
 	bool flag_local_position_valid;
 	bool flag_vector_flight_mode_ok;			/**< position estimation, battery voltage and other critical subsystems are good for autonomous flight */
-	bool flag_external_manual_override_ok;
+	bool flag_external_manual_override_ok;		/**< external override non-fatal for system. Only true for fixed wing */
+	bool flag_valid_launch_position;		/**< indicates a valid launch position */
 };
 
 /**