Merge branch 'master' of github.com:PX4/Firmware into sf0x_test

53 files changed, 2867 insertions, 821 deletions

diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp
index d0854f5e9..b0eb4ab66 100644
--- a/src/drivers/gps/ubx.cpp
+++ b/src/drivers/gps/ubx.cpp
@@ -189,6 +189,18 @@ UBX::configure(unsigned &baudrate)
 		return 1;
 	}
 
+#ifdef UBX_CONFIGURE_SBAS
+	/* send a SBAS message to set the SBAS options */
+	memset(&_buf.payload_tx_cfg_sbas, 0, sizeof(_buf.payload_tx_cfg_sbas));
+	_buf.payload_tx_cfg_sbas.mode		= UBX_TX_CFG_SBAS_MODE;
+
+	send_message(UBX_MSG_CFG_SBAS, _buf.raw, sizeof(_buf.payload_tx_cfg_sbas));
+
+	if (wait_for_ack(UBX_MSG_CFG_SBAS, UBX_CONFIG_TIMEOUT, true) < 0) {
+		return 1;
+	}
+#endif
+
 	/* configure message rates */
 	/* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */
 
diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h
index 219a5762a..c74eb9168 100644
--- a/src/drivers/gps/ubx.h
+++ b/src/drivers/gps/ubx.h
@@ -73,6 +73,7 @@
 #define UBX_ID_CFG_MSG		0x01
 #define UBX_ID_CFG_RATE		0x08
 #define UBX_ID_CFG_NAV5		0x24
+#define UBX_ID_CFG_SBAS		0x16
 #define UBX_ID_MON_VER		0x04
 #define UBX_ID_MON_HW		0x09
 
@@ -89,6 +90,7 @@
 #define UBX_MSG_CFG_MSG		((UBX_CLASS_CFG) | UBX_ID_CFG_MSG << 8)
 #define UBX_MSG_CFG_RATE	((UBX_CLASS_CFG) | UBX_ID_CFG_RATE << 8)
 #define UBX_MSG_CFG_NAV5	((UBX_CLASS_CFG) | UBX_ID_CFG_NAV5 << 8)
+#define UBX_MSG_CFG_SBAS	((UBX_CLASS_CFG) | UBX_ID_CFG_SBAS << 8)
 #define UBX_MSG_MON_HW		((UBX_CLASS_MON) | UBX_ID_MON_HW << 8)
 #define UBX_MSG_MON_VER		((UBX_CLASS_MON) | UBX_ID_MON_VER << 8)
 
@@ -128,6 +130,11 @@
 #define UBX_TX_CFG_NAV5_DYNMODEL	7		/**< 0 Portable, 2 Stationary, 3 Pedestrian, 4 Automotive, 5 Sea, 6 Airborne <1g, 7 Airborne <2g, 8 Airborne <4g */
 #define UBX_TX_CFG_NAV5_FIXMODE		2		/**< 1 2D only, 2 3D only, 3 Auto 2D/3D */
 
+/* TX CFG-SBAS message contents */
+#define UBX_TX_CFG_SBAS_MODE_ENABLED	1				/**< SBAS enabled */
+#define UBX_TX_CFG_SBAS_MODE_DISABLED	0				/**< SBAS disabled */
+#define UBX_TX_CFG_SBAS_MODE		UBX_TX_CFG_SBAS_MODE_DISABLED	/**< SBAS enabled or disabled */
+
 /* TX CFG-MSG message contents */
 #define UBX_TX_CFG_MSG_RATE1_5HZ	0x01 		/**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
 #define UBX_TX_CFG_MSG_RATE1_1HZ	0x05		/**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
@@ -383,6 +390,15 @@ typedef struct {
 	uint32_t	reserved4;
 } ubx_payload_tx_cfg_nav5_t;
 
+/* tx cfg-sbas */
+typedef struct {
+	uint8_t		mode;
+	uint8_t		usage;
+	uint8_t		maxSBAS;
+	uint8_t		scanmode2;
+	uint32_t	scanmode1;
+} ubx_payload_tx_cfg_sbas_t;
+
 /* Tx CFG-MSG */
 typedef struct {
 	union {
@@ -413,6 +429,7 @@ typedef union {
 	ubx_payload_tx_cfg_prt_t		payload_tx_cfg_prt;
 	ubx_payload_tx_cfg_rate_t		payload_tx_cfg_rate;
 	ubx_payload_tx_cfg_nav5_t		payload_tx_cfg_nav5;
+	ubx_payload_tx_cfg_sbas_t		payload_tx_cfg_sbas;
 	ubx_payload_tx_cfg_msg_t		payload_tx_cfg_msg;
 	uint8_t					raw[];
 } ubx_buf_t;
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index 6f5dae7ad..b22bb2e07 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -910,12 +910,14 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 					// adjust filters
 					float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq();
 					float sample_rate = 1.0e6f/ticks;
+					_set_dlpf_filter(cutoff_freq_hz);
 					_accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
 					_accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
 					_accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
 
 
 					float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq();
+					_set_dlpf_filter(cutoff_freq_hz_gyro);
 					_gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
 					_gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
 					_gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
@@ -968,11 +970,9 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return _accel_filter_x.get_cutoff_freq();
 
 	case ACCELIOCSLOWPASS:
-		if (arg == 0) {
-			// allow disabling of on-chip filter using
-			// zero as desired filter frequency
-			_set_dlpf_filter(0);
-		}
+		// set hardware filtering
+		_set_dlpf_filter(arg);
+		// set software filtering
 		_accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
 		_accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
 		_accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
@@ -1053,14 +1053,11 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 	case GYROIOCGLOWPASS:
 		return _gyro_filter_x.get_cutoff_freq();
 	case GYROIOCSLOWPASS:
+		// set hardware filtering
+		_set_dlpf_filter(arg);
 		_gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
 		_gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
 		_gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
-		if (arg == 0) {
-			// allow disabling of on-chip filter using 0
-			// as desired frequency
-			_set_dlpf_filter(0);
-		}
 		return OK;
 
 	case GYROIOCSSCALE:
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 6f8d6f5f6..fbb5d4f2e 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -295,6 +295,7 @@ private:
 	float			_battery_amp_bias;	///< current sensor bias
 	float			_battery_mamphour_total;///< amp hours consumed so far
 	uint64_t		_battery_last_timestamp;///< last amp hour calculation timestamp
+	bool			_cb_flighttermination;	///< true if the flight termination circuit breaker is enabled
 
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	bool			_dsm_vcc_ctl;		///< true if relay 1 controls DSM satellite RX power
@@ -515,7 +516,8 @@ PX4IO::PX4IO(device::Device *interface) :
 	_battery_amp_per_volt(90.0f / 5.0f), // this matches the 3DR current sensor
 	_battery_amp_bias(0),
 	_battery_mamphour_total(0),
-	_battery_last_timestamp(0)
+	_battery_last_timestamp(0),
+	_cb_flighttermination(true)
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	, _dsm_vcc_ctl(false)
 #endif
@@ -1051,6 +1053,9 @@ PX4IO::task_main()
 					}
 				}
 
+				/* Update Circuit breakers */
+				_cb_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY);
+
 			}
 
 		}
@@ -1170,7 +1175,7 @@ PX4IO::io_set_arming_state()
 	}
 
 	/* Do not set failsafe if circuit breaker is enabled */
-	if (armed.force_failsafe && !circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY)) {
+	if (armed.force_failsafe && !_cb_flighttermination) {
 		set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
 	} else {
 		clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
diff --git a/src/lib/conversion/module.mk b/src/lib/conversion/module.mk
index f5f59a2dc..4593c4887 100644
--- a/src/lib/conversion/module.mk
+++ b/src/lib/conversion/module.mk
@@ -36,3 +36,5 @@
 #
 
 SRCS		 = rotation.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp
index da99aa5b1..6a2a61b04 100644
--- a/src/lib/external_lgpl/tecs/tecs.cpp
+++ b/src/lib/external_lgpl/tecs/tecs.cpp
@@ -238,6 +238,7 @@ void TECS::_update_height_demand(float demand, float state)
 	_hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last;
 	_hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last)/_DT;
 	_hgt_dem_adj_last = _hgt_dem_adj;
+
 	// Limit height rate of change
 	if (_hgt_rate_dem > _maxClimbRate) {
 		_hgt_rate_dem = _maxClimbRate;
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index c53b14349..616b855df 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -633,7 +633,29 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 			}
 		}
 		break;
-
+	case VEHICLE_CMD_NAV_GUIDED_ENABLE: {
+			transition_result_t res = TRANSITION_DENIED;
+			static main_state_t main_state_pre_offboard = MAIN_STATE_MANUAL;
+			if (status_local->main_state != MAIN_STATE_OFFBOARD) {
+				main_state_pre_offboard = status_local->main_state;
+			}
+			if (cmd->param1 > 0.5f) {
+				res = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
+				if (res == TRANSITION_DENIED) {
+					print_reject_mode(status_local, "OFFBOARD");
+					status_local->offboard_control_set_by_command = false;
+				} else {
+					/* Set flag that offboard was set via command, main state is not overridden by rc */
+					status_local->offboard_control_set_by_command = true;
+				}
+			} else {
+				/* If the mavlink command is used to enable or disable offboard control:
+				 * switch back to previous mode when disabling */
+				res = main_state_transition(status_local, main_state_pre_offboard);
+				status_local->offboard_control_set_by_command = false;
+			}
+		}
+		break;
 	case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
 	case VEHICLE_CMD_PREFLIGHT_CALIBRATION:
 	case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
@@ -775,6 +797,8 @@ int commander_thread_main(int argc, char *argv[])
 	// CIRCUIT BREAKERS
 	status.circuit_breaker_engaged_power_check = false;
 	status.circuit_breaker_engaged_airspd_check = false;
+	status.circuit_breaker_engaged_enginefailure_check = false;
+	status.circuit_breaker_engaged_gpsfailure_check = false;
 
 	/* publish initial state */
 	status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
@@ -980,8 +1004,8 @@ int commander_thread_main(int argc, char *argv[])
 	int32_t ef_throttle_thres = 1.0f;
 	int32_t ef_current2throttle_thres = 0.0f;
 	int32_t ef_time_thres = 1000.0f;
-	uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine
-											 was healty*/
+	uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine was healty */
+
 	/* check which state machines for changes, clear "changed" flag */
 	bool arming_state_changed = false;
 	bool main_state_changed = false;
@@ -1028,8 +1052,14 @@ int commander_thread_main(int argc, char *argv[])
 				param_get(_param_system_id, &(status.system_id));
 				param_get(_param_component_id, &(status.component_id));
 
-				status.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY);
-				status.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY);
+				status.circuit_breaker_engaged_power_check =
+					circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY);
+				status.circuit_breaker_engaged_airspd_check =
+					circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY);
+				status.circuit_breaker_engaged_enginefailure_check =
+					circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY);
+				status.circuit_breaker_engaged_gpsfailure_check =
+					circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY);
 
 				status_changed = true;
 
@@ -1205,24 +1235,6 @@ int commander_thread_main(int argc, char *argv[])
 
 		check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), &status_changed);
 
-		/* check if GPS fix is ok */
-		if (gps_position.fix_type >= 3 && //XXX check eph and epv ?
-			hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) {
-			/* handle the case where gps was regained */
-			if (status.gps_failure) {
-				status.gps_failure = false;
-				status_changed = true;
-				mavlink_log_critical(mavlink_fd, "gps regained");
-			}
-		} else {
-			if (!status.gps_failure) {
-				status.gps_failure = true;
-				status_changed = true;
-				mavlink_log_critical(mavlink_fd, "gps fix lost");
-			}
-		}
-
-
 		/* update home position */
 		if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed &&
 		    (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) {
@@ -1434,6 +1446,24 @@ int commander_thread_main(int argc, char *argv[])
 			globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, (float)gps_position.alt * 1.0e-3f, hrt_absolute_time());
 		}
 
+		/* check if GPS fix is ok */
+		if (status.circuit_breaker_engaged_gpsfailure_check ||
+				(gps_position.fix_type >= 3 &&
+				hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT)) {
+			/* handle the case where gps was regained */
+			if (status.gps_failure) {
+				status.gps_failure = false;
+				status_changed = true;
+				mavlink_log_critical(mavlink_fd, "gps regained");
+			}
+		} else {
+			if (!status.gps_failure) {
+				status.gps_failure = true;
+				status_changed = true;
+				mavlink_log_critical(mavlink_fd, "gps fix lost");
+			}
+		}
+
 		/* start mission result check */
 		orb_check(mission_result_sub, &updated);
 
@@ -1449,8 +1479,12 @@ int commander_thread_main(int argc, char *argv[])
 				static bool flight_termination_printed = false;
 				if (!flight_termination_printed) {
 					warnx("Flight termination because of navigator request or geofence");
+					mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
 					flight_termination_printed = true;
 				}
+				if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) {
+					mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
+				}
 			} // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
 		}
 
@@ -1612,7 +1646,7 @@ int commander_thread_main(int argc, char *argv[])
 		/* Check engine failure
 		 * only for fixed wing for now
 		 */
-		if (!circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY) &&
+		if (!status.circuit_breaker_engaged_enginefailure_check &&
 				status.is_rotary_wing == false &&
 				armed.armed &&
 				((actuator_controls.control[3] > ef_throttle_thres &&
@@ -1946,6 +1980,11 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 	/* set main state according to RC switches */
 	transition_result_t res = TRANSITION_DENIED;
 
+	/* if offboard is set allready by a mavlink command, abort */
+	if (status.offboard_control_set_by_command) {
+		return main_state_transition(status_local, MAIN_STATE_OFFBOARD);
+	}
+
 	/* offboard switch overrides main switch */
 	if (sp_man->offboard_switch == SWITCH_POS_ON) {
 		res = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
@@ -2138,21 +2177,26 @@ set_control_mode()
 			control_mode.flag_control_position_enabled = false;
 			control_mode.flag_control_velocity_enabled = false;
 			break;
-		case OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY:
+		case OFFBOARD_CONTROL_MODE_DIRECT_FORCE:
 			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;
+			control_mode.flag_control_attitude_enabled = false;
+			control_mode.flag_control_force_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_POSITION:
+		case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED:
+		case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED:
+		case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED:
+		case OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED:
 			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;
+			//XXX: the flags could depend on sp_offboard.ignore
 			break;
 		default:
 			control_mode.flag_control_rates_enabled = false;
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index f4bc7fd7f..1b0c4258b 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -159,7 +159,7 @@ PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f);
  * @min 0.0f
  * @max 7.0f
  */
-PARAM_DEFINE_FLOAT(COM_EF_TIME, 5.0f);
+PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f);
 
 /** RC loss time threshold
  *
diff --git a/src/modules/commander/commander_tests/commander_tests.cpp b/src/modules/commander/commander_tests/commander_tests.cpp
index 0abb84a82..2bfa5d0bb 100644
--- a/src/modules/commander/commander_tests/commander_tests.cpp
+++ b/src/modules/commander/commander_tests/commander_tests.cpp
@@ -48,7 +48,5 @@ extern "C" __EXPORT int commander_tests_main(int argc, char *argv[]);
 
 int commander_tests_main(int argc, char *argv[])
 {
-	stateMachineHelperTest();
-
-	return 0;
+	return stateMachineHelperTest() ? 0 : -1;
 }
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 08dda2fab..874090e93 100644
--- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
@@ -49,7 +49,7 @@ public:
 	StateMachineHelperTest();
 	virtual ~StateMachineHelperTest();
 
-	virtual void runTests(void);
+	virtual bool run_tests(void);
 
 private:
 	bool armingStateTransitionTest();
@@ -488,16 +488,13 @@ bool StateMachineHelperTest::isSafeTest(void)
 	return true;
 }
 
-void StateMachineHelperTest::runTests(void)
+bool StateMachineHelperTest::run_tests(void)
 {
 	ut_run_test(armingStateTransitionTest);
 	ut_run_test(mainStateTransitionTest);
 	ut_run_test(isSafeTest);
+	
+	return (_tests_failed == 0);
 }
 
-void stateMachineHelperTest(void)
-{
-	StateMachineHelperTest* test = new StateMachineHelperTest();
-    test->runTests();
-	test->printResults();
-}
+ut_declare_test(stateMachineHelperTest, StateMachineHelperTest)
\ No newline at end of file
diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.h b/src/modules/commander/commander_tests/state_machine_helper_test.h
index bbf66255e..cf6719095 100644
--- a/src/modules/commander/commander_tests/state_machine_helper_test.h
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.h
@@ -39,6 +39,6 @@
 #ifndef STATE_MACHINE_HELPER_TEST_H_
 #define STATE_MACHINE_HELPER_TEST_
 
-void stateMachineHelperTest(void);
+bool stateMachineHelperTest(void);
 
 #endif /* STATE_MACHINE_HELPER_TEST_H_ */
diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c
index ca1fe9bbb..b2355d4d8 100644
--- a/src/modules/dataman/dataman.c
+++ b/src/modules/dataman/dataman.c
@@ -797,7 +797,7 @@ start(void)
 	sem_init(&g_init_sema, 1, 0);
 
 	/* start the worker thread */
-	if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, task_main, NULL)) <= 0) {
+	if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2000, task_main, NULL)) <= 0) {
 		warn("task start failed");
 		return -1;
 	}
diff --git a/src/modules/dataman/module.mk b/src/modules/dataman/module.mk
index 234607b3d..7ebe09fb7 100644
--- a/src/modules/dataman/module.mk
+++ b/src/modules/dataman/module.mk
@@ -40,3 +40,5 @@ MODULE_COMMAND	= dataman
 SRCS		= dataman.c
 
 MODULE_STACKSIZE = 1200
+
+MAXOPTIMIZATION = -Os
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 97abb76a9..2c50e5c75 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -1369,7 +1369,7 @@ FixedwingEstimator::task_main()
 					if (newRangeData) {
 						_ekf->fuseRngData = true;
 						_ekf->useRangeFinder = true;
-						_ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 500.0f));
+						_ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f));
 						_ekf->GroundEKF();
 					}
 
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 00b12fffa..6017369aa 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -88,7 +88,6 @@
 #include <launchdetection/LaunchDetector.h>
 #include <ecl/l1/ecl_l1_pos_controller.h>
 #include <external_lgpl/tecs/tecs.h>
-#include <drivers/drv_range_finder.h>
 #include "landingslope.h"
 #include "mtecs/mTecs.h"
 
@@ -146,7 +145,6 @@ private:
 	int 		_params_sub;			/**< notification of parameter updates */
 	int 		_manual_control_sub;		/**< notification of manual control updates */
 	int		_sensor_combined_sub;		/**< for body frame accelerations */
-	int		_range_finder_sub;		/**< range finder subscription */
 
 	orb_advert_t	_attitude_sp_pub;		/**< attitude setpoint */
 	orb_advert_t	_tecs_status_pub;		/**< TECS status publication */
@@ -162,17 +160,16 @@ private:
 	struct vehicle_global_position_s		_global_pos;			/**< global vehicle position */
 	struct position_setpoint_triplet_s		_pos_sp_triplet;		/**< triplet of mission items */
 	struct sensor_combined_s			_sensor_combined;		/**< for body frame accelerations */
-	struct range_finder_report 			_range_finder;			/**< range finder report */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
 	/* land states */
-	/* not in non-abort mode for landing yet */
 	bool land_noreturn_horizontal;
 	bool land_noreturn_vertical;
 	bool land_stayonground;
 	bool land_motor_lim;
 	bool land_onslope;
+	bool land_useterrain;
 
 	/* takeoff/launch states */
 	LaunchDetectionResult launch_detection_state;
@@ -243,7 +240,9 @@ private:
 		float land_flare_alt_relative;
 		float land_thrust_lim_alt_relative;
 		float land_heading_hold_horizontal_distance;
-		float range_finder_rel_alt;
+		float land_flare_pitch_min_deg;
+		float land_flare_pitch_max_deg;
+		int land_use_terrain_estimate;
 
 	}		_parameters;			/**< local copies of interesting parameters */
 
@@ -289,7 +288,9 @@ private:
 		param_t land_flare_alt_relative;
 		param_t land_thrust_lim_alt_relative;
 		param_t land_heading_hold_horizontal_distance;
-		param_t range_finder_rel_alt;
+		param_t land_flare_pitch_min_deg;
+		param_t land_flare_pitch_max_deg;
+		param_t land_use_terrain_estimate;
 
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
@@ -321,12 +322,6 @@ private:
 	bool		vehicle_airspeed_poll();
 
 	/**
-	 * Check for range finder updates.
-	 */
-	bool		range_finder_poll();
-
-
-	/**
 	 * Check for position updates.
 	 */
 	void		vehicle_attitude_poll();
@@ -347,9 +342,9 @@ private:
 	void navigation_capabilities_publish();
 
 	/**
-	 * Get the relative alt either from the difference between estimate and waypoint or from the laser range finder
+	 * Return the terrain estimate during landing: uses the wp altitude value or the terrain estimate if available
 	 */
-	float get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt);
+	float get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos);
 
 	/**
 	 * Control position.
@@ -423,7 +418,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_params_sub(-1),
 	_manual_control_sub(-1),
 	_sensor_combined_sub(-1),
-	_range_finder_sub(-1),
 
 /* publications */
 	_attitude_sp_pub(-1),
@@ -441,7 +435,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_global_pos(),
 	_pos_sp_triplet(),
 	_sensor_combined(),
-	_range_finder(),
 
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
@@ -451,6 +444,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	land_stayonground(false),
 	land_motor_lim(false),
 	land_onslope(false),
+	land_useterrain(false),
 	launch_detection_state(LAUNCHDETECTION_RES_NONE),
 	last_manual(false),
 	landingslope(),
@@ -489,7 +483,9 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT");
 	_parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT");
 	_parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST");
-	_parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT");
+	_parameter_handles.land_flare_pitch_min_deg = param_find("FW_FLARE_PMIN");
+	_parameter_handles.land_flare_pitch_max_deg = param_find("FW_FLARE_PMAX");
+	_parameter_handles.land_use_terrain_estimate= param_find("FW_LND_USETER");
 
 	_parameter_handles.time_const = 			param_find("FW_T_TIME_CONST");
 	_parameter_handles.time_const_throt = 			param_find("FW_T_THRO_CONST");
