Merge remote-tracking branch 'upstream/master' into io

Fixed Conflicts:
	apps/multirotor_att_control/multirotor_att_control_main.c

rc loss failsafe throttle tested

26 files changed, 560 insertions, 323 deletions

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_interface.c b/apps/ardrone_interface/ardrone_interface.c
index f96d901fb..aeaf830de 100644
--- a/apps/ardrone_interface/ardrone_interface.c
+++ b/apps/ardrone_interface/ardrone_interface.c
@@ -340,7 +340,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 			}
 		}
 
-		if (counter % 16 == 0) {
+		if (counter % 24 == 0) {
 			if (led_counter == 0) ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0);
 
 			if (led_counter == 1) ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0);
@@ -371,7 +371,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 		}
 
 		/* run at approximately 200 Hz */
-		usleep(5000);
+		usleep(4500);
 
 		counter++;
 	}
diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
index b25e61229..6533390bc 100644
--- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
+++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
@@ -63,6 +63,7 @@
 #include <drivers/drv_hrt.h>
 
 #include <systemlib/systemlib.h>
+#include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
 
 #include "codegen/attitudeKalmanfilter_initialize.h"
@@ -255,6 +256,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 	float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
 	unsigned offset_count = 0;
 
+	/* register the perf counter */
+	perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf");
+
 	/* Main loop*/
 	while (!thread_should_exit) {
 
@@ -306,6 +310,8 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					}
 				} else {
 
+					perf_begin(ekf_loop_perf);
+
 					/* Calculate data time difference in seconds */
 					dt = (raw.timestamp - last_measurement) / 1000000.0f;
 					last_measurement = raw.timestamp;
@@ -400,41 +406,6 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 						/* due to inputs or numerical failure the output is invalid, skip it */
 						continue;
 					}
-					
-					// uint64_t timing_diff = hrt_absolute_time() - timing_start;
-
-					// /* print rotation matrix every 200th time */
-					if (printcounter % 200 == 0) {
-						// printf("x apo:\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n",
-						// 	x_aposteriori[0], x_aposteriori[1], x_aposteriori[2],
-						// 	x_aposteriori[3], x_aposteriori[4], x_aposteriori[5],
-						// 	x_aposteriori[6], x_aposteriori[7], x_aposteriori[8]);
-
-
-						// }
-
-						//printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt));
-						//printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f\n", (double)euler[0], (double)euler[1], (double)euler[2]);
-						//printf("update rates gyro: %8.4f\taccel: %8.4f\tmag:%8.4f\n", (double)sensor_update_hz[0], (double)sensor_update_hz[1], (double)sensor_update_hz[2]);
-						// 	printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100),
-						// 	       (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100),
-						// 	       (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100));
-					}
-
-					// 				int i = printcounter % 9;
-
-					// 	// for (int i = 0; i < 9; i++) {
-					// 		char name[10];
-					// 		sprintf(name, "xapo #%d", i);
-					// 		memcpy(dbg.key, name, sizeof(dbg.key));
-					// 		dbg.value = x_aposteriori[i];
-					// if (pub_dbg < 0) {
-							// pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg);
-					// } else {
-					// 		orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg);
-					// }
-
-					printcounter++;
 
 					if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data);
 
@@ -463,6 +434,8 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					} else {
 						warnx("NaN in roll/pitch/yaw estimate!");
 					}
+
+					perf_end(ekf_loop_perf);
 				}
 			}
 		}
diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 2b59f709a..c3e825a86 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;
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c
index 9c7db8fca..bf50ebad2 100644
--- a/apps/commander/state_machine_helper.c
+++ b/apps/commander/state_machine_helper.c
@@ -213,6 +213,24 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
 	/* publish the new state */
 	current_status->counter++;
 	current_status->timestamp = hrt_absolute_time();
+
+	/* assemble state vector based on flag values */
+	if (current_status->flag_control_rates_enabled) {
+		current_status->onboard_control_sensors_present |= 0x400;
+	} else {
+		current_status->onboard_control_sensors_present &= ~0x400;
+	}
+	current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0;
+	current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0;
+	current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0;
+	current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0;
+
+	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0;
+	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0;
+	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0;
+	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0;
+	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0;
+
 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
 	printf("[commander] new state: %s\n", system_state_txt[current_status->state_machine]);
 }
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_led.h b/apps/drivers/drv_led.h
index 7eb9e4b7c..21044f620 100644
--- a/apps/drivers/drv_led.h
+++ b/apps/drivers/drv_led.h
@@ -47,9 +47,9 @@
 #define _LED_BASE		0x2800
 
 /* PX4 LED colour codes */
