Merged in upstream master

50 files changed, 814 insertions, 513 deletions

diff --git a/.gitignore b/.gitignore
index 71326517f..5b345b34a 100644
--- a/.gitignore
+++ b/.gitignore
@@ -34,5 +34,6 @@ mavlink/include/mavlink/v0.9/
 /Documentation/html/
 /Documentation/doxygen*objdb*tmp
 .tags
+tags
 .tags_sorted_by_file
 .pydevproject
diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom
index 6cbd23643..24372bddc 100644
--- a/ROMFS/px4fmu_common/init.d/3031_phantom
+++ b/ROMFS/px4fmu_common/init.d/3031_phantom
@@ -10,8 +10,8 @@ sh /etc/init.d/rc.fw_defaults
 if [ $DO_AUTOCONFIG == yes ]
 then
 	param set FW_AIRSPD_MIN 13
-	param set FW_AIRSPD_TRIM 18
-	param set FW_AIRSPD_MAX 40
+	param set FW_AIRSPD_TRIM 15
+	param set FW_AIRSPD_MAX 25
 	param set FW_ATT_TC 0.3
 	param set FW_L1_DAMPING 0.75
 	param set FW_L1_PERIOD 15
@@ -23,12 +23,12 @@ then
 	param set FW_P_LIM_MIN -50
 	param set FW_P_RMAX_NEG 0
 	param set FW_P_RMAX_POS 0
-	param set FW_P_ROLLFF 0
+	param set FW_P_ROLLFF 1
 	param set FW_RR_FF 0.5
 	param set FW_RR_I 0.02
 	param set FW_RR_IMAX 0.2
 	param set FW_RR_P 0.08
-	param set FW_R_LIM 70
+	param set FW_R_LIM 50
 	param set FW_R_RMAX 0
 	param set FW_T_HRATE_P 0.01
 	param set FW_T_RLL2THR 15
diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5
index bf5a87068..465166f25 100644
--- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5
+++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5
@@ -23,7 +23,7 @@ then
 	param set FW_P_LIM_MIN -45
 	param set FW_P_RMAX_NEG 0
 	param set FW_P_RMAX_POS 0
-	param set FW_P_ROLLFF 0
+	param set FW_P_ROLLFF 1
 	param set FW_RR_FF 0.3
 	param set FW_RR_I 0
 	param set FW_RR_IMAX 0.2
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 2ab1e2e96..ea5cf8deb 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -20,7 +20,7 @@ echo "[init] Looking for microSD..."
 if mount -t vfat /dev/mmcsd0 /fs/microsd
 then
 	set LOG_FILE /fs/microsd/bootlog.txt
-	echo "[init] microSD card mounted at /fs/microsd"
+	echo "[init] microSD mounted: /fs/microsd"
 	# Start playing the startup tune
 	tone_alarm start
 else
@@ -83,9 +83,9 @@ then
 	param select $PARAM_FILE
 	if param load
 	then
-		echo "[init] Parameters loaded: $PARAM_FILE"
+		echo "[init] Params loaded: $PARAM_FILE"
 	else
-		echo "[init] ERROR: Parameters loading failed: $PARAM_FILE"
+		echo "[init] ERROR: Params loading failed: $PARAM_FILE"
 	fi
 	
 	#
@@ -146,7 +146,7 @@ then
 	#
 	if param compare SYS_AUTOSTART 0
 	then
-		echo "[init] Don't try to find autostart script"
+		echo "[init] No autostart"
 	else
 		sh /etc/init.d/rc.autostart
 	fi
@@ -156,10 +156,10 @@ then
 	#
 	if [ -f $CONFIG_FILE ]
 	then
-		echo "[init] Reading config: $CONFIG_FILE"
+		echo "[init] Config: $CONFIG_FILE"
 		sh $CONFIG_FILE
 	else
-		echo "[init] Config file not found: $CONFIG_FILE"
+		echo "[init] Config not found: $CONFIG_FILE"
 	fi
 
 	#
@@ -264,10 +264,10 @@ then
 		then
 			set FMU_MODE serial
 		fi
-	else
-		# Try to get an USB console if not in HIL mode
-		nshterm /dev/ttyACM0 &
 	fi
+
+	# Try to get an USB console
+	nshterm /dev/ttyACM0 &
 	
 	#
 	# Start the Commander (needs to be this early for in-air-restarts)
@@ -401,23 +401,17 @@ then
 	#
 	if [ $MAVLINK_FLAGS == default ]
 	then
-		if [ $HIL == yes ]
+		# Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s
+		if [ $TTYS1_BUSY == yes ]
 		then
-			sleep 1
-			set MAVLINK_FLAGS "-r 10000 -d /dev/ttyACM0"
+			# Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else
+			set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0"
+
+			# Exit from nsh to free port for mavlink
+			set EXIT_ON_END yes
 		else
-			# Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s
-			if [ $TTYS1_BUSY == yes ]
-			then
-				# Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else
-				set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0"
-				
-				# Exit from nsh to free port for mavlink
-				set EXIT_ON_END yes
-			else
-				# Start MAVLink on default port: ttyS1
-				set MAVLINK_FLAGS "-r 1000"
-			fi
+			# Start MAVLink on default port: ttyS1
+			set MAVLINK_FLAGS "-r 1000"
 		fi
 	fi
 
@@ -436,17 +430,15 @@ then
 	#
 	# Sensors, Logging, GPS
 	#
-	echo "[init] Start sensors"
 	sh /etc/init.d/rc.sensors
 
 	if [ $HIL == no ]
 	then
 		echo "[init] Start logging"
 		sh /etc/init.d/rc.logging
-		
-		echo "[init] Start GPS"
-		gps start
 	fi
+
+	gps start
 	
 	#
 	# Start up ARDrone Motor interface
@@ -561,7 +553,7 @@ then
 		echo "[init] Starting addons script: $EXTRAS_FILE"
 		sh $EXTRAS_FILE
 	else
-		echo "[init] Addons script not found: $EXTRAS_FILE"
+		echo "[init] No addons script: $EXTRAS_FILE"
 	fi
 
 	if [ $EXIT_ON_END == yes ]
diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk
index bb729e103..b519e0e7a 100644
--- a/makefiles/toolchain_gnu-arm-eabi.mk
+++ b/makefiles/toolchain_gnu-arm-eabi.mk
@@ -1,5 +1,5 @@
 #
-#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#   Copyright (C) 2012-2014 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -125,7 +125,11 @@ ARCHWARNINGS		 = -Wall \
 			   -Wlogical-op \
 			   -Wmissing-declarations \
 			   -Wpacked \
-			   -Wno-unused-parameter
+			   -Wno-unused-parameter \
+			   -Werror=format-security \
+			   -Werror=array-bounds \
+			   -Wfatal-errors \
+			   -Wformat=1
 #   -Wcast-qual  - generates spurious noreturn attribute warnings, try again later
 #   -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code
 #   -Wcast-align - would help catch bad casts in some cases, but generates too many false positives
@@ -142,7 +146,8 @@ ARCHCWARNINGS		 = $(ARCHWARNINGS) \
 
 # C++-specific warnings
 #
-ARCHWARNINGSXX		 = $(ARCHWARNINGS)
+ARCHWARNINGSXX		 = $(ARCHWARNINGS) \
+			   -Wno-missing-field-initializers
 
 # pull in *just* libm from the toolchain ... this is grody
 LIBM			:= $(shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a)
diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig
index 00bf83bd5..127418f1f 100644
--- a/nuttx-configs/px4fmu-v1/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v1/nsh/defconfig
@@ -405,7 +405,7 @@ CONFIG_SIG_SIGWORK=4
 CONFIG_MAX_TASKS=32
 CONFIG_MAX_TASK_ARGS=10
 CONFIG_NPTHREAD_KEYS=4
-CONFIG_NFILE_DESCRIPTORS=36
+CONFIG_NFILE_DESCRIPTORS=38
 CONFIG_NFILE_STREAMS=8
 CONFIG_NAME_MAX=32
 CONFIG_PREALLOC_MQ_MSGS=4
diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig
index 9c75e6c59..a982040c6 100644
--- a/nuttx-configs/px4fmu-v2/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v2/nsh/defconfig
@@ -439,7 +439,7 @@ CONFIG_SIG_SIGWORK=4
 CONFIG_MAX_TASKS=32
 CONFIG_MAX_TASK_ARGS=10
 CONFIG_NPTHREAD_KEYS=4
-CONFIG_NFILE_DESCRIPTORS=36
+CONFIG_NFILE_DESCRIPTORS=38
 CONFIG_NFILE_STREAMS=8
 CONFIG_NAME_MAX=32
 CONFIG_PREALLOC_MQ_MSGS=4
diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp
index 524151c90..41942aacd 100644
--- a/src/drivers/airspeed/airspeed.cpp
+++ b/src/drivers/airspeed/airspeed.cpp
@@ -82,10 +82,12 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const cha
 	_buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")),
 	_max_differential_pressure_pa(0),
 	_sensor_ok(false),
+	_last_published_sensor_ok(true), /* initialize differently to force publication */
 	_measure_ticks(0),
 	_collect_phase(false),
 	_diff_pres_offset(0.0f),
 	_airspeed_pub(-1),
+	_subsys_pub(-1),
 	_class_instance(-1),
 	_conversion_interval(conversion_interval),
 	_sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")),
@@ -149,8 +151,7 @@ Airspeed::init()
 	}
 
 	ret = OK;
-	/* sensor is ok, but we don't really know if it is within range */
-	_sensor_ok = true;
+
 out:
 	return ret;
 }
@@ -344,22 +345,6 @@ Airspeed::start()
 
 	/* schedule a cycle to start things */
 	work_queue(HPWORK, &_work, (worker_t)&Airspeed::cycle_trampoline, this, 1);
-
-	/* notify about state change */
-	struct subsystem_info_s info = {
-		true,
-		true,
-		true,
-		SUBSYSTEM_TYPE_DIFFPRESSURE
-	};
-	static orb_advert_t pub = -1;
-
-	if (pub > 0) {
-		orb_publish(ORB_ID(subsystem_info), pub, &info);
-
-	} else {
-		pub = orb_advertise(ORB_ID(subsystem_info), &info);
-	}
 }
 
 void
@@ -369,11 +354,34 @@ Airspeed::stop()
 }
 
 void