@@ -590,8 +586,9 @@ FixedwingPositionControl::parameters_update()
 	}
 
 	param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance));
-
-	param_get(_parameter_handles.range_finder_rel_alt, &(_parameters.range_finder_rel_alt));
+	param_get(_parameter_handles.land_flare_pitch_min_deg, &(_parameters.land_flare_pitch_min_deg));
+	param_get(_parameter_handles.land_flare_pitch_max_deg, &(_parameters.land_flare_pitch_max_deg));
+	param_get(_parameter_handles.land_use_terrain_estimate, &(_parameters.land_use_terrain_estimate));
 
 	_l1_control.set_l1_damping(_parameters.l1_damping);
 	_l1_control.set_l1_period(_parameters.l1_period);
@@ -695,20 +692,6 @@ FixedwingPositionControl::vehicle_airspeed_poll()
 	return airspeed_updated;
 }
 
-bool
-FixedwingPositionControl::range_finder_poll()
-{
-	/* check if there is a range finder measurement */
-	bool range_finder_updated;
-	orb_check(_range_finder_sub, &range_finder_updated);
-
-	if (range_finder_updated) {
-		orb_copy(ORB_ID(sensor_range_finder), _range_finder_sub, &_range_finder);
-	}
-
-	return range_finder_updated;
-}
-
 void
 FixedwingPositionControl::vehicle_attitude_poll()
 {
@@ -846,21 +829,23 @@ void FixedwingPositionControl::navigation_capabilities_publish()
 	}
 }
 
-float FixedwingPositionControl::get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt)
+float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos)
 {
-	float rel_alt_estimated = current_alt - land_setpoint_alt;
-
-	/* only use range finder if:
-	 * parameter (range_finder_use_relative_alt) > 0
-	 * the measurement is valid
-	 * the estimated relative altitude (from global altitude estimate and landing waypoint) <= range_finder_use_relative_alt
-	 */
-	if (range_finder_use_relative_alt < 0 || !range_finder.valid || range_finder.distance > range_finder_use_relative_alt ) {
-		return rel_alt_estimated;
+	if (!isfinite(global_pos.terrain_alt)) {
+		return land_setpoint_alt;
 	}
 
-	return range_finder.distance;
-
+	/* Decide if the terrain estimation can be used, once we switched to using the terrain we stick with it
+	 * for the whole landing */
+	if (_parameters.land_use_terrain_estimate && (global_pos.terrain_alt_valid || land_useterrain)) {
+		if(!land_useterrain) {
+			mavlink_log_info(_mavlink_fd, "#audio: Landing, using terrain estimate");
+			land_useterrain = true;
+		}
+		return global_pos.terrain_alt;
+	} else {
+		return land_setpoint_alt;
+	}
 }
 
 bool
@@ -965,10 +950,17 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
 
 			float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
+			float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
 
 			/* Horizontal landing control */
 			/* switch to heading hold for the last meters, continue heading hold after */
 			float wp_distance = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
+			/* calculate a waypoint distance value which is 0 when the aircraft is behind the waypoint */
+			float wp_distance_save = wp_distance;
+			if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) >= math::radians(90.0f)) {
+				wp_distance_save = 0.0f;
+			}
+
 			//warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO");
 			if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) {
 
@@ -1004,29 +996,30 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 			/* Vertical landing control */
 			//xxx: using the tecs altitude controller for slope control for now
-
-//				/* do not go down too early */
-//				if (wp_distance > 50.0f) {
-//					altitude_error = (_global_triplet.current.alt + 25.0f) - _global_pos.alt;
-//				}
 			/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
 			// XXX this could make a great param
 
-			float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(pos_sp_triplet.current.param1)
 			float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
 			float airspeed_land = 1.3f * _parameters.airspeed_min;
 			float airspeed_approach = 1.3f * _parameters.airspeed_min;
 
+			/* Get an estimate of the terrain altitude if available, otherwise terrain_alt will be
+			 * equal to _pos_sp_triplet.current.alt */
+			float terrain_alt = get_terrain_altitude_landing(_pos_sp_triplet.current.alt, _global_pos);
+
 			/* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */
-			float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f;
+			float L_altitude_rel = _pos_sp_triplet.previous.valid ?
+				_pos_sp_triplet.previous.alt - terrain_alt : 0.0f;
 
-			float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
 			float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp);
 
-			float relative_alt = get_relative_landingalt(_pos_sp_triplet.current.alt, _global_pos.alt, _range_finder, _parameters.range_finder_rel_alt);
-
-			if ( (relative_alt < landingslope.flare_relative_alt()) || land_noreturn_vertical) {  //checking for land_noreturn to avoid unwanted climb out
-
+			/* Check if we should start flaring with a vertical and a
+			 * horizontal limit (with some tolerance)
+			 * The horizontal limit is only applied when we are in front of the wp
+			 */
+			if (((_global_pos.alt < terrain_alt + landingslope.flare_relative_alt()) &&
+						(wp_distance_save < landingslope.flare_length() + 5.0f)) ||
+					land_noreturn_vertical) {  //checking for land_noreturn to avoid unwanted climb out
 				/* land with minimal speed */
 
 //					/* force TECS to only control speed with pitch, altitude is only implicitely controlled now */
@@ -1035,7 +1028,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				/* kill the throttle if param requests it */
 				throttle_max = _parameters.throttle_max;
 
-				 if (relative_alt < landingslope.motor_lim_relative_alt() || land_motor_lim) {
+				 if (_global_pos.alt < terrain_alt + landingslope.motor_lim_relative_alt() || land_motor_lim) {
 					throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
 					if (!land_motor_lim) {
 						land_motor_lim  = true;
@@ -1053,12 +1046,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					land_stayonground = true;
 				}
 
-				tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + flare_curve_alt_rel,
+				tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel,
 						calculate_target_airspeed(airspeed_land), eas2tas,
-						flare_pitch_angle_rad, math::radians(15.0f),
+						 math::radians(_parameters.land_flare_pitch_min_deg),
+						 math::radians(_parameters.land_flare_pitch_max_deg),
 						0.0f, throttle_max, throttle_land,
-						false, flare_pitch_angle_rad,
-						_pos_sp_triplet.current.alt + relative_alt, ground_speed,
+						false,  land_motor_lim ? math::radians(_parameters.land_flare_pitch_min_deg) : math::radians(_parameters.pitch_limit_min),
+						_global_pos.alt, ground_speed,
 						land_motor_lim ? TECS_MODE_LAND_THROTTLELIM : TECS_MODE_LAND);
 
 				if (!land_noreturn_vertical) {
@@ -1079,8 +1073,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				  * if current position is below the slope continue at previous wp altitude
 				  * until the intersection with slope
 				  * */
-				float altitude_desired_rel = relative_alt;
-				if (relative_alt > landing_slope_alt_rel_desired || land_onslope) {
+				float altitude_desired_rel;
+				if (_global_pos.alt > terrain_alt + landing_slope_alt_rel_desired || land_onslope) {
 					/* stay on slope */
 					altitude_desired_rel = landing_slope_alt_rel_desired;
 					if (!land_onslope) {
@@ -1089,10 +1083,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					}
 				} else {
 					/* continue horizontally */
-					altitude_desired_rel =  _pos_sp_triplet.previous.valid ? L_altitude_rel : relative_alt;
+					altitude_desired_rel =  _pos_sp_triplet.previous.valid ? L_altitude_rel :
+						_global_pos.alt - terrain_alt;
 				}
 
-				tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + altitude_desired_rel,
+				tecs_update_pitch_throttle(terrain_alt + altitude_desired_rel,
 						calculate_target_airspeed(airspeed_approach), eas2tas,
 						math::radians(_parameters.pitch_limit_min),
 						math::radians(_parameters.pitch_limit_max),
@@ -1101,7 +1096,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 						_parameters.throttle_cruise,
 						false,
 						math::radians(_parameters.pitch_limit_min),
-						_pos_sp_triplet.current.alt + relative_alt,
+						_global_pos.alt,
 						ground_speed);
 			}
 
@@ -1277,7 +1272,6 @@ FixedwingPositionControl::task_main()
 	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	_range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
 
 	/* rate limit control mode updates to 5Hz */
 	orb_set_interval(_control_mode_sub, 200);
@@ -1356,7 +1350,6 @@ FixedwingPositionControl::task_main()
 			vehicle_setpoint_poll();
 			vehicle_sensor_combined_poll();
 			vehicle_airspeed_poll();
-			range_finder_poll();
 			// vehicle_baro_poll();
 
 			math::Vector<3> ground_speed(_global_pos.vel_n, _global_pos.vel_e,  _global_pos.vel_d);
@@ -1420,6 +1413,7 @@ void FixedwingPositionControl::reset_landing_state()
 	land_stayonground = false;
 	land_motor_lim = false;
 	land_onslope = false;
+	land_useterrain = false;
 }
 
 void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas,
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
index 847ecbb5c..c00d82232 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
@@ -412,12 +412,28 @@ PARAM_DEFINE_FLOAT(FW_LND_TLALT, -1.0f);
 PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f);
 
 /**
- * Relative altitude threshold for range finder measurements for use during landing
+ * Enable or disable usage of terrain estimate during landing
  *
- * range finder measurements will only be used if the estimated relative altitude (gobal_pos.alt - landing_waypoint.alt) is < FW_LND_RFRALT
- * set to < 0 to disable
- * the correct value of this parameter depends on your range measuring device as well as on the terrain at the landing location
+ * 0: disabled, 1: enabled
  *
  * @group L1 Control
  */
-PARAM_DEFINE_FLOAT(FW_LND_RFRALT, -1.0f);
+PARAM_DEFINE_INT32(FW_LND_USETER, 0);
+
+/**
+ * Flare, minimum pitch
+ *
+ * Minimum pitch during flare, a positive sign means nose up
+ * Applied once FW_LND_TLALT is reached
+ *
+ */
+PARAM_DEFINE_FLOAT(FW_FLARE_PMIN, 2.5f);
+
+/**
+ * Flare, maximum pitch
+ *
+ * Maximum pitch during flare, a positive sign means nose up
+ * Applied once FW_LND_TLALT is reached
+ *
+ */
+PARAM_DEFINE_FLOAT(FW_FLARE_PMAX, 15.0f);
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index e49288a74..bec706bad 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -68,6 +68,13 @@ PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING);
  * @group MAVLink
  */
 PARAM_DEFINE_INT32(MAV_USEHILGPS, 0);
+/**
+ * Forward external setpoint messages
+ * If set to 1 incomming external setpoint messages will be directly forwarded to the controllers if in offboard
+ * control mode
+ * @group MAVLink
+ */
+PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1);
 
 mavlink_system_t mavlink_system = {
 	100,
diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h
index 00d427636..0cd08769e 100644
--- a/src/modules/mavlink/mavlink_bridge_header.h
+++ b/src/modules/mavlink/mavlink_bridge_header.h
@@ -84,7 +84,7 @@ void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int leng
 extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan);
 extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan);
 
-#include <v1.0/pixhawk/mavlink.h>
+#include <v1.0/common/mavlink.h>
 
 __END_DECLS
 
diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
index 5b65dc369..f17497aa8 100644
--- a/src/modules/mavlink/mavlink_ftp.cpp
+++ b/src/modules/mavlink/mavlink_ftp.cpp
@@ -31,38 +31,39 @@
  *
  ****************************************************************************/
 
+/// @file mavlink_ftp.cpp
+///	@author px4dev, Don Gagne <don@thegagnes.com>
+
 #include <crc32.h>
 #include <unistd.h>
 #include <stdio.h>
 #include <fcntl.h>
 #include <sys/stat.h>
+#include <errno.h>
 
 #include "mavlink_ftp.h"
+#include "mavlink_tests/mavlink_ftp_test.h"
 
 // Uncomment the line below to get better debug output. Never commit with this left on.
 //#define MAVLINK_FTP_DEBUG
 
-MavlinkFTP *MavlinkFTP::_server;
-
 MavlinkFTP *
-MavlinkFTP::getServer()
+MavlinkFTP::get_server(void)
 {
-	// XXX this really cries out for some locking...
-	if (_server == nullptr) {
-		_server = new MavlinkFTP;
-	}
-	return _server;
+	static MavlinkFTP server;
+        return &server;
 }
 
 MavlinkFTP::MavlinkFTP() :