-#define LED_AMBER		0
-#define LED_RED			0	/* some boards have red rather than amber */
-#define LED_BLUE		1
+#define LED_AMBER		1
+#define LED_RED			1	/* some boards have red rather than amber */
+#define LED_BLUE		0
 #define LED_SAFETY		2
 
 #define LED_ON			_IOC(_LED_BASE, 0)
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 81bc8954b..3849a2e05 100644
--- a/apps/drivers/hmc5883/hmc5883.cpp
+++ b/apps/drivers/hmc5883/hmc5883.cpp
@@ -634,6 +634,7 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case MAGIOCSSCALE:
 		/* set new scale factors */
 		memcpy(&_scale, (mag_scale *)arg, sizeof(_scale));
+		/* check calibration, but not actually return an error */
 		(void)check_calibration();
 		return 0;
 
@@ -648,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);
@@ -1012,7 +1016,12 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 out:
 
 	if (ret == OK) {
-		warnx("mag scale calibration successfully finished.");
+		if (!check_calibration()) {
+			warnx("mag scale calibration successfully finished.");
+		} else {
+			warnx("mag scale calibration finished with invalid results.");
+			ret == ERROR;
+		}
 
 	} else {
 		warnx("mag scale calibration failed.");
@@ -1025,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 = {
@@ -1055,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/gps/mtk.c b/apps/gps/mtk.c
index 604dba05c..7ba4f52b0 100644
--- a/apps/gps/mtk.c
+++ b/apps/gps/mtk.c
@@ -410,6 +410,8 @@ void *mtk_watchdog_loop(void *args)
 		} else {
 			/* gps healthy */
 			mtk_success_count++;
+			mtk_fail_count = 0;
+			once_ok = true; // XXX Should this be true on a single success, or on same criteria as mtk_healthy?
 
 			if (!mtk_healthy && mtk_success_count >= MTK_HEALTH_SUCCESS_COUNTER_LIMIT) {
 				printf("[gps] MTK module found, status ok (baud=%d)\r\n", current_gps_speed);
@@ -418,11 +420,8 @@ void *mtk_watchdog_loop(void *args)
 				mtk_gps->satellite_info_available = 0;
 				// global_data_send_subsystem_info(&mtk_present_enabled_healthy);
 				mavlink_log_info(mavlink_fd, "[gps] MTK custom binary module found, status ok\n");
+				mtk_healthy = true;
 			}
-
-			mtk_healthy = true;
-			mtk_fail_count = 0;
-			once_ok = true;
 		}
 
 		usleep(MTK_WATCHDOG_WAIT_TIME_MICROSECONDS);
diff --git a/apps/gps/ubx.c b/apps/gps/ubx.c
index 21e917bf8..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
@@ -786,22 +786,18 @@ void *ubx_watchdog_loop(void *args)
 			sleep(1);
 
 		} else {
+			/* gps healthy */
+			ubx_success_count++;
+			ubx_fail_count = 0;
+			once_ok = true; // XXX Should this be true on a single success, or on same criteria as ubx_healthy?
 
 			if (!ubx_healthy && ubx_success_count == UBX_HEALTH_SUCCESS_COUNTER_LIMIT) {
 				//printf("[gps] ublox UBX module status ok (baud=%d)\r\n", current_gps_speed);
 				// global_data_send_subsystem_info(&ubx_present_enabled_healthy);
 				mavlink_log_info(mavlink_fd, "[gps] UBX module found, status ok\n");
 				ubx_healthy = true;
-				ubx_fail_count = 0;
-				once_ok = true;
 			}
-
-			/* gps healthy */
-			ubx_success_count++;
-			ubx_healthy = true;
-			ubx_fail_count = 0;
 		}
-
 		usleep(UBX_WATCHDOG_WAIT_TIME_MICROSECONDS);
 	}
 
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c
index de3a4440b..d2de1a787 100644
--- a/apps/multirotor_att_control/multirotor_att_control_main.c
+++ b/apps/multirotor_att_control/multirotor_att_control_main.c
@@ -65,6 +65,7 @@
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/parameter_update.h>
 
 #include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
@@ -107,6 +108,7 @@ mc_thread_main(int argc, char *argv[])
 
 	/* subscribe to attitude, motor setpoints and system state */
 	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	int param_sub = orb_subscribe(ORB_ID(parameter_update));
 	int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
 	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