+Airspeed::update_status()
+{
+	if (_sensor_ok != _last_published_sensor_ok) {
+		/* notify about state change */
+		struct subsystem_info_s info = {
+			true,
+			true,
+			_sensor_ok,
+			SUBSYSTEM_TYPE_DIFFPRESSURE
+		};
+
+		if (_subsys_pub > 0) {
+			orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info);
+		} else {
+			_subsys_pub = orb_advertise(ORB_ID(subsystem_info), &info);
+		}
+
+		_last_published_sensor_ok = _sensor_ok;
+	}
+}
+
+void
 Airspeed::cycle_trampoline(void *arg)
 {
 	Airspeed *dev = (Airspeed *)arg;
 
 	dev->cycle();
+	dev->update_status();
 }
 
 void
diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h
index 186602eda..0b8e949c9 100644
--- a/src/drivers/airspeed/airspeed.h
+++ b/src/drivers/airspeed/airspeed.h
@@ -118,14 +118,21 @@ protected:
 	virtual int	measure() = 0;
 	virtual int	collect() = 0;
 
+	/**
+	 * Update the subsystem status
+	 */
+	void update_status();
+
 	work_s			_work;
 	float			_max_differential_pressure_pa;
 	bool			_sensor_ok;
+	bool			_last_published_sensor_ok;
 	int			_measure_ticks;
 	bool			_collect_phase;
 	float			_diff_pres_offset;
 
 	orb_advert_t		_airspeed_pub;
+	orb_advert_t		_subsys_pub;
 
 	int			_class_instance;
 
diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h
index cf91f7030..e45939b37 100644
--- a/src/drivers/drv_range_finder.h
+++ b/src/drivers/drv_range_finder.h
@@ -46,6 +46,10 @@
 
 #define RANGE_FINDER_DEVICE_PATH	"/dev/range_finder"
 
+enum RANGE_FINDER_TYPE {
+	RANGE_FINDER_TYPE_LASER = 0,
+};
+
 /**
  * range finder report structure.  Reads from the device must be in multiples of this
  * structure.
@@ -53,8 +57,11 @@
 struct range_finder_report {
 	uint64_t timestamp;
 	uint64_t error_count;
-	float distance; 			/** in meters */
-	uint8_t valid;				/** 1 == within sensor range, 0 = outside sensor range */
+	unsigned type;				/**< type, following RANGE_FINDER_TYPE enum */
+	float distance; 			/**< in meters */
+	float minimum_distance;			/**< minimum distance the sensor can measure */
+	float maximum_distance;			/**< maximum distance the sensor can measure */
+	uint8_t valid;				/**< 1 == within sensor range, 0 = outside sensor range */
 };
 
 /*
diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp
index d873a1132..146a06e7c 100644
--- a/src/drivers/ets_airspeed/ets_airspeed.cpp
+++ b/src/drivers/ets_airspeed/ets_airspeed.cpp
@@ -207,14 +207,18 @@ ETSAirspeed::collect()
 void
 ETSAirspeed::cycle()
 {
+	int ret;
+
 	/* collection phase? */
 	if (_collect_phase) {
 
 		/* perform collection */
-		if (OK != collect()) {
+		ret = collect();
+		if (OK != ret) {
 			perf_count(_comms_errors);
 			/* restart the measurement state machine */
 			start();
+			_sensor_ok = false;
 			return;
 		}
 
@@ -238,8 +242,12 @@ ETSAirspeed::cycle()
 	}
 
 	/* measurement phase */
-	if (OK != measure())
-		log("measure error");
+	ret = measure();
+	if (OK != ret) {
+		debug("measure error");
+	}
+
+	_sensor_ok = (ret == OK);
 
 	/* next phase is collection */
 	_collect_phase = true;
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index cb97354ec..cdc9d3106 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -715,7 +715,7 @@ HMC5883::cycle()
 
 		/* perform collection */
 		if (OK != collect()) {
-			log("collection error");
+			debug("collection error");
 			/* restart the measurement state machine */
 			start();
 			return;
@@ -742,7 +742,7 @@ HMC5883::cycle()
 
 	/* measurement phase */
 	if (OK != measure())
-		log("measure error");
+		debug("measure error");
 
 	/* next phase is collection */
 	_collect_phase = true;
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index 1ad383ee0..c0f3c28e0 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -288,13 +288,17 @@ MEASAirspeed::collect()
 void
 MEASAirspeed::cycle()
 {
+	int ret;
+
 	/* collection phase? */
 	if (_collect_phase) {
 
 		/* perform collection */
-		if (OK != collect()) {
+		ret = collect();
+		if (OK != ret) {
 			/* restart the measurement state machine */
 			start();
+			_sensor_ok = false;
 			return;
 		}
 
@@ -318,10 +322,13 @@ MEASAirspeed::cycle()
 	}
 
 	/* measurement phase */
-	if (OK != measure()) {
-		log("measure error");
+	ret = measure();
+	if (OK != ret) {
+		debug("measure error");
 	}
 
+	_sensor_ok = (ret == OK);
+
 	/* next phase is collection */
 	_collect_phase = true;
 
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index 705e98eea..3cb9abc49 100644
--- a/src/drivers/mkblctrl/mkblctrl.cpp
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -1015,7 +1015,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 
 	case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_actuators - 1):
 		/* copy the current output value from the channel */
-		*(servo_position_t *)arg = Motor[cmd - PWM_SERVO_SET(0)].RawPwmValue;
+		*(servo_position_t *)arg = Motor[cmd - PWM_SERVO_GET(0)].RawPwmValue;
 
 		break;
 
diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp
index 12f80c4a3..c555a0a69 100644
--- a/src/lib/launchdetection/CatapultLaunchMethod.cpp
+++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -10,9 +10,9 @@
  *    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 documentation4 and/or other materials provided with the
+ *    the documentation and/or other materials provided with the
  *    distribution.
- * 3. Neither the name ECL nor the names of its contributors may be
+ * 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.
  *
@@ -33,9 +33,10 @@
 
 /**
  * @file CatapultLaunchMethod.cpp
- * Catpult Launch detection
+ * Catapult Launch detection
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  *
- * Authors and acknowledgements in header.
  */
 
 #include "CatapultLaunchMethod.h"
diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h
index 55c46ff3f..23757f6f3 100644
--- a/src/lib/launchdetection/CatapultLaunchMethod.h
+++ b/src/lib/launchdetection/CatapultLaunchMethod.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -10,9 +10,9 @@
  *    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 documentation4 and/or other materials provided with the
+ *    the documentation and/or other materials provided with the
  *    distribution.
- * 3. Neither the name ECL nor the names of its contributors may be
+ * 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.
  *
diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp
index bf539701b..3bf47bbb0 100644
--- a/src/lib/launchdetection/LaunchDetector.cpp
+++ b/src/lib/launchdetection/LaunchDetector.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -12,7 +12,7 @@
  *    notice, this list of conditions and the following disclaimer in
  *    the documentation and/or other materials provided with the
  *    distribution.
- * 3. Neither the name ECL nor the names of its contributors may be
+ * 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.
  *
@@ -30,12 +30,11 @@
  * POSSIBILITY OF SUCH DAMAGE.
  *
  ****************************************************************************/
-
 /**
  * @file launchDetection.cpp
  * Auto Detection for different launch methods (e.g. catapult)
  *
- * Authors and acknowledgements in header.
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
 #include "LaunchDetector.h"
diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h
index 8066ebab3..1a214b66e 100644
--- a/src/lib/launchdetection/LaunchDetector.h
+++ b/src/lib/launchdetection/LaunchDetector.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -10,9 +10,9 @@
  *    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 documentation4 and/or other materials provided with the
+ *    the documentation and/or other materials provided with the
  *    distribution.
- * 3. Neither the name ECL nor the names of its contributors may be
+ * 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.
  *
diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h
index 01fa7050e..e04467f6a 100644
--- a/src/lib/launchdetection/LaunchMethod.h
+++ b/src/lib/launchdetection/LaunchMethod.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -10,9 +10,9 @@
  *    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 documentation4 and/or other materials provided with the
+ *    the documentation and/or other materials provided with the
  *    distribution.
- * 3. Neither the name ECL nor the names of its contributors may be
+ * 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.
  *
diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c
index 45d7957f1..8df8c696c 100644
--- a/src/lib/launchdetection/launchdetection_params.c
+++ b/src/lib/launchdetection/launchdetection_params.c
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -18,7 +17,7 @@
  *    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
+ * "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,
diff --git a/src/lib/launchdetection/module.mk b/src/lib/launchdetection/module.mk
index 13648b74c..de0174e37 100644
--- a/src/lib/launchdetection/module.mk
+++ b/src/lib/launchdetection/module.mk
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
+#   Copyright (c) 2012, 2013, 2014 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
index 4154e3db4..3ff3d9922 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
@@ -40,6 +40,7 @@
  */
 
 #include "attitude_estimator_ekf_params.h"
+#include <math.h>
 
 /* Extended Kalman Filter covariances */
 
@@ -113,6 +114,7 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru
 	param_get(h->yaw_off, &(p->yaw_off));
 
 	param_get(h->mag_decl, &(p->mag_decl));
+	p->mag_decl *= M_PI / 180.0f;
 
 	param_get(h->acc_comp, &(p->acc_comp));
 
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 789e0f63e..d5f05c991 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -117,7 +117,7 @@ extern struct system_load_s system_load;
 #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
 
-#define POSITION_TIMEOUT 30000 /**< consider the local or global position estimate invalid after 30ms */
+#define POSITION_TIMEOUT 100000 /**< consider the local or global position estimate invalid after 100ms */
 #define RC_TIMEOUT 500000
 #define DIFFPRESS_TIMEOUT 2000000
 
@@ -137,7 +137,7 @@ enum MAV_MODE_FLAG {
 };
 
 /* Mavlink file descriptors */
-static int mavlink_fd;
+static int mavlink_fd = 0;
 
 /* flags */
 static bool commander_initialized = false;
@@ -218,11 +218,10 @@ void print_reject_arm(const char *msg);
 
 void print_status();
 
-int arm();
-int disarm();
-
 transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
 
+transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy);
+
 /**
  * Loop that runs at a lower rate and priority for calibration and parameter tasks.
  */
@@ -289,12 +288,12 @@ int commander_main(int argc, char *argv[])
 	}
 
 	if (!strcmp(argv[1], "arm")) {
-		arm();
+		arm_disarm(true, mavlink_fd, "command line");
 		exit(0);
 	}
 
-	if (!strcmp(argv[1], "disarm")) {
-		disarm();
+	if (!strcmp(argv[1], "2")) {
+		arm_disarm(false, mavlink_fd, "command line");
 		exit(0);
 	}
 
@@ -364,30 +363,20 @@ void print_status()
 
 static orb_advert_t status_pub;
 
-int arm()
+transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy)
 {
-	int arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
-
-	if (arming_res == TRANSITION_CHANGED) {
-		mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
-		return 0;
-
-	} else {
-		return 1;
-	}
-}
-
-int disarm()
-{
-	int arming_res = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
-
-	if (arming_res == TRANSITION_CHANGED) {
-		mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
-		return 0;
-
-	} else {
-		return 1;
-	}
+    transition_result_t arming_res = TRANSITION_NOT_CHANGED;
+    
+    // Transition the armed state. By passing mavlink_fd to arming_state_transition it will
+    // output appropriate error messages if the state cannot transition.
+    arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd);
+    if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
+        mavlink_log_info(mavlink_fd, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
+    } else if (arming_res == TRANSITION_DENIED) {
+        tune_negative(true);
+    }
+    
+    return arming_res;
 }
 
 bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub)