-	_session_fds{},
-	_workBufs{},
-	_workFree{},
-	_lock{}
+	_request_bufs{},
+	_request_queue{},
+	_request_queue_sem{},
+	_utRcvMsgFunc{},
+	_ftp_test{}
 {
 	// initialise the request freelist
-	dq_init(&_workFree);
-	sem_init(&_lock, 0, 1);
+	dq_init(&_request_queue);
+	sem_init(&_request_queue_sem, 0, 1);
 
 	// initialize session list
 	for (size_t i=0; i<kMaxSession; i++) {
@@ -71,167 +72,258 @@ MavlinkFTP::MavlinkFTP() :
 
 	// drop work entries onto the free list
 	for (unsigned i = 0; i < kRequestQueueSize; i++) {
-		_qFree(&_workBufs[i]);
+		_return_request(&_request_bufs[i]);
 	}
 
 }
 
+#ifdef MAVLINK_FTP_UNIT_TEST
+void
+MavlinkFTP::set_unittest_worker(ReceiveMessageFunc_t rcvMsgFunc, MavlinkFtpTest *ftp_test)
+{
+	_utRcvMsgFunc = rcvMsgFunc;
+	_ftp_test = ftp_test;
+}
+#endif
+
 void
 MavlinkFTP::handle_message(Mavlink* mavlink, mavlink_message_t *msg)
 {
 	// get a free request
-	auto req = _dqFree();
+	struct Request* req = _get_request();
 
 	// if we couldn't get a request slot, just drop it
-	if (req != nullptr) {
-
-		// decode the request
-		if (req->decode(mavlink, msg)) {
+	if (req == nullptr) {
+		warnx("Dropping FTP request: queue full\n");
+		return;
+	}
 
-			// and queue it for the worker
-			work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0);
-		} else {
-			_qFree(req);
+	if (msg->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) {
+		mavlink_msg_file_transfer_protocol_decode(msg, &req->message);
+		
+#ifdef MAVLINK_FTP_UNIT_TEST
+		if (!_utRcvMsgFunc) {
+			warnx("Incorrectly written unit test\n");
+			return;
+		}
+		// We use fake ids when unit testing
+		req->serverSystemId = MavlinkFtpTest::serverSystemId;
+		req->serverComponentId = MavlinkFtpTest::serverComponentId;
+		req->serverChannel = MavlinkFtpTest::serverChannel;
+#else
+		// Not unit testing, use the real thing
+		req->serverSystemId = mavlink->get_system_id();
+		req->serverComponentId = mavlink->get_component_id();
+		req->serverChannel = mavlink->get_channel();
+#endif
+		
+		// This is the system id we want to target when sending
+		req->targetSystemId = msg->sysid;
+			
+		if (req->message.target_system == req->serverSystemId) {
+			req->mavlink = mavlink;
+#ifdef MAVLINK_FTP_UNIT_TEST
+			// We are running in Unit Test mode. Don't queue, just call _worket directly.
+			_process_request(req);
+#else
+			// We are running in normal mode. Queue the request to the worker
+			work_queue(LPWORK, &req->work, &MavlinkFTP::_worker_trampoline, req, 0);
+#endif
+            return;
 		}
 	}
+
+	_return_request(req);
 }
 
+/// @brief Queued static work queue routine to handle mavlink messages
 void
-MavlinkFTP::_workerTrampoline(void *arg)
+MavlinkFTP::_worker_trampoline(void *arg)
 {
-	auto req = reinterpret_cast<Request *>(arg);
-	auto server = MavlinkFTP::getServer();
+	Request* req = reinterpret_cast<Request *>(arg);
+	MavlinkFTP* server = MavlinkFTP::get_server();
 
 	// call the server worker with the work item
-	server->_worker(req);
+	server->_process_request(req);
 }
 
+/// @brief Processes an FTP message
 void
-MavlinkFTP::_worker(Request *req)
+MavlinkFTP::_process_request(Request *req)
 {
-	auto hdr = req->header();
+	PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&req->message.payload[0]);
+
 	ErrorCode errorCode = kErrNone;
-	uint32_t messageCRC;
 
 	// basic sanity checks; must validate length before use
-	if (hdr->size > kMaxDataLength) {
-		errorCode = kErrNoRequest;
-		goto out;
-	}
-
-	// check request CRC to make sure this is one of ours
-	messageCRC = hdr->crc32;
-	hdr->crc32 = 0;
-	hdr->padding[0] = 0;
-	hdr->padding[1] = 0;
-	hdr->padding[2] = 0;
-	if (crc32(req->rawData(), req->dataSize()) != messageCRC) {
-		errorCode = kErrNoRequest;
+	if (payload->size > kMaxDataLength) {
+		errorCode = kErrInvalidDataSize;
 		goto out;
-		warnx("ftp: bad crc");
 	}
 
 #ifdef MAVLINK_FTP_DEBUG
-	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->serverChannel, payload->opcode, payload->size, payload->offset);
 #endif
 
-	switch (hdr->opcode) {
+	switch (payload->opcode) {
 	case kCmdNone:
 		break;
 
-	case kCmdTerminate:
-		errorCode = _workTerminate(req);
+	case kCmdTerminateSession:
+		errorCode = _workTerminate(payload);
+		break;
+
+	case kCmdResetSessions:
+		errorCode = _workReset(payload);
+		break;
+
+	case kCmdListDirectory:
+		errorCode = _workList(payload);
+		break;
+
+	case kCmdOpenFileRO:
+		errorCode = _workOpen(payload, O_RDONLY);
+		break;
+
+	case kCmdCreateFile:
+		errorCode = _workOpen(payload, O_CREAT | O_EXCL | O_WRONLY);
+		break;
+
+	case kCmdOpenFileWO:
+		errorCode = _workOpen(payload, O_CREAT | O_WRONLY);
 		break;
 
-	case kCmdReset:
-		errorCode = _workReset();
+	case kCmdReadFile:
+		errorCode = _workRead(payload);
 		break;
 
-	case kCmdList:
-		errorCode = _workList(req);
+	case kCmdWriteFile:
+		errorCode = _workWrite(payload);
 		break;
 
-	case kCmdOpen:
-		errorCode = _workOpen(req, false);
+	case kCmdRemoveFile:
+		errorCode = _workRemoveFile(payload);
 		break;
 
-	case kCmdCreate:
-		errorCode = _workOpen(req, true);
+	case kCmdRename:
+		errorCode = _workRename(payload);
 		break;
 
-	case kCmdRead:
-		errorCode = _workRead(req);
+	case kCmdTruncateFile:
+		errorCode = _workTruncateFile(payload);
 		break;
 
-	case kCmdWrite:
-		errorCode = _workWrite(req);
+	case kCmdCreateDirectory:
+		errorCode = _workCreateDirectory(payload);
 		break;
+			
+	case kCmdRemoveDirectory:
+		errorCode = _workRemoveDirectory(payload);
+		break;
+			
 
-	case kCmdRemove:
-		errorCode = _workRemove(req);
+	case kCmdCalcFileCRC32:
+		errorCode = _workCalcFileCRC32(payload);
 		break;
 
 	default:
-		errorCode = kErrNoRequest;
+		errorCode = kErrUnknownCommand;
 		break;	
 	}
 
 out:
 	// handle success vs. error
 	if (errorCode == kErrNone) {
-		hdr->opcode = kRspAck;
+		payload->req_opcode = payload->opcode;
+		payload->opcode = kRspAck;
 #ifdef MAVLINK_FTP_DEBUG
 		warnx("FTP: ack\n");
 #endif
 	} else {
+		int r_errno = errno;
 		warnx("FTP: nak %u", errorCode);
-		hdr->opcode = kRspNak;
-		hdr->size = 1;
-		hdr->data[0] = errorCode;
+		payload->req_opcode = payload->opcode;
+		payload->opcode = kRspNak;
+		payload->size = 1;
+		payload->data[0] = errorCode;
+		if (errorCode == kErrFailErrno) {
+			payload->size = 2;
+			payload->data[1] = r_errno;
+		}
 	}
 
+	
 	// respond to the request
 	_reply(req);
 
-	// free the request buffer back to the freelist
-	_qFree(req);
+	_return_request(req);
 }
 
+/// @brief Sends the specified FTP reponse message out through mavlink
 void
 MavlinkFTP::_reply(Request *req)
 {
-	auto hdr = req->header();
+	PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&req->message.payload[0]);
 	
-	hdr->seqNumber = req->header()->seqNumber + 1;
+	payload->seqNumber = payload->seqNumber + 1;
 
-	// message is assumed to be already constructed in the request buffer, so generate the CRC
-	hdr->crc32 = 0;
-	hdr->padding[0] = 0;
-	hdr->padding[1] = 0;
-	hdr->padding[2] = 0;
-	hdr->crc32 = crc32(req->rawData(), req->dataSize());
+	mavlink_message_t msg;
+	msg.checksum = 0;
+#ifndef MAVLINK_FTP_UNIT_TEST
+	uint16_t len =
+#endif
+	mavlink_msg_file_transfer_protocol_pack_chan(req->serverSystemId,	// Sender system id
+								    req->serverComponentId,	// Sender component id
+								    req->serverChannel,		// Channel to send on
+								    &msg,			// Message to pack payload into
+								    0,				// Target network
+								    req->targetSystemId,	// Target system id
+								    0,				// Target component id
+								    (const uint8_t*)payload);	// Payload to pack into message
+	
+	bool success = true;
+#ifdef MAVLINK_FTP_UNIT_TEST
+	// Unit test hook is set, call that instead
+	_utRcvMsgFunc(&msg, _ftp_test);
+#else
+	Mavlink	*mavlink = req->mavlink;
+	
+	mavlink->lockMessageBufferMutex();
+	success = mavlink->message_buffer_write(&msg, len);
+	mavlink->unlockMessageBufferMutex();
+		
+#endif
 
-	// then pack and send the reply back to the request source
-	req->reply();
+	if (!success) {
+		warnx("FTP TX ERR");
+	}
+#ifdef MAVLINK_FTP_DEBUG
+	else {
+		warnx("wrote: sys: %d, comp: %d, chan: %d, checksum: %d",
+		      req->serverSystemId,
+		      req->serverComponentId,
+		      req->serverChannel,
+		      msg.checksum);
+	}
+#endif
 }
 
+/// @brief Responds to a List command
 MavlinkFTP::ErrorCode
-MavlinkFTP::_workList(Request *req)
+MavlinkFTP::_workList(PayloadHeader* payload)
 {
-	auto hdr = req->header();
-    
     char dirPath[kMaxDataLength];
-    strncpy(dirPath, req->dataAsCString(), kMaxDataLength);
+    strncpy(dirPath, _data_as_cstring(payload), kMaxDataLength);
     
 	DIR *dp = opendir(dirPath);
 
 	if (dp == nullptr) {
 		warnx("FTP: can't open path '%s'", dirPath);
-		return kErrNotDir;
+		return kErrFailErrno;
 	}
     
 #ifdef MAVLINK_FTP_DEBUG
-	warnx("FTP: list %s offset %d", dirPath, hdr->offset);
+	warnx("FTP: list %s offset %d", dirPath, payload->offset);
 #endif
 
 	ErrorCode errorCode = kErrNone;
@@ -239,19 +331,19 @@ MavlinkFTP::_workList(Request *req)
 	unsigned offset = 0;
 
 	// move to the requested offset
-	seekdir(dp, hdr->offset);
+	seekdir(dp, payload->offset);
 
 	for (;;) {
 		// read the directory entry
 		if (readdir_r(dp, &entry, &result)) {
 			warnx("FTP: list %s readdir_r failure\n", dirPath);
-			errorCode = kErrIO;
+			errorCode = kErrFailErrno;
 			break;
 		}
 
 		// no more entries?
 		if (result == nullptr) {
-			if (hdr->offset != 0 && offset == 0) {
+			if (payload->offset != 0 && offset == 0) {
 				// User is requesting subsequent dir entries but there were none. This means the user asked
 				// to seek past EOF.
 				errorCode = kErrEOF;
@@ -276,14 +368,22 @@ MavlinkFTP::_workList(Request *req)
 			}
 			break;
 		case DTYPE_DIRECTORY:
-			direntType = kDirentDir;
+			if (strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) {
+				// Don't bother sending these back
+				direntType = kDirentSkip;
+			} else {
+				direntType = kDirentDir;
+			}
 			break;
 		default:
-			direntType = kDirentUnknown;
-			break;
+			// We only send back file and diretory entries, skip everything else
+			direntType = kDirentSkip;
 		}
 		
-		if (entry.d_type == DTYPE_FILE) {
+		if (direntType == kDirentSkip) {
+			// Skip send only dirent identifier
+			buf[0] = '\0';
+		} else if (direntType == kDirentFile) {
 			// Files send filename and file length
 			snprintf(buf, sizeof(buf), "%s\t%d", entry.d_name, fileSize);
 		} else {
@@ -299,145 +399,230 @@ MavlinkFTP::_workList(Request *req)
 		}
 		
 		// Move the data into the buffer
-		hdr->data[offset++] = direntType;
-		strcpy((char *)&hdr->data[offset], buf);
+		payload->data[offset++] = direntType;
+		strcpy((char *)&payload->data[offset], buf);
 #ifdef MAVLINK_FTP_DEBUG
-		printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]);
+		printf("FTP: list %s %s\n", dirPath, (char *)&payload->data[offset-1]);
 #endif
 		offset += nameLen + 1;
 	}
 
 	closedir(dp);
-	hdr->size = offset;
+	payload->size = offset;
 
 	return errorCode;
 }
 
+/// @brief Responds to an Open command
 MavlinkFTP::ErrorCode
-MavlinkFTP::_workOpen(Request *req, bool create)
+MavlinkFTP::_workOpen(PayloadHeader* payload, int oflag)
 {
-	auto hdr = req->header();
-    
-	int session_index = _findUnusedSession();
+	int session_index = _find_unused_session();
 	if (session_index < 0) {
-		return kErrNoSession;
+		warnx("FTP: Open failed - out of sessions\n");
+		return kErrNoSessionsAvailable;
 	}
 
-	
+	char *filename = _data_as_cstring(payload);
+
 	uint32_t fileSize = 0;
-	if (!create) {
-		struct stat st;
-		if (stat(req->dataAsCString(), &st) != 0) {
-			return kErrNotFile;
-		}
-		fileSize = st.st_size;
+	struct stat st;
+	if (stat(filename, &st) != 0) {
+		// fail only if requested open for read
+		if (oflag & O_RDONLY)
+			return kErrFailErrno;
+		else
+			st.st_size = 0;
 	}
+	fileSize = st.st_size;
 
-	int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY;
-    
-	int fd = ::open(req->dataAsCString(), oflag);
+	int fd = ::open(filename, oflag);
 	if (fd < 0) {
-		return create ? kErrPerm : kErrNotFile;
+		return kErrFailErrno;
 	}
 	_session_fds[session_index] = fd;
 
-	hdr->session = session_index;
-	if (create) {
-		hdr->size = 0;
-	} else {
-		hdr->size = sizeof(uint32_t);
-		*((uint32_t*)hdr->data) = fileSize;
-	}
+	payload->session = session_index;
+	payload->size = sizeof(uint32_t);
+	*((uint32_t*)payload->data) = fileSize;
 
 	return kErrNone;
 }
 
+/// @brief Responds to a Read command
 MavlinkFTP::ErrorCode
-MavlinkFTP::_workRead(Request *req)
+MavlinkFTP::_workRead(PayloadHeader* payload)
 {
-	auto hdr = req->header();
-
-	int session_index = hdr->session;
+	int session_index = payload->session;
 
-	if (!_validSession(session_index)) {
-		return kErrNoSession;
+	if (!_valid_session(session_index)) {
+		return kErrInvalidSession;
 	}
 
 	// Seek to the specified position
 #ifdef MAVLINK_FTP_DEBUG
-	warnx("seek %d", hdr->offset);
+	warnx("seek %d", payload->offset);
 #endif
-	if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) {
+	if (lseek(_session_fds[session_index], payload->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);
+	int bytes_read = ::read(_session_fds[session_index], &payload->data[0], kMaxDataLength);
 	if (bytes_read < 0) {
 		// Negative return indicates error other than eof
 		warnx("read fail %d", bytes_read);
-		return kErrIO;
+		return kErrFailErrno;
 	}
 
-	hdr->size = bytes_read;
+	payload->size = bytes_read;
 
 	return kErrNone;
 }
 
+/// @brief Responds to a Write command
 MavlinkFTP::ErrorCode
-MavlinkFTP::_workWrite(Request *req)
+MavlinkFTP::_workWrite(PayloadHeader* payload)
 {
-#if 0
-    // NYI: Coming soon
-	auto hdr = req->header();
+	int session_index = payload->session;
 
-	// look up session
-	auto session = getSession(hdr->session);
-	if (session == nullptr) {
-		return kErrNoSession;
+	if (!_valid_session(session_index)) {
+		return kErrInvalidSession;
 	}
 
-	// append to file
-	int result = session->append(hdr->offset, &hdr->data[0], hdr->size);
+	// Seek to the specified position
+#ifdef MAVLINK_FTP_DEBUG
+	warnx("seek %d", payload->offset);
+#endif
+	if (lseek(_session_fds[session_index], payload->offset, SEEK_SET) < 0) {
+		// Unable to see to the specified location
+		warnx("seek fail");
+		return kErrFailErrno;
+	}
 
-	if (result < 0) {
-		// XXX might also be no space, I/O, etc.
-		return kErrNotAppend;
+	int bytes_written = ::write(_session_fds[session_index], &payload->data[0], payload->size);
+	if (bytes_written < 0) {
+		// Negative return indicates error other than eof
+		warnx("write fail %d", bytes_written);
+		return kErrFailErrno;
 	}
 
-	hdr->size = result;
+	payload->size = sizeof(uint32_t);
+	*((uint32_t*)payload->data) = bytes_written;
+
 	return kErrNone;
-#else
-	return kErrPerm;
-#endif
 }
 
+/// @brief Responds to a RemoveFile command
 MavlinkFTP::ErrorCode
-MavlinkFTP::_workRemove(Request *req)
+MavlinkFTP::_workRemoveFile(PayloadHeader* payload)
 {
-	//auto hdr = req->header();
+	char file[kMaxDataLength];
+	strncpy(file, _data_as_cstring(payload), kMaxDataLength);
+	
+	if (unlink(file) == 0) {
+		payload->size = 0;
+		return kErrNone;
+	} else {
+		return kErrFailErrno;
+	}
+}
+
+/// @brief Responds to a TruncateFile command
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workTruncateFile(PayloadHeader* payload)
+{
+	char file[kMaxDataLength];
+	const char temp_file[] = "/fs/microsd/.trunc.tmp";
+	strncpy(file, _data_as_cstring(payload), kMaxDataLength);
+	payload->size = 0;
+
+	// emulate truncate(file, payload->offset) by
+	// copying to temp and overwrite with O_TRUNC flag.
+
+	struct stat st;
+	if (stat(file, &st) != 0) {
+		return kErrFailErrno;
+	}
+
+	if (!S_ISREG(st.st_mode)) {
+		errno = EISDIR;
+		return kErrFailErrno;
+	}
+
+	// check perms allow us to write (not romfs)
+	if (!(st.st_mode & (S_IWUSR | S_IWGRP | S_IWOTH))) {
+		errno = EROFS;
+		return kErrFailErrno;
+	}
+
+	if (payload->offset == (unsigned)st.st_size) {
+		// nothing to do
+		return kErrNone;
+	}
+	else if (payload->offset == 0) {
+		// 1: truncate all data
+		int fd = ::open(file, O_TRUNC | O_WRONLY);
+		if (fd < 0) {
+			return kErrFailErrno;
+		}
+
+		::close(fd);
+		return kErrNone;
+	}
+	else if (payload->offset > (unsigned)st.st_size) {
+		// 2: extend file
+		int fd = ::open(file, O_WRONLY);
+		if (fd < 0) {
+			return kErrFailErrno;
+		}
 
-	// for now, send error reply
-	return kErrPerm;
+		if (lseek(fd, payload->offset - 1, SEEK_SET) < 0) {
+			::close(fd);
+			return kErrFailErrno;
+		}
+
+		bool ok = 1 == ::write(fd, "", 1);
+		::close(fd);
+
+		return (ok)? kErrNone : kErrFailErrno;
+	}
+	else {
+		// 3: truncate
+		if (_copy_file(file, temp_file, payload->offset) != 0) {
+			return kErrFailErrno;
+		}
+		if (_copy_file(temp_file, file, payload->offset) != 0) {
+			return kErrFailErrno;
+		}
+		if (::unlink(temp_file) != 0) {
+			return kErrFailErrno;
+		}
+
+		return kErrNone;
+	}
 }
 
+/// @brief Responds to a Terminate command
 MavlinkFTP::ErrorCode
-MavlinkFTP::_workTerminate(Request *req)
+MavlinkFTP::_workTerminate(PayloadHeader* payload)
 {
-	auto hdr = req->header();
-    
-	if (!_validSession(hdr->session)) {
-		return kErrNoSession;
+	if (!_valid_session(payload->session)) {
+		return kErrInvalidSession;
 	}
     
-	::close(_session_fds[hdr->session]);
+	::close(_session_fds[payload->session]);
+	_session_fds[payload->session] = -1;
+	
+	payload->size = 0;
 
 	return kErrNone;
 }
 
+/// @brief Responds to a Reset command
 MavlinkFTP::ErrorCode
-MavlinkFTP::_workReset(void)
+MavlinkFTP::_workReset(PayloadHeader* payload)
 {
 	for (size_t i=0; i<kMaxSession; i++) {
 		if (_session_fds[i] != -1) {
@@ -446,11 +631,104 @@ MavlinkFTP::_workReset(void)
 		}
 	}
 
+	payload->size = 0;
+	
+	return kErrNone;
+}
+
+/// @brief Responds to a Rename command
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workRename(PayloadHeader* payload)
+{
+	char oldpath[kMaxDataLength];
+	char newpath[kMaxDataLength];
+
+	char *ptr = _data_as_cstring(payload);
+	size_t oldpath_sz = strlen(ptr);
+
+	if (oldpath_sz == payload->size) {
+		// no newpath
+		errno = EINVAL;
+		return kErrFailErrno;
+	}
+
+	strncpy(oldpath, ptr, kMaxDataLength);
+	strncpy(newpath, ptr + oldpath_sz + 1, kMaxDataLength);
+
+	if (rename(oldpath, newpath) == 0) {
+		payload->size = 0;
+		return kErrNone;
+	} else {
+		return kErrFailErrno;
+	}
+}
+
+/// @brief Responds to a RemoveDirectory command
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workRemoveDirectory(PayloadHeader* payload)
+{
+	char dir[kMaxDataLength];
+	strncpy(dir, _data_as_cstring(payload), kMaxDataLength);
+	
+	if (rmdir(dir) == 0) {
+		payload->size = 0;
+		return kErrNone;
+	} else {
+		return kErrFailErrno;
+	}
+}
+
+/// @brief Responds to a CreateDirectory command
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workCreateDirectory(PayloadHeader* payload)
+{
+	char dir[kMaxDataLength];
+	strncpy(dir, _data_as_cstring(payload), kMaxDataLength);
+	
+	if (mkdir(dir, S_IRWXU | S_IRWXG | S_IRWXO) == 0) {
+		payload->size = 0;
+		return kErrNone;
+	} else {
+		return kErrFailErrno;
+	}
+}
+
+/// @brief Responds to a CalcFileCRC32 command
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workCalcFileCRC32(PayloadHeader* payload)
+{
+	char file_buf[256];
+	uint32_t checksum = 0;
+	ssize_t bytes_read;
+	strncpy(file_buf, _data_as_cstring(payload), kMaxDataLength);
+
+	int fd = ::open(file_buf, O_RDONLY);
+	if (fd < 0) {
+		return kErrFailErrno;
+	}
+
+	do {
+		bytes_read = ::read(fd, file_buf, sizeof(file_buf));
+		if (bytes_read < 0) {
+			int r_errno = errno;
+			::close(fd);
+			errno = r_errno;
+			return kErrFailErrno;
+		}
+
+		checksum = crc32part((uint8_t*)file_buf, bytes_read, checksum);
+	} while (bytes_read == sizeof(file_buf));
+
+	::close(fd);
+
+	payload->size = sizeof(uint32_t);
+	*((uint32_t*)payload->data) = checksum;
 	return kErrNone;
 }
 
+/// @brief Returns true if the specified session is a valid open session
 bool
-MavlinkFTP::_validSession(unsigned index)
+MavlinkFTP::_valid_session(unsigned index)
 {
 	if ((index >= kMaxSession) || (_session_fds[index] < 0)) {
 		return false;
@@ -458,8 +736,9 @@ MavlinkFTP::_validSession(unsigned index)
 	return true;
 }
 
+/// @brief Returns an unused session index
 int
-MavlinkFTP::_findUnusedSession(void)
+MavlinkFTP::_find_unused_session(void)
 {
 	for (size_t i=0; i<kMaxSession; i++) {
 		if (_session_fds[i] == -1) {
@@ -470,16 +749,105 @@ MavlinkFTP::_findUnusedSession(void)
 	return -1;
 }
 
+/// @brief Guarantees that the payload data is null terminated.
+///     @return Returns a pointer to the payload data as a char *
 char *
-MavlinkFTP::Request::dataAsCString()
+MavlinkFTP::_data_as_cstring(PayloadHeader* payload)
 {
 	// guarantee nul termination
-	if (header()->size < kMaxDataLength) {
-		requestData()[header()->size] = '\0';
+	if (payload->size < kMaxDataLength) {
+		payload->data[payload->size] = '\0';
 	} else {
-		requestData()[kMaxDataLength - 1] = '\0';
+		payload->data[kMaxDataLength - 1] = '\0';
 	}
 
 	// and return data
-	return (char *)&(header()->data[0]);
+	return (char *)&(payload->data[0]);
+}
+
+/// @brief Returns a unused Request entry. NULL if none available.
+MavlinkFTP::Request *
+MavlinkFTP::_get_request(void)
+{
+	_lock_request_queue();
+	Request* req = reinterpret_cast<Request *>(dq_remfirst(&_request_queue));
+	_unlock_request_queue();
+	return req;
+}
+
+/// @brief Locks a semaphore to provide exclusive access to the request queue
+void
+MavlinkFTP::_lock_request_queue(void)
+{
+	do {}
+	while (sem_wait(&_request_queue_sem) != 0);
+}
+
+/// @brief Unlocks the semaphore providing exclusive access to the request queue
+void
+MavlinkFTP::_unlock_request_queue(void)
+{
+	sem_post(&_request_queue_sem);
+}
+
+/// @brief Returns a no longer needed request to the queue
+void
+MavlinkFTP::_return_request(Request *req)
+{
+	_lock_request_queue();
+	dq_addlast(&req->work.dq, &_request_queue);
+	_unlock_request_queue();
+}
+
+/// @brief Copy file (with limited space)
+int
+MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, ssize_t length)
+{
+	char buff[512];
+	int src_fd = -1, dst_fd = -1;
+	int op_errno = 0;
+
+	src_fd = ::open(src_path, O_RDONLY);
+	if (src_fd < 0) {
+		return -1;
+	}
+
+	dst_fd = ::open(dst_path, O_CREAT | O_TRUNC | O_WRONLY);
+	if (dst_fd < 0) {
+		op_errno = errno;
+		::close(src_fd);
+		errno = op_errno;
+		return -1;
+	}
+
+	while (length > 0) {
+		ssize_t bytes_read, bytes_written;
+		size_t blen = (length > sizeof(buff))? sizeof(buff) : length;
+
+		bytes_read = ::read(src_fd, buff, blen);
+		if (bytes_read == 0) {
+			// EOF
+			break;
+		}
+		else if (bytes_read < 0) {
+			warnx("cp: read");
+			op_errno = errno;
+			break;
+		}
+
+		bytes_written = ::write(dst_fd, buff, bytes_read);
+		if (bytes_written != bytes_read) {
+			warnx("cp: short write");
+			op_errno = errno;
+			break;
+		}
+
+		length -= bytes_written;
+	}
+
+	::close(src_fd);
+	::close(dst_fd);
+
+	errno = op_errno;
+	return (length > 0)? -1 : 0;
 }
diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
index 1dd8f102e..bef6775a9 100644
--- a/src/modules/mavlink/mavlink_ftp.h
+++ b/src/modules/mavlink/mavlink_ftp.h
@@ -33,17 +33,8 @@
 
 #pragma once
 
-/**
- * @file mavlink_ftp.h
- *
- * MAVLink remote file server.
- *
- * A limited number of requests (currently 2) may be outstanding at a time.
- * Additional messages will be discarded.
- *
- * Messages consist of a fixed header, followed by a data area.
- *
- */
+/// @file mavlink_ftp.h
+///     @author px4dev, Don Gagne <don@thegagnes.com>
  
 #include <dirent.h>
 #include <queue.h>
@@ -54,183 +45,136 @@
 #include "mavlink_messages.h"
 #include "mavlink_main.h"
 
+class MavlinkFtpTest;
+
+/// @brief MAVLink remote file server. Support FTP like commands using MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL message.
+/// A limited number of requests (kRequestQueueSize) may be outstanding at a time. Additional messages will be discarded.
 class MavlinkFTP
 {
 public:
+	/// @brief Returns the one Mavlink FTP server in the system.
+	static MavlinkFTP* get_server(void);
+    
+	/// @brief Contructor is only public so unit test code can new objects.
 	MavlinkFTP();
-
-	static MavlinkFTP	*getServer();
-
-	// static interface
-	void			handle_message(Mavlink* mavlink,
-					mavlink_message_t *msg);
-
-private:
-
-	static const unsigned	kRequestQueueSize = 2;
-
-	static MavlinkFTP	*_server;
-
-	/// @brief Trying to pack structures across differing compilers is questionable for Clients, so we pad the
-	/// structure ourselves to 32 bit alignment which should get us what we want.
-	struct RequestHeader
+	
+	/// @brief Adds the specified message to the work queue.
+	void handle_message(Mavlink* mavlink, mavlink_message_t *msg);
+    
+	typedef void (*ReceiveMessageFunc_t)(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest);
+	
+	/// @brief Sets up the server to run in unit test mode.
+	///	@param rcvmsgFunc Function which will be called to handle outgoing mavlink messages.
+	///	@param ftp_test MavlinkFtpTest object which the function is associated with
+	void set_unittest_worker(ReceiveMessageFunc_t rcvMsgFunc, MavlinkFtpTest *ftp_test);
+
+	/// @brief This is the payload which is in mavlink_file_transfer_protocol_t.payload. We pad the structure ourselves to
+	/// 32 bit alignment to avoid usage of any pack pragmas.
+	struct PayloadHeader
         {
-		uint16_t	seqNumber;  ///< sequence number for message
-		uint8_t		session;    ///< Session id for read and write commands
-		uint8_t		opcode;     ///< Command opcode
-		uint8_t		size;       ///< Size of data
-		uint8_t		padding[3];
-		uint32_t	crc32;      ///< CRC for entire Request structure, with crc32 and padding set to 0
-		uint32_t	offset;     ///< Offsets for List and Read commands
-		uint8_t		data[];
+		uint16_t	seqNumber;	///< sequence number for message
+		uint8_t		session;	///< Session id for read and write commands
+		uint8_t		opcode;		///< Command opcode
+		uint8_t		size;		///< Size of data
+		uint8_t		req_opcode;	///< Request opcode returned in kRspAck, kRspNak message
+		uint8_t		padding[2];	///< 32 bit aligment padding
+		uint32_t	offset;		///< Offsets for List and Read commands
+		uint8_t		data[];		///< command data, varies by Opcode
         };
-
+	
+	/// @brief Command opcodes
 	enum Opcode : uint8_t
 	{
-		kCmdNone,	// ignored, always acked
-		kCmdTerminate,	// releases sessionID, closes file
-		kCmdReset,	// terminates all sessions
-		kCmdList,	// list files in <path> from <offset>
-		kCmdOpen,	// opens <path> for reading, returns <session>
-		kCmdRead,	// reads <size> bytes from <offset> in <session>
-		kCmdCreate,	// creates <path> for writing, returns <session>
-		kCmdWrite,	// appends <size> bytes at <offset> in <session>
-		kCmdRemove,	// remove file (only if created by server?)
-
-		kRspAck,
-		kRspNak
+		kCmdNone,		///< ignored, always acked
+		kCmdTerminateSession,	///< Terminates open Read session
+		kCmdResetSessions,	///< Terminates all open Read sessions
+		kCmdListDirectory,	///< List files in <path> from <offset>
+		kCmdOpenFileRO,		///< Opens file at <path> for reading, returns <session>
+		kCmdReadFile,		///< Reads <size> bytes from <offset> in <session>
+		kCmdCreateFile,		///< Creates file at <path> for writing, returns <session>
+		kCmdWriteFile,		///< Writes <size> bytes to <offset> in <session>
+		kCmdRemoveFile,		///< Remove file at <path>
+		kCmdCreateDirectory,	///< Creates directory at <path>
+		kCmdRemoveDirectory,	///< Removes Directory at <path>, must be empty
+		kCmdOpenFileWO,		///< Opens file at <path> for writing, returns <session>
+		kCmdTruncateFile,	///< Truncate file at <path> to <offset> length
+		kCmdRename,		///< Rename <path1> to <path2>
+		kCmdCalcFileCRC32,	///< Calculate CRC32 for file at <path>
+		
+		kRspAck = 128,		///< Ack response
+		kRspNak			///< Nak response
 	};
-
+	
+	/// @brief Error codes returned in Nak response PayloadHeader.data[0].
 	enum ErrorCode : uint8_t
-	{
+        {
 		kErrNone,
-		kErrNoRequest,
-		kErrNoSession,
-		kErrSequence,
-		kErrNotDir,
-		kErrNotFile,
-		kErrEOF,
-		kErrNotAppend,
-		kErrTooBig,
-		kErrIO,
-		kErrPerm
-	};
-
-    int _findUnusedSession(void);
-    bool _validSession(unsigned index);
-
-    static const unsigned kMaxSession = 2;
-    int	_session_fds[kMaxSession];
-
-	class Request
+		kErrFail,			///< Unknown failure
+		kErrFailErrno,			///< Command failed, errno sent back in PayloadHeader.data[1]
+		kErrInvalidDataSize,		///< PayloadHeader.size is invalid
+		kErrInvalidSession,		///< Session is not currently open
+		kErrNoSessionsAvailable,	///< All available Sessions in use
+		kErrEOF,			///< Offset past end of file for List and Read commands
+		kErrUnknownCommand		///< Unknown command opcode
+        };
+	
+private:
+	/// @brief Unit of work which is queued to work_queue
+	struct Request
 	{
-	public:
-		union {
-			dq_entry_t	entry;
-			work_s		work;
-		};
-
-		bool		decode(Mavlink *mavlink, mavlink_message_t *fromMessage) {
-			if (fromMessage->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) {
-				_systemId = fromMessage->sysid;
-				_mavlink = mavlink;
-				mavlink_msg_file_transfer_protocol_decode(fromMessage, &_message);
-				return _message.target_system == _mavlink->get_system_id();
-			}
-			return false;
-		}
-
-		void		reply() {
-
-			// XXX the proper way would be an IOCTL / uORB call, rather than exploiting the
-			// flat memory architecture, as we're operating between threads here.
-			mavlink_message_t msg;
-			msg.checksum = 0;
-			unsigned len = mavlink_msg_file_transfer_protocol_pack_chan(_mavlink->get_system_id(),		// Sender system id
-										    _mavlink->get_component_id(),	// Sender component id
-										    _mavlink->get_channel(),		// Channel to send on
-										    &msg,				// Message to pack payload into
-										    0,					// Target network
-										    _systemId,				// Target system id
-										    0,					// Target component id
-										    rawData());				// Payload to pack into message
-
-			_mavlink->lockMessageBufferMutex();
-			bool success = _mavlink->message_buffer_write(&msg, len);
-			_mavlink->unlockMessageBufferMutex();
-
-			if (!success) {
-				warnx("FTP TX ERR");
-			}
-#ifdef MAVLINK_FTP_DEBUG
-			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);
-			}
-#endif
-		}
-
-		uint8_t		*rawData() { return &_message.payload[0]; }
-		RequestHeader 	*header()  { return reinterpret_cast<RequestHeader *>(&_message.payload[0]); }
-		uint8_t         *requestData() { return &(header()->data[0]); }
-		unsigned	dataSize() { return header()->size + sizeof(RequestHeader); }
-		mavlink_channel_t channel() { return _mavlink->get_channel(); }
-
-		char		*dataAsCString();
-
-	private:
-		Mavlink			*_mavlink;
-		mavlink_file_transfer_protocol_t _message;
-		uint8_t _systemId;
-
+		work_s	work;			///< work queue entry
+		Mavlink	*mavlink;		///< Mavlink to reply to
+		uint8_t serverSystemId;		///< System ID to send from
+		uint8_t serverComponentId;	///< Component ID to send from
+		uint8_t serverChannel;		///< Channel to send to
+		uint8_t targetSystemId;		///< System ID to target reply to
+
+		mavlink_file_transfer_protocol_t message;	///< Protocol message
 	};
-
-	static const char	kDirentFile = 'F';
-	static const char	kDirentDir = 'D';
-	static const char	kDirentUnknown = 'U';
-	static const uint8_t	kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(RequestHeader);
-
-	/// Request worker; runs on the low-priority work queue to service
-	/// remote requests.
-	///
-	static void		_workerTrampoline(void *arg);
-	void			_worker(Request *req);
-
-	/// Reply to a request (XXX should be a Request method)
-	///
-	void			_reply(Request *req);
-
-	ErrorCode		_workList(Request *req);
-	ErrorCode		_workOpen(Request *req, bool create);
-	ErrorCode		_workRead(Request *req);
-	ErrorCode		_workWrite(Request *req);
-	ErrorCode		_workRemove(Request *req);
-	ErrorCode		_workTerminate(Request *req);
-	ErrorCode		_workReset();
-
-	// work freelist
-	Request			_workBufs[kRequestQueueSize];
-	dq_queue_t		_workFree;
-	sem_t			_lock;
-
-	void			_qLock() { do {} while (sem_wait(&_lock) != 0); }
-	void			_qUnlock() { sem_post(&_lock); }
-
-	void			_qFree(Request *req) {
-		_qLock();
-		dq_addlast(&req->entry, &_workFree);
-		_qUnlock();
-	}
-
-	Request			*_dqFree() {
-		_qLock();
-		auto req = reinterpret_cast<Request *>(dq_remfirst(&_workFree));
-		_qUnlock();
-		return req;
-	}
-    
+	
+	Request		*_get_request(void);
+	void		_return_request(Request *req);
+	void		_lock_request_queue(void);
+	void		_unlock_request_queue(void);
+	
+	char		*_data_as_cstring(PayloadHeader* payload);
+	
+	static void	_worker_trampoline(void *arg);
+	void		_process_request(Request *req);
+	void		_reply(Request *req);
+	int		_copy_file(const char *src_path, const char *dst_path, ssize_t length);
+
+	ErrorCode	_workList(PayloadHeader *payload);
+	ErrorCode	_workOpen(PayloadHeader *payload, int oflag);
+	ErrorCode	_workRead(PayloadHeader *payload);
+	ErrorCode	_workWrite(PayloadHeader *payload);
+	ErrorCode	_workTerminate(PayloadHeader *payload);
+	ErrorCode	_workReset(PayloadHeader* payload);
+	ErrorCode	_workRemoveDirectory(PayloadHeader *payload);
+	ErrorCode	_workCreateDirectory(PayloadHeader *payload);
+	ErrorCode	_workRemoveFile(PayloadHeader *payload);
+	ErrorCode	_workTruncateFile(PayloadHeader *payload);
+	ErrorCode	_workRename(PayloadHeader *payload);
+	ErrorCode	_workCalcFileCRC32(PayloadHeader *payload);
+
+	static const unsigned	kRequestQueueSize = 2;			///< Max number of queued requests
+	Request			_request_bufs[kRequestQueueSize];	///< Request buffers which hold work
+	dq_queue_t		_request_queue;				///< Queue of available Request buffers
+	sem_t			_request_queue_sem;			///< Semaphore for locking access to _request_queue
+	
+	int _find_unused_session(void);
+	bool _valid_session(unsigned index);
+	
+	static const char	kDirentFile = 'F';	///< Identifies File returned from List command
+	static const char	kDirentDir = 'D';	///< Identifies Directory returned from List command
+	static const char	kDirentSkip = 'S';	///< Identifies Skipped entry from List command
+	
+	/// @brief Maximum data size in RequestHeader::data
+	static const uint8_t	kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(PayloadHeader);
+	
+	static const unsigned kMaxSession = 2;	///< Max number of active sessions
+	int	_session_fds[kMaxSession];	///< Session file descriptors, 0 for empty slot
+	
+	ReceiveMessageFunc_t _utRcvMsgFunc;	///< Unit test override for mavlink message sending
+	MavlinkFtpTest *_ftp_test;		///< Additional parameter to _utRcvMsgFunc;
 };
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index dd830071d..0d932d20a 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -123,6 +123,7 @@ Mavlink::Mavlink() :
 	_task_running(false),
 	_hil_enabled(false),
 	_use_hil_gps(false),
+	_forward_externalsp(false),
 	_is_usb_uart(false),
 	_wait_to_transmit(false),
 	_received_messages(false),
@@ -483,6 +484,7 @@ void Mavlink::mavlink_update_system(void)
 		_param_component_id = param_find("MAV_COMP_ID");
 		_param_system_type = param_find("MAV_TYPE");
 		_param_use_hil_gps = param_find("MAV_USEHILGPS");
+		_param_forward_externalsp = param_find("MAV_FWDEXTSP");
 	}
 
 	/* update system and component id */
@@ -529,6 +531,11 @@ void Mavlink::mavlink_update_system(void)
 	param_get(_param_use_hil_gps, &use_hil_gps);
 
 	_use_hil_gps = (bool)use_hil_gps;
+
+	int32_t forward_externalsp;
+	param_get(_param_forward_externalsp, &forward_externalsp);
+
+	_forward_externalsp = (bool)forward_externalsp;
 }
 
 int Mavlink::get_system_id()
@@ -1387,26 +1394,26 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("SYS_STATUS", 1.0f);
 		configure_stream("GPS_GLOBAL_ORIGIN", 0.5f);
 		configure_stream("HIGHRES_IMU", 1.0f);
-		configure_stream("ATTITUDE", 15.0f);
+		configure_stream("ATTITUDE", 10.0f);
 		configure_stream("VFR_HUD", 8.0f);
 		configure_stream("GPS_RAW_INT", 1.0f);
 		configure_stream("GLOBAL_POSITION_INT", 3.0f);
 		configure_stream("LOCAL_POSITION_NED", 3.0f);
 		configure_stream("RC_CHANNELS_RAW", 1.0f);
 		configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
-		configure_stream("ATTITUDE_TARGET", 15.0f);
+		configure_stream("ATTITUDE_TARGET", 3.0f);
 		configure_stream("DISTANCE_SENSOR", 0.5f);
-		configure_stream("OPTICAL_FLOW", 20.0f);
+		configure_stream("OPTICAL_FLOW", 5.0f);
 		break;
 
 	case MAVLINK_MODE_ONBOARD:
 		configure_stream("SYS_STATUS", 1.0f);
-		// XXX OBC change back: We need to be bandwidth-efficient here too
 		configure_stream("ATTITUDE", 50.0f);
 		configure_stream("GLOBAL_POSITION_INT", 50.0f);
 		configure_stream("CAMERA_CAPTURE", 2.0f);
-		configure_stream("ATTITUDE_TARGET", 50.0f);
-		configure_stream("POSITION_TARGET_GLOBAL_INT", 20.0f);
+		configure_stream("ATTITUDE_TARGET", 10.0f);
+		configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f);
+		configure_stream("VFR_HUD", 10.0f);
 		break;
 
 	default:
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 7f9d225bb..1f96e586b 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -137,6 +137,8 @@ public:
 
 	bool			get_use_hil_gps() { return _use_hil_gps; }
 
+	bool			get_forward_externalsp() { return _forward_externalsp; }
+
 	bool			get_flow_control_enabled() { return _flow_control_enabled; }
 
 	bool			get_forwarding_on() { return _forwarding_on; }
@@ -232,7 +234,7 @@ public:
 	bool			should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
 
 	bool			message_buffer_write(const void *ptr, int size);
-    
+
 	void			lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
 	void			unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); }
 
@@ -275,6 +277,7 @@ private:
 	/* states */
 	bool			_hil_enabled;		/**< Hardware In the Loop mode */
 	bool			_use_hil_gps;		/**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
+	bool			_forward_externalsp;	/**< Forward external setpoint messages to controllers directly if in offboard mode */
 	bool			_is_usb_uart;		/**< Port is USB */
 	bool        		_wait_to_transmit;  	/**< Wait to transmit until received messages. */
 	bool        		_received_messages;	/**< Whether we've received valid mavlink messages. */
@@ -349,6 +352,7 @@ private:
 	param_t			_param_component_id;
 	param_t			_param_system_type;
 	param_t			_param_use_hil_gps;
+	param_t			_param_forward_externalsp;
 
 	perf_counter_t		_loop_perf;			/**< loop performance counter */
 	perf_counter_t		_txerr_perf;			/**< TX error counter */
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 43222880e..e8d783847 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -37,6 +37,7 @@
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
  * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
 /* XXX trim includes */
@@ -105,10 +106,11 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_flow_pub(-1),
 	_range_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),
+	_force_sp_pub(-1),
+	_pos_sp_triplet_pub(-1),
 	_vicon_position_pub(-1),
 	_vision_position_pub(-1),
 	_telemetry_status_pub(-1),
@@ -123,7 +125,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 {
 
 	// make sure the FTP server is started
-	(void)MavlinkFTP::getServer();
+	(void)MavlinkFTP::get_server();
 }
 
 MavlinkReceiver::~MavlinkReceiver()
@@ -154,6 +156,14 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_vicon_position_estimate(msg);
 		break;
 
+	case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
+		handle_message_set_position_target_local_ned(msg);
+		break;
+
+	case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
+		handle_message_set_attitude_target(msg);
+		break;
+
 	case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
 		handle_message_vision_position_estimate(msg);
 		break;
@@ -175,7 +185,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		break;
 
 	case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL:
-		MavlinkFTP::getServer()->handle_message(_mavlink, msg);
+		MavlinkFTP::get_server()->handle_message(_mavlink, msg);
 		break;
 
 	default:
@@ -475,6 +485,189 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg)
 }
 
 void
+MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t *msg)
+{
+	mavlink_set_position_target_local_ned_t set_position_target_local_ned;
+	mavlink_msg_set_position_target_local_ned_decode(msg, &set_position_target_local_ned);
+
+	struct offboard_control_setpoint_s offboard_control_sp;
+	memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));//XXX breaks compatibility with multiple setpoints
+
+	/* Only accept messages which are intended for this system */
+	if ((mavlink_system.sysid == set_position_target_local_ned.target_system ||
+				set_position_target_local_ned.target_system == 0) &&
+			(mavlink_system.compid == set_position_target_local_ned.target_component ||
+			 set_position_target_local_ned.target_component == 0)) {
+
+		/* convert mavlink type (local, NED) to uORB offboard control struct */
+		switch (set_position_target_local_ned.coordinate_frame) {
+			case MAV_FRAME_LOCAL_NED:
+				offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED;
+				break;
+			case MAV_FRAME_LOCAL_OFFSET_NED:
+				offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED;
+				break;
+			case MAV_FRAME_BODY_NED:
+				offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED;
+				break;
+			case MAV_FRAME_BODY_OFFSET_NED:
+				offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED;
+				break;
+			default:
+				/* invalid setpoint, avoid publishing */
+				return;
+		}
+		offboard_control_sp.position[0] = set_position_target_local_ned.x;
+		offboard_control_sp.position[1] = set_position_target_local_ned.y;
+		offboard_control_sp.position[2] = set_position_target_local_ned.z;
+		offboard_control_sp.velocity[0] = set_position_target_local_ned.vx;
+		offboard_control_sp.velocity[1] = set_position_target_local_ned.vy;
+		offboard_control_sp.velocity[2] = set_position_target_local_ned.vz;
+		offboard_control_sp.acceleration[0] = set_position_target_local_ned.afx;
+		offboard_control_sp.acceleration[1] = set_position_target_local_ned.afy;
+		offboard_control_sp.acceleration[2] = set_position_target_local_ned.afz;
+		offboard_control_sp.yaw = set_position_target_local_ned.yaw;
+		offboard_control_sp.yaw_rate = set_position_target_local_ned.yaw_rate;
+		offboard_control_sp.isForceSetpoint = (bool)(set_position_target_local_ned.type_mask & (1 << 9));
+
+		/* If we are in force control mode, for now set offboard mode to force control */
+		if (offboard_control_sp.isForceSetpoint) {
+			offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_FORCE;
+		}
+
+		/* set ignore flags */
+		for (int i = 0; i < 9; i++) {
+			offboard_control_sp.ignore &=  ~(1 << i);
+			offboard_control_sp.ignore |=  (set_position_target_local_ned.type_mask & (1 << i));
+		}
+		offboard_control_sp.ignore &=  ~(1 << OFB_IGN_BIT_YAW);
+			offboard_control_sp.ignore |=  (set_position_target_local_ned.type_mask & (1 << 10)) <<
+					OFB_IGN_BIT_YAW;
+		offboard_control_sp.ignore &=  ~(1 << OFB_IGN_BIT_YAWRATE);
+			offboard_control_sp.ignore |=  (set_position_target_local_ned.type_mask & (1 << 11)) <<
+					OFB_IGN_BIT_YAWRATE;
+
+		offboard_control_sp.timestamp = hrt_absolute_time();
+
+		if (_offboard_control_sp_pub < 0) {
+			_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
+
+		} else {
+			orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp);
+		}
+
+		/* If we are in offboard control mode and offboard control loop through is enabled
+		 * also publish the setpoint topic which is read by the controller */
+		if (_mavlink->get_forward_externalsp()) {
+			bool updated;
+			orb_check(_control_mode_sub, &updated);
+			if (updated) {
+				orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
+			}
+			if (_control_mode.flag_control_offboard_enabled) {
+				if (offboard_control_sp.isForceSetpoint &&
+						offboard_control_sp_ignore_position_all(offboard_control_sp) &&
+						offboard_control_sp_ignore_velocity_all(offboard_control_sp)) {
+					/* The offboard setpoint is a force setpoint only, directly writing to the force
+					 * setpoint topic and not publishing the setpoint triplet topic */
+					struct vehicle_force_setpoint_s	force_sp;
+					force_sp.x = offboard_control_sp.acceleration[0];
+					force_sp.y = offboard_control_sp.acceleration[1];
+					force_sp.z = offboard_control_sp.acceleration[2];
+					//XXX: yaw
+					if (_force_sp_pub < 0) {
+						_force_sp_pub = orb_advertise(ORB_ID(vehicle_force_setpoint), &force_sp);
+					} else {
+						orb_publish(ORB_ID(vehicle_force_setpoint), _force_sp_pub, &force_sp);
+					}
+				} else {
+					/* It's not a pure force setpoint: publish to setpoint triplet  topic */
+					struct position_setpoint_triplet_s pos_sp_triplet;
+					pos_sp_triplet.previous.valid = false;
+					pos_sp_triplet.next.valid = false;
+					pos_sp_triplet.current.valid = true;
+					pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others
+
+					/* set the local pos values if the setpoint type is 'local pos' and none
+					 * of the local pos fields is set to 'ignore' */
+					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
+							!offboard_control_sp_ignore_position_some(offboard_control_sp)) {
+					pos_sp_triplet.current.position_valid = true;
+					pos_sp_triplet.current.x = offboard_control_sp.position[0];
+					pos_sp_triplet.current.y = offboard_control_sp.position[1];
+					pos_sp_triplet.current.z = offboard_control_sp.position[2];
+					} else {
+						pos_sp_triplet.current.position_valid = false;
+					}
+
+					/* set the local vel values if the setpoint type is 'local pos' and none
+					 * of the local vel fields is set to 'ignore' */
+					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
+							!offboard_control_sp_ignore_velocity_some(offboard_control_sp)) {
+						pos_sp_triplet.current.velocity_valid = true;
+						pos_sp_triplet.current.vx = offboard_control_sp.velocity[0];
+						pos_sp_triplet.current.vy = offboard_control_sp.velocity[1];
+						pos_sp_triplet.current.vz = offboard_control_sp.velocity[2];
+					} else {
+						pos_sp_triplet.current.velocity_valid = false;
+					}
+
+					/* set the local acceleration values if the setpoint type is 'local pos' and none
+					 * of the accelerations fields is set to 'ignore' */
+					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
+							!offboard_control_sp_ignore_acceleration_some(offboard_control_sp)) {
+						pos_sp_triplet.current.acceleration_valid = true;
+						pos_sp_triplet.current.a_x = offboard_control_sp.acceleration[0];
+						pos_sp_triplet.current.a_y = offboard_control_sp.acceleration[1];
+						pos_sp_triplet.current.a_z = offboard_control_sp.acceleration[2];
+						pos_sp_triplet.current.acceleration_is_force =
+						offboard_control_sp.isForceSetpoint;
+
+					} else {
+						pos_sp_triplet.current.acceleration_valid = false;
+					}
+
+					/* set the yaw sp value if the setpoint type is 'local pos' and the yaw
+					 * field is not set to 'ignore' */
+					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
+							!offboard_control_sp_ignore_yaw(offboard_control_sp)) {
+						pos_sp_triplet.current.yaw_valid = true;
+						pos_sp_triplet.current.yaw = offboard_control_sp.yaw;
+
+					} else {
+						pos_sp_triplet.current.yaw_valid = false;
+					}
+
+					/* set the yawrate sp value if the setpoint type is 'local pos' and the yawrate
+					 * field is not set to 'ignore' */
+					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
+							!offboard_control_sp_ignore_yawrate(offboard_control_sp)) {
+						pos_sp_triplet.current.yawspeed_valid = true;
+						pos_sp_triplet.current.yawspeed = offboard_control_sp.yaw_rate;
+
+					} else {
+						pos_sp_triplet.current.yawspeed_valid = false;
+					}
+					//XXX handle global pos setpoints (different MAV frames)
+
+
+					if (_pos_sp_triplet_pub < 0) {
+						_pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet),
+								&pos_sp_triplet);
+					} else {
+						orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub,
+								&pos_sp_triplet);
+					}
+
+				}
+
+			}
+
+		}
+	}
+}
+
+void
 MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
 {
 	mavlink_vision_position_estimate_t pos;
@@ -514,6 +707,103 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
 }
 
 void
+MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
+{
+	mavlink_set_attitude_target_t set_attitude_target;
+	mavlink_msg_set_attitude_target_decode(msg, &set_attitude_target);
+
+	struct offboard_control_setpoint_s offboard_control_sp;
+	memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); //XXX breaks compatibility with multiple setpoints
+
+	/* Only accept messages which are intended for this system */
+	if ((mavlink_system.sysid == set_attitude_target.target_system ||
+				set_attitude_target.target_system == 0) &&
+			(mavlink_system.compid == set_attitude_target.target_component ||
+			 set_attitude_target.target_component == 0)) {
+		for (int i = 0; i < 4; i++) {
+			offboard_control_sp.attitude[i] = set_attitude_target.q[i];
+		}
+		offboard_control_sp.attitude_rate[0] = set_attitude_target.body_roll_rate;
+		offboard_control_sp.attitude_rate[1] = set_attitude_target.body_pitch_rate;
+		offboard_control_sp.attitude_rate[2] = set_attitude_target.body_yaw_rate;
+
+		/* set correct ignore flags for body rate fields: copy from mavlink message */
+		for (int i = 0; i < 3; i++) {
+			offboard_control_sp.ignore &=  ~(1 << (i + OFB_IGN_BIT_BODYRATE_X));
+			offboard_control_sp.ignore |=  (set_attitude_target.type_mask & (1 << i)) << OFB_IGN_BIT_BODYRATE_X;
+		}
+		/* set correct ignore flags for thrust field: copy from mavlink message */
+		offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_THRUST);
+		offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 6)) << OFB_IGN_BIT_THRUST);
+		/* set correct ignore flags for attitude field: copy from mavlink message */
+		offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_ATT);
+		offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 7)) << OFB_IGN_BIT_ATT);
+
+
+		offboard_control_sp.timestamp = hrt_absolute_time();
+		offboard_control_sp.mode =OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; //XXX handle rate control mode
+
+		if (_offboard_control_sp_pub < 0) {
+			_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
+
+		} else {
+			orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp);
+		}
+
+		/* If we are in offboard control mode and offboard control loop through is enabled
+		 * also publish the setpoint topic which is read by the controller */
+		if (_mavlink->get_forward_externalsp()) {
+			bool updated;
+			orb_check(_control_mode_sub, &updated);
+			if (updated) {
+				orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
+			}
+
+			if (_control_mode.flag_control_offboard_enabled) {
+
+				/* Publish attitude setpoint if attitude and thrust ignore bits are not set */
+				if (!(offboard_control_sp_ignore_attitude(offboard_control_sp) ||
+							offboard_control_sp_ignore_thrust(offboard_control_sp))) {
+					struct vehicle_attitude_setpoint_s att_sp;
+					att_sp.timestamp = hrt_absolute_time();
+					mavlink_quaternion_to_euler(set_attitude_target.q,
+							&att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body);
+					mavlink_quaternion_to_dcm(set_attitude_target.q, att_sp.R_body);
+					att_sp.R_valid = true;
+					att_sp.thrust = set_attitude_target.thrust;
+					memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d));
+					att_sp.q_d_valid = true;
+					if (_att_sp_pub < 0) {
+						_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
+					} else {
+						orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp);
+					}
+				}
+
+				/* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */
+				///XXX add support for ignoring individual axes
+				if (!(offboard_control_sp_ignore_bodyrates_some(offboard_control_sp) ||
+							offboard_control_sp_ignore_thrust(offboard_control_sp))) {
+					struct vehicle_rates_setpoint_s rates_sp;
+					rates_sp.timestamp = hrt_absolute_time();
+					rates_sp.roll = set_attitude_target.body_roll_rate;
+					rates_sp.pitch = set_attitude_target.body_pitch_rate;
+					rates_sp.yaw = set_attitude_target.body_yaw_rate;
+					rates_sp.thrust = set_attitude_target.thrust;
+
+					if (_att_sp_pub < 0) {
+						_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
+					} else {
+						orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &rates_sp);
+					}
+				}
+			}
+
+		}
+	}
+}
+
+void
 MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 {
 	/* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index c4e12d8d8..e5f2c6a73 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -71,6 +71,7 @@
 #include <uORB/topics/debug_key_value.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
+#include <uORB/topics/vehicle_force_setpoint.h>
 
 #include "mavlink_ftp.h"
 
@@ -117,6 +118,8 @@ private:
 	void handle_message_vicon_position_estimate(mavlink_message_t *msg);
 	void handle_message_vision_position_estimate(mavlink_message_t *msg);
 	void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg);
+	void handle_message_set_position_target_local_ned(mavlink_message_t *msg);
+	void handle_message_set_attitude_target(mavlink_message_t *msg);
 	void handle_message_radio_status(mavlink_message_t *msg);
 	void handle_message_manual_control(mavlink_message_t *msg);
 	void handle_message_heartbeat(mavlink_message_t *msg);
@@ -145,10 +148,11 @@ private:
 	orb_advert_t _flow_pub;
 	orb_advert_t _range_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 _force_sp_pub;
+	orb_advert_t _pos_sp_triplet_pub;
 	orb_advert_t _vicon_position_pub;
 	orb_advert_t _vision_position_pub;
 	orb_advert_t _telemetry_status_pub;
diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp
new file mode 100644
index 000000000..4caa820b6
--- /dev/null
+++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp
@@ -0,0 +1,761 @@
+/****************************************************************************
+ *
+ *   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 mavlink_ftp_test.cpp
+///	@author Don Gagne <don@thegagnes.com>
+
+#include <sys/stat.h>
+#include <crc32.h>
+#include <stdio.h>
+#include <fcntl.h>
+
+#include "mavlink_ftp_test.h"
+#include "../mavlink_ftp.h"
+
+/// @brief Test case file name for Read command. File are generated using mavlink_ftp_test_data.py
+const MavlinkFtpTest::ReadTestCase MavlinkFtpTest::_rgReadTestCases[] = {
+	{ "/etc/unit_test_data/mavlink_tests/test_238.data",	MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) - 1},	// Read takes less than single packet
+	{ "/etc/unit_test_data/mavlink_tests/test_239.data",	MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) },		// Read completely fills single packet
+	{ "/etc/unit_test_data/mavlink_tests/test_240.data",	MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) + 1 },	// Read take two packets
+};
+
+const char MavlinkFtpTest::_unittest_microsd_dir[] = "/fs/microsd/ftp_unit_test_dir";
+const char MavlinkFtpTest::_unittest_microsd_file[] = "/fs/microsd/ftp_unit_test_dir/file";
+
+MavlinkFtpTest::MavlinkFtpTest() :
+	_ftp_server{},
+	_reply_msg{},
+	_lastOutgoingSeqNumber{}
+{
+}
+
+MavlinkFtpTest::~MavlinkFtpTest()
+{
+	
+}
+
+/// @brief Called before every test to initialize the FTP Server.
+void MavlinkFtpTest::_init(void)
+{
+	_ftp_server = new MavlinkFTP;;
+	_ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message, this);
+
+	_cleanup_microsd();
+}
+
+/// @brief Called after every test to take down the FTP Server.
+void MavlinkFtpTest::_cleanup(void)
+{
+	delete _ftp_server;
+	
+	_cleanup_microsd();
+}
+
+/// @brief Tests for correct behavior of an Ack response.
+bool MavlinkFtpTest::_ack_test(void)
+{
+	MavlinkFTP::PayloadHeader		payload;
+	mavlink_file_transfer_protocol_t	ftp_msg;
+	MavlinkFTP::PayloadHeader		*reply;
+	
+	payload.opcode = MavlinkFTP::kCmdNone;
+
+	bool success = _send_receive_msg(&payload,	// FTP payload header
+					 0,		// size in bytes of data
+					 nullptr,	// Data to start into FTP message payload
+					 &ftp_msg,	// Response from server
+					 &reply);	// Payload inside FTP message response
+	if (!success) {
+		return false;
+	}
+	
+	ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
+	ut_compare("Incorrect payload size", reply->size, 0);
+	
+	return true;
+}
+
+/// @brief Tests for correct response to an invalid opcpde.
+bool MavlinkFtpTest::_bad_opcode_test(void)
+{
+	MavlinkFTP::PayloadHeader		payload;
+	mavlink_file_transfer_protocol_t	ftp_msg;
+	MavlinkFTP::PayloadHeader		*reply;
+	
+	payload.opcode = 0xFF;	// bogus opcode
+
+	bool success = _send_receive_msg(&payload,	// FTP payload header
+					 0,		// size in bytes of data
+					 nullptr,	// Data to start into FTP message payload
+					 &ftp_msg,	// Response from server
+					 &reply);	// Payload inside FTP message response
+	if (!success) {
+		return false;
+	}
+	
+	ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
+	ut_compare("Incorrect payload size", reply->size, 1);
+	ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrUnknownCommand);
+	
+	return true;
+}
+
+/// @brief Tests for correct reponse to a payload which an invalid data size field.
+bool MavlinkFtpTest::_bad_datasize_test(void)
+{
+	mavlink_message_t			msg;
+	MavlinkFTP::PayloadHeader		payload;
+	mavlink_file_transfer_protocol_t	ftp_msg;
+	MavlinkFTP::PayloadHeader		*reply;
+	
+	payload.opcode = MavlinkFTP::kCmdListDirectory;
+	
+	_setup_ftp_msg(&payload, 0, nullptr, &msg);
+	
+	// Set the data size to be one larger than is legal
+	((MavlinkFTP::PayloadHeader*)((mavlink_file_transfer_protocol_t*)msg.payload64)->payload)->size = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN + 1;
+	
+	_ftp_server->handle_message(nullptr /* mavlink */, &msg);
+	
+	if (!_decode_message(&_reply_msg, &ftp_msg, &reply)) {
+		return false;
+	}
+	
+	ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
+	ut_compare("Incorrect payload size", reply->size, 1);
+	ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidDataSize);
+	
+	return true;
+}
+
+bool MavlinkFtpTest::_list_test(void)
+{
+	MavlinkFTP::PayloadHeader		payload;
+	mavlink_file_transfer_protocol_t	ftp_msg;
+	MavlinkFTP::PayloadHeader		*reply;
+	
+	char response1[] = "Dempty_dir|Ftest_238.data\t238|Ftest_239.data\t239|Ftest_240.data\t240";
+	char response2[] = "Ddev|Detc|Dfs|Dobj";
+	
+	struct _testCase {
+		const char	*dir;		///< Directory to run List command on
+		char		*response;	///< Expected response entries from List command
+		int		response_count;	///< Number of directories that should be returned
+		bool		success;	///< true: List command should succeed, false: List command should fail
+	};
+	struct _testCase rgTestCases[] = {
+		{ "/bogus",				nullptr,	0,	false },
+		{ "/etc/unit_test_data/mavlink_tests",	response1,	4,	true },
+		{ "/",					response2,	4,	true },
+	};
+
+	for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
+		const struct _testCase *test = &rgTestCases[i];
+		
+		payload.opcode = MavlinkFTP::kCmdListDirectory;
+		payload.offset = 0;
+		
+		bool success = _send_receive_msg(&payload,		// FTP payload header
+						strlen(test->dir)+1,	// size in bytes of data
+						(uint8_t*)test->dir,	// Data to start into FTP message payload
+						&ftp_msg,		// Response from server
+						&reply);		// Payload inside FTP message response
+		if (!success) {
+			return false;
+		}
+		
+		if (test->success) {
+			ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
+			ut_compare("Incorrect payload size", reply->size, strlen(test->response) + 1);
+			
+			// The return order of directories from the List command is not repeatable. So we can't do a direct comparison
+			// to a hardcoded return result string.
+			
+			// Convert null terminators to seperator char so we can use strok to parse returned data
+			for (uint8_t j=0; j<reply->size-1; j++) {
+				if (reply->data[j] == 0) {
+					reply->data[j] = '|';
+				}
+			}
+			
+			// Loop over returned directory entries trying to find then in the response list
+			char *dir;
+			int response_count = 0;
+			dir = strtok((char *)&reply->data[0], "|");
+			while (dir != nullptr) {
+				ut_assert("Returned directory not found in expected response", strstr(test->response, dir));
+				response_count++;
+				dir = strtok(nullptr, "|");
+			}
+			
+			ut_compare("Incorrect number of directory entires returned", test->response_count, response_count);
+		} else {
+			ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
+			ut_compare("Incorrect payload size", reply->size, 2);
+			ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno);
+		}
+	}
+	
+	return true;
+}
+
+/// @brief Tests for correct reponse to a List command on a valid directory, but with an offset that
+/// is beyond the last directory entry.
+bool MavlinkFtpTest::_list_eof_test(void)
+{
+	MavlinkFTP::PayloadHeader		payload;
+	mavlink_file_transfer_protocol_t	ftp_msg;
+	MavlinkFTP::PayloadHeader		*reply;
+	const char				*dir = "/";
+	
+	payload.opcode = MavlinkFTP::kCmdListDirectory;
+	payload.offset = 4;	// offset past top level dirs
+	
+	bool success = _send_receive_msg(&payload,	// FTP payload header
+					strlen(dir)+1,	// size in bytes of data
+					(uint8_t*)dir,	// Data to start into FTP message payload
+					&ftp_msg,	// Response from server
+					&reply);	// Payload inside FTP message response
+	if (!success) {
+		return false;
+	}
+
+	ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
+	ut_compare("Incorrect payload size", reply->size, 1);
+	ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrEOF);
+	
+	return true;
+}
+
+/// @brief Tests for correct reponse to an Open command on a file which does not exist.
+bool MavlinkFtpTest::_open_badfile_test(void)
+{
+	MavlinkFTP::PayloadHeader		payload;
+	mavlink_file_transfer_protocol_t	ftp_msg;
+	MavlinkFTP::PayloadHeader		*reply;
+	const char				*dir = "/foo";	// non-existent file
+	
+	payload.opcode = MavlinkFTP::kCmdOpenFileRO;
+	payload.offset = 0;
+	
+	bool success = _send_receive_msg(&payload,	// FTP payload header
+					strlen(dir)+1,	// size in bytes of data
+					(uint8_t*)dir,	// Data to start into FTP message payload
+					&ftp_msg,	// Response from server
+					&reply);	// Payload inside FTP message response
+	if (!success) {
+		return false;
+	}
+	
+	ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
+	ut_compare("Incorrect payload size", reply->size, 2);
+	ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno);
+	
+	return true;
+}
+
+/// @brief Tests for correct reponse to an Open command on a file, followed by Terminate
+bool MavlinkFtpTest::_open_terminate_test(void)
+{
+	MavlinkFTP::PayloadHeader		payload;
+	mavlink_file_transfer_protocol_t	ftp_msg;
+	MavlinkFTP::PayloadHeader		*reply;
+	
+	for (size_t i=0; i<sizeof(_rgReadTestCases)/sizeof(_rgReadTestCases[0]); i++) {
+		struct stat st;
+		const ReadTestCase *test = &_rgReadTestCases[i];
+		
+		payload.opcode = MavlinkFTP::kCmdOpenFileRO;
+		payload.offset = 0;
+		
+		bool success = _send_receive_msg(&payload,		// FTP payload header
+						 strlen(test->file)+1,	// size in bytes of data
+						 (uint8_t*)test->file,	// Data to start into FTP message payload
+						 &ftp_msg,		// Response from server
+						 &reply);		// Payload inside FTP message response
+		if (!success) {
+			return false;
+		}
+		
+		ut_compare("stat failed", stat(test->file, &st), 0);
+		
+		
+		ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
+		ut_compare("Incorrect payload size", reply->size, sizeof(uint32_t));
+		ut_compare("File size incorrect", *((uint32_t*)&reply->data[0]), (uint32_t)st.st_size);
+
+		payload.opcode = MavlinkFTP::kCmdTerminateSession;
+		payload.session = reply->session;
+		payload.size = 0;
+		
+		success = _send_receive_msg(&payload,	// FTP payload header
+					    0,		// size in bytes of data
+					    nullptr,	// Data to start into FTP message payload
+					    &ftp_msg,	// Response from server
+					    &reply);	// Payload inside FTP message response
+		if (!success) {
+			return false;
+		}
+		
+		ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
+		ut_compare("Incorrect payload size", reply->size, 0);
+	}
+
+	return true;
+}
+
+/// @brief Tests for correct reponse to a Terminate command on an invalid session.
+bool MavlinkFtpTest::_terminate_badsession_test(void)
+{
+	MavlinkFTP::PayloadHeader		payload;
+	mavlink_file_transfer_protocol_t	ftp_msg;
+	MavlinkFTP::PayloadHeader		*reply;
+	const char				*file = _rgReadTestCases[0].file;
+	
+	payload.opcode = MavlinkFTP::kCmdOpenFileRO;
+	payload.offset = 0;
+	
+	bool success = _send_receive_msg(&payload,	// FTP payload header
+					strlen(file)+1,	// size in bytes of data
+					(uint8_t*)file,	// Data to start into FTP message payload
+					&ftp_msg,	// Response from server
+					&reply);	// Payload inside FTP message response
+	if (!success) {
+		return false;
+	}
+	
+	ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
+	
+	payload.opcode = MavlinkFTP::kCmdTerminateSession;
+	payload.session = reply->session + 1;
+	payload.size = 0;
+	
+	success = _send_receive_msg(&payload,	// FTP payload header
+				   0,		// size in bytes of data
+				   nullptr,	// Data to start into FTP message payload
+				   &ftp_msg,	// Response from server
+				   &reply);	// Payload inside FTP message response
+	if (!success) {
+		return false;
+	}
+	
+	ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
+	ut_compare("Incorrect payload size", reply->size, 1);
+	ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidSession);
+	
+	return true;
+}
+
+/// @brief Tests for correct reponse to a Read command on an open session.
+bool MavlinkFtpTest::_read_test(void)
+{
+	MavlinkFTP::PayloadHeader		payload;
+	mavlink_file_transfer_protocol_t	ftp_msg;
+	MavlinkFTP::PayloadHeader		*reply;
+	
+	for (size_t i=0; i<sizeof(_rgReadTestCases)/sizeof(_rgReadTestCases[0]); i++) {
+		struct stat st;
+		const ReadTestCase *test = &_rgReadTestCases[i];
+		
+		// Read in the file so we can compare it to what we get back
+		ut_compare("stat failed", stat(test->file, &st), 0);
+		uint8_t *bytes = new uint8_t[st.st_size];
+		ut_assert("new failed", bytes != nullptr);
+		int fd = ::open(test->file, O_RDONLY);
+		ut_assert("open failed", fd != -1);
+		int bytes_read = ::read(fd, bytes, st.st_size);
+		ut_compare("read failed", bytes_read, st.st_size);
+		::close(fd);
+		
+		// Test case data files are created for specific boundary conditions
+		ut_compare("Test case data files are out of date", test->length, st.st_size);
+		
+		payload.opcode = MavlinkFTP::kCmdOpenFileRO;
+		payload.offset = 0;
+		
+		bool success = _send_receive_msg(&payload,		// FTP payload header
+						 strlen(test->file)+1,	// size in bytes of data
+						 (uint8_t*)test->file,	// Data to start into FTP message payload
+						 &ftp_msg,		// Response from server
+						 &reply);		// Payload inside FTP message response
+		if (!success) {
+			return false;
+		}
+		
+		ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
+		
+		payload.opcode = MavlinkFTP::kCmdReadFile;
+		payload.session = reply->session;
+		payload.offset = 0;
+		
+		success = _send_receive_msg(&payload,	// FTP payload header
+					    0,		// size in bytes of data
+					    nullptr,	// Data to start into FTP message payload
+					    &ftp_msg,	// Response from server
+					    &reply);	// Payload inside FTP message response
+		if (!success) {
+			return false;
+		}
+		
+		ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
+		ut_compare("Offset incorrect", reply->offset, 0);
+		
+		if (test->length <= MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader)) {
+			ut_compare("Payload size incorrect", reply->size, (uint32_t)st.st_size);
+			ut_compare("File contents differ", memcmp(reply->data, bytes, st.st_size), 0);
+		}
+		
+		payload.opcode = MavlinkFTP::kCmdTerminateSession;
+		payload.session = reply->session;
+		payload.size = 0;
+		
+		success = _send_receive_msg(&payload,	// FTP payload header
+					    0,		// size in bytes of data
+					    nullptr,	// Data to start into FTP message payload
+					    &ftp_msg,	// Response from server
+					    &reply);	// Payload inside FTP message response
+		if (!success) {
+			return false;
+		}
+		
+		ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
+		ut_compare("Incorrect payload size", reply->size, 0);
+	}
+	
+	return true;
+}
+
+/// @brief Tests for correct reponse to a Read command on an invalid session.
+bool MavlinkFtpTest::_read_badsession_test(void)
+{
+	MavlinkFTP::PayloadHeader		payload;
+	mavlink_file_transfer_protocol_t	ftp_msg;
+	MavlinkFTP::PayloadHeader		*reply;
+	const char				*file = _rgReadTestCases[0].file;
+	
+	payload.opcode = MavlinkFTP::kCmdOpenFileRO;
+	payload.offset = 0;
+	
+	bool success = _send_receive_msg(&payload,	// FTP payload header
+					 strlen(file)+1,	// size in bytes of data
+					 (uint8_t*)file,	// Data to start into FTP message payload
+					 &ftp_msg,	// Response from server
+					 &reply);	// Payload inside FTP message response
+	if (!success) {
+		return false;
+	}
+	
+	ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
+	
+	payload.opcode = MavlinkFTP::kCmdReadFile;
+	payload.session = reply->session + 1;	// Invalid session
+	payload.offset = 0;
+	
+	success = _send_receive_msg(&payload,	// FTP payload header
+				    0,		// size in bytes of data
+				    nullptr,	// Data to start into FTP message payload
+				    &ftp_msg,	// Response from server
+				    &reply);	// Payload inside FTP message response
+	if (!success) {
+		return false;
+	}
+	
+	ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
+	ut_compare("Incorrect payload size", reply->size, 1);
+	ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidSession);
+	
+	return true;
+}
+
+bool MavlinkFtpTest::_removedirectory_test(void)
+{
+	MavlinkFTP::PayloadHeader		payload;
+	mavlink_file_transfer_protocol_t	ftp_msg;
+	MavlinkFTP::PayloadHeader		*reply;
+	int					fd;
+	
+	struct _testCase {
+		const char	*dir;
+		bool		success;
+		bool		deleteFile;
+	};
+	static const struct _testCase rgTestCases[] = {
+		{ "/bogus",						false,	false },
+		{ "/etc/unit_test_data/mavlink_tests/empty_dir",	false,	false },
+		{ _unittest_microsd_dir,				false,	false },
+		{ _unittest_microsd_file,				false,	false },
+		{ _unittest_microsd_dir,				true,	true },
+	};
+	
+	ut_compare("mkdir failed", ::mkdir(_unittest_microsd_dir, S_IRWXU | S_IRWXG | S_IRWXO), 0);
+	ut_assert("open failed", (fd = ::open(_unittest_microsd_file, O_CREAT | O_EXCL)) != -1);
+	::close(fd);
+	
+	for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
+		const struct _testCase *test = &rgTestCases[i];
+		
+		if (test->deleteFile) {
+			ut_compare("unlink failed", ::unlink(_unittest_microsd_file), 0);
+		}
+		
+		payload.opcode = MavlinkFTP::kCmdRemoveDirectory;
+		payload.offset = 0;
+		
+		bool success = _send_receive_msg(&payload,		// FTP payload header
+						 strlen(test->dir)+1,	// size in bytes of data
+						 (uint8_t*)test->dir,	// Data to start into FTP message payload
+						 &ftp_msg,		// Response from server
+						 &reply);		// Payload inside FTP message response
+		if (!success) {
+			return false;
+		}
+		
+		if (test->success) {
+			ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
+			ut_compare("Incorrect payload size", reply->size, 0);
+		} else {
+			ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
+			ut_compare("Incorrect payload size", reply->size, 2);
+			ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno);
+		}
+	}
+	
+	return true;
+}
+
+bool MavlinkFtpTest::_createdirectory_test(void)
+{
+	MavlinkFTP::PayloadHeader		payload;
+	mavlink_file_transfer_protocol_t	ftp_msg;
+	MavlinkFTP::PayloadHeader		*reply;
+	
+	struct _testCase {
+		const char	*dir;
+		bool		success;
+	};
+	static const struct _testCase rgTestCases[] = {
+		{ "/etc/bogus",			false },
+		{ _unittest_microsd_dir,	true },
+		{ _unittest_microsd_dir,	false },
+		{ "/fs/microsd/bogus/bogus",	false },
+	};
+	
+	for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
+		const struct _testCase *test = &rgTestCases[i];
+		
+		payload.opcode = MavlinkFTP::kCmdCreateDirectory;
+		payload.offset = 0;
+		
+		bool success = _send_receive_msg(&payload,		// FTP payload header
+						 strlen(test->dir)+1,	// size in bytes of data
+						 (uint8_t*)test->dir,	// Data to start into FTP message payload
+						 &ftp_msg,		// Response from server
+						 &reply);		// Payload inside FTP message response
+		if (!success) {
+			return false;
+		}
+		
+		if (test->success) {
+			ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
+			ut_compare("Incorrect payload size", reply->size, 0);
+		} else {
+			ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
+			ut_compare("Incorrect payload size", reply->size, 2);
+			ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno);
+		}
+	}
+	
+	return true;
+}
+
+bool MavlinkFtpTest::_removefile_test(void)
+{
+	MavlinkFTP::PayloadHeader		payload;
+	mavlink_file_transfer_protocol_t	ftp_msg;
+	MavlinkFTP::PayloadHeader		*reply;
+	int					fd;
+	
+	struct _testCase {
+		const char	*file;
+		bool		success;
+	};
+	static const struct _testCase rgTestCases[] = {
+		{ "/bogus",			false },
+		{ _rgReadTestCases[0].file,	false },
+		{ _unittest_microsd_dir,	false },
+		{ _unittest_microsd_file,	true },
+		{ _unittest_microsd_file,	false },
+	};
+	
+	ut_compare("mkdir failed", ::mkdir(_unittest_microsd_dir, S_IRWXU | S_IRWXG | S_IRWXO), 0);
+	ut_assert("open failed", (fd = ::open(_unittest_microsd_file, O_CREAT | O_EXCL)) != -1);
+	::close(fd);
+	
+	for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
+		const struct _testCase *test = &rgTestCases[i];
+		
+		payload.opcode = MavlinkFTP::kCmdRemoveFile;
+		payload.offset = 0;
+		
+		bool success = _send_receive_msg(&payload,		// FTP payload header
+						 strlen(test->file)+1,	// size in bytes of data
+						 (uint8_t*)test->file,	// Data to start into FTP message payload
+						 &ftp_msg,		// Response from server
+						 &reply);		// Payload inside FTP message response
+		if (!success) {
+			return false;
+		}
+		
+		if (test->success) {
+			ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
+			ut_compare("Incorrect payload size", reply->size, 0);
+		} else {
+			ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
+			ut_compare("Incorrect payload size", reply->size, 2);
+			ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno);
+		}
+	}
+	
+	return true;
+}
+
+/// @brief Static method used as callback from MavlinkFTP. This method will be called by MavlinkFTP when
+/// it needs to send a message out on Mavlink.
+void MavlinkFtpTest::receive_message(const mavlink_message_t *msg, MavlinkFtpTest *ftp_test)
+{
+	ftp_test->_receive_message(msg);
+}
+
+/// @brief Non-Static version of receive_message
+void MavlinkFtpTest::_receive_message(const mavlink_message_t *msg)
+{
+	// Move the message into our own member variable
+	memcpy(&_reply_msg, msg, sizeof(mavlink_message_t));
+}
+
+/// @brief Decode and validate the incoming message
+bool MavlinkFtpTest::_decode_message(const mavlink_message_t		*msg,		///< Mavlink message to decode
+				     mavlink_file_transfer_protocol_t	*ftp_msg,	///< Decoded FTP message
+				     MavlinkFTP::PayloadHeader		**payload)	///< Payload inside FTP message response
+{
+	mavlink_msg_file_transfer_protocol_decode(msg, ftp_msg);
+	
+	// Make sure the targets are correct
+	ut_compare("Target network non-zero", ftp_msg->target_network, 0);
+	ut_compare("Target system id mismatch", ftp_msg->target_system, clientSystemId);
+	ut_compare("Target component id mismatch", ftp_msg->target_component, clientComponentId);
+	
+	*payload = reinterpret_cast<MavlinkFTP::PayloadHeader *>(ftp_msg->payload);
+	
+	// Make sure we have a good sequence number
+	ut_compare("Sequence number mismatch", (*payload)->seqNumber, _lastOutgoingSeqNumber + 1);
+	
+	// Bump sequence number for next outgoing message
+	_lastOutgoingSeqNumber++;
+	
+	return true;
+}
+
+/// @brief Initializes an FTP message into a mavlink message
+void MavlinkFtpTest::_setup_ftp_msg(MavlinkFTP::PayloadHeader	*payload_header,	///< FTP payload header
+				    uint8_t			size,			///< size in bytes of data
+				    const uint8_t		*data,			///< Data to start into FTP message payload
+				    mavlink_message_t		*msg)			///< Returned mavlink message
+{
+	uint8_t payload_bytes[MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN];
+	MavlinkFTP::PayloadHeader *payload = reinterpret_cast<MavlinkFTP::PayloadHeader *>(payload_bytes);
+			     
+	memcpy(payload, payload_header, sizeof(MavlinkFTP::PayloadHeader));
+	
+	payload->seqNumber = _lastOutgoingSeqNumber;
+	payload->size = size;
+	if (size != 0) {
+		memcpy(payload->data, data, size);
+	}
+    
+	payload->padding[0] = 0;
+	payload->padding[1] = 0;
+	
+	msg->checksum = 0;
+	mavlink_msg_file_transfer_protocol_pack(clientSystemId,		// Sender system id
+						clientComponentId,	// Sender component id
+						msg,			// Message to pack payload into
+						0,			// Target network
+						serverSystemId,		// Target system id
+						serverComponentId,	// Target component id
+						payload_bytes);		// Payload to pack into message
+}
+
+/// @brief Sends the specified FTP message to the server and returns response
+bool MavlinkFtpTest::_send_receive_msg(MavlinkFTP::PayloadHeader	*payload_header,	///< FTP payload header
+				      uint8_t				size,			///< size in bytes of data
+				      const uint8_t			*data,			///< Data to start into FTP message payload
+				      mavlink_file_transfer_protocol_t	*ftp_msg_reply,		///< Response from server
+				      MavlinkFTP::PayloadHeader		**payload_reply)	///< Payload inside FTP message response
+{
+	mavlink_message_t msg;
+
+	_setup_ftp_msg(payload_header, size, data, &msg);
+	_ftp_server->handle_message(nullptr /* mavlink */, &msg);
+	return _decode_message(&_reply_msg, ftp_msg_reply, payload_reply);
+}
+	
+/// @brief Cleans up an files created on microsd during testing
+void MavlinkFtpTest::_cleanup_microsd(void)
+{
+	::unlink(_unittest_microsd_file);
+	::rmdir(_unittest_microsd_dir);
+}
+
+/// @brief Runs all the unit tests
+bool MavlinkFtpTest::run_tests(void)
+{
+	ut_run_test(_ack_test);
+	ut_run_test(_bad_opcode_test);
+	ut_run_test(_bad_datasize_test);
+	ut_run_test(_list_test);
+	ut_run_test(_list_eof_test);
+	ut_run_test(_open_badfile_test);
+	ut_run_test(_open_terminate_test);
+	ut_run_test(_terminate_badsession_test);
+	ut_run_test(_read_test);
+	ut_run_test(_read_badsession_test);
+	ut_run_test(_removedirectory_test);
+	ut_run_test(_createdirectory_test);
+	ut_run_test(_removefile_test);
+	
+	return (_tests_failed == 0);
+
+}
+
+ut_declare_test(mavlink_ftp_test, MavlinkFtpTest)
diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h
new file mode 100644
index 000000000..2696192cc
--- /dev/null
+++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h
@@ -0,0 +1,108 @@
+/****************************************************************************
+ *
+ *   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 mavlink_ftp_test.h
+///	@author Don Gagne <don@thegagnes.com>
+
+#pragma once
+
+#include <unit_test/unit_test.h>
+#include "../mavlink_bridge_header.h"
+#include "../mavlink_ftp.h"
+
+class MavlinkFtpTest : public UnitTest
+{
+public:
+	MavlinkFtpTest();
+	virtual ~MavlinkFtpTest();
+	
+	virtual bool run_tests(void);
+	
+	static void receive_message(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest);
+	
+	static const uint8_t serverSystemId = 50;	///< System ID for server
+	static const uint8_t serverComponentId = 1;	///< Component ID for server
+	static const uint8_t serverChannel = 0;		///< Channel to send to
+	
+	static const uint8_t clientSystemId = 1;	///< System ID for client
+	static const uint8_t clientComponentId = 0;	///< Component ID for client
+	
+	// We don't want any of these
+	MavlinkFtpTest(const MavlinkFtpTest&);
+	MavlinkFtpTest& operator=(const MavlinkFtpTest&);
+	
+private:
+	virtual void _init(void);
+	virtual void _cleanup(void);
+	
+	bool _ack_test(void);
+	bool _bad_opcode_test(void);
+	bool _bad_datasize_test(void);
+	bool _list_test(void);
+	bool _list_eof_test(void);
+	bool _open_badfile_test(void);
+	bool _open_terminate_test(void);
+	bool _terminate_badsession_test(void);
+	bool _read_test(void);
+	bool _read_badsession_test(void);
+	bool _removedirectory_test(void);
+	bool _createdirectory_test(void);
+	bool _removefile_test(void);
+	
+	void _receive_message(const mavlink_message_t *msg);
+	void _setup_ftp_msg(MavlinkFTP::PayloadHeader *payload_header, uint8_t size, const uint8_t *data, mavlink_message_t *msg);
+	bool _decode_message(const mavlink_message_t *msg, mavlink_file_transfer_protocol_t *ftp_msg, MavlinkFTP::PayloadHeader **payload);
+	bool _send_receive_msg(MavlinkFTP::PayloadHeader	*payload_header,
+                           uint8_t				size,
+                           const uint8_t			*data,
+                           mavlink_file_transfer_protocol_t	*ftp_msg_reply,
+                           MavlinkFTP::PayloadHeader		**payload_reply);
+	void _cleanup_microsd(void);
+	
+	MavlinkFTP *_ftp_server;
+    
+	mavlink_message_t _reply_msg;
+    
+	uint16_t _lastOutgoingSeqNumber;
+	
+	struct ReadTestCase {
+		const char	*file;
+		const uint16_t	length;
+	};
+	static const ReadTestCase _rgReadTestCases[];
+	
+	static const char _unittest_microsd_dir[];
+	static const char _unittest_microsd_file[];
+};
+
+bool mavlink_ftp_test(void);
diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test_data.py b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test_data.py
new file mode 100644
index 000000000..a7e68f2a3
--- /dev/null
+++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test_data.py
@@ -0,0 +1,7 @@
+import sys
+print 'Arguments: file - ' + sys.argv[1] + ', length - ' + sys.argv[2]
+file = open(sys.argv[1], 'w')
+for i in range(int(sys.argv[2])):
+	b = bytearray([i & 0xFF])
+	file.write(b)
+file.close()
\ No newline at end of file
diff --git a/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp b/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp
new file mode 100644
index 000000000..10cbcb0ec
--- /dev/null
+++ b/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp
@@ -0,0 +1,47 @@
+/****************************************************************************
+ *
+ *   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 mavlink_ftp_tests.cpp
+ */
+
+#include <systemlib/err.h>
+
+#include "mavlink_ftp_test.h"
+
+extern "C" __EXPORT int mavlink_tests_main(int argc, char *argv[]);
+
+int mavlink_tests_main(int argc, char *argv[])
+{
+	return mavlink_ftp_test() ? 0 : -1;
+}
diff --git a/src/modules/mavlink/mavlink_tests/module.mk b/src/modules/mavlink/mavlink_tests/module.mk
new file mode 100644
index 000000000..1cc28cce1
--- /dev/null
+++ b/src/modules/mavlink/mavlink_tests/module.mk
@@ -0,0 +1,50 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# System state machine tests.
+#
+
+MODULE_COMMAND		= mavlink_tests
+SRCS			= mavlink_tests.cpp \
+			mavlink_ftp_test.cpp \
+			../mavlink_ftp.cpp \
+			../mavlink.c
+
+INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
+
+MODULE_STACKSIZE = 5000
+
+MAXOPTIMIZATION	 = -Os
+
+EXTRACXXFLAGS	= -Weffc++ -DMAVLINK_FTP_UNIT_TEST
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 ecc40428d..d52138522 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -76,6 +76,7 @@
 
 #define TILT_COS_MAX	0.7f
 #define SIGMA			0.000001f
