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

Conflicts:
	makefiles/config_px4fmu-v2_test.mk

84 files changed, 1010 insertions, 417 deletions

diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp
index 293690d27..3a1e1b7b5 100644
--- a/src/drivers/airspeed/airspeed.cpp
+++ b/src/drivers/airspeed/airspeed.cpp
@@ -147,7 +147,7 @@ Airspeed::init()
 		_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp);
 
 		if (_airspeed_pub < 0)
-			warnx("failed to create airspeed sensor object. uORB started?");
+			warnx("uORB started?");
 	}
 
 	ret = OK;
diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c
index e5bb772b3..9d2c1441d 100644
--- a/src/drivers/ardrone_interface/ardrone_interface.c
+++ b/src/drivers/ardrone_interface/ardrone_interface.c
@@ -89,8 +89,8 @@ static void
 usage(const char *reason)
 {
 	if (reason)
-		fprintf(stderr, "%s\n", reason);
-	fprintf(stderr, "usage: ardrone_interface {start|stop|status} [-d <UART>]\n\n");
+		warnx("%s\n", reason);
+	warnx("usage: {start|stop|status} [-d <UART>]\n\n");
 	exit(1);
 }
 
@@ -110,7 +110,7 @@ int ardrone_interface_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 
 		if (thread_running) {
-			printf("ardrone_interface already running\n");
+			warnx("already running\n");
 			/* this is not an error */
 			exit(0);
 		}
@@ -132,9 +132,9 @@ int ardrone_interface_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
-			printf("\tardrone_interface is running\n");
+			warnx("running");
 		} else {
-			printf("\tardrone_interface not started\n");
+			warnx("not started");
 		}
 		exit(0);
 	}
@@ -158,7 +158,7 @@ static int ardrone_open_uart(char *uart_name, struct termios *uart_config_origin
 
 	/* Back up the original uart configuration to restore it after exit */
 	if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
-		fprintf(stderr, "[ardrone_interface] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state);
+		warnx("ERR: TCGETATTR %s: %d", uart_name, termios_state);
 		close(uart);
 		return -1;
 	}
@@ -171,14 +171,14 @@ static int ardrone_open_uart(char *uart_name, struct termios *uart_config_origin
 
 	/* Set baud rate */
 	if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
-		fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
+		warnx("ERR: cfsetispeed %s: %d", uart_name, termios_state);
 		close(uart);
 		return -1;
 	}
 
 
 	if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
-		fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
+		warnx("ERR: tcsetattr: %s", uart_name);
 		close(uart);
 		return -1;
 	}
@@ -192,9 +192,6 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 
 	char *device = "/dev/ttyS1";
 
-	/* welcome user */
-	printf("[ardrone_interface] Control started, taking over motors\n");
-
 	/* File descriptors */
 	int gpios;
 
@@ -237,7 +234,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	struct termios uart_config_original;
 
 	if (motor_test_mode) {
-		printf("[ardrone_interface] Motor test mode enabled, setting 10 %% thrust.\n");
+		warnx("setting 10 %% thrust.\n");
 	}
 
 	/* Led animation */
@@ -255,9 +252,6 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
 	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
 
-	printf("[ardrone_interface] Motors initialized - ready.\n");
-	fflush(stdout);
-
 	/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
 	ardrone_write = ardrone_open_uart(device, &uart_config_original);
 
@@ -265,7 +259,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	gpios = ar_multiplexing_init();
 
 	if (ardrone_write < 0) {
-		fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n");
+		warnx("No UART, exiting.");
 		thread_running = false;
 		exit(ERROR);
 	}
@@ -273,7 +267,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	/* initialize motors */
 	if (OK != ar_init_motors(ardrone_write, gpios)) {
 		close(ardrone_write);
-		fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n");
+		warnx("motor init fail");
 		thread_running = false;
 		exit(ERROR);
 	}
@@ -294,7 +288,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	gpios = ar_multiplexing_init();
 
 	if (ardrone_write < 0) {
-		fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n");
+		warnx("write fail");
 		thread_running = false;
 		exit(ERROR);
 	}
@@ -302,7 +296,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	/* initialize motors */
 	if (OK != ar_init_motors(ardrone_write, gpios)) {
 		close(ardrone_write);
-		fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n");
+		warnx("motor init fail");
 		thread_running = false;
 		exit(ERROR);
 	}
@@ -378,11 +372,9 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	int termios_state;
 
 	if ((termios_state = tcsetattr(ardrone_write, TCSANOW, &uart_config_original)) < 0) {
-		fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for (tcsetattr)\n");
+		warnx("ERR: tcsetattr");
 	}
 
-	printf("[ardrone_interface] Restored original UART config, exiting..\n");
-
 	/* close uarts */
 	close(ardrone_write);
 	ar_multiplexing_deinit(gpios);
diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c
index fc017dd58..4fa24275f 100644
--- a/src/drivers/ardrone_interface/ardrone_motor_control.c
+++ b/src/drivers/ardrone_interface/ardrone_motor_control.c
@@ -301,7 +301,7 @@ int ar_init_motors(int ardrone_uart, int gpios)
 	ardrone_write_motor_commands(ardrone_uart, 0, 0, 0, 0);
 
 	if (errcounter != 0) {
-		fprintf(stderr, "[ardrone_interface] init sequence incomplete, failed %d times", -errcounter);
+		warnx("Failed %d times", -errcounter);
 		fflush(stdout);
 	}
 	return errcounter;
diff --git a/src/drivers/boards/aerocore/module.mk b/src/drivers/boards/aerocore/module.mk
index b53fe0a29..0a2d91009 100644
--- a/src/drivers/boards/aerocore/module.mk
+++ b/src/drivers/boards/aerocore/module.mk
@@ -6,3 +6,5 @@ SRCS		 = aerocore_init.c \
 		   aerocore_pwm_servo.c \
 		   aerocore_spi.c \
 		   aerocore_led.c
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/boards/px4fmu-v1/module.mk b/src/drivers/boards/px4fmu-v1/module.mk
index 66b776917..5e1a27d5a 100644
--- a/src/drivers/boards/px4fmu-v1/module.mk
+++ b/src/drivers/boards/px4fmu-v1/module.mk
@@ -8,3 +8,5 @@ SRCS		 = px4fmu_can.c \
 		   px4fmu_spi.c \
 		   px4fmu_usb.c \
 		   px4fmu_led.c
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/boards/px4fmu-v2/module.mk b/src/drivers/boards/px4fmu-v2/module.mk
index 99d37eeca..103232b0c 100644
--- a/src/drivers/boards/px4fmu-v2/module.mk
+++ b/src/drivers/boards/px4fmu-v2/module.mk
@@ -8,3 +8,5 @@ SRCS		 = px4fmu_can.c \
 		   px4fmu_spi.c \
 		   px4fmu_usb.c \
 		   px4fmu2_led.c
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/boards/px4io-v1/module.mk b/src/drivers/boards/px4io-v1/module.mk
index 2601a1c15..a7a14dd07 100644
--- a/src/drivers/boards/px4io-v1/module.mk
+++ b/src/drivers/boards/px4io-v1/module.mk
@@ -4,3 +4,5 @@
 
 SRCS	= px4io_init.c \
 	  px4io_pwm_servo.c
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/boards/px4io-v2/module.mk b/src/drivers/boards/px4io-v2/module.mk
index 85f94e8be..3f0e9a0b3 100644
--- a/src/drivers/boards/px4io-v2/module.mk
+++ b/src/drivers/boards/px4io-v2/module.mk
@@ -4,3 +4,5 @@
 
 SRCS		= px4iov2_init.c \
 		  px4iov2_pwm_servo.c
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h
index 9d684e394..67aaa0aff 100644
--- a/src/drivers/device/device.h
+++ b/src/drivers/device/device.h
@@ -130,7 +130,8 @@ public:
 	enum DeviceBusType {
 		DeviceBusType_UNKNOWN = 0,
 		DeviceBusType_I2C     = 1,
-		DeviceBusType_SPI     = 2
+		DeviceBusType_SPI     = 2,
+		DeviceBusType_UAVCAN  = 3,
 	};
 
 	/*
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index 6873f24b6..b10c3e18a 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -117,6 +117,23 @@ struct pwm_output_values {
 	unsigned			channel_count;
 };
 
+
+/**
+ * RC config values for a channel
+ *
+ * This allows for PX4IO_PAGE_RC_CONFIG values to be set without a
+ * param_get() dependency
+ */
+struct pwm_output_rc_config {
+	uint8_t channel;
+	uint16_t rc_min;
+	uint16_t rc_trim;
+	uint16_t rc_max;
+	uint16_t rc_dz;
+	uint16_t rc_assignment;
+	bool     rc_reverse;
+};
+
 /*
  * ORB tag for PWM outputs.
  */
@@ -214,7 +231,19 @@ ORB_DECLARE(output_pwm);
 #define PWM_SERVO_SET_TERMINATION_FAILSAFE	_IOC(_PWM_SERVO_BASE, 25)
 
 /** force safety switch on (to enable use of safety switch) */
-#define PWM_SERVO_SET_FORCE_SAFETY_ON  _IOC(_PWM_SERVO_BASE, 26)
+#define PWM_SERVO_SET_FORCE_SAFETY_ON		_IOC(_PWM_SERVO_BASE, 26)
+
+/** set RC config for a channel. This takes a pointer to pwm_output_rc_config */
+#define PWM_SERVO_SET_RC_CONFIG			_IOC(_PWM_SERVO_BASE, 27)
+
+/** set the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */
+#define PWM_SERVO_SET_OVERRIDE_OK		_IOC(_PWM_SERVO_BASE, 28)
+
+/** clear the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */
+#define PWM_SERVO_CLEAR_OVERRIDE_OK		_IOC(_PWM_SERVO_BASE, 29)
+
+/** setup OVERRIDE_IMMEDIATE behaviour on FMU fail */
+#define PWM_SERVO_SET_OVERRIDE_IMMEDIATE	_IOC(_PWM_SERVO_BASE, 30)
 
 /*
  *
diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c
index 6e0839043..bccdf1190 100644
--- a/src/drivers/frsky_telemetry/frsky_telemetry.c
+++ b/src/drivers/frsky_telemetry/frsky_telemetry.c
@@ -84,7 +84,7 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or
 	/* Back up the original UART configuration to restore it after exit */
 	int termios_state;
 	if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
-		warnx("ERROR get termios config %s: %d\n", uart_name, termios_state);
+		warnx("ERR: tcgetattr%s: %d\n", uart_name, termios_state);
 		close(uart);
 		return -1;
 	}
@@ -100,13 +100,13 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or
 	static const speed_t speed = B9600;
 
 	if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
-		warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
+		warnx("ERR: %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
 		close(uart);
 		return -1;
 	}
 
 	if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
-		warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
+		warnx("ERR: %s (tcsetattr)\n", uart_name);
 		close(uart);
 		return -1;
 	}
@@ -151,9 +151,6 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
 		}
 	}
 
-	/* Print welcome text */
-	warnx("FrSky telemetry interface starting...");
-
 	/* Open UART */
 	struct termios uart_config_original;
 	const int uart = frsky_open_uart(device_name, &uart_config_original);
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 5fb4b9ff8..47c907bd3 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -274,7 +274,6 @@ GPS::task_main_trampoline(void *arg)
 void
 GPS::task_main()
 {
-	log("starting");
 
 	/* open the serial port */
 	_serial_fd = ::open(_port, O_RDWR);
diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp
index f0dc0c651..9b5c8133b 100644
--- a/src/drivers/hil/hil.cpp
+++ b/src/drivers/hil/hil.cpp
@@ -442,8 +442,6 @@ HIL::task_main()
 	/* make sure servos are off */
 	// up_pwm_servo_deinit();
 
-	log("stopping");
-
 	/* note - someone else is responsible for restoring the GPIO config */
 
 	/* tell the dtor that we are exiting */
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index e4ecfa6b5..81f767965 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -1049,11 +1049,9 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 	 * LSM/Ga, giving 1.16 and 1.08 */
 	float expected_cal[3] = { 1.16f, 1.08f, 1.08f };
 
-	warnx("starting mag scale calibration");
-
 	/* start the sensor polling at 50 Hz */
 	if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) {
-		warn("failed to set 2Hz poll rate");
+		warn("FAILED: SENSORIOCSPOLLRATE 2Hz");
 		ret = 1;
 		goto out;
 	}
@@ -1061,25 +1059,25 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 	/* Set to 2.5 Gauss. We ask for 3 to get the right part of
          * the chained if statement above. */
 	if (OK != ioctl(filp, MAGIOCSRANGE, 3)) {
-		warnx("failed to set 2.5 Ga range");
+		warnx("FAILED: MAGIOCSRANGE 3.3 Ga");
 		ret = 1;
 		goto out;
 	}
 
 	if (OK != ioctl(filp, MAGIOCEXSTRAP, 1)) {
-		warnx("failed to enable sensor calibration mode");
+		warnx("FAILED: MAGIOCEXSTRAP 1");
 		ret = 1;
 		goto out;
 	}
 
 	if (OK != ioctl(filp, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) {
-		warn("WARNING: failed to get scale / offsets for mag");
+		warn("FAILED: MAGIOCGSCALE 1");
 		ret = 1;
 		goto out;
 	}
 
 	if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
-		warn("WARNING: failed to set null scale / offsets for mag");
+		warn("FAILED: MAGIOCSSCALE 1");
 		ret = 1;
 		goto out;
 	}
@@ -1094,7 +1092,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 		ret = ::poll(&fds, 1, 2000);
 
 		if (ret != 1) {
-			warn("timed out waiting for sensor data");
+			warn("ERROR: TIMEOUT 1");
 			goto out;
 		}
 
@@ -1102,7 +1100,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 		sz = ::read(fd, &report, sizeof(report));
 
 		if (sz != sizeof(report)) {
-			warn("periodic read failed");
+			warn("ERROR: READ 1");
 			ret = -EIO;
 			goto out;
 		}
@@ -1118,7 +1116,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 		ret = ::poll(&fds, 1, 2000);
 
 		if (ret != 1) {
-			warn("timed out waiting for sensor data");
+			warn("ERROR: TIMEOUT 2");
 			goto out;
 		}
 
@@ -1126,7 +1124,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 		sz = ::read(fd, &report, sizeof(report));
 
 		if (sz != sizeof(report)) {
-			warn("periodic read failed");
+			warn("ERROR: READ 2");
 			ret = -EIO;
 			goto out;
 		}
@@ -1142,33 +1140,19 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 			sum_excited[1] += cal[1];
 			sum_excited[2] += cal[2];
 		}
-
-		//warnx("periodic read %u", i);
-		//warnx("measurement: %.6f  %.6f  %.6f", (double)report.x, (double)report.y, (double)report.z);
-		//warnx("cal: %.6f  %.6f  %.6f", (double)cal[0], (double)cal[1], (double)cal[2]);
 	}
 
 	if (good_count < 5) {
-		warn("failed calibration");
 		ret = -EIO;
 		goto out;
 	}
 
