Merge branch master into dataman_state_nav_rewrite

28 files changed, 773 insertions, 152 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 4e6b124a4..a82655c9b 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -128,6 +128,7 @@ extern struct system_load_s system_load;
 #define FAILSAFE_DEFAULT_TIMEOUT	(3 * 1000 * 1000)	/**< hysteresis time - the failsafe will trigger after 3 seconds in this state */
 #define RC_TIMEOUT			500000
 #define DL_TIMEOUT			5 * 1000* 1000
+#define OFFBOARD_TIMEOUT		500000
 #define DIFFPRESS_TIMEOUT		2000000
 
 #define PRINT_INTERVAL	5000000
@@ -168,6 +169,7 @@ static struct vehicle_status_s status;
 static struct actuator_armed_s armed;
 static struct safety_s safety;
 static struct vehicle_control_mode_s control_mode;
+static struct offboard_control_setpoint_s sp_offboard;
 
 /* tasks waiting for low prio thread */
 typedef enum {
@@ -441,6 +443,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
 					/* ACRO */
 					main_ret = main_state_transition(status, MAIN_STATE_ACRO);
+
+				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
+					/* OFFBOARD */
+					main_ret = main_state_transition(status, MAIN_STATE_OFFBOARD);
 				}
 
 			} else {
@@ -644,6 +650,7 @@ int commander_thread_main(int argc, char *argv[])
 	main_states_str[MAIN_STATE_AUTO_LOITER]			= "AUTO_LOITER";
 	main_states_str[MAIN_STATE_AUTO_RTL]			= "AUTO_RTL";
 	main_states_str[MAIN_STATE_ACRO]			= "ACRO";
+	main_states_str[MAIN_STATE_OFFBOARD]			= "OFFBOARD";
 
 	const char *arming_states_str[ARMING_STATE_MAX];
 	arming_states_str[ARMING_STATE_INIT]			= "INIT";
@@ -666,6 +673,7 @@ int commander_thread_main(int argc, char *argv[])
 	nav_states_str[NAVIGATION_STATE_LAND]			= "LAND";
 	nav_states_str[NAVIGATION_STATE_DESCEND]		= "DESCEND";
 	nav_states_str[NAVIGATION_STATE_TERMINATION]		= "TERMINATION";
+	nav_states_str[NAVIGATION_STATE_OFFBOARD]		= "OFFBOARD";
 
 	/* pthread for slow low prio thread */
 	pthread_t commander_low_prio_thread;
@@ -817,7 +825,6 @@ int commander_thread_main(int argc, char *argv[])
 
 	/* Subscribe to offboard control data */
 	int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
-	struct offboard_control_setpoint_s sp_offboard;
 	memset(&sp_offboard, 0, sizeof(sp_offboard));
 
 	/* Subscribe to telemetry status topics */
@@ -979,6 +986,19 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
 		}
 
+		if (sp_offboard.timestamp != 0 &&
+		    sp_offboard.timestamp + OFFBOARD_TIMEOUT > hrt_absolute_time()) {
+			if (status.offboard_control_signal_lost) {
+				status.offboard_control_signal_lost = false;
+				status_changed = true;
+			}
+		} else {
+			if (!status.offboard_control_signal_lost) {
+				status.offboard_control_signal_lost = true;
+				status_changed = true;
+			}
+		}
+
 		for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
 			orb_check(telemetry_subs[i], &updated);
 
@@ -1696,6 +1716,18 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
 	/* set main state according to RC switches */
 	transition_result_t res = TRANSITION_DENIED;
 
