manual merge of origin/master into fw_control

59 files changed, 2088 insertions, 355 deletions

diff --git a/ROMFS/Makefile b/ROMFS/Makefile
index 56ae63d27..ec4221b93 100644
--- a/ROMFS/Makefile
+++ b/ROMFS/Makefile
@@ -29,6 +29,10 @@ ROMFS_FSSPEC	:= $(SRCROOT)/scripts/rcS~init.d/rcS \
 		   $(SRCROOT)/mixers/FMU_RET.mix~mixers/FMU_ERT.mix \
 		   $(SRCROOT)/mixers/FMU_quad_x.mix~mixers/FMU_quad_x.mix \
 		   $(SRCROOT)/mixers/FMU_quad_+.mix~mixers/FMU_quad_+.mix \
+		   $(SRCROOT)/mixers/FMU_hex_x.mix~mixers/FMU_hex_x.mix \
+		   $(SRCROOT)/mixers/FMU_hex_+.mix~mixers/FMU_hex_+.mix \
+		   $(SRCROOT)/mixers/FMU_octo_x.mix~mixers/FMU_octo_x.mix \
+		   $(SRCROOT)/mixers/FMU_octo_+.mix~mixers/FMU_octo_+.mix \
 		   $(SRCROOT)/logging/logconv.m~logging/logconv.m
 
 #
diff --git a/ROMFS/logging/logconv.m b/ROMFS/logging/logconv.m
index 88bcfec96..48399f1eb 100644
--- a/ROMFS/logging/logconv.m
+++ b/ROMFS/logging/logconv.m
@@ -32,7 +32,7 @@ if exist(filePath, 'file')
     fileSize = fileInfo.bytes;
     
     fid = fopen(filePath, 'r');
-    elements = int64(fileSize./(16*4+8))/4
+    elements = int64(fileSize./(8+(3+3+3+1+1+1+4+8+1+3+3+3+3+9+6+4+6)*4))
     
     for i=1:elements       
         % timestamp
@@ -79,6 +79,15 @@ if exist(filePath, 'file')
         
         % RotMatrix (9 channels)
         sensors(i,39:47) = fread(fid, 9, 'float', 0, 'ieee-le');
+
+        % vicon position (6 channels)
+        sensors(i,48:53) = fread(fid, 6, 'float', 0, 'ieee-le');
+
+        % actuator control effective (4 channels) 
+        sensors(i,54:57) = fread(fid, 4, 'float', 0, 'ieee-le');
+
+        % optical flow (6 values) 
+        sensors(i,58:63) = fread(fid, 6, 'float', 0, 'ieee-le');
     end
     time_us = sensors(elements,1) - sensors(1,1);
     time_s = time_us*1e-6
diff --git a/ROMFS/mixers/FMU_hex_+.mix b/ROMFS/mixers/FMU_hex_+.mix
new file mode 100644
index 000000000..b5e38ce9e
--- /dev/null
+++ b/ROMFS/mixers/FMU_hex_+.mix
@@ -0,0 +1,7 @@
+Multirotor mixer for PX4FMU
+===========================
+
+This file defines a single mixer for a hexacopter in the + configuration. All controls
+are mixed 100%.
+
+R: 6+ 10000 10000 10000 0
diff --git a/ROMFS/mixers/FMU_hex_x.mix b/ROMFS/mixers/FMU_hex_x.mix
new file mode 100644
index 000000000..8e8d122ad
--- /dev/null
+++ b/ROMFS/mixers/FMU_hex_x.mix
@@ -0,0 +1,7 @@
+Multirotor mixer for PX4FMU
+===========================
+
+This file defines a single mixer for a hexacopter in the X configuration.  All controls
+are mixed 100%.
+
+R: 6x 10000 10000 10000 0
diff --git a/ROMFS/mixers/FMU_octo_+.mix b/ROMFS/mixers/FMU_octo_+.mix
new file mode 100644
index 000000000..2cb70e814
--- /dev/null
+++ b/ROMFS/mixers/FMU_octo_+.mix
@@ -0,0 +1,7 @@
+Multirotor mixer for PX4FMU
+===========================
+
+This file defines a single mixer for a octocopter in the + configuration. All controls
+are mixed 100%.
+
+R: 8+ 10000 10000 10000 0
diff --git a/ROMFS/mixers/FMU_octo_x.mix b/ROMFS/mixers/FMU_octo_x.mix
new file mode 100644
index 000000000..edc71f013
--- /dev/null
+++ b/ROMFS/mixers/FMU_octo_x.mix
@@ -0,0 +1,7 @@
+Multirotor mixer for PX4FMU
+===========================
+
+This file defines a single mixer for a octocopter in the X configuration.  All controls
+are mixed 100%.
+
+R: 8x 10000 10000 10000 0
diff --git a/ROMFS/scripts/rc.sensors b/ROMFS/scripts/rc.sensors
index f913e82af..536fcfd2c 100644
--- a/ROMFS/scripts/rc.sensors
+++ b/ROMFS/scripts/rc.sensors
@@ -8,21 +8,26 @@
 #
 
 ms5611 start
-mpu6000 start
-hmc5883 start
+
+if mpu6000 start
+then
+	echo "using MPU6000 and HMC5883L"
+	hmc5883 start
+else
+	echo "using L3GD20 and LSM303D"
+	l3gd20 start
+	lsm303 start
+fi
 
 #
 # Start the sensor collection task.
+# IMPORTANT: this also loads param offsets
+# ALWAYS start this task before the
+# preflight_check.
 #
 sensors start
 
 #
-# Test sensor functionality
-#
-# XXX integrate with 'sensors start' ?
+# Check sensors - run AFTER 'sensors start'
 #
-#if sensors quicktest
-#then
-#	echo "[init] sensor initialisation FAILED."
-#	reboot
-#fi
+preflight_check
\ No newline at end of file
diff --git a/ROMFS/scripts/rc.standalone b/ROMFS/scripts/rc.standalone
index 8ccdb577b..67e95215b 100644
--- a/ROMFS/scripts/rc.standalone
+++ b/ROMFS/scripts/rc.standalone
@@ -10,68 +10,4 @@ echo "[init] doing standalone PX4FMU startup..."
 #
 uorb start
 
-#
-# Init the EEPROM
-#
-echo "[init] eeprom"
-eeprom start
-if [ -f /eeprom/parameters ]
-then
-	param load
-fi
-
-#
-# Start the sensors.
-#
-#sh /etc/init.d/rc.sensors
-
-#
-# Start MAVLink
-#
-# mavlink -d /dev/ttyS0 -b 57600 &
-
-#
-# Start the commander.
-#
-# XXX this should be 'commander start'.
-#
-#commander &
-
-#
-# Start the attitude estimator
-#
-# XXX this should be '<command> start'.
-#
-#attitude_estimator_bm &
-#position_estimator &
-
-#
-# Start the fixed-wing controller.
-#
-# XXX should this be looking for configuration to decide
-# whether the board is configured for fixed-wing use?
-#
-# XXX this should be 'fixedwing_control start'.
-#
-#fixedwing_control &
-
-#
-# Configure FMU for standalone mode
-#
-# XXX arguments?
-#
-#px4fmu start
-
-#
-# Start looking for a GPS.
-#
-# XXX this should not need to be backgrounded
-#
-#gps -d /dev/ttyS3 -m all &
-
-#
-# Start logging to microSD if we can
-#
-sh /etc/init.d/rc.logging
-
 echo "[init] startup done"
diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c
index 70b8ad6de..f15c74a22 100644
--- a/apps/ardrone_interface/ardrone_motor_control.c
+++ b/apps/ardrone_interface/ardrone_motor_control.c
@@ -45,6 +45,7 @@
 #include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_controls_effective.h>
 #include <systemlib/err.h>
 
 #include "ardrone_motor_control.h"
@@ -385,6 +386,11 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
 	const float startpoint_full_control = 0.25f;	/**< start full control at 25% thrust */
 	float yaw_factor = 1.0f;
 
+	static bool initialized = false;
+	/* publish effective outputs */
+	static struct actuator_controls_effective_s actuator_controls_effective;
+	static orb_advert_t actuator_controls_effective_pub;
+
 	if (motor_thrust <= min_thrust) {
 		motor_thrust = min_thrust;
 		output_band = 0.0f;
@@ -417,17 +423,18 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
 	      && motor_calc[3] < motor_thrust + output_band && motor_calc[3] > motor_thrust - output_band)) {
 
 		yaw_factor = 0.5f;
+		yaw_control *= yaw_factor;
 		// FRONT (MOTOR 1)
-		motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control * yaw_factor);
+		motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control);
 
 		// RIGHT (MOTOR 2)
-		motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control * yaw_factor);
+		motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control);
 
 		// BACK (MOTOR 3)
-		motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control * yaw_factor);
+		motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control);
 
 		// LEFT (MOTOR 4)
-		motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control * yaw_factor);
+		motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control);
 	}
 
 	for (int i = 0; i < 4; i++) {
@@ -441,6 +448,23 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
 		}
 	}
 
+	/* publish effective outputs */
+	actuator_controls_effective.control_effective[0] = roll_control;
+	actuator_controls_effective.control_effective[1] = pitch_control;
+	/* yaw output after limiting */
+	actuator_controls_effective.control_effective[2] = yaw_control;
+	/* possible motor thrust limiting */
+	actuator_controls_effective.control_effective[3] = (motor_calc[0] + motor_calc[1] + motor_calc[2] + motor_calc[3]) / 4.0f;
+
+	if (!initialized) {
+		/* advertise and publish */
+		actuator_controls_effective_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, &actuator_controls_effective);
+		initialized = true;
+	} else {
+		/* already initialized, just publishing */
+		orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, actuator_controls_effective_pub, &actuator_controls_effective);
+	}
+
 	/* set the motor values */
 
 	/* scale up from 0..1 to 10..512) */
diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 2b59f709a..7277e9fa4 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -63,7 +63,6 @@
 #include <string.h>
 #include <drivers/drv_led.h>
 #include <drivers/drv_hrt.h>
-#include <drivers/drv_hrt.h>
 #include <drivers/drv_tone_alarm.h>
 #include "state_machine_helper.h"
 #include "systemlib/systemlib.h"