-#if 0
-	warnx("measurement avg: %.6f  %.6f  %.6f", 
-	      (double)sum_excited[0]/good_count, 
-	      (double)sum_excited[1]/good_count, 
-	      (double)sum_excited[2]/good_count);
-#endif
-
 	float scaling[3];
 
 	scaling[0] = sum_excited[0] / good_count;
 	scaling[1] = sum_excited[1] / good_count;
 	scaling[2] = sum_excited[2] / good_count;
 
-	warnx("axes scaling: %.6f  %.6f  %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
-
 	/* set scaling in device */
 	mscale_previous.x_scale = scaling[0];
 	mscale_previous.y_scale = scaling[1];
@@ -1179,29 +1163,26 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 out:
 
 	if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
-		warn("failed to set new scale / offsets for mag");
+		warn("FAILED: MAGIOCSSCALE 2");
 	}
 
 	/* set back to normal mode */
 	/* Set to 1.1 Gauss */
 	if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) {
-		warnx("failed to set 1.1 Ga range");
+		warnx("FAILED: MAGIOCSRANGE 1.1 Ga");
 	}
 
 	if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
-		warnx("failed to disable sensor calibration mode");
+		warnx("FAILED: MAGIOCEXSTRAP 0");
 	}
 
 	if (ret == OK) {
-		if (!check_scale()) {
-			warnx("mag scale calibration successfully finished.");
-		} else {
-			warnx("mag scale calibration finished with invalid results.");
+		if (check_scale()) {
+			/* failed */
+			warnx("FAILED: SCALE");
 			ret = ERROR;
 		}
 
-	} else {
-		warnx("mag scale calibration failed.");
 	}
 
 	return ret;
diff --git a/src/drivers/hmc5883/module.mk b/src/drivers/hmc5883/module.mk
index 5daa01dc5..be2ee7276 100644
--- a/src/drivers/hmc5883/module.mk
+++ b/src/drivers/hmc5883/module.mk
@@ -42,3 +42,5 @@ SRCS		= hmc5883.cpp
 MODULE_STACKSIZE	= 1200
 
 EXTRACXXFLAGS	= -Weffc++
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/hott/comms.cpp b/src/drivers/hott/comms.cpp
index cb8bbba37..60a49b559 100644
--- a/src/drivers/hott/comms.cpp
+++ b/src/drivers/hott/comms.cpp
@@ -55,7 +55,7 @@ open_uart(const char *device)
 	const int uart = open(device, O_RDWR | O_NOCTTY);
 
 	if (uart < 0) {
-		err(1, "Error opening port: %s", device);
+		err(1, "ERR: opening %s", device);
 	}
 	
 	/* Back up the original uart configuration to restore it after exit */	
@@ -63,7 +63,7 @@ open_uart(const char *device)
 	struct termios uart_config_original;
 	if ((termios_state = tcgetattr(uart, &uart_config_original)) < 0) {
 		close(uart);
-		err(1, "Error getting baudrate / termios config for %s: %d", device, termios_state);
+		err(1, "ERR: %s: %d", device, termios_state);
 	}
 
 	/* Fill the struct for the new configuration */
@@ -76,13 +76,13 @@ open_uart(const char *device)
 	/* Set baud rate */
 	if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
 		close(uart);
-		err(1, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)",
+		err(1, "ERR: %s: %d (cfsetispeed, cfsetospeed)",
 			 device, termios_state);
 	}
 
 	if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
 		close(uart);
-		err(1, "Error setting baudrate / termios config for %s (tcsetattr)", device);
+		err(1, "ERR: %s (tcsetattr)", device);
 	}
 
 	/* Activate single wire mode */
diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp
index a3d3a3933..8ab9d8d55 100644
--- a/src/drivers/hott/hott_sensors/hott_sensors.cpp
+++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp
@@ -204,7 +204,7 @@ hott_sensors_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 
 		if (thread_running) {
-			warnx("deamon already running");
+			warnx("already running");
 			exit(0);
 		}
 
@@ -225,10 +225,10 @@ hott_sensors_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
-			warnx("daemon is running");
+			warnx("is running");
 
 		} else {
-			warnx("daemon not started");
+			warnx("not started");
 		}
 
 		exit(0);
diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp
index d293f9954..edbb14172 100644
--- a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp
+++ b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp
@@ -230,7 +230,7 @@ hott_telemetry_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 
 		if (thread_running) {
-			warnx("deamon already running");
+			warnx("already running");
 			exit(0);
 		}
 
@@ -251,10 +251,10 @@ hott_telemetry_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
-			warnx("daemon is running");
+			warnx("is running");
 
 		} else {
-			warnx("daemon not started");
+			warnx("not started");
 		}
 
 		exit(0);
diff --git a/src/drivers/l3gd20/module.mk b/src/drivers/l3gd20/module.mk
index 5630e7aec..3d64d62be 100644
--- a/src/drivers/l3gd20/module.mk
+++ b/src/drivers/l3gd20/module.mk
@@ -8,3 +8,5 @@ SRCS		 = l3gd20.cpp
 MODULE_STACKSIZE	= 1200
 
 EXTRACXXFLAGS	= -Weffc++
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/lsm303d/module.mk b/src/drivers/lsm303d/module.mk
index b4f3974f4..0421eb113 100644
--- a/src/drivers/lsm303d/module.mk
+++ b/src/drivers/lsm303d/module.mk
@@ -8,3 +8,5 @@ SRCS		 = lsm303d.cpp
 MODULE_STACKSIZE	= 1200
 
 EXTRACXXFLAGS	= -Weffc++
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index 1d9a463ad..ba46de379 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -519,7 +519,7 @@ test()
 		ret = poll(&fds, 1, 2000);
 
 		if (ret != 1) {
-			errx(1, "timed out waiting for sensor data");
+			errx(1, "timed out");
 		}
 
 		/* now go get it */
diff --git a/src/drivers/mpu6000/module.mk b/src/drivers/mpu6000/module.mk
index 5b4893b12..da9fcc0fc 100644
--- a/src/drivers/mpu6000/module.mk
+++ b/src/drivers/mpu6000/module.mk
@@ -42,3 +42,5 @@ SRCS		= mpu6000.cpp
 MODULE_STACKSIZE	= 1200
 
 EXTRACXXFLAGS	= -Weffc++
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp
index 6c68636c6..804027b05 100644
--- a/src/drivers/px4flow/px4flow.cpp
+++ b/src/drivers/px4flow/px4flow.cpp
@@ -76,7 +76,7 @@
 #define PX4FLOW_BUS 			PX4_I2C_BUS_EXPANSION
 #define I2C_FLOW_ADDRESS 		0x42 //* 7-bit address. 8-bit address is 0x84
 //range 0x42 - 0x49
- 
+
 /* PX4FLOW Registers addresses */
 #define PX4FLOW_REG	0x00		/* Measure Register */
 
@@ -200,7 +200,7 @@ PX4FLOW::PX4FLOW(int bus, int address) :
 	_buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows"))
 {
 	// enable debug() calls
-	_debug_enabled = true;
+	_debug_enabled = false;
 
 	// work_cancel in the dtor will explode if we don't do this...
 	memset(&_work, 0, sizeof(_work));
@@ -240,7 +240,7 @@ PX4FLOW::init()
 	_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report);
 
 	if (_px4flow_topic < 0) {
-		debug("failed to create px4flow object. Did you start uOrb?");
+		warnx("failed to create px4flow object. Did you start uOrb?");
 	}
 
 	ret = OK;
@@ -442,8 +442,6 @@ PX4FLOW::measure()
 
 	if (OK != ret) {
 		perf_count(_comms_errors);
-		log("i2c::transfer returned %d", ret);
-		printf("i2c::transfer flow returned %d");
 		return ret;
 	}
 
@@ -465,7 +463,7 @@ PX4FLOW::collect()
 	ret = transfer(nullptr, 0, &val[0], 22);
 
 	if (ret < 0) {
-		log("error reading from sensor: %d", ret);
+		debug("error reading from sensor: %d", ret);
 		perf_count(_comms_errors);
 		perf_end(_sample_perf);
 		return ret;
@@ -565,7 +563,7 @@ PX4FLOW::cycle()
 
 		/* perform collection */
 		if (OK != collect()) {
-			log("collection error");
+			debug("collection error");
 			/* restart the measurement state machine */
 			start();
 			return;
@@ -592,7 +590,7 @@ PX4FLOW::cycle()
 
 	/* measurement phase */
 	if (OK != measure()) {
-		log("measure error");
+		debug("measure error");
 	}
 
 	/* next phase is collection */
diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk
index a60f1a434..a06323a52 100644
--- a/src/drivers/px4fmu/module.mk
+++ b/src/drivers/px4fmu/module.mk
@@ -8,3 +8,5 @@ SRCS		 = fmu.cpp
 MODULE_STACKSIZE = 1200
 
 EXTRACXXFLAGS	= -Weffc++
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/px4io/module.mk b/src/drivers/px4io/module.mk
index 5b838fb75..924283356 100644
--- a/src/drivers/px4io/module.mk
+++ b/src/drivers/px4io/module.mk
@@ -48,3 +48,5 @@ INCLUDE_DIRS    += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/com
 MODULE_STACKSIZE = 1200
 
 EXTRACXXFLAGS	= -Weffc++
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index fd9eb4170..519ba663a 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1245,34 +1245,40 @@ PX4IO::io_set_rc_config()
 	 *       for compatibility reasons with existing
 	 *       autopilots / GCS'.
 	 */
-	param_get(param_find("RC_MAP_ROLL"), &ichan);
 
-	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
+	/* ROLL */
+	param_get(param_find("RC_MAP_ROLL"), &ichan);
+	if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
 		input_map[ichan - 1] = 0;
+	}
 
+	/* PITCH */
 	param_get(param_find("RC_MAP_PITCH"), &ichan);
-
-	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
+	if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
 		input_map[ichan - 1] = 1;
+	}
 
+	/* YAW */
 	param_get(param_find("RC_MAP_YAW"), &ichan);
-
-	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
+	if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
 		input_map[ichan - 1] = 2;
+	}
 
+	/* THROTTLE */
 	param_get(param_find("RC_MAP_THROTTLE"), &ichan);
-
-	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
+	if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
 		input_map[ichan - 1] = 3;
+	}
 
+	/* FLAPS */
 	param_get(param_find("RC_MAP_FLAPS"), &ichan);
-
-	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
+	if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
 		input_map[ichan - 1] = 4;
+	}
 
+	/* MAIN MODE SWITCH */
 	param_get(param_find("RC_MAP_MODE_SW"), &ichan);
-
-	if ((ichan >= 0) && (ichan < (int)_max_rc_input)) {
+	if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
 		/* use out of normal bounds index to indicate special channel */
 		input_map[ichan - 1] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH;
 	}
@@ -1651,10 +1657,6 @@ PX4IO::io_publish_raw_rc()
 int
 PX4IO::io_publish_pwm_outputs()
 {
-	/* if no FMU comms(!) just don't publish */
-	if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK))
-		return OK;
-
 	/* data we are going to fetch */
 	actuator_outputs_s outputs;
 	outputs.timestamp = hrt_absolute_time();
@@ -2055,7 +2057,7 @@ PX4IO::print_status(bool extended_status)
 		((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "")
 		);
 	uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
-	printf("arming 0x%04x%s%s%s%s%s%s%s%s\n",
+	printf("arming 0x%04x%s%s%s%s%s%s%s%s%s%s\n",
 	       arming,
 	       ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)		? " FMU_ARMED" : " FMU_DISARMED"),
 	       ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)		? " IO_ARM_OK" : " IO_ARM_DENIED"),
@@ -2065,7 +2067,8 @@ PX4IO::print_status(bool extended_status)
 	       ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)	? " ALWAYS_PWM_ENABLE" : ""),
 	       ((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN)		? " LOCKDOWN" : ""),
 	       ((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE)		? " FORCE_FAILSAFE" : ""),
-	       ((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : "")
+	       ((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : ""),
+	       ((arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) ? " OVERRIDE_IMMEDIATE" : "")
 	       );
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
@@ -2305,6 +2308,19 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 		}
 		break;
 
+	case PWM_SERVO_SET_OVERRIDE_IMMEDIATE:
+		/* control whether override on FMU failure is
+                   immediate or waits for override threshold on mode
+                   switch */
+		if (arg == 0) {
+			/* clear override immediate flag */
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE, 0);
+		} else {
+			/* set override immediate flag */
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE);
+		}
+		break;
+
 	case DSM_BIND_START:
 
 		/* only allow DSM2, DSM-X and DSM-X with more than 7 channels */
@@ -2566,6 +2582,42 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 
 		break;
 
+	case PWM_SERVO_SET_RC_CONFIG: {
+		/* enable setting of RC configuration without relying
+		   on param_get() 
+		*/
+		struct pwm_output_rc_config* config = (struct pwm_output_rc_config*)arg;
+		if (config->channel >= _max_actuators) {
+			/* fail with error */
+			return E2BIG;
+		}
+
+		/* copy values to registers in IO */
+		uint16_t regs[PX4IO_P_RC_CONFIG_STRIDE];
+		uint16_t offset = config->channel * PX4IO_P_RC_CONFIG_STRIDE;
+		regs[PX4IO_P_RC_CONFIG_MIN]        = config->rc_min;
+		regs[PX4IO_P_RC_CONFIG_CENTER]     = config->rc_trim;
+		regs[PX4IO_P_RC_CONFIG_MAX]        = config->rc_max;
+		regs[PX4IO_P_RC_CONFIG_DEADZONE]   = config->rc_dz;
+		regs[PX4IO_P_RC_CONFIG_ASSIGNMENT] = config->rc_assignment;
+		regs[PX4IO_P_RC_CONFIG_OPTIONS]    = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
+		if (config->rc_reverse) {
+			regs[PX4IO_P_RC_CONFIG_OPTIONS] |= PX4IO_P_RC_CONFIG_OPTIONS_REVERSE;
+		}
+		ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE);
+		break;
+	}
+
+	case PWM_SERVO_SET_OVERRIDE_OK:
+		/* set the 'OVERRIDE OK' bit */
+		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK);
+		break;
+
+	case PWM_SERVO_CLEAR_OVERRIDE_OK:
+		/* clear the 'OVERRIDE OK' bit */
+		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0);
+		break;
+
 	default:
 		/* see if the parent class can make any use of it */
 		ret = CDev::ioctl(filep, cmd, arg);
diff --git a/src/drivers/stm32/adc/module.mk b/src/drivers/stm32/adc/module.mk
index 48620feea..38ea490a0 100644
--- a/src/drivers/stm32/adc/module.mk
+++ b/src/drivers/stm32/adc/module.mk
@@ -40,3 +40,5 @@ MODULE_COMMAND	= adc
 SRCS		= adc.cpp
 
 INCLUDE_DIRS	+= $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/stm32/module.mk b/src/drivers/stm32/module.mk