@@ -121,7 +123,10 @@ mc_thread_main(int argc, char *argv[])
 	 * rate-limit the attitude subscription to 200Hz to pace our loop
 	 * orb_set_interval(att_sub, 5);
 	 */
-	struct pollfd fds = { .fd = att_sub, .events = POLLIN };
+	struct pollfd fds[2] = {
+					{ .fd = att_sub, .events = POLLIN },
+					{ .fd = param_sub, .events = POLLIN }
+				};
 
 	/* publish actuator controls */
 	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
@@ -134,7 +139,9 @@ mc_thread_main(int argc, char *argv[])
 	int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
 
 	/* register the perf counter */
-	perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control");
+	perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control_runtime");
+	perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "multirotor_att_control_interval");
+	perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "multirotor_att_control_err");
 
 	/* welcome user */
 	printf("[multirotor_att_control] starting\n");
@@ -143,7 +150,6 @@ mc_thread_main(int argc, char *argv[])
 	bool flag_control_manual_enabled = false;
 	bool flag_control_attitude_enabled = false;
 	bool flag_system_armed = false;
-	bool man_yaw_zero_once = false;
 
 	/* prepare the handle for the failsafe throttle */
 	param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR");
@@ -152,180 +158,185 @@ mc_thread_main(int argc, char *argv[])
 	while (!thread_should_exit) {
 
 		/* wait for a sensor update, check for exit condition every 500 ms */
-		poll(&fds, 1, 500);
+		int ret = poll(fds, 2, 500);
+
+		if (ret < 0) {
+			/* poll error, count it in perf */
+			perf_count(mc_err_perf);
+		} else if (ret == 0) {
+			/* no return value, ignore */
+		} else {
+
+			/* only update parameters if they changed */
+			if (fds[1].revents & POLLIN) {
+				/* read from param to clear updated flag */
+				struct parameter_update_s update;
+				orb_copy(ORB_ID(parameter_update), param_sub, &update);
+
+				/* update parameters */
+				// XXX no params here yet
+			}
 
-		perf_begin(mc_loop_perf);
+			/* only run controller if attitude changed */
+			if (fds[0].revents & POLLIN) {
 
-		/* get a local copy of system state */
-		bool updated;
-		orb_check(state_sub, &updated);
-		if (updated) {
-			orb_copy(ORB_ID(vehicle_status), state_sub, &state);
-		}
-		/* get a local copy of manual setpoint */
-		orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
-		/* get a local copy of attitude */
-		orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
-		/* get a local copy of attitude setpoint */
-		orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp);
-		/* get a local copy of rates setpoint */
-		orb_check(setpoint_sub, &updated);
-		if (updated) {
-			orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp);
-		}
-		/* get a local copy of the current sensor values */
-		orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
-
-
-		/** 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;
-						if (state.offboard_control_signal_lost) {
-							/* give up in rate mode on signal loss */
-							rates_sp.thrust = 0.0f;
-						} else {
-							/* no signal loss, normal throttle */
-							rates_sp.thrust = offboard_sp.p4;
+				perf_begin(mc_loop_perf);
+
+				/* get a local copy of system state */
+				bool updated;
+				orb_check(state_sub, &updated);
+				if (updated) {
+					orb_copy(ORB_ID(vehicle_status), state_sub, &state);
+				}
+				/* get a local copy of manual setpoint */
+				orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
+				/* get a local copy of attitude */
+				orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
+				/* get a local copy of attitude setpoint */
+				orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp);
+				/* get a local copy of rates setpoint */
+				orb_check(setpoint_sub, &updated);
+				if (updated) {
+					orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp);
+				}
+				/* get a local copy of the current sensor values */
+				orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
+
+
+				/** 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 */
 						}
