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

22 files changed, 928 insertions, 110 deletions

diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index a7c10f52f..97eddfdd2 100644
--- a/makefiles/config_px4fmu-v1_default.mk
+++ b/makefiles/config_px4fmu-v1_default.mk
@@ -25,7 +25,6 @@ MODULES		+= drivers/mpu6000
 MODULES		+= drivers/hmc5883
 MODULES		+= drivers/ms5611
 MODULES		+= drivers/mb12xx
-MODULES		+= drivers/ll40ls
 MODULES		+= drivers/gps
 MODULES		+= drivers/hil
 MODULES		+= drivers/hott/hott_telemetry
@@ -44,7 +43,6 @@ MODULES		+= modules/sensors
 #
 MODULES		+= systemcmds/mtd
 MODULES		+= systemcmds/bl_update
-MODULES		+= systemcmds/i2c
 MODULES		+= systemcmds/mixer
 MODULES		+= systemcmds/param
 MODULES		+= systemcmds/perf
@@ -152,5 +150,4 @@ endef
 #                  command                 priority                   stack  entrypoint
 BUILTIN_COMMANDS := \
 	$(call _B, sercon,                 ,                          2048,  sercon_main                ) \
-	$(call _B, serdis,                 ,                          2048,  serdis_main                ) \
-	$(call _B, sysinfo,                ,                          2048,  sysinfo_main               )
+	$(call _B, serdis,                 ,                          2048,  serdis_main                )
diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp
index 41942aacd..293690d27 100644
--- a/src/drivers/airspeed/airspeed.cpp
+++ b/src/drivers/airspeed/airspeed.cpp
@@ -165,7 +165,7 @@ Airspeed::probe()
 	*/
 	_retries = 4;
 	int ret = measure();
-	_retries = 2;
+	_retries = 0;
 	return ret;
 }
 
@@ -381,7 +381,10 @@ Airspeed::cycle_trampoline(void *arg)
 	Airspeed *dev = (Airspeed *)arg;
 
 	dev->cycle();
-	dev->update_status();
+	// XXX we do not know if this is
+	// really helping - do not update the
+	// subsys state right now
+	//dev->update_status();
 }
 
 void
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index 0e9a961ac..e4ecfa6b5 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -1283,14 +1283,13 @@ int HMC5883::set_excitement(unsigned enable)
 	if (OK != ret)
 		perf_count(_comms_errors);
 
+	_conf_reg &= ~0x03; // reset previous excitement mode
 	if (((int)enable) < 0) {
 		_conf_reg |= 0x01;
 
 	} else if (enable > 0) {
 		_conf_reg |= 0x02;
 
-	} else {
-		_conf_reg &= ~0x03;
 	}
 
         // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)_conf_reg);
diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp
index 873fa62c4..889643d0d 100644
--- a/src/drivers/ms5611/ms5611.cpp
+++ b/src/drivers/ms5611/ms5611.cpp
@@ -130,7 +130,7 @@ protected:
 	float			_T;
 
 	/* altitude conversion calibration */
-	unsigned		_msl_pressure;	/* in kPa */
+	unsigned		_msl_pressure;	/* in Pa */
 
 	orb_advert_t		_baro_topic;
 
@@ -466,7 +466,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
 			irqrestore(flags);
 			return -ENOMEM;
 		}
-		irqrestore(flags);		
+		irqrestore(flags);
 		return OK;
 	}
 