index bb751c7f6..54428e270 100644
--- a/src/drivers/stm32/module.mk
+++ b/src/drivers/stm32/module.mk
@@ -41,3 +41,5 @@ SRCS		= drv_hrt.c \
 		  drv_pwm_servo.c
 
 INCLUDE_DIRS	+= $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/stm32/tone_alarm/module.mk b/src/drivers/stm32/tone_alarm/module.mk
index 827cf30b2..25a194ef6 100644
--- a/src/drivers/stm32/tone_alarm/module.mk
+++ b/src/drivers/stm32/tone_alarm/module.mk
@@ -40,3 +40,5 @@ MODULE_COMMAND	= tone_alarm
 SRCS		= tone_alarm.cpp
 
 INCLUDE_DIRS	+= $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp
index 0ed210cf4..cfcc48b62 100644
--- a/src/lib/external_lgpl/tecs/tecs.cpp
+++ b/src/lib/external_lgpl/tecs/tecs.cpp
@@ -321,31 +321,29 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
 			ff_throttle = nomThr - STEdot_dem / _STEdot_min * nomThr;
 		}
 
-		// Calculate PD + FF throttle
+		// Calculate PD + FF throttle and constrain to avoid blow-up of the integrator later
 		_throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle;
+		_throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
 
 		// Rate limit PD + FF throttle
 		// Calculate the throttle increment from the specified slew time
 		if (fabsf(_throttle_slewrate) > 0.01f) {
 			float thrRateIncr = _DT * (_THRmaxf - _THRminf) * _throttle_slewrate;
-
 			_throttle_dem = constrain(_throttle_dem,
-						  _last_throttle_dem - thrRateIncr,
-						  _last_throttle_dem + thrRateIncr);
+						_last_throttle_dem - thrRateIncr,
+						_last_throttle_dem + thrRateIncr);
 		}
 
 		// Ensure _last_throttle_dem is always initialized properly
-		// Also: The throttle_slewrate limit is only applied to throttle_dem, but does not limit the integrator!!
 		_last_throttle_dem = _throttle_dem;
 
-
 		// Calculate integrator state upper and lower limits
-		// Set to a value thqat will allow 0.1 (10%) throttle saturation to allow for noise on the demand
+		// Set to a value that will allow 0.1 (10%) throttle saturation to allow for noise on the demand
 		float integ_max = (_THRmaxf - _throttle_dem + 0.1f);
 		float integ_min = (_THRminf - _throttle_dem - 0.1f);
 
 		// Calculate integrator state, constraining state
-		// Set integrator to a max throttle value dduring climbout
+		// Set integrator to a max throttle value during climbout
 		_integ6_state = _integ6_state + (_STE_error * _integGain) * _DT * K_STE2Thr;
 
 		if (_climbOutDem) {
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index 35dc39ec6..e2dbc30dd 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -206,10 +206,6 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 			      0,  0,  1.f
 			     };		/**< init: identity matrix */
 
-	// print text
-	printf("Extended Kalman Filter Attitude Estimator initialized..\n\n");
-	fflush(stdout);
-
 	int overloadcounter = 19;
 
 	/* Initialize filter */
diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp
index 31c9157e1..e0bcbc6e9 100644
--- a/src/modules/bottle_drop/bottle_drop.cpp
+++ b/src/modules/bottle_drop/bottle_drop.cpp
@@ -223,7 +223,7 @@ BottleDrop::start()
 	_main_task = task_spawn_cmd("bottle_drop",
 				    SCHED_DEFAULT,
 				    SCHED_PRIORITY_DEFAULT + 15,
-				    2048,
+				    1500,
 				    (main_t)&BottleDrop::task_main_trampoline,
 				    nullptr);
 
@@ -283,7 +283,6 @@ BottleDrop::drop()
 	// force the door open if we have to
 	if (_doors_opened == 0) {
 		open_bay();
-		warnx("bay not ready, forced open");
 	}
 
 	while (hrt_elapsed_time(&_doors_opened) < 500 * 1000 && hrt_elapsed_time(&starttime) < 2000000) {
@@ -723,16 +722,16 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd)
 		if (cmd->param1 > 0.5f && cmd->param2 > 0.5f) {
 			open_bay();
 			drop();
-			mavlink_log_info(_mavlink_fd, "#audio: drop bottle");
+			mavlink_log_critical(_mavlink_fd, "drop bottle");
 
 		} else if (cmd->param1 > 0.5f) {
 			open_bay();
-			mavlink_log_info(_mavlink_fd, "#audio: opening bay");
+			mavlink_log_critical(_mavlink_fd, "opening bay");
 
 		} else {
 			lock_release();
 			close_bay();
-			mavlink_log_info(_mavlink_fd, "#audio: closing bay");
+			mavlink_log_critical(_mavlink_fd, "closing bay");
 		}
 
 		answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
@@ -743,12 +742,12 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd)
 		switch ((int)(cmd->param1 + 0.5f)) {
 		case 0:
 			_drop_approval = false;
-			mavlink_log_info(_mavlink_fd, "#audio: got drop position, no approval");
+			mavlink_log_critical(_mavlink_fd, "got drop position, no approval");
 			break;
 
 		case 1:
 			_drop_approval = true;
-			mavlink_log_info(_mavlink_fd, "#audio: got drop position and approval");
+			mavlink_log_critical(_mavlink_fd, "got drop position and approval");
 			break;
 
 		default:
@@ -818,19 +817,19 @@ BottleDrop::answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESUL
 		break;
 
 	case VEHICLE_CMD_RESULT_DENIED:
-		mavlink_log_critical(_mavlink_fd, "#audio: command denied: %u", cmd->command);
+		mavlink_log_critical(_mavlink_fd, "command denied: %u", cmd->command);
 		break;
 
 	case VEHICLE_CMD_RESULT_FAILED:
-		mavlink_log_critical(_mavlink_fd, "#audio: command failed: %u", cmd->command);
+		mavlink_log_critical(_mavlink_fd, "command failed: %u", cmd->command);
 		break;
 
 	case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
-		mavlink_log_critical(_mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command);
+		mavlink_log_critical(_mavlink_fd, "command temporarily rejected: %u", cmd->command);
 		break;
 
 	case VEHICLE_CMD_RESULT_UNSUPPORTED:
-		mavlink_log_critical(_mavlink_fd, "#audio: command unsupported: %u", cmd->command);
+		mavlink_log_critical(_mavlink_fd, "command unsupported: %u", cmd->command);
 		break;
 
 	default:
diff --git a/src/modules/bottle_drop/module.mk b/src/modules/bottle_drop/module.mk
index 6b18fff55..df9f5473b 100644
--- a/src/modules/bottle_drop/module.mk
+++ b/src/modules/bottle_drop/module.mk
@@ -39,3 +39,7 @@ MODULE_COMMAND		= bottle_drop
 
 SRCS			= bottle_drop.cpp \
 			  bottle_drop_params.c
+
+MAXOPTIMIZATION	 = -Os
+
+MODULE_STACKSIZE = 1200
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 46caddd46..e081955d0 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -220,8 +220,6 @@ void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actua
 
 void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
 
-void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status);
-
 transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man);
 
 void set_control_mode();
@@ -312,12 +310,16 @@ int commander_main(int argc, char *argv[])
 	}
 
 	if (!strcmp(argv[1], "arm")) {
-		arm_disarm(true, mavlink_fd, "command line");
+		int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0);
+		arm_disarm(true, mavlink_fd_local, "command line");
+		close(mavlink_fd_local);
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "disarm")) {
-		arm_disarm(false, mavlink_fd, "command line");
+		int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0);
+		arm_disarm(false, mavlink_fd_local, "command line");
+		close(mavlink_fd_local);
 		exit(0);
 	}
 
@@ -760,7 +762,10 @@ int commander_thread_main(int argc, char *argv[])
 	nav_states_str[NAVIGATION_STATE_AUTO_MISSION]		= "AUTO_MISSION";
 	nav_states_str[NAVIGATION_STATE_AUTO_LOITER]		= "AUTO_LOITER";
 	nav_states_str[NAVIGATION_STATE_AUTO_RTL]		= "AUTO_RTL";
+	nav_states_str[NAVIGATION_STATE_AUTO_RCRECOVER]		= "AUTO_RCRECOVER";
 	nav_states_str[NAVIGATION_STATE_AUTO_RTGS]		= "AUTO_RTGS";
+	nav_states_str[NAVIGATION_STATE_AUTO_LANDENGFAIL]	= "AUTO_LANDENGFAIL";
+	nav_states_str[NAVIGATION_STATE_AUTO_LANDGPSFAIL]	= "AUTO_LANDGPSFAIL";
 	nav_states_str[NAVIGATION_STATE_ACRO]			= "ACRO";
 	nav_states_str[NAVIGATION_STATE_LAND]			= "LAND";
 	nav_states_str[NAVIGATION_STATE_DESCEND]		= "DESCEND";
@@ -1848,7 +1853,11 @@ int commander_thread_main(int argc, char *argv[])
 
 		if (status.failsafe != failsafe_old) {
 			status_changed = true;
-			mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %i", status.failsafe);
+			if (status.failsafe) {
+				mavlink_log_critical(mavlink_fd, "failsafe mode on");
+			} else {
+				mavlink_log_critical(mavlink_fd, "failsafe mode off");
+			}
 			failsafe_old = status.failsafe;
 		}
 
@@ -2196,18 +2205,6 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
-	case NAVIGATION_STATE_ACRO:
-		control_mode.flag_control_manual_enabled = true;
-		control_mode.flag_control_auto_enabled = false;
-		control_mode.flag_control_rates_enabled = true;
-		control_mode.flag_control_attitude_enabled = false;
-		control_mode.flag_control_altitude_enabled = false;
-		control_mode.flag_control_climb_rate_enabled = false;
-		control_mode.flag_control_position_enabled = false;
-		control_mode.flag_control_velocity_enabled = false;
-		control_mode.flag_control_termination_enabled = false;
-		break;
-
 	case NAVIGATION_STATE_ALTCTL:
 		control_mode.flag_control_manual_enabled = true;
 		control_mode.flag_control_auto_enabled = false;
@@ -2220,64 +2217,6 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
-	case NAVIGATION_STATE_OFFBOARD:
-		control_mode.flag_control_manual_enabled = false;
-		control_mode.flag_control_auto_enabled = false;
-		control_mode.flag_control_offboard_enabled = true;
-
-		switch (sp_offboard.mode) {
-		case OFFBOARD_CONTROL_MODE_DIRECT_RATES:
-			control_mode.flag_control_rates_enabled = true;
-			control_mode.flag_control_attitude_enabled = false;
-			control_mode.flag_control_altitude_enabled = false;
-			control_mode.flag_control_climb_rate_enabled = false;
-			control_mode.flag_control_position_enabled = false;
-			control_mode.flag_control_velocity_enabled = false;
-			break;
-
-		case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE:
-			control_mode.flag_control_rates_enabled = true;
-			control_mode.flag_control_attitude_enabled = true;
-			control_mode.flag_control_altitude_enabled = false;
-			control_mode.flag_control_climb_rate_enabled = false;
-			control_mode.flag_control_position_enabled = false;
-			control_mode.flag_control_velocity_enabled = false;
-			break;
-
-		case OFFBOARD_CONTROL_MODE_DIRECT_FORCE:
-			control_mode.flag_control_rates_enabled = true;
-			control_mode.flag_control_attitude_enabled = false;
-			control_mode.flag_control_force_enabled = true;
-			control_mode.flag_control_altitude_enabled = false;
-			control_mode.flag_control_climb_rate_enabled = false;
-			control_mode.flag_control_position_enabled = false;
-			control_mode.flag_control_velocity_enabled = false;
-			break;
-
-		case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED:
-		case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED:
-		case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED:
-		case OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED:
-			control_mode.flag_control_rates_enabled = true;
-			control_mode.flag_control_attitude_enabled = true;
-			control_mode.flag_control_altitude_enabled = true;
-			control_mode.flag_control_climb_rate_enabled = true;
-			control_mode.flag_control_position_enabled = true;
-			control_mode.flag_control_velocity_enabled = true;
-			//XXX: the flags could depend on sp_offboard.ignore
-			break;
-
-		default:
-			control_mode.flag_control_rates_enabled = false;
-			control_mode.flag_control_attitude_enabled = false;
-			control_mode.flag_control_altitude_enabled = false;
-			control_mode.flag_control_climb_rate_enabled = false;
-			control_mode.flag_control_position_enabled = false;
-			control_mode.flag_control_velocity_enabled = false;
-		}
-
-		break;
-
 	case NAVIGATION_STATE_POSCTL:
 		control_mode.flag_control_manual_enabled = true;
 		control_mode.flag_control_auto_enabled = false;
@@ -2293,6 +2232,7 @@ set_control_mode()
 	case NAVIGATION_STATE_AUTO_MISSION:
 	case NAVIGATION_STATE_AUTO_LOITER:
 	case NAVIGATION_STATE_AUTO_RTL:
+	case NAVIGATION_STATE_AUTO_RCRECOVER:
 	case NAVIGATION_STATE_AUTO_RTGS:
 	case NAVIGATION_STATE_AUTO_LANDENGFAIL:
 		control_mode.flag_control_manual_enabled = false;
@@ -2318,6 +2258,19 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
+	case NAVIGATION_STATE_ACRO:
+		control_mode.flag_control_manual_enabled = true;
+		control_mode.flag_control_auto_enabled = false;
+		control_mode.flag_control_rates_enabled = true;
+		control_mode.flag_control_attitude_enabled = false;
+		control_mode.flag_control_altitude_enabled = false;
+		control_mode.flag_control_climb_rate_enabled = false;
+		control_mode.flag_control_position_enabled = false;
+		control_mode.flag_control_velocity_enabled = false;
+		control_mode.flag_control_termination_enabled = false;
+		break;
+
+
 	case NAVIGATION_STATE_LAND:
 		control_mode.flag_control_manual_enabled = false;
 		control_mode.flag_control_auto_enabled = true;
@@ -2331,6 +2284,19 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
+	case NAVIGATION_STATE_DESCEND:
+		/* TODO: check if this makes sense */
+		control_mode.flag_control_manual_enabled = false;
+		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 = false;
+		control_mode.flag_control_velocity_enabled = false;
+		control_mode.flag_control_altitude_enabled = false;
+		control_mode.flag_control_climb_rate_enabled = true;
+		control_mode.flag_control_termination_enabled = false;
+		break;
+
 	case NAVIGATION_STATE_TERMINATION:
 		/* disable all controllers on termination */
 		control_mode.flag_control_manual_enabled = false;
@@ -2344,6 +2310,63 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = true;
 		break;
 