+#define MIN_DIST		0.01f
 
 /**
  * Multicopter position control app start / stop handling function
@@ -179,6 +180,7 @@ private:
 
 	bool _reset_pos_sp;
 	bool _reset_alt_sp;
+	bool _mode_auto;
 
 	math::Vector<3> _pos;
 	math::Vector<3> _pos_sp;
@@ -220,6 +222,11 @@ private:
 	void		reset_alt_sp();
 
 	/**
+	 * Check if position setpoint is too far from current position and adjust it if needed.
+	 */
+	void		limit_pos_sp_offset();
+
+	/**
 	 * Set position setpoint using manual control
 	 */
 	void		control_manual(float dt);
@@ -229,6 +236,14 @@ private:
 	 */
 	void		control_offboard(float dt);
 
+	bool		cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r,
+					const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res);
+
+	/**
+	 * Set position setpoint for AUTO
+	 */
+	void		control_auto(float dt);
+
 	/**
 	 * Select between barometric and global (AMSL) altitudes
 	 */
@@ -283,7 +298,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_ref_timestamp(0),
 
 	_reset_pos_sp(true),
-	_reset_alt_sp(true)
+	_reset_alt_sp(true),
+	_mode_auto(false)
 {
 	memset(&_att, 0, sizeof(_att));
 	memset(&_att_sp, 0, sizeof(_att_sp));
@@ -534,6 +550,29 @@ MulticopterPositionControl::reset_alt_sp()
 }
 
 void
+MulticopterPositionControl::limit_pos_sp_offset()
+{
+	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);
+	}
+}
+
+void
 MulticopterPositionControl::control_manual(float dt)
 {
 	_sp_move_rate.zero();
@@ -614,7 +653,6 @@ MulticopterPositionControl::control_offboard(float dt)
 			_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) {
 			/* control velocity */
@@ -624,6 +662,11 @@ MulticopterPositionControl::control_offboard(float dt)
 			/* set position setpoint move rate */
 			_sp_move_rate(0) = _pos_sp_triplet.current.vx;
 			_sp_move_rate(1) = _pos_sp_triplet.current.vy;
+		}
+
+		if (_pos_sp_triplet.current.yaw_valid) {
+			_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
+		} else if (_pos_sp_triplet.current.yawspeed_valid) {
 			_att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt;
 		}
 
@@ -647,6 +690,170 @@ MulticopterPositionControl::control_offboard(float dt)
 	}
 }
 
+bool
+MulticopterPositionControl::cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r,
+		const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res)
+{
+	/* project center of sphere on line */
+	/* normalized AB */
+	math::Vector<3> ab_norm = line_b - line_a;
+	ab_norm.normalize();
+	math::Vector<3> d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm);
+	float cd_len = (sphere_c - d).length();
+
+	/* we have triangle CDX with known CD and CX = R, find DX */
+	if (sphere_r > cd_len) {
+		/* have two roots, select one in A->B direction from D */
+		float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len);
+		res = d + ab_norm * dx_len;
+		return true;
+
+	} else {
+		/* have no roots, return D */
+		res = d;
+		return false;
+	}
+}
+
+void
+MulticopterPositionControl::control_auto(float dt)
+{
+	if (!_mode_auto) {
+		_mode_auto = true;
+		/* reset position setpoint on AUTO mode activation */
+		reset_pos_sp();
+		reset_alt_sp();
+	}
+
+	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) {
+		/* in case of interrupted mission don't go to waypoint but stay at current position */
+		_reset_pos_sp = true;
+		_reset_alt_sp = true;
+
+		/* project setpoint to local frame */
+		math::Vector<3> curr_sp;
+		map_projection_project(&_ref_pos,
+				       _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
+				       &curr_sp.data[0], &curr_sp.data[1]);
+		curr_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);
+
+		/* scaled space: 1 == position error resulting max allowed speed, L1 = 1 in this space */
+		math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max);	// TODO add mult param here
+
+		/* convert current setpoint to scaled space */
+		math::Vector<3> curr_sp_s = curr_sp.emult(scale);
+
+		/* by default use current setpoint as is */
+		math::Vector<3> pos_sp_s = curr_sp_s;
+
+		if (_pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) {
+			/* follow "previous - current" line */
+			math::Vector<3> prev_sp;
+			map_projection_project(&_ref_pos,
+						   _pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon,
+						   &prev_sp.data[0], &prev_sp.data[1]);
+			prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt);
+
+			if ((curr_sp - prev_sp).length() > MIN_DIST) {
+
+				/* find X - cross point of L1 sphere and trajectory */
+				math::Vector<3> pos_s = _pos.emult(scale);
+				math::Vector<3> prev_sp_s = prev_sp.emult(scale);
+				math::Vector<3> prev_curr_s = curr_sp_s - prev_sp_s;
+				math::Vector<3> curr_pos_s = pos_s - curr_sp_s;
+				float curr_pos_s_len = curr_pos_s.length();
+				if (curr_pos_s_len < 1.0f) {
+					/* copter is closer to waypoint than L1 radius */
+					/* check next waypoint and use it to avoid slowing down when passing via waypoint */
+					if (_pos_sp_triplet.next.valid) {
+						math::Vector<3> next_sp;
+						map_projection_project(&_ref_pos,
+									   _pos_sp_triplet.next.lat, _pos_sp_triplet.next.lon,
+									   &next_sp.data[0], &next_sp.data[1]);
+						next_sp(2) = -(_pos_sp_triplet.next.alt - _ref_alt);
+
+						if ((next_sp - curr_sp).length() > MIN_DIST) {
+							math::Vector<3> next_sp_s = next_sp.emult(scale);
+
+							/* calculate angle prev - curr - next */
+							math::Vector<3> curr_next_s = next_sp_s - curr_sp_s;
+							math::Vector<3> prev_curr_s_norm = prev_curr_s.normalized();
+
+							/* cos(a) * curr_next, a = angle between current and next trajectory segments */
+							float cos_a_curr_next = prev_curr_s_norm * curr_next_s;
+
+							/* cos(b), b = angle pos - curr_sp - prev_sp */
+							float cos_b = -curr_pos_s * prev_curr_s_norm / curr_pos_s_len;
+
+							if (cos_a_curr_next > 0.0f && cos_b > 0.0f) {
+								float curr_next_s_len = curr_next_s.length();
+								/* if curr - next distance is larger than L1 radius, limit it */
+								if (curr_next_s_len > 1.0f) {
+									cos_a_curr_next /= curr_next_s_len;
+								}
+
+								/* feed forward position setpoint offset */
+								math::Vector<3> pos_ff = prev_curr_s_norm *
+										cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) *
+										(1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f));
+								pos_sp_s += pos_ff;
+							}
+						}
+					}
+
+				} else {
+					bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, pos_sp_s);
+					if (near) {
+						/* L1 sphere crosses trajectory */
+
+					} else {
+						/* copter is too far from trajectory */
+						/* if copter is behind prev waypoint, go directly to prev waypoint */
+						if ((pos_sp_s - prev_sp_s) * prev_curr_s < 0.0f) {
+							pos_sp_s = prev_sp_s;
+						}
+
+						/* if copter is in front of curr waypoint, go directly to curr waypoint */
+						if ((pos_sp_s - curr_sp_s) * prev_curr_s > 0.0f) {
+							pos_sp_s = curr_sp_s;
+						}
+
+						pos_sp_s = pos_s + (pos_sp_s - pos_s).normalized();
+					}
+				}
+			}
+		}
+
+		/* move setpoint not faster than max allowed speed */
+		math::Vector<3> pos_sp_old_s = _pos_sp.emult(scale);
+
+		/* difference between current and desired position setpoints, 1 = max speed */
+		math::Vector<3> d_pos_m = (pos_sp_s - pos_sp_old_s).edivide(_params.pos_p);
+		float d_pos_m_len = d_pos_m.length();
+		if (d_pos_m_len > dt) {
+			pos_sp_s = pos_sp_old_s + (d_pos_m / d_pos_m_len * dt).emult(_params.pos_p);
+		}
+
+		/* scale result back to normal space */
+		_pos_sp = pos_sp_s.edivide(scale);
+
+		/* update yaw setpoint if needed */
+		if (isfinite(_pos_sp_triplet.current.yaw)) {
+			_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
+		}
+
+	} else {
+		/* no waypoint, do nothing, setpoint was already reset */
+	}
+}
+
 void
 MulticopterPositionControl::task_main()
 {
@@ -750,41 +957,16 @@ MulticopterPositionControl::task_main()
 			if (_control_mode.flag_control_manual_enabled) {
 				/* manual control */
 				control_manual(dt);
+				_mode_auto = false;
 
 			} else if (_control_mode.flag_control_offboard_enabled) {
 				/* offboard control */
 				control_offboard(dt);
+				_mode_auto = false;
 
 			} else {
 				/* AUTO */
-				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) {
-					/* in case of interrupted mission don't go to waypoint but stay at current position */
-					_reset_pos_sp = true;
-					_reset_alt_sp = true;
-
-					/* project setpoint to local frame */
-					map_projection_project(&_ref_pos,
-							       _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
-							       &_pos_sp.data[0], &_pos_sp.data[1]);
-					_pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);
-
-					/* update yaw setpoint if needed */
-					if (isfinite(_pos_sp_triplet.current.yaw)) {
-						_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
-					}
-
-				} else {
-					/* no waypoint, loiter, reset position setpoint if needed */
-					reset_pos_sp();
-					reset_alt_sp();
-				}
+				control_auto(dt);
 			}
 
 			/* fill local position setpoint */