@@ -269,6 +268,7 @@ void tune_confirm(void) {
 
 void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 {
+
 	/* set to mag calibration mode */
 	status->flag_preflight_mag_calibration = true;
 	state_machine_publish(status_pub, status, mavlink_fd);
@@ -325,7 +325,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 	uint64_t axis_deadline = hrt_absolute_time();
 	uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
 
-	const char axislabels[3] = { 'X', 'Z', 'Y'};
+	const char axislabels[3] = { 'X', 'Y', 'Z'};
 	int axis_index = -1;
 
 	float *x = (float*)malloc(sizeof(float) * calibration_maxcount);
@@ -471,6 +471,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 		int save_ret = param_save_default();
 		if(save_ret != 0) {
 			warn("WARNING: auto-save of params to storage failed");
+			mavlink_log_info(mavlink_fd, "[cmd] FAILED storing calibration");
 		}
 
 		printf("[mag cal]\tscale: %.6f %.6f %.6f\n         \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n",
@@ -1132,7 +1133,7 @@ int commander_main(int argc, char *argv[])
 		daemon_task = task_spawn("commander",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 50,
-					 4096,
+					 4000,
 					 commander_thread_main,
 					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		thread_running = true;
@@ -1392,7 +1393,7 @@ int commander_thread_main(int argc, char *argv[])
 		else if (battery_voltage_valid && (bat_remain < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) {
 			if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
 				critical_battery_voltage_actions_done = true;
-				mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY! CIRITICAL BATTERY!");
+				mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY! CRITICAL BATTERY!");
 				state_machine_emergency(stat_pub, &current_status, mavlink_fd);
 			}
 
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c
index 657c9af9a..46793c89b 100644
--- a/apps/commander/state_machine_helper.c
+++ b/apps/commander/state_machine_helper.c
@@ -238,11 +238,8 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
 void publish_armed_status(const struct vehicle_status_s *current_status) {
 	struct actuator_armed_s armed;
 	armed.armed = current_status->flag_system_armed;
-	/* lock down actuators if required */
-	// XXX FIXME Currently any loss of RC will completely disable all actuators
-	// needs proper failsafe
-	armed.lockdown = ((current_status->rc_signal_lost && current_status->offboard_control_signal_lost)
-	 || current_status->flag_hil_enabled) ? true : false;
+	/* lock down actuators if required, only in HIL */
+	armed.lockdown = (current_status->flag_hil_enabled) ? true : false;
 	orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
 	orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
 }
@@ -260,7 +257,9 @@ void state_machine_emergency_always_critical(int status_pub, struct vehicle_stat
 		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR);
 
 	} else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) {
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT);
+		
+		// DO NOT abort mission
+		//do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT);
 
 	} else {
 		fprintf(stderr, "[commander] Unknown system state: #%d\n", current_status->state_machine);
diff --git a/apps/drivers/drv_accel.h b/apps/drivers/drv_accel.h
index 3834795e0..794de584b 100644
--- a/apps/drivers/drv_accel.h
+++ b/apps/drivers/drv_accel.h
@@ -115,4 +115,7 @@ ORB_DECLARE(sensor_accel);
 /** get the current accel measurement range in g */
 #define ACCELIOCGRANGE		_ACCELIOC(8)
 
+/** get the result of a sensor self-test */
+#define ACCELIOCSELFTEST	_ACCELIOC(9)
+
 #endif /* _DRV_ACCEL_H */
diff --git a/apps/drivers/drv_gyro.h b/apps/drivers/drv_gyro.h
index 0dbf05c5b..1d0fef6fc 100644
--- a/apps/drivers/drv_gyro.h
+++ b/apps/drivers/drv_gyro.h
@@ -111,4 +111,7 @@ ORB_DECLARE(sensor_gyro);
 /** get the current gyro measurement range in degrees per second */
 #define GYROIOCGRANGE		_GYROIOC(7)
 
+/** check the status of the sensor */
+#define GYROIOCSELFTEST		_GYROIOC(8)
+
 #endif /* _DRV_GYRO_H */
diff --git a/apps/drivers/drv_mag.h b/apps/drivers/drv_mag.h
index 114bcb646..9aab995a1 100644
--- a/apps/drivers/drv_mag.h
+++ b/apps/drivers/drv_mag.h
@@ -108,4 +108,7 @@ ORB_DECLARE(sensor_mag);
 /** excite strap */
 #define MAGIOCEXSTRAP		_MAGIOC(6)
 
+/** perform self test and report status */
+#define MAGIOCSELFTEST		_MAGIOC(7)
+
 #endif /* _DRV_MAG_H */
diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp
index a1587b783..3849a2e05 100644
--- a/apps/drivers/hmc5883/hmc5883.cpp
+++ b/apps/drivers/hmc5883/hmc5883.cpp
@@ -634,7 +634,9 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case MAGIOCSSCALE:
 		/* set new scale factors */
 		memcpy(&_scale, (mag_scale *)arg, sizeof(_scale));
-		return check_calibration();
+		/* check calibration, but not actually return an error */
+		(void)check_calibration();
+		return 0;
 
 	case MAGIOCGSCALE:
 		/* copy out scale factors */
@@ -647,6 +649,9 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case MAGIOCEXSTRAP:
 		return set_excitement(arg);
 
+	case MAGIOCSELFTEST:
+		return check_calibration();
+
 	default:
 		/* give it to the superclass */
 		return I2C::ioctl(filp, cmd, arg);
@@ -1029,27 +1034,27 @@ int HMC5883::check_calibration()
 {
 	bool scale_valid, offset_valid;
 
-	if ((-2.0f * FLT_EPSILON + 1.0f < _scale.x_scale && _scale.x_scale < 2.0f * FLT_EPSILON + 1.0f) &&
-		(-2.0f * FLT_EPSILON + 1.0f < _scale.y_scale && _scale.y_scale < 2.0f * FLT_EPSILON + 1.0f) &&
-		(-2.0f * FLT_EPSILON + 1.0f < _scale.z_scale && _scale.z_scale < 2.0f * FLT_EPSILON + 1.0f)) {
-		/* scale is different from one */
-		scale_valid = true;
-	} else {
+	if ((-FLT_EPSILON + 1.0f < _scale.x_scale && _scale.x_scale < FLT_EPSILON + 1.0f) &&
+		(-FLT_EPSILON + 1.0f < _scale.y_scale && _scale.y_scale < FLT_EPSILON + 1.0f) &&
+		(-FLT_EPSILON + 1.0f < _scale.z_scale && _scale.z_scale < FLT_EPSILON + 1.0f)) {
+		/* scale is one */
 		scale_valid = false;
+	} else {
+		scale_valid = true;
 	}
 
 	if ((-2.0f * FLT_EPSILON < _scale.x_offset && _scale.x_offset < 2.0f * FLT_EPSILON) &&
 		(-2.0f * FLT_EPSILON < _scale.y_offset && _scale.y_offset < 2.0f * FLT_EPSILON) &&
 		(-2.0f * FLT_EPSILON < _scale.z_offset && _scale.z_offset < 2.0f * FLT_EPSILON)) {
-		/* offset is different from zero */
-		offset_valid = true;
-	} else {
+		/* offset is zero */
 		offset_valid = false;
+	} else {
+		offset_valid = true;
 	}
 
 	if (_calibrated != (offset_valid && scale_valid)) {
-		warnx("warning: mag cal changed: %s%s", (scale_valid) ? "" : "scale invalid. ",
-					  (offset_valid) ? "" : "offset invalid.");
+		warnx("mag cal status changed %s%s", (scale_valid) ? "" : "scale invalid ",
+					  (offset_valid) ? "" : "offset invalid");
 		_calibrated = (offset_valid && scale_valid);
 		/* notify about state change */
 		struct subsystem_info_s info = {
@@ -1059,7 +1064,9 @@ int HMC5883::check_calibration()
 			SUBSYSTEM_TYPE_MAG};
 		orb_advert_t pub = orb_advertise(ORB_ID(subsystem_info), &info);
 	}
-	return 0;
+
+	/* return 0 if calibrated, 1 else */
+	return (!_calibrated);
 }
 
 int HMC5883::set_excitement(unsigned enable)
diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp
index 90786886a..ed79440cc 100644
--- a/apps/drivers/mpu6000/mpu6000.cpp
+++ b/apps/drivers/mpu6000/mpu6000.cpp
@@ -260,6 +260,13 @@ private:
 	 */
 	uint16_t		swap16(uint16_t val) { return (val >> 8) | (val << 8);	}
 
+	/**
+	 * Self test
+	 *
+	 * @return 0 on success, 1 on failure
+	 */
+	 int 			self_test();
+
 };
 
 /**
@@ -494,6 +501,17 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen)
 	return ret;
 }
 
+int
+MPU6000::self_test()
+{
+	if (_reads == 0) {
+		measure();
+	}
+
+	/* return 0 on success, 1 else */
+	return (_reads > 0) ? 0 : 1;
+}
+
 ssize_t
 MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen)
 {
@@ -592,9 +610,17 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return -EINVAL;
 
 	case ACCELIOCSSCALE:
-		/* copy scale in */
-		memcpy(&_accel_scale, (struct accel_scale *) arg, sizeof(_accel_scale));
-		return OK;
+		{
+			/* copy scale, but only if off by a few percent */
+			struct accel_scale *s = (struct accel_scale *) arg;
+			float sum = s->x_scale + s->y_scale + s->z_scale;
+			if (sum > 2.0f && sum < 4.0f) {
+				memcpy(&_accel_scale, s, sizeof(_accel_scale));
+				return OK;
+			} else {
+				return -EINVAL;
+			}
+		}
 
 	case ACCELIOCGSCALE:
 		/* copy scale out */
@@ -609,6 +635,9 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 		// _accel_range_rad_s = 8.0f * 9.81f;
 		return -EINVAL;
 
+	case ACCELIOCSELFTEST:
+		return self_test();
+
 	default:
 		/* give it to the superclass */
 		return SPI::ioctl(filp, cmd, arg);
@@ -656,6 +685,9 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 		// _gyro_range_m_s2 = xx
 		return -EINVAL;
 
+	case GYROIOCSELFTEST:
+		return self_test();
+
 	default:
 		/* give it to the superclass */
 		return SPI::ioctl(filp, cmd, arg);
@@ -813,7 +845,11 @@ MPU6000::measure()
 	 * Fetch the full set of measurements from the MPU6000 in one pass.
 	 */
 	mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS;
-	transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report));
+	if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report)))
+		return;
+
+	/* count measurement */
+	_reads++;
 
 	/*
 	 * Convert from big to little endian
diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index 0e84760d5..f2c87473c 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -37,7 +37,6 @@
  *
  * PX4IO is connected via serial (or possibly some other interface at a later
  * point).
-
  */
 
 #include <nuttx/config.h>
@@ -384,7 +383,7 @@ PX4IO::task_main()
 
 			if (fds[2].revents & POLLIN) {
 
-				orb_copy(ORB_ID(actuator_armed), _t_armed, &_controls);
+				orb_copy(ORB_ID(actuator_armed), _t_armed, &_armed);
 				_send_needed = true;
 			}
 		}
@@ -539,7 +538,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 
 	case PWM_SERVO_DISARM:
 		/* fake a disarmed transition */
-		_armed.armed = true;
+		_armed.armed = false;
 		_send_needed = true;
 		break;
 
diff --git a/apps/fixedwing_att_control/fixedwing_att_control_att.c b/apps/fixedwing_att_control/fixedwing_att_control_att.c
index 24142dece..98442ed55 100644
--- a/apps/fixedwing_att_control/fixedwing_att_control_att.c
+++ b/apps/fixedwing_att_control/fixedwing_att_control_att.c
@@ -144,10 +144,10 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att
 	}
 
 	/* Roll (P) */
-	rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_tait_bryan, att->roll, 0, 0);
+	rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0);
 
 	/* Pitch (P) */
-	float pitch_sp_rollcompensation = att_sp->pitch_tait_bryan + p.pitch_roll_compensation_p * att_sp->roll_tait_bryan;
+	float pitch_sp_rollcompensation = att_sp->pitch_body + p.pitch_roll_compensation_p * att_sp->roll_body;
 	rates_sp->pitch = pid_calculate(&pitch_controller, pitch_sp_rollcompensation, att->pitch, 0, 0);
 
 	/* Yaw (from coordinated turn constraint or lateral force) */
diff --git a/apps/fixedwing_pos_control/fixedwing_pos_control.h b/apps/fixedwing_pos_control/fixedwing_pos_control.h
index f631c90a1..90d717d9f 100644
--- a/apps/fixedwing_pos_control/fixedwing_pos_control.h
+++ b/apps/fixedwing_pos_control/fixedwing_pos_control.h
@@ -42,26 +42,7 @@
 #ifndef FIXEDWING_POS_CONTROL_H_
 #define FIXEDWING_POS_CONTROL_H_
 
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/actuator_controls.h>
 
-#endif 
-
-
-struct planned_path_segments_s {
-	bool segment_type;
-	double start_lat;			// Start of line or center of arc
-	double start_lon;
-	double end_lat;
-	double end_lon;
-	float radius;				// Radius of arc
-	float arc_start_bearing;		// Bearing from center to start of arc
-	float arc_sweep;				// Angle (radians) swept out by arc around center.  
-								// Positive for clockwise, negative for counter-clockwise
-};
 
 
 float _wrap180(float bearing);
diff --git a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c
index 8fc272eb6..640edba99 100644
--- a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c
+++ b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c
@@ -95,6 +95,19 @@ struct fw_pos_control_param_handles {
 };
 
 
+struct planned_path_segments_s {
+	bool segment_type;
+	double start_lat;			// Start of line or center of arc
+	double start_lon;
+	double end_lat;
+	double end_lon;
+	float radius;				// Radius of arc
+	float arc_start_bearing;		// Bearing from center to start of arc
+	float arc_sweep;				// Angle (radians) swept out by arc around center.  
+								// Positive for clockwise, negative for counter-clockwise
+};
+
+
 /* Prototypes */
 /* Internal Prototypes */
 static int parameters_init(struct fw_pos_control_param_handles *h);
@@ -185,9 +198,9 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
 		memset(&attitude_setpoint, 0, sizeof(attitude_setpoint));
 
 		/* publish attitude setpoint */
-		attitude_setpoint.roll_tait_bryan = 0.0f;
-		attitude_setpoint.pitch_tait_bryan = 0.0f;
-		attitude_setpoint.yaw_tait_bryan = 0.0f;
+		attitude_setpoint.roll_body = 0.0f;
+		attitude_setpoint.pitch_body = 0.0f;
+		attitude_setpoint.yaw_body = 0.0f;
 		orb_advert_t attitude_setpoint_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &attitude_setpoint);
 
 		/* subscribe */
@@ -261,7 +274,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
 				global_sp_updated_set_once = true;
 				psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
 						(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
-				printf("psi_track: %0.4f\n", psi_track);
+				printf("psi_track: %0.4f\n", (double)psi_track);
 			}
 
 			/* Control */
@@ -287,7 +300,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
 					float psi_e = psi_c - att.yaw;
 
 					/* shift error to prevent wrapping issues */
-					psi_e = _wrapPI(psi_e);
+					psi_e = _wrap_pi(psi_e);
 
 					/* calculate roll setpoint, do this artificially around zero */
 					//TODO: psi rate loop incomplete
@@ -301,8 +314,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
 
 					float psi_rate_e_scaled = psi_rate_e * sqrtf(pow(global_pos.vx,2) + pow(global_pos.vy,2)) / 9.81f; //* V_gr / g
 
-					attitude_setpoint.roll_tait_bryan = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT);
-
+					attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT);
 
 //					if (counter % 100 == 0)
 //						printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n",xtrack_err.distance, delta_psi_c);
@@ -319,7 +331,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
 			{
 
 				//TODO: take care of relative vs. ab. altitude
-				attitude_setpoint.pitch_tait_bryan = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f);
+				attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f);
 
 			}
 			/*Publish the attitude setpoint */
@@ -401,64 +413,3 @@ int fixedwing_pos_control_main(int argc, char *argv[])
 	usage("unrecognized command");
 	exit(1);
 }
-
-
-float _wrapPI(float bearing)
-{
-
-	while (bearing > M_PI_F) {
-		bearing = bearing - M_TWOPI_F;
-	}
-
-	while (bearing <=  -M_PI_F) {
-		bearing = bearing + M_TWOPI_F;
-	}
-
-	return bearing;
-}
-
-float _wrap2PI(float bearing)
-{
-
-	while (bearing >= M_TWOPI_F) {
-		bearing = bearing - M_TWOPI_F;
-	}
-
-	while (bearing <  0.0f) {
-		bearing = bearing + M_TWOPI_F;
-	}
-
-	return bearing;
-}
-
-float _wrap180(float bearing)
-{
-
-	while (bearing > 180.0f) {
-		bearing = bearing - 360.0f;
-	}
-
-	while (bearing <=  -180.0f) {
-		bearing = bearing + 360.0f;
-	}
-
-	return bearing;
-}
-
-float _wrap360(float bearing)
-{
-
-	while (bearing >= 360.0f) {
-		bearing = bearing - 360.0f;
-	}
-
-	while (bearing <  0.0f) {
-		bearing = bearing + 360.0f;
-	}
-
-	return bearing;
-}
-
-
-
-
diff --git a/apps/gps/ubx.c b/apps/gps/ubx.c
index 2bbecb12e..e15ed4e00 100644
--- a/apps/gps/ubx.c
+++ b/apps/gps/ubx.c
@@ -49,7 +49,7 @@
 #include <mavlink/mavlink_log.h>
 
 #define UBX_HEALTH_SUCCESS_COUNTER_LIMIT 2