+	case NAVIGATION_STATE_OFFBOARD:
+		control_mode.flag_control_manual_enabled = false;
+		control_mode.flag_control_auto_enabled = false;
+		control_mode.flag_control_offboard_enabled = true;
+
+		switch (sp_offboard.mode) {
+		case OFFBOARD_CONTROL_MODE_DIRECT_RATES:
+			control_mode.flag_control_rates_enabled = true;
+			control_mode.flag_control_attitude_enabled = false;
+			control_mode.flag_control_altitude_enabled = false;
+			control_mode.flag_control_climb_rate_enabled = false;
+			control_mode.flag_control_position_enabled = false;
+			control_mode.flag_control_velocity_enabled = false;
+			break;
+
+		case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE:
+			control_mode.flag_control_rates_enabled = true;
+			control_mode.flag_control_attitude_enabled = true;
+			control_mode.flag_control_altitude_enabled = false;
+			control_mode.flag_control_climb_rate_enabled = false;
+			control_mode.flag_control_position_enabled = false;
+			control_mode.flag_control_velocity_enabled = false;
+			break;
+
+		case OFFBOARD_CONTROL_MODE_DIRECT_FORCE:
+			control_mode.flag_control_rates_enabled = true;
+			control_mode.flag_control_attitude_enabled = false;
+			control_mode.flag_control_force_enabled = true;
+			control_mode.flag_control_altitude_enabled = false;
+			control_mode.flag_control_climb_rate_enabled = false;
+			control_mode.flag_control_position_enabled = false;
+			control_mode.flag_control_velocity_enabled = false;
+			break;
+
+		case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED:
+		case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED:
+		case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED:
+		case OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED:
+			control_mode.flag_control_rates_enabled = true;
+			control_mode.flag_control_attitude_enabled = true;
+			control_mode.flag_control_altitude_enabled = true;
+			control_mode.flag_control_climb_rate_enabled = true;
+			control_mode.flag_control_position_enabled = true;
+			control_mode.flag_control_velocity_enabled = true;
+			//XXX: the flags could depend on sp_offboard.ignore
+			break;
+
+		default:
+			control_mode.flag_control_rates_enabled = false;
+			control_mode.flag_control_attitude_enabled = false;
+			control_mode.flag_control_altitude_enabled = false;
+			control_mode.flag_control_climb_rate_enabled = false;
+			control_mode.flag_control_position_enabled = false;
+			control_mode.flag_control_velocity_enabled = false;
+		}
+		break;
+
 	default:
 		break;
 	}
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 9568752ae..e37019d02 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -444,7 +444,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
  * Check failsafe and main status and set navigation status for navigator accordingly
  */
 bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished,
-		const bool stay_in_failsafe)
+		   const bool stay_in_failsafe)
 {
 	navigation_state_t nav_state_old = status->nav_state;
 
@@ -497,11 +497,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 		break;
 
 	case MAIN_STATE_AUTO_MISSION:
+
 		/* go into failsafe
 		 * - if commanded to do so
 		 * - if we have an engine failure
-		 * - if either the datalink is enabled and lost as well as RC is lost
-		 * - if there is no datalink and the mission is finished */
+		 * - depending on datalink, RC and if the mission is finished */
+
+		/* first look at the commands */
 		if (status->engine_failure_cmd) {
 			status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
 		} else if (status->data_link_lost_cmd) {
@@ -509,14 +511,17 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 		} else if (status->gps_failure_cmd) {
 			status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL;
 		} else if (status->rc_signal_lost_cmd) {
-			status->nav_state = NAVIGATION_STATE_AUTO_RTGS; //XXX
-		/* Finished handling commands which have priority , now handle failures */
+			status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
+
+		/* finished handling commands which have priority, now handle failures */
 		} else if (status->gps_failure) {
 			status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL;
 		} else if (status->engine_failure) {
 			status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
-		} else if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) ||
-		    (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) {
+
+		/* datalink loss enabled:
+		 * check for datalink lost: this should always trigger RTGS */
+		} else if (data_link_loss_enabled && status->data_link_lost) {
 			status->failsafe = true;
 
 			if (status->condition_global_position_valid && status->condition_home_position_valid) {
@@ -529,12 +534,15 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 				status->nav_state = NAVIGATION_STATE_TERMINATION;
 			}
 
-		/* also go into failsafe if just datalink is lost */
-		} else if (status->data_link_lost && data_link_loss_enabled) {
+		/* datalink loss disabled:
+		 * check if both, RC and datalink are lost during the mission
+		 * or RC is lost after the mission is finished: this should always trigger RCRECOVER */
+		} else if (!data_link_loss_enabled && ((status->rc_signal_lost && status->data_link_lost) ||
+						       (status->rc_signal_lost && mission_finished))) {
 			status->failsafe = true;
 
 			if (status->condition_global_position_valid && status->condition_home_position_valid) {
-				status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
+				status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
 			} else if (status->condition_local_position_valid) {
 				status->nav_state = NAVIGATION_STATE_LAND;
 			} else if (status->condition_local_altitude_valid) {
@@ -543,13 +551,8 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 				status->nav_state = NAVIGATION_STATE_TERMINATION;
 			}
 
-		/* don't bother if RC is lost and mission is not yet finished */
-		} else if (status->rc_signal_lost && !stay_in_failsafe) {
-
-			/* this mode is ok, we don't need RC for missions */
-			status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
+		/* stay where you are if you should stay in failsafe, otherwise everything is perfect */
 		} else if (!stay_in_failsafe){
-			/* everything is perfect */
 			status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
 		}
 		break;
diff --git a/src/modules/controllib/module.mk b/src/modules/controllib/module.mk
index f0139a4b7..2860d1efc 100644
--- a/src/modules/controllib/module.mk
+++ b/src/modules/controllib/module.mk
@@ -39,3 +39,5 @@ SRCS		 = test_params.c \
 		   block/BlockParam.cpp \
 		   uorb/blocks.cpp \
 		   blocks.cpp
+
+MAXOPTIMIZATION = -Os
diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c
index b2355d4d8..705e54be3 100644
--- a/src/modules/dataman/dataman.c
+++ b/src/modules/dataman/dataman.c
@@ -629,9 +629,6 @@ task_main(int argc, char *argv[])
 {
 	work_q_item_t *work;
 
-	/* inform about start */
-	warnx("Initializing..");
-
 	/* Initialize global variables */
 	g_key_offsets[0] = 0;
 
@@ -694,16 +691,15 @@ task_main(int argc, char *argv[])
 		if (sys_restart_val == DM_INIT_REASON_POWER_ON) {
 			warnx("Power on restart");
 			_restart(DM_INIT_REASON_POWER_ON);
-		}
-		else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) {
+		} else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) {
 			warnx("In flight restart");
 			_restart(DM_INIT_REASON_IN_FLIGHT);
-		}
-		else
+		} else {
 			warnx("Unknown restart");
-	}
-	else
+		}
+	} else {
 		warnx("Unknown restart");
+	}
 
 	/* We use two file descriptors, one for the caller context and one for the worker thread */
 	/* They are actually the same but we need to some way to reject caller request while the */
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 685f5e12f..e7805daa9 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -613,8 +613,11 @@ FixedwingEstimator::check_filter_state()
 			warn_index = max_warn_index;
 		}
 
-		warnx("reset: %s", feedback[warn_index]);
-		mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]);
+		// Do not warn about accel offset if we have no position updates
+		if (!(warn_index == 5 && _ekf->staticMode)) {
+			warnx("reset: %s", feedback[warn_index]);
+			mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]);
+		}
 	}
 
 	struct estimator_status_report rep;
@@ -1557,7 +1560,7 @@ FixedwingEstimator::start()
 	_estimator_task = task_spawn_cmd("ekf_att_pos_estimator",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 40,
-					 5000,
+					 7500,
 					 (main_t)&FixedwingEstimator::task_main_trampoline,
 					 nullptr);
 
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index bec706bad..9afe74af1 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -78,11 +78,7 @@ PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1);
 
 mavlink_system_t mavlink_system = {
 	100,
-	50,
-	MAV_TYPE_FIXED_WING,
-	0,
-	0,
-	0
+	50
 }; // System ID, 1-255, Component/Subsystem ID, 1-255
 
 /*
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 6b4edff78..fb9f65cf5 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -167,8 +167,10 @@ Mavlink::Mavlink() :
 	_param_initialized(false),
 	_param_system_id(0),
 	_param_component_id(0),
-	_param_system_type(0),
+	_param_system_type(MAV_TYPE_FIXED_WING),
 	_param_use_hil_gps(0),
+	_param_forward_externalsp(0),
+	_system_type(0),
 
 	/* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
@@ -524,7 +526,7 @@ void Mavlink::mavlink_update_system(void)
 	param_get(_param_system_type, &system_type);
 
 	if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
-		mavlink_system.type = system_type;
+		_system_type = system_type;
 	}
 
 	int32_t use_hil_gps;
@@ -755,7 +757,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg)
 
 	pthread_mutex_lock(&_send_mutex);
 
-	int buf_free = get_free_tx_buf();
+	unsigned buf_free = get_free_tx_buf();
 
 	uint8_t payload_len = mavlink_message_lengths[msgid];
 	unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
@@ -820,14 +822,14 @@ Mavlink::resend_message(mavlink_message_t *msg)
 
 	pthread_mutex_lock(&_send_mutex);
 
-	int buf_free = get_free_tx_buf();
+	unsigned buf_free = get_free_tx_buf();
 
 	_last_write_try_time = hrt_absolute_time();
 
 	unsigned packet_len = msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 
 	/* check if there is space in the buffer, let it overflow else */
-	if (buf_free < TX_BUFFER_GAP) {
+	if ((buf_free < TX_BUFFER_GAP) || (buf_free < packet_len)) {
 		/* no enough space in buffer to send */
 		count_txerr();
 		count_txerrbytes(packet_len);
@@ -1634,7 +1636,7 @@ Mavlink::start(int argc, char *argv[])
 	task_spawn_cmd(buf,
 		       SCHED_DEFAULT,
 		       SCHED_PRIORITY_DEFAULT,
-		       2900,
+		       2800,
 		       (main_t)&Mavlink::start_helper,
 		       (const char **)argv);
 
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 1f96e586b..ad5e5001b 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -265,6 +265,8 @@ public:
 
 	struct mavlink_logbuffer	*get_logbuffer() { return &_logbuffer; }
 
+	unsigned		get_system_type() { return _system_type; }
+
 protected:
 	Mavlink			*next;
 
@@ -354,6 +356,8 @@ private:
 	param_t			_param_use_hil_gps;
 	param_t			_param_forward_externalsp;
 
+	unsigned		_system_type;
+
 	perf_counter_t		_loop_perf;			/**< loop performance counter */
 	perf_counter_t		_txerr_perf;			/**< TX error counter */
 
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index cccb698bf..978aee118 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -302,7 +302,7 @@ protected:
 		msg.base_mode = 0;
 		msg.custom_mode = 0;
 		get_mavlink_mode_state(&status, &pos_sp_triplet, &msg.system_status, &msg.base_mode, &msg.custom_mode);
-		msg.type = mavlink_system.type;
+		msg.type = _mavlink->get_system_type();
 		msg.autopilot = MAV_AUTOPILOT_PX4;
 		msg.mavlink_version = 3;
 
@@ -382,11 +382,11 @@ protected:
 						clock_gettime(CLOCK_REALTIME, &ts);
 						/* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */
 						time_t gps_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9);
-						struct tm t;
-						gmtime_r(&gps_time_sec, &t);
+						struct tm tt;
+						gmtime_r(&gps_time_sec, &tt);
 
 						// XXX we do not want to interfere here with the SD log app
-						strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &t);
+						strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &tt);
 						snprintf(log_file_path, sizeof(log_file_path), "/fs/microsd/%s", log_file_name);
 						fp = fopen(log_file_path, "ab");
 					}
@@ -1353,15 +1353,17 @@ protected:
 
 			const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2;
 
+			unsigned system_type = _mavlink->get_system_type();
+
 			/* scale outputs depending on system type */