@@ -430,37 +419,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			if (hil_ret == OK)
 				ret = true;
 
-			// TODO remove debug code
-			//mavlink_log_critical(mavlink_fd, "#audio: command setmode: %d %d", base_mode, custom_main_mode);
-			/* set arming state */
-			arming_res = TRANSITION_NOT_CHANGED;
-
-			if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
-				if (safety->safety_switch_available && !safety->safety_off && status->hil_state == HIL_STATE_OFF) {
-					print_reject_arm("NOT ARMING: Press safety switch first.");
-					arming_res = TRANSITION_DENIED;
-
-				} else {
-					arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
-				}
-
-				if (arming_res == TRANSITION_CHANGED) {
-					mavlink_log_info(mavlink_fd, "[cmd] ARMED by command");
-				}
-
-			} else {
-				if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) {
-					arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
-					arming_res = arming_state_transition(status, safety, new_arming_state, armed);
-
-					if (arming_res == TRANSITION_CHANGED) {
-						mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command");
-					}
-
-				} else {
-					arming_res = TRANSITION_NOT_CHANGED;
-				}
-			}
+            // Transition the arming state
+            arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
 
 			if (arming_res == TRANSITION_CHANGED)
 				ret = true;
@@ -519,27 +479,19 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		}
 
 	case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
-			transition_result_t arming_res = TRANSITION_NOT_CHANGED;
-
-			if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) {
-				if (safety->safety_switch_available && !safety->safety_off && status->hil_state == HIL_STATE_OFF) {
-					print_reject_arm("NOT ARMING: Press safety switch first.");
-					arming_res = TRANSITION_DENIED;
-
-				} else {
-					arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
-				}
-
-				if (arming_res == TRANSITION_CHANGED) {
-					mavlink_log_critical(mavlink_fd, "#audio: ARMED by component arm cmd");
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-					ret = true;
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
-					result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
-				}
-			}
+            // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm.
+            // We use an float epsilon delta to test float equality.
+            if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) {
+				mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", cmd->param1);
+            } else {
+                transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command");
+                if (arming_res == TRANSITION_DENIED) {
+                    mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
+                    result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+                } else {
+                    result = VEHICLE_CMD_RESULT_ACCEPTED;
+                }
+            }
 		}
 		break;
 
@@ -1057,7 +1009,7 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(battery_status), battery_sub, &battery);
 
 			/* only consider battery voltage if system has been running 2s and battery voltage is valid */
-			if (status.hil_state == HIL_STATE_OFF && hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
+			if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
 				status.battery_voltage = battery.voltage_filtered_v;
 				status.battery_current = battery.current_a;
 				status.condition_battery_voltage_valid = true;
@@ -1100,7 +1052,7 @@ int commander_thread_main(int argc, char *argv[])
 			status_changed = true;
 		}
 