+	/* offboard switch overrides main switch */
+	if (sp_man->offboard_switch == SWITCH_POS_ON) {
+		res = main_state_transition(status, MAIN_STATE_OFFBOARD);
+		if (res == TRANSITION_DENIED) {
+			print_reject_mode(status, "OFFBOARD");
+
+		} else {
+			return res;
+		}
+	}
+
+	/* offboard switched off or denied, check main mode switch */
 	switch (sp_man->mode_switch) {
 	case SWITCH_POS_NONE:
 		res = TRANSITION_NOT_CHANGED;
@@ -1815,6 +1847,7 @@ set_control_mode()
 	/* TODO: check this */
 	control_mode.flag_external_manual_override_ok = !status.is_rotary_wing;
 	control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON;
+	control_mode.flag_control_offboard_enabled = false;
 
 	switch (status.nav_state) {
 	case NAVIGATION_STATE_MANUAL:
@@ -1853,6 +1886,54 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
+	case NAVIGATION_STATE_OFFBOARD:
+		control_mode.flag_control_manual_enabled = false;
+		control_mode.flag_control_auto_enabled = false;
+		control_mode.flag_control_offboard_enabled = true;
+
+		switch (sp_offboard.mode) {
+		case OFFBOARD_CONTROL_MODE_DIRECT_RATES:
+			control_mode.flag_control_rates_enabled = true;
+			control_mode.flag_control_attitude_enabled = false;
+			control_mode.flag_control_altitude_enabled = false;
+			control_mode.flag_control_climb_rate_enabled = false;
+			control_mode.flag_control_position_enabled = false;
+			control_mode.flag_control_velocity_enabled = false;
+			break;
+		case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE:
+			control_mode.flag_control_rates_enabled = true;
+			control_mode.flag_control_attitude_enabled = true;
+			control_mode.flag_control_altitude_enabled = false;
+			control_mode.flag_control_climb_rate_enabled = false;
+			control_mode.flag_control_position_enabled = false;
+			control_mode.flag_control_velocity_enabled = false;
+			break;
+		case OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY:
+			control_mode.flag_control_rates_enabled = true;
+			control_mode.flag_control_attitude_enabled = true;
+			control_mode.flag_control_altitude_enabled = true; /* XXX: hack for now */
+			control_mode.flag_control_climb_rate_enabled = true;
+			control_mode.flag_control_position_enabled = true; /* XXX: hack for now */
+			control_mode.flag_control_velocity_enabled = true;
+			break;
+		case OFFBOARD_CONTROL_MODE_DIRECT_POSITION:
+			control_mode.flag_control_rates_enabled = true;
+			control_mode.flag_control_attitude_enabled = true;
+			control_mode.flag_control_altitude_enabled = true;
+			control_mode.flag_control_climb_rate_enabled = true;
+			control_mode.flag_control_position_enabled = true;
+			control_mode.flag_control_velocity_enabled = true;
+			break;
+		default:
+			control_mode.flag_control_rates_enabled = false;
+			control_mode.flag_control_attitude_enabled = false;
+			control_mode.flag_control_altitude_enabled = false;
+			control_mode.flag_control_climb_rate_enabled = false;
+			control_mode.flag_control_position_enabled = false;
+			control_mode.flag_control_velocity_enabled = false;
+		}
+		break;
+
 	case NAVIGATION_STATE_POSCTL:
 		control_mode.flag_control_manual_enabled = true;
 		control_mode.flag_control_auto_enabled = false;
diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
index ee0d08391..2e18c4284 100644
--- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
@@ -183,12 +183,12 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
         
         // Safety switch arming tests
         
-        { "transition: init to standby, no safety switch",
+        { "transition: standby to armed, no safety switch",
             { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF,
             ARMING_STATE_ARMED,
             { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
         
-        { "transition: init to standby, safety switch off",
+        { "transition: standby to armed, safety switch off",
             { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF,
             ARMING_STATE_ARMED,
             { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
@@ -286,7 +286,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
         armed.ready_to_arm = test->current_state.ready_to_arm;
         
         // Attempt transition
-        transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed);
+        transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, 0 /* no mavlink_fd */);
         
         // Validate result of transition
         ut_assert(test->assertMsg, test->expected_transition_result == result);
@@ -300,70 +300,151 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
 
 bool StateMachineHelperTest::mainStateTransitionTest(void)
 {
-	struct vehicle_status_s current_state;
-	main_state_t new_main_state;
+	// This structure represent a single test case for testing Main State transitions.
+	typedef struct {
+		const char*     assertMsg;				// Text to show when test case fails
+		uint8_t		condition_bits;				// Bits for various condition_* values
+		main_state_t	from_state;				// State prior to transition request
+		main_state_t	to_state;				// State to transition to
+		transition_result_t	expected_transition_result;	// Expected result from main_state_transition call
+	} MainTransitionTest_t;
+	
+	// Bits for condition_bits
+	#define MTT_ALL_NOT_VALID		0
+	#define MTT_ROTARY_WING			1 << 0
+	#define MTT_LOC_ALT_VALID		1 << 1
+	#define MTT_LOC_POS_VALID		1 << 2
+	#define MTT_HOME_POS_VALID		1 << 3
+	#define MTT_GLOBAL_POS_VALID		1 << 4
+	
+	static const MainTransitionTest_t rgMainTransitionTests[] = {
+		
+		// TRANSITION_NOT_CHANGED tests
+		
+		{ "no transition: identical states",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED },
+				
+		// TRANSITION_CHANGED tests
+		
+		{ "transition: MANUAL to ACRO",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_ACRO, TRANSITION_CHANGED },
+
+		{ "transition: ACRO to MANUAL",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_ACRO, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+
+		{ "transition: MANUAL to AUTO_MISSION - global position valid",
+			MTT_GLOBAL_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED },
+
+		{ "transition: AUTO_MISSION to MANUAL - global position valid",
+			MTT_GLOBAL_POS_VALID,
+			MAIN_STATE_AUTO_MISSION, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+
+		{ "transition: MANUAL to AUTO_LOITER - global position valid",
+			MTT_GLOBAL_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED },
+
+		{ "transition: AUTO_LOITER to MANUAL - global position valid",
+			MTT_GLOBAL_POS_VALID,
+			MAIN_STATE_AUTO_LOITER, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+
+		{ "transition: MANUAL to AUTO_RTL - global position valid, home position valid",
+			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED },
+
+		{ "transition: AUTO_RTL to MANUAL - global position valid, home position valid",
+			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
+			MAIN_STATE_AUTO_RTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+
+		{ "transition: MANUAL to ALTCTL - not rotary",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
+
+		{ "transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude valid",
+			MTT_ROTARY_WING | MTT_LOC_ALT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
+		
+		{ "transition: MANUAL to ALTCTL - rotary, global position valid, local altitude not valid",
+			MTT_ROTARY_WING | MTT_GLOBAL_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
+		
+		{ "transition: ALTCTL to MANUAL - local altitude valid",
+			MTT_LOC_ALT_VALID,
+			MAIN_STATE_ALTCTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+
+		{ "transition: MANUAL to POSCTL - local position not valid, global position valid",
+			MTT_GLOBAL_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_CHANGED },
+
+		{ "transition: MANUAL to POSCTL - local position valid, global position not valid",
+			MTT_LOC_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_CHANGED },
+
+		{ "transition: POSCTL to MANUAL - local position valid, global position valid",
+			MTT_LOC_POS_VALID,
+			MAIN_STATE_POSCTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+
+		// TRANSITION_DENIED tests
+
+		{ "no transition: MANUAL to AUTO_MISSION - global position not valid",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED },
+		
+		{ "no transition: MANUAL to AUTO_LOITER - global position not valid",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED },
+		
+		{ "no transition: MANUAL to AUTO_RTL - global position not valid, home position not valid",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
+		
+		{ "no transition: MANUAL to AUTO_RTL - global position not valid, home position valid",
+			MTT_HOME_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
+
+		{ "no transition: MANUAL to AUTO_RTL - global position valid, home position not valid",
+			MTT_GLOBAL_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
+		
+		{ "no transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude not valid",
+			MTT_ROTARY_WING,
+			MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_DENIED },
+		
+		{ "no transition: MANUAL to POSCTL - local position not valid, global position not valid",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_DENIED },
+	};
 	
-	// Identical states.
-	current_state.main_state = MAIN_STATE_MANUAL;
-	new_main_state = MAIN_STATE_MANUAL;
-	ut_assert("no transition: identical states",
-		  TRANSITION_NOT_CHANGED == main_state_transition(&current_state, new_main_state));
-	ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);	
-
-	// AUTO to MANUAL.
-	current_state.main_state = MAIN_STATE_AUTO;
-	new_main_state = MAIN_STATE_MANUAL;
-	ut_assert("transition changed: auto to manual",
-		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
-	ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state);
-
-	// MANUAL to ALTCTRL.
-	current_state.main_state = MAIN_STATE_MANUAL;
-	current_state.condition_local_altitude_valid = true;
-	new_main_state = MAIN_STATE_ALTCTL;
-	ut_assert("tranisition: manual to altctrl",
-		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
-	ut_assert("new state: altctrl", MAIN_STATE_ALTCTL == current_state.main_state);
-
-	// MANUAL to ALTCTRL, invalid local altitude.
-	current_state.main_state = MAIN_STATE_MANUAL;
-	current_state.condition_local_altitude_valid = false;
-	new_main_state = MAIN_STATE_ALTCTL;
-	ut_assert("no transition: invalid local altitude",
-		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
-	ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
-
-	// MANUAL to POSCTRL.
-	current_state.main_state = MAIN_STATE_MANUAL;
-	current_state.condition_local_position_valid = true;
-	new_main_state = MAIN_STATE_POSCTL;
-	ut_assert("transition: manual to posctrl",
-		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
-	ut_assert("current state: posctrl", MAIN_STATE_POSCTL == current_state.main_state);
-
-	// MANUAL to POSCTRL, invalid local position.
-	current_state.main_state = MAIN_STATE_MANUAL;
-	current_state.condition_local_position_valid = false;
-	new_main_state = MAIN_STATE_POSCTL;
-	ut_assert("no transition: invalid position",
-		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
-	ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
-
-	// MANUAL to AUTO.
-	current_state.main_state = MAIN_STATE_MANUAL;
-	current_state.condition_global_position_valid = true;
-	new_main_state = MAIN_STATE_AUTO;
-	ut_assert("transition: manual to auto",
-		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
-	ut_assert("current state: auto", MAIN_STATE_AUTO == current_state.main_state);
-
-	// MANUAL to AUTO, invalid global position.
-	current_state.main_state = MAIN_STATE_MANUAL;
-	current_state.condition_global_position_valid = false;
-	new_main_state = MAIN_STATE_AUTO;
-	ut_assert("no transition: invalid global position",
-		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
-	ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
+	size_t cMainTransitionTests = sizeof(rgMainTransitionTests) / sizeof(rgMainTransitionTests[0]);
+	for (size_t i=0; i<cMainTransitionTests; i++) {
+		const MainTransitionTest_t* test = &rgMainTransitionTests[i];
+
+		// Setup initial machine state
+		struct vehicle_status_s current_state;
+		current_state.main_state = test->from_state;
+		current_state.is_rotary_wing = test->condition_bits & MTT_ROTARY_WING;
+		current_state.condition_local_altitude_valid = test->condition_bits & MTT_LOC_ALT_VALID;
+		current_state.condition_local_position_valid = test->condition_bits & MTT_LOC_POS_VALID;
+		current_state.condition_home_position_valid = test->condition_bits & MTT_HOME_POS_VALID;
+		current_state.condition_global_position_valid = test->condition_bits & MTT_GLOBAL_POS_VALID;
+		
+		// Attempt transition
+		transition_result_t result = main_state_transition(&current_state, test->to_state);
+		
+		// Validate result of transition
+		ut_assert(test->assertMsg, test->expected_transition_result == result);
+		if (test->expected_transition_result == result) {
+			if (test->expected_transition_result == TRANSITION_CHANGED) {
+				ut_assert(test->assertMsg, test->to_state == current_state.main_state);
+			} else {
+				ut_assert(test->assertMsg, test->from_state == current_state.main_state);
+			}
+		}
+	}
+
 
 	return true;
 }
diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h
index 7f5f93801..eaf309288 100644
--- a/src/modules/commander/px4_custom_mode.h
+++ b/src/modules/commander/px4_custom_mode.h
@@ -16,6 +16,7 @@ enum PX4_CUSTOM_MAIN_MODE {
 	PX4_CUSTOM_MAIN_MODE_POSCTL,
 	PX4_CUSTOM_MAIN_MODE_AUTO,
 	PX4_CUSTOM_MAIN_MODE_ACRO,
+	PX4_CUSTOM_MAIN_MODE_OFFBOARD,
 };
 
 enum PX4_CUSTOM_SUB_MODE_AUTO {
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 7fb2e08db..6b96e3a3f 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -286,6 +286,15 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 		}
 		break;
 
+	case MAIN_STATE_OFFBOARD:
+
+		/* need offboard signal */
+		if (!status->offboard_control_signal_lost) {
+			ret = TRANSITION_CHANGED;
+		}
+
+		break;
+
 	case MAIN_STATE_MAX:
 	default:
 		break;
@@ -584,6 +593,25 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 		}
 		break;
 
+	case MAIN_STATE_OFFBOARD:
+		/* require offboard control, otherwise stay where you are */
+		if (status->offboard_control_signal_lost && !status->rc_signal_lost) {
+			status->failsafe = true;
+
+			status->nav_state = NAVIGATION_STATE_POSCTL;
+		} else if (status->offboard_control_signal_lost && status->rc_signal_lost) {
+			status->failsafe = true;
+
+			if (status->condition_local_position_valid) {
+				status->nav_state = NAVIGATION_STATE_LAND;
+			} else if (status->condition_local_altitude_valid) {
+				status->nav_state = NAVIGATION_STATE_DESCEND;
+			} else {
+				status->nav_state = NAVIGATION_STATE_TERMINATION;
+			}
+		} else {
+			status->nav_state = NAVIGATION_STATE_OFFBOARD;
+		}
 	default:
 		break;
 	}
diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
index 973de1382..f33a1d780 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
@@ -1742,7 +1742,6 @@ void AttPosEKF::FuseOptFlow()
     static float vd = 0.0f;
     static float pd = 0.0f;
     static float ptd = 0.0f;
-    static Vector3f delAng;
     static float R_LOS = 0.01f;
     static float losPred[2];
 
@@ -1820,9 +1819,6 @@ void AttPosEKF::FuseOptFlow()
             // calculate relative velocity in sensor frame
             relVelSensor = Tns*velNED_local;
 
-            // calculate delta angles in sensor axes
-            Vector3f delAngRel = Tbs*delAng;
-
             // divide velocity by range  and include angular rate effects to get predicted angular LOS rates relative to X and Y axes
             losPred[0] =  relVelSensor.y/range;
             losPred[1] = -relVelSensor.x/range;
@@ -1959,7 +1955,7 @@ void AttPosEKF::FuseOptFlow()
             }
             // normalise the quaternion states
             float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
-            if (quatMag > 1e-12)
+            if (quatMag > 1e-12f)
             {
                 for (uint8_t j= 0; j<=3; j++)
                 {
diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
index 55a472bf6..675a6870e 100644
--- a/src/modules/mavlink/mavlink_ftp.cpp
+++ b/src/modules/mavlink/mavlink_ftp.cpp
@@ -117,10 +117,10 @@ MavlinkFTP::_worker(Request *req)
 	if (crc32(req->rawData(), req->dataSize()) != messageCRC) {
 		errorCode = kErrNoRequest;
 		goto out;
-		printf("ftp: bad crc\n");
+		warnx("ftp: bad crc");
 	}
 
-	printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset);
+	//printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset);
 
 	switch (hdr->opcode) {
 	case kCmdNone:
@@ -167,9 +167,9 @@ out:
 	// handle success vs. error
 	if (errorCode == kErrNone) {
 		hdr->opcode = kRspAck;
-		printf("FTP: ack\n");
+		//warnx("FTP: ack\n");
 	} else {
-		printf("FTP: nak %u\n", errorCode);
+		warnx("FTP: nak %u", errorCode);
 		hdr->opcode = kRspNak;
 		hdr->size = 1;
 		hdr->data[0] = errorCode;
@@ -199,12 +199,18 @@ MavlinkFTP::ErrorCode
 MavlinkFTP::_workList(Request *req)
 {
 	auto hdr = req->header();
-	DIR *dp = opendir(req->dataAsCString());
+    
+    char dirPath[kMaxDataLength];
+    strncpy(dirPath, req->dataAsCString(), kMaxDataLength);
+    
+	DIR *dp = opendir(dirPath);
 
 	if (dp == nullptr) {
-		printf("FTP: can't open path '%s'\n", req->dataAsCString());
+		warnx("FTP: can't open path '%s'", dirPath);
 		return kErrNotDir;
 	}
+    
+	//warnx("FTP: list %s offset %d", dirPath, hdr->offset);
 
 	ErrorCode errorCode = kErrNone;
 	struct dirent entry, *result = nullptr;
@@ -216,6 +222,7 @@ MavlinkFTP::_workList(Request *req)
 	for (;;) {
 		// read the directory entry
 		if (readdir_r(dp, &entry, &result)) {
+			warnx("FTP: list %s readdir_r failure\n", dirPath);
 			errorCode = kErrIO;
 			break;
 		}
@@ -251,8 +258,8 @@ MavlinkFTP::_workList(Request *req)
 
 		// copy the name, which we know will fit
 		strcpy((char *)&hdr->data[offset], entry.d_name);
+		//printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]);
 		offset += strlen(entry.d_name) + 1;
-		printf("FTP: list %s\n", entry.d_name);
 	}
 
 	closedir(dp);
@@ -297,19 +304,20 @@ MavlinkFTP::_workRead(Request *req)
 	}
 
 	// Seek to the specified position
-	printf("Seek %d\n", hdr->offset);
+	//warnx("seek %d", hdr->offset);
 	if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) {
 		// Unable to see to the specified location
+		warnx("seek fail");
 		return kErrEOF;
 	}
 
 	int bytes_read = ::read(_session_fds[session_index], &hdr->data[0], kMaxDataLength);
 	if (bytes_read < 0) {
 		// Negative return indicates error other than eof
+		warnx("read fail %d", bytes_read);
 		return kErrIO;
 	}
 
-	printf("Read success %d\n", bytes_read);
 	hdr->size = bytes_read;
 
 	return kErrNone;
diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
index e22e61553..59237a4c4 100644
--- a/src/modules/mavlink/mavlink_ftp.h
+++ b/src/modules/mavlink/mavlink_ftp.h
@@ -147,20 +147,21 @@ private:
 			unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(),
 				_mavlink->get_channel(), &msg, sequence(), rawData());
 
-            _mavlink->lockMessageBufferMutex();
-            bool fError = _mavlink->message_buffer_write(&msg, len);
-            _mavlink->unlockMessageBufferMutex();
-            
-            if (!fError) {
+			_mavlink->lockMessageBufferMutex();
+			bool success = _mavlink->message_buffer_write(&msg, len);
+			_mavlink->unlockMessageBufferMutex();
+
+			if (!success) {
 				warnx("FTP TX ERR");
-			} else {
-				warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d",
-					_mavlink->get_system_id(),
-					_mavlink->get_component_id(),
-					_mavlink->get_channel(),
-					len,
-					msg.checksum);
-			}
+			} 
+			// else {
+				// warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d",
+				// 	_mavlink->get_system_id(),
+				// 	_mavlink->get_component_id(),
+				// 	_mavlink->get_channel(),
+				// 	len,
+				// 	msg.checksum);
+			// }
 		}
 
 		uint8_t		*rawData() { return &_message.data[0]; }
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index f3fb34bba..99ec98ee9 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -101,18 +101,25 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_cmd_pub(-1),
 	_flow_pub(-1),
 	_offboard_control_sp_pub(-1),
+	_local_pos_sp_pub(-1),
+	_global_vel_sp_pub(-1),
+	_att_sp_pub(-1),
+	_rates_sp_pub(-1),
 	_vicon_position_pub(-1),
 	_telemetry_status_pub(-1),
 	_rc_pub(-1),
 	_manual_pub(-1),
 	_telemetry_heartbeat_time(0),
 	_radio_status_available(false),
+	_control_mode_sub(-1),
 	_hil_frames(0),
 	_old_timestamp(0),
 	_hil_local_proj_inited(0),
 	_hil_local_alt0(0.0)
 {
+	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	memset(&hil_local_pos, 0, sizeof(hil_local_pos));
+	memset(&_control_mode, 0, sizeof(_control_mode));
 
 	// make sure the FTP server is started
 	(void)MavlinkFTP::getServer();
@@ -358,53 +365,21 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg)
 void
 MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg)
 {
-	mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint;
-	mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint);
+	mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t swarm_offboard_control;
+	mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &swarm_offboard_control);
 
-	if (mavlink_system.sysid < 4) {
+	/* Only accept system IDs from 1 to 4 */
+	if (mavlink_system.sysid >= 1 && mavlink_system.sysid <= 4) {
 		struct offboard_control_setpoint_s offboard_control_sp;
 		memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));
 
-		uint8_t ml_mode = 0;
-		bool ml_armed = false;
+		/* Convert values * 1000 back */
+		offboard_control_sp.p1 = (float)swarm_offboard_control.roll[mavlink_system.sysid - 1] / 1000.0f;
+		offboard_control_sp.p2 = (float)swarm_offboard_control.pitch[mavlink_system.sysid - 1] / 1000.0f;
+		offboard_control_sp.p3 = (float)swarm_offboard_control.yaw[mavlink_system.sysid - 1] / 1000.0f;
+		offboard_control_sp.p4 = (float)swarm_offboard_control.thrust[mavlink_system.sysid - 1] / 1000.0f;
 
-		switch (quad_motors_setpoint.mode) {
-		case 0:
-			ml_armed = false;
-			break;
-
-		case 1:
-			ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES;
-			ml_armed = true;
-
-			break;
-
-		case 2:
-			ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE;
-			ml_armed = true;
-
-			break;
-
-		case 3:
-			ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY;
-			break;
-
-		case 4:
-			ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION;
-			break;
-		}
-
-		offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1]   / (float)INT16_MAX;
-		offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1]  / (float)INT16_MAX;
-		offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1]    / (float)INT16_MAX;
-		offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX;
-
-		if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) {
-			ml_armed = false;
-		}
-
-		offboard_control_sp.armed = ml_armed;
-		offboard_control_sp.mode = static_cast<enum OFFBOARD_CONTROL_MODE>(ml_mode);
+		offboard_control_sp.mode = (enum OFFBOARD_CONTROL_MODE)swarm_offboard_control.mode;
 
 		offboard_control_sp.timestamp = hrt_absolute_time();
 
@@ -466,6 +441,8 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 	manual.r = man.r / 1000.0f;
 	manual.z = man.z / 1000.0f;
 
+	warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, (double)manual.z);
+
 	if (_manual_pub < 0) {
 		_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
 
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 80e0ea813..8d38b9973 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -36,6 +36,7 @@
  * MAVLink 1.0 uORB listener definition
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #pragma once
@@ -44,6 +45,7 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/rc_channels.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_global_position.h>
@@ -53,6 +55,7 @@
 #include <uORB/topics/offboard_control_setpoint.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/vehicle_global_velocity_setpoint.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_vicon_position.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
@@ -122,6 +125,7 @@ private:
 
 	mavlink_status_t status;
 	struct vehicle_local_position_s hil_local_pos;
+	struct vehicle_control_mode_s _control_mode;
 	orb_advert_t _global_pos_pub;
 	orb_advert_t _local_pos_pub;
 	orb_advert_t _attitude_pub;
@@ -136,12 +140,17 @@ private:
 	orb_advert_t _cmd_pub;
 	orb_advert_t _flow_pub;
 	orb_advert_t _offboard_control_sp_pub;
+	orb_advert_t _local_pos_sp_pub;
+	orb_advert_t _global_vel_sp_pub;
+	orb_advert_t _att_sp_pub;
+	orb_advert_t _rates_sp_pub;
 	orb_advert_t _vicon_position_pub;
 	orb_advert_t _telemetry_status_pub;
 	orb_advert_t _rc_pub;
 	orb_advert_t _manual_pub;
 	hrt_abstime _telemetry_heartbeat_time;
 	bool _radio_status_available;
+	int _control_mode_sub;
 	int _hil_frames;
 	uint64_t _old_timestamp;
 	bool _hil_local_proj_inited;
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index bcc3e2ae7..9b0f69226 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -106,33 +106,36 @@ public:
 
 private:
 
-	bool	_task_should_exit;		/**< if true, task should exit */
+	bool		_task_should_exit;		/**< if true, task should exit */
 	int		_control_task;			/**< task handle for task */
 	int		_mavlink_fd;			/**< mavlink fd */
 
 	int		_att_sub;				/**< vehicle attitude subscription */
 	int		_att_sp_sub;			/**< vehicle attitude setpoint */
 	int		_control_mode_sub;		/**< vehicle control mode subscription */
-	int 	_params_sub;			/**< notification of parameter updates */
-	int 	_manual_sub;			/**< notification of manual control updates */
+	int		_params_sub;			/**< notification of parameter updates */
+	int		_manual_sub;			/**< notification of manual control updates */
 	int		_arming_sub;			/**< arming status of outputs */
 	int		_local_pos_sub;			/**< vehicle local position */
-	int		_pos_sp_triplet_sub;		/**< position setpoint triplet */
+	int		_pos_sp_triplet_sub;	/**< position setpoint triplet */
+	int		_local_pos_sp_sub;		/**< offboard local position setpoint */
+	int		_global_vel_sp_sub;		/**< offboard global velocity setpoint */
 
 	orb_advert_t	_att_sp_pub;			/**< attitude setpoint publication */
 	orb_advert_t	_local_pos_sp_pub;		/**< vehicle local position setpoint publication */
 	orb_advert_t	_global_vel_sp_pub;		/**< vehicle global velocity setpoint publication */
 
 	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
-	struct vehicle_attitude_setpoint_s	_att_sp;		/**< vehicle attitude setpoint */
-	struct manual_control_setpoint_s	_manual;		/**< r/c channel data */
-	struct vehicle_control_mode_s		_control_mode;	/**< vehicle control mode */
+	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude setpoint */
+	struct manual_control_setpoint_s		_manual;		/**< r/c channel data */
+	struct vehicle_control_mode_s			_control_mode;	/**< vehicle control mode */
 	struct actuator_armed_s				_arming;		/**< actuator arming status */
-	struct vehicle_local_position_s		_local_pos;		/**< vehicle local position */
+	struct vehicle_local_position_s			_local_pos;		/**< vehicle local position */
 	struct position_setpoint_triplet_s		_pos_sp_triplet;	/**< vehicle global position setpoint triplet */
 	struct vehicle_local_position_setpoint_s	_local_pos_sp;		/**< vehicle local position setpoint */
 	struct vehicle_global_velocity_setpoint_s	_global_vel_sp;	/**< vehicle global velocity setpoint */
 
+
 	struct {
 		param_t thr_min;
 		param_t thr_max;
@@ -256,6 +259,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_arming_sub(-1),
 	_local_pos_sub(-1),
 	_pos_sp_triplet_sub(-1),
+	_global_vel_sp_sub(-1),
 
 /* publications */
 	_att_sp_pub(-1),
@@ -528,6 +532,9 @@ MulticopterPositionControl::task_main()
 	_arming_sub = orb_subscribe(ORB_ID(actuator_armed));
 	_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
 	_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
+	_local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
+	_global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
+
 
 	parameters_update(true);
 
@@ -548,6 +555,9 @@ MulticopterPositionControl::task_main()
 
 	math::Vector<3> sp_move_rate;
 	sp_move_rate.zero();
+
+	float yaw_sp_move_rate;
+
 	math::Vector<3> thrust_int;
 	thrust_int.zero();
 	math::Matrix<3, 3> R;
@@ -663,6 +673,82 @@ MulticopterPositionControl::task_main()
 					_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
 				}
 
+			} else if (_control_mode.flag_control_offboard_enabled) {
+				/* Offboard control */
+				bool updated;
+				orb_check(_pos_sp_triplet_sub, &updated);
+
+				if (updated) {
+					orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
+				}
+
+				if (_pos_sp_triplet.current.valid) {
+
+					if (_control_mode.flag_control_position_enabled && _pos_sp_triplet.current.position_valid) {
+
+						_pos_sp(0) = _pos_sp_triplet.current.x;
+						_pos_sp(1) = _pos_sp_triplet.current.y;
+						_pos_sp(2) = _pos_sp_triplet.current.z;
+						_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
+
+					} else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) {
+						/* reset position setpoint to current position if needed */
+						reset_pos_sp();
+						/* move position setpoint with roll/pitch stick */
+						sp_move_rate(0) = _pos_sp_triplet.current.vx;
+						sp_move_rate(1) = _pos_sp_triplet.current.vy;
+						yaw_sp_move_rate = _pos_sp_triplet.current.yawspeed;
+						_att_sp.yaw_body = _att.yaw + yaw_sp_move_rate * dt;
+					}
+
+					if (_control_mode.flag_control_altitude_enabled) {
+						/* reset alt setpoint to current altitude if needed */
+						reset_alt_sp();
+
+						/* move altitude setpoint with throttle stick */
+						sp_move_rate(2) = -scale_control(_pos_sp_triplet.current.vz - 0.5f, 0.5f, alt_ctl_dz);;
+					}
+
+					/* limit setpoint move rate */
+					float sp_move_norm = sp_move_rate.length();
+
+					if (sp_move_norm > 1.0f) {
+						sp_move_rate /= sp_move_norm;
+					}
+
+					/* scale to max speed and rotate around yaw */
+					math::Matrix<3, 3> R_yaw_sp;
+					R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
+					sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max);
+
+					/* move position setpoint */
+					_pos_sp += sp_move_rate * dt;
+
+					/* check if position setpoint is too far from actual position */
+					math::Vector<3> pos_sp_offs;
+					pos_sp_offs.zero();
+
+					if (_control_mode.flag_control_position_enabled) {
+						pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
+						pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
+					}
+
+					if (_control_mode.flag_control_altitude_enabled) {
+						pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
+					}
+
+					float pos_sp_offs_norm = pos_sp_offs.length();
+
+					if (pos_sp_offs_norm > 1.0f) {
+						pos_sp_offs /= pos_sp_offs_norm;
+						_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
+					}
+
+				} else {
+					reset_pos_sp();
+					reset_alt_sp();
+				}
+
 			} else {
 				/* AUTO */
 				bool updated;
@@ -709,6 +795,7 @@ MulticopterPositionControl::task_main()
 				_local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp);
 			}
 
+
 			if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) {
 				/* idle state, don't run controller and set zero thrust */
 				R.identity();
@@ -752,6 +839,7 @@ MulticopterPositionControl::task_main()
 					_vel_sp(2) = _params.land_speed;
 				}
 
+
 				if (!_control_mode.flag_control_manual_enabled) {
 					/* limit 3D speed only in non-manual modes */
 					float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length();
@@ -1039,6 +1127,7 @@ MulticopterPositionControl::task_main()
 			_reset_pos_sp = true;
 			reset_int_z = true;
 			reset_int_xy = true;
+
 		}
 
 		/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 06e915f27..eb8dcc717 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -66,6 +66,8 @@ Mission::Mission(Navigator *navigator, const char *name) :
 	_offboard_mission({0}),
 	_current_onboard_mission_index(-1),
 	_current_offboard_mission_index(-1),
+	_need_takeoff(true),
+	_takeoff(false),
 	_mission_result_pub(-1),
 	_mission_result({0}),
 	_mission_type(MISSION_TYPE_NONE),
@@ -399,7 +401,7 @@ Mission::set_mission_items()
 			takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get());
 		}
 
-		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: takeoff to %.1fm above home", takeoff_alt - _navigator->get_home_position()->alt);
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: takeoff to %.1fm above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
 
 		_mission_item.lat = _navigator->get_global_position()->lat;
 		_mission_item.lon = _navigator->get_global_position()->lon;
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index a1c6f09d0..4adf77dce 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -57,10 +57,10 @@
 
 MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
 	NavigatorMode(navigator, name),
+	_mission_item({0}),
 	_waypoint_position_reached(false),
 	_waypoint_yaw_reached(false),
-	_time_first_inside_orbit(0),
-	_mission_item({0})
+	_time_first_inside_orbit(0)
 {
 }
 
diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk
index a1e109030..637eaae59 100644
--- a/src/modules/navigator/module.mk
+++ b/src/modules/navigator/module.mk
@@ -46,6 +46,7 @@ SRCS		= navigator_main.cpp \
 		  loiter.cpp \
 		  rtl.cpp \
 		  rtl_params.c \
+		  offboard.cpp \
 		  mission_feasibility_checker.cpp \
 		  geofence.cpp \
 		  geofence_params.c
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 5dcd8859e..bf6e2ea0e 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -56,13 +56,14 @@
 #include "mission.h"
 #include "loiter.h"
 #include "rtl.h"
+#include "offboard.h"
 #include "geofence.h"
 
 /**
  * Number of navigation modes that need on_active/on_inactive calls
  * Currently: mission, loiter, and rtl
  */
-#define NAVIGATOR_MODE_ARRAY_SIZE 3
+#define NAVIGATOR_MODE_ARRAY_SIZE 4
 
 class Navigator : public control::SuperBlock
 {
@@ -109,11 +110,13 @@ public:
 	 * Getters
 	 */
 	struct vehicle_status_s*	    get_vstatus() { return &_vstatus; }
+	struct vehicle_control_mode_s*	    get_control_mode() { return &_control_mode; }
 	struct vehicle_global_position_s*   get_global_position() { return &_global_pos; }
 	struct home_position_s*		    get_home_position() { return &_home_pos; }
 	struct position_setpoint_triplet_s*		    get_position_setpoint_triplet() { return &_pos_sp_triplet; }
 	int		get_onboard_mission_sub() { return _onboard_mission_sub; }
 	int		get_offboard_mission_sub() { return _offboard_mission_sub; }
+	int		get_offboard_control_sp_sub() { return _offboard_control_sp_sub; }
 	Geofence&	get_geofence() { return _geofence; }
 	bool		get_can_loiter_at_sp() { return _can_loiter_at_sp; }
 	float		get_loiter_radius() { return _param_loiter_radius.get(); }
@@ -131,6 +134,7 @@ private:
 	int		_home_pos_sub;			/**< home position subscription */
 	int		_vstatus_sub;			/**< vehicle status subscription */
 	int		_capabilities_sub;		/**< notification of vehicle capabilities updates */
+	int		_offboard_control_sp_sub;	/*** offboard control subscription */
 	int		_control_mode_sub;		/**< vehicle control mode subscription */
 	int		_onboard_mission_sub;		/**< onboard mission subscription */
 	int		_offboard_mission_sub;		/**< offboard mission subscription */
@@ -160,6 +164,7 @@ private:
 	Mission		_mission;			/**< class that handles the missions */
 	Loiter		_loiter;			/**< class that handles loiter */
 	RTL 		_rtl;				/**< class that handles RTL */
+	Offboard	_offboard;			/**< class that handles offboard */
 
 	NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE];	/**< array of navigation modes */
 
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index bb48db829..1a5ba4c1a 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -67,6 +67,7 @@
 #include <uORB/topics/mission.h>
 #include <uORB/topics/fence.h>
 #include <uORB/topics/navigation_capabilities.h>
+#include <uORB/topics/offboard_control_setpoint.h>
 
 #include <systemlib/err.h>
 #include <systemlib/systemlib.h>
@@ -100,6 +101,7 @@ Navigator::Navigator() :
 	_home_pos_sub(-1),
 	_vstatus_sub(-1),
 	_capabilities_sub(-1),
+	_offboard_control_sp_sub(-1),
 	_control_mode_sub(-1),
 	_onboard_mission_sub(-1),
 	_offboard_mission_sub(-1),
@@ -121,7 +123,7 @@ Navigator::Navigator() :
 	_mission(this, "MIS"),
 	_loiter(this, "LOI"),
 	_rtl(this, "RTL"),
-	_pos_sp_triplet_updated(false),
+	_offboard(this, "OFF"),
 	_param_loiter_radius(this, "LOITER_RAD"),
 	_param_acceptance_radius(this, "ACC_RAD")
 {
@@ -129,6 +131,7 @@ Navigator::Navigator() :
 	_navigation_mode_array[0] = &_mission;
 	_navigation_mode_array[1] = &_loiter;
 	_navigation_mode_array[2] = &_rtl;
+	_navigation_mode_array[3] = &_offboard;
 
 	updateParams();
 }
@@ -241,6 +244,7 @@ Navigator::task_main()
 	_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
 	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
 	_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
+	_offboard_control_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
 
 	/* copy all topics first time */
 	vehicle_status_update();
@@ -363,6 +367,9 @@ Navigator::task_main()
 				break;
 			case NAVIGATION_STATE_LAND:
 			case NAVIGATION_STATE_TERMINATION:
+			case NAVIGATION_STATE_OFFBOARD:
+				_navigation_mode = &_offboard;
+				break;
 			default:
 				_navigation_mode = nullptr;
 				_can_loiter_at_sp = false;
diff --git a/src/modules/navigator/offboard.cpp b/src/modules/navigator/offboard.cpp
new file mode 100644
index 000000000..dcb5c6000
--- /dev/null
+++ b/src/modules/navigator/offboard.cpp
@@ -0,0 +1,129 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file offboard.cpp
+ *
+ * Helper class for offboard commands
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include <string.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <math.h>
+#include <fcntl.h>
+
+#include <mavlink/mavlink_log.h>
+#include <systemlib/err.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+
+#include "navigator.h"
+#include "offboard.h"
+
+Offboard::Offboard(Navigator *navigator, const char *name) :
+	NavigatorMode(navigator, name),
+	_offboard_control_sp({0})
+{
+	/* load initial params */
+	updateParams();
+	/* initial reset */
+	on_inactive();
+}
+
+Offboard::~Offboard()
+{
+}
+
+void
+Offboard::on_inactive()
+{
+}
+
+void
+Offboard::on_activation()
+{
+}
+
+void
+Offboard::on_active()
+{
+	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	bool updated;
+	orb_check(_navigator->get_offboard_control_sp_sub(), &updated);
+	if (updated) {
+		update_offboard_control_setpoint();
+	}
+
+	/* copy offboard setpoints to the corresponding topics */
+	if (_navigator->get_control_mode()->flag_control_position_enabled
+	    && _offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION) {
+		/* position control */
+		pos_sp_triplet->current.x = _offboard_control_sp.p1;
+		pos_sp_triplet->current.y = _offboard_control_sp.p2;
+		pos_sp_triplet->current.yaw = _offboard_control_sp.p3;
+		pos_sp_triplet->current.z = -_offboard_control_sp.p4;
+
+		pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD;
+		pos_sp_triplet->current.valid = true;
+		pos_sp_triplet->current.position_valid = true;
+
+		_navigator->set_position_setpoint_triplet_updated();
+
+	} else if (_navigator->get_control_mode()->flag_control_velocity_enabled
+	           && _offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY) {
+		/* velocity control */
+		pos_sp_triplet->current.vx = _offboard_control_sp.p2;
+		pos_sp_triplet->current.vy = _offboard_control_sp.p1;
+		pos_sp_triplet->current.yawspeed = _offboard_control_sp.p3;
+		pos_sp_triplet->current.vz = _offboard_control_sp.p4;
+
+		pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD;
+		pos_sp_triplet->current.valid = true;
+		pos_sp_triplet->current.velocity_valid = true;
+
+		_navigator->set_position_setpoint_triplet_updated();
+	}
+
+}
+
+
+void
+Offboard::update_offboard_control_setpoint()
+{
+	orb_copy(ORB_ID(offboard_control_setpoint), _navigator->get_offboard_control_sp_sub(), &_offboard_control_sp);
+
+}
diff --git a/src/modules/navigator/offboard.h b/src/modules/navigator/offboard.h
new file mode 100644
index 000000000..66b923bdb
--- /dev/null
+++ b/src/modules/navigator/offboard.h
@@ -0,0 +1,72 @@
+/***************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file offboard.h
+ *
+ * Helper class for offboard commands
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#ifndef NAVIGATOR_OFFBOARD_H
+#define NAVIGATOR_OFFBOARD_H
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/offboard_control_setpoint.h>
+
+#include "navigator_mode.h"
+
+class Navigator;
+
+class Offboard : public NavigatorMode
+{
+public:
+	Offboard(Navigator *navigator, const char *name);
+
+	~Offboard();
+
+	virtual void on_inactive();
+
+	virtual void on_activation();
+
+	virtual void on_active();
+private:
+	void update_offboard_control_setpoint();
+
+	struct offboard_control_setpoint_s _offboard_control_sp;
+};
+
+#endif
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index c8a3ec8f0..5deb471d6 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -665,6 +665,15 @@ PARAM_DEFINE_INT32(RC_MAP_LOITER_SW, 0);
 PARAM_DEFINE_INT32(RC_MAP_ACRO_SW, 0);
 
 /**
+ * Offboard switch channel mapping.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
+
+/**
  * Flaps channel mapping.
  *
  * @min 0
@@ -811,3 +820,20 @@ PARAM_DEFINE_FLOAT(RC_LOITER_TH, 0.5f);
  *
  */
 PARAM_DEFINE_FLOAT(RC_ACRO_TH, 0.5f);
+
+
+/**
+ * Threshold for selecting offboard mode
+ *
+ * min:-1
+ * max:+1
+ *
+ * 0-1 indicate where in the full channel range the threshold sits
+ * 		0 : min
+ * 		1 : max
+ * sign indicates polarity of comparison
+ * 		positive : true when channel>th
+ * 		negative : true when channel<th
+ *
+ */
+PARAM_DEFINE_FLOAT(RC_OFFB_TH, 0.5f);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 3307354a0..6e2d906e6 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -266,6 +266,7 @@ private:
 		int rc_map_posctl_sw;
 		int rc_map_loiter_sw;
 		int rc_map_acro_sw;
+		int rc_map_offboard_sw;
 
 		int rc_map_flaps;
 
@@ -282,12 +283,14 @@ private:
 		float rc_return_th;
 		float rc_loiter_th;
 		float rc_acro_th;
+		float rc_offboard_th;
 		bool rc_assist_inv;
 		bool rc_auto_inv;
 		bool rc_posctl_inv;
 		bool rc_return_inv;
 		bool rc_loiter_inv;
 		bool rc_acro_inv;
+		bool rc_offboard_inv;
 
 		float battery_voltage_scaling;
 		float battery_current_scaling;
@@ -321,6 +324,7 @@ private:
 		param_t rc_map_posctl_sw;
 		param_t rc_map_loiter_sw;
 		param_t rc_map_acro_sw;
+		param_t rc_map_offboard_sw;
 
 		param_t rc_map_flaps;
 
@@ -337,6 +341,7 @@ private:
 		param_t rc_return_th;
 		param_t rc_loiter_th;
 		param_t rc_acro_th;
+		param_t rc_offboard_th;
 
 		param_t battery_voltage_scaling;
 		param_t battery_current_scaling;
@@ -540,6 +545,7 @@ Sensors::Sensors() :
 	_parameter_handles.rc_map_posctl_sw = param_find("RC_MAP_POSCTL_SW");
 	_parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW");
 	_parameter_handles.rc_map_acro_sw = param_find("RC_MAP_ACRO_SW");
+	_parameter_handles.rc_map_offboard_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");
@@ -555,6 +561,7 @@ Sensors::Sensors() :
 	_parameter_handles.rc_return_th = param_find("RC_RETURN_TH");
 	_parameter_handles.rc_loiter_th = param_find("RC_LOITER_TH");
 	_parameter_handles.rc_acro_th = param_find("RC_ACRO_TH");
+	_parameter_handles.rc_offboard_th = param_find("RC_OFFB_TH");
 
 	/* gyro offsets */
 	_parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF");
@@ -707,6 +714,10 @@ Sensors::parameters_update()
 		warnx("%s", paramerr);
 	}
 
+	if (param_get(_parameter_handles.rc_map_offboard_sw, &(_parameters.rc_map_offboard_sw)) != OK) {
+		warnx("%s", paramerr);
+	}
+
 	if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
 		warnx("%s", paramerr);
 	}
@@ -735,6 +746,9 @@ Sensors::parameters_update()
 	param_get(_parameter_handles.rc_acro_th, &(_parameters.rc_acro_th));
 	_parameters.rc_acro_inv = (_parameters.rc_acro_th < 0);
 	_parameters.rc_acro_th = fabs(_parameters.rc_acro_th);
+	param_get(_parameter_handles.rc_offboard_th, &(_parameters.rc_offboard_th));
+	_parameters.rc_offboard_inv = (_parameters.rc_offboard_th < 0);
+	_parameters.rc_offboard_th = fabs(_parameters.rc_offboard_th);
 
 	/* update RC function mappings */
 	_rc.function[THROTTLE] = _parameters.rc_map_throttle - 1;
@@ -747,6 +761,7 @@ Sensors::parameters_update()
 	_rc.function[POSCTL] = _parameters.rc_map_posctl_sw - 1;
 	_rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1;
 	_rc.function[ACRO] = _parameters.rc_map_acro_sw - 1;
+	_rc.function[OFFBOARD] = _parameters.rc_map_offboard_sw - 1;
 
 	_rc.function[FLAPS] = _parameters.rc_map_flaps - 1;
 
@@ -1545,6 +1560,7 @@ Sensors::rc_poll()
 			manual.return_switch = get_rc_sw2pos_position(RETURN, _parameters.rc_return_th, _parameters.rc_return_inv);
 			manual.loiter_switch = get_rc_sw2pos_position(LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv);
 			manual.acro_switch = get_rc_sw2pos_position(ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv);
+			manual.offboard_switch = get_rc_sw2pos_position(OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv);
 
 			/* publish manual_control_setpoint topic */
 			if (_manual_control_pub > 0) {
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index 9b118205e..c5831462b 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -206,6 +206,9 @@ ORB_DEFINE(encoders, struct encoders_s);
 #include "topics/estimator_status.h"
 ORB_DEFINE(estimator_status, struct estimator_status_report);
 
+#include "topics/vehicle_force_setpoint.h"
+ORB_DEFINE(vehicle_force_setpoint, struct vehicle_force_setpoint_s);
+
 #include "topics/tecs_status.h"
 ORB_DEFINE(tecs_status, struct tecs_status_s);
 
diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h
index 910b8a623..dde237adc 100644
--- a/src/modules/uORB/topics/manual_control_setpoint.h
+++ b/src/modules/uORB/topics/manual_control_setpoint.h
@@ -98,6 +98,7 @@ struct manual_control_setpoint_s {
 	switch_pos_t posctl_switch;			/**< position control 2 position switch (optional): _ALTCTL_, POSCTL */
 	switch_pos_t loiter_switch;			/**< loiter 2 position switch (optional): _MISSION_, LOITER */
 	switch_pos_t acro_switch;			/**< acro 2 position switch (optional): _MANUAL_, ACRO */
+	switch_pos_t offboard_switch;		/**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */
 }; /**< manual control inputs */
 
 /**
diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h
index 68d3e494b..d7b131e3c 100644
--- a/src/modules/uORB/topics/offboard_control_setpoint.h
+++ b/src/modules/uORB/topics/offboard_control_setpoint.h
@@ -69,7 +69,7 @@ struct offboard_control_setpoint_s {
 	uint64_t timestamp;
 
 	enum OFFBOARD_CONTROL_MODE mode;		 /**< The current control inputs mode */
-	bool armed;	/**< Armed flag set, yes / no */
+
 	float p1;	/**< ailerons roll / roll rate input */
 	float p2;	/**< elevator / pitch / pitch rate */
 	float p3;	/**< rudder / yaw rate / yaw */
diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h
index ce42035ba..673c0e491 100644
--- a/src/modules/uORB/topics/position_setpoint_triplet.h
+++ b/src/modules/uORB/topics/position_setpoint_triplet.h
@@ -60,16 +60,26 @@ enum SETPOINT_TYPE
 	SETPOINT_TYPE_TAKEOFF,		/**< takeoff setpoint */
 	SETPOINT_TYPE_LAND,		/**< land setpoint, altitude must be ignored, descend until landing */
 	SETPOINT_TYPE_IDLE,		/**< do nothing, switch off motors or keep at idle speed (MC) */
+	SETPOINT_TYPE_OFFBOARD, 	/**< setpoint set by offboard */
 };
 
 struct position_setpoint_s
 {
 	bool valid;			/**< true if setpoint is valid */
 	enum SETPOINT_TYPE type;	/**< setpoint type to adjust behavior of position controller */
+	float x;			/**< local position setpoint in m in NED */
+	float y;			/**< local position setpoint in m in NED */
+	float z;			/**< local position setpoint in m in NED */
+	bool position_valid;	/**< true if local position setpoint valid */
+	float vx;			/**< local velocity setpoint in m in NED */
+	float vy;			/**< local velocity setpoint in m in NED */
+	float vz;			/**< local velocity setpoint in m in NED */
+	bool velocity_valid;		/**< true if local velocity setpoint valid */
 	double lat;			/**< latitude, in deg */
 	double lon;			/**< longitude, in deg */
 	float alt;			/**< altitude AMSL, in m */
 	float yaw;			/**< yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw */
+	float yawspeed;			/**< yawspeed (only for multirotors, in rad/s) */
 	float loiter_radius;		/**< loiter radius (only for fixed wing), in m */
 	int8_t loiter_direction;	/**< loiter direction: 1 = CW, -1 = CCW */
 	float pitch_min;		/**< minimal pitch angle for fixed wing takeoff waypoints */
diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h
index 829d8e57d..8978de471 100644
--- a/src/modules/uORB/topics/rc_channels.h
+++ b/src/modules/uORB/topics/rc_channels.h
@@ -56,7 +56,7 @@ enum RC_CHANNELS_FUNCTION {
 	RETURN,
 	POSCTL,
 	LOITER,
-	OFFBOARD_MODE,
+	OFFBOARD,
 	ACRO,
 	FLAPS,
 	AUX_1,
diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h
index c4b99d520..3347724a5 100644
--- a/src/modules/uORB/topics/telemetry_status.h
+++ b/src/modules/uORB/topics/telemetry_status.h
@@ -86,4 +86,14 @@ static const struct orb_metadata *telemetry_status_orb_id[TELEMETRY_STATUS_ORB_I
 	ORB_ID(telemetry_status_3),
 };
 
+// This is a hack to quiet an unused-variable warning for when telemetry_status.h is
+// included but telemetry_status_orb_id is not referenced. The inline works if you
+// choose to use it, but you can continue to just directly index into the array as well.
+// If you don't use the inline this ends up being a no-op with no additional code emitted.
+extern inline const struct orb_metadata *telemetry_status_orb_id_lookup(size_t index);
+extern inline const struct orb_metadata *telemetry_status_orb_id_lookup(size_t index)
+{
+	return telemetry_status_orb_id[index];
+}
+
 #endif /* TOPIC_TELEMETRY_STATUS_H */
diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h
index ea554006f..49e2ba4b5 100644
--- a/src/modules/uORB/topics/vehicle_control_mode.h
+++ b/src/modules/uORB/topics/vehicle_control_mode.h
@@ -74,6 +74,7 @@ struct vehicle_control_mode_s {
 
 	bool flag_control_manual_enabled;		/**< true if manual input is mixed in */
 	bool flag_control_auto_enabled;			/**< true if onboard autopilot should act */
+	bool flag_control_offboard_enabled;		/**< true if offboard control should be used */
 	bool flag_control_rates_enabled;		/**< true if rates are stabilized */
 	bool flag_control_attitude_enabled;		/**< true if attitude stabilization is mixed in */
 	bool flag_control_velocity_enabled;		/**< true if horizontal velocity (implies direction) is controlled */
diff --git a/src/modules/uORB/topics/vehicle_force_setpoint.h b/src/modules/uORB/topics/vehicle_force_setpoint.h
new file mode 100644
index 000000000..e3a7360b2
--- /dev/null
+++ b/src/modules/uORB/topics/vehicle_force_setpoint.h
@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file vehicle_force_setpoint.h
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ * Definition of force (NED) setpoint uORB topic. Typically this can be used
+ * by a position control app together with an attitude control app.
+ */
+
+#ifndef TOPIC_VEHICLE_FORCE_SETPOINT_H_
+#define TOPIC_VEHICLE_FORCE_SETPOINT_H_
+
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+struct vehicle_force_setpoint_s {
+	float x;		/**< in N NED			  		*/
+	float y;		/**< in N NED			  		*/
+	float z;		/**< in N NED			  		*/
+	float yaw;		/**< right-hand rotation around downward axis (rad, equivalent to Tait-Bryan yaw) */
+}; /**< Desired force in NED frame */
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vehicle_force_setpoint);
+
+#endif
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 56590047f..b46c00b75 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -70,7 +70,8 @@ typedef enum {
 	MAIN_STATE_AUTO_LOITER,
 	MAIN_STATE_AUTO_RTL,
 	MAIN_STATE_ACRO,
-	MAIN_STATE_MAX,
+	MAIN_STATE_OFFBOARD,
+	MAIN_STATE_MAX
 } main_state_t;
 
 // If you change the order, add or remove arming_state_t states make sure to update the arrays
@@ -106,6 +107,7 @@ typedef enum {
 	NAVIGATION_STATE_LAND,			/**< Land mode */
 	NAVIGATION_STATE_DESCEND,			/**< Descend mode (no position control) */
 	NAVIGATION_STATE_TERMINATION,		/**< Termination mode */
+	NAVIGATION_STATE_OFFBOARD,
 	NAVIGATION_STATE_MAX,
 } navigation_state_t;