-			if (mavlink_system.type == MAV_TYPE_QUADROTOR ||
-				mavlink_system.type == MAV_TYPE_HEXAROTOR ||
-				mavlink_system.type == MAV_TYPE_OCTOROTOR) {
+			if (system_type == MAV_TYPE_QUADROTOR ||
+				system_type == MAV_TYPE_HEXAROTOR ||
+				system_type == MAV_TYPE_OCTOROTOR) {
 				/* multirotors: set number of rotor outputs depending on type */
 
 				unsigned n;
 
-				switch (mavlink_system.type) {
+				switch (system_type) {
 				case MAV_TYPE_QUADROTOR:
 					n = 4;
 					break;
@@ -1717,7 +1719,53 @@ protected:
 			msg.chan16_raw = (rc.channel_count > 15) ? rc.values[15] : UINT16_MAX;
 			msg.chan17_raw = (rc.channel_count > 16) ? rc.values[16] : UINT16_MAX;
 			msg.chan18_raw = (rc.channel_count > 17) ? rc.values[17] : UINT16_MAX;
-			msg.rssi = rc.rssi;
+
+			/* RSSI has a max value of 100, and when Spektrum or S.BUS are
+			 * available, the RSSI field is invalid, as they do not provide
+			 * an RSSI measurement. Use an out of band magic value to signal
+			 * these digital ports. XXX revise MAVLink spec to address this.
+			 * One option would be to use the top bit to toggle between RSSI
+			 * and input source mode.
+			 *
+			 * Full RSSI field: 0b 1 111 1111
+			 *
+			 *                     ^ If bit is set, RSSI encodes type + RSSI
+			 *
+			 *                       ^ These three bits encode a total of 8
+			 *                         digital RC input types.
+			 *                         0: PPM, 1: SBUS, 2: Spektrum, 2: ST24
+			 *                           ^ These four bits encode a total of
+			 *                             16 RSSI levels. 15 = full, 0 = no signal
+			 *
+			 */
+
+			/* Initialize RSSI with the special mode level flag */
+			msg.rssi = (1 << 7);
+
+			/* Set RSSI */
+			msg.rssi |= (rc.rssi <= 100) ? ((rc.rssi / 7) + 1) : 15;
+
+			switch (rc.input_source) {
+				case RC_INPUT_SOURCE_PX4FMU_PPM:
+				/* fallthrough */
+				case RC_INPUT_SOURCE_PX4IO_PPM:
+					msg.rssi |= (0 << 4);
+					break;
+				case RC_INPUT_SOURCE_PX4IO_SPEKTRUM:
+					msg.rssi |= (1 << 4);
+					break;
+				case RC_INPUT_SOURCE_PX4IO_SBUS:
+					msg.rssi |= (2 << 4);
+					break;
+				case RC_INPUT_SOURCE_PX4IO_ST24:
+					msg.rssi |= (3 << 4);
+					break;
+			}
+
+			if (rc.rc_lost) {
+				/* RSSI is by definition zero */
+				msg.rssi = 0;
+			}
 
 			_mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg);
 		}
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index bc092c7e9..ca00d1a67 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -538,12 +538,16 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 			offboard_control_sp.ignore &=  ~(1 << i);
 			offboard_control_sp.ignore |=  (set_position_target_local_ned.type_mask & (1 << i));
 		}
+
 		offboard_control_sp.ignore &=  ~(1 << OFB_IGN_BIT_YAW);
-			offboard_control_sp.ignore |=  (set_position_target_local_ned.type_mask & (1 << 10)) <<
-					OFB_IGN_BIT_YAW;
+		if (set_position_target_local_ned.type_mask & (1 << 10)) {
+			offboard_control_sp.ignore |=  (1 << OFB_IGN_BIT_YAW);
+		}
+
 		offboard_control_sp.ignore &=  ~(1 << OFB_IGN_BIT_YAWRATE);
-			offboard_control_sp.ignore |=  (set_position_target_local_ned.type_mask & (1 << 11)) <<
-					OFB_IGN_BIT_YAWRATE;
+		if (set_position_target_local_ned.type_mask & (1 << 11)) {
+			offboard_control_sp.ignore |=  (1 << OFB_IGN_BIT_YAWRATE);
+		}
 
 		offboard_control_sp.timestamp = hrt_absolute_time();
 
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 19c10198c..81a13edb8 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -721,8 +721,6 @@ MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[])
 void
 MulticopterAttitudeControl::task_main()
 {
-	warnx("started");
-	fflush(stdout);
 
 	/*
 	 * do subscriptions
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 d52138522..5918d6bc5 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -535,7 +535,7 @@ MulticopterPositionControl::reset_pos_sp()
 				- _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0);
 		_pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1)
 				- _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1);
-		mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1));
+		mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1));
 	}
 }
 
@@ -545,7 +545,7 @@ MulticopterPositionControl::reset_alt_sp()
 	if (_reset_alt_sp) {
 		_reset_alt_sp = false;
 		_pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2);
-		mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2));
+		mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2));
 	}
 }
 
@@ -857,10 +857,8 @@ MulticopterPositionControl::control_auto(float dt)
 void
 MulticopterPositionControl::task_main()
 {
-	warnx("started");
 
 	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-	mavlink_log_info(_mavlink_fd, "[mpc] started");
 
 	/*
 	 * do subscriptions
diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp
index 66f1c8c73..e789fd10d 100644
--- a/src/modules/navigator/datalinkloss.cpp
+++ b/src/modules/navigator/datalinkloss.cpp
@@ -156,6 +156,7 @@ DataLinkLoss::set_dll_item()
 		/* Request flight termination from the commander */
 		_navigator->get_mission_result()->flight_termination = true;
 		_navigator->publish_mission_result();
+		reset_mission_item_reached();
 		warnx("not switched to manual: request flight termination");
 		pos_sp_triplet->previous.valid = false;
 		pos_sp_triplet->current.valid = false;
@@ -188,6 +189,7 @@ DataLinkLoss::advance_dll()
 			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home");
 			_navigator->get_mission_result()->stay_in_failsafe = true;
 			_navigator->publish_mission_result();
+			reset_mission_item_reached();
 			_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
 		} else {
 			if (!_param_skipcommshold.get()) {
@@ -208,6 +210,7 @@ DataLinkLoss::advance_dll()
 		_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
 		_navigator->get_mission_result()->stay_in_failsafe = true;
 		_navigator->publish_mission_result();
+		reset_mission_item_reached();
 		break;
 	case DLL_STATE_FLYTOAIRFIELDHOMEWP:
 		_dll_state = DLL_STATE_TERMINATE;
@@ -215,6 +218,7 @@ DataLinkLoss::advance_dll()
 		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating");
 		_navigator->get_mission_result()->stay_in_failsafe = true;
 		_navigator->publish_mission_result();
+		reset_mission_item_reached();
 		break;
 	case DLL_STATE_TERMINATE:
 		warnx("dll end");
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 7fac69a61..0765e9b7c 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -371,7 +371,7 @@ Mission::set_mission_items()
 	} else {
 		/* no mission available or mission finished, switch to loiter */
 		if (_mission_type != MISSION_TYPE_NONE) {
-			mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished");
 
 			/* use last setpoint for loiter */
 			_navigator->set_can_loiter_at_sp(true);
@@ -595,13 +595,15 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 		dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
 	}
 
-	if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
-		/* mission item index out of bounds */
-		return false;
-	}
-
-	/* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */
+	/* Repeat this several times in case there are several DO JUMPS that we need to follow along, however, after
+	 * 10 iterations we have to assume that the DO JUMPS are probably cycling and give up. */
 	for (int i = 0; i < 10; i++) {
+
+		if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
+			/* mission item index out of bounds */
+			return false;
+		}
+
 		const ssize_t len = sizeof(struct mission_item_s);
 
 		/* read mission item to temp storage first to not overwrite current mission item if data damaged */
@@ -626,11 +628,12 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 				if (is_current) {
 					(mission_item_tmp.do_jump_current_count)++;
 					/* save repeat count */
-					if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_IN_FLIGHT_RESET, &mission_item_tmp, len) != len) {
+					if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_POWER_ON_RESET,
+					    &mission_item_tmp, len) != len) {
 						/* not supposed to happen unless the datamanager can't access the
 						 * dataman */
 						mavlink_log_critical(_navigator->get_mavlink_fd(),
-								"ERROR DO JUMP waypoint could not be written");
+								     "ERROR DO JUMP waypoint could not be written");
 						return false;
 					}
 				}
@@ -639,8 +642,10 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 				*mission_index_ptr = mission_item_tmp.do_jump_mission_index;
 
 			} else {
-				mavlink_log_critical(_navigator->get_mavlink_fd(),
-						 "DO JUMP repetitions completed");
+				if (is_current) {
+					mavlink_log_critical(_navigator->get_mavlink_fd(),
+							     "DO JUMP repetitions completed");
+				}
 				/* no more DO_JUMPS, therefore just try to continue with next mission item */
 				(*mission_index_ptr)++;
 			}
@@ -707,6 +712,7 @@ Mission::set_mission_item_reached()
 	_navigator->get_mission_result()->reached = true;
 	_navigator->get_mission_result()->seq_reached = _current_offboard_mission_index;
 	_navigator->publish_mission_result();
+	reset_mission_item_reached();
 }
 
 void
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index d550dcc4c..9cd609955 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -205,6 +205,7 @@ private:
 
 	bool		_can_loiter_at_sp;			/**< flags if current position SP can be used to loiter */
 	bool		_pos_sp_triplet_updated;		/**< flags if position SP triplet needs to be published */
+	bool 		_pos_sp_triplet_published_invalid_once;	/**< flags if position SP triplet has been published once to UORB */
 
 	control::BlockParamFloat _param_loiter_radius;	/**< loiter radius for fixedwing */
 	control::BlockParamFloat _param_acceptance_radius;	/**< acceptance for takeoff */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index a867dd0da..df620e5e7 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -137,6 +137,7 @@ Navigator::Navigator() :
 	_gpsFailure(this, "GPSF"),
 	_can_loiter_at_sp(false),
 	_pos_sp_triplet_updated(false),
+	_pos_sp_triplet_published_invalid_once(false),
 	_param_loiter_radius(this, "LOITER_RAD"),
 	_param_acceptance_radius(this, "ACC_RAD"),
 	_param_datalinkloss_obc(this, "DLL_OBC"),
@@ -289,8 +290,9 @@ Navigator::task_main()
 	navigation_capabilities_update();
 	params_update();
 
-	/* rate limit position updates to 50 Hz */
+	/* rate limit position and sensor updates to 50 Hz */
 	orb_set_interval(_global_pos_sub, 20);
+	orb_set_interval(_sensor_combined_sub, 20);
 
 	hrt_abstime mavlink_open_time = 0;
 	const hrt_abstime mavlink_open_interval = 500000;
@@ -426,12 +428,15 @@ Navigator::task_main()
 				_can_loiter_at_sp = false;
 				break;
 			case NAVIGATION_STATE_AUTO_MISSION:
+				_pos_sp_triplet_published_invalid_once = false;
 				_navigation_mode = &_mission;
 				break;
 			case NAVIGATION_STATE_AUTO_LOITER:
+				_pos_sp_triplet_published_invalid_once = false;
 				_navigation_mode = &_loiter;
 				break;
 			case NAVIGATION_STATE_AUTO_RCRECOVER:
+				_pos_sp_triplet_published_invalid_once = false;
 				if (_param_rcloss_obc.get() != 0) {
 					_navigation_mode = &_rcLoss;
 				} else {
@@ -439,11 +444,13 @@ Navigator::task_main()
 				}
 				break;
 			case NAVIGATION_STATE_AUTO_RTL:
-					_navigation_mode = &_rtl;
+				_pos_sp_triplet_published_invalid_once = false;
+				_navigation_mode = &_rtl;
 				break;
 			case NAVIGATION_STATE_AUTO_RTGS:
 				/* Use complex data link loss mode only when enabled via param
 				* otherwise use rtl */
+				_pos_sp_triplet_published_invalid_once = false;
 				if (_param_datalinkloss_obc.get() != 0) {
 					_navigation_mode = &_dataLinkLoss;
 				} else {
@@ -451,9 +458,11 @@ Navigator::task_main()
 				}
 				break;
 			case NAVIGATION_STATE_AUTO_LANDENGFAIL:
+				_pos_sp_triplet_published_invalid_once = false;
 				_navigation_mode = &_engineFailure;
 				break;
 			case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
+				_pos_sp_triplet_published_invalid_once = false;
 				_navigation_mode = &_gpsFailure;
 				break;
 			default:
@@ -467,9 +476,9 @@ Navigator::task_main()
 			_navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]);
 		}
 
-		/* if nothing is running, set position setpoint triplet invalid */
-		if (_navigation_mode == nullptr) {
-			// TODO publish empty sp only once
+		/* if nothing is running, set position setpoint triplet invalid once */
+		if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) {
+			_pos_sp_triplet_published_invalid_once = true;
 			_pos_sp_triplet.previous.valid = false;
 			_pos_sp_triplet.current.valid = false;
 			_pos_sp_triplet.next.valid = false;
@@ -497,8 +506,8 @@ Navigator::start()
 	/* start the task */
 	_navigator_task = task_spawn_cmd("navigator",
 					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 5,
-					 2000,
+					 SCHED_PRIORITY_DEFAULT + 20,
+					 1800,
 					 (main_t)&Navigator::task_main_trampoline,
 					 nullptr);
 
@@ -630,9 +639,6 @@ Navigator::publish_mission_result()
 		/* advertise and publish */
 		_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
 	}
-	/* reset reached bool */
-	_mission_result.reached = false;
-	_mission_result.finished = false;
 }
 
 void
diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp
index 5564a1c42..42392e739 100644
--- a/src/modules/navigator/rcloss.cpp
+++ b/src/modules/navigator/rcloss.cpp
@@ -163,6 +163,7 @@ RCLoss::advance_rcl()
 			_rcl_state = RCL_STATE_TERMINATE;
 			_navigator->get_mission_result()->stay_in_failsafe = true;
 			_navigator->publish_mission_result();
+			reset_mission_item_reached();
 		}
 		break;
 	case RCL_STATE_LOITER:
@@ -171,6 +172,7 @@ RCLoss::advance_rcl()
 		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating");
 		_navigator->get_mission_result()->stay_in_failsafe = true;
 		_navigator->publish_mission_result();
+		reset_mission_item_reached();
 		break;
 	case RCL_STATE_TERMINATE:
 		warnx("rcl end");
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 81bbaa658..e736a86d7 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -161,7 +161,7 @@ int position_estimator_inav_main(int argc, char *argv[])
 			thread_should_exit = true;
 
 		} else {
-			warnx("app not started");
+			warnx("not started");
 		}
 
 		exit(0);
@@ -169,10 +169,10 @@ int position_estimator_inav_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
-			warnx("app is running");
+			warnx("is running");
 
 		} else {
-			warnx("app not started");
+			warnx("not started");
 		}
 
 		exit(0);
@@ -210,10 +210,8 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e
  ****************************************************************************/
 int position_estimator_inav_thread_main(int argc, char *argv[])
 {
-	warnx("started");
 	int mavlink_fd;
 	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-	mavlink_log_info(mavlink_fd, "[inav] started");
 
 	float x_est[2] = { 0.0f, 0.0f };	// pos, vel
 	float y_est[2] = { 0.0f, 0.0f };	// pos, vel
@@ -389,8 +387,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					} else {
 						wait_baro = false;
 						baro_offset /= (float) baro_init_cnt;
-						warnx("baro offs: %.2f", (double)baro_offset);
-						mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", (double)baro_offset);
+						warnx("baro offs: %d", (int)baro_offset);
+						mavlink_log_info(mavlink_fd, "[inav] baro offs: %d", (int)baro_offset);
 						local_pos.z_valid = true;
 						local_pos.v_z_valid = true;
 					}
@@ -520,7 +518,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 							sonar_valid_time = t;
 							sonar_valid = true;
 							local_pos.surface_bottom_timestamp = t;
-							mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", (double)surface_offset);
+							mavlink_log_info(mavlink_fd, "[inav] new surface level: %d", (int)surface_offset);
 						}
 
 					} else {
@@ -721,8 +719,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 							/* initialize projection */
 							map_projection_init(&ref, lat, lon);
-							warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", (double)lat, (double)lon, (double)alt);
-							mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %.2f", (double)lat, (double)lon, (double)alt);
+							// XXX replace this print
+							warnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt);
+							mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt);
 						}
 					}
 
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index ad60ee03e..58c9429b6 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -41,6 +41,7 @@
 #include <stdbool.h>
 
 #include <drivers/drv_hrt.h>
+#include <drivers/drv_rc_input.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/ppm_decode.h>
 #include <rc/st24.h>
@@ -70,7 +71,6 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated)
 	uint8_t *bytes;
 	*dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes);
 	if (*dsm_updated) {
-		r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
 		r_raw_rc_count = temp_count & 0x7fff;
 		if (temp_count & 0x8000)
 			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
@@ -91,6 +91,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated)
 
 	for (unsigned i = 0; i < n_bytes; i++) {
 		/* set updated flag if one complete packet was parsed */
+		st24_rssi = RC_INPUT_RSSI_MAX;
 		*st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &rx_count,
 					&st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
 	}
@@ -170,6 +171,12 @@ controls_tick() {
 	perf_begin(c_gather_dsm);
 	bool dsm_updated, st24_updated;
 	(void)dsm_port_input(&rssi, &dsm_updated, &st24_updated);
+	if (dsm_updated) {
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
+	}
+	if (st24_updated) {
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24;
+	}
 	perf_end(c_gather_dsm);
 
 	perf_begin(c_gather_sbus);
@@ -417,6 +424,15 @@ controls_tick() {
 		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH))
 			override = true;
 