diff --git a/src/drivers/pca9685/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt b/src/drivers/pca9685/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt
new file mode 100644
index 000000000..9c5eb42eb
--- /dev/null
+++ b/src/drivers/pca9685/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt
@@ -0,0 +1,29 @@
+The following license agreement covers re-used code from the arduino driver
+for the Adafruit I2C to PWM converter.
+
+Software License Agreement (BSD License)
+
+Copyright (c) 2012, Adafruit Industries
+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 of the copyright holders 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 ''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 HOLDER 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.
diff --git a/src/drivers/pca9685/module.mk b/src/drivers/pca9685/module.mk
new file mode 100644
index 000000000..7a5c7996e
--- /dev/null
+++ b/src/drivers/pca9685/module.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# 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.
+#
+############################################################################
+
+#
+# Driver for the PCA9685 I2C PWM controller
+# The chip is used on the adafruit I2C PWM converter,
+# which allows to control servos via I2C.
+# https://www.adafruit.com/product/815
+
+MODULE_COMMAND	= pca9685
+
+SRCS		= pca9685.cpp
diff --git a/src/drivers/pca9685/pca9685.cpp b/src/drivers/pca9685/pca9685.cpp
new file mode 100644
index 000000000..6f69ce8a1
--- /dev/null
+++ b/src/drivers/pca9685/pca9685.cpp
@@ -0,0 +1,651 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * 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 pca9685.cpp
+ *
+ * Driver for the PCA9685 I2C PWM module
+ * The chip is used on the Adafruit I2C/PWM converter https://www.adafruit.com/product/815
+ *
+ * Parts of the code are adapted from the arduino library for the board
+ * https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library
+ * for the license of these parts see the
+ * arduino_Adafruit_PWM_Servo_Driver_Library_license.txt file
+ * see https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library for contributors
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include <nuttx/config.h>
+
+#include <drivers/device/i2c.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <ctype.h>
+#include <math.h>
+
+#include <nuttx/wqueue.h>
+#include <nuttx/clock.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/actuator_controls.h>
+
+#include <board_config.h>
+#include <drivers/drv_io_expander.h>
+
+#define PCA9685_SUBADR1 0x2
+#define PCA9685_SUBADR2 0x3
+#define PCA9685_SUBADR3 0x4
+
+#define PCA9685_MODE1 0x0
+#define PCA9685_PRESCALE 0xFE
+
+#define LED0_ON_L 0x6
+#define LED0_ON_H 0x7
+#define LED0_OFF_L 0x8
+#define LED0_OFF_H 0x9
+
+#define ALLLED_ON_L 0xFA
+#define ALLLED_ON_H 0xFB
+#define ALLLED_OFF_L 0xFC
+#define ALLLED_OF
+
+#define ADDR 0x40	// I2C adress
+
+#define PCA9685_DEVICE_PATH "/dev/pca9685"
+#define PCA9685_BUS PX4_I2C_BUS_EXPANSION
+#define PCA9685_PWMFREQ 60.0f
+#define PCA9685_NCHANS 16 // total amount of pwm outputs
+
+#define PCA9685_PWMMIN 150 // this is the 'minimum' pulse length count (out of 4096)
+#define PCA9685_PWMMAX 600 // this is the 'maximum' pulse length count (out of 4096)_PWMFREQ 60.0f
+
+#define PCA9685_PWMCENTER ((PCA9685_PWMMAX + PCA9685_PWMMIN)/2)
+#define PCA9685_MAXSERVODEG 90.0f /* maximal servo deflection in degrees
+				     PCA9685_PWMMIN <--> -PCA9685_MAXSERVODEG
+				     PCA9685_PWMMAX <--> PCA9685_MAXSERVODEG
+				     */
+#define PCA9685_SCALE ((PCA9685_PWMMAX - PCA9685_PWMCENTER)/(M_DEG_TO_RAD_F * PCA9685_MAXSERVODEG)) // scales from rad to PWM
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+class PCA9685 : public device::I2C
+{
+public:
+	PCA9685(int bus=PCA9685_BUS, uint8_t address=ADDR);
+	virtual ~PCA9685();
+
+
+	virtual int		init();
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+	virtual int		info();
+	virtual int		reset();
+	bool			is_running() { return _running; }
+
+private:
+	work_s			_work;
+
+
+	enum IOX_MODE		_mode;
+	bool			_running;
+	int			_i2cpwm_interval;
+	bool			_should_run;
+	perf_counter_t		_comms_errors;
+
+	uint8_t			_msg[6];
+
+	int			_actuator_controls_sub;
+	struct actuator_controls_s  _actuator_controls;
+	uint16_t	    	_current_values[NUM_ACTUATOR_CONTROLS]; /**< stores the current pwm output
+										  values as sent to the setPin() */
+
+	bool _mode_on_initialized;  /** Set to true after the first call of i2cpwm in mode IOX_MODE_ON */
+
+	static void		i2cpwm_trampoline(void *arg);
+	void			i2cpwm();
+
+	/**
+	 * Helper function to set the pwm frequency
+	 */
+	int setPWMFreq(float freq);
+
+	/**
+	 * Helper function to set the demanded pwm value
+	 * @param num pwm output number
+	 */
+	int setPWM(uint8_t num, uint16_t on, uint16_t off);
+
+	/**
+	 * Sets pin without having to deal with on/off tick placement and properly handles
+	 * a zero value as completely off.  Optional invert parameter supports inverting
+	 * the pulse for sinking to ground.
+	 * @param num pwm output number
+	 * @param val should be a value from 0 to 4095 inclusive.
+	 */
+	int setPin(uint8_t num, uint16_t val, bool invert = false);
+
+
+	/* Wrapper to read a byte from addr */
+	int read8(uint8_t addr, uint8_t &value);
+
+	/* Wrapper to wite a byte to addr */
+	int write8(uint8_t addr, uint8_t value);
+
+};
+
+/* for now, we only support one board */
+namespace
+{
+PCA9685 *g_pca9685;
+}
+
+void pca9685_usage();
+
+extern "C" __EXPORT int pca9685_main(int argc, char *argv[]);
+
+PCA9685::PCA9685(int bus, uint8_t address) :
+	I2C("pca9685", PCA9685_DEVICE_PATH, bus, address, 100000),
+	_mode(IOX_MODE_OFF),
+	_running(false),
+	_i2cpwm_interval(SEC2TICK(1.0f/60.0f)),
+	_should_run(false),
+	_comms_errors(perf_alloc(PC_COUNT, "actuator_controls_2_comms_errors")),
+	_actuator_controls_sub(-1),
+	_actuator_controls(),
+	_mode_on_initialized(false)
+{
+	memset(&_work, 0, sizeof(_work));
+	memset(_msg, 0, sizeof(_msg));
+	memset(_current_values, 0, sizeof(_current_values));
+}
+
+PCA9685::~PCA9685()
+{
+}
+
+int
+PCA9685::init()
+{
+	int ret;
+	ret = I2C::init();
+	if (ret != OK) {
+		return ret;
+	}
+
+	ret = reset();
+	if (ret != OK) {
+		return ret;
+	}
+
+	ret = setPWMFreq(PCA9685_PWMFREQ);
+
+	return ret;
+}
+
+int
+PCA9685::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	int ret = -EINVAL;
+	switch (cmd) {
+
+	case IOX_SET_MODE:
+
+		if (_mode != (IOX_MODE)arg) {
+
+			switch ((IOX_MODE)arg) {
+			case IOX_MODE_OFF:
+				warnx("shutting down");
+				break;
+			case IOX_MODE_ON:
+				warnx("starting");
+				break;
+			case IOX_MODE_TEST_OUT:
+				warnx("test starting");
+				break;
+
+			default:
+				return -1;
+			}
+
+			_mode = (IOX_MODE)arg;
+		}
+
+		// if not active, kick it
+		if (!_running) {
+			_running = true;
+			work_queue(LPWORK, &_work, (worker_t)&PCA9685::i2cpwm_trampoline, this, 1);
+		}
+
+
+		return OK;
+
+	default:
+		// see if the parent class can make any use of it
+		ret = CDev::ioctl(filp, cmd, arg);
+		break;
+	}
+
+	return ret;
+}
+
+int
+PCA9685::info()
+{
+	int ret = OK;
+
+	if (is_running()) {
+		warnx("Driver is running, mode: %u", _mode);
+	} else {
+		warnx("Driver started but not running");
+	}
+
+	return ret;
+}
+
+void
+PCA9685::i2cpwm_trampoline(void *arg)
+{
+	PCA9685 *i2cpwm = reinterpret_cast<PCA9685 *>(arg);
+
+	i2cpwm->i2cpwm();
+}
+
+/**
+ * Main loop function
+ */
+void
+PCA9685::i2cpwm()
+{
+	if (_mode == IOX_MODE_TEST_OUT) {
+		setPin(0, PCA9685_PWMCENTER);
+		_should_run = true;
+	} else if (_mode == IOX_MODE_OFF) {
+		_should_run = false;
+	} else {
+		if (!_mode_on_initialized) {
+			/* Subscribe to actuator control 2 (payload group for gimbal) */
+			_actuator_controls_sub = orb_subscribe(ORB_ID(actuator_controls_2));
+			/* set the uorb update interval lower than the driver pwm interval */
+			orb_set_interval(_actuator_controls_sub, 1000.0f / PCA9685_PWMFREQ - 5);
+
+			_mode_on_initialized = true;
+		}
+
+		/* Read the servo setpoints from the actuator control topics (gimbal) */
+		bool updated;
+		orb_check(_actuator_controls_sub, &updated);
+		if (updated) {
+			orb_copy(ORB_ID(actuator_controls_2), _actuator_controls_sub, &_actuator_controls);
+			for (int i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
+				/* Scale the controls to PWM, first multiply by pi to get rad,
+				 * the control[i] values are on the range -1 ... 1 */
+				uint16_t new_value = PCA9685_PWMCENTER +
+					(_actuator_controls.control[i] * M_PI_F * PCA9685_SCALE);
+				debug("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value,
+						(double)_actuator_controls.control[i]);
+				if (new_value != _current_values[i] &&
+						isfinite(new_value) &&
+						new_value >= PCA9685_PWMMIN &&
+						new_value <= PCA9685_PWMMAX) {
+					/* This value was updated, send the command to adjust the PWM value */
+					setPin(i, new_value);
+					_current_values[i] = new_value;
+				}
+			}
+		}
+		_should_run = true;
+	}
+
+	// check if any activity remains, else stop
+	if (!_should_run) {
+		_running = false;
+		return;
+	}
+
+	// re-queue ourselves to run again later
+	_running = true;
+	work_queue(LPWORK, &_work, (worker_t)&PCA9685::i2cpwm_trampoline, this, _i2cpwm_interval);
+}
+
+int
+PCA9685::setPWM(uint8_t num, uint16_t on, uint16_t off)
+{
+	int ret;
+	/* convert to correct message */
+	_msg[0] = LED0_ON_L + 4 * num;
+	_msg[1] = on;
+	_msg[2] = on >> 8;
+	_msg[3] = off;
+	_msg[4] = off >> 8;
+
+	/* try i2c transfer */
+	ret = transfer(_msg, 5, nullptr, 0);
+
+	if (OK != ret) {
+		perf_count(_comms_errors);
+		log("i2c::transfer returned %d", ret);
+	}
+
+	return ret;
+}
+
+int
+PCA9685::setPin(uint8_t num, uint16_t val, bool invert)
+{
+	// Clamp value between 0 and 4095 inclusive.
+	if (val > 4095) {
+		val = 4095;
+	}
+	if (invert) {
+		if (val == 0) {
+			// Special value for signal fully on.
+			return setPWM(num, 4096, 0);
+		} else if (val == 4095) {
+			// Special value for signal fully off.
+			return setPWM(num, 0, 4096);
+		} else {
+			return setPWM(num, 0, 4095-val);
+		}
+	} else {
+		if (val == 4095) {
+			// Special value for signal fully on.
+			return setPWM(num, 4096, 0);
+		} else if (val == 0) {
+			// Special value for signal fully off.
+			return setPWM(num, 0, 4096);
+		} else {
+			return setPWM(num, 0, val);
+		}
+	}
+
+	return ERROR;
+}
+
+int
+PCA9685::setPWMFreq(float freq)
+{
+	int ret  = OK;
+	freq *= 0.9f;  /* Correct for overshoot in the frequency setting (see issue
+		https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library/issues/11). */
+	float prescaleval = 25000000;
+	prescaleval /= 4096;
+	prescaleval /= freq;
+	prescaleval -= 1;
+	uint8_t prescale = uint8_t(prescaleval + 0.5f); //implicit floor()
+	uint8_t oldmode;
+	ret = read8(PCA9685_MODE1, oldmode);
+	if (ret != OK) {
+		return ret;
+	}
+	uint8_t newmode = (oldmode&0x7F) | 0x10; // sleep
+
+	ret = write8(PCA9685_MODE1, newmode); // go to sleep
+	if (ret != OK) {
+		return ret;
+	}
+	ret = write8(PCA9685_PRESCALE, prescale); // set the prescaler
+	if (ret != OK) {
+		return ret;
+	}
+	ret = write8(PCA9685_MODE1, oldmode);
+	if (ret != OK) {
+		return ret;
+	}
+
+	usleep(5000); //5ms delay (from arduino driver)
+
+	ret = write8(PCA9685_MODE1, oldmode | 0xa1);  //  This sets the MODE1 register to turn on auto increment.
+	if (ret != OK) {
+		return ret;
+	}
+
+	return ret;
+}
+
+/* Wrapper to read a byte from addr */
+int
+PCA9685::read8(uint8_t addr, uint8_t &value)
+{
+	int ret = OK;
+
+	/* send addr */
+	ret = transfer(&addr, sizeof(addr), nullptr, 0);
+	if (ret != OK) {
+		goto fail_read;
+	}
+
+	/* get value */
+	ret = transfer(nullptr, 0, &value, 1);
+	if (ret != OK) {
+		goto fail_read;
+	}
+
+	return ret;
+
+fail_read:
+	perf_count(_comms_errors);
+	log("i2c::transfer returned %d", ret);
+
+	return ret;
+}
+
+int PCA9685::reset(void) {
+	warnx("resetting");
+	return write8(PCA9685_MODE1, 0x0);
+}
+
+/* Wrapper to wite a byte to addr */
+int
+PCA9685::write8(uint8_t addr, uint8_t value) {
+	int ret = OK;
+	_msg[0] = addr;
+	_msg[1] = value;
+	/* send addr and value */
+	ret = transfer(_msg, 2, nullptr, 0);
+	if (ret != OK) {
+		perf_count(_comms_errors);
+		log("i2c::transfer returned %d", ret);
+	}
+	return ret;
+}
+
+void
+pca9685_usage()
+{
+	warnx("missing command: try 'start', 'test', 'stop', 'info'");
+	warnx("options:");
+	warnx("    -b i2cbus (%d)", PX4_I2C_BUS_EXPANSION);
+	warnx("    -a addr (0x%x)", ADDR);
+}
+
+int
+pca9685_main(int argc, char *argv[])
+{
+	int i2cdevice = -1;
+	int i2caddr = ADDR; // 7bit
+
+	int ch;
+
+	// jump over start/off/etc and look at options first
+	while ((ch = getopt(argc, argv, "a:b:")) != EOF) {
+		switch (ch) {
+		case 'a':
+			i2caddr = strtol(optarg, NULL, 0);
+			break;
+
+		case 'b':
+			i2cdevice = strtol(optarg, NULL, 0);
+			break;
+
+		default:
+			pca9685_usage();
+			exit(0);
+		}
+	}
+
+	if (optind >= argc) {
+		pca9685_usage();
+		exit(1);
+	}
+
+	const char *verb = argv[optind];
+
+	int fd;
+	int ret;
+
+	if (!strcmp(verb, "start")) {
+		if (g_pca9685 != nullptr) {
+			errx(1, "already started");
+		}
+
+		if (i2cdevice == -1) {
+			// try the external bus first
+			i2cdevice = PX4_I2C_BUS_EXPANSION;
+			g_pca9685 = new PCA9685(PX4_I2C_BUS_EXPANSION, i2caddr);
+
+			if (g_pca9685 != nullptr && OK != g_pca9685->init()) {
+				delete g_pca9685;
+				g_pca9685 = nullptr;
+			}
+
+			if (g_pca9685 == nullptr) {
+				errx(1, "init failed");
+			}
+		}
+
+		if (g_pca9685 == nullptr) {
+			g_pca9685 = new PCA9685(i2cdevice, i2caddr);
+
+			if (g_pca9685 == nullptr) {
+				errx(1, "new failed");
+			}
+
+			if (OK != g_pca9685->init()) {
+				delete g_pca9685;
+				g_pca9685 = nullptr;
+				errx(1, "init failed");
+			}
+		}
+		fd = open(PCA9685_DEVICE_PATH, 0);
+		if (fd == -1) {
+			errx(1, "Unable to open " PCA9685_DEVICE_PATH);
+		}
+		ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_ON);
+		close(fd);
+
+
+		exit(0);
+	}
+
+	// need the driver past this point
+	if (g_pca9685 == nullptr) {
+		warnx("not started, run pca9685 start");
+		exit(1);
+	}
+
+	if (!strcmp(verb, "info")) {
+		g_pca9685->info();
+		exit(0);
+	}
+
+	if (!strcmp(verb, "reset")) {
+		g_pca9685->reset();
+		exit(0);
+	}
+
+
+	if (!strcmp(verb, "test")) {
+		fd = open(PCA9685_DEVICE_PATH, 0);
+
+		if (fd == -1) {
+			errx(1, "Unable to open " PCA9685_DEVICE_PATH);
+		}
+
+		ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_TEST_OUT);
+
+		close(fd);
+		exit(ret);
+	}
+
+	if (!strcmp(verb, "stop")) {
+		fd = open(PCA9685_DEVICE_PATH, 0);
+
+		if (fd == -1) {
+			errx(1, "Unable to open " PCA9685_DEVICE_PATH);
+		}
+
+		ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_OFF);
+		close(fd);
+
+		// wait until we're not running any more
+		for (unsigned i = 0; i < 15; i++) {
+			if (!g_pca9685->is_running()) {
+				break;
+			}
+
+			usleep(50000);
+			printf(".");
+			fflush(stdout);
+		}
+		printf("\n");
+		fflush(stdout);
+
+		if (!g_pca9685->is_running()) {
+			delete g_pca9685;
+			g_pca9685= nullptr;
+			warnx("stopped, exiting");
+			exit(0);
+		} else {
+			warnx("stop failed.");
+			exit(1);
+		}
+	}
+
+	pca9685_usage();
+	exit(0);
+}
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 32069cf09..97919538f 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1968,7 +1968,7 @@ PX4IO::print_status(bool extended_status)
 	printf("actuators");
 
 	for (unsigned i = 0; i < _max_actuators; i++)