-#define UBX_HEALTH_FAIL_COUNTER_LIMIT 2
+#define UBX_HEALTH_FAIL_COUNTER_LIMIT 3
 #define UBX_HEALTH_PROBE_COUNTER_LIMIT 4
 
 #define UBX_BUFFER_SIZE 1000
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index 991bbfbab..b393620e2 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -547,6 +547,9 @@ int mavlink_thread_main(int argc, char *argv[])
 	/* print welcome text */
 	warnx("MAVLink v1.0 serial interface starting...");
 
+	/* inform about mode */
+	warnx((mavlink_link_mode == MAVLINK_INTERFACE_MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE");
+
 	/* Flush stdout in case MAVLink is about to take it over */
 	fflush(stdout);
 
diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c
index 45fe1ccb5..e2f28dabd 100644
--- a/apps/mavlink/mavlink_receiver.c
+++ b/apps/mavlink/mavlink_receiver.c
@@ -206,6 +206,10 @@ handle_message(mavlink_message_t *msg)
 		vicon_position.y = pos.y;
 		vicon_position.z = pos.z;
 
+		vicon_position.roll = pos.roll;
+		vicon_position.pitch = pos.pitch;
+		vicon_position.yaw = pos.yaw;
+
 		if (vicon_position_pub <= 0) {
 			vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
 		} else {
@@ -356,6 +360,11 @@ handle_message(mavlink_message_t *msg)
 			struct manual_control_setpoint_s mc;
 			static orb_advert_t mc_pub = 0;
 
+			int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+
+			/* get a copy first, to prevent altering values that are not sent by the mavlink command */
+			orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &mc);
+
 			mc.timestamp = rc_hil.timestamp;
 			mc.roll = man.x / 1000.0f;
 			mc.pitch = man.y / 1000.0f;
diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c
index 3ac229d73..4567a08f8 100644
--- a/apps/mavlink/orb_listener.c
+++ b/apps/mavlink/orb_listener.c
@@ -113,6 +113,7 @@ static void	l_actuator_armed(struct listener *l);
 static void	l_manual_control_setpoint(struct listener *l);
 static void	l_vehicle_attitude_controls(struct listener *l);
 static void	l_debug_key_value(struct listener *l);
+static void	l_optical_flow(struct listener *l);
 
 struct listener listeners[] = {
 	{l_sensor_combined,		&mavlink_subs.sensor_sub,	0},
@@ -134,6 +135,7 @@ struct listener listeners[] = {
 	{l_manual_control_setpoint,	&mavlink_subs.man_control_sp_sub, 0},
 	{l_vehicle_attitude_controls,	&mavlink_subs.actuators_sub,	0},
 	{l_debug_key_value,		&mavlink_subs.debug_key_value,	0},
+	{l_optical_flow,		&mavlink_subs.optical_flow,	0},
 };
 
 static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]);
@@ -586,6 +588,17 @@ l_debug_key_value(struct listener *l)
 					   debug.value);
 }
 