+		/*
+		  if the FMU is dead then enable override if we have a
+		  mixer and OVERRIDE_IMMEDIATE is set
+		 */
+		if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
+		    (r_setup_arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) &&
+		    (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK))
+			override = true;                
+
 		if (override) {
 
 			r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 9b2e047cb..c7e9ae3eb 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -183,6 +183,7 @@
 #define PX4IO_P_SETUP_ARMING_LOCKDOWN		(1 << 7) /* If set, the system operates normally, but won't actuate any servos */
 #define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE	(1 << 8) /* If set, the system will always output the failsafe values */
 #define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE	(1 << 9) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */
+#define PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE	(1 << 10) /* If set then on FMU failure override is immediate. Othewise it waits for the mode switch to go past the override thrshold */
 
 #define PX4IO_P_SETUP_PWM_RATES			2	/* bitmask, 0 = low rate, 1 = high rate */
 #define PX4IO_P_SETUP_PWM_DEFAULTRATE		3	/* 'low' PWM frame output rate in Hz */
diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c
index 30f32b38e..14ee9cb40 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -85,6 +85,9 @@ static volatile uint8_t msg_next_out, msg_next_in;
 #define NUM_MSG 2
 static char msg[NUM_MSG][40];
 
+static void heartbeat_blink(void);
+static void ring_blink(void);
+
 /*
  * add a debug message to be printed on the console
  */
@@ -124,8 +127,64 @@ heartbeat_blink(void)
 {
 	static bool heartbeat = false;
 	LED_BLUE(heartbeat = !heartbeat);
+}
+
+static void
+ring_blink(void)
+{
 #ifdef GPIO_LED4
-	LED_RING(heartbeat);
+
+	if (/* IO armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
+	/* and FMU is armed */ && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
+		LED_RING(1);
+		return;
+	}
+
+	// XXX this led code does have
+	// intentionally a few magic numbers.
+	const unsigned max_brightness = 118;
+
+	static unsigned counter = 0;
+	static unsigned brightness = max_brightness;
+	static unsigned brightness_counter = 0;
+	static unsigned on_counter = 0;
+
+	if (brightness_counter < max_brightness) {
+
+		bool on = ((on_counter * 100) / brightness_counter+1) <= ((brightness * 100) / max_brightness+1);
+
+		// XXX once led is PWM driven,
+		// remove the ! in the line below
+		// to return to the proper breathe
+		// animation / pattern (currently inverted)
+		LED_RING(!on);
+		brightness_counter++;
+
+		if (on) {
+			on_counter++;
+		}
+
+	} else {
+
+		if (counter >= 62) {
+			counter = 0;
+		}
+
+		int n;
+
+		if (counter < 32) {
+			n = counter;
+
+		} else {
+			n = 62 - counter;
+		}
+
+		brightness = (n * n) / 8;
+		brightness_counter = 0;
+		on_counter = 0;
+		counter++;
+	}
+
 #endif
 }
 
@@ -300,6 +359,8 @@ user_start(int argc, char *argv[])
                     heartbeat_blink();
                 }
 
+		ring_blink();
+
                 check_reboot();
 
 		/* check for debug activity (default: none) */
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 8186e4c78..93a33490f 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -140,7 +140,7 @@ extern pwm_limit_t pwm_limit;
 #define LED_BLUE(_s)			stm32_gpiowrite(GPIO_LED1, !(_s))
 #define LED_AMBER(_s)			stm32_gpiowrite(GPIO_LED2, !(_s))
 #define LED_SAFETY(_s)			stm32_gpiowrite(GPIO_LED3, !(_s))
-#define LED_RING(_s)			stm32_gpiowrite(GPIO_LED4, !(_s))
+#define LED_RING(_s)			stm32_gpiowrite(GPIO_LED4, (_s))
 
 #ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 49c2a9f56..fbfdd35db 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -191,7 +191,8 @@ volatile uint16_t	r_page_setup[] =
 					 PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \
 					 PX4IO_P_SETUP_ARMING_LOCKDOWN | \
 					 PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \
-					 PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)
+					 PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE | \
+					 PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE)
 #define PX4IO_P_SETUP_RATES_VALID	((1 << PX4IO_SERVO_COUNT) - 1)
 #define PX4IO_P_SETUP_RELAYS_VALID	((1 << PX4IO_RELAY_CHANNELS) - 1)
 
diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
index 6ead38d61..d76ec55f0 100644
--- a/src/modules/px4iofirmware/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
@@ -57,6 +57,7 @@
 #define SBUS_FLAGS_BYTE		23
 #define SBUS_FAILSAFE_BIT	3
 #define SBUS_FRAMELOST_BIT	2
+#define SBUS1_FRAME_DELAY	14000
 
 /*
   Measured values with Futaba FX-30/R6108SB:
@@ -80,6 +81,7 @@ static int sbus_fd = -1;
 
 static hrt_abstime last_rx_time;
 static hrt_abstime last_frame_time;
+static hrt_abstime last_txframe_time = 0;
 
 static uint8_t	frame[SBUS_FRAME_SIZE];
 
@@ -122,10 +124,42 @@ sbus_init(const char *device)
 void
 sbus1_output(uint16_t *values, uint16_t num_values)
 {
-	char a = 'A';
-	write(sbus_fd, &a, 1);
-}
+	uint8_t byteindex = 1; /*Data starts one byte into the sbus frame. */
+	uint8_t offset = 0;
+	uint16_t value;
+	hrt_abstime now;
+
+	now = hrt_absolute_time();
+
+	if ((now - last_txframe_time) > SBUS1_FRAME_DELAY) {
+		last_txframe_time = now;
+		uint8_t	oframe[SBUS_FRAME_SIZE] = { 0x0f };
+
+		/* 16 is sbus number of servos/channels minus 2 single bit channels.
+		* currently ignoring single bit channels.  */
+
+		for (unsigned i = 0; (i < num_values) && (i < 16); ++i) {
+			value = (uint16_t)(((values[i] - SBUS_SCALE_OFFSET) / SBUS_SCALE_FACTOR) + .5f);
+
+			/*protect from out of bounds values and limit to 11 bits*/
+			if (value > 0x07ff ) {
+				value = 0x07ff;
+			}
+
+			while (offset >= 8) {
+				++byteindex;
+				offset -= 8;
+			}
 
+			oframe[byteindex] |= (value << (offset)) & 0xff;
+			oframe[byteindex + 1] |= (value >> (8 - offset)) & 0xff;
+			oframe[byteindex + 2] |= (value >> (16 - offset)) & 0xff;
+			offset += 11;
+		}
+
+		write(sbus_fd, oframe, SBUS_FRAME_SIZE);
+	}
+}
 void
 sbus2_output(uint16_t *values, uint16_t num_values)
 {
diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk
index a28d43e72..d4a00af39 100644
--- a/src/modules/sdlog2/module.mk
+++ b/src/modules/sdlog2/module.mk
@@ -43,3 +43,5 @@ SRCS = sdlog2.c \
        logbuffer.c
 
 MODULE_STACKSIZE = 1200
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/modules/sensors/module.mk b/src/modules/sensors/module.mk
index 5b1bc5e86..dfbba92d9 100644
--- a/src/modules/sensors/module.mk
+++ b/src/modules/sensors/module.mk
@@ -42,3 +42,5 @@ SRCS		= sensors.cpp \
 		  sensor_params.c
 
 MODULE_STACKSIZE = 1200
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/modules/systemlib/mcu_version.c b/src/modules/systemlib/mcu_version.c
new file mode 100644
index 000000000..4bcf95784
--- /dev/null
+++ b/src/modules/systemlib/mcu_version.c
@@ -0,0 +1,109 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mcu_version.c
+ * 
+ * Read out the microcontroller version from the board
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
+ *
+ */
+
+#include "mcu_version.h"
+
+#include <nuttx/config.h>
+
+#ifdef CONFIG_ARCH_CHIP_STM32
+#include <up_arch.h>
+
+#define DBGMCU_IDCODE	0xE0042000
+
+#define STM32F40x_41x	0x413
+#define STM32F42x_43x	0x419
+
+#define REVID_MASK	0xFFFF0000
+#define DEVID_MASK	0xFFF
+
+#endif
+
+
+
+int mcu_version(char* rev, char** revstr)
+{
+#ifdef CONFIG_ARCH_CHIP_STM32
+	uint32_t abc = getreg32(DBGMCU_IDCODE);
+
+	int32_t chip_version = abc & DEVID_MASK;
+	enum MCU_REV revid = (abc & REVID_MASK) >> 16;
+
+	switch (chip_version) {
+	case STM32F40x_41x:
+		*revstr = "STM32F40x";
+		break;
+	case STM32F42x_43x:
+		*revstr = "STM32F42x";
+		break;
+	default:
+		*revstr = "STM32F???";
+		break;
+	}
+
+	switch (revid) {
+
+		case MCU_REV_STM32F4_REV_A:
+			*rev = 'A';
+			break;
+		case MCU_REV_STM32F4_REV_Z:
+			*rev = 'Z';
+			break;
+		case MCU_REV_STM32F4_REV_Y:
+			*rev = 'Y';
+			break;
+		case MCU_REV_STM32F4_REV_1:
+			*rev = '1';
+			break;
+		case MCU_REV_STM32F4_REV_3:
+			*rev = '3';
+			break;
+		default:
+			*rev = '?';
+			revid = -1;
+			break;
+	}
+
+	return revid;
+#else
+	return -1;
+#endif
+}
diff --git a/src/modules/systemlib/mcu_version.h b/src/modules/systemlib/mcu_version.h
new file mode 100644
index 000000000..1b3d0aba9
--- /dev/null
+++ b/src/modules/systemlib/mcu_version.h
@@ -0,0 +1,52 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+/* magic numbers from reference manual */
+enum MCU_REV {
+	MCU_REV_STM32F4_REV_A = 0x1000,
+	MCU_REV_STM32F4_REV_Z = 0x1001,
+	MCU_REV_STM32F4_REV_Y = 0x1003,
+	MCU_REV_STM32F4_REV_1 = 0x1007,
+	MCU_REV_STM32F4_REV_3 = 0x2001
+};
+
+/**
+ * Reports the microcontroller version of the main CPU.
+ *
+ * @param rev The silicon revision character
+ * @param revstr The full chip name string
+ * @return The silicon revision / version number as integer
+ */
+__EXPORT int mcu_version(char* rev, char** revstr);
diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c
index bf3428a50..0d629d610 100644
--- a/src/modules/systemlib/mixer/mixer_load.c
+++ b/src/modules/systemlib/mixer/mixer_load.c
@@ -91,6 +91,7 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
 		/* if the line is too long to fit in the buffer, bail */
 		if ((strlen(line) + strlen(buf) + 1) >= maxlen) {
 			warnx("line too long");
+			fclose(fp);
 			return -1;
 		}
 
@@ -98,6 +99,7 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
 		strcat(buf, line);
 	}
 
+	fclose(fp);
 	return 0;
 }
 
diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk
index 147903aa0..fe8b7e75a 100644
--- a/src/modules/systemlib/module.mk
+++ b/src/modules/systemlib/module.mk
@@ -53,5 +53,7 @@ SRCS		 = err.c \
 		   otp.c \
 		   board_serial.c \
 		   pwm_limit/pwm_limit.c \
-		   circuit_breaker.c
+		   circuit_breaker.c \
+		   mcu_version.c
 
+MAXOPTIMIZATION	 = -Os
diff --git a/src/modules/uORB/module.mk b/src/modules/uORB/module.mk
index 0c29101fe..9385ce253 100644
--- a/src/modules/uORB/module.mk
+++ b/src/modules/uORB/module.mk
@@ -44,3 +44,5 @@ SRCS			= uORB.cpp \
 			  objects_common.cpp \
 			  Publication.cpp \
 			  Subscription.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h
index dde237adc..50b7bd9e5 100644
--- a/src/modules/uORB/topics/manual_control_setpoint.h
+++ b/src/modules/uORB/topics/manual_control_setpoint.h
@@ -47,7 +47,7 @@
  * Switch position
  */
 typedef enum {
-	SWITCH_POS_NONE = 0,	/**< switch is not mapped */
+	SWITCH_POS_NONE = 0,		/**< switch is not mapped */
 	SWITCH_POS_ON,			/**< switch activated (value = 1) */
 	SWITCH_POS_MIDDLE,		/**< middle position (value = 0) */
 	SWITCH_POS_OFF			/**< switch not activated (value = -1) */
@@ -93,13 +93,13 @@ struct manual_control_setpoint_s {
 	float aux4;			/**< default function: camera roll */
 	float aux5;			/**< default function: payload drop */
 
-	switch_pos_t mode_switch;			/**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */
-	switch_pos_t return_switch;			/**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */
-	switch_pos_t posctl_switch;			/**< position control 2 position switch (optional): _ALTCTL_, POSCTL */
-	switch_pos_t loiter_switch;			/**< loiter 2 position switch (optional): _MISSION_, LOITER */
-	switch_pos_t acro_switch;			/**< acro 2 position switch (optional): _MANUAL_, ACRO */
-	switch_pos_t offboard_switch;		/**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */
-}; /**< manual control inputs */
+	switch_pos_t mode_switch;	/**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */
+	switch_pos_t return_switch;	/**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */
+	switch_pos_t posctl_switch;	/**< position control 2 position switch (optional): _ALTCTL_, POSCTL */
+	switch_pos_t loiter_switch;	/**< loiter 2 position switch (optional): _MISSION_, LOITER */
+	switch_pos_t acro_switch;	/**< acro 2 position switch (optional): _MANUAL_, ACRO */
+	switch_pos_t offboard_switch;	/**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */
+};
 
 /**
  * @}
diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h
index b14ae1edd..16916cc4d 100644
--- a/src/modules/uORB/topics/rc_channels.h
+++ b/src/modules/uORB/topics/rc_channels.h
@@ -34,6 +34,8 @@
 /**
  * @file rc_channels.h
  * Definition of the rc_channels uORB topic.
+ *
+ * @deprecated DO NOT USE FOR NEW CODE
  */
 
 #ifndef RC_CHANNELS_H_
@@ -63,10 +65,13 @@ enum RC_CHANNELS_FUNCTION {
 	AUX_2,
 	AUX_3,
 	AUX_4,
-	AUX_5,
-	RC_CHANNELS_FUNCTION_MAX /**< Indicates the number of functions. There can be more functions than RC channels. */
+	AUX_5
 };
 
+// MAXIMUM FUNCTIONS IS != MAXIMUM RC INPUT CHANNELS
+
+#define RC_CHANNELS_FUNCTION_MAX 18
+
 /**
  * @addtogroup topics
  * @{
diff --git a/src/modules/uORB/topics/test_motor.h b/src/modules/uORB/topics/test_motor.h
index 491096934..2dd90e69d 100644
--- a/src/modules/uORB/topics/test_motor.h
+++ b/src/modules/uORB/topics/test_motor.h
@@ -57,7 +57,7 @@
 struct test_motor_s {
 	uint64_t 	timestamp;				/**< output timestamp in us since system boot */
 	unsigned 	motor_number;				/**< number of motor to spin */