-		/* update subsystem */
+		/* update position setpoint triplet */
 		orb_check(pos_sp_triplet_sub, &updated);
 
 		if (updated) {
@@ -1276,7 +1228,11 @@ int commander_thread_main(int argc, char *argv[])
 				status.set_nav_state_timestamp = hrt_absolute_time();
 
 			} else {
+<<<<<<< HEAD
 				/* LOITER switch */
+=======
+				/* MISSION switch */
+>>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863
 				if (sp_man.loiter_switch == SWITCH_POS_ON) {
 					/* stick is in LOITER position */
 					status.set_nav_state = NAV_STATE_LOITER;
@@ -1319,10 +1275,15 @@ int commander_thread_main(int argc, char *argv[])
 
 				} else {
 					/* failsafe for manual modes */
-					transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
+					transition_result_t res = TRANSITION_DENIED;
+
+					if (!status.condition_landed) {
+						/* vehicle is not landed, try to perform RTL */
+						res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
+					}
 
 					if (res == TRANSITION_DENIED) {
-						/* RTL not allowed (no global position estimate), try LAND */
+						/* RTL not allowed (no global position estimate) or not wanted, try LAND */
 						res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
 
 						if (res == TRANSITION_DENIED) {
@@ -1742,8 +1703,11 @@ set_control_mode()
 		control_mode.flag_control_auto_enabled = true;
 		control_mode.flag_control_rates_enabled = true;
 		control_mode.flag_control_attitude_enabled = true;
-		control_mode.flag_control_position_enabled = true;
-		control_mode.flag_control_velocity_enabled = true;
+
+		/* in failsafe LAND mode position may be not available */
+		control_mode.flag_control_position_enabled = status.condition_local_position_valid;
+		control_mode.flag_control_velocity_enabled = status.condition_local_position_valid;
+
 		control_mode.flag_control_altitude_enabled = true;
 		control_mode.flag_control_climb_rate_enabled = true;
 	}
diff --git a/src/modules/commander/commander_tests/commander_tests.cpp b/src/modules/commander/commander_tests/commander_tests.cpp
index 6e72cf0d9..0abb84a82 100644
--- a/src/modules/commander/commander_tests/commander_tests.cpp
+++ b/src/modules/commander/commander_tests/commander_tests.cpp
@@ -48,8 +48,7 @@ extern "C" __EXPORT int commander_tests_main(int argc, char *argv[]);
 
 int commander_tests_main(int argc, char *argv[])
 {
-	state_machine_helper_test();
-	//state_machine_test();
+	stateMachineHelperTest();
 
 	return 0;
 }
diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
index 40bedd9f3..8a946543d 100644
--- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
@@ -49,13 +49,12 @@ public:
 	StateMachineHelperTest();
 	virtual ~StateMachineHelperTest();
 
-	virtual const char*	run_tests();
+	virtual void runTests(void);
 
 private:
-	const char*	arming_state_transition_test();
-	const char*	arming_state_transition_arm_disarm_test();
-	const char*	main_state_transition_test();
-	const char*	is_safe_test();
+	bool armingStateTransitionTest();
+	bool mainStateTransitionTest();
+	bool isSafeTest();
 };
 
 StateMachineHelperTest::StateMachineHelperTest() {
@@ -64,61 +63,242 @@ StateMachineHelperTest::StateMachineHelperTest() {
 StateMachineHelperTest::~StateMachineHelperTest() {
 }
 
-const char*
-StateMachineHelperTest::arming_state_transition_test()
+bool StateMachineHelperTest::armingStateTransitionTest(void)
 {
+    // These are the critical values from vehicle_status_s and actuator_armed_s which must be primed
+    // to simulate machine state prior to testing an arming state transition. This structure is also
+    // use to represent the expected machine state after the transition has been requested.
+    typedef struct {
+        arming_state_t  arming_state;   // vehicle_status_s.arming_state
+        bool            armed;          // actuator_armed_s.armed
+        bool            ready_to_arm;   // actuator_armed_s.ready_to_arm
+    } ArmingTransitionVolatileState_t;
+    
+    // This structure represents a test case for arming_state_transition. It contains the machine
+    // state prior to transtion, the requested state to transition to and finally the expected
+    // machine state after transition.
+    typedef struct {
+        const char*                     assertMsg;                              // Text to show when test case fails
+        ArmingTransitionVolatileState_t current_state;                          // Machine state prior to transtion
+        hil_state_t                     hil_state;                              // Current vehicle_status_s.hil_state
+        bool                            condition_system_sensors_initialized;   // Current vehicle_status_s.condition_system_sensors_initialized
+        bool                            safety_switch_available;                // Current safety_s.safety_switch_available
+        bool                            safety_off;                             // Current safety_s.safety_off
+        arming_state_t                  requested_state;                        // Requested arming state to transition to
+        ArmingTransitionVolatileState_t expected_state;                         // Expected machine state after transition
+        transition_result_t             expected_transition_result;             // Expected result from arming_state_transition
+    } ArmingTransitionTest_t;
+    
+    // We use these defines so that our test cases are more readable
+    #define ATT_ARMED true
+    #define ATT_DISARMED false
+    #define ATT_READY_TO_ARM true
+    #define ATT_NOT_READY_TO_ARM false
+    #define ATT_SENSORS_INITIALIZED true
+    #define ATT_SENSORS_NOT_INITIALIZED false
+    #define ATT_SAFETY_AVAILABLE true
+    #define ATT_SAFETY_NOT_AVAILABLE true
+    #define ATT_SAFETY_OFF true
+    #define ATT_SAFETY_ON false
+    
+    // These are test cases for arming_state_transition
+    static const ArmingTransitionTest_t rgArmingTransitionTests[] = {
+        // TRANSITION_NOT_CHANGED tests
+        
+        { "no transition: identical states",
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_INIT,
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_NOT_CHANGED },
+        
+        // TRANSITION_CHANGED tests
+        
+        // Check all basic valid transitions, these don't require special state in vehicle_status_t or safety_s
+        
+        { "transition: init to standby",
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY,
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: init to standby error",
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY_ERROR,
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: init to reboot",
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_REBOOT,
+            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: standby to init",
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_INIT,
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: standby to standby error",
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY_ERROR,
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: standby to reboot",
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_REBOOT,
+            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: armed to standby",
+            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY,
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: armed to armed error",
+            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_ARMED_ERROR,
+            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: armed error to standby error",
+            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY_ERROR,
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: standby error to reboot",
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_REBOOT,
+            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: in air restore to armed",
+            { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_ARMED,
+            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: in air restore to reboot",
+            { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_REBOOT,
+            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        // hil on tests, standby error to standby not normally allowed
+        
+        { "transition: standby error to standby, hil on",
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY,
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        // Safety switch arming tests
+        
+        { "transition: init to standby, no safety switch",
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF,
+            ARMING_STATE_ARMED,
+            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        { "transition: init to standby, safety switch off",
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF,
+            ARMING_STATE_ARMED,
+            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        // standby error
+        { "transition: armed error to standby error requested standby",
+            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY,
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
+        
+        // TRANSITION_DENIED tests
+        
+        // Check some important basic invalid transitions, these don't require special state in vehicle_status_t or safety_s
+        
+        { "no transition: init to armed",
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_ARMED,
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        { "no transition: standby to armed error",
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_ARMED_ERROR,
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        { "no transition: armed to init",
+            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_INIT,
+            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        { "no transition: armed to reboot",
+            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_REBOOT,
+            { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        { "no transition: armed error to armed",
+            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_ARMED,
+            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        { "no transition: armed error to reboot",
+            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_REBOOT,
+            { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        { "no transition: standby error to armed",
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_ARMED,
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        { "no transition: standby error to standby",
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY,
+            { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        { "no transition: reboot to armed",
+            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_ARMED,
+            { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        { "no transition: in air restore to standby",
+            { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY,
+            { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        // Sensor tests
+        
+        { "no transition: init to standby - sensors not initialized",
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_STANDBY,
+            { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
+        
+        // Safety switch arming tests
+        
+        { "no transition: init to standby, safety switch on",
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
+            ARMING_STATE_ARMED,
+            { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
+    };
+    
 	struct vehicle_status_s status;
-	struct safety_s safety;
-	arming_state_t new_arming_state;
-	struct actuator_armed_s armed;
-
-	// Identical states.
-	status.arming_state = ARMING_STATE_INIT;
-	new_arming_state = ARMING_STATE_INIT;
-	mu_assert("no transition: identical states",
-		  TRANSITION_NOT_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed));
-
-	// INIT to STANDBY.
-	armed.armed = false;
-	armed.ready_to_arm = false;
-	status.arming_state = ARMING_STATE_INIT;
-	status.condition_system_sensors_initialized = true;
-	new_arming_state = ARMING_STATE_STANDBY;
-	mu_assert("transition: init to standby",
-		  TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed));
-	mu_assert("current state: standby", ARMING_STATE_STANDBY == status.arming_state);
-	mu_assert("not armed", !armed.armed);
-	mu_assert("ready to arm", armed.ready_to_arm);
-
-	// INIT to STANDBY, sensors not initialized.
-	armed.armed = false;
-	armed.ready_to_arm = false;
-	status.arming_state = ARMING_STATE_INIT;
-	status.condition_system_sensors_initialized = false;
-	new_arming_state = ARMING_STATE_STANDBY;
-	mu_assert("no transition: sensors not initialized",
-		  TRANSITION_DENIED == arming_state_transition(&status, &safety, new_arming_state, &armed));
-	mu_assert("current state: init", ARMING_STATE_INIT == status.arming_state);
-	mu_assert("not armed", !armed.armed);
-	mu_assert("not ready to arm", !armed.ready_to_arm);
-
-	return 0;
-}
-
-const char*
-StateMachineHelperTest::arming_state_transition_arm_disarm_test()
-{
-	struct vehicle_status_s status;
-	struct safety_s safety;
-	arming_state_t new_arming_state;
+	struct safety_s         safety;
 	struct actuator_armed_s armed;
-
-	// TODO(sjwilks): ARM then DISARM.
-	return 0;
+    
+    size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]);
+    for (size_t i=0; i<cArmingTransitionTests; i++) {
+        const ArmingTransitionTest_t* test = &rgArmingTransitionTests[i];
+        
+        // Setup initial machine state
+        status.arming_state = test->current_state.arming_state;
+        status.condition_system_sensors_initialized = test->condition_system_sensors_initialized;
+        status.hil_state = test->hil_state;
+        safety.safety_switch_available = test->safety_switch_available;
+        safety.safety_off = test->safety_off;
+        armed.armed = test->current_state.armed;
+        armed.ready_to_arm = test->current_state.ready_to_arm;
+        
+        // Attempt transition
+        transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed);
+        
+        // Validate result of transition
+        ut_assert(test->assertMsg, test->expected_transition_result == result);
+        ut_assert(test->assertMsg, status.arming_state == test->expected_state.arming_state);
+        ut_assert(test->assertMsg, armed.armed == test->expected_state.armed);
+        ut_assert(test->assertMsg, armed.ready_to_arm == test->expected_state.ready_to_arm);
+    }
+
+	return true;
 }
 
-const char*
-StateMachineHelperTest::main_state_transition_test()
+bool StateMachineHelperTest::mainStateTransitionTest(void)
 {
 	struct vehicle_status_s current_state;
 	main_state_t new_main_state;
@@ -126,70 +306,69 @@ StateMachineHelperTest::main_state_transition_test()
 	// Identical states.
 	current_state.main_state = MAIN_STATE_MANUAL;
 	new_main_state = MAIN_STATE_MANUAL;
-	mu_assert("no transition: identical states",
+	ut_assert("no transition: identical states",
 		  TRANSITION_NOT_CHANGED == main_state_transition(&current_state, new_main_state));
-	mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);	
+	ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);	
 
 	// AUTO to MANUAL.
 	current_state.main_state = MAIN_STATE_AUTO;
 	new_main_state = MAIN_STATE_MANUAL;
-	mu_assert("transition changed: auto to manual",
+	ut_assert("transition changed: auto to manual",
 		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
-	mu_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state);
+	ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state);
 
 	// MANUAL to SEATBELT.
 	current_state.main_state = MAIN_STATE_MANUAL;
 	current_state.condition_local_altitude_valid = true;
 	new_main_state = MAIN_STATE_SEATBELT;
-	mu_assert("tranisition: manual to seatbelt", 
+	ut_assert("tranisition: manual to seatbelt", 
 		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
-	mu_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state);
+	ut_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state);
 
 	// MANUAL to SEATBELT, invalid local altitude.
 	current_state.main_state = MAIN_STATE_MANUAL;
 	current_state.condition_local_altitude_valid = false;
 	new_main_state = MAIN_STATE_SEATBELT;
-	mu_assert("no transition: invalid local altitude",
+	ut_assert("no transition: invalid local altitude",
 		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
-	mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
+	ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
 
 	// MANUAL to EASY.
 	current_state.main_state = MAIN_STATE_MANUAL;
 	current_state.condition_local_position_valid = true;
 	new_main_state = MAIN_STATE_EASY;
-	mu_assert("transition: manual to easy",
+	ut_assert("transition: manual to easy",
 		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
-	mu_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state);
+	ut_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state);
 
 	// MANUAL to EASY, invalid local position.
 	current_state.main_state = MAIN_STATE_MANUAL;
 	current_state.condition_local_position_valid = false;
 	new_main_state = MAIN_STATE_EASY;
-	mu_assert("no transition: invalid position",
+	ut_assert("no transition: invalid position",
 		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
-	mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
+	ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
 
 	// MANUAL to AUTO.
 	current_state.main_state = MAIN_STATE_MANUAL;
 	current_state.condition_global_position_valid = true;
 	new_main_state = MAIN_STATE_AUTO;
-	mu_assert("transition: manual to auto",
+	ut_assert("transition: manual to auto",
 		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
-	mu_assert("current state: auto", MAIN_STATE_AUTO == current_state.main_state);
+	ut_assert("current state: auto", MAIN_STATE_AUTO == current_state.main_state);
 
 	// MANUAL to AUTO, invalid global position.
 	current_state.main_state = MAIN_STATE_MANUAL;
 	current_state.condition_global_position_valid = false;
 	new_main_state = MAIN_STATE_AUTO;
-	mu_assert("no transition: invalid global position",
+	ut_assert("no transition: invalid global position",
 		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
-	mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
+	ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
 
-	return 0;
+	return true;
 }
 
-const char*
-StateMachineHelperTest::is_safe_test()
+bool StateMachineHelperTest::isSafeTest(void)
 {
 	struct vehicle_status_s current_state;
 	struct safety_s safety;
@@ -199,49 +378,45 @@ StateMachineHelperTest::is_safe_test()
 	armed.lockdown = false;
 	safety.safety_switch_available = true;
 	safety.safety_off = false;
-	mu_assert("is safe: not armed", is_safe(&current_state, &safety, &armed));
+	ut_assert("is safe: not armed", is_safe(&current_state, &safety, &armed));
 
 	armed.armed = false;
 	armed.lockdown = true;
 	safety.safety_switch_available = true;
 	safety.safety_off = true;
-	mu_assert("is safe: software lockdown", is_safe(&current_state, &safety, &armed));
+	ut_assert("is safe: software lockdown", is_safe(&current_state, &safety, &armed));
 
 	armed.armed = true;
 	armed.lockdown = false;
 	safety.safety_switch_available = true;
 	safety.safety_off = true;
-	mu_assert("not safe: safety off", !is_safe(&current_state, &safety, &armed));
+	ut_assert("not safe: safety off", !is_safe(&current_state, &safety, &armed));
 
 	armed.armed = true;
 	armed.lockdown = false;
 	safety.safety_switch_available = true;
 	safety.safety_off = false;
-	mu_assert("is safe: safety off", is_safe(&current_state, &safety, &armed));
+	ut_assert("is safe: safety off", is_safe(&current_state, &safety, &armed));
 
 	armed.armed = true;
 	armed.lockdown = false;
 	safety.safety_switch_available = false;
 	safety.safety_off = false;
-	mu_assert("not safe: no safety switch", !is_safe(&current_state, &safety, &armed));
+	ut_assert("not safe: no safety switch", !is_safe(&current_state, &safety, &armed));
 
-	return 0;
+	return true;
 }
 
-const char*
-StateMachineHelperTest::run_tests()
+void StateMachineHelperTest::runTests(void)
 {
-	mu_run_test(arming_state_transition_test);
-	mu_run_test(arming_state_transition_arm_disarm_test);
-	mu_run_test(main_state_transition_test);
-	mu_run_test(is_safe_test);
-
-	return 0;
+	ut_run_test(armingStateTransitionTest);
+	ut_run_test(mainStateTransitionTest);
+	ut_run_test(isSafeTest);
 }
 
-void
-state_machine_helper_test()
+void stateMachineHelperTest(void)
 {
 	StateMachineHelperTest* test = new StateMachineHelperTest();
-	test->UnitTest::print_results(test->run_tests());
+    test->runTests();
+	test->printResults();
 }
diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.h b/src/modules/commander/commander_tests/state_machine_helper_test.h
index 10a68e602..bbf66255e 100644
--- a/src/modules/commander/commander_tests/state_machine_helper_test.h
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.h
@@ -39,6 +39,6 @@
 #ifndef STATE_MACHINE_HELPER_TEST_H_
 #define STATE_MACHINE_HELPER_TEST_
 
-void state_machine_helper_test();
+void stateMachineHelperTest(void);
 
 #endif /* STATE_MACHINE_HELPER_TEST_H_ */
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index e6f245cf9..f09d586c7 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -69,10 +69,44 @@ static bool arming_state_changed = true;
 static bool main_state_changed = true;
 static bool failsafe_state_changed = true;
 
+// This array defines the arming state transitions. The rows are the new state, and the columns
+// are the current state. Using new state and current  state you can index into the array which
+// will be true for a valid transition or false for a invalid transition. In some cases even
+// though the transition is marked as true additional checks must be made. See arming_state_transition
+// code for those checks.
+static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = {
+    //                                  INIT,   STANDBY,    ARMED,  ARMED_ERROR,    STANDBY_ERROR,  REBOOT,     IN_AIR_RESTORE
+    { /* ARMING_STATE_INIT */           true,   true,       false,  false,          false,          false,      false },
+    { /* ARMING_STATE_STANDBY */        true,   true,       true,   true,           false,          false,      false },
+    { /* ARMING_STATE_ARMED */          false,  true,       true,   false,          false,          false,      true },
+    { /* ARMING_STATE_ARMED_ERROR */    false,  false,      true,   true,           false,          false,      false },
+    { /* ARMING_STATE_STANDBY_ERROR */  true,   true,       false,  true,           true,           false,      false },
+    { /* ARMING_STATE_REBOOT */         true,   true,       false,  false,          true,           true,       true },
+    { /* ARMING_STATE_IN_AIR_RESTORE */ false,  false,      false,  false,          false,          false,      false }, // NYI
+};
+
+// You can index into the array with an arming_state_t in order to get it's textual representation
+static const char* state_names[ARMING_STATE_MAX] = {
+    "ARMING_STATE_INIT",
+    "ARMING_STATE_STANDBY",
+    "ARMING_STATE_ARMED",
+    "ARMING_STATE_ARMED_ERROR",
+    "ARMING_STATE_STANDBY_ERROR",
+    "ARMING_STATE_REBOOT",
+    "ARMING_STATE_IN_AIR_RESTORE",
+};
+
 transition_result_t
-arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety,
-			arming_state_t new_arming_state, struct actuator_armed_s *armed)
+arming_state_transition(struct vehicle_status_s *status,            /// current vehicle status
+                        const struct safety_s   *safety,            /// current safety settings
+                        arming_state_t          new_arming_state,   /// arming state requested
+                        struct actuator_armed_s *armed,             /// current armed status
+                        const int               mavlink_fd)         /// mavlink fd for error reporting, 0 for none
 {
+    // Double check that our static arrays are still valid
+    ASSERT(ARMING_STATE_INIT == 0);
+    ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1);
+    
 	/*
 	 * Perform an atomic state update
 	 */
@@ -83,116 +117,63 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
 	/* only check transition if the new state is actually different from the current one */
 	if (new_arming_state == status->arming_state) {
 		ret = TRANSITION_NOT_CHANGED;
-
 	} else {
-
 		/* enforce lockdown in HIL */
 		if (status->hil_state == HIL_STATE_ON) {
 			armed->lockdown = true;
-
 		} else {
 			armed->lockdown = false;
 		}
-
-		switch (new_arming_state) {
-		case ARMING_STATE_INIT:
-
-			/* allow going back from INIT for calibration */
-			if (status->arming_state == ARMING_STATE_STANDBY) {
-				ret = TRANSITION_CHANGED;
-				armed->armed = false;
-				armed->ready_to_arm = false;
-			}
-
-			break;
-
-		case ARMING_STATE_STANDBY:
-
-			/* allow coming from INIT and disarming from ARMED */
-			if (status->arming_state == ARMING_STATE_INIT
-			    || status->arming_state == ARMING_STATE_ARMED
-			    || status->hil_state == HIL_STATE_ON) {
-
-				/* sensors need to be initialized for STANDBY state */
-				if (status->condition_system_sensors_initialized) {
-					ret = TRANSITION_CHANGED;
-					armed->armed = false;
-					armed->ready_to_arm = true;
-				}
-			}
-
-			break;
-
-		case ARMING_STATE_ARMED:
-
-			/* allow arming from STANDBY and IN-AIR-RESTORE */
-			if ((status->arming_state == ARMING_STATE_STANDBY
-			     || status->arming_state == ARMING_STATE_IN_AIR_RESTORE)
-			    && (!safety->safety_switch_available || safety->safety_off || status->hil_state == HIL_STATE_ON)) { /* only allow arming if safety is off */
-				ret = TRANSITION_CHANGED;
-				armed->armed = true;
-				armed->ready_to_arm = true;
-			}
-
-			break;
-
-		case ARMING_STATE_ARMED_ERROR:
-
-			/* an armed error happens when ARMED obviously */
-			if (status->arming_state == ARMING_STATE_ARMED) {
-				ret = TRANSITION_CHANGED;
-				armed->armed = true;
-				armed->ready_to_arm = false;
-			}
-
-			break;
-
-		case ARMING_STATE_STANDBY_ERROR:
-
-			/* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */
-			if (status->arming_state == ARMING_STATE_STANDBY
-			    || status->arming_state == ARMING_STATE_INIT
-			    || status->arming_state == ARMING_STATE_ARMED_ERROR) {
-				ret = TRANSITION_CHANGED;
-				armed->armed = false;
-				armed->ready_to_arm = false;
-			}
-
-			break;
-
-		case ARMING_STATE_REBOOT:
-
-			/* an armed error happens when ARMED obviously */
-			if (status->arming_state == ARMING_STATE_INIT
-			    || status->arming_state == ARMING_STATE_STANDBY
-			    || status->arming_state == ARMING_STATE_STANDBY_ERROR) {
-				ret = TRANSITION_CHANGED;
-				armed->armed = false;
-				armed->ready_to_arm = false;
-			}
-
-			break;
-
-		case ARMING_STATE_IN_AIR_RESTORE:
-
-			/* XXX implement */
-			break;
-
-		default:
-			break;
-		}
-
-		if (ret == TRANSITION_CHANGED) {
-			status->arming_state = new_arming_state;
-			arming_state_changed = true;
-		}
-	}
-
+        
+        // Check that we have a valid state transition
+        bool valid_transition = arming_transitions[new_arming_state][status->arming_state];
+        if (valid_transition) {
+            // We have a good transition. Now perform any secondary validation.
+            if (new_arming_state == ARMING_STATE_ARMED) {
+                // Fail transition if we need safety switch press
+                //      Allow if coming from in air restore
+                //      Allow if HIL_STATE_ON
+                if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) {
+                    if (mavlink_fd) {
+                        mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first.");
+                    }
+                    valid_transition = false;
+                }
+            } else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) {
+                new_arming_state = ARMING_STATE_STANDBY_ERROR;
+            }
+        }
+        
+        // HIL can always go to standby
+        if (status->hil_state == HIL_STATE_ON && new_arming_state == ARMING_STATE_STANDBY) {
+            valid_transition = true;
+        }
+        
+        /* Sensors need to be initialized for STANDBY state */
+        if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
+            valid_transition = false;
+        }
+
+        // Finish up the state transition
+        if (valid_transition) {
+            armed->armed = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_ARMED_ERROR;
+            armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY;
+            ret = TRANSITION_CHANGED;
+            status->arming_state = new_arming_state;
+            arming_state_changed = true;
+        }
+    }
+    
 	/* end of atomic state update */
 	irqrestore(flags);
 
-	if (ret == TRANSITION_DENIED)
-		warnx("arming transition rejected");
+    if (ret == TRANSITION_DENIED) {
+        static const char* errMsg = "Invalid arming transition from %s to %s";
+        if (mavlink_fd) {
+            mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]);
+        }
+        warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]);
+    }
 
 	return ret;
 }
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index f04879ff9..0ddd4f05a 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -57,7 +57,7 @@ typedef enum {
 } transition_result_t;
 
 transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
-		arming_state_t new_arming_state, struct actuator_armed_s *armed);
+		arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd = 0);
 
 bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
 
diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c
index 34d20e485..c132b0040 100644
--- a/src/modules/dataman/dataman.c
+++ b/src/modules/dataman/dataman.c
@@ -44,7 +44,9 @@
 #include <stdlib.h>
 #include <fcntl.h>
 #include <systemlib/systemlib.h>
+#include <systemlib/err.h>
 #include <queue.h>
+#include <string.h>
 
 #include "dataman.h"
 
@@ -594,6 +596,20 @@ task_main(int argc, char *argv[])
 
 	sem_init(&g_work_queued_sema, 1, 0);
 
+	/* See if the data manage file exists and is a multiple of the sector size */
+	g_task_fd = open(k_data_manager_device_path, O_RDONLY | O_BINARY);
+	if (g_task_fd >= 0) {
+		/* File exists, check its size */
+		int file_size = lseek(g_task_fd, 0, SEEK_END);
+		if ((file_size % k_sector_size) != 0) {
+			warnx("Incompatible data manager file %s, resetting it", k_data_manager_device_path);
+			close(g_task_fd);
+			unlink(k_data_manager_device_path);
+		}
+		else
+			close(g_task_fd);
+	}
+
 	/* Open or create the data manager file */
 	g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY);
 
@@ -603,7 +619,7 @@ task_main(int argc, char *argv[])
 		return -1;
 	}
 
-	if (lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) {
+	if ((unsigned)lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) {
 		close(g_task_fd);
 		warnx("Could not seek data manager file %s", k_data_manager_device_path);
 		sem_post(&g_init_sema); /* Don't want to hang startup */
@@ -776,4 +792,3 @@ dataman_main(int argc, char *argv[])
 
 	exit(1);
 }
-
diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h
index a70638ccc..33c9fcd15 100644
--- a/src/modules/dataman/dataman.h
+++ b/src/modules/dataman/dataman.h
@@ -79,7 +79,7 @@ extern "C" {
 	} dm_reset_reason;
 
 	/* Maximum size in bytes of a single item instance */
-	#define DM_MAX_DATA_SIZE 126
+	#define DM_MAX_DATA_SIZE 124
 
 	/* Retrieve from the data manager store */
 	__EXPORT ssize_t
diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
index 9d2d9ed5f..f076c94fd 100644
--- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
+++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
@@ -734,21 +734,21 @@ FixedwingEstimator::task_main()
 				case 1:
 				{
 					const char* str = "NaN in states, resetting";
-					warnx(str);
+					warnx("%s", str);
 					mavlink_log_critical(_mavlink_fd, str);
 					break;
 				}
 				case 2:
 				{
 					const char* str = "stale IMU data, resetting";
-					warnx(str);
+					warnx("%s", str);
 					mavlink_log_critical(_mavlink_fd, str);
 					break;
 				}
 				case 3:
 				{
 					const char* str = "switching dynamic / static state";
-					warnx(str);
+					warnx("%s", str);
 					mavlink_log_critical(_mavlink_fd, str);
 					break;
 				}
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index ad435b251..e49288a74 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -62,6 +62,12 @@ PARAM_DEFINE_INT32(MAV_COMP_ID, 50);
  * @group MAVLink
  */
 PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING);
+/**
+ * Use/Accept HIL GPS message (even if not in HIL mode)
+ * If set to 1 incomming HIL GPS messages are parsed.
+ * @group MAVLink
+ */
+PARAM_DEFINE_INT32(MAV_USEHILGPS, 0);
 
 mavlink_system_t mavlink_system = {
 	100,
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 2416a1264..7ecca0d65 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -208,6 +208,7 @@ Mavlink::Mavlink() :
 	_mavlink_fd(-1),
 	_task_running(false),
 	_hil_enabled(false),
+	_use_hil_gps(false),
 	_is_usb_uart(false),
 	_wait_to_transmit(false),
 	_received_messages(false),
@@ -487,11 +488,13 @@ void Mavlink::mavlink_update_system(void)
 	static param_t param_system_id;
 	static param_t param_component_id;
 	static param_t param_system_type;
+	static param_t param_use_hil_gps;
 
 	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");
+		param_use_hil_gps = param_find("MAV_USEHILGPS");
 		initialized = true;
 	}
 
@@ -516,6 +519,11 @@ void Mavlink::mavlink_update_system(void)
 	if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
 		mavlink_system.type = system_type;
 	}
+
+	int32_t use_hil_gps;
+	param_get(param_use_hil_gps, &use_hil_gps);
+
+	_use_hil_gps = (bool)use_hil_gps;
 }
 
 int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
@@ -574,6 +582,11 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 	/* open uart */
 	_uart_fd = open(uart_name, O_RDWR | O_NOCTTY);
 
+	if (_uart_fd < 0) {
+		return _uart_fd;
+	}
+
+
 	/* Try to set baud rate */
 	struct termios uart_config;
 	int termios_state;
@@ -1953,6 +1966,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult);
 		configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult);
 		configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult);
+		configure_stream("DISTANCE_SENSOR", 0.5f);
 		break;
 
 	case MAVLINK_MODE_CAMERA:
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 66d82b471..1bf51fd31 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -157,6 +157,8 @@ public:
 
 	bool		get_hil_enabled() { return _hil_enabled; }
 
+	bool		get_use_hil_gps() { return _use_hil_gps; }
+
 	bool		get_flow_control_enabled() { return _flow_control_enabled; }
 
 	bool		get_forwarding_on() { return _forwarding_on; }
@@ -223,6 +225,7 @@ private:
 
 	/* states */
 	bool		_hil_enabled;		/**< Hardware In the Loop mode */
+	bool		_use_hil_gps;		/**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
 	bool		_is_usb_uart;		/**< Port is USB */
 	bool        _wait_to_transmit;  /**< Wait to transmit until received messages. */
 	bool        _received_messages; /**< Whether we've received valid mavlink messages. */
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 648228e3b..678ce1645 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -72,6 +72,7 @@
 #include <uORB/topics/navigation_capabilities.h>
 #include <drivers/drv_rc_input.h>
 #include <drivers/drv_pwm_output.h>
+#include <drivers/drv_range_finder.h>
 
 #include "mavlink_messages.h"
 
@@ -270,7 +271,7 @@ protected:
 						status->load * 1000.0f,
 						status->battery_voltage * 1000.0f,
 						status->battery_current * 1000.0f,
-						status->battery_remaining,
+						status->battery_remaining * 100.0f,
 						status->drop_rate_comm,
 						status->errors_comm,
 						status->errors_count1,
@@ -1312,6 +1313,51 @@ protected:
 	}
 };
 
+class MavlinkStreamDistanceSensor : public MavlinkStream
+{
+public:
+	const char *get_name()
+	{
+		return "DISTANCE_SENSOR";
+	}
+
+	MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamDistanceSensor();
+	}
+
+private:
+	MavlinkOrbSubscription *range_sub;
+	struct range_finder_report *range;
+
+protected:
+	void subscribe(Mavlink *mavlink)
+	{
+		range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder));
+		range = (struct range_finder_report *)range_sub->get_data();
+	}
+
+	void send(const hrt_abstime t)
+	{
+		(void)range_sub->update(t);
+
+		uint8_t type;
+
+		switch (range->type) {
+			case RANGE_FINDER_TYPE_LASER:
+			type = MAV_DISTANCE_SENSOR_LASER;
+			break;
+		}
+
+		uint8_t id = 0;
+		uint8_t orientation = 0;
+		uint8_t covariance = 20;
+
+		mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation, 
+			range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance);
+	}
+};
+
 MavlinkStream *streams_list[] = {
 	new MavlinkStreamHeartbeat(),
 	new MavlinkStreamSysStatus(),
@@ -1338,6 +1384,7 @@ MavlinkStream *streams_list[] = {
 	new MavlinkStreamAttitudeControls(),
 	new MavlinkStreamNamedValueFloat(),
 	new MavlinkStreamCameraCapture(),
+	new MavlinkStreamDistanceSensor(),
 	new MavlinkStreamViconPositionEstimate(),
 	nullptr
 };
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 0762b76f2..33a4fef12 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -160,6 +160,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 	 * The HIL mode is enabled by the HIL bit flag
 	 * in the system mode. Either send a set mode
 	 * COMMAND_LONG message or a SET_MODE message
+	 *
+	 * Accept HIL GPS messages if use_hil_gps flag is true.
+	 * This allows to provide fake gps measurements to the system.
 	 */
 	if (_mavlink->get_hil_enabled()) {
 		switch (msg->msgid) {
@@ -167,10 +170,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 			handle_message_hil_sensor(msg);
 			break;
 
-		case MAVLINK_MSG_ID_HIL_GPS:
-			handle_message_hil_gps(msg);
-			break;
-
 		case MAVLINK_MSG_ID_HIL_STATE_QUATERNION:
 			handle_message_hil_state_quaternion(msg);
 			break;
@@ -180,7 +179,20 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		}
 	}
 
-	/* If we've received a valid message, mark the flag indicating so.
+
+   	if (_mavlink->get_hil_enabled() || (_mavlink->get_use_hil_gps() && msg->sysid == mavlink_system.sysid)) {
+		switch (msg->msgid) {
+		case MAVLINK_MSG_ID_HIL_GPS:
+			handle_message_hil_gps(msg);
+			break;
+
+		default:
+			break;
+		}
+
+	}
+	
+    /* If we've received a valid message, mark the flag indicating so.
 	   This is used in the '-w' command-line flag. */
 	_mavlink->set_has_received_messages(true);
 }
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index c44354ff0..dcca11977 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -47,4 +47,4 @@ SRCS		 += mavlink_main.cpp \
 
 INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
 
-MAXOPTIMIZATION		 = -Os
+MAXOPTIMIZATION	 = -Os
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 6b797f222..65f4cbeaa 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -356,8 +356,10 @@ MulticopterPositionControl::parameters_update(bool force)
 		param_get(_params_handles.thr_min, &_params.thr_min);
 		param_get(_params_handles.thr_max, &_params.thr_max);
 		param_get(_params_handles.tilt_max, &_params.tilt_max);
+		_params.tilt_max = math::radians(_params.tilt_max);
 		param_get(_params_handles.land_speed, &_params.land_speed);
 		param_get(_params_handles.land_tilt_max, &_params.land_tilt_max);
+		_params.land_tilt_max = math::radians(_params.land_tilt_max);
 
 		float v;
 		param_get(_params_handles.xy_p, &v);
@@ -1001,6 +1003,18 @@ MulticopterPositionControl::task_main()
 						_att_sp.roll_body = euler(0);
 						_att_sp.pitch_body = euler(1);
 						/* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */
+
+					} else if (!_control_mode.flag_control_manual_enabled) {
+						/* autonomous altitude control without position control (failsafe landing),
+						 * force level attitude, don't change yaw */
+						R.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
+
+						/* copy rotation matrix to attitude setpoint topic */
+						memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
+						_att_sp.R_valid = true;
+
+						_att_sp.roll_body = 0.0f;
+						_att_sp.pitch_body = 0.0f;
 					}
 
 					_att_sp.thrust = thrust_abs;
diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c
index 0082a5e6a..104df4e59 100644
--- a/src/modules/mc_pos_control/mc_pos_control_params.c
+++ b/src/modules/mc_pos_control/mc_pos_control_params.c
@@ -100,6 +100,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f);
  *
  * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (SEATBELT, EASY).
  *
+ * @unit m/s
  * @min 0.0
  * @group Multicopter Position Control
  */
@@ -155,6 +156,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f);
  *
  * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (EASY).
  *
+ * @unit m/s
  * @min 0.0
  * @group Multicopter Position Control
  */
@@ -172,31 +174,35 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f);
 PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f);
 
 /**
- * Maximum tilt
+ * Maximum tilt angle in air
  *
- * Limits maximum tilt in AUTO and EASY modes.
+ * Limits maximum tilt in AUTO and EASY modes during flight.
  *
+ * @unit deg
  * @min 0.0
- * @max 1.57
+ * @max 90.0
  * @group Multicopter Position Control
  */
-PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 1.0f);
+PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR, 45.0f);
 
 /**
- * Landing descend rate
+ * Maximum tilt during landing
+ *
+ * Limits maximum tilt angle on landing.
  *
+ * @unit deg
  * @min 0.0
+ * @max 90.0
  * @group Multicopter Position Control
  */
-PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f);
+PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 15.0f);
 
 /**
- * Maximum landing tilt
- *
- * Limits maximum tilt on landing.
+ * Landing descend rate
  *
+ * @unit m/s
  * @min 0.0
- * @max 1.57
  * @group Multicopter Position Control
  */