+void
+l_optical_flow(struct listener *l)
+{
+	struct optical_flow_s flow;
+
+	orb_copy(ORB_ID(optical_flow), mavlink_subs.optical_flow, &flow);
+
+	mavlink_msg_optical_flow_send(MAVLINK_COMM_0, flow.timestamp, flow.sensor_id, flow.flow_raw_x, flow.flow_raw_y,
+		flow.flow_comp_x_m, flow.flow_comp_y_m, flow.quality, flow.ground_distance_m);
+}
+
 static void *
 uorb_receive_thread(void *arg)
 {
@@ -710,6 +723,10 @@ uorb_receive_start(void)
 	mavlink_subs.debug_key_value = orb_subscribe(ORB_ID(debug_key_value));
 	orb_set_interval(mavlink_subs.debug_key_value, 100);	/* 10Hz updates */
 
+	/* --- FLOW SENSOR --- */
+	mavlink_subs.optical_flow = orb_subscribe(ORB_ID(optical_flow));
+	orb_set_interval(mavlink_subs.optical_flow, 200); 	/* 5Hz updates */
+
 	/* start the listener loop */
 	pthread_attr_t uorb_attr;
 	pthread_attr_init(&uorb_attr);
diff --git a/apps/mavlink/orb_topics.h b/apps/mavlink/orb_topics.h
index 6860702d2..1b371e5cd 100644
--- a/apps/mavlink/orb_topics.h
+++ b/apps/mavlink/orb_topics.h
@@ -77,6 +77,7 @@ struct mavlink_subscriptions {
 	int spg_sub;
 	int debug_key_value;
 	int input_rc_sub;
+	int optical_flow;
 };
 
 extern struct mavlink_subscriptions mavlink_subs;
diff --git a/apps/mavlink_onboard/Makefile b/apps/mavlink_onboard/Makefile
new file mode 100644
index 000000000..8b1cb9b2c
--- /dev/null
+++ b/apps/mavlink_onboard/Makefile
@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (C) 2012 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.
+#
+############################################################################
+
+#
+# Mavlink Application
+#
+
+APPNAME		 = mavlink_onboard
+PRIORITY	 = SCHED_PRIORITY_DEFAULT
+STACKSIZE	 = 2048
+
+INCLUDES	 = $(TOPDIR)/../mavlink/include/mavlink
+
+include $(APPDIR)/mk/app.mk
diff --git a/apps/mavlink_onboard/mavlink.c b/apps/mavlink_onboard/mavlink.c
new file mode 100644
index 000000000..33ebe8600
--- /dev/null
+++ b/apps/mavlink_onboard/mavlink.c
@@ -0,0 +1,529 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * 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.c
+ * MAVLink 1.0 protocol implementation.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <pthread.h>
+#include <stdio.h>
+#include <math.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <mqueue.h>
+#include <string.h>
+#include "mavlink_bridge_header.h"
+#include <v1.0/common/mavlink.h>
+#include <drivers/drv_hrt.h>
+#include <time.h>
+#include <float.h>
+#include <unistd.h>
+#include <nuttx/sched.h>
+#include <sys/prctl.h>
+#include <termios.h>
+#include <errno.h>
+#include <stdlib.h>
+#include <poll.h>
+
+#include <systemlib/param/param.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/err.h>
+
+#include "orb_topics.h"
+#include "util.h"
+
+__EXPORT int mavlink_onboard_main(int argc, char *argv[]);
+
+static int mavlink_thread_main(int argc, char *argv[]);
+
+/* thread state */
+volatile bool thread_should_exit = false;
+static volatile bool thread_running = false;
+static int mavlink_task;
+
+/* pthreads */
+static pthread_t receive_thread;
+
+/* terminate MAVLink on user request - disabled by default */
+static bool mavlink_link_termination_allowed = false;
+
+mavlink_system_t mavlink_system = {
+	100,
+	50,
+	MAV_TYPE_QUADROTOR,
+	0,
+	0,
+	0
+}; // System ID, 1-255, Component/Subsystem ID, 1-255
+
+/* XXX not widely used */
+uint8_t chan = MAVLINK_COMM_0;
+
+/* XXX probably should be in a header... */
+extern pthread_t receive_start(int uart);
+
+bool mavlink_hil_enabled = false;
+
+/* protocol interface */
+static int uart;
+static int baudrate;
+bool gcs_link = true;
+
+/* interface mode */
+static enum {
+	MAVLINK_INTERFACE_MODE_OFFBOARD,
+	MAVLINK_INTERFACE_MODE_ONBOARD
+} mavlink_link_mode = MAVLINK_INTERFACE_MODE_OFFBOARD;
+
+static void mavlink_update_system(void);
+static int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
+static void usage(void);
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
+{
+	/* process baud rate */
+	int speed;
+
+	switch (baud) {
+	case 0:      speed = B0;      break;
+
+	case 50:     speed = B50;     break;
+
+	case 75:     speed = B75;     break;
+
+	case 110:    speed = B110;    break;
+
+	case 134:    speed = B134;    break;
+
+	case 150:    speed = B150;    break;
+
+	case 200:    speed = B200;    break;
+
+	case 300:    speed = B300;    break;
+
+	case 600:    speed = B600;    break;
+
+	case 1200:   speed = B1200;   break;
+
+	case 1800:   speed = B1800;   break;
+
+	case 2400:   speed = B2400;   break;
+
+	case 4800:   speed = B4800;   break;
+
+	case 9600:   speed = B9600;   break;
+
+	case 19200:  speed = B19200;  break;
+
+	case 38400:  speed = B38400;  break;
+
+	case 57600:  speed = B57600;  break;
+
+	case 115200: speed = B115200; break;
+
+	case 230400: speed = B230400; break;
+
+	case 460800: speed = B460800; break;
+
+	case 921600: speed = B921600; break;
+
+	default:
+		fprintf(stderr, "[mavlink] ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud);
+		return -EINVAL;
+	}
+
+	/* open uart */
+	printf("[mavlink] UART is %s, baudrate is %d\n", uart_name, baud);
+	uart = open(uart_name, O_RDWR | O_NOCTTY);
+
+	/* Try to set baud rate */
+	struct termios uart_config;
+	int termios_state;
+	*is_usb = false;
+
+	if (strcmp(uart_name, "/dev/ttyACM0") != OK) {
+		/* Back up the original uart configuration to restore it after exit */
+		if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
+			fprintf(stderr, "[mavlink] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state);
+			close(uart);
+			return -1;
+		}
+
+		/* Fill the struct for the new configuration */
+		tcgetattr(uart, &uart_config);
+
+		/* Clear ONLCR flag (which appends a CR for every LF) */
+		uart_config.c_oflag &= ~ONLCR;
+
+		/* Set baud rate */
+		if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
+			fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
+			close(uart);
+			return -1;
+		}
+
+
+		if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
+			fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
+			close(uart);
+			return -1;
+		}
+
+	} else {
+		*is_usb = true;
+	}
+
+	return uart;
+}
+
+void 
+mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length)
+{
+	write(uart, ch, (size_t)(sizeof(uint8_t) * length));
+}
+
+/*
+ * Internal function to give access to the channel status for each channel
+ */
+mavlink_status_t* mavlink_get_channel_status(uint8_t channel)
+{
+	static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
+	return &m_mavlink_status[channel];
+}
+
+/*
+ * Internal function to give access to the channel buffer for each channel
+ */
+mavlink_message_t* mavlink_get_channel_buffer(uint8_t channel)
+{
+	static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
+	return &m_mavlink_buffer[channel];
+}
+
+void mavlink_update_system(void)
+{
+	static bool initialized = false;
+	param_t param_system_id;
+	param_t param_component_id;
+	param_t param_system_type;
+
+	if (!initialized) {
+		param_system_id = param_find("MAV_SYS_ID");
+		param_component_id = param_find("MAV_COMP_ID");
+		param_system_type = param_find("MAV_TYPE");
+	}
+
+	/* update system and component id */
+	int32_t system_id;
+	param_get(param_system_id, &system_id);
+	if (system_id > 0 && system_id < 255) {
+		mavlink_system.sysid = system_id;
+	}
+
+	int32_t component_id;
+	param_get(param_component_id, &component_id);
+	if (component_id > 0 && component_id < 255) {
+		mavlink_system.compid = component_id;
+	}
+	
+	int32_t system_type;
+	param_get(param_system_type, &system_type);
+	if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
+		mavlink_system.type = system_type;
+	}
+}
+
+void
+get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed,
+	uint8_t *mavlink_state, uint8_t *mavlink_mode)
+{
+	/* reset MAVLink mode bitfield */
+	*mavlink_mode = 0;
+
+	/* set mode flags independent of system state */
+	if (v_status->flag_control_manual_enabled) {
+		*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+	}
+
+	if (v_status->flag_hil_enabled) {
+		*mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED;
+	}
+
+	/* set arming state */
+	if (armed->armed) {
+		*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
+	} else {
+		*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
+	}
+
+	switch (v_status->state_machine) {
+	case SYSTEM_STATE_PREFLIGHT:
+		if (v_status->flag_preflight_gyro_calibration ||
+		    v_status->flag_preflight_mag_calibration ||
+		    v_status->flag_preflight_accel_calibration) {
+			*mavlink_state = MAV_STATE_CALIBRATING;
+		} else {
+			*mavlink_state = MAV_STATE_UNINIT;
+		}
+		break;
+
+	case SYSTEM_STATE_STANDBY:
+		*mavlink_state = MAV_STATE_STANDBY;
+		break;
+
+	case SYSTEM_STATE_GROUND_READY:
+		*mavlink_state = MAV_STATE_ACTIVE;
+		break;
+
+	case SYSTEM_STATE_MANUAL:
+		*mavlink_state = MAV_STATE_ACTIVE;
+		*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+		break;
+
+	case SYSTEM_STATE_STABILIZED:
+		*mavlink_state = MAV_STATE_ACTIVE;
+		*mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
+		break;
+
+	case SYSTEM_STATE_AUTO:
+		*mavlink_state = MAV_STATE_ACTIVE;
+		*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
+		break;
+
+	case SYSTEM_STATE_MISSION_ABORT:
+		*mavlink_state = MAV_STATE_EMERGENCY;
+		break;
+
+	case SYSTEM_STATE_EMCY_LANDING:
+		*mavlink_state = MAV_STATE_EMERGENCY;
+		break;
+
+	case SYSTEM_STATE_EMCY_CUTOFF:
+		*mavlink_state = MAV_STATE_EMERGENCY;
+		break;
+
+	case SYSTEM_STATE_GROUND_ERROR:
+		*mavlink_state = MAV_STATE_EMERGENCY;
+		break;
+
+	case SYSTEM_STATE_REBOOT:
+		*mavlink_state = MAV_STATE_POWEROFF;
+		break;
+	}
+
+}
+
+/**
+ * MAVLink Protocol main function.
+ */
+int mavlink_thread_main(int argc, char *argv[])
+{
+	int ch;
+	char *device_name = "/dev/ttyS1";
+	baudrate = 57600;
+
+	struct vehicle_status_s v_status;
+	struct actuator_armed_s armed;
+
+	/* work around some stupidity in task_create's argv handling */
+	argc -= 2;
+	argv += 2;
+
+	while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) {
+		switch (ch) {
+		case 'b':
+			baudrate = strtoul(optarg, NULL, 10);
+			if (baudrate == 0)
+				errx(1, "invalid baud rate '%s'", optarg);
+			break;
+
+		case 'd':
+			device_name = optarg;
+			break;
+
+		case 'e':
+			mavlink_link_termination_allowed = true;
+			break;
+
+		case 'o':
+			mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD;
+			break;
+
+		default:
+			usage();
+		}
+	}
+
+	struct termios uart_config_original;
+	bool usb_uart;
+
+	/* print welcome text */
+	warnx("MAVLink v1.0 serial interface starting...");
+
+	/* inform about mode */
+	warnx((mavlink_link_mode == MAVLINK_INTERFACE_MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE");
+
+	/* Flush stdout in case MAVLink is about to take it over */
+	fflush(stdout);
+
+	/* default values for arguments */
+	uart = mavlink_open_uart(baudrate, device_name, &uart_config_original, &usb_uart);
+	if (uart < 0)
+		err(1, "could not open %s", device_name);
+
+	/* Initialize system properties */
+	mavlink_update_system();
+
+	/* start the MAVLink receiver */
+	receive_thread = receive_start(uart);
+
+	thread_running = true;
+
+	/* arm counter to go off immediately */
+	unsigned lowspeed_counter = 10;
+
+	while (!thread_should_exit) {
+
+		/* 1 Hz */
+		if (lowspeed_counter == 10) {
+			mavlink_update_system();
+
+			/* translate the current system state to mavlink state and mode */
+			uint8_t mavlink_state = 0;
+			uint8_t mavlink_mode = 0;
+			get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode);
+
+			/* send heartbeat */
+			mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state);
+
+			/* send status (values already copied in the section above) */
+			mavlink_msg_sys_status_send(chan,
+						    v_status.onboard_control_sensors_present,
+						    v_status.onboard_control_sensors_enabled,
+						    v_status.onboard_control_sensors_health,
+						    v_status.load,
+						    v_status.voltage_battery * 1000.0f,
+						    v_status.current_battery * 1000.0f,
+						    v_status.battery_remaining,
+						    v_status.drop_rate_comm,
+						    v_status.errors_comm,
+						    v_status.errors_count1,
+						    v_status.errors_count2,
+						    v_status.errors_count3,
+						    v_status.errors_count4);
+			lowspeed_counter = 0;
+		}
+		lowspeed_counter++;
+
+		/* sleep 1000 ms */
+		usleep(1000000);
+	}
+
+	/* wait for threads to complete */
+	pthread_join(receive_thread, NULL);
+
+	/* Reset the UART flags to original state */
+	if (!usb_uart)
+		tcsetattr(uart, TCSANOW, &uart_config_original);
+
+	thread_running = false;
+
+	exit(0);
+}
+
+static void
+usage()
+{
+	fprintf(stderr, "usage: mavlink start [-d <devicename>] [-b <baud rate>]\n"
+			"       mavlink stop\n"
+			"       mavlink status\n");
+	exit(1);
+}
+
+int mavlink_onboard_main(int argc, char *argv[])
+{
+
+	if (argc < 2) {
+		warnx("missing command");
+		usage();
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		/* this is not an error */
+		if (thread_running)
+			errx(0, "mavlink already running\n");
+
+		thread_should_exit = false;
+		mavlink_task = task_spawn("mavlink_onboard",
+					  SCHED_DEFAULT,
+					  SCHED_PRIORITY_DEFAULT,
+					  6000 /* XXX probably excessive */,
+					  mavlink_thread_main,
+					  (const char**)argv);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+		while (thread_running) {
+			usleep(200000);
+		}
+		warnx("terminated.");
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			errx(0, "running");
+		} else {
+			errx(1, "not running");
+		}
+	}
+
+	warnx("unrecognized command");
+	usage();
+	/* not getting here */
+	return 0;
+}
+
diff --git a/apps/mavlink_onboard/mavlink_bridge_header.h b/apps/mavlink_onboard/mavlink_bridge_header.h
new file mode 100644
index 000000000..bf7c5354c
--- /dev/null
+++ b/apps/mavlink_onboard/mavlink_bridge_header.h
@@ -0,0 +1,81 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * 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_bridge_header
+ * MAVLink bridge header for UART access.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+/* MAVLink adapter header */
+#ifndef MAVLINK_BRIDGE_HEADER_H
+#define MAVLINK_BRIDGE_HEADER_H
+
+#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
+
+/* use efficient approach, see mavlink_helpers.h */
+#define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes
+
+#define MAVLINK_GET_CHANNEL_BUFFER mavlink_get_channel_buffer
+#define MAVLINK_GET_CHANNEL_STATUS mavlink_get_channel_status
+
+#include "v1.0/mavlink_types.h"
+#include <unistd.h>
+
+
+/* Struct that stores the communication settings of this system.
+   you can also define / alter these settings elsewhere, as long
+   as they're included BEFORE mavlink.h.
+   So you can set the
+
+   mavlink_system.sysid = 100; // System ID, 1-255
+   mavlink_system.compid = 50; // Component/Subsystem ID, 1-255
+
+   Lines also in your main.c, e.g. by reading these parameter from EEPROM.
+ */
+extern mavlink_system_t mavlink_system;
+
+/**
+ * @brief Send multiple chars (uint8_t) over a comm channel
+ *
+ * @param chan MAVLink channel to use, usually MAVLINK_COMM_0 = UART0
+ * @param ch Character to send
+ */
+extern void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t *ch, int length);
+
+mavlink_status_t* mavlink_get_channel_status(uint8_t chan);
+mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan);
+
+#endif /* MAVLINK_BRIDGE_HEADER_H */
diff --git a/apps/mavlink_onboard/mavlink_receiver.c b/apps/mavlink_onboard/mavlink_receiver.c
new file mode 100644
index 000000000..87e2496d8
--- /dev/null
+++ b/apps/mavlink_onboard/mavlink_receiver.c
@@ -0,0 +1,332 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * 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_receiver.c
+ * MAVLink protocol message receive and dispatch
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+/* XXX trim includes */
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <pthread.h>
+#include <stdio.h>
+#include <math.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <mqueue.h>
+#include <string.h>
+#include "mavlink_bridge_header.h"
+#include <v1.0/common/mavlink.h>
+#include <drivers/drv_hrt.h>
+#include <time.h>
+#include <float.h>
+#include <unistd.h>
+#include <nuttx/sched.h>
+#include <sys/prctl.h>
+#include <termios.h>
+#include <errno.h>
+#include <stdlib.h>
+#include <poll.h>
+
+#include <systemlib/param/param.h>
+#include <systemlib/systemlib.h>
+
+#include "util.h"
+#include "orb_topics.h"
+
+/* XXX should be in a header somewhere */
+pthread_t receive_start(int uart);
+
+static void handle_message(mavlink_message_t *msg);
+static void *receive_thread(void *arg);
+
+static mavlink_status_t status;
+static struct vehicle_vicon_position_s vicon_position;
+static struct vehicle_command_s vcmd;
+static struct offboard_control_setpoint_s offboard_control_sp;
+
+struct vehicle_global_position_s hil_global_pos;
+struct vehicle_attitude_s hil_attitude;
+orb_advert_t pub_hil_global_pos = -1;
+orb_advert_t pub_hil_attitude = -1;
+
+static orb_advert_t cmd_pub = -1;
+static orb_advert_t flow_pub = -1;
+
+static orb_advert_t offboard_control_sp_pub = -1;
+static orb_advert_t vicon_position_pub = -1;
+
+extern bool gcs_link;
+
+static void
+handle_message(mavlink_message_t *msg)
+{
+	if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
+
+		mavlink_command_long_t cmd_mavlink;
+		mavlink_msg_command_long_decode(msg, &cmd_mavlink);
+
+		if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid)
+			|| (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
+			//check for MAVLINK terminate command
+			if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
+				/* This is the link shutdown command, terminate mavlink */
+				printf("[mavlink] Terminating .. \n");
+				fflush(stdout);
+				usleep(50000);
+
+				/* terminate other threads and this thread */
+				thread_should_exit = true;
+
+			} else {
+
+				/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
+				vcmd.param1 = cmd_mavlink.param1;
+				vcmd.param2 = cmd_mavlink.param2;
+				vcmd.param3 = cmd_mavlink.param3;
+				vcmd.param4 = cmd_mavlink.param4;
+				vcmd.param5 = cmd_mavlink.param5;
+				vcmd.param6 = cmd_mavlink.param6;
+				vcmd.param7 = cmd_mavlink.param7;
+				vcmd.command = cmd_mavlink.command;
+				vcmd.target_system = cmd_mavlink.target_system;
+				vcmd.target_component = cmd_mavlink.target_component;
+				vcmd.source_system = msg->sysid;
+				vcmd.source_component = msg->compid;
+				vcmd.confirmation =  cmd_mavlink.confirmation;
+
+				/* check if topic is advertised */
+				if (cmd_pub <= 0) {
+					cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
+				}
+				/* publish */
+				orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
+			}
+		}
+	}
+
+	if (msg->msgid == MAVLINK_MSG_ID_OPTICAL_FLOW) {
+		mavlink_optical_flow_t flow;
+		mavlink_msg_optical_flow_decode(msg, &flow);
+
+		struct optical_flow_s f;
+
+		f.timestamp = hrt_absolute_time();
+		f.flow_raw_x = flow.flow_x;
+		f.flow_raw_y = flow.flow_y;
+		f.flow_comp_x_m = flow.flow_comp_m_x;
+		f.flow_comp_y_m = flow.flow_comp_m_y;
+		f.ground_distance_m = flow.ground_distance;
+		f.quality = flow.quality;
+		f.sensor_id = flow.sensor_id;
+
+		/* check if topic is advertised */
+		if (flow_pub <= 0) {
+			flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
+		} else {
+			/* publish */
+			orb_publish(ORB_ID(optical_flow), flow_pub, &f);
+		}
+
+		printf("GOT FLOW!\n");
+	}
+
+	if (msg->msgid == MAVLINK_MSG_ID_SET_MODE) {
+		/* Set mode on request */
+		mavlink_set_mode_t new_mode;
+		mavlink_msg_set_mode_decode(msg, &new_mode);
+
+		/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
+		vcmd.param1 = new_mode.base_mode;
+		vcmd.param2 = new_mode.custom_mode;
+		vcmd.param3 = 0;
+		vcmd.param4 = 0;
+		vcmd.param5 = 0;
+		vcmd.param6 = 0;
+		vcmd.param7 = 0;
+		vcmd.command = MAV_CMD_DO_SET_MODE;
+		vcmd.target_system = new_mode.target_system;
+		vcmd.target_component = MAV_COMP_ID_ALL;
+		vcmd.source_system = msg->sysid;
+		vcmd.source_component = msg->compid;
+		vcmd.confirmation = 1;
+
+		/* check if topic is advertised */
+		if (cmd_pub <= 0) {
+			cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
+		} else {
+			/* create command */
+			orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
+		}
+	}
+
+	/* Handle Vicon position estimates */
+	if (msg->msgid == MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) {
+		mavlink_vicon_position_estimate_t pos;
+		mavlink_msg_vicon_position_estimate_decode(msg, &pos);
+
+		vicon_position.x = pos.x;
+		vicon_position.y = pos.y;
+		vicon_position.z = pos.z;
+
+		if (vicon_position_pub <= 0) {
+			vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
+		} else {
+			orb_publish(ORB_ID(vehicle_vicon_position), vicon_position_pub, &vicon_position);
+		}
+	}
+
+	/* Handle quadrotor motor setpoints */
+
+	if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) {
+		mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint;
+		mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint);
+
+		if (mavlink_system.sysid < 4) {
+
+			/* switch to a receiving link mode */
+			gcs_link = false;
+
+			/* 
+			 * rate control mode - defined by MAVLink
+			 */
+
+			uint8_t ml_mode = 0;
+			bool ml_armed = false;
+
+			switch (quad_motors_setpoint.mode) {
+				case 0:
+					ml_armed = false;
+					break;
+				case 1:
+					ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES;
+					ml_armed = true;
+
+					break;
+				case 2:
+					ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE;
+					ml_armed = true;
+
+					break;
+				case 3:
+					ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY;
+					break;
+				case 4:
+					ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION;
+					break;
+			}
+
+			offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid-1]   / (float)INT16_MAX;
+			offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid-1]  / (float)INT16_MAX;
+			offboard_control_sp.p3= (float)quad_motors_setpoint.yaw[mavlink_system.sysid-1]    / (float)INT16_MAX;
+			offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid-1]/(float)UINT16_MAX;
+
+			if (quad_motors_setpoint.thrust[mavlink_system.sysid-1] == 0) {
+				ml_armed = false;
+			}
+
+			offboard_control_sp.armed = ml_armed;
+			offboard_control_sp.mode = ml_mode;
+
+			offboard_control_sp.timestamp = hrt_absolute_time();
+
+			/* check if topic has to be advertised */
+			if (offboard_control_sp_pub <= 0) {
+				offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
+			} else {
+				/* Publish */
+				orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp);
+			}
+		}
+	}
+
+}
+
+
+/**
+ * Receive data from UART.
+ */
+static void *
+receive_thread(void *arg)
+{
+	int uart_fd = *((int*)arg);
+
+	const int timeout = 1000;
+	uint8_t ch;
+
+	mavlink_message_t msg;
+
+	prctl(PR_SET_NAME, "mavlink offb rcv", getpid());
+
+	while (!thread_should_exit) {
+
+		struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } };
+
+		if (poll(fds, 1, timeout) > 0) {
+			/* non-blocking read until buffer is empty */
+			int nread = 0;
+
+			do {
+				nread = read(uart_fd, &ch, 1);
+
+				if (mavlink_parse_char(chan, ch, &msg, &status)) { //parse the char
+					/* handle generic messages and commands */
+					handle_message(&msg);
+				}
+			} while (nread > 0);
+		}
+	}
+
+	return NULL;
+}
+
+pthread_t
+receive_start(int uart)
+{
+	pthread_attr_t receiveloop_attr;
+	pthread_attr_init(&receiveloop_attr);
+
+	struct sched_param param;
+  	param.sched_priority = SCHED_PRIORITY_MAX - 40;
+  	(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
+
+	pthread_attr_setstacksize(&receiveloop_attr, 2048);
+
+	pthread_t thread;
+	pthread_create(&thread, &receiveloop_attr, receive_thread, &uart);
+	return thread;
+}
\ No newline at end of file
diff --git a/apps/mavlink_onboard/orb_topics.h b/apps/mavlink_onboard/orb_topics.h
new file mode 100644
index 000000000..f18f56243
--- /dev/null
+++ b/apps/mavlink_onboard/orb_topics.h
@@ -0,0 +1,100 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * 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 orb_topics.h
+ * Common sets of topics subscribed to or published by the MAVLink driver,
+ * and structures maintained by those subscriptions.
+ */
+#pragma once
+
+#include <uORB/uORB.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/rc_channels.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/offboard_control_setpoint.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/vehicle_vicon_position.h>
+#include <uORB/topics/vehicle_global_position_setpoint.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/debug_key_value.h>
+#include <drivers/drv_rc_input.h>
+
+struct mavlink_subscriptions {
+	int sensor_sub;
+	int att_sub;
+	int global_pos_sub;
+	int act_0_sub;
+	int act_1_sub;
+	int act_2_sub;
+	int act_3_sub;
+	int gps_sub;
+	int man_control_sp_sub;
+	int armed_sub;
+	int actuators_sub;
+	int local_pos_sub;
+	int spa_sub;
+	int spl_sub;
+	int spg_sub;
+	int debug_key_value;
+	int input_rc_sub;
+};
+
+extern struct mavlink_subscriptions mavlink_subs;
+
+/** Global position */
+extern struct vehicle_global_position_s global_pos;
+
+/** Local position */
+extern struct vehicle_local_position_s local_pos;
+
+/** Vehicle status */
+// extern struct vehicle_status_s v_status;
+
+/** RC channels */
+extern struct rc_channels_s rc;
+
+/** Actuator armed state */
+// extern struct actuator_armed_s armed;
+
+/** Worker thread starter */
+extern pthread_t uorb_receive_start(void);
diff --git a/apps/mavlink_onboard/util.h b/apps/mavlink_onboard/util.h
new file mode 100644
index 000000000..38a4db372
--- /dev/null
+++ b/apps/mavlink_onboard/util.h
@@ -0,0 +1,54 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * 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 util.h
+ * Utility and helper functions and data.
+ */
+
+#pragma once
+
+#include "orb_topics.h"
+
+/** MAVLink communications channel */
+extern uint8_t chan;
+
+/** Shutdown marker */
+extern volatile bool thread_should_exit;
+
+/**
+ * Translate the custom state into standard mavlink modes and state.
+ */
+extern void get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed,
+	uint8_t *mavlink_state, uint8_t *mavlink_mode);
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c
index fd29e3660..10d155ffc 100644
--- a/apps/multirotor_att_control/multirotor_att_control_main.c
+++ b/apps/multirotor_att_control/multirotor_att_control_main.c
@@ -69,10 +69,13 @@
 
 #include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