-	float		value;					/**< output data, in natural output units */
+	float		value;					/**< output power, range [0..1] */
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index a1b2667e3..91491c148 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -108,7 +108,7 @@ typedef enum {
 	NAVIGATION_STATE_AUTO_LANDGPSFAIL,	/**< Auto land on gps failure (e.g. open loop loiter down) */
 	NAVIGATION_STATE_ACRO,			/**< Acro mode */
 	NAVIGATION_STATE_LAND,			/**< Land mode */
-	NAVIGATION_STATE_DESCEND,			/**< Descend mode (no position control) */
+	NAVIGATION_STATE_DESCEND,		/**< Descend mode (no position control) */
 	NAVIGATION_STATE_TERMINATION,		/**< Termination mode */
 	NAVIGATION_STATE_OFFBOARD,
 	NAVIGATION_STATE_MAX,
diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp
index 1990651ef..1d23099f3 100644
--- a/src/modules/uavcan/actuators/esc.cpp
+++ b/src/modules/uavcan/actuators/esc.cpp
@@ -40,6 +40,9 @@
 #include "esc.hpp"
 #include <systemlib/err.h>
 
+
+#define MOTOR_BIT(x) (1<<(x))
+
 UavcanEscController::UavcanEscController(uavcan::INode &node) :
 	_node(node),
 	_uavcan_pub_raw_cmd(node),
@@ -95,9 +98,8 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
 
 	static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max();
 
-	if (_armed) {
-		for (unsigned i = 0; i < num_outputs; i++) {
-
+	for (unsigned i = 0; i < num_outputs; i++) {
+		if (_armed_mask & MOTOR_BIT(i)) {
 			float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max;
 			if (scaled < 1.0F) {
 				scaled = 1.0F;  // Since we're armed, we don't want to stop it completely
@@ -111,6 +113,8 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
 			msg.cmd.push_back(static_cast<int>(scaled));
 
 			_esc_status.esc[i].esc_setpoint_raw = abs(static_cast<int>(scaled));
+		} else {
+			msg.cmd.push_back(static_cast<unsigned>(0));
 		}
 	}
 
@@ -121,9 +125,22 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
 	(void)_uavcan_pub_raw_cmd.broadcast(msg);
 }
 
-void UavcanEscController::arm_esc(bool arm)
+void UavcanEscController::arm_all_escs(bool arm)
+{
+	if (arm) {
+		_armed_mask = -1;
+	} else {
+		_armed_mask = 0;
+	}
+}
+
+void UavcanEscController::arm_single_esc(int num, bool arm)
 {
-	_armed = arm;
+	if (arm) {
+		_armed_mask = MOTOR_BIT(num);
+	} else {
+		_armed_mask = 0;
+	}
 }
 
 void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg)
diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp
index f4a3877e6..12c035542 100644
--- a/src/modules/uavcan/actuators/esc.hpp
+++ b/src/modules/uavcan/actuators/esc.hpp
@@ -61,7 +61,8 @@ public:
 
 	void update_outputs(float *outputs, unsigned num_outputs);
 
-	void arm_esc(bool arm);
+	void arm_all_escs(bool arm);
+	void arm_single_esc(int num, bool arm);
 
 private:
 	/**
@@ -99,6 +100,11 @@ private:
 	uavcan::TimerEventForwarder<TimerCbBinder>				_orb_timer;
 
 	/*
+	 * ESC states
+	 */
+	uint32_t 			_armed_mask = 0;
+
+	/*
 	 * Perf counters
 	 */
 	perf_counter_t _perfcnt_invalid_input = perf_alloc(PC_COUNT, "uavcan_esc_invalid_input");
diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp
index 80c5e3828..8741ae20d 100644
--- a/src/modules/uavcan/sensors/baro.cpp
+++ b/src/modules/uavcan/sensors/baro.cpp
@@ -91,11 +91,7 @@ void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure<
 {
 	auto report = ::baro_report();
 
-	report.timestamp = msg.getUtcTimestamp().toUSec();
-	if (report.timestamp == 0) {
-		report.timestamp = msg.getMonotonicTimestamp().toUSec();
-	}
-
+	report.timestamp   = msg.getMonotonicTimestamp().toUSec();
 	report.temperature = msg.static_temperature;
 	report.pressure    = msg.static_pressure / 100.0F;  // Convert to millibar
 
diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp
index 24afe6aaf..a375db37f 100644
--- a/src/modules/uavcan/sensors/gnss.cpp
+++ b/src/modules/uavcan/sensors/gnss.cpp
@@ -92,7 +92,7 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
 
 	auto report = ::vehicle_gps_position_s();
 
-	report.timestamp_position = hrt_absolute_time();
+	report.timestamp_position = msg.getMonotonicTimestamp().toUSec();
 	report.lat = msg.latitude_deg_1e8 / 10;
 	report.lon = msg.longitude_deg_1e8 / 10;
 	report.alt = msg.height_msl_mm;
diff --git a/src/modules/uavcan/sensors/gnss.hpp b/src/modules/uavcan/sensors/gnss.hpp
index e8466b401..2111ff80b 100644
--- a/src/modules/uavcan/sensors/gnss.hpp
+++ b/src/modules/uavcan/sensors/gnss.hpp
@@ -43,8 +43,6 @@
 
 #pragma once
 
-#include <drivers/drv_hrt.h>
-
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_gps_position.h>
 
diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp
index 8e6a9a22f..35ebee542 100644
--- a/src/modules/uavcan/sensors/mag.cpp
+++ b/src/modules/uavcan/sensors/mag.cpp
@@ -37,6 +37,8 @@
 
 #include "mag.hpp"
 
+#include <systemlib/err.h>
+
 static const orb_id_t MAG_TOPICS[3] = {
 	ORB_ID(sensor_mag0),
 	ORB_ID(sensor_mag1),
@@ -49,6 +51,8 @@ UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) :
 UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, MAG_TOPICS),
 _sub_mag(node)
 {
+	_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;
+
 	_scale.x_scale = 1.0F;
 	_scale.y_scale = 1.0F;
 	_scale.z_scale = 1.0F;
@@ -69,9 +73,36 @@ int UavcanMagnetometerBridge::init()
 	return 0;
 }
 
+ssize_t UavcanMagnetometerBridge::read(struct file *filp, char *buffer, size_t buflen)
+{
+	static uint64_t last_read = 0;
+	struct mag_report *mag_buf = reinterpret_cast<struct mag_report *>(buffer);
+
+	/* buffer must be large enough */
+	unsigned count = buflen / sizeof(struct mag_report);
+	if (count < 1) {
+		return -ENOSPC;
+	}
+
+	if (last_read < _report.timestamp) {
+		/* copy report */
+		lock();
+		*mag_buf = _report;
+		last_read = _report.timestamp;
+		unlock();
+		return sizeof(struct mag_report);
+	} else {
+		/* no new data available, warn caller */
+		return -EAGAIN;
+	}
+}
+
 int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
 {
 	switch (cmd) {
+	case SENSORIOCSQUEUEDEPTH: {
+		return OK;			// Pretend that this stuff is supported to keep APM happy
+	}
 	case MAGIOCSSCALE: {
 		std::memcpy(&_scale, reinterpret_cast<const void*>(arg), sizeof(_scale));
 		return 0;
@@ -84,7 +115,7 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar
 		return 0;           // Nothing to do
 	}
 	case MAGIOCGEXTERNAL: {
-		return 0;           // We don't want anyone to transform the coordinate frame, so we declare it onboard
+		return 1;           // declare it external rise it's priority and to allow for correct orientation compensation
 	}
 	case MAGIOCSSAMPLERATE: {
 		return 0;           // Pretend that this stuff is supported to keep the sensor app happy
@@ -106,18 +137,14 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar
 
 void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg)
 {
-	auto report = ::mag_report();
-
-	report.range_ga = 1.3F;   // Arbitrary number, doesn't really mean anything
-
-	report.timestamp = msg.getUtcTimestamp().toUSec();
-	if (report.timestamp == 0) {
-		report.timestamp = msg.getMonotonicTimestamp().toUSec();
-	}
+	lock();
+	_report.range_ga = 1.3F;   // Arbitrary number, doesn't really mean anything
+	_report.timestamp = msg.getMonotonicTimestamp().toUSec();
 
-	report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale;
-	report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale;
-	report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale;
+	_report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale;
+	_report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale;
+	_report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale;
+	unlock();
 
-	publish(msg.getSrcNodeID().get(), &report);
+	publish(msg.getSrcNodeID().get(), &_report);
 }
diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp
index 6d413a8f7..74077d883 100644
--- a/src/modules/uavcan/sensors/mag.hpp
+++ b/src/modules/uavcan/sensors/mag.hpp
@@ -54,6 +54,7 @@ public:
 	int init() override;
 
 private:
+	ssize_t	read(struct file *filp, char *buffer, size_t buflen);
 	int ioctl(struct file *filp, int cmd, unsigned long arg) override;
 
 	void mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg);
@@ -65,4 +66,5 @@ private:
 
 	uavcan::Subscriber<uavcan::equipment::ahrs::Magnetometer, MagCbBinder> _sub_mag;
 	mag_scale _scale = {};
+	mag_report _report =  {};
 };
diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp
index 9608ce680..0999938fc 100644
--- a/src/modules/uavcan/sensors/sensor_bridge.cpp
+++ b/src/modules/uavcan/sensors/sensor_bridge.cpp
@@ -103,6 +103,9 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
 			return;
 		}
 
+		// update device id as we now know our device node_id
+		_device_id.devid_s.address = static_cast<uint8_t>(node_id);
+
 		// Ask the CDev helper which class instance we can take
 		const int class_instance = register_class_devname(_class_devname);
 		if (class_instance < 0 || class_instance >= int(_max_channels)) {
diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp
index 1316f7ecc..e31960537 100644
--- a/src/modules/uavcan/sensors/sensor_bridge.hpp
+++ b/src/modules/uavcan/sensors/sensor_bridge.hpp
@@ -112,6 +112,8 @@ protected:
 	_channels(new Channel[MaxChannels])
 	{
 		memcpy(_orb_topics, orb_topics, sizeof(orb_id_t) * MaxChannels);
+		_device_id.devid_s.bus_type = DeviceBusType_UAVCAN;
+		_device_id.devid_s.bus = 0;
 	}
 
 	/**
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index a8485ee44..2c543462e 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -279,6 +279,7 @@ int UavcanNode::run()
 	_output_count = 2;
 
 	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+	_test_motor_sub = orb_subscribe(ORB_ID(test_motor));
 
 	actuator_outputs_s outputs;
 	memset(&outputs, 0, sizeof(outputs));
@@ -344,7 +345,13 @@ int UavcanNode::run()
 			}
 
 			// can we mix?
-			if (controls_updated && (_mixers != nullptr)) {
+			if (_test_in_progress) {
+				float test_outputs[NUM_ACTUATOR_OUTPUTS] = {};
+				test_outputs[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f;
+
+				// Output to the bus
+				_esc_controller.update_outputs(test_outputs, NUM_ACTUATOR_OUTPUTS);
+			} else if (controls_updated && (_mixers != nullptr)) {
 
 				// XXX one output group has 8 outputs max,
 				// but this driver could well serve multiple groups.
@@ -384,15 +391,27 @@ int UavcanNode::run()
 			}
 		}
 
-		// Check arming state
+		// Check motor test state
 		bool updated = false;
+		orb_check(_test_motor_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(test_motor), _test_motor_sub, &_test_motor);
+
+			// Update the test status and check that we're not locked down
+			_test_in_progress = (_test_motor.value > 0);
+			_esc_controller.arm_single_esc(_test_motor.motor_number, _test_in_progress);
+		}
+
+		// Check arming state
 		orb_check(_armed_sub, &updated);
 
 		if (updated) {
 			orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
 
-			// Update the armed status and check that we're not locked down
-			bool set_armed = _armed.armed && !_armed.lockdown;
+			// Update the armed status and check that we're not locked down and motor 
+			// test is not running
+			bool set_armed = _armed.armed && !_armed.lockdown && !_test_in_progress;
 
 			arm_actuators(set_armed);
 		}
@@ -429,7 +448,7 @@ int
 UavcanNode::arm_actuators(bool arm)
 {
 	_is_armed = arm;
-	_esc_controller.arm_esc(arm);
+	_esc_controller.arm_all_escs(arm);
 	return OK;
 }
 
diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp
index be7db9741..274321f0d 100644
--- a/src/modules/uavcan/uavcan_main.hpp
+++ b/src/modules/uavcan/uavcan_main.hpp
@@ -41,6 +41,7 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/test_motor.h>
 
 #include "actuators/esc.hpp"
 #include "sensors/sensor_bridge.hpp"
@@ -103,6 +104,10 @@ private:
 	actuator_armed_s	_armed;				///< the arming request of the system
 	bool			_is_armed = false;		///< the arming status of the actuators on the bus
 
+	int			_test_motor_sub = -1;   ///< uORB subscription of the test_motor status
+	test_motor_s		_test_motor;
+	bool			_test_in_progress = false;
+
 	unsigned		_output_count = 0;		///< number of actuators currently available
 
 	static UavcanNode	*_instance;			///< singleton pointer
diff --git a/src/systemcmds/motor_test/module.mk b/src/systemcmds/motor_test/module.mk
index eb36d2ded..261428ef9 100644
--- a/src/systemcmds/motor_test/module.mk
+++ b/src/systemcmds/motor_test/module.mk
@@ -39,3 +39,5 @@ MODULE_COMMAND	 = motor_test
 SRCS		 = motor_test.c
 
 MODULE_STACKSIZE = 4096
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/systemcmds/motor_test/motor_test.c b/src/systemcmds/motor_test/motor_test.c
index 079f99674..1b7ff75f7 100644
--- a/src/systemcmds/motor_test/motor_test.c
+++ b/src/systemcmds/motor_test/motor_test.c
@@ -54,7 +54,8 @@
 #include "systemlib/err.h"
 
 
-__EXPORT int	motor_test_main(int argc, char *argv[]);
+__EXPORT int motor_test_main(int argc, char *argv[]);
+
 static void motor_test(unsigned channel, float value);
 static void usage(const char *reason);
 
@@ -67,19 +68,20 @@ void motor_test(unsigned channel, float value)
 	_test_motor.timestamp = hrt_absolute_time();
 	_test_motor.value = value;
 
-        if (_test_motor_pub > 0) {
-	    /* publish armed state */
-            orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor);
-        } else {
-            /* advertise and publish */
-            _test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor);
-        }
+    if (_test_motor_pub > 0) {
+    /* publish test state */
+        orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor);
+    } else {
+        /* advertise and publish */
+        _test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor);
+    }
 }
 
 static void usage(const char *reason)
 {
-	if (reason != NULL)
+	if (reason != NULL) {
 		warnx("%s", reason);
+	}
 
 	errx(1,
 		"usage:\n"
@@ -90,8 +92,9 @@ static void usage(const char *reason)
 
 int motor_test_main(int argc, char *argv[])
 {
-	unsigned long channel, lval;
-	float value;
+	unsigned long channel = 0;
+	unsigned long lval;
+	float value = 0.0f;
 	int ch;
 
 	if (argc != 5) {
@@ -102,18 +105,18 @@ int motor_test_main(int argc, char *argv[])
 		switch (ch) {
 
 		case 'm':
-			/* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */
+			/* Read in motor number */
 			channel = strtoul(optarg, NULL, 0);
 			break;
 
 		case 'p':
-			/* Read in custom low value */
+			/* Read in power value */
 			lval = strtoul(optarg, NULL, 0);
 
 			if (lval > 100)
 				usage("value invalid");
 
-			value = (float)lval/100.f;
+			value = ((float)lval)/100.f;
 			break;
 		default:
 			usage(NULL);
@@ -122,7 +125,7 @@ int motor_test_main(int argc, char *argv[])
 
 	motor_test(channel, value);
 
-	printf("motor %d set to %.2f\n", channel, value);
+	printf("motor %d set to %.2f\n", channel, (double)value);
 
 	exit(0);
 }
diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c
index f06c49552..edeb5c624 100644
--- a/src/systemcmds/nshterm/nshterm.c
+++ b/src/systemcmds/nshterm/nshterm.c
@@ -51,6 +51,8 @@
 #include <fcntl.h>
 #include <systemlib/err.h>
 
+#include <uORB/topics/actuator_armed.h>
+
 __EXPORT int nshterm_main(int argc, char *argv[]);
 
 int
@@ -62,9 +64,30 @@ nshterm_main(int argc, char *argv[])
     }
     unsigned retries = 0;
     int fd = -1;
+    int armed_fd = orb_subscribe(ORB_ID(actuator_armed));
+    struct actuator_armed_s armed;
+    /* we assume the system does not provide arming status feedback */
+    bool armed_updated = false;
+
+    /* try the first 30 seconds or if arming system is ready */
+    while ((retries < 300) || armed_updated) {
+
+        /* abort if an arming topic is published and system is armed */
+        bool updated = false;
+        if (orb_check(armed_fd, &updated)) {
+            /* the system is now providing arming status feedback.
+             * instead of timing out, we resort to abort bringing
+             * up the terminal.
+             */
+            armed_updated = true;
+            orb_copy(ORB_ID(actuator_armed), armed_fd, &armed);
+
+            if (armed.armed) {
+                /* this is not an error, but we are done */
+                exit(0);
+            }
+        }
 
-    /* try the first 30 seconds */
-    while (retries < 300) {
         /* the retries are to cope with the behaviour of /dev/ttyACM0 */
         /* which may not be ready immediately. */
         fd = open(argv[1], O_RDWR);
diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c
index 478c2a772..eeba89fa8 100644
--- a/src/systemcmds/pwm/pwm.c
+++ b/src/systemcmds/pwm/pwm.c
@@ -74,34 +74,34 @@ usage(const char *reason)
 		"usage:\n"
 		"pwm arm|disarm|rate|failsafe|disarmed|min|max|test|info  ...\n"
 		"\n"
-		"  arm                      Arm output\n"
-		"  disarm                   Disarm output\n"
+		"arm\t\t\t\tArm output\n"
+		"disarm\t\t\t\tDisarm output\n"
 		"\n"
-		"  rate ...                 Configure PWM rates\n"
-		"    [-g <channel group>]   Channel group that should update at the alternate rate\n"
-		"    [-m <chanmask> ]       Directly supply channel mask\n"
-		"    [-a]                   Configure all outputs\n"
-		"    -r <alt_rate>          PWM rate (50 to 400 Hz)\n"
+		"rate ...\t\t\tConfigure PWM rates\n"
+		"\t[-g <channel group>]\t(e.g. 0,1,2)\n"
+		"\t[-m <channel mask> ]\t(e.g. 0xF)\n"
+		"\t[-a]\t\t\tConfigure all outputs\n"
+		"\t-r <alt_rate>\t\tPWM rate (50 to 400 Hz)\n"
 		"\n"
-		"  failsafe ...      	    Configure failsafe PWM values\n"
-		"  disarmed ...      	    Configure disarmed PWM values\n"
-		"  min ...           	    Configure minimum PWM values\n"
-		"  max ...           	    Configure maximum PWM values\n"
-		"    [-c <channels>]        Supply channels (e.g. 1234)\n"
-		"    [-m <chanmask> ]       Directly supply channel mask (e.g. 0xF)\n"
-		"    [-a]                   Configure all outputs\n"
-		"    -p <pwm value>         PWM value\n"
+		"failsafe ...\t\t\tFailsafe PWM\n"
+		"disarmed ...\t\t\tDisarmed PWM\n"
+		"min ...\t\t\t\tMinimum PWM\n"
+		"max ...\t\t\t\tMaximum PWM\n"
+		"\t[-c <channels>]\t\t(e.g. 1234)\n"
+		"\t[-m <channel mask> ]\t(e.g. 0xF)\n"
+		"\t[-a]\t\t\tConfigure all outputs\n"
+		"\t-p <pwm value>\t\tPWM value\n"
 		"\n"
-		"  test ...                 Directly set PWM values\n"
-		"    [-c <channels>]        Supply channels (e.g. 1234)\n"
-		"    [-m <chanmask> ]       Directly supply channel mask (e.g. 0xF)\n"
-		"    [-a]                   Configure all outputs\n"
-		"    -p <pwm value>         PWM value\n"
+		"test ...\t\t\tDirectly set PWM\n"
+		"\t[-c <channels>]\t\t(e.g. 1234)\n"
+		"\t[-m <channel mask> ]\t(e.g. 0xF)\n"
+		"\t[-a]\t\t\tConfigure all outputs\n"
+		"\t-p <pwm value>\t\tPWM value\n"
 		"\n"
-		"  info                     Print information about the PWM device\n"
+		"info\t\t\t\tPrint information\n"
 		"\n"
-		"    -v                     Print verbose information\n"
-		"    -d <device>            PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n"
+		"\t-v\t\t\tVerbose\n"
+		"\t-d <dev>\t\t(default " PWM_OUTPUT_DEVICE_PATH ")\n"
 		);
 
 }
@@ -123,7 +123,7 @@ pwm_main(int argc, char *argv[])
 	unsigned single_ch = 0;
 	unsigned pwm_value = 0;
 
-	if (argc < 1)
+	if (argc < 2)
 		usage(NULL);
 
 	while ((ch = getopt(argc-1, &argv[1], "d:vc:g:m:ap:r:")) != EOF) {
@@ -165,7 +165,7 @@ pwm_main(int argc, char *argv[])
 			/* Read in mask directly */
 			set_mask = strtoul(optarg, &ep, 0);
 			if (*ep != '\0')
-				usage("bad set_mask value");
+				usage("BAD set_mask VAL");
 			break;
 
 		case 'a':
@@ -176,12 +176,12 @@ pwm_main(int argc, char *argv[])
 		case 'p':
 			pwm_value = strtoul(optarg, &ep, 0);
 			if (*ep != '\0')
-				usage("bad PWM value provided");
+				usage("BAD PWM VAL");
 			break;
 		case 'r':
 			alt_rate = strtoul(optarg, &ep, 0);
 			if (*ep != '\0')
-				usage("bad alternative rate provided");
+				usage("BAD rate VAL");
 			break;
 		default:
 			break;
@@ -189,7 +189,7 @@ pwm_main(int argc, char *argv[])
 	}
 
 	if (print_verbose && set_mask > 0) {
-		warnx("Chose channels: ");
+		warnx("Channels: ");
 		printf("    ");
 		for (unsigned i = 0; i<PWM_OUTPUT_MAX_CHANNELS; i++) {
 			if (set_mask & 1<<i)
@@ -364,7 +364,7 @@ pwm_main(int argc, char *argv[])
 			usage("no channels set");
 		}
 		if (pwm_value == 0)
-			usage("no PWM value provided");
+			usage("no PWM provided");
 
 		struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0};
 
@@ -383,7 +383,7 @@ pwm_main(int argc, char *argv[])
 
 			ret = ioctl(fd, PWM_SERVO_SET_FAILSAFE_PWM, (long unsigned int)&pwm_values);
 			if (ret != OK)
-				errx(ret, "failed setting failsafe values");
+				errx(ret, "BAD input VAL");
 		}
 		exit(0);
 
@@ -393,7 +393,7 @@ pwm_main(int argc, char *argv[])
 			usage("no channels set");
 		}
 		if (pwm_value == 0)
-			usage("no PWM value provided");
+			usage("no PWM provided");
 
 		/* get current servo values */
 		struct pwm_output_values last_spos;
@@ -487,7 +487,7 @@ pwm_main(int argc, char *argv[])
 			steps_timing_index < sizeof(steps_timings_us) / sizeof(steps_timings_us[0]);
 			steps_timing_index++ ) {
 
-			warnx("Sending step input with 0 to 100%% over a %u microseconds ramp", steps_timings_us[steps_timing_index]);
+			warnx("Step input (0 to 100%%) over %u us ramp", steps_timings_us[steps_timing_index]);
 
 			while (1) {
 				for (unsigned i = 0; i < servo_count; i++) {
@@ -526,7 +526,7 @@ pwm_main(int argc, char *argv[])
 												err(1, "PWM_SERVO_SET(%d)", i);
 										}
 									}
-						warnx("Key pressed, user abort\n");
+						warnx("User abort\n");
 						exit(0);
 					}
 				}