-		printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i));
+		printf(" %hi", int16_t(io_reg_get(PX4IO_PAGE_ACTUATORS, i)));
 
 	printf("\n");
 	printf("servos");
diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp
index bca1715fa..80ecab2ee 100644
--- a/src/drivers/sf0x/sf0x.cpp
+++ b/src/drivers/sf0x/sf0x.cpp
@@ -598,7 +598,8 @@ SF0X::collect()
 				memcpy(_linebuf, buf, (lend + 1) - (i + 1));
 			}
 
-			if (_linebuf[i] == '.') {
+			/* we need a digit before the dot and a dot for a valid number */
+			if (i > 0 && _linebuf[i] == '.') {
 				valid = true;
 			}
 		}
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
index 46db788a6..926a8db2a 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
@@ -169,7 +169,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
 
 	if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
 
-		float id = _rate_error * dt;
+		float id = _rate_error * dt * scaler;
 
 		/*
 		 * anti-windup: do not allow integrator to increase if actuator is at limit
@@ -190,7 +190,9 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
 	float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
 
 	/* Apply PI rate controller and store non-limited output */
-	_last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained) * scaler * scaler;  //scaler is proportional to 1/airspeed
+	_last_output = _bodyrate_setpoint * _k_ff * scaler +
+		_rate_error * _k_p * scaler * scaler
+		+ integrator_constrained;  //scaler is proportional to 1/airspeed
 //	warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
 //	warnx("roll: _last_output %.4f", (double)_last_output);
 	return math::constrain(_last_output, -1.0f, 1.0f);
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
index 9894a34d7..94bd26f03 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
@@ -135,7 +135,7 @@ float ECL_RollController::control_bodyrate(float pitch,
 
 	if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
 
-		float id = _rate_error * dt;
+		float id = _rate_error * dt * scaler;
 
 		/*
 		* anti-windup: do not allow integrator to increase if actuator is at limit
@@ -157,7 +157,9 @@ float ECL_RollController::control_bodyrate(float pitch,
 	//warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max);
 
 	/* Apply PI rate controller and store non-limited output */
-	_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler;  //scaler is proportional to 1/airspeed
+	_last_output = _bodyrate_setpoint * _k_ff * scaler +
+		_rate_error * _k_p * scaler * scaler
+		+ integrator_constrained;  //scaler is proportional to 1/airspeed
 
 	return math::constrain(_last_output, -1.0f, 1.0f);
 }
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index db9ec9005..a5a772825 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -129,7 +129,7 @@ extern struct system_load_s system_load;
 #define POSITION_TIMEOUT		(2 * 1000 * 1000)	/**< consider the local or global position estimate invalid after 600ms */
 #define FAILSAFE_DEFAULT_TIMEOUT	(3 * 1000 * 1000)	/**< hysteresis time - the failsafe will trigger after 3 seconds in this state */
 #define RC_TIMEOUT			500000