@@ -846,16 +1028,6 @@ 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();
-
-					if (vel_sp_norm > 1.0f) {
-						_vel_sp /= vel_sp_norm;
-					}
-				}
-
 				_global_vel_sp.vx = _vel_sp(0);
 				_global_vel_sp.vy = _vel_sp(1);
 				_global_vel_sp.vz = _vel_sp(2);
@@ -1132,9 +1304,9 @@ MulticopterPositionControl::task_main()
 			/* position controller disabled, reset setpoints */
 			_reset_alt_sp = true;
 			_reset_pos_sp = true;
+			_mode_auto = false;
 			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 e9e03a44e..7fac69a61 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -37,6 +37,7 @@
  *
  * @author Julian Oes <julian@oes.ch>
  * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #include <sys/types.h>
@@ -113,6 +114,7 @@ Mission::on_inactive()
 		update_offboard_mission();
 	}
 
+	/* require takeoff after non-loiter or landing */
 	if (!_navigator->get_can_loiter_at_sp() || _navigator->get_vstatus()->condition_landed) {
 		_need_takeoff = true;
 	}
@@ -147,8 +149,19 @@ Mission::on_active()
 
 	/* lets check if we reached the current mission item */
 	if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
-		advance_mission();
-		set_mission_items();
+		if (_mission_item.autocontinue) {
+			/* switch to next waypoint if 'autocontinue' flag set */
+			advance_mission();
+			set_mission_items();
+
+		} else {
+			/* else just report that item reached */
+			if (_mission_type == MISSION_TYPE_OFFBOARD) {
+				if (!(_navigator->get_mission_result()->seq_reached == _current_offboard_mission_index && _navigator->get_mission_result()->reached)) {
+					set_mission_item_reached();
+				}
+			}
+		}
 
 	} else if (_mission_type != MISSION_TYPE_NONE &&_param_altmode.get() == MISSION_ALTMODE_FOH) {
 		altitude_sp_foh_update();
@@ -221,7 +234,7 @@ Mission::update_offboard_mission()
 		_current_offboard_mission_index = 0;
 	}
 
-	report_current_offboard_mission_item();
+	set_current_offboard_mission_item();
 }
 
 
@@ -359,6 +372,10 @@ Mission::set_mission_items()
 		/* no mission available or mission finished, switch to loiter */
 		if (_mission_type != MISSION_TYPE_NONE) {
 			mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
+
+			/* use last setpoint for loiter */
+			_navigator->set_can_loiter_at_sp(true);
+
 		} else if (!user_feedback_done) {
 			/* only tell users that we got no mission if there has not been any
 			 * better, more specific feedback yet
@@ -375,10 +392,11 @@ Mission::set_mission_items()
 		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
 		pos_sp_triplet->next.valid = false;
 
+		/* reuse setpoint for LOITER only if it's not IDLE */
 		_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER);
 
 		reset_mission_item_reached();
-		report_mission_finished();
+		set_mission_finished();
 
 		_navigator->set_position_setpoint_triplet_updated();
 		return;
@@ -415,7 +433,7 @@ Mission::set_mission_items()
 			takeoff_alt += _navigator->get_home_position()->alt;
 		}
 
-		/* perform takeoff at least to NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
+		/* takeoff to at least NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
 		if (_navigator->get_vstatus()->condition_landed) {
 			takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _param_takeoff_alt.get());
 
@@ -423,32 +441,46 @@ Mission::set_mission_items()
 			takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get());
 		}
 
-		mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
+		/* check if we already above takeoff altitude */
+		if (_navigator->get_global_position()->alt < takeoff_alt - _navigator->get_acceptance_radius()) {
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters 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;
-		_mission_item.altitude = takeoff_alt;
-		_mission_item.altitude_is_relative = false;
+			_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
+			_mission_item.lat = _navigator->get_global_position()->lat;
+			_mission_item.lon = _navigator->get_global_position()->lon;
+			_mission_item.altitude = takeoff_alt;
+			_mission_item.altitude_is_relative = false;
+			_mission_item.autocontinue = true;
+			_mission_item.time_inside = 0;
 
-		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+			mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
 
-	} else {
-		/* set current position setpoint from mission item */
-		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+			_navigator->set_position_setpoint_triplet_updated();
+			return;
 
-		/* require takeoff after landing or idle */
-		if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) {
-			_need_takeoff = true;
+		} else {
+			/* skip takeoff */
+			_takeoff = false;
 		}
+	}
 
-		_navigator->set_can_loiter_at_sp(false);
-		reset_mission_item_reached();
+	/* set current position setpoint from mission item */
+	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
 
-		if (_mission_type == MISSION_TYPE_OFFBOARD) {
-			report_current_offboard_mission_item();
-		}
-		// TODO: report onboard mission item somehow
+	/* require takeoff after landing or idle */
+	if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) {
+		_need_takeoff = true;
+	}
+
+	_navigator->set_can_loiter_at_sp(false);
+	reset_mission_item_reached();
+
+	if (_mission_type == MISSION_TYPE_OFFBOARD) {
+		set_current_offboard_mission_item();
+	}
+	// TODO: report onboard mission item somehow
 
+	if (_mission_item.autocontinue && _mission_item.time_inside <= 0.001f) {
 		/* try to read next mission item */
 		struct mission_item_s mission_item_next;
 
@@ -460,6 +492,10 @@ Mission::set_mission_items()
 			/* next mission item is not available */
 			pos_sp_triplet->next.valid = false;
 		}
+
+	} else {
+		/* vehicle will be paused on current waypoint, don't set next item */
+		pos_sp_triplet->next.valid = false;
 	}
 
 	/* Save the distance between the current sp and the previous one */
@@ -666,19 +702,19 @@ Mission::save_offboard_mission_state()
 }
 
 void
-Mission::report_mission_item_reached()
+Mission::set_mission_item_reached()
 {
-	if (_mission_type == MISSION_TYPE_OFFBOARD) {
-		_navigator->get_mission_result()->reached = true;
-		_navigator->get_mission_result()->seq_reached = _current_offboard_mission_index;
-	}
+	_navigator->get_mission_result()->reached = true;
+	_navigator->get_mission_result()->seq_reached = _current_offboard_mission_index;
 	_navigator->publish_mission_result();
 }
 
 void
-Mission::report_current_offboard_mission_item()
+Mission::set_current_offboard_mission_item()
 {
 	warnx("current offboard mission index: %d", _current_offboard_mission_index);
+	_navigator->get_mission_result()->reached = false;
+	_navigator->get_mission_result()->finished = false;
 	_navigator->get_mission_result()->seq_current = _current_offboard_mission_index;
 	_navigator->publish_mission_result();
 
@@ -686,9 +722,8 @@ Mission::report_current_offboard_mission_item()
 }
 
 void
-Mission::report_mission_finished()
+Mission::set_mission_finished()
 {
 	_navigator->get_mission_result()->finished = true;
 	_navigator->publish_mission_result();
 }
-
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index 176529209..ea7cc0927 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -37,6 +37,7 @@
  *
  * @author Julian Oes <julian@oes.ch>
  * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #ifndef NAVIGATOR_MISSION_H
@@ -130,19 +131,19 @@ private:
 	void save_offboard_mission_state();
 
 	/**
-	 * Report that a mission item has been reached
+	 * Set a mission item as reached
 	 */
-	void report_mission_item_reached();
+	void set_mission_item_reached();
 
 	/**
-	 * Rport the current mission item
+	 * Set the current offboard mission item
 	 */
-	void report_current_offboard_mission_item();
+	void set_current_offboard_mission_item();
 
 	/**
-	 * Report that the mission is finished if one exists or that none exists
+	 * Set that the mission is finished if one exists or that none exists
 	 */
-	void report_mission_finished();
+	void set_mission_finished();
 
 	control::BlockParamInt _param_onboard_enabled;
 	control::BlockParamFloat _param_takeoff_alt;
@@ -154,8 +155,8 @@ private:
 
 	int _current_onboard_mission_index;
 	int _current_offboard_mission_index;
-	bool _need_takeoff;
-	bool _takeoff;
+	bool _need_takeoff;					/**< if true, then takeoff must be performed before going to the first waypoint (if needed) */
+	bool _takeoff;						/**< takeoff state flag */
 
 	enum {
 		MISSION_TYPE_NONE,
diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp
index 937e4fa5a..389cdd0d2 100644
--- a/src/modules/navigator/mission_feasibility_checker.cpp
+++ b/src/modules/navigator/mission_feasibility_checker.cpp
@@ -84,7 +84,12 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_
 bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
 {
 
-	return  (checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt));
+	/* Perform checks and issue feedback to the user for all checks */
+	bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence);
+	bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt);
+
+	/* Mission is only marked as feasible if all checks return true */
+	return (resGeofence && resHomeAltitude);
 }
 
 bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
@@ -93,7 +98,13 @@ bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_curre
 	updateNavigationCapabilities();
 //	warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement);
 
-	return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt));
+	/* Perform checks and issue feedback to the user for all checks */
+	bool resLanding = checkFixedWingLanding(dm_current, nMissionItems);
+	bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence);
+	bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt);
+
+	/* Mission is only marked as feasible if all checks return true */
+	return (resLanding && resGeofence && resHomeAltitude);
 }
 
 bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
@@ -216,9 +227,8 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
 		}
 	}
 
-
-//	float slope_alt = wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt;
-	return false;
+	/* No landing waypoints or no waypoints */
+	return true;
 }
 
 void MissionFeasibilityChecker::updateNavigationCapabilities()
diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk
index c075903b7..72dc2815c 100644
--- a/src/modules/navigator/module.mk
+++ b/src/modules/navigator/module.mk
@@ -46,7 +46,6 @@ SRCS		= navigator_main.cpp \
 		  loiter.cpp \
 		  rtl.cpp \
 		  rtl_params.c \
-		  offboard.cpp \
 		  mission_feasibility_checker.cpp \
 		  geofence.cpp \
 		  geofence_params.c \
@@ -63,3 +62,5 @@ INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
 MODULE_STACKSIZE = 1200
 
 EXTRACXXFLAGS	= -Weffc++
+
+MAXOPTIMIZATION = -Os
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index bf42acff9..d550dcc4c 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -60,7 +60,6 @@
 #include "mission.h"
 #include "loiter.h"
 #include "rtl.h"
-#include "offboard.h"
 #include "datalinkloss.h"
 #include "enginefailure.h"
 #include "gpsfailure.h"
@@ -70,7 +69,7 @@
 /**
  * Number of navigation modes that need on_active/on_inactive calls
  */
-#define NAVIGATOR_MODE_ARRAY_SIZE 8
+#define NAVIGATOR_MODE_ARRAY_SIZE 7
 
 class Navigator : public control::SuperBlock
 {
@@ -139,7 +138,6 @@ public:
 
 	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(); }
@@ -159,7 +157,6 @@ 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 */
@@ -199,7 +196,6 @@ private:
 	RTL 		_rtl;				/**< class that handles RTL */
 	RCLoss 		_rcLoss;				/**< class that handles RTL according to
 							  OBC rules (rc loss mode) */
-	Offboard	_offboard;			/**< class that handles offboard */
 	DataLinkLoss	_dataLinkLoss;			/**< class that handles the OBC datalink loss mode */
 	EngineFailure	_engineFailure;			/**< class that handles the engine failure mode
 							  (FW only!) */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 0e7f116ea..a867dd0da 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -68,7 +68,6 @@
 #include <uORB/topics/mission.h>
 #include <uORB/topics/fence.h>
 #include <uORB/topics/navigation_capabilities.h>
-#include <uORB/topics/offboard_control_setpoint.h>
 #include <drivers/drv_baro.h>
 
 #include <systemlib/err.h>
@@ -87,6 +86,7 @@
  */
 extern "C" __EXPORT int navigator_main(int argc, char *argv[]);
 
+#define GEOFENCE_CHECK_INTERVAL 200000
 
 namespace navigator
 {
@@ -104,7 +104,6 @@ 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),
@@ -133,7 +132,6 @@ Navigator::Navigator() :
 	_loiter(this, "LOI"),
 	_rtl(this, "RTL"),
 	_rcLoss(this, "RCL"),
-	_offboard(this, "OFF"),
 	_dataLinkLoss(this, "DLL"),
 	_engineFailure(this, "EF"),
 	_gpsFailure(this, "GPSF"),
@@ -148,11 +146,10 @@ Navigator::Navigator() :
 	_navigation_mode_array[0] = &_mission;
 	_navigation_mode_array[1] = &_loiter;
 	_navigation_mode_array[2] = &_rtl;
-	_navigation_mode_array[3] = &_offboard;
-	_navigation_mode_array[4] = &_dataLinkLoss;
-	_navigation_mode_array[5] = &_engineFailure;
-	_navigation_mode_array[6] = &_gpsFailure;
-	_navigation_mode_array[7] = &_rcLoss;
+	_navigation_mode_array[3] = &_dataLinkLoss;
+	_navigation_mode_array[4] = &_engineFailure;
+	_navigation_mode_array[5] = &_gpsFailure;
+	_navigation_mode_array[6] = &_rcLoss;
 
 	updateParams();
 }
@@ -281,7 +278,6 @@ 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();
@@ -343,7 +339,7 @@ Navigator::task_main()
 			_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 		}
 
-		bool have_geofence_position_data = false;
+		static bool have_geofence_position_data = false;
 
 		/* gps updated */
 		if (fds[7].revents & POLLIN) {
@@ -393,8 +389,11 @@ Navigator::task_main()
 		}
 
 		/* Check geofence violation */
-		if (have_geofence_position_data) {
+		static hrt_abstime last_geofence_check = 0;
+		if (have_geofence_position_data && hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL) {
 			bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter);
+			last_geofence_check = hrt_absolute_time();
+			have_geofence_position_data = false;
 			if (!inside) {
 				/* inform other apps via the mission result */
 				_mission_result.geofence_violated = true;
@@ -422,13 +421,11 @@ Navigator::task_main()
 			case NAVIGATION_STATE_POSCTL:
 			case NAVIGATION_STATE_LAND:
 			case NAVIGATION_STATE_TERMINATION:
+			case NAVIGATION_STATE_OFFBOARD:
 				_navigation_mode = nullptr;
 				_can_loiter_at_sp = false;
 				break;
 			case NAVIGATION_STATE_AUTO_MISSION:
-				/* Some failsafe modes prohibit the fallback to mission
-				 * usually this is done after some time to make sure
-				 * that the full failsafe operation is performed */
 				_navigation_mode = &_mission;
 				break;
 			case NAVIGATION_STATE_AUTO_LOITER:
@@ -444,7 +441,7 @@ Navigator::task_main()
 			case NAVIGATION_STATE_AUTO_RTL:
 					_navigation_mode = &_rtl;
 				break;
-			case NAVIGATION_STATE_AUTO_RTGS: //XXX OBC: differentiate between rc loss and dl loss here
+			case NAVIGATION_STATE_AUTO_RTGS:
 				/* Use complex data link loss mode only when enabled via param
 				* otherwise use rtl */
 				if (_param_datalinkloss_obc.get() != 0) {
@@ -459,9 +456,6 @@ Navigator::task_main()
 			case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
 				_navigation_mode = &_gpsFailure;
 				break;
-			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
deleted file mode 100644
index dcb5c6000..000000000
--- a/src/modules/navigator/offboard.cpp
+++ /dev/null
@@ -1,129 +0,0 @@
-/****************************************************************************
- *
- *   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/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c
index 9e5429988..12187d70e 100644
--- a/src/modules/systemlib/circuit_breaker.c
+++ b/src/modules/systemlib/circuit_breaker.c
@@ -106,7 +106,7 @@ PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0);
  * @max 121212
  * @group Circuit Breaker
  */
-PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 0);
+PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212);
 
 /**
  * Circuit breaker for engine failure detection
@@ -120,7 +120,21 @@ PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 0);
  * @max 284953
  * @group Circuit Breaker
  */
-PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 0);
+PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953);
+
+/**
+ * Circuit breaker for gps failure detection
+ *
+ * Setting this parameter to 240024 will disable the gps failure detection.
+ * If the aircraft is in gps failure mode the gps failure flag will be
+ * set to healthy
+ * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
+ *
+ * @min 0
+ * @max 240024
+ * @group Circuit Breaker
+ */
+PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024);
 
 bool circuit_breaker_enabled(const char* breaker, int32_t magic)
 {
diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h
index 6a55e4948..b3431722f 100644
--- a/src/modules/systemlib/circuit_breaker.h
+++ b/src/modules/systemlib/circuit_breaker.h
@@ -55,6 +55,7 @@
 #define CBRK_AIRSPD_CHK_KEY	162128
 #define CBRK_FLIGHTTERM_KEY	121212
 #define CBRK_ENGINEFAIL_KEY	284953
+#define CBRK_GPSFAIL_KEY	240024
 
 #include <stdbool.h>
 
diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h
index cdf60febc..17989558e 100644
--- a/src/modules/systemlib/mixer/mixer.h
+++ b/src/modules/systemlib/mixer/mixer.h
@@ -130,6 +130,9 @@
 
 #include <nuttx/config.h>
 #include "drivers/drv_mixer.h"
+
+#include <uORB/topics/multirotor_motor_limits.h>
+
 #include "mixer_load.h"
 
 /**
@@ -531,6 +534,9 @@ private:
 	float				_yaw_scale;
 	float				_idle_speed;
 
+	orb_advert_t			_limits_pub;
+	multirotor_motor_limits_s 	_limits;
+
 	unsigned			_rotor_count;
 	const Rotor			*_rotors;
 
diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp
index 4b22a46d0..57e17b67d 100644
--- a/src/modules/systemlib/mixer/mixer_multirotor.cpp
+++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp
@@ -36,7 +36,8 @@
  *
  * Multi-rotor mixers.
  */
-
+#include <uORB/uORB.h>
+#include <uORB/topics/multirotor_motor_limits.h>
 #include <nuttx/config.h>
 
 #include <sys/types.h>
@@ -302,6 +303,11 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 	float		min_out = 0.0f;
 	float		max_out = 0.0f;
 
+	_limits.roll_pitch = false;
+	_limits.yaw = false;
+	_limits.throttle_upper = false;
+	_limits.throttle_lower = false;
+
 	/* perform initial mix pass yielding unbounded outputs, ignore yaw */
 	for (unsigned i = 0; i < _rotor_count; i++) {
 		float out = roll * _rotors[i].roll_scale +
@@ -311,6 +317,7 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 		/* limit yaw if it causes outputs clipping */
 		if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) {
 			yaw = -out / _rotors[i].yaw_scale;
+			_limits.yaw = true;
 		}
 
 		/* calculate min and max output values */
@@ -332,6 +339,7 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 		for (unsigned i = 0; i < _rotor_count; i++) {
 			outputs[i] = scale_in * (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) + thrust;
 		}
+		_limits.roll_pitch = true;
 
 	} else {
 		/* roll/pitch mixed without limiting, add yaw control */
@@ -344,6 +352,7 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 	float scale_out;
 	if (max_out > 1.0f) {
 		scale_out = 1.0f / max_out;
+		_limits.throttle_upper = true;
 
 	} else {
 		scale_out = 1.0f;
@@ -351,9 +360,20 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 
 	/* scale outputs to range _idle_speed..1, and do final limiting */
 	for (unsigned i = 0; i < _rotor_count; i++) {
+		if (outputs[i] < _idle_speed) {
+			_limits.throttle_lower = true;
+		}
 		outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed) * scale_out), _idle_speed, 1.0f);
 	}
 
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) || defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+        /* publish/advertise motor limits if running on FMU */
+        if (_limits_pub > 0) {
+            orb_publish(ORB_ID(multirotor_motor_limits), _limits_pub, &_limits);
+        } else {
+            _limits_pub = orb_advertise(ORB_ID(multirotor_motor_limits), &_limits);
+        }
+#endif
 	return _rotor_count;
 }
 
diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c
index e44e6cdb0..6b8d0e634 100644
--- a/src/modules/systemlib/param/param.c
+++ b/src/modules/systemlib/param/param.c
@@ -322,7 +322,8 @@ param_get_value_ptr(param_t param)
 			v = &param_info_base[param].val;
 		}
 