-PARAM_DEFINE_FLOAT(MPC_LAND_TILT, 0.3f);
+PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f);
+
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index ca71aab70..12fd35a0a 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -692,6 +692,9 @@ Navigator::task_main()
 
 			/* evaluate state requested by commander */
 			if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) {
+				/* publish position setpoint triplet on each status update if navigator active */
+				_pos_sp_triplet_updated = true;
+
 				if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) {
 					/* commander requested new navigation mode, try to set it */
 					switch (_vstatus.set_nav_state) {
@@ -733,6 +736,13 @@ Navigator::task_main()
 					}
 				}
 
+				/* check if waypoint has been reached in MISSION, RTL and LAND modes */
+				if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) {
+					if (check_mission_item_reached()) {
+						on_mission_item_reached();
+					}
+				}
+
 			} else {
 				/* navigator shouldn't act */
 				dispatch(EVENT_NONE_REQUESTED);
@@ -777,8 +787,8 @@ Navigator::task_main()
 		if (fds[1].revents & POLLIN) {
 			global_position_update();
 
-			/* publish position setpoint triplet on each position update if navigator active */
 			if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) {
+				/* publish position setpoint triplet on each position update if navigator active */
 				_pos_sp_triplet_updated = true;
 
 				if (myState == NAV_STATE_LAND && !_global_pos_valid) {
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 97be2ce4a..b74d4183b 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -908,9 +908,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 	hrt_abstime barometer_timestamp = 0;
 	hrt_abstime differential_pressure_timestamp = 0;
 
-	/* track changes in distance status */
-	bool dist_bottom_present = false;
-
 	/* enable logging on start if needed */
 	if (log_on_start) {
 		/* check GPS topic to get GPS time */
@@ -963,6 +960,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.msg_type = LOG_STAT_MSG;
 			log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state;
 			log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state;
+			log_msg.body.log_STAT.failsafe_state = (uint8_t) buf_status.failsafe_state;
 			log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
 			log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning;
 			log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed;
@@ -1099,6 +1097,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_LPOS.x = buf.local_pos.x;
 			log_msg.body.log_LPOS.y = buf.local_pos.y;
 			log_msg.body.log_LPOS.z = buf.local_pos.z;
+			log_msg.body.log_LPOS.ground_dist = buf.local_pos.dist_bottom;
+			log_msg.body.log_LPOS.ground_dist_rate = buf.local_pos.dist_bottom_rate;
 			log_msg.body.log_LPOS.vx = buf.local_pos.vx;
 			log_msg.body.log_LPOS.vy = buf.local_pos.vy;
 			log_msg.body.log_LPOS.vz = buf.local_pos.vz;
@@ -1108,19 +1108,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0);
 			log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0);
 			log_msg.body.log_LPOS.landed = buf.local_pos.landed;
+			log_msg.body.log_LPOS.ground_dist_flags = (buf.local_pos.dist_bottom_valid ? 1 : 0);
 			LOGBUFFER_WRITE_AND_COUNT(LPOS);
-
-			if (buf.local_pos.dist_bottom_valid) {
-				dist_bottom_present = true;
-			}
-
-			if (dist_bottom_present) {
-				log_msg.msg_type = LOG_DIST_MSG;
-				log_msg.body.log_DIST.bottom = buf.local_pos.dist_bottom;
-				log_msg.body.log_DIST.bottom_rate = buf.local_pos.dist_bottom_rate;
-				log_msg.body.log_DIST.flags = (buf.local_pos.dist_bottom_valid ? 1 : 0);
-				LOGBUFFER_WRITE_AND_COUNT(DIST);
-			}
 		}
 
 		/* --- LOCAL POSITION SETPOINT --- */
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index a37437346..595a787d6 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -101,6 +101,8 @@ struct log_LPOS_s {
 	float x;
 	float y;
 	float z;
+	float ground_dist;
+	float ground_dist_rate;
 	float vx;
 	float vy;
 	float vz;
@@ -110,6 +112,7 @@ struct log_LPOS_s {
 	uint8_t xy_flags;
 	uint8_t z_flags;
 	uint8_t landed;
+	uint8_t ground_dist_flags;
 };
 
 /* --- LPSP - LOCAL POSITION SETPOINT --- */
@@ -151,6 +154,7 @@ struct log_ATTC_s {
 struct log_STAT_s {
 	uint8_t main_state;
 	uint8_t arming_state;
+	uint8_t failsafe_state;
 	float battery_remaining;
 	uint8_t battery_warning;
 	uint8_t landed;
@@ -340,30 +344,30 @@ struct log_PARM_s {
 /* construct list of all message formats */
 static const struct log_format_s log_formats[] = {
 	/* business-level messages, ID < 0x80 */
-	LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
-	LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
-	LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
-	LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"),
-	LOG_FORMAT(LPOS, "ffffffLLfBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"),
-	LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
-	LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
-	LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
-	LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"),
-	LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"),
-	LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
-	LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"),
-	LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
-	LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
-	LOG_FORMAT(GPOS, "LLffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV"),
-	LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
-	LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
-	LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
-	LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
-	LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"),
-	LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"),
-	LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"),
-	LOG_FORMAT(PWR, "fffBBBBB", "Periph_5V,Servo_5V,RSSI,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"),
-	LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"),
+	LOG_FORMAT(ATT, "fffffffff",		"Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
+	LOG_FORMAT(ATSP, "ffff",		"RollSP,PitchSP,YawSP,ThrustSP"),
+	LOG_FORMAT(IMU, "fffffffff",		"AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
+	LOG_FORMAT(SENS, "fffff",		"BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"),
+	LOG_FORMAT(LPOS, "ffffffffLLfBBBB",	"X,Y,Z,dist,distR,VX,VY,VZ,RLat,RLon,RAlt,XYFlg,ZFlg,LFlg,GFlg"),
+	LOG_FORMAT(LPSP, "ffff",		"X,Y,Z,Yaw"),
+	LOG_FORMAT(GPS, "QBffLLfffff",		"GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
+	LOG_FORMAT(ATTC, "ffff",		"Roll,Pitch,Yaw,Thrust"),
+	LOG_FORMAT(STAT, "BBBfBB",		"MainState,ArmState,FailsafeState,BatRem,BatWarn,Landed"),
+	LOG_FORMAT(RC, "ffffffffBB",		"Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"),
+	LOG_FORMAT(OUT0, "ffffffff",		"Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
+	LOG_FORMAT(AIRS, "fff",			"IndSpeed,TrueSpeed,AirTemp"),
+	LOG_FORMAT(ARSP, "fff",			"RollRateSP,PitchRateSP,YawRateSP"),
+	LOG_FORMAT(FLOW, "hhfffBB",		"RawX,RawY,CompX,CompY,Dist,Q,SensID"),
+	LOG_FORMAT(GPOS, "LLffffff",		"Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV"),
+	LOG_FORMAT(GPSP, "BLLffBfbf",		"NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
+	LOG_FORMAT(ESC, "HBBBHHHHHHfH",		"count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
+	LOG_FORMAT(GVSP, "fff",			"VX,VY,VZ"),
+	LOG_FORMAT(BATT, "ffff",		"V,VFilt,C,Discharged"),
+	LOG_FORMAT(DIST, "ffB",			"Bottom,BottomRate,Flags"),
+	LOG_FORMAT(TELE, "BBBBHHB",		"RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"),
+	LOG_FORMAT(ESTM, "ffffffffffBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,nStat,statNaN,covNaN,kGainNaN"),
+	LOG_FORMAT(PWR, "fffBBBBB",		"Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"),
+	LOG_FORMAT(VICN, "ffffff",		"X,Y,Z,Roll,Pitch,Yaw"),
 
 	/* system-level messages, ID >= 0x80 */
 	/* FMT: don't write format of format message, it's useless */
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index a19180ad4..dd6e21bdd 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -608,7 +608,7 @@ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0);
 PARAM_DEFINE_INT32(RC_MAP_EASY_SW, 0);
 
 /**
- * Mission switch channel mapping.
+ * Loiter switch channel mapping.
  *
  * @min 0
  * @max 18
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 28c08422e..9a750db12 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -255,7 +255,11 @@ private:
 
 		int rc_map_mode_sw;
 		int rc_map_return_sw;
+<<<<<<< HEAD
 		int rc_map_easy_sw;
+=======
+		int rc_map_assisted_sw;
+>>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863
 		int rc_map_loiter_sw;
 
 //		int rc_map_offboard_ctrl_mode_sw;
@@ -309,7 +313,11 @@ private:
 
 		param_t rc_map_mode_sw;
 		param_t rc_map_return_sw;
+<<<<<<< HEAD
 		param_t rc_map_easy_sw;
+=======
+		param_t rc_map_assisted_sw;
+>>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863
 		param_t rc_map_loiter_sw;
 
 //		param_t rc_map_offboard_ctrl_mode_sw;
@@ -526,7 +534,11 @@ Sensors::Sensors() :
 	_parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS");
 
 	/* optional mode switches, not mapped per default */
+<<<<<<< HEAD
 	_parameter_handles.rc_map_easy_sw = param_find("RC_MAP_EASY_SW");
+=======
+	_parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW");
+>>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863
 	_parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW");
 
 //	_parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW");
@@ -651,19 +663,19 @@ Sensors::parameters_update()
 
 	/* channel mapping */
 	if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) {
-		warnx(paramerr);
+		warnx("%s", paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_pitch, &(_parameters.rc_map_pitch)) != OK) {
-		warnx(paramerr);
+		warnx("%s", paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_yaw, &(_parameters.rc_map_yaw)) != OK) {
-		warnx(paramerr);
+		warnx("%s", paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)) != OK) {
-		warnx(paramerr);
+		warnx("%s", paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_failsafe, &(_parameters.rc_map_failsafe)) != OK) {
@@ -671,23 +683,32 @@ Sensors::parameters_update()
 	}
 
 	if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) {
-		warnx(paramerr);
+		warnx("%s", paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) {
-		warnx(paramerr);
+		warnx("%s", paramerr);
 	}
 
+<<<<<<< HEAD
 	if (param_get(_parameter_handles.rc_map_easy_sw, &(_parameters.rc_map_easy_sw)) != OK) {
 		warnx(paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) {
 		warnx(paramerr);
+=======
+	if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) {
+		warnx("%s", paramerr);
+	}
+
+	if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) {
+		warnx("%s", paramerr);
+>>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863
 	}
 
 	if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
-		warnx(paramerr);
+		warnx("%s", paramerr);
 	}
 
 //	if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) {
@@ -724,7 +745,11 @@ Sensors::parameters_update()
 
 	_rc.function[MODE] = _parameters.rc_map_mode_sw - 1;
 	_rc.function[RETURN] = _parameters.rc_map_return_sw - 1;
+<<<<<<< HEAD
 	_rc.function[EASY] = _parameters.rc_map_easy_sw - 1;
+=======
+	_rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1;
+>>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863
 	_rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1;
 
 	_rc.function[FLAPS] = _parameters.rc_map_flaps - 1;
@@ -768,12 +793,12 @@ Sensors::parameters_update()
 
 	/* scaling of ADC ticks to battery voltage */
 	if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) {
-		warnx(paramerr);
+		warnx("%s", paramerr);
 	}
 
 	/* scaling of ADC ticks to battery current */
 	if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) {
-		warnx(paramerr);
+		warnx("%s", paramerr);
 	}
 
 	param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
@@ -1285,7 +1310,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 				}
 			}
 			_last_adc = t;
-			if (_battery_status.voltage_v > 0.0f) {
+			if (_battery_status.voltage_filtered_v > BATT_V_IGNORE_THRESHOLD) {
 				/* announce the battery status if needed, just publish else */
 				if (_battery_pub > 0) {
 					orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
@@ -1477,10 +1502,17 @@ Sensors::rc_poll()
 			manual.aux5 = get_rc_value(AUX_5, -1.0, 1.0);
 
 			/* mode switches */
+<<<<<<< HEAD
 			manual.mode_switch = get_rc_sw3pos_position(MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assisted_th, _parameters.rc_assisted_inv);
 			manual.easy_switch = get_rc_sw2pos_position(EASY, _parameters.rc_easy_th, _parameters.rc_easy_inv);
 			manual.return_switch = get_rc_sw2pos_position(RETURN, _parameters.rc_return_th, _parameters.rc_return_inv);
 			manual.loiter_switch = get_rc_sw2pos_position(LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv);
+=======
+			manual.mode_switch = get_rc_switch_position(MODE);
+			manual.assisted_switch = get_rc_switch_position(ASSISTED);
+			manual.loiter_switch = get_rc_switch_position(LOITER);
+			manual.return_switch = get_rc_switch_position(RETURN);
+>>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863
 
 			/* publish manual_control_setpoint topic */
 			if (_manual_control_pub > 0) {
@@ -1590,12 +1622,10 @@ Sensors::task_main()
 
 	while (!_task_should_exit) {
 
-		/* wait for up to 100ms for data */
-		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
+		/* wait for up to 50ms for data */
+		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50);
 
-		/* timed out - periodic check for _task_should_exit, etc. */
-		if (pret == 0)
-			continue;
+		/* if pret == 0 it timed out - periodic check for _task_should_exit, etc. */
 
 		/* this is undesirable but not much we can do - might want to flag unhappy status */
 		if (pret < 0) {
@@ -1634,7 +1664,7 @@ Sensors::task_main()
 		perf_end(_loop_perf);
 	}
 
-	printf("[sensors] exiting.\n");
+	warnx("[sensors] exiting.");
 
 	_sensors_task = -1;
 	_exit(0);
diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h
index b6257e22a..c61987df6 100644
--- a/src/modules/uORB/topics/manual_control_setpoint.h
+++ b/src/modules/uORB/topics/manual_control_setpoint.h
@@ -78,7 +78,11 @@ struct manual_control_setpoint_s {
 
 	switch_pos_t mode_switch;			/**< mode 3 position switch (mandatory): manual, assisted, auto */
 	switch_pos_t return_switch;			/**< land 2 position switch (mandatory): land, no effect */
+<<<<<<< HEAD
 	switch_pos_t easy_switch;			/**< assisted 2 position switch (optional): seatbelt, simple */
+=======
+	switch_pos_t assisted_switch;			/**< assisted 2 position switch (optional): seatbelt, simple */
+>>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863
 	switch_pos_t loiter_switch;		/**< mission 2 position switch (optional): mission, loiter */
 }; /**< manual control inputs */
 
diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h
index d99203ff6..df651e78d 100644
--- a/src/modules/uORB/topics/rc_channels.h
+++ b/src/modules/uORB/topics/rc_channels.h
@@ -64,7 +64,11 @@ enum RC_CHANNELS_FUNCTION {
 	YAW      = 3,
 	MODE = 4,
 	RETURN = 5,
+<<<<<<< HEAD
 	EASY = 6,
+=======
+	ASSISTED = 6,
+>>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863
 	LOITER = 7,
 	OFFBOARD_MODE = 8,
 	FLAPS   = 9,
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 48af4c9e2..435230432 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -69,6 +69,8 @@ typedef enum {
 	MAIN_STATE_MAX
 } main_state_t;
 
+// If you change the order, add or remove arming_state_t states make sure to update the arrays
+// in state_machine_helper.cpp as well.
 typedef enum {
 	ARMING_STATE_INIT = 0,
 	ARMING_STATE_STANDBY,
diff --git a/src/modules/unit_test/unit_test.cpp b/src/modules/unit_test/unit_test.cpp
index 64ee544a2..02d1af481 100644
--- a/src/modules/unit_test/unit_test.cpp
+++ b/src/modules/unit_test/unit_test.cpp
@@ -32,17 +32,10 @@
  *
  ****************************************************************************/
 
-/**
- * @file unit_test.cpp
- * A unit test library.
- *
- */
-
 #include "unit_test.h"
 
 #include <systemlib/err.h>
 
-
 UnitTest::UnitTest()
 {
 }
@@ -51,15 +44,15 @@ UnitTest::~UnitTest()
 {
 }
 
-void
-UnitTest::print_results(const char* result)
+void UnitTest::printResults(void)
+{
+    warnx(mu_tests_failed() ? "SOME TESTS FAILED" : "ALL TESTS PASSED");
+    warnx("  Tests passed : %d", mu_tests_passed());
+    warnx("  Tests failed : %d", mu_tests_failed());
+    warnx("  Assertions : %d", mu_assertion());
+}
+
+void UnitTest::printAssert(const char* msg, const char* test, const char* file, int line)
 {
-	if (result != 0) {
-        	warnx("Failed: %s:%d", mu_last_test(), mu_line());
-        	warnx("%s", result);
-    	} else {
-        	warnx("ALL TESTS PASSED");
-        	warnx("  Tests run : %d", mu_tests_run());
-        	warnx("  Assertion : %d", mu_assertion());
-    	}
+    warnx("Assertion failed: %s - %s (%s:%d)", msg, test, file, line);
 }
diff --git a/src/modules/unit_test/unit_test.h b/src/modules/unit_test/unit_test.h
index 3020734f4..32eb8c308 100644
--- a/src/modules/unit_test/unit_test.h
+++ b/src/modules/unit_test/unit_test.h
@@ -32,62 +32,55 @@
  *
  ****************************************************************************/
 
-/**
- * @file unit_test.h
- * A unit test library based on MinUnit (http://www.jera.com/techinfo/jtns/jtn002.html).
- *
- */
-
 #ifndef UNIT_TEST_H_
-#define UNIT_TEST_
+#define UNIT_TEST_H_
 
 #include <systemlib/err.h>
 
-
 class __EXPORT UnitTest
 {
 
 public:
-#define xstr(s) str(s)
-#define str(s) #s
 #define INLINE_GLOBAL(type,func) inline type& func() { static type x; return x; }
 
 INLINE_GLOBAL(int, mu_tests_run)
+INLINE_GLOBAL(int, mu_tests_failed)
+INLINE_GLOBAL(int, mu_tests_passed)
 INLINE_GLOBAL(int, mu_assertion)
 INLINE_GLOBAL(int, mu_line)
 INLINE_GLOBAL(const char*, mu_last_test)
 
-#define mu_assert(message, test)                                           \
-    do                                                                     \
-    {                                                                      \
-        if (!(test))                                                       \
-            return __FILE__ ":" xstr(__LINE__) " " message " (" #test ")"; \
-        else                                                               \
-            mu_assertion()++;                                              \
-    } while (0)
-
-
-#define mu_run_test(test)                                                  \
-do                                                                         \
-{                                                                          \
-    const char *message;                                                   \
-    mu_last_test() = #test;                                                \
-    mu_line() = __LINE__;                                                  \
-    message = test();                                                      \
-    mu_tests_run()++;                                                      \
-    if (message)                                                           \
-        return message;                                                    \
-} while (0)
-
-
-public:
 	UnitTest();
     virtual ~UnitTest();
 
-    virtual const char*     run_tests() = 0;
-    virtual void            print_results(const char* result);
-};
-
+    virtual void runTests(void) = 0;
+    void printResults(void);
+    
+    void printAssert(const char* msg, const char* test, const char* file, int line);
+    
+#define ut_assert(message, test)                                \
+    do {                                                        \
+        if (!(test)) {                                          \
+            printAssert(message, #test, __FILE__, __LINE__);    \
+            return false;                                       \
+        } else {                                                \
+            mu_assertion()++;                                   \
+        }                                                       \
+    } while (0)
+    
+#define ut_run_test(test)                       \
+    do {                                        \
+        warnx("RUNNING TEST: %s", #test);       \
+        mu_tests_run()++;                       \
+        if (!test()) {                          \
+            warnx("TEST FAILED: %s", #test);    \
+            mu_tests_failed()++;                \
+        } else {                                \
+            warnx("TEST PASSED: %s", #test);    \
+            mu_tests_passed()++;                \
+        }                                       \
+    } while (0)
 
+};
 
 #endif /* UNIT_TEST_H_ */
diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c
index 476015f3e..e6d4b763b 100644
--- a/src/systemcmds/config/config.c
+++ b/src/systemcmds/config/config.c
@@ -121,7 +121,7 @@ do_device(int argc, char *argv[])
 				errx(ret,"uORB publications could not be unblocked");
 
 		} else {
-			errx("no valid command: %s", argv[1]);
+			errx(1, "no valid command: %s", argv[1]);
 		}
 	}