-#define DL_TIMEOUT			5 * 1000* 1000
+#define DL_TIMEOUT			(10 * 1000 * 1000)
 #define OFFBOARD_TIMEOUT		500000
 #define DIFFPRESS_TIMEOUT		2000000
 
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 408605939..ed7e879d3 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -134,44 +134,44 @@ Mavlink::Mavlink() :
 	_mode(MAVLINK_MODE_NORMAL),
 	_channel(MAVLINK_COMM_0),
 	_logbuffer {},
-	   _total_counter(0),
-	   _receive_thread {},
-	   _verbose(false),
-	   _forwarding_on(false),
-	   _passing_on(false),
-	   _ftp_on(false),
-	   _uart_fd(-1),
-	   _baudrate(57600),
-	   _datarate(1000),
-	   _datarate_events(500),
-	   _rate_mult(1.0f),
-	   _mavlink_param_queue_index(0),
-	   mavlink_link_termination_allowed(false),
-	   _subscribe_to_stream(nullptr),
-	   _subscribe_to_stream_rate(0.0f),
-	   _flow_control_enabled(true),
-	   _last_write_success_time(0),
-	   _last_write_try_time(0),
-	   _bytes_tx(0),
-	   _bytes_txerr(0),
-	   _bytes_rx(0),
-	   _bytes_timestamp(0),
-	   _rate_tx(0.0f),
-	   _rate_txerr(0.0f),
-	   _rate_rx(0.0f),
-	   _rstatus {},
-	   _message_buffer {},
-	   _message_buffer_mutex {},
-	   _send_mutex {},
-	   _param_initialized(false),
-	   _param_system_id(0),
-	   _param_component_id(0),
-	   _param_system_type(0),
-	   _param_use_hil_gps(0),
-
-	   /* performance counters */
-	   _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
-	   _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe"))
+	_total_counter(0),
+	_receive_thread {},
+	_verbose(false),
+	_forwarding_on(false),
+	_passing_on(false),
+	_ftp_on(false),
+	_uart_fd(-1),
+	_baudrate(57600),
+	_datarate(1000),
+	_datarate_events(500),
+	_rate_mult(1.0f),
+	_mavlink_param_queue_index(0),
+	mavlink_link_termination_allowed(false),
+	_subscribe_to_stream(nullptr),
+	_subscribe_to_stream_rate(0.0f),
+	_flow_control_enabled(true),
+	_last_write_success_time(0),
+	_last_write_try_time(0),
+	_bytes_tx(0),
+	_bytes_txerr(0),
+	_bytes_rx(0),
+	_bytes_timestamp(0),
+	_rate_tx(0.0f),
+	_rate_txerr(0.0f),
+	_rate_rx(0.0f),
+	_rstatus {},
+	_message_buffer {},
+	_message_buffer_mutex {},
+	_send_mutex {},
+	_param_initialized(false),
+	_param_system_id(0),
+	_param_component_id(0),
+	_param_system_type(0),
+	_param_use_hil_gps(0),
+
+	/* performance counters */
+	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
+	_txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe"))
 {
 	fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl;
 
@@ -217,6 +217,8 @@ Mavlink::Mavlink() :
 		errx(1, "instance ID is out of range");
 		break;
 	}