-		if (param_type(param) == PARAM_TYPE_STRUCT) {
+		if (param_type(param) >= PARAM_TYPE_STRUCT
+				&& param_type(param) <= PARAM_TYPE_STRUCT_MAX) {
 			result = v->p;
 
 		} else {
diff --git a/src/modules/systemlib/param/param.h b/src/modules/systemlib/param/param.h
index 084cd931a..dc73b37e9 100644
--- a/src/modules/systemlib/param/param.h
+++ b/src/modules/systemlib/param/param.h
@@ -307,7 +307,7 @@ __EXPORT int 		param_load_default(void);
 	struct param_info_s __param__##_name = {	\
 		#_name,					\
 		PARAM_TYPE_STRUCT + sizeof(_default),	\
-		.val.p = &_default;			\
+		.val.p = &_default			\
 	}
 
 /**
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index f7d5f8737..b921865eb 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -192,6 +192,9 @@ ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s);
 ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s);
 ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s);
 
+#include "topics/multirotor_motor_limits.h"
+ORB_DEFINE(multirotor_motor_limits, struct multirotor_motor_limits_s);
+
 #include "topics/telemetry_status.h"
 ORB_DEFINE(telemetry_status_0, struct telemetry_status_s);
 ORB_DEFINE(telemetry_status_1, struct telemetry_status_s);
diff --git a/src/modules/navigator/offboard.h b/src/modules/uORB/topics/multirotor_motor_limits.h
index 66b923bdb..44c51e4d9 100644
--- a/src/modules/navigator/offboard.h
+++ b/src/modules/uORB/topics/multirotor_motor_limits.h
@@ -1,6 +1,6 @@
-/***************************************************************************
+/****************************************************************************
  *
- *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012-2013 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
@@ -30,43 +30,40 @@
  * POSSIBILITY OF SUCH DAMAGE.
  *
  ****************************************************************************/
+
 /**
- * @file offboard.h
- *
- * Helper class for offboard commands
+ * @file multirotor_motor_limits.h
  *
- * @author Julian Oes <julian@oes.ch>
+ * Definition of multirotor_motor_limits  topic
  */
 
-#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>
+#ifndef MULTIROTOR_MOTOR_LIMITS_H_
+#define MULTIROTOR_MOTOR_LIMITS_H_
 
-#include "navigator_mode.h"
+#include "../uORB.h"
+#include <stdint.h>
 
-class Navigator;
-
-class Offboard : public NavigatorMode
-{
-public:
-	Offboard(Navigator *navigator, const char *name);
-
-	~Offboard();
-
-	virtual void on_inactive();
+/**
+ * @addtogroup topics
+ * @{
+ */
 
-	virtual void on_activation();
+/**
+ * Motor limits
+ */
+struct multirotor_motor_limits_s {
+        uint8_t roll_pitch	: 1; // roll/pitch limit reached
+        uint8_t yaw		: 1; // yaw limit reached
+        uint8_t throttle_lower	: 1; // lower throttle limit reached
+        uint8_t throttle_upper	: 1; // upper throttle limit reached
+        uint8_t reserved	: 4;
+};
 
-	virtual void on_active();
-private:
-	void update_offboard_control_setpoint();
+/**
+ * @}
+ */
 
-	struct offboard_control_setpoint_s _offboard_control_sp;
-};
+/* register this as object request broker structure */
+ORB_DECLARE(multirotor_motor_limits);
 
 #endif
diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h
index d7b131e3c..72a28e501 100644
--- a/src/modules/uORB/topics/offboard_control_setpoint.h
+++ b/src/modules/uORB/topics/offboard_control_setpoint.h
@@ -53,11 +53,42 @@ enum OFFBOARD_CONTROL_MODE {
 	OFFBOARD_CONTROL_MODE_DIRECT = 0,
 	OFFBOARD_CONTROL_MODE_DIRECT_RATES = 1,
 	OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE = 2,
-	OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY = 3,
-	OFFBOARD_CONTROL_MODE_DIRECT_POSITION = 4,
-	OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 5,
-	OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 6,
-	OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 7, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */
+	OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED = 3,
+	OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED = 4,
+	OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED = 5,
+	OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED = 6,
+	OFFBOARD_CONTROL_MODE_DIRECT_GLOBAL = 7,
+	OFFBOARD_CONTROL_MODE_DIRECT_FORCE = 8,
+	OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 9,
+	OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 10,
+	OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 11 /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */
+};
+
+enum OFFBOARD_CONTROL_FRAME {
+	OFFBOARD_CONTROL_FRAME_LOCAL_NED = 0,
+	OFFBOARD_CONTROL_FRAME_LOCAL_OFFSET_NED = 1,
+	OFFBOARD_CONTROL_FRAME_BODY_NED = 2,
+	OFFBOARD_CONTROL_FRAME_BODY_OFFSET_NED = 3,
+	OFFBOARD_CONTROL_FRAME_GLOBAL = 4
+};
+
+/* mappings for the ignore bitmask */
+enum {OFB_IGN_BIT_POS_X,
+	OFB_IGN_BIT_POS_Y,
+	OFB_IGN_BIT_POS_Z,
+	OFB_IGN_BIT_VEL_X,
+	OFB_IGN_BIT_VEL_Y,
+	OFB_IGN_BIT_VEL_Z,
+	OFB_IGN_BIT_ACC_X,
+	OFB_IGN_BIT_ACC_Y,
+	OFB_IGN_BIT_ACC_Z,
+	OFB_IGN_BIT_BODYRATE_X,
+	OFB_IGN_BIT_BODYRATE_Y,
+	OFB_IGN_BIT_BODYRATE_Z,
+	OFB_IGN_BIT_ATT,
+	OFB_IGN_BIT_THRUST,
+	OFB_IGN_BIT_YAW,
+	OFB_IGN_BIT_YAWRATE,
 };
 
 /**
@@ -70,10 +101,21 @@ struct offboard_control_setpoint_s {
 
 	enum OFFBOARD_CONTROL_MODE mode;		 /**< The current control inputs mode */
 
-	float p1;	/**< ailerons roll / roll rate input */
-	float p2;	/**< elevator / pitch / pitch rate */
-	float p3;	/**< rudder / yaw rate / yaw */
-	float p4;	/**< throttle / collective thrust / altitude */
+	double position[3];	/**< lat, lon, alt / x, y, z */
+	float velocity[3];	/**< x vel, y vel, z vel */
+	float acceleration[3];	/**< x acc, y acc, z acc */
+	float attitude[4];	/**< attitude of vehicle (quaternion) */
+	float attitude_rate[3];	/**< body angular rates (x, y, z) */
+	float thrust;	/**< thrust */
+	float yaw;	/**< yaw: this is the yaw from the position_target message
+			  (not from the full attitude_target message) */
+	float yaw_rate;	/**< yaw rate: this is the yaw from the position_target message
+			  (not from the full attitude_target message) */
+
+	uint16_t ignore; /**< if field i is set to true, the value should be ignored, see definition at top of file
+			   for mapping */
+
+	bool isForceSetpoint; /**< the acceleration vector should be interpreted as force */
 
 	float override_mode_switch;
 
@@ -87,6 +129,147 @@ struct offboard_control_setpoint_s {
  * @}
  */
 
+/**
+ * Returns true if the position setpoint at index should be ignored
+ */
+inline bool offboard_control_sp_ignore_position(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
+	return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_POS_X +  index)));
+}
+
+/**
+ * Returns true if all position setpoints should be ignored
+ */
+inline bool offboard_control_sp_ignore_position_all(const struct offboard_control_setpoint_s &offboard_control_sp) {
+	for (int i = 0; i < 3; i++) {
+		if (!offboard_control_sp_ignore_position(offboard_control_sp, i))	{
+			return false;
+		}
+	}
+	return true;
+}
+
+/**
+ * Returns true if some position setpoints should be ignored
+ */
+inline bool offboard_control_sp_ignore_position_some(const struct offboard_control_setpoint_s &offboard_control_sp) {
+	for (int i = 0; i < 3; i++) {
+		if (offboard_control_sp_ignore_position(offboard_control_sp, i))	{
+			return true;
+		}
+	}
+	return false;
+}
+
+/**
+ * Returns true if the velocity setpoint at index should be ignored
+ */
+inline bool offboard_control_sp_ignore_velocity(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
+	return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_VEL_X + index)));
+}
+
+/**
+ * Returns true if all velocity setpoints should be ignored
+ */
+inline bool offboard_control_sp_ignore_velocity_all(const struct offboard_control_setpoint_s &offboard_control_sp) {
+	for (int i = 0; i < 3; i++) {
+		if (!offboard_control_sp_ignore_velocity(offboard_control_sp, i))	{
+			return false;
+		}
+	}
+	return true;
+}
+
+/**
+ * Returns true if some velocity setpoints should be ignored
+ */
+inline bool offboard_control_sp_ignore_velocity_some(const struct offboard_control_setpoint_s &offboard_control_sp) {
+	for (int i = 0; i < 3; i++) {
+		if (offboard_control_sp_ignore_velocity(offboard_control_sp, i))	{
+			return true;
+		}
+	}
+	return false;
+}
+
+/**
+ * Returns true if the acceleration setpoint at index should be ignored
+ */
+inline bool offboard_control_sp_ignore_acceleration(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
+	return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_ACC_X + index)));
+}
+
+/**
+ * Returns true if all acceleration setpoints should be ignored
+ */
+inline bool offboard_control_sp_ignore_acceleration_all(const struct offboard_control_setpoint_s &offboard_control_sp) {
+	for (int i = 0; i < 3; i++) {
+		if (!offboard_control_sp_ignore_acceleration(offboard_control_sp, i))	{
+			return false;
+		}
+	}
+	return true;
+}
+
+/**
+ * Returns true if some acceleration setpoints should be ignored
+ */
+inline bool offboard_control_sp_ignore_acceleration_some(const struct offboard_control_setpoint_s &offboard_control_sp) {
+	for (int i = 0; i < 3; i++) {
+		if (offboard_control_sp_ignore_acceleration(offboard_control_sp, i))	{
+			return true;
+		}
+	}
+	return false;
+}
+
+/**
+ * Returns true if the bodyrate setpoint at index should be ignored
+ */
+inline bool offboard_control_sp_ignore_bodyrates(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
+	return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_BODYRATE_X + index)));
+}
+
+/**
+ * Returns true if some of the bodyrate setpoints should be ignored
+ */
+inline bool offboard_control_sp_ignore_bodyrates_some(const struct offboard_control_setpoint_s &offboard_control_sp) {
+	for (int i = 0; i < 3; i++) {
+		if (offboard_control_sp_ignore_bodyrates(offboard_control_sp, i))	{
+			return true;
+		}
+	}
+	return false;
+}
+
+/**
+ * Returns true if the attitude setpoint should be ignored
+ */
+inline bool offboard_control_sp_ignore_attitude(const struct offboard_control_setpoint_s &offboard_control_sp) {
+	return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_ATT));
+}
+
+/**
+ * Returns true if the thrust setpoint should be ignored
+ */
+inline bool offboard_control_sp_ignore_thrust(const struct offboard_control_setpoint_s &offboard_control_sp) {
+	return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_THRUST));
+}
+
+/**
+ * Returns true if the yaw setpoint should be ignored
+ */
+inline bool offboard_control_sp_ignore_yaw(const struct offboard_control_setpoint_s &offboard_control_sp) {
+	return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_YAW));
+}
+
+/**
+ * Returns true if the yaw rate setpoint should be ignored
+ */
+inline bool offboard_control_sp_ignore_yawrate(const struct offboard_control_setpoint_s &offboard_control_sp) {
+	return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_YAWRATE));
+}
+
+
 /* register this as object request broker structure */
 ORB_DECLARE(offboard_control_setpoint);
 
diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h
index ec2131abd..cb2262534 100644
--- a/src/modules/uORB/topics/position_setpoint_triplet.h
+++ b/src/modules/uORB/topics/position_setpoint_triplet.h
@@ -79,10 +79,17 @@ struct position_setpoint_s
 	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 */
+	bool yaw_valid;			/**< true if yaw setpoint valid */
 	float yawspeed;			/**< yawspeed (only for multirotors, in rad/s) */
+	bool yawspeed_valid;		/**< true if yawspeed setpoint valid */
 	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 */
+	float a_x;			//**< acceleration x setpoint */
+	float a_y;			//**< acceleration y setpoint */
+	float a_z;			//**< acceleration z setpoint */
+	bool acceleration_valid;	//*< true if acceleration setpoint is valid/should be used */
+	bool acceleration_is_force;	//*< interprete acceleration as force */
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h
index 44aa50572..f264accbb 100644
--- a/src/modules/uORB/topics/vehicle_command.h
+++ b/src/modules/uORB/topics/vehicle_command.h
@@ -65,6 +65,9 @@ enum VEHICLE_CMD {
 	VEHICLE_CMD_NAV_TAKEOFF = 22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude|  */
 	VEHICLE_CMD_NAV_ROI = 80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|  */
 	VEHICLE_CMD_NAV_PATHPLANNING = 81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal|  */
+	VEHICLE_CMD_NAV_GUIDED_LIMITS=90, /* set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, WGS84) - if vehicle moves below this alt, the command will be aborted and the mission will continue.  0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue.  0 means no upper altitude limit| horizontal move limit (in meters, WGS84) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty|  */
+	VEHICLE_CMD_NAV_GUIDED_MASTER=91, /* set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty|  */
+	VEHICLE_CMD_NAV_GUIDED_ENABLE=92, /* hand control over to an external controller |On / Off (> 0.5f on)| Empty| Empty| Empty| Empty| Empty| Empty|  */
 	VEHICLE_CMD_NAV_LAST = 95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
 	VEHICLE_CMD_CONDITION_DELAY = 112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty|  */
 	VEHICLE_CMD_CONDITION_CHANGE_ALT = 113, /* Ascend/descend at rate.  Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude|  */
diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h
index 49e2ba4b5..ca7705456 100644
--- a/src/modules/uORB/topics/vehicle_control_mode.h
+++ b/src/modules/uORB/topics/vehicle_control_mode.h
@@ -77,6 +77,7 @@ struct vehicle_control_mode_s {
 	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_force_enabled;		/**< true if force control is mixed in */
 	bool flag_control_velocity_enabled;		/**< true if horizontal velocity (implies direction) is controlled */
 	bool flag_control_position_enabled;		/**< true if position is controlled */
 	bool flag_control_altitude_enabled;		/**< true if altitude is controlled */
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 9dccb2309..a1b2667e3 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -218,6 +218,8 @@ struct vehicle_status_s {
 	bool offboard_control_signal_lost;
 	bool offboard_control_signal_weak;
 	uint64_t offboard_control_signal_lost_interval;	/**< interval in microseconds without an offboard control message */
+	bool offboard_control_set_by_command; /**< true if the offboard mode was set by a mavlink command
+						    and should not be overridden by RC */
 
 	/* see SYS_STATUS mavlink message for the following */
 	uint32_t onboard_control_sensors_present;
@@ -239,6 +241,8 @@ struct vehicle_status_s {
 
 	bool circuit_breaker_engaged_power_check;
 	bool circuit_breaker_engaged_airspd_check;
+	bool circuit_breaker_engaged_enginefailure_check;
+	bool circuit_breaker_engaged_gpsfailure_check;
 };
 
 /**
diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp
index 0d67aad47..24afe6aaf 100644
--- a/src/modules/uavcan/sensors/gnss.cpp
+++ b/src/modules/uavcan/sensors/gnss.cpp
@@ -43,8 +43,6 @@
 #include <systemlib/err.h>
 #include <mathlib/mathlib.h>
 
-#define MM_PER_CM 			10	// Millimeters per centimeter
-
 const char *const UavcanGnssBridge::NAME = "gnss";
 
 UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
@@ -95,9 +93,9 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
 	auto report = ::vehicle_gps_position_s();
 
 	report.timestamp_position = hrt_absolute_time();
-	report.lat = msg.lat_1e7;
-	report.lon = msg.lon_1e7;
-	report.alt = msg.alt_1e2 * MM_PER_CM;	// Convert from centimeter (1e2) to millimeters (1e3)
+	report.lat = msg.latitude_deg_1e8 / 10;
+	report.lon = msg.longitude_deg_1e8 / 10;
+	report.alt = msg.height_msl_mm;
 
 	report.timestamp_variance = report.timestamp_position;
 
diff --git a/src/modules/unit_test/unit_test.cpp b/src/modules/unit_test/unit_test.cpp
index 02d1af481..b7568ce3a 100644
--- a/src/modules/unit_test/unit_test.cpp
+++ b/src/modules/unit_test/unit_test.cpp
@@ -36,7 +36,11 @@
 
 #include <systemlib/err.h>
 
-UnitTest::UnitTest()
+UnitTest::UnitTest() :
+	_tests_run(0),
+	_tests_failed(0),
+	_tests_passed(0),
+	_assertions(0)
 {
 }
 
@@ -44,15 +48,22 @@ UnitTest::~UnitTest()
 {
 }
 
-void UnitTest::printResults(void)
+void UnitTest::print_results(void)
 {
-    warnx(mu_tests_failed() ? "SOME TESTS FAILED" : "ALL TESTS PASSED");
-    warnx("  Tests passed : %d", mu_tests_passed());
-    warnx("  Tests failed : %d", mu_tests_failed());
-    warnx("  Assertions : %d", mu_assertion());
+	warnx(_tests_failed ? "SOME TESTS FAILED" : "ALL TESTS PASSED");
+	warnx("  Tests passed : %d", _tests_passed);
+	warnx("  Tests failed : %d", _tests_failed);
+	warnx("  Assertions : %d", _assertions);
 }
 
-void UnitTest::printAssert(const char* msg, const char* test, const char* file, int line)
+/// @brief Used internally to the ut_assert macro to print assert failures.
+void UnitTest::_print_assert(const char* msg, const char* test, const char* file, int line)
 {
-    warnx("Assertion failed: %s - %s (%s:%d)", msg, test, file, line);
+	warnx("Assertion failed: %s - %s (%s:%d)", msg, test, file, line);
+}
+
+/// @brief Used internally to the ut_compare macro to print assert failures.
+void UnitTest::_print_compare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line)
+{
+	warnx("Compare failed: %s - (%s:%d) (%s:%d) (%s:%d)", msg, v1_text, v1, v2_text, v2, file, line);
 }
diff --git a/src/modules/unit_test/unit_test.h b/src/modules/unit_test/unit_test.h
index 32eb8c308..8ed4efadf 100644
--- a/src/modules/unit_test/unit_test.h
+++ b/src/modules/unit_test/unit_test.h
@@ -37,50 +37,85 @@
 
 #include <systemlib/err.h>
 
+/// @brief Base class to be used for unit tests.
 class __EXPORT UnitTest
 {
-
 public:
-#define INLINE_GLOBAL(type,func) inline type& func() { static type x; return x; }
-
-INLINE_GLOBAL(int, mu_tests_run)
-INLINE_GLOBAL(int, mu_tests_failed)
-INLINE_GLOBAL(int, mu_tests_passed)
-INLINE_GLOBAL(int, mu_assertion)
-INLINE_GLOBAL(int, mu_line)
-INLINE_GLOBAL(const char*, mu_last_test)
 
 	UnitTest();
-    virtual ~UnitTest();
-
-    virtual void runTests(void) = 0;
-    void printResults(void);
-    
-    void printAssert(const char* msg, const char* test, const char* file, int line);
-    
-#define ut_assert(message, test)                                \
-    do {                                                        \
-        if (!(test)) {                                          \
-            printAssert(message, #test, __FILE__, __LINE__);    \
-            return false;                                       \
-        } else {                                                \
-            mu_assertion()++;                                   \
-        }                                                       \
-    } while (0)
-    
-#define ut_run_test(test)                       \
-    do {                                        \
-        warnx("RUNNING TEST: %s", #test);       \
-        mu_tests_run()++;                       \
-        if (!test()) {                          \
-            warnx("TEST FAILED: %s", #test);    \
-            mu_tests_failed()++;                \
-        } else {                                \
-            warnx("TEST PASSED: %s", #test);    \
-            mu_tests_passed()++;                \
-        }                                       \
-    } while (0)
+	virtual ~UnitTest();
+	
+	/// @brief Override to run your unit tests. Unit tests should be called using ut_run_test macro.
+	/// @return true: all unit tests succeeded, false: one or more unit tests failed
+	virtual bool run_tests(void) = 0;
+	
+	/// @brief Prints results from running of unit tests.
+	void print_results(void);
+	
+/// @brief Macro to create a function which will run a unit test class and print results.
+#define ut_declare_test(test_function, test_class)	\
+	bool test_function(void)			\
+	{						\
+		test_class* test = new test_class();	\
+		bool success = test->run_tests();	\
+		test->print_results();			\
+		return success;				\
+	}
+	
+protected:
+	
+/// @brief Runs a single unit test. Unit tests must have the function signature of bool test(void). The unit
+/// test should return true if it succeeded, false for fail.
+#define ut_run_test(test)					\
+	do {							\
+		warnx("RUNNING TEST: %s", #test);		\
+		_tests_run++;					\
+		_init();						\
+		if (!test()) {					\
+			warnx("TEST FAILED: %s", #test);	\
+			_tests_failed++;			\
+		} else {					\
+			warnx("TEST PASSED: %s", #test);	\
+			_tests_passed++;			\
+		}						\
+		_cleanup();					\
+	} while (0)
+	
+/// @brief Used to assert a value within a unit test.
+#define ut_assert(message, test)						\
+	do {									\
+		if (!(test)) {							\
+			_print_assert(message, #test, __FILE__, __LINE__);	\
+			return false;						\
+		} else {							\
+			_assertions++;						\
+		}								\
+	} while (0)
+	
+/// @brief Used to compare two integer values within a unit test. If possible use ut_compare instead of ut_assert
+/// since it will give you better error reporting of the actual values being compared.
+#define ut_compare(message, v1, v2)								\
+	do {											\
+		int _v1 = v1;									\
+		int _v2 = v2;									\
+		if (_v1 != _v2) {								\
+			_print_compare(message, #v1, _v1, #v2, _v2, __FILE__, __LINE__);	\
+			return false;								\
+		} else {									\
+			_assertions++;								\
+		}										\
+	} while (0)
+	
+	virtual void _init(void) { };		///< Run before each unit test. Override to provide custom behavior.
+	virtual void _cleanup(void) { };	///< Run after each unit test. Override to provide custom behavior.
+	
+	void _print_assert(const char* msg, const char* test, const char* file, int line);
+	void _print_compare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line);
 
+	int _tests_run;		///< The number of individual unit tests run
+	int _tests_failed;	///< The number of unit tests which failed
+	int _tests_passed;	///< The number of unit tests which passed
+	int _assertions;	///< Total number of assertions tested by all unit tests
 };
 
 #endif /* UNIT_TEST_H_ */