-//						printf("thrust_rate=%8.4f\n",offboard_sp.p4);
+				else if (state.flag_control_manual_enabled) {
+
+					/* manual inputs, from RC control or joystick */
+					if (state.flag_control_rates_enabled && !state.flag_control_attitude_enabled) {
+						rates_sp.roll = manual.roll;
+
+						rates_sp.pitch = manual.pitch;
+						rates_sp.yaw = manual.yaw;
+						rates_sp.thrust = manual.throttle;
 						rates_sp.timestamp = hrt_absolute_time();
-						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;
-
-						/* check if control signal is lost */
-						if (state.offboard_control_signal_lost) {
-							/* set failsafe thrust */
+					}
+
+					if (state.flag_control_attitude_enabled) {
+
+						/* initialize to current yaw if switching to manual or att control */
+						if (state.flag_control_attitude_enabled != flag_control_attitude_enabled ||
+					 	    state.flag_control_manual_enabled != flag_control_manual_enabled ||
+					 	    state.flag_system_armed != flag_system_armed) {
+							att_sp.yaw_body = att.yaw;
+						}
+
+						static bool rc_loss_first_time = true;
+
+						/* if the RC signal is lost, try to stay level and go slowly back down to ground */
+						if(state.rc_signal_lost) {
+							/* 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 {
-							/* no signal loss, normal throttle */
-							att_sp.thrust = offboard_sp.p4;
+							rc_loss_first_time = true;
+
+							att_sp.roll_body = manual.roll;
+							att_sp.pitch_body = manual.pitch;
+
+							/* 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;
+							}
+							att_sp.thrust = manual.throttle;
+							att_sp.timestamp = hrt_absolute_time();
 						}
-						
-//						printf("thrust_att=%8.4f\n",offboard_sp.p4);
+					}
+					/* STEP 2: publish the result to the vehicle actuators */
+					orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
+
+					if (motor_test_mode) {
+						printf("testmode");
+						att_sp.roll_body = 0.0f;
+						att_sp.pitch_body = 0.0f;
+						att_sp.yaw_body = 0.0f;
+						att_sp.thrust = 0.1f;
 						att_sp.timestamp = hrt_absolute_time();
 						/* STEP 2: publish the result to the vehicle actuators */
 						orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
 					}
 
-					/* decide wether we want rate or position input */
 				}
-		else if (state.flag_control_manual_enabled) {
-			/* manual inputs, from RC control or joystick */
-
-			if (state.flag_control_rates_enabled && !state.flag_control_attitude_enabled) {
-				rates_sp.roll = manual.roll;
-
-				rates_sp.pitch = manual.pitch;
-				rates_sp.yaw = manual.yaw;
-				rates_sp.thrust = manual.throttle;
-				rates_sp.timestamp = hrt_absolute_time();
-			}
-
-			if (state.flag_control_attitude_enabled) {
 
-				/* initialize to current yaw if switching to manual or att control */
-				if (state.flag_control_attitude_enabled != flag_control_attitude_enabled ||
-			 	    state.flag_control_manual_enabled != flag_control_manual_enabled ||
-			 	    state.flag_system_armed != flag_system_armed) {
-					att_sp.yaw_body = att.yaw;
+				/** 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);
+				 	orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
 				}
 
-				att_sp.roll_body = manual.roll;
-				att_sp.pitch_body = manual.pitch;
-
-				/* 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;
-				}
-				att_sp.thrust = manual.throttle;
-
-				// XXX disable this for now until its better checked
-
-				 static bool rc_loss_first_time = true;
-				 /* if the RC signal is lost, try to stay level and go slowly back down to ground */
-				 if(state.rc_signal_lost) {
-				 	/* 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;
-					
-				 	/* 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.
-				 	att_sp.thrust = failsafe_throttle;
-				 	rc_loss_first_time = false;
-				 } else {
-				 	rc_loss_first_time = true;
-				 }
-
-				att_sp.timestamp = hrt_absolute_time();
-			}
-			/* STEP 2: publish the result to the vehicle actuators */
-			orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-
-			if (motor_test_mode) {
-				printf("testmode");
-				att_sp.roll_body = 0.0f;
-				att_sp.pitch_body = 0.0f;
-				att_sp.yaw_body = 0.0f;
-				att_sp.thrust = 0.1f;
-				att_sp.timestamp = hrt_absolute_time();
-				/* STEP 2: publish the result to the vehicle actuators */
-				orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-			}
-
-		}
-
-		/** 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);
-		 	orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
-		}
-
+				/* measure in what intervals the controller runs */
+				perf_count(mc_interval_perf);
 
-		if (state.flag_control_rates_enabled) {
+				float gyro[3];
 
-			float gyro[3];
-
-			/* get current rate setpoint */
-			bool rates_sp_valid = false;
-			orb_check(rates_sp_sub, &rates_sp_valid);
-			if (rates_sp_valid) {
-				orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp);
-			}
+				/* get current rate setpoint */
+				bool rates_sp_valid = false;
+				orb_check(rates_sp_sub, &rates_sp_valid);
+				if (rates_sp_valid) {
+					orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp);
+				}
 
-			/* apply controller */
-			gyro[0] = att.rollspeed;
-			gyro[1] = att.pitchspeed;
-			gyro[2] = att.yawspeed;
+				/* apply controller */
+				gyro[0] = att.rollspeed;
+				gyro[1] = att.pitchspeed;
+				gyro[2] = att.yawspeed;
 
-			multirotor_control_rates(&rates_sp, gyro, &actuators);
-			orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
-		}
+				multirotor_control_rates(&rates_sp, gyro, &actuators);
+				orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
 
-		/* update state */
-		flag_control_attitude_enabled = state.flag_control_attitude_enabled;
-		flag_control_manual_enabled = state.flag_control_manual_enabled;
-		flag_system_armed = state.flag_system_armed;
+				/* update state */
+				flag_control_attitude_enabled = state.flag_control_attitude_enabled;
+				flag_control_manual_enabled = state.flag_control_manual_enabled;
+				flag_system_armed = state.flag_system_armed;
 
-		perf_end(mc_loop_perf);
+				perf_end(mc_loop_perf);
+			} /* end of poll call for attitude updates */
+		} /* end of poll return value check */
 	}
 
 	printf("[multirotor att control] stopping, disarming motors.\n");
diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c
index 839d56d29..8ffa90238 100644
--- a/apps/multirotor_att_control/multirotor_attitude_control.c
+++ b/apps/multirotor_att_control/multirotor_attitude_control.c
@@ -199,9 +199,9 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 		/* update parameters from storage */
 		parameters_update(&h, &p);
 
-		/* apply parameters */
-		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));
+		//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);
 	}