+#include <systemlib/param/param.h>
 
 #include "multirotor_attitude_control.h"
 #include "multirotor_rate_control.h"
 
+PARAM_DEFINE_FLOAT(MC_RCLOSS_THR, 0.0f); // This defines the throttle when the RC signal is lost.
+
 __EXPORT int multirotor_att_control_main(int argc, char *argv[]);
 
 static bool thread_should_exit;
@@ -102,6 +105,7 @@ mc_thread_main(int argc, char *argv[])
 	memset(&rates_sp, 0, sizeof(rates_sp));
 
 	struct actuator_controls_s actuators;
+	memset(&actuators, 0, sizeof(actuators));
 
 	/* subscribe to attitude, motor setpoints and system state */
 	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
@@ -148,6 +152,17 @@ mc_thread_main(int argc, char *argv[])
 	bool flag_control_attitude_enabled = false;
 	bool flag_system_armed = false;
 
+	/* store if yaw position or yaw speed has been changed */
+	bool control_yaw_position = true;
+
+	/* store if we stopped a yaw movement */
+	bool first_time_after_yaw_speed_control = true;
+
+	/* prepare the handle for the failsafe throttle */
+	param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR");
+	float failsafe_throttle = 0.0f;
+
+
 	while (!thread_should_exit) {
 
 		/* wait for a sensor update, check for exit condition every 500 ms */
@@ -198,31 +213,30 @@ mc_thread_main(int argc, char *argv[])
 
 				/** STEP 1: Define which input is the dominating control input */
 				if (state.flag_control_offboard_enabled) {
-							/* offboard inputs */
-							if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
-								rates_sp.roll = offboard_sp.p1;
-								rates_sp.pitch = offboard_sp.p2;
-								rates_sp.yaw = offboard_sp.p3;
-								rates_sp.thrust = offboard_sp.p4;
-		//						printf("thrust_rate=%8.4f\n",offboard_sp.p4);
-								rates_sp.timestamp = hrt_absolute_time();
-								orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
-							} else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) {
-								att_sp.roll_body = offboard_sp.p1;
-								att_sp.pitch_body = offboard_sp.p2;
-								att_sp.yaw_body = offboard_sp.p3;
-								att_sp.thrust = offboard_sp.p4;
-		//						printf("thrust_att=%8.4f\n",offboard_sp.p4);
-								att_sp.timestamp = hrt_absolute_time();
-								/* STEP 2: publish the result to the vehicle actuators */
-								orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-							}
-
-							/* decide wether we want rate or position input */
-						}
+					/* offboard inputs */
+					if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
+						rates_sp.roll = offboard_sp.p1;
+						rates_sp.pitch = offboard_sp.p2;
+						rates_sp.yaw = offboard_sp.p3;
+						rates_sp.thrust = offboard_sp.p4;
+//						printf("thrust_rate=%8.4f\n",offboard_sp.p4);
+						rates_sp.timestamp = hrt_absolute_time();
+						orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
+					} else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) {
+						att_sp.roll_body = offboard_sp.p1;
+						att_sp.pitch_body = offboard_sp.p2;
+						att_sp.yaw_body = offboard_sp.p3;
+						att_sp.thrust = offboard_sp.p4;
+//						printf("thrust_att=%8.4f\n",offboard_sp.p4);
+						att_sp.timestamp = hrt_absolute_time();
+						/* STEP 2: publish the result to the vehicle actuators */
+						orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
+					}
+					/* decide wether we want rate or position input */
+				}
 				else if (state.flag_control_manual_enabled) {
-					/* manual inputs, from RC control or joystick */
 
+					/* manual inputs, from RC control or joystick */
 					if (state.flag_control_rates_enabled && !state.flag_control_attitude_enabled) {
 						rates_sp.roll = manual.roll;
 
@@ -241,18 +255,61 @@ mc_thread_main(int argc, char *argv[])
 							att_sp.yaw_body = att.yaw;
 						}
 
-						att_sp.roll_body = manual.roll;
-						att_sp.pitch_body = manual.pitch;
+						static bool rc_loss_first_time = true;
 
-						/* only move setpoint if manual input is != 0 */
-						// XXX turn into param
-						if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) {
-							att_sp.yaw_body = att_sp.yaw_body + manual.yaw * 0.0025f;
-						} else if (manual.throttle <= 0.3f) {
-							att_sp.yaw_body = att.yaw;
+						/* if the RC signal is lost, try to stay level and go slowly back down to ground */
+						if(state.rc_signal_lost) {
+							/* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */
+							param_get(failsafe_throttle_handle, &failsafe_throttle);
+							att_sp.roll_body = 0.0f;
+							att_sp.pitch_body = 0.0f;
+							att_sp.thrust = failsafe_throttle;
+
+							/* keep current yaw, do not attempt to go to north orientation,
+							 * since if the pilot regains RC control, he will be lost regarding
+							 * the current orientation.
+							 */
+							if (rc_loss_first_time)
+								att_sp.yaw_body = att.yaw;
+
+							// XXX hard-limit it to prevent ballistic mishaps - this is just supposed to
+							// slow a crash down, not actually keep the system in-air.
+
+							rc_loss_first_time = false;
+
+						} else {
+							rc_loss_first_time = true;
+
+							att_sp.roll_body = manual.roll;
+							att_sp.pitch_body = manual.pitch;
+
+							/* set attitude if arming */
+							if (!flag_control_attitude_enabled && state.flag_system_armed) {
+								att_sp.yaw_body = att.yaw;
+							}
+
+							/* only move setpoint if manual input is != 0 */
+
+							if(manual.mode == MANUAL_CONTROL_MODE_ATT_YAW_POS) {
+								if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) {
+									rates_sp.yaw = manual.yaw;
+									control_yaw_position = false;
+									first_time_after_yaw_speed_control = true;
+								} else {
+									if (first_time_after_yaw_speed_control) {
+										att_sp.yaw_body = att.yaw;
+										first_time_after_yaw_speed_control = false;
+									}
+									control_yaw_position = true;
+								}
+							} else if (manual.mode == MANUAL_CONTROL_MODE_ATT_YAW_RATE) {
+								rates_sp.yaw = manual.yaw;
+								control_yaw_position = false;
+							}
+
+							att_sp.thrust = manual.throttle;
+							att_sp.timestamp = hrt_absolute_time();
 						}
-						att_sp.thrust = manual.throttle;
-						att_sp.timestamp = hrt_absolute_time();
 					}
 					/* STEP 2: publish the result to the vehicle actuators */
 					orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
@@ -272,7 +329,8 @@ mc_thread_main(int argc, char *argv[])
 
 				/** STEP 3: Identify the controller setup to run and set up the inputs correctly */
 				if (state.flag_control_attitude_enabled) {
-				 	multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL);
+				 	multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position);
+
 				 	orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
 				}
 
diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c
index 8ffa90238..e94d7c55d 100644
--- a/apps/multirotor_att_control/multirotor_attitude_control.c
+++ b/apps/multirotor_att_control/multirotor_attitude_control.c
@@ -158,7 +158,7 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc
 }
 
 void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
-		const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators)
+		const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position)
 {
 	static uint64_t last_run = 0;
 	static uint64_t last_input = 0;
@@ -181,6 +181,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 
 	static bool initialized = false;
 
+	static float yaw_error;
+
 	/* initialize the pid controllers when the function is called for the first time */
 	if (initialized == false) {
 		parameters_init(&h);
@@ -199,8 +201,6 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 		/* update parameters from storage */
 		parameters_update(&h, &p);
 
-		//printf("att ctrl: delays: %d us sens->ctrl, rate: %d Hz, input: %d Hz\n", sensor_delay, (int)(1.0f/deltaT), (int)(1.0f/dT_input));
-
 		/* apply parameters */
 		pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
 		pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
@@ -216,9 +216,22 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 	rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body ,
 			att->roll, att->rollspeed, deltaT);
 
-	/* control yaw rate */
-	rates_sp->yaw = p.yaw_p * atan2f(cosf(att->yaw - att_sp->yaw_body), sinf(att->yaw - att_sp->yaw_body)) - (p.yaw_d * att->yawspeed);
+	if(control_yaw_position) {
+		/* control yaw rate */
+
+		/* positive error: rotate to right, negative error, rotate to left (NED frame) */
+		// yaw_error = _wrap_pi(att_sp->yaw_body - att->yaw);
 
+		yaw_error = att_sp->yaw_body - att->yaw;
+
+		if (yaw_error > M_PI_F) {
+			yaw_error -= M_TWOPI_F;
+		} else if (yaw_error < -M_PI_F) {
+			yaw_error += M_TWOPI_F;
+		}
+
+		rates_sp->yaw = p.yaw_p * (yaw_error) - (p.yaw_d * att->yawspeed);
+	}
 	rates_sp->thrust = att_sp->thrust;
 
 	motor_skip_counter++;
diff --git a/apps/multirotor_att_control/multirotor_attitude_control.h b/apps/multirotor_att_control/multirotor_attitude_control.h
index d9d3c0444..abc94d617 100644
--- a/apps/multirotor_att_control/multirotor_attitude_control.h
+++ b/apps/multirotor_att_control/multirotor_attitude_control.h
@@ -52,6 +52,6 @@
 #include <uORB/topics/actuator_controls.h>
 
 void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
-	const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators);
+	const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position);
 
 #endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */
diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c
index bbaec3033..93f7085ae 100644
--- a/apps/multirotor_att_control/multirotor_rate_control.c
+++ b/apps/multirotor_att_control/multirotor_rate_control.c
@@ -209,6 +209,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 
 	/* control yaw rate */
 	float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]);
+	/* increase resilience to faulty control inputs */
+	if (!isfinite(yaw_rate_control)) {
+		yaw_rate_control = 0.0f;
+		warnx("rej. NaN ctrl yaw");
+	}
 
 	actuators->control[0] = roll_control;
 	actuators->control[1] = pitch_control;