+
+	_rstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
 }
 
 Mavlink::~Mavlink()
@@ -1227,7 +1229,10 @@ Mavlink::task_main(int argc, char *argv[])
 				_mode = MAVLINK_MODE_CUSTOM;
 
 			} else if (strcmp(optarg, "camera") == 0) {
-				_mode = MAVLINK_MODE_CAMERA;
+				// left in here for compatibility
+				_mode = MAVLINK_MODE_ONBOARD;
+			} else if (strcmp(optarg, "onboard") == 0) {
+				_mode = MAVLINK_MODE_ONBOARD;
 			}
 
 			break;
@@ -1287,8 +1292,8 @@ Mavlink::task_main(int argc, char *argv[])
 		warnx("mode: CUSTOM");
 		break;
 
-	case MAVLINK_MODE_CAMERA:
-		warnx("mode: CAMERA");
+	case MAVLINK_MODE_ONBOARD:
+		warnx("mode: ONBOARD");
 		break;
 
 	default:
@@ -1391,9 +1396,10 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
 		configure_stream("ATTITUDE_TARGET", 3.0f);
 		configure_stream("DISTANCE_SENSOR", 0.5f);
+		configure_stream("OPTICAL_FLOW", 0.5f);
 		break;
 
-	case MAVLINK_MODE_CAMERA:
+	case MAVLINK_MODE_ONBOARD:
 		configure_stream("SYS_STATUS", 1.0f);
 		configure_stream("ATTITUDE", 15.0f);
 		configure_stream("GLOBAL_POSITION_INT", 15.0f);