@@ -675,7 +675,7 @@ pwm_main(int argc, char *argv[])
 		exit(0);
 	}
 
-	usage("specify arm|disarm|rate|failsafe\n\t\tdisarmed|min|max|test|info|forcefail|terminatefail");
+	usage(NULL);
 	return 0;
 }
 
diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c
index 9ae080ee2..2ead3e632 100644
--- a/src/systemcmds/ver/ver.c
+++ b/src/systemcmds/ver/ver.c
@@ -44,14 +44,16 @@
 #include <string.h>
 #include <version/version.h>
 #include <systemlib/err.h>
+#include <systemlib/mcu_version.h>
 
-// string constants for version commands
+/* string constants for version commands */
 static const char sz_ver_hw_str[] 	= "hw";
 static const char sz_ver_hwcmp_str[] = "hwcmp";
 static const char sz_ver_git_str[] 	= "git";
 static const char sz_ver_bdate_str[] = "bdate";
 static const char sz_ver_gcc_str[] 	= "gcc";
 static const char sz_ver_all_str[] 	= "all";
+static const char mcu_ver_str[]		= "mcu";
 
 static void usage(const char *reason)
 {
@@ -59,55 +61,87 @@ static void usage(const char *reason)
 		printf("%s\n", reason);
 	}
 
-	printf("usage: ver {hw|hwcmp|git|bdate|gcc|all}\n\n");
+	printf("usage: ver {hw|hwcmp|git|bdate|gcc|all|mcu}\n\n");
 }
 
 __EXPORT int ver_main(int argc, char *argv[]);
 
 int ver_main(int argc, char *argv[])
 {
-	int ret = 1;	//defaults to an error
+	/* defaults to an error */
+	int ret = 1;
 
-	// first check if there are at least 2 params
+	/* first check if there are at least 2 params */
 	if (argc >= 2) {
 		if (argv[1] != NULL) {
-			if (!strncmp(argv[1], sz_ver_hw_str, sizeof(sz_ver_hw_str))) {
-				printf("%s\n", HW_ARCH);
-				ret = 0;
 
-			} else if (!strncmp(argv[1], sz_ver_hwcmp_str, sizeof(sz_ver_hwcmp_str))) {
+			if (!strncmp(argv[1], sz_ver_hwcmp_str, sizeof(sz_ver_hwcmp_str))) {
 				if (argc >= 3 && argv[2] != NULL) {
-					// compare 3rd parameter with HW_ARCH string, in case of match, return 0
+					/* compare 3rd parameter with HW_ARCH string, in case of match, return 0 */
 					ret = strncmp(HW_ARCH, argv[2], strlen(HW_ARCH));
 
 					if (ret == 0) {
 						printf("ver hwcmp match: %s\n", HW_ARCH);
 					}
+					return ret;
 
 				} else {
 					errx(1, "Not enough arguments, try 'ver hwcmp PX4FMU_V2'");
 				}
+			}
 
-			} else if (!strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) {
-				printf("%s\n", FW_GIT);
-				ret = 0;
+			/* check if we want to show all */
+			bool show_all = !strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str));
 
-			} else if (!strncmp(argv[1], sz_ver_bdate_str, sizeof(sz_ver_bdate_str))) {
-				printf("%s %s\n", __DATE__, __TIME__);
+			if (show_all || !strncmp(argv[1], sz_ver_hw_str, sizeof(sz_ver_hw_str))) {
+				printf("HW arch: %s\n", HW_ARCH);
 				ret = 0;
 
-			} else if (!strncmp(argv[1], sz_ver_gcc_str, sizeof(sz_ver_gcc_str))) {
-				printf("%s\n", __VERSION__);
+			}
+
+			if (show_all || !strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) {
+				printf("FW git-hash: %s\n", FW_GIT);
 				ret = 0;
 
-			} else if (!strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str))) {
-				printf("HW arch: %s\n", HW_ARCH);
+			}
+
+			if (show_all || !strncmp(argv[1], sz_ver_bdate_str, sizeof(sz_ver_bdate_str))) {
 				printf("Build datetime: %s %s\n", __DATE__, __TIME__);
-				printf("FW git-hash: %s\n", FW_GIT);
-				printf("GCC toolchain: %s\n", __VERSION__);
 				ret = 0;
 
-			} else {
+			}
+
+			if (show_all || !strncmp(argv[1], sz_ver_gcc_str, sizeof(sz_ver_gcc_str))) {
+				printf("Toolchain: %s\n", __VERSION__);
+				ret = 0;
+
+			}
+
+			if (show_all || !strncmp(argv[1], mcu_ver_str, sizeof(mcu_ver_str))) {
+
+				char rev;
+				char* revstr;
+
+				int chip_version = mcu_version(&rev, &revstr);
+
+				if (chip_version < 0) {
+					printf("UNKNOWN MCU\n");
+
+				} else {
+					printf("MCU: %s, rev. %c\n", revstr, rev);
+
+					if (chip_version < MCU_REV_STM32F4_REV_3) {
+						printf("\nWARNING   WARNING   WARNING!\n"
+							"Revision %c has a silicon errata\n"
+							"This device can only utilize a maximum of 1MB flash safely!\n"
+							"http://px4.io/help/errata\n\n", rev);
+					}
+				}
+
+				ret = 0;
+			}
+
+			if (ret == 1) {
 				errx(1, "unknown command.\n");
 			}