diff --git a/apps/px4/tests/test_sensors.c b/apps/px4/tests/test_sensors.c
index dc1f39046..14503276c 100644
--- a/apps/px4/tests/test_sensors.c
+++ b/apps/px4/tests/test_sensors.c
@@ -125,7 +125,7 @@ accel(int argc, char *argv[])
 	/* read data - expect samples */
 	ret = read(fd, &buf, sizeof(buf));
 
-	if (ret < 3) {
+	if (ret != sizeof(buf)) {
 		printf("\tACCEL: read1 fail (%d)\n", ret);
 		return ERROR;
 
@@ -177,7 +177,7 @@ gyro(int argc, char *argv[])
 	/* read data - expect samples */
 	ret = read(fd, &buf, sizeof(buf));
 
-	if (ret < 3) {
+	if (ret != sizeof(buf)) {
 		printf("\tGYRO: read fail (%d)\n", ret);
 		return ERROR;
 
@@ -214,7 +214,7 @@ mag(int argc, char *argv[])
 	/* read data - expect samples */
 	ret = read(fd, &buf, sizeof(buf));
 
-	if (ret < 3) {
+	if (ret != sizeof(buf)) {
 		printf("\tMAG: read fail (%d)\n", ret);
 		return ERROR;
 
@@ -251,7 +251,7 @@ baro(int argc, char *argv[])
 	/* read data - expect samples */
 	ret = read(fd, &buf, sizeof(buf));
 
-	if (ret < 3) {
+	if (ret != sizeof(buf)) {
 		printf("\tBARO: read fail (%d)\n", ret);
 		return ERROR;
 
diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c
index 3964d7503..f1dac433f 100644
--- a/apps/px4io/comms.c
+++ b/apps/px4io/comms.c
@@ -239,13 +239,19 @@ comms_handle_command(const void *buffer, size_t length)
 	for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++)
 		system_state.fmu_channel_data[i] = cmd->servo_command[i];
 
+	/* if the IO is armed and the FMU gets disarmed, the IO must also disarm */
+	if(system_state.arm_ok && !cmd->arm_ok) {
+		system_state.armed = false;
+	}
+
 	system_state.arm_ok = cmd->arm_ok;
 	system_state.mixer_use_fmu = true;
 	system_state.fmu_data_received = true;
 
+
 	/* handle changes signalled by FMU */
-	if (!system_state.arm_ok && system_state.armed)
-		system_state.armed = false;
+//	if (!system_state.arm_ok && system_state.armed)
+//		system_state.armed = false;
 
 	/* XXX do relay changes here */	
 	for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++)
diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c
index b614f3fa4..572344d00 100644
--- a/apps/px4io/mixer.c
+++ b/apps/px4io/mixer.c
@@ -82,7 +82,7 @@ static void	mixer_get_rc_input(void);
 static void	mixer_update(int mixer, uint16_t *inputs, int input_count);
 
 /* current servo arm/disarm state */
-bool mixer_servos_armed;
+bool mixer_servos_armed = false;
 
 /*
  * Each mixer consumes a set of inputs and produces a single output.
@@ -159,7 +159,7 @@ mixer_tick(void *arg)
 			/*
 			 * If we are armed, update the servo output.
 			 */
-			if (system_state.armed)
+			if (system_state.armed && system_state.arm_ok)
 				up_pwm_servo_set(i, mixers[i].current_value);
 		}
 	}
@@ -167,7 +167,8 @@ mixer_tick(void *arg)
 	/*
 	 * Decide whether the servos should be armed right now.
 	 */
-	should_arm = system_state.armed && (control_count > 0);
+
+	should_arm = system_state.armed && system_state.arm_ok && (control_count > 0) && system_state.mixer_use_fmu;
 	if (should_arm && !mixer_servos_armed) {
 		/* need to arm, but not armed */
 		up_pwm_servo_arm(true);
diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c
index e02d865d5..dec2cdde6 100644
--- a/apps/px4io/px4io.c
+++ b/apps/px4io/px4io.c
@@ -133,8 +133,9 @@ int user_start(int argc, char *argv[])
 		/* print some simple status */
 		if (timers[TIMER_STATUS_PRINT] == 0) {
 			timers[TIMER_STATUS_PRINT] = 1000;
-			lib_lowprintf("%c %s | %s | %s | C=%d F=%d B=%d   \r",
+			lib_lowprintf("%c %s | %s | %s | %s | C=%d F=%d B=%d   \r",
 				cursor[cycle++ & 3],
+				(system_state.arm_ok        ? "FMU_ARMED"  : "FMU_SAFE"),
 				(system_state.armed         ? "ARMED"  : "SAFE"),
 				(system_state.rc_channels   ? "RC OK"  : "NO RC"),
 				(system_state.mixer_use_fmu ? "FMU OK" : "NO FMU"),
diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h
index 536dbebf1..f50e29252 100644
--- a/apps/px4io/px4io.h
+++ b/apps/px4io/px4io.h
@@ -68,7 +68,7 @@
 struct sys_state_s 
 {
 
-	bool		armed;		/* actually armed */
+	bool		armed;		/* IO armed */
 	bool		arm_ok;		/* FMU says OK to arm */
 
 	/*
diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c
index f27432664..24fc9951a 100644
--- a/apps/px4io/safety.c
+++ b/apps/px4io/safety.c
@@ -56,11 +56,13 @@ static struct hrt_call arming_call;
  * Count the number of times in a row that we see the arming button
  * held down.
  */
-static unsigned arm_counter;
+static unsigned counter;
+
 #define ARM_COUNTER_THRESHOLD	10
+#define DISARM_COUNTER_THRESHOLD	2
 
 static bool safety_led_state;
-
+static bool safety_button_pressed;
 static void safety_check_button(void *arg);
 
 void
@@ -76,19 +78,35 @@ safety_check_button(void *arg)
 	/* 
 	 * Debounce the safety button, change state if it has been held for long enough.
 	 *
-	 * Ignore the button if FMU has not said it's OK to arm yet.
 	 */
-	if (BUTTON_SAFETY && system_state.arm_ok) {
-		if (arm_counter < ARM_COUNTER_THRESHOLD) {
-			arm_counter++;
-		} else if (arm_counter == ARM_COUNTER_THRESHOLD) {
-			/* change our armed state and notify the FMU */
-			system_state.armed = !system_state.armed;
-			arm_counter++;
+	safety_button_pressed = BUTTON_SAFETY;
+
+	if(safety_button_pressed) {
+		//printf("Pressed, Arm counter: %d, Disarm counter: %d\n", arm_counter, disarm_counter);
+	}
+
+	/* Keep pressed for a while to arm */
+	if (safety_button_pressed && !system_state.armed) {
+		if (counter < ARM_COUNTER_THRESHOLD) {
+			counter++;
+		} else if (counter == ARM_COUNTER_THRESHOLD) {
+			/* change to armed state and notify the FMU */
+			system_state.armed = true;
+			counter++;
+			system_state.fmu_report_due = true;
+		}
+	/* Disarm quickly */
+	} else if (safety_button_pressed && system_state.armed) {
+		if (counter < DISARM_COUNTER_THRESHOLD) {
+			counter++;
+		} else if (counter == DISARM_COUNTER_THRESHOLD) {
+			/* change to disarmed state and notify the FMU */
+			system_state.armed = false;
+			counter++;
 			system_state.fmu_report_due = true;
 		}
 	} else {
-		arm_counter = 0;
+		counter = 0;
 	}
 
 	/* when armed, toggle the LED; when safe, leave it on */
diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c
index eed72125a..d38bf9122 100644
--- a/apps/sdlog/sdlog.c
+++ b/apps/sdlog/sdlog.c
@@ -59,10 +59,13 @@
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_effective.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_vicon_position.h>
+#include <uORB/topics/optical_flow.h>
 
 #include <systemlib/systemlib.h>
 
@@ -295,10 +298,13 @@ int sdlog_thread_main(int argc, char *argv[]) {
 		struct vehicle_attitude_setpoint_s att_sp;
 		struct actuator_outputs_s act_outputs;
 		struct actuator_controls_s act_controls;
+		struct actuator_controls_effective_s act_controls_effective;
 		struct vehicle_command_s cmd;
 		struct vehicle_local_position_s local_pos;
 		struct vehicle_global_position_s global_pos;
 		struct vehicle_gps_position_s gps_pos;
+		struct vehicle_vicon_position_s vicon_pos;
+		struct optical_flow_s flow;
 	} buf;
 	memset(&buf, 0, sizeof(buf));
 
@@ -308,10 +314,13 @@ int sdlog_thread_main(int argc, char *argv[]) {
 		int att_sub;
 		int spa_sub;
 		int act_0_sub;
-		int controls0_sub;
+		int controls_0_sub;
+		int controls_effective_0_sub;
 		int local_pos_sub;
 		int global_pos_sub;
 		int gps_pos_sub;
+		int vicon_pos_sub;
+		int flow_sub;
 	} subs;
 
 	/* --- MANAGEMENT - LOGGING COMMAND --- */
@@ -353,8 +362,15 @@ int sdlog_thread_main(int argc, char *argv[]) {
 
 	/* --- ACTUATOR CONTROL VALUE --- */
 	/* subscribe to ORB for actuator control */
-	subs.controls0_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
-	fds[fdsc_count].fd = subs.controls0_sub;
+	subs.controls_0_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
+	fds[fdsc_count].fd = subs.controls_0_sub;
+	fds[fdsc_count].events = POLLIN;
+	fdsc_count++;
+
+	/* --- ACTUATOR CONTROL EFFECTIVE VALUE --- */
+	/* subscribe to ORB for actuator control */
+	subs.controls_effective_0_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
+	fds[fdsc_count].fd = subs.controls_effective_0_sub;
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
@@ -379,6 +395,20 @@ int sdlog_thread_main(int argc, char *argv[]) {
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
+	/* --- VICON POSITION --- */
+	/* subscribe to ORB for vicon position */
+	subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
+	fds[fdsc_count].fd = subs.vicon_pos_sub;
+	fds[fdsc_count].events = POLLIN;
+	fdsc_count++;
+
+	/* --- FLOW measurements --- */
+	/* subscribe to ORB for flow measurements */
+	subs.flow_sub = orb_subscribe(ORB_ID(optical_flow));
+	fds[fdsc_count].fd = subs.flow_sub;
+	fds[fdsc_count].events = POLLIN;
+	fdsc_count++;
+
 	/* WARNING: If you get the error message below,
 	 * then the number of registered messages (fdsc)
 	 * differs from the number of messages in the above list.
@@ -481,7 +511,8 @@ int sdlog_thread_main(int argc, char *argv[]) {
 
 		/* copy sensors raw data into local buffer */
 		orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw);
-		orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls0_sub, &buf.act_controls);
+		orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls_0_sub, &buf.act_controls);
+		orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.controls_effective_0_sub, &buf.act_controls_effective);
 		/* copy actuator data into local buffer */
 		orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs);
 		orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp);
@@ -489,6 +520,8 @@ int sdlog_thread_main(int argc, char *argv[]) {
 		orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos);
 		orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos);
 		orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att);
+		orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos);
+		orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow);
 
 		#pragma pack(push, 1)
 		struct {
@@ -507,6 +540,9 @@ int sdlog_thread_main(int argc, char *argv[]) {
 			int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter]
 			float attitude[3]; //pitch, roll, yaw [rad]
 			float rotMatrix[9]; //unitvectors
+			float vicon[6];
+			float control_effective[4]; //roll, pitch, yaw [-1..1], thrust [0..1]
+			float flow[6]; // flow raw x, y, flow metric x, y, flow ground dist, flow quality
 		} sysvector = {
 			.timestamp = buf.raw.timestamp,
 			.gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]},
@@ -523,13 +559,16 @@ int sdlog_thread_main(int argc, char *argv[]) {
 			.local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z},
 			.gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt},
 			.attitude = {buf.att.pitch, buf.att.roll, buf.att.yaw},
-			.rotMatrix = {buf.att.R[0][0], buf.att.R[0][1], buf.att.R[0][2], buf.att.R[1][0], buf.att.R[1][1], buf.att.R[1][2], buf.att.R[2][0], buf.att.R[2][1], buf.att.R[2][2]}
+			.rotMatrix = {buf.att.R[0][0], buf.att.R[0][1], buf.att.R[0][2], buf.att.R[1][0], buf.att.R[1][1], buf.att.R[1][2], buf.att.R[2][0], buf.att.R[2][1], buf.att.R[2][2]},
+			.vicon = {buf.vicon_pos.x, buf.vicon_pos.y, buf.vicon_pos.z, buf.vicon_pos.roll, buf.vicon_pos.pitch, buf.vicon_pos.yaw},
+			.control_effective = {buf.act_controls_effective.control_effective[0], buf.act_controls_effective.control_effective[1], buf.act_controls_effective.control_effective[2], buf.act_controls_effective.control_effective[3]},
+			.flow = {buf.flow.flow_raw_x, buf.flow.flow_raw_y, buf.flow.flow_comp_x_m, buf.flow.flow_comp_y_m, buf.flow.ground_distance_m, buf.flow.quality}
 		};
 		#pragma pack(pop)
 
 		sysvector_bytes += write(sysvector_file, (const char*)&sysvector, sizeof(sysvector));
 
-		usleep(10000);   //10000 corresponds in reality to ca. 76 Hz
+		usleep(3500);   // roughly 150 Hz
 	}
 
 	fsync(sysvector_file);
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index eea51cc1e..466284a1b 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -151,6 +151,7 @@ private:
 	int		_baro_sub;			/**< raw baro data subscription */
 	int		_vstatus_sub;			/**< vehicle status subscription */
 	int 		_params_sub;			/**< notification of parameter updates */
+	int 		_manual_control_sub;			/**< notification of manual control updates */
 
 	orb_advert_t	_sensor_pub;			/**< combined sensor data topic */
 	orb_advert_t	_manual_control_pub;		/**< manual control signal topic */