@@ -1653,6 +1659,8 @@ Mavlink::display_status()
 		printf("\tGCS heartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time));
 	}
 
+	printf("\tmavlink chan: #%u\n", _channel);
+
 	if (_rstatus.timestamp > 0) {
 
 		printf("\ttype:\t\t");
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 1e2e94cef..7f9d225bb 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -127,7 +127,7 @@ public:
 	enum MAVLINK_MODE {
 		MAVLINK_MODE_NORMAL = 0,
 		MAVLINK_MODE_CUSTOM,
-		MAVLINK_MODE_CAMERA
+		MAVLINK_MODE_ONBOARD
 	};
 
 	void			set_mode(enum MAVLINK_MODE);
diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp
index 7a761588a..d436c95e9 100644
--- a/src/modules/mavlink/mavlink_mission.cpp
+++ b/src/modules/mavlink/mavlink_mission.cpp
@@ -58,6 +58,10 @@
 #endif
 static const int ERROR = -1;
 
+#define CHECK_SYSID_COMPID_MISSION(_msg)		(_msg.target_system == mavlink_system.sysid && \
+						((_msg.target_component == mavlink_system.compid) || \
+						(_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \
+						(_msg.target_component == MAV_COMP_ID_ALL)))
 
 MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(mavlink),
 	_state(MAVLINK_WPM_STATE_IDLE),
@@ -79,8 +83,7 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(m
 	_mission_result_sub(-1),
 	_offboard_mission_pub(-1),
 	_slow_rate_limiter(_interval / 10.0f),
-	_verbose(false),
-	_comp_id(MAV_COMP_ID_MISSIONPLANNER)
+	_verbose(false)
 {
 	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
 	_mission_result_sub = orb_subscribe(ORB_ID(mission_result));
@@ -384,7 +387,7 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg)
 	mavlink_mission_ack_t wpa;
 	mavlink_msg_mission_ack_decode(msg, &wpa);
 
-	if (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/) {
+	if (CHECK_SYSID_COMPID_MISSION(wpa)) {
 		if ((msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid)) {
 			if (_state == MAVLINK_WPM_STATE_SENDLIST) {
 				_time_last_recv = hrt_absolute_time();
@@ -416,7 +419,7 @@ MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg)
 	mavlink_mission_set_current_t wpc;
 	mavlink_msg_mission_set_current_decode(msg, &wpc);
 
-	if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) {
+	if (CHECK_SYSID_COMPID_MISSION(wpc)) {
 		if (_state == MAVLINK_WPM_STATE_IDLE) {
 			_time_last_recv = hrt_absolute_time();
 
@@ -451,7 +454,7 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg)
 	mavlink_mission_request_list_t wprl;
 	mavlink_msg_mission_request_list_decode(msg, &wprl);
 
-	if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) {
+	if (CHECK_SYSID_COMPID_MISSION(wprl)) {
 		if (_state == MAVLINK_WPM_STATE_IDLE || _state == MAVLINK_WPM_STATE_SENDLIST) {
 			_time_last_recv = hrt_absolute_time();
 
@@ -487,7 +490,7 @@ MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg)
 	mavlink_mission_request_t wpr;
 	mavlink_msg_mission_request_decode(msg, &wpr);
 
-	if (wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) {
+	if (CHECK_SYSID_COMPID_MISSION(wpr)) {
 		if (msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid) {
 			if (_state == MAVLINK_WPM_STATE_SENDLIST) {
 				_time_last_recv = hrt_absolute_time();
@@ -558,7 +561,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
 	mavlink_mission_count_t wpc;
 	mavlink_msg_mission_count_decode(msg, &wpc);
 
-	if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) {
+	if (CHECK_SYSID_COMPID_MISSION(wpc)) {
 		if (_state == MAVLINK_WPM_STATE_IDLE) {
 			_time_last_recv = hrt_absolute_time();
 
@@ -624,7 +627,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
 	mavlink_mission_item_t wp;
 	mavlink_msg_mission_item_decode(msg, &wp);
 
-	if (wp.target_system == mavlink_system.sysid && wp.target_component == _comp_id) {
+	if (CHECK_SYSID_COMPID_MISSION(wp)) {
 		if (_state == MAVLINK_WPM_STATE_GETLIST) {
 			_time_last_recv = hrt_absolute_time();
 
@@ -710,7 +713,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
 	mavlink_mission_clear_all_t wpca;
 	mavlink_msg_mission_clear_all_decode(msg, &wpca);
 
-	if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) {
+	if (CHECK_SYSID_COMPID_MISSION(wpca)) {
 
 		if (_state == MAVLINK_WPM_STATE_IDLE) {
 			/* don't touch mission items storage itself, but only items count in mission state */
diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h
index 8ff27f87d..a7bb94c22 100644
--- a/src/modules/mavlink/mavlink_mission.h
+++ b/src/modules/mavlink/mavlink_mission.h
@@ -126,8 +126,6 @@ private:
 
 	bool _verbose;
 
-	uint8_t			_comp_id;
-
 	/* do not allow top copying this class */
 	MavlinkMissionManager(MavlinkMissionManager &);
 	MavlinkMissionManager& operator = (const MavlinkMissionManager &);
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index c0fae0a2f..a602344fd 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -112,7 +112,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_telemetry_status_pub(-1),
 	_rc_pub(-1),
 	_manual_pub(-1),
-	_radio_status_available(false),
 	_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
 	_hil_frames(0),
 	_old_timestamp(0),
@@ -137,6 +136,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_command_long(msg);
 		break;
 
+	case MAVLINK_MSG_ID_COMMAND_INT:
+		handle_message_command_int(msg);
+		break;
+
 	case MAVLINK_MSG_ID_OPTICAL_FLOW:
 		handle_message_optical_flow(msg);
 		break;
@@ -277,6 +280,62 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
 }
 
 void
+MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
+{
+	/* command */
+	mavlink_command_int_t cmd_mavlink;
+	mavlink_msg_command_int_decode(msg, &cmd_mavlink);
+
+	if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid)
+			|| (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
+		//check for MAVLINK terminate command
+		if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
+			/* This is the link shutdown command, terminate mavlink */
+			warnx("terminated by remote command");
+			fflush(stdout);
+			usleep(50000);
+
+			/* terminate other threads and this thread */
+			_mavlink->_task_should_exit = true;
+
+		} else {
+
+			if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
+				warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID",
+				      mavlink_system.sysid, mavlink_system.compid);
+				return;
+			}
+
+			struct vehicle_command_s vcmd;
+			memset(&vcmd, 0, sizeof(vcmd));
+
+			/* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */
+			vcmd.param1 = cmd_mavlink.param1;
+			vcmd.param2 = cmd_mavlink.param2;
+			vcmd.param3 = cmd_mavlink.param3;
+			vcmd.param4 = cmd_mavlink.param4;
+			/* these are coordinates as 1e7 scaled integers to work around the 32 bit floating point limits */
+			vcmd.param5 = ((double)cmd_mavlink.x) / 1e7;
+			vcmd.param6 = ((double)cmd_mavlink.y) / 1e7;
+			vcmd.param7 = cmd_mavlink.z;
+			// XXX do proper translation
+			vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command;
+			vcmd.target_system = cmd_mavlink.target_system;
+			vcmd.target_component = cmd_mavlink.target_component;
+			vcmd.source_system = msg->sysid;
+			vcmd.source_component = msg->compid;
+
+			if (_cmd_pub < 0) {
+				_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
+
+			} else {
+				orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd);
+			}
+		}
+	}
+}
+
+void
 MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
 {
 	/* optical flow */
@@ -430,9 +489,6 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 		} else {
 			orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
 		}
-
-		/* this means that heartbeats alone won't be published to the radio status no more */
-		_radio_status_available = true;
 	}
 }
 
@@ -474,25 +530,17 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
 
 			struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
 
-			hrt_abstime tnow = hrt_absolute_time();
-
-			/* always set heartbeat, publish only if telemetry link not up */
-			tstatus.heartbeat_time = tnow;
-
-			/* if no radio status messages arrive, lets at least publish that heartbeats were received */
-			if (!_radio_status_available) {
+			/* set heartbeat time and topic time and publish -
+			 * the telem status also gets updated on telemetry events
+			 */
+			tstatus.timestamp = hrt_absolute_time();
+			tstatus.heartbeat_time = tstatus.timestamp;
 
-				tstatus.timestamp = tnow;
-				/* telem_time indicates the timestamp of a telemetry status packet and we got none */
-				tstatus.telem_time = 0;
-				tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
+			if (_telemetry_status_pub < 0) {
+				_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
 
-				if (_telemetry_status_pub < 0) {
-					_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
-
-				} else {
-					orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
-				}
+			} else {
+				orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
 			}
 		}
 	}
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 014193100..91125736c 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -110,6 +110,7 @@ private:
 
 	void handle_message(mavlink_message_t *msg);
 	void handle_message_command_long(mavlink_message_t *msg);
+	void handle_message_command_int(mavlink_message_t *msg);
 	void handle_message_optical_flow(mavlink_message_t *msg);
 	void handle_message_set_mode(mavlink_message_t *msg);
 	void handle_message_vicon_position_estimate(mavlink_message_t *msg);
@@ -151,7 +152,6 @@ private:
 	orb_advert_t _telemetry_status_pub;
 	orb_advert_t _rc_pub;
 	orb_advert_t _manual_pub;
-	bool _radio_status_available;
 	int _control_mode_sub;
 	int _hil_frames;
 	uint64_t _old_timestamp;
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 331a9a728..042c46afd 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -353,6 +353,8 @@ Navigator::task_main()
 			case NAVIGATION_STATE_ACRO:
 			case NAVIGATION_STATE_ALTCTL:
 			case NAVIGATION_STATE_POSCTL:
+			case NAVIGATION_STATE_LAND:
+			case NAVIGATION_STATE_TERMINATION:
 				_navigation_mode = nullptr;
 				_can_loiter_at_sp = false;
 				break;
@@ -368,8 +370,6 @@ Navigator::task_main()
 			case NAVIGATION_STATE_AUTO_RTGS:
 				_navigation_mode = &_rtl; /* TODO: change this to something else */
 				break;
-			case NAVIGATION_STATE_LAND:
-			case NAVIGATION_STATE_TERMINATION:
 			case NAVIGATION_STATE_OFFBOARD:
 				_navigation_mode = &_offboard;
 				break;
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 7ce6ef5ef..229bfe3ce 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -214,6 +214,16 @@ PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f);
  */
 PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0);
 
+/**
+ * QNH for barometer
+ *
+ * @min 500
+ * @max 1500
+ * @group Sensor Calibration
+ * @unit hPa
+ */
+PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f);
+
 
 /**
  * Board rotation
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 4e8a8c01d..f40034d79 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -143,6 +143,12 @@
 
 #define STICK_ON_OFF_LIMIT 0.75f
 
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
 /**
  * Sensor app start / stop handling function
  *
@@ -235,7 +241,7 @@ private:
 	math::Matrix<3, 3>	_board_rotation;		/**< rotation matrix for the orientation that the board is mounted */
 	math::Matrix<3, 3>	_external_mag_rotation;		/**< rotation matrix for the orientation that an external mag is mounted */
 	bool		_mag_is_external;		/**< true if the active mag is on an external board */
-	
+
 	uint64_t _battery_discharged;			/**< battery discharged current in mA*ms */
 	hrt_abstime _battery_current_timestamp;	/**< timestamp of last battery current reading */
 
@@ -258,7 +264,7 @@ private:
 
 		int board_rotation;
 		int external_mag_rotation;
-		
+
 		float board_offset[3];
 
 		int rc_map_roll;
@@ -301,6 +307,8 @@ private:
 		float battery_voltage_scaling;
 		float battery_current_scaling;
 
+		float baro_qnh;
+
 	}		_parameters;			/**< local copies of interesting parameters */
 
 	struct {
@@ -354,9 +362,11 @@ private:
 
 		param_t board_rotation;
 		param_t external_mag_rotation;
-		
+
 		param_t board_offset[3];
 
+		param_t baro_qnh;
+
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
@@ -462,12 +472,6 @@ private:
 namespace sensors
 {
 
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-static const int ERROR = -1;
-
 Sensors	*g_sensors = nullptr;
 }
 
@@ -611,12 +615,15 @@ Sensors::Sensors() :
 	/* rotations */
 	_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
 	_parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT");
-	
+
 	/* rotation offsets */
 	_parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF");
 	_parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF");
 	_parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF");
 
+	/* Barometer QNH */
+	_parameter_handles.baro_qnh = param_find("SENS_BARO_QNH");
+
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -828,19 +835,37 @@ Sensors::parameters_update()
 
 	get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
 	get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation);
-	
+
 	param_get(_parameter_handles.board_offset[0], &(_parameters.board_offset[0]));
 	param_get(_parameter_handles.board_offset[1], &(_parameters.board_offset[1]));
 	param_get(_parameter_handles.board_offset[2], &(_parameters.board_offset[2]));
-	
+
 	/** fine tune board offset on parameter update **/
-	math::Matrix<3, 3> board_rotation_offset; 
+	math::Matrix<3, 3> board_rotation_offset;
 	board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0],
 							 M_DEG_TO_RAD_F * _parameters.board_offset[1],
 							 M_DEG_TO_RAD_F * _parameters.board_offset[2]);
-	
+
 	_board_rotation = _board_rotation * board_rotation_offset;
 
+	/* update barometer qnh setting */
+	param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh));
+	int	fd;
+	fd = open(BARO_DEVICE_PATH, 0);
+	if (fd < 0) {
+		warn("%s", BARO_DEVICE_PATH);
+		errx(1, "FATAL: no barometer found");
+
+	} else {
+		int ret = ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100));
+		if (ret) {
+			warnx("qnh could not be set");
+			close(fd);
+			return ERROR;
+		}
+		close(fd);
+	}
+
 	return OK;
 }
 
diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c
index a4f17eebd..e005bf9c1 100644
--- a/src/systemcmds/tests/test_sensors.c
+++ b/src/systemcmds/tests/test_sensors.c
@@ -331,7 +331,7 @@ mag(int argc, char *argv[])
 
 	float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z);
 
-	if (len < 1.0f || len > 3.0f) {
+	if (len < 0.25f || len > 3.0f) {
 		warnx("MAG scale error!");
 		return ERROR;
 	}