diff --git a/apps/px4/tests/test_adc.c b/apps/px4/tests/test_adc.c
index c33af1311..c5960e757 100644
--- a/apps/px4/tests/test_adc.c
+++ b/apps/px4/tests/test_adc.c
@@ -127,18 +127,18 @@ int test_adc(int argc, char *argv[])
 
 				message("channel: %d value: %d\n",
 					(int)sample1.am_channel1, sample1.am_data1);
-				message("channel: %d value: %d",
+				message("channel: %d value: %d\n",
 					(int)sample1.am_channel2, sample1.am_data2);
-				message("channel: %d value: %d",
+				message("channel: %d value: %d\n",
 					(int)sample1.am_channel3, sample1.am_data3);
-				message("channel: %d value: %d",
+				message("channel: %d value: %d\n",
 					(int)sample1.am_channel4, sample1.am_data4);
 			}
 		}
 		fflush(stdout);
 	}
 
-	message("\t ADC test successful.");
+	message("\t ADC test successful.\n");
 
 errout_with_dev:
 
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/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index 7c1503f0d..eea51cc1e 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -870,7 +870,12 @@ Sensors::ppm_poll()
 	/* we are accepting this message */
 	_ppm_last_valid = ppm_last_valid_decode;
 
-	if (ppm_decoded_channels > 2 && hrt_absolute_time() - _ppm_last_valid < PPM_INPUT_TIMEOUT_INTERVAL) {
+	/*
+	 * relying on two decoded channels is very noise-prone,
+	 * in particular if nothing is connected to the pins.
+	 * requiring a minimum of four channels
+	 */
+	if (ppm_decoded_channels > 4 && hrt_absolute_time() - _ppm_last_valid < PPM_INPUT_TIMEOUT_INTERVAL) {
 
 		for (int i = 0; i < ppm_decoded_channels; i++) {
 			raw.values[i] = ppm_buffer[i];
@@ -898,8 +903,8 @@ Sensors::ppm_poll()
 
 		struct manual_control_setpoint_s manual_control;
 
-		/* require at least two chanels to consider the signal valid */
-		if (rc_input.channel_count < 2)
+		/* require at least four channels to consider the signal valid */
+		if (rc_input.channel_count < 4)
 			return;
 
 		unsigned channel_limit = rc_input.channel_count;
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/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/nuttx/arch/arm/src/stm32/stm32_adc.c b/nuttx/arch/arm/src/stm32/stm32_adc.c
index a45b732ae..844f1fd0f 100644
--- a/nuttx/arch/arm/src/stm32/stm32_adc.c
+++ b/nuttx/arch/arm/src/stm32/stm32_adc.c
@@ -1427,15 +1427,12 @@ static void adc_shutdown(FAR struct adc_dev_s *dev)
   /* power down ADC */
   adc_enable(priv, false);
 
-  adc_rxint(priv, false);
+  adc_rxint(dev, false);
 
   /* Disable ADC interrupts and detach the ADC interrupt handler */
   up_disable_irq(priv->irq);
 //  irq_detach(priv->irq);
-
-
-
-
+  // XXX Why is irq_detach here commented out?
 }
 
 /****************************************************************************
diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig
index 798f57e93..b03ec1eb3 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
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;
             }