@@ -341,6 +342,7 @@ Sensors::Sensors() :
 	_baro_sub(-1),
 	_vstatus_sub(-1),
 	_params_sub(-1),
+	_manual_control_sub(-1),
 
 	/* publications */
 	_sensor_pub(-1),
@@ -903,6 +905,9 @@ Sensors::ppm_poll()
 
 		struct manual_control_setpoint_s manual_control;
 
+		/* get a copy first, to prevent altering values */
+		orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &manual_control);
+
 		/* require at least four channels to consider the signal valid */
 		if (rc_input.channel_count < 4)
 			return;
@@ -1023,6 +1028,7 @@ Sensors::task_main()
 	_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
 	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
+	_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 
 	/* rate limit vehicle status updates to 5Hz */
 	orb_set_interval(_vstatus_sub, 200);
@@ -1052,20 +1058,18 @@ Sensors::task_main()
 	_sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
 
 	/* advertise the manual_control topic */
-	{
-		struct manual_control_setpoint_s manual_control;
-		manual_control.mode = MANUAL_CONTROL_MODE_ATT_YAW_RATE;
-		manual_control.roll = 0.0f;
-		manual_control.pitch = 0.0f;
-		manual_control.yaw = 0.0f;
-		manual_control.throttle = 0.0f;
-		manual_control.aux1_cam_pan_flaps = 0.0f;
-		manual_control.aux2_cam_tilt = 0.0f;
-		manual_control.aux3_cam_zoom = 0.0f;
-		manual_control.aux4_cam_roll = 0.0f;
-
-		_manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control);
-	}
+	struct manual_control_setpoint_s manual_control;
+	manual_control.mode = MANUAL_CONTROL_MODE_ATT_YAW_POS;
+	manual_control.roll = 0.0f;
+	manual_control.pitch = 0.0f;
+	manual_control.yaw = 0.0f;
+	manual_control.throttle = 0.0f;
+	manual_control.aux1_cam_pan_flaps = 0.0f;
+	manual_control.aux2_cam_tilt = 0.0f;
+	manual_control.aux3_cam_zoom = 0.0f;
+	manual_control.aux4_cam_roll = 0.0f;
+
+	_manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control);
 
 	/* advertise the rc topic */
 	{
diff --git a/apps/systemcmds/preflight_check/Makefile b/apps/systemcmds/preflight_check/Makefile
new file mode 100644
index 000000000..f138e2640
--- /dev/null
+++ b/apps/systemcmds/preflight_check/Makefile
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (C) 2012 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.
+#
+############################################################################
+
+#
+# Reboot command.
+#
+
+APPNAME		 = preflight_check
+PRIORITY	 = SCHED_PRIORITY_DEFAULT
+STACKSIZE	 = 2048
+
+include $(APPDIR)/mk/app.mk
diff --git a/apps/systemcmds/preflight_check/preflight_check.c b/apps/systemcmds/preflight_check/preflight_check.c
new file mode 100644
index 000000000..391eea3a8
--- /dev/null
+++ b/apps/systemcmds/preflight_check/preflight_check.c
@@ -0,0 +1,198 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * 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 reboot.c
+ * Tool similar to UNIX reboot command
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <fcntl.h>
+#include <errno.h>
+
+#include <systemlib/err.h>
+
+#include <drivers/drv_led.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_tone_alarm.h>
+#include <drivers/drv_mag.h>
+#include <drivers/drv_gyro.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_baro.h>
+
+__EXPORT int preflight_check_main(int argc, char *argv[]);
+static int led_toggle(int leds, int led);
+static int led_on(int leds, int led);
+static int led_off(int leds, int led);
+
+int preflight_check_main(int argc, char *argv[])
+{
+	bool fail_on_error = false;
+
+	if (argc > 1 && !strcmp(argv[1], "--help")) {
+		warnx("usage: preflight_check [--fail-on-error]\n\tif fail on error is enabled, will return 1 on error");
+		exit(1);
+	}
+
+	if (argc > 1 && !strcmp(argv[1], "--fail-on-error")) {
+		fail_on_error = true;
+	}
+
+	bool system_ok = true;
+
+	int fd;
+	int ret;
+
+	/* give the system some time to sample the sensors in the background */
+	usleep(150000);
+
+
+	/* ---- MAG ---- */
+	close(fd);
+	fd = open(MAG_DEVICE_PATH, 0);
+	if (fd < 0) {
+		warn("failed to open magnetometer - start with 'hmc5883 start' or 'lsm303d start'");
+		system_ok = false;
+		goto system_eval;
+	}
+	ret = ioctl(fd, MAGIOCSELFTEST, 0);
+	
+	if (ret != OK) {
+		warnx("magnetometer calibration missing or bad - calibrate magnetometer first");
+		system_ok = false;
+		goto system_eval;
+	}
+
+	/* ---- ACCEL ---- */
+
+	close(fd);
+	fd = open(ACCEL_DEVICE_PATH, 0);
+	ret = ioctl(fd, ACCELIOCSELFTEST, 0);
+	
+	if (ret != OK) {
+		warnx("accel self test failed");
+		system_ok = false;
+		goto system_eval;
+	}
+
+	/* ---- GYRO ---- */
+
+	close(fd);
+	fd = open(GYRO_DEVICE_PATH, 0);
+	ret = ioctl(fd, GYROIOCSELFTEST, 0);
+	
+	if (ret != OK) {
+		warnx("gyro self test failed");
+		system_ok = false;
+		goto system_eval;
+	}
+
+	/* ---- BARO ---- */
+
+	close(fd);
+	fd = open(BARO_DEVICE_PATH, 0);
+
+		
+
+system_eval:
+	
+	if (system_ok) {
+		/* all good, exit silently */
+		exit(0);
+	} else {
+		fflush(stdout);
+
+		int buzzer = open("/dev/tone_alarm", O_WRONLY);
+		int leds = open(LED_DEVICE_PATH, 0);
+
+		/* flip blue led into alternating amber */
+		led_off(leds, LED_BLUE);
+		led_off(leds, LED_AMBER);
+		led_toggle(leds, LED_BLUE);
+
+		/* display and sound error */
+		for (int i = 0; i < 200; i++)
+		{
+			led_toggle(leds, LED_BLUE);
+			led_toggle(leds, LED_AMBER);
+
+			if (i % 10 == 0) {
+				ioctl(buzzer, TONE_SET_ALARM, 4);
+			} else if (i % 5 == 0) {
+				ioctl(buzzer, TONE_SET_ALARM, 2);
+			}
+			usleep(100000);
+		}
+
+		/* stop alarm */
+		ioctl(buzzer, TONE_SET_ALARM, 0);
+
+		/* switch on leds */
+		led_on(leds, LED_BLUE);
+		led_on(leds, LED_AMBER);
+
+		if (fail_on_error) {
+			/* exit with error message */
+			exit(1);
+		} else {
+			/* do not emit an error code to make sure system still boots */
+			exit(0);
+		}
+	}
+}
+
+static int led_toggle(int leds, int led)
+{
+	static int last_blue = LED_ON;
+	static int last_amber = LED_ON;
+
+	if (led == LED_BLUE) last_blue = (last_blue == LED_ON) ? LED_OFF : LED_ON;
+
+	if (led == LED_AMBER) last_amber = (last_amber == LED_ON) ? LED_OFF : LED_ON;
+
+	return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led);
+}
+
+static int led_off(int leds, int led)
+{
+	return ioctl(leds, LED_OFF, led);
+}
+
+static int led_on(int leds, int led)
+{
+	return ioctl(leds, LED_ON, led);
+}
\ No newline at end of file
diff --git a/apps/systemlib/geo/geo.c b/apps/systemlib/geo/geo.c
index 2f4b37e79..6746e8ff3 100644
--- a/apps/systemlib/geo/geo.c
+++ b/apps/systemlib/geo/geo.c
@@ -60,14 +60,7 @@ static double cos_phi_1;
 static double lambda_0;
 static double scale;
 
-/**
- * Initializes the map transformation.
- *
- * Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane
- * @param lat in degrees (47.1234567°, not 471234567°)
- * @param lon in degrees (8.1234567°, not 81234567°)
- */
-__EXPORT static void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
+__EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
 {
 	/* notation and formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
 	phi_1 = lat_0 / 180.0 * M_PI;
@@ -105,14 +98,7 @@ __EXPORT static void map_projection_init(double lat_0, double lon_0) //lat_0, lo
 
 }
 
-/**
- * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane
- * @param x north
- * @param y east
- * @param lat in degrees (47.1234567°, not 471234567°)
- * @param lon in degrees (8.1234567°, not 81234567°)
- */
-__EXPORT static void map_projection_project(double lat, double lon, float *x, float *y)
+__EXPORT void map_projection_project(double lat, double lon, float *x, float *y)
 {
 	/* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
 	double phi = lat / 180.0 * M_PI;
@@ -135,15 +121,7 @@ __EXPORT static void map_projection_project(double lat, double lon, float *x, fl
 //	printf("%phi_1=%.10f, lambda_0 =%.10f\n", phi_1, lambda_0);
 }
 
-/**
- * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system
- *
- * @param x north
- * @param y east
- * @param lat in degrees (47.1234567°, not 471234567°)
- * @param lon in degrees (8.1234567°, not 81234567°)
- */
-__EXPORT static void map_projection_reproject(float x, float y, double *lat, double *lon)
+__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon)
 {
 	/* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
 
@@ -228,7 +206,7 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub
 	/* conscious mix of double and float trig function to maximize speed and efficiency */
 	float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon));
 
-	theta = _wrapPI(theta);
+	theta = _wrap_pi(theta);
 
 	return theta;
 }
@@ -257,7 +235,7 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error,
 	bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
 	bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end);
 	bearing_diff = bearing_track - bearing_end;
-	bearing_diff = _wrapPI(bearing_diff);
+	bearing_diff = _wrap_pi(bearing_diff);
 
 	// Return past_end = true if past end point of line
 	if (bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) {
@@ -270,10 +248,10 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error,
 	crosstrack_error->distance = (dist_to_end) * sin(bearing_diff);
 
 	if (sin(bearing_diff) >= 0) {
-		crosstrack_error->bearing = _wrapPI(bearing_track - M_PI_2_F);
+		crosstrack_error->bearing = _wrap_pi(bearing_track - M_PI_2_F);
 
 	} else {
-		crosstrack_error->bearing = _wrapPI(bearing_track + M_PI_2_F);
+		crosstrack_error->bearing = _wrap_pi(bearing_track + M_PI_2_F);
 	}
 
 	return_value = OK;
@@ -380,22 +358,36 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d
 	return return_value;
 }
 
-float _wrapPI(float bearing)
+__EXPORT float _wrap_pi(float bearing)
 {
+	/* value is inf or NaN */
+	if (!isfinite(bearing) || bearing == 0) {
+		return bearing;
+	}
 
-	while (bearing > M_PI_F) {
-		bearing = bearing - M_TWOPI_F;
+	int c = 0;
+
+	while (bearing > M_PI_F && c < 30) {
+		bearing -= M_TWOPI_F;
+		c++;
 	}
 
-	while (bearing <=  -M_PI_F) {
-		bearing = bearing + M_TWOPI_F;
+	c = 0;
+
+	while (bearing <=  -M_PI_F && c < 30) {
+		bearing += M_TWOPI_F;
+		c++;
 	}
 
 	return bearing;
 }
 
-float _wrap2PI(float bearing)
+__EXPORT float _wrap_2pi(float bearing)
 {
+	/* value is inf or NaN */
+	if (!isfinite(bearing)) {
+		return bearing;
+	}
 
 	while (bearing >= M_TWOPI_F) {
 		bearing = bearing - M_TWOPI_F;
@@ -408,8 +400,12 @@ float _wrap2PI(float bearing)
 	return bearing;
 }
 
-float _wrap180(float bearing)
+__EXPORT float _wrap_180(float bearing)
 {
+	/* value is inf or NaN */
+	if (!isfinite(bearing)) {
+		return bearing;
+	}
 
 	while (bearing > 180.0f) {
 		bearing = bearing - 360.0f;
@@ -422,8 +418,12 @@ float _wrap180(float bearing)
 	return bearing;
 }
 
-float _wrap360(float bearing)
+__EXPORT float _wrap_360(float bearing)
 {
+	/* value is inf or NaN */
+	if (!isfinite(bearing)) {
+		return bearing;
+	}
 
 	while (bearing >= 360.0f) {
 		bearing = bearing - 360.0f;
diff --git a/apps/systemlib/geo/geo.h b/apps/systemlib/geo/geo.h
index 7aad79a8c..0c0b5c533 100644
--- a/apps/systemlib/geo/geo.h
+++ b/apps/systemlib/geo/geo.h
@@ -54,24 +54,44 @@ struct crosstrack_error_s {
 	float bearing;		// Bearing in radians to closest point on line/arc
 } ;
 
-__EXPORT static void map_projection_init(double lat_0, double lon_0);
+/**
+ * Initializes the map transformation.
+ *
+ * Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane
+ * @param lat in degrees (47.1234567°, not 471234567°)
+ * @param lon in degrees (8.1234567°, not 81234567°)
+ */
+__EXPORT void map_projection_init(double lat_0, double lon_0);
 
-__EXPORT static void map_projection_project(double lat, double lon, float *x, float *y);
+/**
+ * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane
+ * @param x north
+ * @param y east
+ * @param lat in degrees (47.1234567°, not 471234567°)
+ * @param lon in degrees (8.1234567°, not 81234567°)
+ */
+__EXPORT void map_projection_project(double lat, double lon, float *x, float *y);
 
-__EXPORT static void map_projection_reproject(float x, float y, double *lat, double *lon);
+/**
+ * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system
+ *
+ * @param x north
+ * @param y east
+ * @param lat in degrees (47.1234567°, not 471234567°)
+ * @param lon in degrees (8.1234567°, not 81234567°)
+ */
+__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon);
 
 __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
 
 __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
 
-//
-
 __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end);
 
 __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
 		float radius, float arc_start_bearing, float arc_sweep);
 
-float _wrap180(float bearing);
-float _wrap360(float bearing);
-float _wrapPI(float bearing);
-float _wrap2PI(float bearing);
+__EXPORT float _wrap_180(float bearing);
+__EXPORT float _wrap_360(float bearing);
+__EXPORT float _wrap_pi(float bearing);
+__EXPORT float _wrap_2pi(float bearing);
diff --git a/apps/systemlib/mixer/mixer_multirotor.cpp b/apps/systemlib/mixer/mixer_multirotor.cpp
index 0e714ed50..b5b25e532 100644
--- a/apps/systemlib/mixer/mixer_multirotor.cpp
+++ b/apps/systemlib/mixer/mixer_multirotor.cpp
@@ -161,6 +161,28 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 	float		max     = 0.0f;
 	float		fixup_scale;
 
+	/* use an output factor to prevent too strong control signals at low throttle */
+	float min_thrust = 0.05f;
+	float max_thrust = 1.0f;
+	float startpoint_full_control = 0.40f;
+	float output_factor;
+
+	/* keep roll, pitch and yaw control to 0 below min thrust */
+	if (thrust <= min_thrust) {
+		output_factor = 0.0f;
+	/* linearly increase the output factor from 0 to 1 between min_thrust and startpoint_full_control */
+	} else if (thrust < startpoint_full_control && thrust > min_thrust) {
+		output_factor = (thrust/max_thrust)/(startpoint_full_control-min_thrust);
+	/* and then stay at full control */
+	} else {
+		output_factor = max_thrust;
+	}
+
+	roll *= output_factor;
+	pitch *= output_factor;
+	yaw *= output_factor;
+
+
 	/* perform initial mix pass yielding un-bounded outputs */
 	for (unsigned i = 0; i < _rotor_count; i++) {
 		float tmp = roll  * _rotors[i].roll_scale +
diff --git a/apps/systemlib/param/param.c b/apps/systemlib/param/param.c
index ddf9b0975..ebb72ca3e 100644
--- a/apps/systemlib/param/param.c
+++ b/apps/systemlib/param/param.c
@@ -517,13 +517,11 @@ param_save_default(void)
 	}
 
 	int result = param_export(fd, false);
-	/* should not be necessary, over-careful here */
-	fsync(fd);
 	close(fd);
 
 	if (result != 0) {
-		unlink(param_get_default_file());
 		warn("error exporting parameters to '%s'", param_get_default_file());
+		unlink(param_get_default_file());
 		return -2;
 	}
 
diff --git a/apps/systemlib/param/param.h b/apps/systemlib/param/param.h
index 6fa73b5a4..084cd931a 100644
--- a/apps/systemlib/param/param.h
+++ b/apps/systemlib/param/param.h
@@ -250,7 +250,7 @@ __EXPORT int 		param_set_default_file(const char* filename);
  *			a result of a call to param_set_default_file, or the 
  *			built-in default.
  */
-__EXPORT const char 	*param_get_default_file(void);
+__EXPORT const char*	param_get_default_file(void);
 
 /**
  * Save parameters to the default file.
diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp
index b5e1d739c..dbee15050 100644
--- a/apps/uORB/objects_common.cpp
+++ b/apps/uORB/objects_common.cpp
@@ -95,6 +95,9 @@ ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoi
 #include "topics/vehicle_global_position_setpoint.h"
 ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setpoint_s);
 
+#include "topics/vehicle_global_position_set_triplet.h"
+ORB_DEFINE(vehicle_global_position_set_triplet, struct vehicle_global_position_set_triplet_s);
+
 #include "topics/vehicle_attitude_setpoint.h"
 ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s);
 
@@ -110,6 +113,7 @@ ORB_DEFINE(optical_flow, struct optical_flow_s);
 #include "topics/subsystem_info.h"
 ORB_DEFINE(subsystem_info, struct subsystem_info_s);
 
+/* actuator controls, as requested by controller */
 #include "topics/actuator_controls.h"
 ORB_DEFINE(actuator_controls_0, struct actuator_controls_s);
 ORB_DEFINE(actuator_controls_1, struct actuator_controls_s);
@@ -117,6 +121,13 @@ ORB_DEFINE(actuator_controls_2, struct actuator_controls_s);
 ORB_DEFINE(actuator_controls_3, struct actuator_controls_s);
 ORB_DEFINE(actuator_armed, struct actuator_armed_s);
 
+/* actuator controls, as set by actuators / mixers after limiting */
+#include "topics/actuator_controls_effective.h"
+ORB_DEFINE(actuator_controls_effective_0, struct actuator_controls_effective_s);
+ORB_DEFINE(actuator_controls_effective_1, struct actuator_controls_effective_s);
+ORB_DEFINE(actuator_controls_effective_2, struct actuator_controls_effective_s);
+ORB_DEFINE(actuator_controls_effective_3, struct actuator_controls_effective_s);
+
 #include "topics/actuator_outputs.h"
 ORB_DEFINE(actuator_outputs_0, struct actuator_outputs_s);
 ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s);
diff --git a/apps/uORB/topics/actuator_controls_effective.h b/apps/uORB/topics/actuator_controls_effective.h
new file mode 100644
index 000000000..3c72e4cb7
--- /dev/null
+++ b/apps/uORB/topics/actuator_controls_effective.h
@@ -0,0 +1,69 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 actuator_controls.h
+ *
+ * Actuator control topics - mixer inputs.
+ *
+ * Values published to these topics are the outputs of the vehicle control
+ * system, and are expected to be mixed and used to drive the actuators
+ * (servos, speed controls, etc.) that operate the vehicle.
+ *
+ * Each topic can be published by a single controller
+ */
+
+#ifndef TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H
+#define TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H
+
+#include <stdint.h>
+#include "../uORB.h"
+
+#define NUM_ACTUATOR_CONTROLS		8
+#define NUM_ACTUATOR_CONTROL_GROUPS	4	/**< for sanity checking */
+
+struct actuator_controls_effective_s {
+	uint64_t timestamp;
+	float	control_effective[NUM_ACTUATOR_CONTROLS];
+};
+
+/* actuator control sets; this list can be expanded as more controllers emerge */
+ORB_DECLARE(actuator_controls_effective_0);
+ORB_DECLARE(actuator_controls_effective_1);
+ORB_DECLARE(actuator_controls_effective_2);
+ORB_DECLARE(actuator_controls_effective_3);
+
+/* control sets with pre-defined applications */
+#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE	ORB_ID(actuator_controls_effective_0)
+
+#endif /* TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H */
\ No newline at end of file
diff --git a/apps/uORB/topics/manual_control_setpoint.h b/apps/uORB/topics/manual_control_setpoint.h
index a7f5be708..1368cb716 100644
--- a/apps/uORB/topics/manual_control_setpoint.h
+++ b/apps/uORB/topics/manual_control_setpoint.h
@@ -48,13 +48,6 @@
  * @{
  */
 
-/**
- * Defines how RC channels map to control inputs.
- *
- * The default mode on quadrotors and fixed wing is
- * roll and pitch position of the right stick and
- * throttle and yaw rate on the left stick
- */
 enum MANUAL_CONTROL_MODE
 {
 	MANUAL_CONTROL_MODE_DIRECT = 0,
diff --git a/apps/uORB/topics/vehicle_attitude_setpoint.h b/apps/uORB/topics/vehicle_attitude_setpoint.h
index 8663846fc..a7cda34a8 100644
--- a/apps/uORB/topics/vehicle_attitude_setpoint.h
+++ b/apps/uORB/topics/vehicle_attitude_setpoint.h
@@ -56,18 +56,13 @@ struct vehicle_attitude_setpoint_s
 {
 	uint64_t timestamp;		/**< in microseconds since system start, is set whenever the writing thread stores new data */
 
-	float roll_tait_bryan;			/**< Tait-Bryan angle in NED frame		*/
-	float pitch_tait_bryan;			/**< Tait-Bryan angle in NED frame		*/
-	float yaw_tait_bryan;			/**< Tait-Bryan angle in NED frame		*/
-	//float tait_bryan_valid;			/**< Set to true if Tait-Bryan angles are valid */
-
 	float roll_body;				/**< body angle in NED frame		*/
 	float pitch_body;				/**< body angle in NED frame		*/
 	float yaw_body;					/**< body angle in NED frame		*/
 	//float body_valid;				/**< Set to true if body angles are valid */
 
-	//float R_body[9];				/**< Rotation matrix describing the setpoint as rotation from the current body frame */
-	//bool R_valid;					/**< Set to true if rotation matrix is valid */
+	float R_body[9];				/**< Rotation matrix describing the setpoint as rotation from the current body frame */
+	bool R_valid;					/**< Set to true if rotation matrix is valid */
 
 	float thrust;					/**< Thrust in Newton the power system should generate */
 
diff --git a/apps/uORB/topics/vehicle_global_position_set_triplet.h b/apps/uORB/topics/vehicle_global_position_set_triplet.h
new file mode 100644
index 000000000..318abba89
--- /dev/null
+++ b/apps/uORB/topics/vehicle_global_position_set_triplet.h
@@ -0,0 +1,78 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           @author Julian Oes <joes@student.ethz.ch>
+ *           @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file vehicle_global_position_setpoint.h
+ * Definition of the global WGS84 position setpoint uORB topic.
+ */
+
+#ifndef TOPIC_VEHICLE_GLOBAL_POSITION_SET_TRIPLET_H_
+#define TOPIC_VEHICLE_GLOBAL_POSITION_SET_TRIPLET_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+#include "vehicle_global_position_setpoint.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * Global position setpoint triplet in WGS84 coordinates.
+ *
+ * This are the three next waypoints (or just the next two or one).
+ */
+struct vehicle_global_position_set_triplet_s
+{
+	bool previous_valid;
+	bool next_valid;
+
+	struct vehicle_global_position_setpoint_s previous;
+	struct vehicle_global_position_setpoint_s current;
+	struct vehicle_global_position_setpoint_s next;
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vehicle_global_position_set_triplet);
+
+#endif
diff --git a/apps/uORB/topics/vehicle_global_position_setpoint.h b/apps/uORB/topics/vehicle_global_position_setpoint.h
index b73e2a363..eec6a8229 100644
--- a/apps/uORB/topics/vehicle_global_position_setpoint.h
+++ b/apps/uORB/topics/vehicle_global_position_setpoint.h
@@ -60,12 +60,12 @@
 struct vehicle_global_position_setpoint_s
 {
 	bool altitude_is_relative;	/**< true if altitude is relative from start point	*/
-	int32_t lat;				/**< latitude in degrees * 1E7						*/
-	int32_t lon;				/**< longitude in degrees * 1E7						*/
-	float altitude;				/**< altitude in meters								*/
-	float yaw;					/**< in radians NED -PI..+PI 						*/
-	float loiter_radius;		/**< loiter radius in meters, 0 for a VTOL to hover */
-	bool is_loiter;				/**< true if loitering is enabled */
+	int32_t lat;			/**< latitude in degrees * 1E7				*/
+	int32_t lon;			/**< longitude in degrees * 1E7				*/
+	float altitude;			/**< altitude in meters					*/
+	float yaw;			/**< in radians NED -PI..+PI 				*/
+	float loiter_radius;		/**< loiter radius in meters, 0 for a VTOL to hover     */
+	bool is_loiter;			/**< true if loitering is enabled			*/
 };
 
 /**
diff --git a/apps/uORB/topics/vehicle_vicon_position.h b/apps/uORB/topics/vehicle_vicon_position.h
index 91d933f44..0822fa89a 100644
--- a/apps/uORB/topics/vehicle_vicon_position.h
+++ b/apps/uORB/topics/vehicle_vicon_position.h
@@ -60,9 +60,9 @@ struct vehicle_vicon_position_s
 	float x;				/**< X positin in meters in NED earth-fixed frame */
 	float y;				/**< X positin in meters in NED earth-fixed frame */
 	float z;				/**< Z positin in meters in NED earth-fixed frame (negative altitude) */
-	float vx;
-	float vy;
-	float vz;
+	float roll;
+	float pitch;
+	float yaw;
 
 	// TODO Add covariances here
 
diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig
index 798f57e93..5833e3575 100644
--- a/nuttx/configs/px4fmu/nsh/appconfig
+++ b/nuttx/configs/px4fmu/nsh/appconfig
@@ -54,6 +54,7 @@ CONFIGURED_APPS += systemcmds/mixer
 CONFIGURED_APPS += systemcmds/eeprom
 CONFIGURED_APPS += systemcmds/param
 CONFIGURED_APPS += systemcmds/bl_update
+CONFIGURED_APPS += systemcmds/preflight_check
 #CONFIGURED_APPS += systemcmds/calibration
 
 # Tutorial code from
@@ -68,6 +69,7 @@ CONFIGURED_APPS += systemcmds/bl_update
 CONFIGURED_APPS += uORB
 
 CONFIGURED_APPS += mavlink
+CONFIGURED_APPS += mavlink_onboard
 CONFIGURED_APPS += gps
 CONFIGURED_APPS += commander
 CONFIGURED_APPS += sdlog
diff --git a/nuttx/fs/fs_files.c b/nuttx/fs/fs_files.c
index 425e7c73f..4da2d28a5 100644
--- a/nuttx/fs/fs_files.c
+++ b/nuttx/fs/fs_files.c
@@ -400,6 +400,7 @@ int files_allocate(FAR struct inode *inode, int oflags, off_t pos, int minfd)
                list->fl_files[i].f_oflags = oflags;
                list->fl_files[i].f_pos    = pos;
                list->fl_files[i].f_inode  = inode;
+               list->fl_files[i].f_priv   = NULL;
                _files_semgive(list);
                return i;
             }