Merged master

126 files changed, 4823 insertions, 1704 deletions

diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h
index 0b8e949c9..d6a88714b 100644
--- a/src/drivers/airspeed/airspeed.h
+++ b/src/drivers/airspeed/airspeed.h
@@ -107,6 +107,10 @@ private:
 	RingBuffer		*_reports;
 	perf_counter_t		_buffer_overflows;
 
+	/* this class has pointer data members and should not be copied */
+	Airspeed(const Airspeed&);
+	Airspeed& operator=(const Airspeed&);
+
 protected:
 	virtual int	probe();
 
diff --git a/src/drivers/airspeed/module.mk b/src/drivers/airspeed/module.mk
index 4eef06161..5fbc75309 100644
--- a/src/drivers/airspeed/module.mk
+++ b/src/drivers/airspeed/module.mk
@@ -36,3 +36,5 @@
 #
 
 SRCS			= airspeed.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/bma180/module.mk b/src/drivers/bma180/module.mk
index 4c60ee082..33433307a 100644
--- a/src/drivers/bma180/module.mk
+++ b/src/drivers/bma180/module.mk
@@ -38,3 +38,5 @@
 MODULE_COMMAND	= bma180
 
 SRCS		= bma180.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h
index 7df234cab..9d684e394 100644
--- a/src/drivers/device/device.h
+++ b/src/drivers/device/device.h
@@ -240,6 +240,7 @@ private:
 	 * @param context	Pointer to the interrupted context.
 	 */
 	static void	dev_interrupt(int irq, void *context);
+
 };
 
 /**
@@ -469,6 +470,10 @@ private:
 	 * @return		OK, or -errno on error.
 	 */
 	int		remove_poll_waiter(struct pollfd *fds);
+
+	/* do not allow copying this class */
+	CDev(const CDev&);
+	CDev operator=(const CDev&);
 };
 
 /**
@@ -538,6 +543,10 @@ private:
 } // namespace device
 
 // class instance for primary driver of each class
-#define CLASS_DEVICE_PRIMARY 0
+enum CLASS_DEVICE {
+	CLASS_DEVICE_PRIMARY=0,
+	CLASS_DEVICE_SECONDARY=1,
+	CLASS_DEVICE_TERTIARY=2
+};
 
 #endif /* _DEVICE_DEVICE_H */
diff --git a/src/drivers/device/i2c.h b/src/drivers/device/i2c.h
index 549879352..705b398b0 100644
--- a/src/drivers/device/i2c.h
+++ b/src/drivers/device/i2c.h
@@ -138,6 +138,9 @@ private:
 	uint16_t		_address;
 	uint32_t		_frequency;
 	struct i2c_dev_s	*_dev;
+
+	I2C(const device::I2C&);
+	I2C operator=(const device::I2C&);
 };
 
 } // namespace device
diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h
index a9e22eaa6..b26e2e7c8 100644
--- a/src/drivers/device/ringbuffer.h
+++ b/src/drivers/device/ringbuffer.h
@@ -162,6 +162,10 @@ private:
 	volatile unsigned	_tail;	/**< removal point in _item_size units */
 
 	unsigned		_next(unsigned index);
+
+	/* we don't want this class to be copied */
+	RingBuffer(const RingBuffer&);
+	RingBuffer operator=(const RingBuffer&);
 };
 
 RingBuffer::RingBuffer(unsigned num_items, size_t item_size) :
diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h
index 54849c8c3..1d9837689 100644
--- a/src/drivers/device/spi.h
+++ b/src/drivers/device/spi.h
@@ -129,10 +129,15 @@ private:
 	uint32_t		_frequency;
 	struct spi_dev_s	*_dev;
 
+	/* this class does not allow copying */
+	SPI(const SPI&);
+	SPI operator=(const SPI&);
+
 protected:
 	int			_bus;
 
 	int	_transfer(uint8_t *send, uint8_t *recv, unsigned len);
+
 };
 
 } // namespace device
diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h
index 7d93ed938..1f98d966b 100644
--- a/src/drivers/drv_accel.h
+++ b/src/drivers/drv_accel.h
@@ -81,7 +81,9 @@ struct accel_scale {
 /*
  * ObjDev tag for raw accelerometer data.
  */
-ORB_DECLARE(sensor_accel);
+ORB_DECLARE(sensor_accel0);
+ORB_DECLARE(sensor_accel1);
+ORB_DECLARE(sensor_accel2);
 
 /*
  * ioctl() definitions
diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h
index e2f0137ae..248b9a73d 100644
--- a/src/drivers/drv_baro.h
+++ b/src/drivers/drv_baro.h
@@ -63,7 +63,8 @@ struct baro_report {
 /*
  * ObjDev tag for raw barometer data.
  */
-ORB_DECLARE(sensor_baro);
+ORB_DECLARE(sensor_baro0);
+ORB_DECLARE(sensor_baro1);
 
 /*
  * ioctl() definitions
diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h
index 2ae8c7058..41b013a44 100644
--- a/src/drivers/drv_gyro.h
+++ b/src/drivers/drv_gyro.h
@@ -81,7 +81,9 @@ struct gyro_scale {
 /*
  * ObjDev tag for raw gyro data.
  */
-ORB_DECLARE(sensor_gyro);
+ORB_DECLARE(sensor_gyro0);
+ORB_DECLARE(sensor_gyro1);
+ORB_DECLARE(sensor_gyro2);
 
 /*
  * ioctl() definitions
diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h
index a259ac9c0..5ddf5d08e 100644
--- a/src/drivers/drv_mag.h
+++ b/src/drivers/drv_mag.h
@@ -79,8 +79,9 @@ struct mag_scale {
 /*
  * ObjDev tag for raw magnetometer data.
  */
-ORB_DECLARE(sensor_mag);
-
+ORB_DECLARE(sensor_mag0);
+ORB_DECLARE(sensor_mag1);
+ORB_DECLARE(sensor_mag2);
 
 /*
  * mag device types, for _device_id
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index 972573f9f..84815fdfb 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -202,6 +202,9 @@ ORB_DECLARE(output_pwm);
 /** force safety switch off (to disable use of safety switch) */
 #define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23)
 
+/** force failsafe mode (failsafe values are set immediately even if failsafe condition not met) */
+#define PWM_SERVO_SET_FORCE_FAILSAFE _IOC(_PWM_SERVO_BASE, 24)
+
 /*
  *
  *
diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h
index b7981e9c4..19f792d19 100644
--- a/src/drivers/drv_tone_alarm.h
+++ b/src/drivers/drv_tone_alarm.h
@@ -149,6 +149,7 @@ enum {
 	TONE_GPS_WARNING_TUNE,
 	TONE_ARMING_FAILURE_TUNE,
 	TONE_PARACHUTE_RELEASE_TUNE,
+	TONE_EKF_WARNING_TUNE,
 	TONE_NUMBER_OF_TUNES
 };
 
diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp
index c15a0cee4..f98d615a2 100644
--- a/src/drivers/ets_airspeed/ets_airspeed.cpp
+++ b/src/drivers/ets_airspeed/ets_airspeed.cpp
@@ -286,6 +286,9 @@ void	info();
 
 /**
  * Start the driver.
+ *
+ * This function only returns if the sensor is up and running
+ * or could not be detected successfully.
  */
 void
 start(int i2c_bus)
diff --git a/src/drivers/ets_airspeed/module.mk b/src/drivers/ets_airspeed/module.mk
index 15346c5c5..8aaaf0ebb 100644
--- a/src/drivers/ets_airspeed/module.mk
+++ b/src/drivers/ets_airspeed/module.mk
@@ -36,6 +36,9 @@
 #
 
 MODULE_COMMAND		= ets_airspeed
-MODULE_STACKSIZE	= 2048
 
 SRCS			= ets_airspeed.cpp
+
+MODULE_STACKSIZE	= 1200
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/hil/module.mk b/src/drivers/hil/module.mk
index f8895f5d5..f1fc49fb3 100644
--- a/src/drivers/hil/module.mk
+++ b/src/drivers/hil/module.mk
@@ -38,3 +38,5 @@
 MODULE_COMMAND	= hil
 
 SRCS		= hil.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index 4aef43102..0e9a961ac 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -162,6 +162,7 @@ private:
 
 	orb_advert_t		_mag_topic;
 	orb_advert_t		_subsystem_pub;
+	orb_id_t		_mag_orb_id;
 
 	perf_counter_t		_sample_perf;
 	perf_counter_t		_comms_errors;
@@ -337,6 +338,9 @@ private:
 	 */
 	 int 			check_offset();
 
+	/* this class has pointer data members, do not allow copying it */
+	HMC5883(const HMC5883&);
+	HMC5883 operator=(const HMC5883&);
 };
 
 /*
@@ -347,14 +351,17 @@ extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]);
 
 HMC5883::HMC5883(int bus, const char *path, enum Rotation rotation) :
 	I2C("HMC5883", path, bus, HMC5883L_ADDRESS, 400000),
+	_work{},
 	_measure_ticks(0),
 	_reports(nullptr),
+	_scale{},
 	_range_scale(0), /* default range scale from counts to gauss */
 	_range_ga(1.3f),
 	_collect_phase(false),
 	_class_instance(-1),
 	_mag_topic(-1),
 	_subsystem_pub(-1),
+	_mag_orb_id(nullptr),
 	_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
 	_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")),
@@ -423,6 +430,20 @@ HMC5883::init()
 
 	_class_instance = register_class_devname(MAG_DEVICE_PATH);
 
+	switch (_class_instance) {
+		case CLASS_DEVICE_PRIMARY:
+			_mag_orb_id = ORB_ID(sensor_mag0);
+			break;
+
+		case CLASS_DEVICE_SECONDARY:
+			_mag_orb_id = ORB_ID(sensor_mag1);
+			break;
+
+		case CLASS_DEVICE_TERTIARY:
+			_mag_orb_id = ORB_ID(sensor_mag2);
+			break;
+	}
+
 	ret = OK;
 	/* sensor is ok, but not calibrated */
 	_sensor_ok = true;
@@ -867,7 +888,8 @@ HMC5883::collect()
 	struct {
 		int16_t		x, y, z;
 	} report;
-	int	ret = -EIO;
+
+	int	ret;
 	uint8_t	cmd;
 	uint8_t check_counter;
 
@@ -945,16 +967,16 @@ HMC5883::collect()
 	// apply user specified rotation
 	rotate_3f(_rotation, new_report.x, new_report.y, new_report.z);
 
-	if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
+	if (!(_pub_blocked)) {
 
 		if (_mag_topic != -1) {
 			/* publish it */
-			orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
+			orb_publish(_mag_orb_id, _mag_topic, &new_report);
 		} else {
-			_mag_topic = orb_advertise(ORB_ID(sensor_mag), &new_report);
+			_mag_topic = orb_advertise(_mag_orb_id, &new_report);
 
 			if (_mag_topic < 0)
-				debug("failed to create sensor_mag publication");
+				debug("ADVERT FAIL");
 		}
 	}
 
@@ -1157,7 +1179,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 out:
 
 	if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
-		warn("WARNING: failed to set new scale / offsets for mag");
+		warn("failed to set new scale / offsets for mag");
 	}
 
 	/* set back to normal mode */
@@ -1341,8 +1363,8 @@ namespace hmc5883
 #endif
 const int ERROR = -1;
 
-HMC5883	*g_dev_int;
-HMC5883	*g_dev_ext;
+HMC5883	*g_dev_int = nullptr;
+HMC5883	*g_dev_ext = nullptr;
 
 void	start(int bus, enum Rotation rotation);
 void	test(int bus);
@@ -1353,6 +1375,9 @@ void	usage();
 
 /**
  * Start the driver.
+ *
+ * This function call only returns once the driver
+ * is either successfully up and running or failed to start.
  */
 void
 start(int bus, enum Rotation rotation)
@@ -1378,6 +1403,11 @@ start(int bus, enum Rotation rotation)
 			errx(0, "already started internal");
 		g_dev_int = new HMC5883(PX4_I2C_BUS_ONBOARD, HMC5883L_DEVICE_PATH_INT, rotation);
 		if (g_dev_int != nullptr && OK != g_dev_int->init()) {
+
+			/* tear down the failing onboard instance */
+			delete g_dev_int;
+			g_dev_int = nullptr;
+
 			if (bus == PX4_I2C_BUS_ONBOARD) {
 				goto fail;
 			}
@@ -1443,7 +1473,7 @@ test(int bus)
 	int fd = open(path, O_RDONLY);
 
 	if (fd < 0)
-		err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", path);
+		err(1, "%s open failed (try 'hmc5883 start')", path);
 
 	/* do a simple demand read */
 	sz = read(fd, &report, sizeof(report));
diff --git a/src/drivers/hmc5883/module.mk b/src/drivers/hmc5883/module.mk
index 07377556d..5daa01dc5 100644
--- a/src/drivers/hmc5883/module.mk
+++ b/src/drivers/hmc5883/module.mk
@@ -37,7 +37,8 @@
 
 MODULE_COMMAND	= hmc5883
 
-# XXX seems excessive, check if 2048 is sufficient
-MODULE_STACKSIZE	= 4096
-
 SRCS		= hmc5883.cpp
+
+MODULE_STACKSIZE	= 1200
+
+EXTRACXXFLAGS	= -Weffc++
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 64d1a7e55..cfae8761c 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -213,6 +213,7 @@ private:
 	float			_gyro_range_scale;
 	float			_gyro_range_rad_s;
 	orb_advert_t		_gyro_topic;
+	orb_id_t		_orb_id;
 	int			_class_instance;
 
 	unsigned		_current_rate;
@@ -330,15 +331,22 @@ private:
 	 * @return 0 on success, 1 on failure
 	 */
 	 int 			self_test();
+
+	/* this class does not allow copying */
+	L3GD20(const L3GD20&);
+	L3GD20 operator=(const L3GD20&);
 };
 
 L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation) :
 	SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */),
+	_call{},
 	_call_interval(0),
 	_reports(nullptr),
+	_gyro_scale{},
 	_gyro_range_scale(0.0f),
 	_gyro_range_rad_s(0.0f),
 	_gyro_topic(-1),
+	_orb_id(nullptr),
 	_class_instance(-1),
 	_current_rate(0),
 	_orientation(SENSOR_BOARD_ROTATION_DEFAULT),
@@ -399,21 +407,32 @@ L3GD20::init()
 
 	_class_instance = register_class_devname(GYRO_DEVICE_PATH);
 
-	reset();
+	switch (_class_instance) {
+		case CLASS_DEVICE_PRIMARY:
+			_orb_id = ORB_ID(sensor_gyro0);
+			break;
 
-	measure();
+		case CLASS_DEVICE_SECONDARY:
+			_orb_id = ORB_ID(sensor_gyro1);
+			break;
 
-	if (_class_instance == CLASS_DEVICE_PRIMARY) {
+		case CLASS_DEVICE_TERTIARY:
+			_orb_id = ORB_ID(sensor_gyro2);
+			break;
+	}
 
-		/* advertise sensor topic, measure manually to initialize valid report */
-		struct gyro_report grp;
-		_reports->get(&grp);
+	reset();
 
-		_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp);
+	measure();
 
-		if (_gyro_topic < 0)
-			debug("failed to create sensor_gyro publication");
+	/* advertise sensor topic, measure manually to initialize valid report */
+	struct gyro_report grp;
+	_reports->get(&grp);
 
+	_gyro_topic = orb_advertise(_orb_id, &grp);
+
+	if (_gyro_topic < 0) {
+		debug("failed to create sensor_gyro publication");
 	}
 
 	ret = OK;
@@ -931,9 +950,9 @@ L3GD20::measure()
 	poll_notify(POLLIN);
 
 	/* publish for subscribers */
-	if (_gyro_topic > 0 && !(_pub_blocked)) {
+	if (!(_pub_blocked)) {
 		/* publish it */
-		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
+		orb_publish(_orb_id, _gyro_topic, &report);
 	}
 
 	_read++;
@@ -990,6 +1009,9 @@ void	info();
 
 /**
  * Start the driver.
+ *
+ * This function call only returns once the driver
+ * started or failed to detect the sensor.
  */
 void
 start(bool external_bus, enum Rotation rotation)
diff --git a/src/drivers/l3gd20/module.mk b/src/drivers/l3gd20/module.mk
index 23e77e871..5630e7aec 100644
--- a/src/drivers/l3gd20/module.mk
+++ b/src/drivers/l3gd20/module.mk
@@ -4,3 +4,7 @@
 
 MODULE_COMMAND	 = l3gd20
 SRCS		 = l3gd20.cpp
+
+MODULE_STACKSIZE	= 1200
+
+EXTRACXXFLAGS	= -Weffc++
diff --git a/src/drivers/led/module.mk b/src/drivers/led/module.mk
index 777f3e442..5b7b4491b 100644
--- a/src/drivers/led/module.mk
+++ b/src/drivers/led/module.mk
@@ -36,3 +36,5 @@
 #
 
 SRCS		= led.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 45e775055..6880cf0f8 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -284,6 +284,7 @@ private:
 	unsigned		_mag_samplerate;
 
 	orb_advert_t		_accel_topic;
+	orb_id_t		_accel_orb_id;
 	int			_accel_class_instance;
 
 	unsigned		_accel_read;
@@ -461,6 +462,10 @@ private:
 	 * @return		OK if the value can be supported.
 	 */
 	int			mag_set_samplerate(unsigned frequency);
+
+	/* this class cannot be copied */
+	LSM303D(const LSM303D&);
+	LSM303D operator=(const LSM303D&);
 };
 
 /**
@@ -485,29 +490,39 @@ private:
 	LSM303D				*_parent;
 
 	orb_advert_t			_mag_topic;
+	orb_id_t			_mag_orb_id;
 	int				_mag_class_instance;
 
 	void				measure();
 
 	void				measure_trampoline(void *arg);
+
+	/* this class does not allow copying due to ptr data members */
+	LSM303D_mag(const LSM303D_mag&);
+	LSM303D_mag operator=(const LSM303D_mag&);
 };
 
 
 LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rotation) :
 	SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within safety margins for LSM303D */),
 	_mag(new LSM303D_mag(this)),
+	_accel_call{},
+	_mag_call{},
 	_call_accel_interval(0),
 	_call_mag_interval(0),
 	_accel_reports(nullptr),
 	_mag_reports(nullptr),
+	_accel_scale{},
 	_accel_range_m_s2(0.0f),
 	_accel_range_scale(0.0f),
 	_accel_samplerate(0),
 	_accel_onchip_filter_bandwith(0),
+	_mag_scale{},
 	_mag_range_ga(0.0f),
 	_mag_range_scale(0.0f),
 	_mag_samplerate(0),
 	_accel_topic(-1),
+	_accel_orb_id(nullptr),
 	_accel_class_instance(-1),
 	_accel_read(0),
 	_mag_read(0),
@@ -524,6 +539,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota
 	_reg7_expected(0),
 	_accel_log_fd(-1),
 	_accel_logging_enabled(false),
+	_last_extreme_us(0),
 	_last_log_us(0),
 	_last_log_sync_us(0),
 	_last_log_reg_us(0),
@@ -611,34 +627,56 @@ LSM303D::init()
 	/* fill report structures */
 	measure();
 
-	if (_mag->_mag_class_instance == CLASS_DEVICE_PRIMARY) {
+	/* advertise sensor topic, measure manually to initialize valid report */
+	struct mag_report mrp;
+	_mag_reports->get(&mrp);
 
-		/* advertise sensor topic, measure manually to initialize valid report */
-		struct mag_report mrp;
-		_mag_reports->get(&mrp);
+	/* measurement will have generated a report, publish */
+	switch (_mag->_mag_class_instance) {
+		case CLASS_DEVICE_PRIMARY:
+			_mag->_mag_orb_id = ORB_ID(sensor_mag0);
+			break;
 
-		/* measurement will have generated a report, publish */
-		_mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag), &mrp);
+		case CLASS_DEVICE_SECONDARY:
+			_mag->_mag_orb_id = ORB_ID(sensor_mag1);
+			break;
+
+		case CLASS_DEVICE_TERTIARY:
+			_mag->_mag_orb_id = ORB_ID(sensor_mag2);
+			break;
+	}
 
-		if (_mag->_mag_topic < 0)
-			debug("failed to create sensor_mag publication");
+	_mag->_mag_topic = orb_advertise(_mag->_mag_orb_id, &mrp);
 
+	if (_mag->_mag_topic < 0) {
+		warnx("ADVERT ERR");
 	}
 
 	_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
 
-	if (_accel_class_instance == CLASS_DEVICE_PRIMARY) {
+	/* advertise sensor topic, measure manually to initialize valid report */
+	struct accel_report arp;
+	_accel_reports->get(&arp);
 
-		/* advertise sensor topic, measure manually to initialize valid report */
-		struct accel_report arp;
-		_accel_reports->get(&arp);
+	/* measurement will have generated a report, publish */
+	switch (_accel_class_instance) {
+		case CLASS_DEVICE_PRIMARY:
+			_accel_orb_id = ORB_ID(sensor_accel0);
+			break;
+
+		case CLASS_DEVICE_SECONDARY:
+			_accel_orb_id = ORB_ID(sensor_accel1);
+			break;
 
-		/* measurement will have generated a report, publish */
-		_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp);
+		case CLASS_DEVICE_TERTIARY:
+			_accel_orb_id = ORB_ID(sensor_accel2);
+			break;
+	}
 
-		if (_accel_topic < 0)
-			debug("failed to create sensor_accel publication");
+	_accel_topic = orb_advertise(_accel_orb_id, &arp);
 
+	if (_accel_topic < 0) {
+		warnx("ADVERT ERR");
 	}
 
 out:
@@ -1557,9 +1595,9 @@ LSM303D::measure()
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 
-	if (_accel_topic > 0 && !(_pub_blocked)) {
+	if (!(_pub_blocked)) {
 		/* publish it */
-		orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
+		orb_publish(_accel_orb_id, _accel_topic, &accel_report);
 	}
 
 	_accel_read++;
@@ -1634,9 +1672,9 @@ LSM303D::mag_measure()
 	/* notify anyone waiting for data */
 	poll_notify(POLLIN);
 
-	if (_mag->_mag_topic > 0 && !(_pub_blocked)) {
+	if (!(_pub_blocked)) {
 		/* publish it */
-		orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
+		orb_publish(_mag->_mag_orb_id, _mag->_mag_topic, &mag_report);
 	}
 
 	_mag_read++;
@@ -1730,6 +1768,7 @@ LSM303D_mag::LSM303D_mag(LSM303D *parent) :
 	CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG),
 	_parent(parent),
 	_mag_topic(-1),
+	_mag_orb_id(nullptr),
 	_mag_class_instance(-1)
 {
 }
@@ -1803,6 +1842,9 @@ void	usage();
 
 /**
  * Start the driver.
+ *
+ * This function call only returns once the driver is
+ * up and running or failed to detect the sensor.
  */
 void
 start(bool external_bus, enum Rotation rotation)
diff --git a/src/drivers/lsm303d/module.mk b/src/drivers/lsm303d/module.mk
index e40f718c5..b4f3974f4 100644
--- a/src/drivers/lsm303d/module.mk
+++ b/src/drivers/lsm303d/module.mk
@@ -5,4 +5,6 @@
 MODULE_COMMAND	 = lsm303d
 SRCS		 = lsm303d.cpp
 
+MODULE_STACKSIZE	= 1200
 
+EXTRACXXFLAGS	= -Weffc++
diff --git a/src/drivers/md25/module.mk b/src/drivers/md25/module.mk
index 13821a6b5..3f9cf2d89 100644
--- a/src/drivers/md25/module.mk
+++ b/src/drivers/md25/module.mk
@@ -40,3 +40,5 @@ MODULE_COMMAND		= md25
 
 SRCS			= md25.cpp \
 			  md25_main.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index 07611f903..9cfa7f383 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -92,7 +92,7 @@
 #include <drivers/airspeed/airspeed.h>
 
 /* I2C bus address is 1010001x */
-#define I2C_ADDRESS_MS4525DO	0x28	//0x51 /* 7-bit address. */
+#define I2C_ADDRESS_MS4525DO	0x28	/**< 7-bit address. Depends on the order code (this is for code "I") */
 #define PATH_MS4525		"/dev/ms4525"
 /* The MS5525DSO address is 111011Cx, where C is the complementary value of the pin CSB */
 #define I2C_ADDRESS_MS5525DSO	0x77	//0x77/* 7-bit address, addr. pin pulled low */
@@ -102,9 +102,9 @@
 #define ADDR_READ_MR			0x00	/* write to this address to start conversion */
 
 /* Measurement rate is 100Hz */
-#define MEAS_RATE 100.0f
-#define MEAS_DRIVER_FILTER_FREQ 3.0f
-#define CONVERSION_INTERVAL	(1000000 / 100)	/* microseconds */
+#define MEAS_RATE 100
+#define MEAS_DRIVER_FILTER_FREQ 1.5f
+#define CONVERSION_INTERVAL	(1000000 / MEAS_RATE)	/* microseconds */
 
 class MEASAirspeed : public Airspeed
 {
@@ -140,9 +140,9 @@ extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]);
 MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address,
 	CONVERSION_INTERVAL, path),
 	_filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ),
-	_t_system_power(-1)
+	_t_system_power(-1),
+	system_power{}
 {
-	memset(&system_power, 0, sizeof(system_power));
 }
 
 int
@@ -420,6 +420,9 @@ void	info();
 
 /**
  * Start the driver.
+ *
+ * This function call only returns once the driver is up and running
+ * or failed to detect the sensor.
  */
 void
 start(int i2c_bus)
diff --git a/src/drivers/meas_airspeed/module.mk b/src/drivers/meas_airspeed/module.mk
index fed4078b6..6f5909978 100644
--- a/src/drivers/meas_airspeed/module.mk
+++ b/src/drivers/meas_airspeed/module.mk
@@ -36,6 +36,11 @@
 #
 
 MODULE_COMMAND		= meas_airspeed
-MODULE_STACKSIZE	= 2048
 
 SRCS			= meas_airspeed.cpp
+
+MODULE_STACKSIZE	= 1200
+
+EXTRACXXFLAGS	= -Weffc++
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/mpu6000/module.mk b/src/drivers/mpu6000/module.mk
index c7d9cd3ef..5b4893b12 100644
--- a/src/drivers/mpu6000/module.mk
+++ b/src/drivers/mpu6000/module.mk
@@ -37,7 +37,8 @@
 
 MODULE_COMMAND	= mpu6000
 
-# XXX seems excessive, check if 2048 is not sufficient
-MODULE_STACKSIZE	 = 4096
-
 SRCS		= mpu6000.cpp
+
+MODULE_STACKSIZE	= 1200
+
+EXTRACXXFLAGS	= -Weffc++
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index 1b3a96a0d..6f5dae7ad 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -215,6 +215,7 @@ private:
 	float			_accel_range_scale;
 	float			_accel_range_m_s2;
 	orb_advert_t		_accel_topic;
+	orb_id_t		_accel_orb_id;
 	int			_accel_class_instance;
 
 	RingBuffer		*_gyro_reports;
@@ -343,6 +344,9 @@ private:
 	*/
 	void _set_sample_rate(uint16_t desired_sample_rate_hz);
 
+	/* do not allow to copy this class due to pointer data members */
+	MPU6000(const MPU6000&);
+	MPU6000 operator=(const MPU6000&);
 };
 
 /**
@@ -367,8 +371,12 @@ protected:
 private:
 	MPU6000			*_parent;
 	orb_advert_t		_gyro_topic;
+	orb_id_t		_gyro_orb_id;
 	int			_gyro_class_instance;
 
+	/* do not allow to copy this class due to pointer data members */
+	MPU6000_gyro(const MPU6000_gyro&);
+	MPU6000_gyro operator=(const MPU6000_gyro&);
 };
 
 /** driver 'main' command */
@@ -378,13 +386,17 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
 	SPI("MPU6000", path_accel, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED),
 	_gyro(new MPU6000_gyro(this, path_gyro)),
 	_product(0),
+	_call{},
 	_call_interval(0),
 	_accel_reports(nullptr),
+	_accel_scale{},
 	_accel_range_scale(0.0f),
 	_accel_range_m_s2(0.0f),
 	_accel_topic(-1),
+	_accel_orb_id(nullptr),
 	_accel_class_instance(-1),
 	_gyro_reports(nullptr),
+	_gyro_scale{},
 	_gyro_range_scale(0.0f),
 	_gyro_range_rad_s(0.0f),
 	_sample_rate(1000),
@@ -498,33 +510,58 @@ MPU6000::init()
 
 	measure();
 
-	if (_accel_class_instance == CLASS_DEVICE_PRIMARY) {
+	/* advertise sensor topic, measure manually to initialize valid report */
+	struct accel_report arp;
+	_accel_reports->get(&arp);
 
-		/* advertise sensor topic, measure manually to initialize valid report */
-		struct accel_report arp;
-		_accel_reports->get(&arp);
+	/* measurement will have generated a report, publish */
+	switch (_accel_class_instance) {
+		case CLASS_DEVICE_PRIMARY:
+			_accel_orb_id = ORB_ID(sensor_accel0);
+			break;
+		
+		case CLASS_DEVICE_SECONDARY:
+			_accel_orb_id = ORB_ID(sensor_accel1);
+			break;
 
-		/* measurement will have generated a report, publish */
-		_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp);
+		case CLASS_DEVICE_TERTIARY:
+			_accel_orb_id = ORB_ID(sensor_accel2);
+			break;
 
-		if (_accel_topic < 0)
-			debug("failed to create sensor_accel publication");
+	}
+
+	_accel_topic = orb_advertise(_accel_orb_id, &arp);
 
+	if (_accel_topic < 0) {
+		warnx("ADVERT FAIL");
 	}
 
-	if (_gyro->_gyro_class_instance == CLASS_DEVICE_PRIMARY) {
 
-		/* advertise sensor topic, measure manually to initialize valid report */
-		struct gyro_report grp;
-		_gyro_reports->get(&grp);
+	/* advertise sensor topic, measure manually to initialize valid report */
+	struct gyro_report grp;
+	_gyro_reports->get(&grp);
+
+	switch (_gyro->_gyro_class_instance) {
+		case CLASS_DEVICE_PRIMARY:
+			_gyro->_gyro_orb_id = ORB_ID(sensor_gyro0);
+			break;
 
-		_gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp);
+		case CLASS_DEVICE_SECONDARY:
+			_gyro->_gyro_orb_id = ORB_ID(sensor_gyro1);
+			break;
 
-		if (_gyro->_gyro_topic < 0)
-			debug("failed to create sensor_gyro publication");
+		case CLASS_DEVICE_TERTIARY:
+			_gyro->_gyro_orb_id = ORB_ID(sensor_gyro2);
+			break;
 
 	}
 
+	_gyro->_gyro_topic = orb_advertise(_gyro->_gyro_orb_id, &grp);
+
+	if (_gyro->_gyro_topic < 0) {
+		warnx("ADVERT FAIL");
+	}
+
 out:
 	return ret;
 }
@@ -1345,14 +1382,14 @@ MPU6000::measure()
 	poll_notify(POLLIN);
 	_gyro->parent_poll_notify();
 
-	if (_accel_topic > 0 && !(_pub_blocked)) {
+	if (!(_pub_blocked)) {
 		/* publish it */
-		orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
+		orb_publish(_accel_orb_id, _accel_topic, &arb);
 	}
 
-	if (_gyro->_gyro_topic > 0 && !(_pub_blocked)) {
+	if (!(_pub_blocked)) {
 		/* publish it */
-		orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
+		orb_publish(_gyro->_gyro_orb_id, _gyro->_gyro_topic, &grb);
 	}
 
 	/* stop measuring */
@@ -1373,6 +1410,7 @@ MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) :
 	CDev("MPU6000_gyro", path),
 	_parent(parent),
 	_gyro_topic(-1),
+	_gyro_orb_id(nullptr),
 	_gyro_class_instance(-1)
 {
 }
@@ -1437,6 +1475,9 @@ void	usage();
 
 /**
  * Start the driver.
+ *
+ * This function only returns if the driver is up and running
+ * or failed to detect the sensor.
  */
 void
 start(bool external_bus, enum Rotation rotation)
@@ -1507,7 +1548,7 @@ test(bool external_bus)
 	int fd = open(path_accel, O_RDONLY);
 
 	if (fd < 0)
-		err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)",
+		err(1, "%s open failed (try 'mpu6000 start')",
 		    path_accel);
 
 	/* get the driver */
diff --git a/src/drivers/ms5611/module.mk b/src/drivers/ms5611/module.mk
index 20f8aa173..ee74058fc 100644
--- a/src/drivers/ms5611/module.mk
+++ b/src/drivers/ms5611/module.mk
@@ -38,3 +38,5 @@
 MODULE_COMMAND	= ms5611
 
 SRCS		= ms5611.cpp ms5611_spi.cpp ms5611_i2c.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp
index fe669b5f5..873fa62c4 100644
--- a/src/drivers/ms5611/ms5611.cpp
+++ b/src/drivers/ms5611/ms5611.cpp
@@ -300,12 +300,17 @@ MS5611::init()
 
 		ret = OK;
 
-		if (_class_instance == CLASS_DEVICE_PRIMARY) {
-
-			_baro_topic = orb_advertise(ORB_ID(sensor_baro), &brp);
+		switch (_class_instance) {
+			case CLASS_DEVICE_PRIMARY:
+				_baro_topic = orb_advertise(ORB_ID(sensor_baro0), &brp);
+				break;
+			case CLASS_DEVICE_SECONDARY:
+				_baro_topic = orb_advertise(ORB_ID(sensor_baro1), &brp);
+				break;
+		}
 
-			if (_baro_topic < 0)
-				debug("failed to create sensor_baro publication");
+		if (_baro_topic < 0) {
+			warnx("failed to create sensor_baro publication");
 		}
 
 	} while (0);
@@ -722,9 +727,17 @@ MS5611::collect()
 		report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
 
 		/* publish it */
-		if (_baro_topic > 0 && !(_pub_blocked)) {
+		if (!(_pub_blocked)) {
 			/* publish it */
-			orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
+			switch (_class_instance) {
+				case CLASS_DEVICE_PRIMARY:
+					orb_publish(ORB_ID(sensor_baro0), _baro_topic, &report);
+					break;
+
+				case CLASS_DEVICE_SECONDARY:
+					orb_publish(ORB_ID(sensor_baro1), _baro_topic, &report);
+					break;
+			}
 		}
 
 		if (_reports->force(&report)) {
diff --git a/src/drivers/pca8574/module.mk b/src/drivers/pca8574/module.mk
index 825ee9bb7..c53ed9ab2 100644
--- a/src/drivers/pca8574/module.mk
+++ b/src/drivers/pca8574/module.mk
@@ -4,3 +4,5 @@
 
 MODULE_COMMAND	 = pca8574
 SRCS		 = pca8574.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/px4flow/module.mk b/src/drivers/px4flow/module.mk
index d3062e457..460bec7b9 100644
--- a/src/drivers/px4flow/module.mk
+++ b/src/drivers/px4flow/module.mk
@@ -38,3 +38,5 @@
 MODULE_COMMAND	= px4flow
 
 SRCS		= px4flow.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 8cc1141aa..82977a032 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -179,6 +179,9 @@ private:
 	uint32_t	gpio_read(void);
 	int		gpio_ioctl(file *filp, int cmd, unsigned long arg);
 
+	/* do not allow to copy due to ptr data members */
+	PX4FMU(const PX4FMU&);
+	PX4FMU operator=(const PX4FMU&);
 };
 
 const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = {
@@ -242,6 +245,7 @@ PX4FMU::PX4FMU() :
 	_task(-1),
 	_armed_sub(-1),
 	_outputs_pub(-1),
+	_armed{},
 	_num_outputs(0),
 	_primary_pwm_device(false),
 	_task_should_exit(false),
@@ -252,6 +256,7 @@ PX4FMU::PX4FMU() :
 	_groups_subscribed(0),
 	_control_subs{-1},
 	_poll_fds_num(0),
+	_pwm_limit{},
 	_failsafe_pwm{0},
 	_disarmed_pwm{0},
 	_num_failsafe_set(0),
diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk
index eeb59e1a1..a60f1a434 100644
--- a/src/drivers/px4fmu/module.mk
+++ b/src/drivers/px4fmu/module.mk
@@ -6,3 +6,5 @@ MODULE_COMMAND	 = fmu
 SRCS		 = fmu.cpp
 
 MODULE_STACKSIZE = 1200
+
+EXTRACXXFLAGS	= -Weffc++
diff --git a/src/drivers/px4io/module.mk b/src/drivers/px4io/module.mk
index c14f1f783..5b838fb75 100644
--- a/src/drivers/px4io/module.mk
+++ b/src/drivers/px4io/module.mk
@@ -46,3 +46,5 @@ SRCS			= px4io.cpp \
 INCLUDE_DIRS    += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
 
 MODULE_STACKSIZE = 1200
+
+EXTRACXXFLAGS	= -Weffc++
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 7d78b0d27..d93009c47 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -453,6 +453,9 @@ private:
 	 */
 	void			io_handle_vservo(uint16_t vservo, uint16_t vrssi);
 
+	/* do not allow to copy this class due to ptr data members */
+	PX4IO(const PX4IO&);
+	PX4IO operator=(const PX4IO&);
 };
 
 namespace
@@ -496,6 +499,8 @@ PX4IO::PX4IO(device::Device *interface) :
 	_to_battery(0),
 	_to_servorail(0),
 	_to_safety(0),
+	_outputs{},
+	_servorail_status{},
 	_primary_pwm_device(false),
 	_lockdown_override(false),
 	_battery_amp_per_volt(90.0f / 5.0f), // this matches the 3DR current sensor
@@ -1144,6 +1149,12 @@ PX4IO::io_set_arming_state()
 		clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
 	}
 
+	if (armed.force_failsafe) {
+		set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
+	} else {
+		clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
+	}
+
 	if (armed.ready_to_arm) {
 		set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
 
@@ -1997,7 +2008,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\n",
+	printf("arming 0x%04x%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"),
@@ -2005,7 +2016,9 @@ PX4IO::print_status(bool extended_status)
 	       ((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)		? " FAILSAFE_CUSTOM" : ""),
 	       ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)	? " INAIR_RESTART_OK" : ""),
 	       ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)	? " ALWAYS_PWM_ENABLE" : ""),
-	       ((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN)		? " LOCKDOWN" : ""));
+	       ((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN)		? " LOCKDOWN" : ""),
+	       ((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE)		? " FORCE_FAILSAFE" : "")
+	       );
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
 	       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
@@ -2217,6 +2230,17 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC);
 		break;
 
+	case PWM_SERVO_SET_FORCE_FAILSAFE:
+		/* force failsafe mode instantly */
+		if (arg == 0) {
+			/* clear force failsafe flag */
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE, 0);
+		} else {
+			/* set force failsafe flag */
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE);
+		}
+		break;
+
 	case DSM_BIND_START:
 
 		/* only allow DSM2, DSM-X and DSM-X with more than 7 channels */
@@ -2419,7 +2443,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 		break;
 
 	case PX4IO_CHECK_CRC: {
-		/* check IO firmware CRC against passed value */		
+		/* check IO firmware CRC against passed value */
 		uint32_t io_crc = 0;
 		ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC, (uint16_t *)&io_crc, 2);
 		if (ret != OK)
@@ -2679,7 +2703,7 @@ checkcrc(int argc, char *argv[])
 	int fd = open(argv[1], O_RDONLY);
 	if (fd == -1) {
 		printf("open of %s failed - %d\n", argv[1], errno);
-		exit(1);			
+		exit(1);
 	}
 	const uint32_t app_size_max = 0xf000;
 	uint32_t fw_crc = 0;
@@ -2694,7 +2718,7 @@ checkcrc(int argc, char *argv[])
 	close(fd);
 	while (nbytes < app_size_max) {
 		uint8_t b = 0xff;
-		fw_crc = crc32part(&b, 1, fw_crc);			
+		fw_crc = crc32part(&b, 1, fw_crc);
 		nbytes++;
 	}
 
@@ -2707,7 +2731,7 @@ checkcrc(int argc, char *argv[])
 
 	if (ret != OK) {
 		printf("check CRC failed - %d\n", ret);
-		exit(1);			
+		exit(1);
 	}
 	printf("CRCs match\n");
 	exit(0);
@@ -2737,12 +2761,12 @@ bind(int argc, char *argv[])
 		pulses = DSMX_BIND_PULSES;
 	else if (!strcmp(argv[2], "dsmx8"))
 		pulses = DSMX8_BIND_PULSES;
-	else 
+	else
 		errx(1, "unknown parameter %s, use dsm2, dsmx or dsmx8", argv[2]);
 	// Test for custom pulse parameter
 	if (argc > 3)
 		pulses = atoi(argv[3]);
-	if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) 
+	if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
 		errx(1, "system must not be armed");
 
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
@@ -2944,7 +2968,7 @@ lockdown(int argc, char *argv[])
 				(void)g_dev->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 0);
 				warnx("ACTUATORS ARE NOW SAFE IN HIL.");
 			}
-			
+
 		} else {
 			errx(1, "driver not loaded, exiting");
 		}
diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp
index c39494fb0..d227e15d5 100644
--- a/src/drivers/px4io/px4io_serial.cpp
+++ b/src/drivers/px4io/px4io_serial.cpp
@@ -157,6 +157,10 @@ private:
 	perf_counter_t		_pc_idle;
 	perf_counter_t		_pc_badidle;
 
+	/* do not allow top copying this class */
+	PX4IO_serial(PX4IO_serial &);
+	PX4IO_serial& operator = (const PX4IO_serial &);
+
 };
 
 IOPacket PX4IO_serial::_dma_buffer;
@@ -173,7 +177,9 @@ PX4IO_serial::PX4IO_serial() :
 	_tx_dma(nullptr),
 	_rx_dma(nullptr),
 	_rx_dma_status(_dma_status_inactive),
-	_pc_txns(perf_alloc(PC_ELAPSED,		"io_txns     ")),
+	_bus_semaphore(SEM_INITIALIZER(0)),
+	_completion_semaphore(SEM_INITIALIZER(0)),
+	_pc_txns(perf_alloc(PC_ELAPSED, "io_txns     ")),
 	_pc_dmasetup(perf_alloc(PC_ELAPSED,	"io_dmasetup ")),
 	_pc_retries(perf_alloc(PC_COUNT,	"io_retries  ")),
 	_pc_timeouts(perf_alloc(PC_COUNT,	"io_timeouts ")),
diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp
index d134c0246..fb16f891f 100644
--- a/src/drivers/px4io/px4io_uploader.cpp
+++ b/src/drivers/px4io/px4io_uploader.cpp
@@ -65,7 +65,8 @@
 
 PX4IO_Uploader::PX4IO_Uploader() :
 	_io_fd(-1),
-	_fw_fd(-1)
+	_fw_fd(-1),
+	bl_rev(0)
 {
 }
 
@@ -204,12 +205,8 @@ PX4IO_Uploader::upload(const char *filenames[])
 
 		if (bl_rev <= 2) {
 			ret = verify_rev2(fw_size);
-		} else if(bl_rev == 3) {
-			ret = verify_rev3(fw_size);
 		} else {
-			/* verify rev 4 and higher still uses the same approach and
-			 * every version *needs* to be verified.
-			 */
+			/* verify rev 3 and higher. Every version *needs* to be verified. */
 			ret = verify_rev3(fw_size);
 		}
 
@@ -249,7 +246,7 @@ PX4IO_Uploader::upload(const char *filenames[])
 }
 
 int
-PX4IO_Uploader::recv(uint8_t &c, unsigned timeout)
+PX4IO_Uploader::recv_byte_with_timeout(uint8_t *c, unsigned timeout)
 {
 	struct pollfd fds[1];
 
@@ -266,24 +263,24 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout)
 		return -ETIMEDOUT;
 	}
 
-	read(_io_fd, &c, 1);
+	read(_io_fd, c, 1);
 #ifdef UDEBUG
-	log("recv 0x%02x", c);
+	log("recv_bytes 0x%02x", c);
 #endif
 	return OK;
 }
 
 int
-PX4IO_Uploader::recv(uint8_t *p, unsigned count)
+PX4IO_Uploader::recv_bytes(uint8_t *p, unsigned count)
 {
+	int ret = OK;
 	while (count--) {
-		int ret = recv(*p++, 5000);
+		ret = recv_byte_with_timeout(p++, 5000);
 
 		if (ret != OK)
-			return ret;
+			break;
 	}
-
-	return OK;
+	return ret;
 }
 
 void
@@ -293,10 +290,10 @@ PX4IO_Uploader::drain()
 	int ret;
 
 	do {
-		// the small recv timeout here is to allow for fast
+		// the small recv_bytes timeout here is to allow for fast
 		// drain when rebooting the io board for a forced
 		// update of the fw without using the safety switch
-		ret = recv(c, 40);
+		ret = recv_byte_with_timeout(&c, 40);
 
 #ifdef UDEBUG
 		if (ret == OK) {
@@ -314,21 +311,19 @@ PX4IO_Uploader::send(uint8_t c)
 #endif
 	if (write(_io_fd, &c, 1) != 1)
 		return -errno;
-
 	return OK;
 }
 
 int
 PX4IO_Uploader::send(uint8_t *p, unsigned count)
 {
+	int ret;
 	while (count--) {
-		int ret = send(*p++);
-
+		ret = send(*p++);
 		if (ret != OK)
-			return ret;
+			break;
 	}
-
-	return OK;
+	return ret;
 }
 
 int
@@ -337,12 +332,12 @@ PX4IO_Uploader::get_sync(unsigned timeout)
 	uint8_t c[2];
 	int ret;
 
-	ret = recv(c[0], timeout);
+	ret = recv_byte_with_timeout(c, timeout);
 
 	if (ret != OK)
 		return ret;
 
-	ret = recv(c[1], timeout);
+	ret = recv_byte_with_timeout(c + 1, timeout);
 
 	if (ret != OK)
 		return ret;
@@ -378,7 +373,7 @@ PX4IO_Uploader::get_info(int param, uint32_t &val)
 	send(param);
 	send(PROTO_EOC);
 
-	ret = recv((uint8_t *)&val, sizeof(val));
+	ret = recv_bytes((uint8_t *)&val, sizeof(val));
 
 	if (ret != OK)
 		return ret;
@@ -419,12 +414,15 @@ PX4IO_Uploader::program(size_t fw_size)
 	int ret;
 	size_t sent = 0;
 
-	file_buf = (uint8_t *)malloc(PROG_MULTI_MAX);
+	file_buf = new uint8_t[PROG_MULTI_MAX];
 	if (!file_buf) {
 		log("Can't allocate program buffer");
 		return -ENOMEM;
 	}
 
+	ASSERT((fw_size & 3) == 0);
+	ASSERT((PROG_MULTI_MAX & 3) == 0);
+
 	log("programming %u bytes...", (unsigned)fw_size);
 
 	ret = lseek(_fw_fd, 0, SEEK_SET);
@@ -443,34 +441,26 @@ PX4IO_Uploader::program(size_t fw_size)
 			    (unsigned)sent,
 			    (int)count,
 			    (int)errno);
-		}
-
-		if (count == 0) {
-			free(file_buf);
-			return OK;
+			ret = -errno;
+			break;
 		}
 
 		sent += count;
 
-		if (count < 0)
-			return -errno;
-
-		ASSERT((count % 4) == 0);
-
 		send(PROTO_PROG_MULTI);
 		send(count);
-		send(&file_buf[0], count);
+		send(file_buf, count);
 		send(PROTO_EOC);
 
 		ret = get_sync(1000);
 
 		if (ret != OK) {
-			free(file_buf);
-			return ret;
+			break;
 		}
 	}
-	free(file_buf);
-	return OK;
+
+	delete [] file_buf;
+	return ret;
 }
 
 int
@@ -524,7 +514,7 @@ PX4IO_Uploader::verify_rev2(size_t fw_size)
 		for (ssize_t i = 0; i < count; i++) {
 			uint8_t c;
 
-			ret = recv(c, 5000);
+			ret = recv_byte_with_timeout(&c, 5000);
 
 			if (ret != OK) {
 				log("%d: got %d waiting for bytes", sent + i, ret);
@@ -611,7 +601,7 @@ PX4IO_Uploader::verify_rev3(size_t fw_size_local)
 	send(PROTO_GET_CRC);
 	send(PROTO_EOC);
 
-	ret = recv((uint8_t*)(&crc), sizeof(crc));
+	ret = recv_bytes((uint8_t*)(&crc), sizeof(crc));
 
 	if (ret != OK) {
 		log("did not receive CRC checksum");
diff --git a/src/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h
index 55f63eef9..e17523413 100644
--- a/src/drivers/px4io/uploader.h
+++ b/src/drivers/px4io/uploader.h
@@ -74,20 +74,19 @@ private:
 		INFO_BOARD_REV		= 3,		/**< board revision */
 		INFO_FLASH_SIZE		= 4,		/**< max firmware size in bytes */
 
-		PROG_MULTI_MAX		= 60,		/**< protocol max is 255, must be multiple of 4 */
-		READ_MULTI_MAX		= 60,		/**< protocol max is 255, something overflows with >= 64 */
+		PROG_MULTI_MAX		= 248,		/**< protocol max is 255, must be multiple of 4 */
 
 	};
 
 	int			_io_fd;
 	int			_fw_fd;
 
-	uint32_t	bl_rev; /**< bootloader revision */
+	uint32_t		bl_rev; /**< bootloader revision */
 
 	void			log(const char *fmt, ...);
 
-	int			recv(uint8_t &c, unsigned timeout);
-	int			recv(uint8_t *p, unsigned count);
+	int			recv_byte_with_timeout(uint8_t *c, unsigned timeout);
+	int			recv_bytes(uint8_t *p, unsigned count);
 	void			drain();
 	int			send(uint8_t c);
 	int			send(uint8_t *p, unsigned count);
diff --git a/src/drivers/rgbled/module.mk b/src/drivers/rgbled/module.mk
index 39b53ec9e..c287e35f3 100644
--- a/src/drivers/rgbled/module.mk
+++ b/src/drivers/rgbled/module.mk
@@ -4,3 +4,5 @@
 
 MODULE_COMMAND	 = rgbled
 SRCS		 = rgbled.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/roboclaw/module.mk b/src/drivers/roboclaw/module.mk
index 1abecf198..c5e55bdc3 100644
--- a/src/drivers/roboclaw/module.mk
+++ b/src/drivers/roboclaw/module.mk
@@ -39,3 +39,5 @@ MODULE_COMMAND		= roboclaw
 
 SRCS			= roboclaw_main.cpp \
 			  RoboClaw.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
index 810f4aacc..03c7bd399 100644
--- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp
+++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
@@ -336,6 +336,7 @@ ToneAlarm::ToneAlarm() :
 	_default_tunes[TONE_GPS_WARNING_TUNE] = "MFT255L4AAAL1F#";					//gps warning slow
 	_default_tunes[TONE_ARMING_FAILURE_TUNE] = "MFT255L4<<<BAP";
 	_default_tunes[TONE_PARACHUTE_RELEASE_TUNE] = "MFT255L16agagagag";			// parachute release
+	_default_tunes[TONE_EKF_WARNING_TUNE] = "MFT255L8ddd#d#eeff";				// ekf warning
 
 	_tune_names[TONE_STARTUP_TUNE] = "startup";			// startup tune
 	_tune_names[TONE_ERROR_TUNE] = "error";				// ERROR tone
@@ -348,6 +349,7 @@ ToneAlarm::ToneAlarm() :
 	_tune_names[TONE_GPS_WARNING_TUNE] = "gps_warning";	            // gps warning
 	_tune_names[TONE_ARMING_FAILURE_TUNE] = "arming_failure";            //fail to arm
 	_tune_names[TONE_PARACHUTE_RELEASE_TUNE] = "parachute_release";	// parachute release
+	_tune_names[TONE_EKF_WARNING_TUNE] = "ekf_warning";				// ekf warning
 }
 
 ToneAlarm::~ToneAlarm()
diff --git a/src/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c
new file mode 100644
index 000000000..c66bebeec
--- /dev/null
+++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c
@@ -0,0 +1,247 @@
+/****************************************************************************
+ *
+ *   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 matlab_csv_serial_main.c
+ *
+ * Matlab CSV / ASCII format interface at 921600 baud, 8 data bits,
+ * 1 stop bit, no parity
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <float.h>
+#include <nuttx/sched.h>
+#include <sys/prctl.h>
+#include <drivers/drv_hrt.h>
+#include <termios.h>
+#include <errno.h>
+#include <limits.h>
+#include <math.h>
+#include <uORB/uORB.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_gyro.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/err.h>
+#include <poll.h>
+
+__EXPORT int matlab_csv_serial_main(int argc, char *argv[]);
+static bool thread_should_exit = false;		/**< Daemon exit flag */
+static bool thread_running = false;		/**< Daemon status flag */
+static int daemon_task;				/**< Handle of daemon task / thread */
+
+int matlab_csv_serial_thread_main(int argc, char *argv[]);
+static void usage(const char *reason);
+
+static void usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+	fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
+	exit(1);
+}
+
+/**
+ * The daemon app only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ *
+ * The actual stack size should be set in the call
+ * to task_spawn_cmd().
+ */
+int matlab_csv_serial_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start"))
+	{
+		if (thread_running)
+		{
+			warnx("already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		daemon_task = task_spawn_cmd("matlab_csv_serial",
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 5,
+					 2000,
+					 matlab_csv_serial_thread_main,
+					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop"))
+	{
+		thread_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status"))
+	{
+		if (thread_running) {
+			warnx("running");
+		} else {
+			warnx("stopped");
+		}
+
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+int matlab_csv_serial_thread_main(int argc, char *argv[])
+{
+
+	if (argc < 2) {
+		errx(1, "need a serial port name as argument");
+	}
+
+	const char* uart_name = argv[1];
+
+	warnx("opening port %s", uart_name);
+
+	int serial_fd = open(uart_name, O_RDWR | O_NOCTTY);
+
+	unsigned speed = 921600;
+
+	if (serial_fd < 0) {
+		err(1, "failed to open port: %s", uart_name);
+	}
+
+	/* Try to set baud rate */
+	struct termios uart_config;
+	int termios_state;
+
+	/* Back up the original uart configuration to restore it after exit */
+	if ((termios_state = tcgetattr(serial_fd, &uart_config)) < 0) {
+		warnx("ERR GET CONF %s: %d\n", uart_name, termios_state);
+		close(serial_fd);
+		return -1;
+	}
+
+	/* Clear ONLCR flag (which appends a CR for every LF) */
+	uart_config.c_oflag &= ~ONLCR;
+
+	/* USB serial is indicated by /dev/ttyACM0*/
+	if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/ttyACM1") != OK) {
+
+		/* Set baud rate */
+		if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
+			warnx("ERR SET BAUD %s: %d\n", uart_name, termios_state);
+			close(serial_fd);
+			return -1;
+		}
+
+	}
+
+	if ((termios_state = tcsetattr(serial_fd, TCSANOW, &uart_config)) < 0) {
+		warnx("ERR SET CONF %s\n", uart_name);
+		close(serial_fd);
+		return -1;
+	}
+
+	/* subscribe to vehicle status, attitude, sensors and flow*/
+	struct accel_report accel0;
+	struct accel_report accel1;
+	struct gyro_report gyro0;
+	struct gyro_report gyro1;
+
+	/* subscribe to parameter changes */
+	int accel0_sub = orb_subscribe(ORB_ID(sensor_accel0));
+	int accel1_sub = orb_subscribe(ORB_ID(sensor_accel1));
+	int gyro0_sub = orb_subscribe(ORB_ID(sensor_gyro0));
+	int gyro1_sub = orb_subscribe(ORB_ID(sensor_gyro1));
+
+	thread_running = true;
+
+	while (!thread_should_exit)
+	{
+
+		/*This runs at the rate of the sensors */
+		struct pollfd fds[] = {
+				{ .fd = accel0_sub, .events = POLLIN }
+		};
+
+		/* wait for a sensor update, check for exit condition every 500 ms */
+		int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), 500);
+
+		if (ret < 0)
+		{
+			/* poll error, ignore */
+
+		}
+		else if (ret == 0)
+		{
+			/* no return value, ignore */
+			warnx("no sensor data");
+		}
+		else
+		{
+
+			/* accel0 update available? */
+			if (fds[0].revents & POLLIN)
+			{
+				orb_copy(ORB_ID(sensor_accel0), accel0_sub, &accel0);
+				orb_copy(ORB_ID(sensor_accel1), accel1_sub, &accel1);
+				orb_copy(ORB_ID(sensor_gyro0), gyro0_sub, &gyro0);
+				orb_copy(ORB_ID(sensor_gyro1), gyro1_sub, &gyro1);
+
+				// write out on accel 0, but collect for all other sensors as they have updates
+				dprintf(serial_fd, "%llu,%d,%d,%d,%d,%d,%d\n", accel0.timestamp, (int)accel0.x_raw, (int)accel0.y_raw, (int)accel0.z_raw,
+					(int)accel1.x_raw, (int)accel1.y_raw, (int)accel1.z_raw);
+			}
+
+		}
+	}
+
+	warnx("exiting");
+	thread_running = false;
+
+	fflush(stdout);
+	return 0;
+}
+
+
diff --git a/src/examples/matlab_csv_serial/module.mk b/src/examples/matlab_csv_serial/module.mk
new file mode 100644
index 000000000..1629c2ce4
--- /dev/null
+++ b/src/examples/matlab_csv_serial/module.mk
@@ -0,0 +1,40 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Build position estimator
+#
+
+MODULE_COMMAND		= matlab_csv_serial
+
+SRCS		= matlab_csv_serial.c
diff --git a/src/lib/ecl/module.mk b/src/lib/ecl/module.mk
index f2aa3db6a..93a5b511f 100644
--- a/src/lib/ecl/module.mk
+++ b/src/lib/ecl/module.mk
@@ -39,3 +39,5 @@ SRCS		 = 	attitude_fw/ecl_pitch_controller.cpp \
 			attitude_fw/ecl_roll_controller.cpp \
 			attitude_fw/ecl_yaw_controller.cpp \
 			l1/ecl_l1_pos_controller.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/lib/external_lgpl/module.mk b/src/lib/external_lgpl/module.mk
index 53f1629e3..29d3514f6 100644
--- a/src/lib/external_lgpl/module.mk
+++ b/src/lib/external_lgpl/module.mk
@@ -46,3 +46,5 @@
 #
 
 SRCS		 = 	tecs/tecs.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp
index ea0cf4ca1..ca931e2da 100644
--- a/src/lib/mathlib/math/Matrix.hpp
+++ b/src/lib/mathlib/math/Matrix.hpp
@@ -69,27 +69,34 @@ public:
 
 	/**
 	 * trivial ctor
-	 * note that this ctor will not initialize elements
+	 * Initializes the elements to zero.
 	 */
-	MatrixBase() {
-		arm_mat = {M, N, &data[0][0]};
+	MatrixBase() : 
+		data{},
+		arm_mat{M, N, &data[0][0]}
+	{
 	}
 
+	virtual ~MatrixBase() {};
+
 	/**
 	 * copyt ctor
 	 */
-	MatrixBase(const MatrixBase<M, N> &m) {
-		arm_mat = {M, N, &data[0][0]};
+	MatrixBase(const MatrixBase<M, N> &m) :
+		arm_mat{M, N, &data[0][0]}
+	{
 		memcpy(data, m.data, sizeof(data));
 	}
 
-	MatrixBase(const float *d) {
-		arm_mat = {M, N, &data[0][0]};
+	MatrixBase(const float *d) :
+		arm_mat{M, N, &data[0][0]}
+	{
 		memcpy(data, d, sizeof(data));
 	}
 
-	MatrixBase(const float d[M][N]) {
-		arm_mat = {M, N, &data[0][0]};
+	MatrixBase(const float d[M][N]) : 
+		arm_mat{M, N, &data[0][0]}
+	{
 		memcpy(data, d, sizeof(data));
 	}
 
diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp
index c7323c215..0ddf77615 100644
--- a/src/lib/mathlib/math/Vector.hpp
+++ b/src/lib/mathlib/math/Vector.hpp
@@ -69,25 +69,32 @@ public:
 
 	/**
 	 * trivial ctor
-	 * note that this ctor will not initialize elements
+	 * initializes elements to zero
 	 */
-	VectorBase() {
-		arm_col = {N, 1, &data[0]};
+	VectorBase() :
+		data{},
+		arm_col{N, 1, &data[0]}
+	{
+
 	}
 
+	virtual ~VectorBase() {};
+
 	/**
 	 * copy ctor
 	 */
-	VectorBase(const VectorBase<N> &v) {
-		arm_col = {N, 1, &data[0]};
+	VectorBase(const VectorBase<N> &v) :
+		arm_col{N, 1, &data[0]}
+	{
 		memcpy(data, v.data, sizeof(data));
 	}
 
 	/**
 	 * setting ctor
 	 */
-	VectorBase(const float d[N]) {
-		arm_col = {N, 1, &data[0]};
+	VectorBase(const float d[N]) :
+		arm_col{N, 1, &data[0]}
+	{
 		memcpy(data, d, sizeof(data));
 	}
 
diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp
index 74cd5d78c..436065175 100644
--- a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp
+++ b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp
@@ -46,10 +46,18 @@ class __EXPORT LowPassFilter2p
 {
 public:
     // constructor
-    LowPassFilter2p(float sample_freq, float cutoff_freq) {
+    LowPassFilter2p(float sample_freq, float cutoff_freq) : 
+        _cutoff_freq(cutoff_freq),
+        _a1(0.0f),
+        _a2(0.0f),
+        _b0(0.0f),
+        _b1(0.0f),
+        _b2(0.0f),
+        _delay_element_1(0.0f),
+        _delay_element_2(0.0f)
+    {
         // set initial parameters
         set_cutoff_frequency(sample_freq, cutoff_freq);
-        _delay_element_1 = _delay_element_2 = 0;
     }
 
     /**
diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp
index 598cfe9e2..0e58c68b6 100644
--- a/src/modules/commander/airspeed_calibration.cpp
+++ b/src/modules/commander/airspeed_calibration.cpp
@@ -186,7 +186,7 @@ int do_airspeed_calibration(int mavlink_fd)
 	usleep(500 * 1000);
 
 	calibration_counter = 0;
-	const int maxcount = 3000;
+	const unsigned maxcount = 3000;
 
 	/* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */
 	while (calibration_counter < maxcount) {
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 0c4d48dea..2b528eef6 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -124,7 +124,7 @@ extern struct system_load_s system_load;
 #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
 
-#define POSITION_TIMEOUT		(600 * 1000)		/**< consider the local or global position estimate invalid after 600ms */
+#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
@@ -163,7 +163,8 @@ static bool on_usb_power = false;
 
 static float takeoff_alt = 5.0f;
 static int parachute_enabled = 0;
-static float eph_epv_threshold = 5.0f;
+static float eph_threshold = 5.0f;
+static float epv_threshold = 10.0f;
 
 static struct vehicle_status_s status;
 static struct actuator_armed_s armed;
@@ -335,6 +336,7 @@ void print_status()
 {
 	warnx("type: %s", (status.is_rotary_wing) ? "ROTARY" : "PLANE");
 	warnx("usb powered: %s", (on_usb_power) ? "yes" : "no");
+	warnx("avionics rail: %6.2f V", (double)status.avionics_power_rail_voltage);
 
 	/* read all relevant states */
 	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
@@ -546,24 +548,19 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 		}
 		break;
 
-#if 0
 	/* Flight termination */
-	case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
-
-			//XXX: to enable the parachute, a param needs to be set
-			//xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
-			if (armed_local->armed && cmd->param3 > 0.5 && parachute_enabled) {
-				transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
-				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
-
+	case VEHICLE_CMD_DO_FLIGHTTERMINATION: {
+			if (cmd->param1 > 0.5f) {
+				//XXX update state machine?
+				armed_local->force_failsafe = true;
+				warnx("forcing failsafe");
 			} else {
-				/* reject parachute depoyment not armed */
-				cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				armed_local->force_failsafe = false;
+				warnx("disabling failsafe");
 			}
-
+			cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 		}
 		break;
-#endif
 
 	case VEHICLE_CMD_DO_SET_HOME: {
 			bool use_current = cmd->param1 > 0.5f;
@@ -741,6 +738,7 @@ int commander_thread_main(int argc, char *argv[])
 
 	// CIRCUIT BREAKERS
 	status.circuit_breaker_engaged_power_check = false;
+	status.circuit_breaker_engaged_airspd_check = false;
 
 	/* publish initial state */
 	status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
@@ -918,6 +916,11 @@ int commander_thread_main(int argc, char *argv[])
 	struct system_power_s system_power;
 	memset(&system_power, 0, sizeof(system_power));
 
+	/* Subscribe to actuator controls (outputs) */
+	int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
+	struct actuator_controls_s actuator_controls;
+	memset(&actuator_controls, 0, sizeof(actuator_controls));
+
 	control_status_leds(&status, &armed, true);
 
 	/* now initialized */
@@ -977,6 +980,7 @@ int commander_thread_main(int argc, char *argv[])
 				param_get(_param_component_id, &(status.component_id));
 
 				status.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY);
+				status.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY);
 
 				status_changed = true;
 
@@ -1106,32 +1110,32 @@ int commander_thread_main(int argc, char *argv[])
 
 		/* update condition_global_position_valid */
 		/* hysteresis for EPH/EPV */
-		bool eph_epv_good;
+		bool eph_good;
 
 		if (status.condition_global_position_valid) {
-			if (global_position.eph > eph_epv_threshold * 2.0f || global_position.epv > eph_epv_threshold * 2.0f) {
-				eph_epv_good = false;
+			if (global_position.eph > eph_threshold * 2.5f) {
+				eph_good = false;
 
 			} else {
-				eph_epv_good = true;
+				eph_good = true;
 			}
 
 		} else {
-			if (global_position.eph < eph_epv_threshold && global_position.epv < eph_epv_threshold) {
-				eph_epv_good = true;
+			if (global_position.eph < eph_threshold) {
+				eph_good = true;
 
 			} else {
-				eph_epv_good = false;
+				eph_good = false;
 			}
 		}
 
-		check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_epv_good, &(status.condition_global_position_valid), &status_changed);
+		check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), &status_changed);
 
 		/* check if GPS fix is ok */
 
 		/* update home position */
 		if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed &&
-		    (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
+		    (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) {
 
 			home.lat = global_position.lat;
 			home.lon = global_position.lon;
@@ -1161,8 +1165,8 @@ int commander_thread_main(int argc, char *argv[])
 		/* hysteresis for EPH */
 		bool local_eph_good;
 
-		if (status.condition_global_position_valid) {
-			if (local_position.eph > eph_epv_threshold * 2.0f) {
+		if (status.condition_local_position_valid) {
+			if (local_position.eph > eph_threshold * 2.5f) {
 				local_eph_good = false;
 
 			} else {
@@ -1170,7 +1174,7 @@ int commander_thread_main(int argc, char *argv[])
 			}
 
 		} else {
-			if (local_position.eph < eph_epv_threshold) {
+			if (local_position.eph < eph_threshold) {
 				local_eph_good = true;
 
 			} else {
@@ -1199,13 +1203,17 @@ int commander_thread_main(int argc, char *argv[])
 
 		if (updated) {
 			orb_copy(ORB_ID(battery_status), battery_sub, &battery);
+			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
 
 			/* only consider battery voltage if system has been running 2s and battery voltage is valid */
 			if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
 				status.battery_voltage = battery.voltage_filtered_v;
 				status.battery_current = battery.current_a;
 				status.condition_battery_voltage_valid = true;
-				status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah);
+
+				/* get throttle (if armed), as we only care about energy negative throttle also counts */
+				float throttle = (armed.armed) ? fabsf(actuator_controls.control[3]) : 0.0f;
+				status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah, throttle);
 			}
 		}
 
@@ -1267,13 +1275,13 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 		/* if battery voltage is getting lower, warn using buzzer, etc. */
-		if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) {
+		if (status.condition_battery_voltage_valid && status.battery_remaining < 0.18f && !low_battery_voltage_actions_done) {
 			low_battery_voltage_actions_done = true;
 			mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED");
 			status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
 			status_changed = true;
 
-		} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
+		} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.09f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
 			/* critical battery voltage, this is rather an emergency, change state machine */
 			critical_battery_voltage_actions_done = true;
 			mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY");
@@ -1412,8 +1420,12 @@ int commander_thread_main(int argc, char *argv[])
 				arming_state_changed = true;
 
 			} else if (arming_ret == TRANSITION_DENIED) {
-				/* DENIED here indicates bug in the commander */
-				mavlink_log_critical(mavlink_fd, "arming state transition denied");
+				/*
+				 * the arming transition can be denied to a number of reasons:
+				 *  - pre-flight check failed (sensors not ok or not calibrated)
+				 *  - safety not disabled
+				 *  - system not in manual mode
+				 */
 				tune_negative(true);
 			}
 
@@ -1441,7 +1453,7 @@ int commander_thread_main(int argc, char *argv[])
 		/* data links check */
 		bool have_link = false;
 		for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
-			if (hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) {
+			if (telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) {
 				/* handle the case where data link was regained */
 				if (telemetry_lost[i]) {
 					mavlink_log_critical(mavlink_fd, "data link %i regained", i);
@@ -1494,7 +1506,7 @@ int commander_thread_main(int argc, char *argv[])
 
 			/* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */
 			if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid &&
-			    (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
+			    (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) {
 
 				// TODO remove code duplication
 				home.lat = global_position.lat;
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
index d5fe122cb..2022e99fb 100644
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@@ -281,15 +281,17 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern)
 	}
 }
 
-float battery_remaining_estimate_voltage(float voltage, float discharged)
+float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized)
 {
 	float ret = 0;
 	static param_t bat_v_empty_h;
 	static param_t bat_v_full_h;
 	static param_t bat_n_cells_h;
 	static param_t bat_capacity_h;
-	static float bat_v_empty = 3.2f;
-	static float bat_v_full = 4.0f;
+	static param_t bat_v_load_drop_h;
+	static float bat_v_empty = 3.4f;
+	static float bat_v_full = 4.2f;
+	static float bat_v_load_drop = 0.06f;
 	static int bat_n_cells = 3;
 	static float bat_capacity = -1.0f;
 	static bool initialized = false;
@@ -297,23 +299,26 @@ float battery_remaining_estimate_voltage(float voltage, float discharged)
 
 	if (!initialized) {
 		bat_v_empty_h = param_find("BAT_V_EMPTY");
-		bat_v_full_h = param_find("BAT_V_FULL");
+		bat_v_full_h = param_find("BAT_V_CHARGED");
 		bat_n_cells_h = param_find("BAT_N_CELLS");
 		bat_capacity_h = param_find("BAT_CAPACITY");
+		bat_v_load_drop_h = param_find("BAT_V_LOAD_DROP");
 		initialized = true;
 	}
 
 	if (counter % 100 == 0) {
 		param_get(bat_v_empty_h, &bat_v_empty);
 		param_get(bat_v_full_h, &bat_v_full);
+		param_get(bat_v_load_drop_h, &bat_v_load_drop);
 		param_get(bat_n_cells_h, &bat_n_cells);
 		param_get(bat_capacity_h, &bat_capacity);
 	}
 
 	counter++;
 
-	/* remaining charge estimate based on voltage */
-	float remaining_voltage = (voltage - bat_n_cells * bat_v_empty) / (bat_n_cells * (bat_v_full - bat_v_empty));
+	/* remaining charge estimate based on voltage and internal resistance (drop under load) */
+	float bat_v_full_dynamic = bat_v_full - (bat_v_load_drop * throttle_normalized);
+	float remaining_voltage = (voltage - (bat_n_cells * bat_v_empty)) / (bat_n_cells * (bat_v_full_dynamic - bat_v_empty));
 
 	if (bat_capacity > 0.0f) {
 		/* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */
diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h
index a49c9e263..4a77fe487 100644
--- a/src/modules/commander/commander_helper.h
+++ b/src/modules/commander/commander_helper.h
@@ -80,8 +80,9 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern);
  *
  * @param voltage the current battery voltage
  * @param discharged the discharged capacity
+ * @param throttle_normalized the normalized throttle magnitude from 0 to 1. Negative throttle should be converted to this range as well, as it consumes energy.
  * @return the estimated remaining capacity in 0..1
  */
-float battery_remaining_estimate_voltage(float voltage, float discharged);
+float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized);
 
 #endif /* COMMANDER_HELPER_H_ */
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index 4750f9d5c..dba68700b 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -65,7 +65,17 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f);
  *
  * @group Battery Calibration
  */
-PARAM_DEFINE_FLOAT(BAT_V_FULL, 3.9f);
+PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f);
+
+/**
+ * Voltage drop per cell on 100% load
+ *
+ * This implicitely defines the internal resistance
+ * to maximum current ratio and assumes linearity.
+ *
+ * @group Battery Calibration
+ */
+PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f);
 
 /**
  * Number of cells.
diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
index cbc2844c1..d89c67c2b 100644
--- a/src/modules/commander/gyro_calibration.cpp
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -92,7 +92,7 @@ int do_gyro_calibration(int mavlink_fd)
 		unsigned poll_errcount = 0;
 
 		/* subscribe to gyro sensor topic */
-		int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro));
+		int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro0));
 		struct gyro_report gyro_report;
 
 		while (calibration_counter < calibration_count) {
@@ -104,7 +104,7 @@ int do_gyro_calibration(int mavlink_fd)
 			int poll_ret = poll(fds, 1, 1000);
 
 			if (poll_ret > 0) {
-				orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report);
+				orb_copy(ORB_ID(sensor_gyro0), sub_sensor_gyro, &gyro_report);
 				gyro_scale.x_offset += gyro_report.x;
 				gyro_scale.y_offset += gyro_report.y;
 				gyro_scale.z_offset += gyro_report.z;
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index 0ead22f77..23900f386 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -145,7 +145,7 @@ int do_mag_calibration(int mavlink_fd)
 	}
 
 	if (res == OK) {
-		int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
+		int sub_mag = orb_subscribe(ORB_ID(sensor_mag0));
 		struct mag_report mag;
 
 		/* limit update rate to get equally spaced measurements over time (in ms) */
@@ -170,7 +170,7 @@ int do_mag_calibration(int mavlink_fd)
 			int poll_ret = poll(fds, 1, 1000);
 
 			if (poll_ret > 0) {
-				orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
+				orb_copy(ORB_ID(sensor_mag0), sub_mag, &mag);
 
 				x[calibration_counter] = mag.x;
 				y[calibration_counter] = mag.y;
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 372ba9d7d..3c3d2f233 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -110,8 +110,8 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 	ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1);
 
 	transition_result_t ret = TRANSITION_DENIED;
-
 	arming_state_t current_arming_state = status->arming_state;
+	bool feedback_provided = false;
 
 	/* only check transition if the new state is actually different from the current one */
 	if (new_arming_state == current_arming_state) {
@@ -156,13 +156,15 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 
 					// Fail transition if pre-arm check fails
 					if (prearm_ret) {
+						/* the prearm check already prints the reject reason */
+						feedback_provided = true;
 						valid_transition = false;
 
 					// Fail transition if we need safety switch press
 					} else if (safety->safety_switch_available && !safety->safety_off) {
 
-						mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch!");
-
+						mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!");
+						feedback_provided = true;
 						valid_transition = false;
 					}
 
@@ -173,6 +175,7 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 						if (!status->condition_power_input_valid) {
 
 							mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module.");
+							feedback_provided = true;
 							valid_transition = false;
 						}
 
@@ -182,6 +185,7 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 							(status->avionics_power_rail_voltage < 4.9f))) {
 
 							mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage);
+							feedback_provided = true;
 							valid_transition = false;
 						}
 					}
@@ -200,6 +204,8 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 
 		/* Sensors need to be initialized for STANDBY state */
 		if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
+			mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational.");
+			feedback_provided = true;
 			valid_transition = false;
 		}
 
@@ -216,11 +222,14 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 	}
 
 	if (ret == TRANSITION_DENIED) {
-		static const char *errMsg = "INVAL: %s - %s";
+		const char * str = "INVAL: %s - %s";
+		/* only print to console here by default as this is too technical to be useful during operation */
+		warnx(str, state_names[status->arming_state], state_names[new_arming_state]);
 
-		mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]);
-
-		warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]);
+		/* print to MAVLink if we didn't provide any feedback yet */
+		if (!feedback_provided) {
+			mavlink_log_critical(mavlink_fd, str, state_names[status->arming_state], state_names[new_arming_state]);
+		}
 	}
 
 	return ret;
@@ -648,8 +657,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
 		float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
 
 		if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
-			mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE");
-			mavlink_log_critical(mavlink_fd, "hold still while arming");
+			mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE, hold still");
 			/* this is frickin' fatal */
 			failed = true;
 			goto system_eval;
@@ -661,7 +669,9 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
 		goto system_eval;
 	}
 
-	if (!status->is_rotary_wing) {
+	/* Perform airspeed check only if circuit breaker is not
+	 * engaged and it's not a rotary wing */
+	if (!status->circuit_breaker_engaged_airspd_check && !status->is_rotary_wing) {
 		/* accel done, close it */
 		close(fd);
 		fd = orb_subscribe(ORB_ID(airspeed));
diff --git a/src/modules/controllib/block/Block.hpp b/src/modules/controllib/block/Block.hpp
index 736698e21..9bd80b15b 100644
--- a/src/modules/controllib/block/Block.hpp
+++ b/src/modules/controllib/block/Block.hpp
@@ -93,6 +93,11 @@ protected:
 	List<uORB::SubscriptionBase *> _subscriptions;
 	List<uORB::PublicationBase *> _publications;
 	List<BlockParamBase *> _params;
+
+private:
+	/* this class has pointer data members and should not be copied (private constructor) */
+	Block(const control::Block&);
+	Block operator=(const control::Block&);
 };
 
 class __EXPORT SuperBlock :
diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp
index c6c374300..f739446fa 100644
--- a/src/modules/controllib/blocks.cpp
+++ b/src/modules/controllib/blocks.cpp
@@ -121,6 +121,9 @@ int blockLimitSymTest()
 
 float BlockLowPass::update(float input)
 {
+	if (!isfinite(getState())) {
+		setState(input);
+	}
 	float b = 2 * float(M_PI) * getFCut() * getDt();
 	float a = b / (1 + b);
 	setState(a * input + (1 - a)*getState());
@@ -293,7 +296,18 @@ int blockIntegralTrapTest()
 
 float BlockDerivative::update(float input)
 {
-	float output = _lowPass.update((input - getU()) / getDt());
+	float output;
+	if (_initialized) {
+		output = _lowPass.update((input - getU()) / getDt());
+	} else {
+		// if this is the first call to update
+		// we have no valid derivative
+		// and so we use the assumption the
+		// input value is not changing much,
+		// which is the best we can do here.
+		output = 0.0f;
+		_initialized = true;
+	}
 	setU(input);
 	return output;
 }
diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp
index 66e929038..bffc355a8 100644
--- a/src/modules/controllib/blocks.hpp
+++ b/src/modules/controllib/blocks.hpp
@@ -114,7 +114,7 @@ public:
 // methods
 	BlockLowPass(SuperBlock *parent, const char *name) :
 		Block(parent, name),
-		_state(0),
+		_state(0.0f/0.0f /* initialize to invalid val, force into is_finite() check on first call */),
 		_fCut(this, "") // only one parameter, no need to name
 	{};
 	virtual ~BlockLowPass() {};
@@ -238,9 +238,25 @@ public:
 	BlockDerivative(SuperBlock *parent, const char *name) :
 		SuperBlock(parent, name),
 		_u(0),
+		_initialized(false),
 		_lowPass(this, "LP")
 	{};
 	virtual ~BlockDerivative() {};
+
+	/**
+	 * Update the state and get current derivative
+	 *
+	 * This call updates the state and gets the current
+	 * derivative. As the derivative is only valid
+	 * on the second call to update, it will return
+	 * no change (0) on the first. To get a closer
+	 * estimate of the derivative on the first call,
+	 * call setU() one time step before using the
+	 * return value of update().
+	 *
+	 * @param input the variable to calculate the derivative of
+	 * @return the current derivative
+	 */
 	float update(float input);
 // accessors
 	void setU(float u) {_u = u;}
@@ -249,6 +265,7 @@ public:
 protected:
 // attributes
 	float _u; /**< previous input */
+	bool _initialized;
 	BlockLowPass _lowPass; /**< low pass filter */
 };
 
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 40cb6043f..91d17e787 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
@@ -112,6 +112,10 @@ public:
 	 */
 	FixedwingEstimator();
 
+	/* we do not want people ever copying this class */
+	FixedwingEstimator(const FixedwingEstimator& that) = delete;
+	FixedwingEstimator operator=(const FixedwingEstimator&) = delete;
+
 	/**
 	 * Destructor, also kills the sensors task.
 	 */
@@ -371,9 +375,10 @@ FixedwingEstimator::FixedwingEstimator() :
 	_mag_offsets({}),
 
 	#ifdef SENSOR_COMBINED_SUB
-	_sensor_combined({}),
+	_sensor_combined{},
 	#endif
 
+	_pos_ref{},
 	_baro_ref(0.0f),
 	_baro_ref_offset(0.0f),
 	_baro_gps_offset(0.0f),
@@ -390,12 +395,18 @@ FixedwingEstimator::FixedwingEstimator() :
 /* states */
 	_baro_init(false),
 	_gps_initialized(false),
+	_gps_start_time(0),
+	_filter_start_time(0),
+	_last_sensor_timestamp(0),
+	_last_run(0),
 	_gyro_valid(false),
 	_accel_valid(false),
 	_mag_valid(false),
 	_ekf_logging(true),
 	_debug(0),
 	_mavlink_fd(-1),
+	_parameters{},
+	_parameter_handles{},
 	_ekf(nullptr),
 	_velocity_xy_filtered(0.0f),
 	_velocity_z_filtered(0.0f),
@@ -619,10 +630,10 @@ FixedwingEstimator::check_filter_state()
 		rep.health_flags |= (((uint8_t)ekf_report.posHealth)	<< 1);
 		rep.health_flags |= (((uint8_t)ekf_report.hgtHealth)	<< 2);
 		rep.health_flags |= (((uint8_t)!ekf_report.gyroOffsetsExcessive)	<< 3);
-		// rep.health_flags |= (((uint8_t)ekf_report.onGround)	<< 4);
-		// rep.health_flags |= (((uint8_t)ekf_report.staticMode)	<< 5);
-		// rep.health_flags |= (((uint8_t)ekf_report.useCompass)	<< 6);
-		// rep.health_flags |= (((uint8_t)ekf_report.useAirspeed)	<< 7);
+		rep.health_flags |= (((uint8_t)ekf_report.onGround)	<< 4);
+		rep.health_flags |= (((uint8_t)ekf_report.staticMode)	<< 5);
+		rep.health_flags |= (((uint8_t)ekf_report.useCompass)	<< 6);
+		rep.health_flags |= (((uint8_t)ekf_report.useAirspeed)	<< 7);
 
 		rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout)	<< 0);
 		rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout)	<< 1);
@@ -693,7 +704,7 @@ FixedwingEstimator::task_main()
 	/*
 	 * do subscriptions
 	 */
-	_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
+	_baro_sub = orb_subscribe(ORB_ID(sensor_baro0));
 	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 	_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
 	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
@@ -1041,7 +1052,7 @@ FixedwingEstimator::task_main()
 			orb_check(_baro_sub, &baro_updated);
 
 			if (baro_updated) {
-				orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
+				orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro);
 
 				_ekf->baroHgt = _baro.altitude;
 
@@ -1133,7 +1144,7 @@ FixedwingEstimator::task_main()
 					initVelNED[2] = _gps.vel_d_m_s;
 
 					// Set up height correctly
-					orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
+					orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro);
 					_baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame
 					_baro_gps_offset = _baro.altitude - gps_alt;
 					_ekf->baroHgt = _baro.altitude;
@@ -1458,25 +1469,6 @@ FixedwingEstimator::task_main()
 							_wind.timestamp = _global_pos.timestamp;
 							_wind.windspeed_north = _ekf->states[14];
 							_wind.windspeed_east = _ekf->states[15];
-							_wind.covariance_north = 0.0f; // XXX get form filter
-							_wind.covariance_east = 0.0f;
-
-							/* lazily publish the wind estimate only once available */
-							if (_wind_pub > 0) {
-								/* publish the wind estimate */
-								orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind);
-
-							} else {
-								/* advertise and publish */
-								_wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind);
-							}
-
-						}
-
-						if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
-							_wind.timestamp = _global_pos.timestamp;
-							_wind.windspeed_north = _ekf->states[14];
-							_wind.windspeed_east = _ekf->states[15];
 							_wind.covariance_north = _ekf->P[14][14];
 							_wind.covariance_east = _ekf->P[15][15];
 
diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
index 768e0be35..ffdd29a5b 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
@@ -2773,7 +2773,7 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error)
         ResetHeight();
         ResetStoredStates();
 
-        ret = 3;
+        ret = 0;
     }
 
     // Reset the filter if gyro offsets are excessive
@@ -3028,6 +3028,10 @@ void AttPosEKF::GetFilterState(struct ekf_status_report *err)
         current_ekf_state.states[i] = states[i];
     }
     current_ekf_state.n_states = n_states;
+    current_ekf_state.onGround = onGround;
+    current_ekf_state.staticMode = staticMode;
+    current_ekf_state.useCompass = useCompass;
+    current_ekf_state.useAirspeed = useAirspeed;
 
     memcpy(err, &current_ekf_state, sizeof(*err));
 
diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp
index 29a8c8d1e..77cc1eeeb 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp
@@ -45,8 +45,11 @@ void Vector3f::zero(void)
     z = 0.0f;
 }
 
-Mat3f::Mat3f() {
-    identity();
+Mat3f::Mat3f() :
+    x{1.0f, 0.0f, 0.0f},
+    y{0.0f, 1.0f, 0.0f},
+    z{0.0f, 0.0f, 1.0f}
+{
 }
 
 void Mat3f::identity() {
diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h
index 6d1f47b68..a6b670c4d 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_utilities.h
+++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h
@@ -68,6 +68,10 @@ struct ekf_status_report {
     bool posTimeout;
     bool hgtTimeout;
     bool imuTimeout;
+    bool onGround;
+    bool staticMode;
+    bool useCompass;
+    bool useAirspeed;
     uint32_t velFailTime;
     uint32_t posFailTime;
     uint32_t hgtFailTime;
diff --git a/src/modules/ekf_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk
index dc5220bf0..36d854ddd 100644
--- a/src/modules/ekf_att_pos_estimator/module.mk
+++ b/src/modules/ekf_att_pos_estimator/module.mk
@@ -41,3 +41,5 @@ SRCS		= ekf_att_pos_estimator_main.cpp \
 		  ekf_att_pos_estimator_params.c \
 		  estimator_23states.cpp \
 		  estimator_utilities.cpp
+
+EXTRACXXFLAGS	= -Weffc++
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index 4cdba735a..0cea13cc4 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -529,7 +529,7 @@ FixedwingAttitudeControl::vehicle_accel_poll()
 	orb_check(_accel_sub, &accel_updated);
 
 	if (accel_updated) {
-		orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
+		orb_copy(ORB_ID(sensor_accel0), _accel_sub, &_accel);
 	}
 }
 
@@ -577,7 +577,7 @@ FixedwingAttitudeControl::task_main()
 	 */
 	_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
+	_accel_sub = orb_subscribe(ORB_ID(sensor_accel0));
 	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 	_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 08c996ebc..eadb63f40 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -42,8 +42,8 @@
  *    Proceedings of the AIAA Guidance, Navigation and Control
  *    Conference, Aug 2004. AIAA-2004-4900.
  *
- * Implementation for total energy control class:
- *    Thomas Gubler
+ * Original implementation for total energy control class:
+ *    Paul Riseborough and Andrew Tridgell, 2013 (code in lib/external_lgpl)
  *
  * More details and acknowledgements in the referenced library headers.
  *
@@ -87,10 +87,13 @@
 #include <mavlink/mavlink_log.h>
 #include <launchdetection/LaunchDetector.h>
 #include <ecl/l1/ecl_l1_pos_controller.h>
+#include <external_lgpl/tecs/tecs.h>
 #include <drivers/drv_range_finder.h>
 #include "landingslope.h"
 #include "mtecs/mTecs.h"
 
+static int	_control_task = -1;			/**< task handle for sensor task */
+
 
 /**
  * L1 control app start / stop handling function
@@ -115,9 +118,9 @@ public:
 	/**
 	 * Start the sensors task.
 	 *
-	 * @return		OK on success.
+	 * @return	OK on success.
 	 */
-	int		start();
+	static int	start();
 
 	/**
 	 * Task status
@@ -131,12 +134,10 @@ private:
 
 	bool		_task_should_exit;		/**< if true, sensor task should exit */
 	bool		_task_running;			/**< if true, task is running in its mainloop */
-	int		_control_task;			/**< task handle for sensor task */
 
 	int		_global_pos_sub;
 	int		_pos_sp_triplet_sub;
 	int		_att_sub;			/**< vehicle attitude subscription */
-	int		_attitude_sub;			/**< raw rc channels data subscription */
 	int		_airspeed_sub;			/**< airspeed subscription */
 	int		_control_mode_sub;		/**< vehicle status subscription */
 	int 		_params_sub;			/**< notification of parameter updates */
@@ -160,18 +161,6 @@ private:
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
-	/** manual control states */
-	float		_altctrl_hold_heading;		/**< heading the system should hold in altctrl mode */
-	double		_loiter_hold_lat;
-	double		_loiter_hold_lon;
-	float		_loiter_hold_alt;
-	bool		_loiter_hold;
-
-	double		_launch_lat;
-	double		_launch_lon;
-	float		_launch_alt;
-	bool		_launch_valid;
-
 	/* land states */
 	/* not in non-abort mode for landing yet */
 	bool land_noreturn_horizontal;
@@ -188,8 +177,8 @@ private:
 
 	/* Landingslope object */
 	Landingslope landingslope;
-
 	float flare_curve_alt_rel_last;
+
 	/* heading hold */
 	float target_bearing;
 
@@ -205,6 +194,7 @@ private:
 	math::Matrix<3, 3> _R_nb;			///< current attitude
 
 	ECL_L1_Pos_Controller				_l1_control;
+	TECS						_tecs;
 	fwPosctrl::mTecs				_mTecs;
 	bool						_was_pos_control_mode;
 
@@ -216,6 +206,8 @@ private:
 		float min_sink_rate;
 		float max_sink_rate;
 		float max_climb_rate;
+		float heightrate_p;
+		float speedrate_p;
 		float throttle_damp;
 		float integrator_gain;
 		float vertical_accel_limit;
@@ -238,9 +230,6 @@ private:
 
 		float throttle_land_max;
 
-		float heightrate_p;
-		float speedrate_p;
-
 		float land_slope_angle;
 		float land_H1_virt;
 		float land_flare_alt_relative;
@@ -259,6 +248,8 @@ private:
 		param_t min_sink_rate;
 		param_t max_sink_rate;
 		param_t max_climb_rate;
+		param_t heightrate_p;
+		param_t speedrate_p;
 		param_t throttle_damp;
 		param_t integrator_gain;
 		param_t vertical_accel_limit;
@@ -281,9 +272,6 @@ private:
 
 		param_t throttle_land_max;
 
-		param_t heightrate_p;
-		param_t speedrate_p;
-
 		param_t land_slope_angle;
 		param_t land_H1_virt;
 		param_t land_flare_alt_relative;
@@ -406,7 +394,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_mavlink_fd(-1),
 	_task_should_exit(false),
 	_task_running(false),
-	_control_task(-1),
 
 /* subscriptions */
 	_global_pos_sub(-1),
@@ -416,12 +403,14 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_control_mode_sub(-1),
 	_params_sub(-1),
 	_manual_control_sub(-1),
+	_sensor_combined_sub(-1),
 	_range_finder_sub(-1),
 
 /* publications */
 	_attitude_sp_pub(-1),
 	_nav_capabilities_pub(-1),
 
+/* states */
 	_att(),
 	_att_sp(),
 	_nav_capabilities(),
@@ -436,8 +425,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
 
-/* states */
-	_loiter_hold(false),
 	land_noreturn_horizontal(false),
 	land_noreturn_vertical(false),
 	land_stayonground(false),
@@ -446,12 +433,16 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	launch_detected(false),
 	usePreTakeoffThrust(false),
 	last_manual(false),
+	landingslope(),
 	flare_curve_alt_rel_last(0.0f),
+	target_bearing(0.0f),
 	launchDetector(),
 	_airspeed_error(0.0f),
 	_airspeed_valid(false),
+	_airspeed_last_valid(0),
 	_groundspeed_undershoot(0.0f),
 	_global_pos_valid(false),
+	_l1_control(),
 	_mTecs(),
 	_was_pos_control_mode(false)
 {
@@ -572,6 +563,23 @@ FixedwingPositionControl::parameters_update()
 	_l1_control.set_l1_period(_parameters.l1_period);
 	_l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit));
 
+	_tecs.set_time_const(_parameters.time_const);
+	_tecs.set_min_sink_rate(_parameters.min_sink_rate);
+	_tecs.set_max_sink_rate(_parameters.max_sink_rate);
+	_tecs.set_throttle_damp(_parameters.throttle_damp);
+	_tecs.set_integrator_gain(_parameters.integrator_gain);
+	_tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit);
+	_tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega);
+	_tecs.set_speed_comp_filter_omega(_parameters.speed_comp_filter_omega);
+	_tecs.set_roll_throttle_compensation(_parameters.roll_throttle_compensation);
+	_tecs.set_speed_weight(_parameters.speed_weight);
+	_tecs.set_pitch_damping(_parameters.pitch_damping);
+	_tecs.set_indicated_airspeed_min(_parameters.airspeed_min);
+	_tecs.set_indicated_airspeed_max(_parameters.airspeed_max);
+	_tecs.set_max_climb_rate(_parameters.max_climb_rate);
+	_tecs.set_heightrate_p(_parameters.heightrate_p);
+	_tecs.set_speedrate_p(_parameters.speedrate_p);
+
 	/* sanity check parameters */
 	if (_parameters.airspeed_max < _parameters.airspeed_min ||
 	    _parameters.airspeed_max < 5.0f ||
@@ -609,17 +617,7 @@ FixedwingPositionControl::vehicle_control_mode_poll()
 	orb_check(_control_mode_sub, &vstatus_updated);
 
 	if (vstatus_updated) {
-
-		bool was_armed = _control_mode.flag_armed;
-
 		orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
-
-		if (!was_armed && _control_mode.flag_armed) {
-			_launch_lat = _global_pos.lat;
-			_launch_lon = _global_pos.lon;
-			_launch_alt = _global_pos.alt;
-			_launch_valid = true;
-		}
 	}
 }
 
@@ -643,6 +641,9 @@ FixedwingPositionControl::vehicle_airspeed_poll()
 		}
 	}
 
+	/* update TECS state */
+	_tecs.enable_airspeed(_airspeed_valid);
+
 	return airspeed_updated;
 }
 
@@ -703,7 +704,17 @@ FixedwingPositionControl::vehicle_setpoint_poll()
 void
 FixedwingPositionControl::task_main_trampoline(int argc, char *argv[])
 {
+	l1_control::g_control = new FixedwingPositionControl();
+
+	if (l1_control::g_control == nullptr) {
+		warnx("OUT OF MEM");
+		return;
+	}
+
+	/* only returns on exit */
 	l1_control::g_control->task_main();
+	delete l1_control::g_control;
+	l1_control::g_control = nullptr;
 }
 
 float
@@ -817,7 +828,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 	/* filter speed and altitude for controller */
 	math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
+	math::Vector<3> accel_earth = _R_nb * accel_body;
 
+	if (!_mTecs.getEnabled()) {
+		_tecs.update_50hz(_global_pos.alt /* XXX might switch to alt err here */, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
+	}
+
+	/* define altitude error */
 	float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt;
 
 	/* no throttle limit as default */
@@ -842,6 +859,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		/* get circle mode */
 		bool was_circle_mode = _l1_control.circle_mode();
 
+		/* restore speed weight, in case changed intermittently (e.g. in landing handling) */
+		_tecs.set_speed_weight(_parameters.speed_weight);
+
 		/* current waypoint (the one currently heading for) */
 		math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
 
@@ -949,7 +969,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			float airspeed_land = 1.3f * _parameters.airspeed_min;
 			float airspeed_approach = 1.3f * _parameters.airspeed_min;
 
-			/* Calculate altitude of last ordinary waypoint L */
+			/* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */
 			float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f;
 
 			float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
@@ -1115,15 +1135,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			}
 		}
 
-		// warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(),
-		//       (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing());
-		// warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp(0), (double)prev_wp(1),
-		//       (double)next_wp(0), (double)next_wp(1), (pos_sp_triplet.previous_valid) ? "valid" : "invalid");
-
-		// XXX at this point we always want no loiter hold if a
-		// mission is active
-		_loiter_hold = false;
-
 		/* reset landing state */
 		if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) {
 			reset_landing_state();
@@ -1139,89 +1150,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			_att_sp.roll_reset_integral = true;
 		}
 
-	} else if (0/* posctrl mode enabled */) {
-
-		_was_pos_control_mode = false;
-
-		/** POSCTRL FLIGHT **/
-
-	if (0/* switched from another mode to posctrl */) {
-		_altctrl_hold_heading = _att.yaw;
-	}
-
-	if (0/* posctrl on and manual control yaw non-zero */) {
-		_altctrl_hold_heading = _att.yaw + _manual.r;
-	}
-
-	//XXX not used
-
-	/* climb out control */
-//		bool climb_out = false;
-//
-//		/* user wants to climb out */
-//		if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
-//			climb_out = true;
-//		}
-
-	/* if in altctrl mode, set airspeed based on manual control */
-
-	// XXX check if ground speed undershoot should be applied here
-	float altctrl_airspeed = _parameters.airspeed_min +
-					(_parameters.airspeed_max - _parameters.airspeed_min) *
-					_manual.z;
-
-		_l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed_2d);
-		_att_sp.roll_body = _l1_control.nav_roll();
-		_att_sp.yaw_body = _l1_control.nav_bearing();
-
-		tecs_update_pitch_throttle(_global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, eas2tas,
-					math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
-					_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-					false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
-
-	} else if (0/* altctrl mode enabled */) {
-
-		_was_pos_control_mode = false;
-
-		/** ALTCTRL FLIGHT **/
-
-		if (0/* switched from another mode to altctrl */) {
-			_altctrl_hold_heading = _att.yaw;
-		}
-
-		if (0/* altctrl on and manual control yaw non-zero */) {
-			_altctrl_hold_heading = _att.yaw + _manual.r;
-		}
-
-		/* if in altctrl mode, set airspeed based on manual control */
-
-		// XXX check if ground speed undershoot should be applied here
-		float altctrl_airspeed = _parameters.airspeed_min +
-					  (_parameters.airspeed_max - _parameters.airspeed_min) *
-					  _manual.z;
-
-		/* user switched off throttle */
-		if (_manual.z < 0.1f) {
-			throttle_max = 0.0f;
-		}
-
-		/* climb out control */
-		bool climb_out = false;
-
-		/* user wants to climb out */
-		if (_manual.x > 0.3f && _manual.z > 0.8f) {
-			climb_out = true;
-		}
-
-		_l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed_2d);
-		_att_sp.roll_body =	_manual.y;
-		_att_sp.yaw_body =	_manual.r;
-		tecs_update_pitch_throttle(_global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, eas2tas,
-					math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
-					_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-					climb_out, math::radians(_parameters.pitch_limit_min),
-					_global_pos.alt, ground_speed);
-
 	} else {
 
 		_was_pos_control_mode = false;
@@ -1242,9 +1170,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
 	}
 	else {
-		_att_sp.thrust = math::min(_mTecs.getThrottleSetpoint(), throttle_max);
+		_att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : _tecs.get_throttle_demand(), throttle_max);
 	}
-	_att_sp.pitch_body = _mTecs.getPitchSetpoint();
+	_att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
 
 	if (_control_mode.flag_control_position_enabled) {
 		last_manual = false;
@@ -1260,10 +1188,6 @@ void
 FixedwingPositionControl::task_main()
 {
 
-	/* inform about start */
-	warnx("Initializing..");
-	fflush(stdout);
-
 	/*
 	 * do subscriptions
 	 */
@@ -1339,14 +1263,6 @@ FixedwingPositionControl::task_main()
 				_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 			}
 
-			static uint64_t last_run = 0;
-			float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
-			last_run = hrt_absolute_time();
-
-			/* guard against too large deltaT's */
-			if (deltaT > 1.0f)
-				deltaT = 0.01f;
-
 			/* load local copies */
 			orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
 
@@ -1432,20 +1348,29 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 		const math::Vector<3> &ground_speed,
 		tecs_mode mode)
 {
-	/* Using mtecs library: prepare arguments for mtecs call */
-	float flightPathAngle = 0.0f;
-	float ground_speed_length = ground_speed.length();
-	if (ground_speed_length > FLT_EPSILON) {
-		flightPathAngle = -asinf(ground_speed(2)/ground_speed_length);
-	}
-	fwPosctrl::LimitOverride limitOverride;
-	if (climbout_mode) {
-		limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad);
+	if (_mTecs.getEnabled()) {
+		/* Using mtecs library: prepare arguments for mtecs call */
+		float flightPathAngle = 0.0f;
+		float ground_speed_length = ground_speed.length();
+		if (ground_speed_length > FLT_EPSILON) {
+			flightPathAngle = -asinf(ground_speed(2)/ground_speed_length);
+		}
+		fwPosctrl::LimitOverride limitOverride;
+		if (climbout_mode) {
+			limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad);
+		} else {
+			limitOverride.disablePitchMinOverride();
+		}
+		_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
+				limitOverride);
 	} else {
-		limitOverride.disablePitchMinOverride();
+		/* Using tecs library */
+		_tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp,
+					    _airspeed.indicated_airspeed_m_s, eas2tas,
+					    climbout_mode, climbout_pitch_min_rad,
+					    throttle_min, throttle_max, throttle_cruise,
+					    pitch_min_rad, pitch_max_rad);
 	}
-	_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
-			limitOverride);
 }
 
 int
@@ -1479,14 +1404,7 @@ int fw_pos_control_l1_main(int argc, char *argv[])
 		if (l1_control::g_control != nullptr)
 			errx(1, "already running");
 
-		l1_control::g_control = new FixedwingPositionControl;
-
-		if (l1_control::g_control == nullptr)
-			errx(1, "alloc failed");
-
-		if (OK != l1_control::g_control->start()) {
-			delete l1_control::g_control;
-			l1_control::g_control = nullptr;
+		if (OK != FixedwingPositionControl::start()) {
 			err(1, "start failed");
 		}
 
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
index 2e37d166e..afc411a60 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
@@ -59,6 +59,7 @@ mTecs::mTecs() :
 	_controlAltitude(this, "FPA", true),
 	_controlAirSpeed(this, "ACC"),
 	_flightPathAngleLowpass(this, "FPA_LP"),
+	_altitudeLowpass(this, "ALT_LP"),
 	_airspeedLowpass(this, "A_LP"),
 	_airspeedDerivative(this, "AD"),
 	_throttleSp(0.0f),
@@ -75,6 +76,7 @@ mTecs::mTecs() :
 	_counter(0),
 	_debug(false)
 {
+	warnx("starting");
 }
 
 mTecs::~mTecs()
@@ -93,18 +95,23 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti
 	/* time measurement */
 	updateTimeMeasurement();
 
+	/* Filter altitude */
+	float altitudeFiltered = _altitudeLowpass.update(altitude);
+
+
 	/* calculate flight path angle setpoint from altitude setpoint */
-	float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitude);
+	float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitudeFiltered);
 
 	/* Debug output */
 	if (_counter % 10 == 0) {
 		debug("***");
-		debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp);
+		debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, altitude filtered %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)altitudeFiltered, (double)flightPathAngleSp);
 	}
 
 	/* Write part of the status message */
 	_status.altitudeSp = altitudeSp;
 	_status.altitude = altitude;
+	_status.altitudeFiltered = altitudeFiltered;
 
 
 	/* use flightpath angle setpoint for total energy control */
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
index efa89a5d3..ae6867d38 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
@@ -115,6 +115,7 @@ protected:
 
 	/* Other calculation Blocks */
 	control::BlockLowPass _flightPathAngleLowpass;	/**< low pass filter for the flight path angle */
+	control::BlockLowPass _altitudeLowpass;	/**< low pass filter for altitude */
 	control::BlockLowPass _airspeedLowpass;		/**< low pass filter for airspeed */
 	control::BlockDerivative _airspeedDerivative;	/**< airspeed derivative calulation */
 
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
index c22e60ae0..bb2065472 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
@@ -72,8 +72,8 @@ public:
 	 * @return: true if the limit is applied, false otherwise
 	 */
 	bool limit(float& value, float& difference) {
-		float minimum = isAngularLimit() ? getMin() * M_DEG_TO_RAD_F : getMin();
-		float maximum = isAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax();
+		float minimum = getIsAngularLimit() ? getMin() * M_DEG_TO_RAD_F : getMin();
+		float maximum = getIsAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax();
 		if (value < minimum) {
 			difference = value - minimum;
 			value = minimum;
@@ -86,7 +86,7 @@ public:
 		return false;
 	}
 //accessor:
-	bool isAngularLimit() {return _isAngularLimit ;}
+	bool getIsAngularLimit() {return _isAngularLimit ;}
 	float getMin() { return _min.get(); }
 	float getMax() { return _max.get(); }
 	void setMin(float value) { _min.set(value); }
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
index 5b9238780..7cfe63fbc 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
@@ -175,6 +175,13 @@ PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f);
 PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f);
 
 /**
+ * Lowpass (cutoff freq.) for altitude
+ *
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_ALT_LP, 1.0f);
+
+/**
  * Lowpass (cutoff freq.) for the flight path angle
  *
  * @group mTECS
@@ -234,7 +241,14 @@ PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f);
  *
  * @group mTECS
  */
-PARAM_DEFINE_FLOAT(MT_A_LP, 1.0f);
+PARAM_DEFINE_FLOAT(MT_A_LP, 0.5f);
+
+/**
+ * Airspeed derivative calculation lowpass
+ *
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_AD_LP, 0.5f);
 
 /**
  * P gain for the airspeed control
@@ -261,7 +275,7 @@ PARAM_DEFINE_FLOAT(MT_ACC_D, 0.0f);
  *
  * @group mTECS
  */
-PARAM_DEFINE_FLOAT(MT_ACC_D_LP, 1.0f);
+PARAM_DEFINE_FLOAT(MT_ACC_D_LP, 0.5f);
 
 /**
  * Minimal acceleration (air)
@@ -280,13 +294,6 @@ PARAM_DEFINE_FLOAT(MT_ACC_MIN, -40.0f);
 PARAM_DEFINE_FLOAT(MT_ACC_MAX, 40.0f);
 
 /**
- * Airspeed derivative calculation lowpass
- *
- * @group mTECS
- */
-PARAM_DEFINE_FLOAT(MT_AD_LP, 1.0f);
-
-/**
  * Minimal throttle during takeoff
  *
  * @min 0.0f
diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h
index 374d1511c..0cd08769e 100644
--- a/src/modules/mavlink/mavlink_bridge_header.h
+++ b/src/modules/mavlink/mavlink_bridge_header.h
@@ -44,7 +44,12 @@
 
 __BEGIN_DECLS
 
-#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
+/*
+ * We are NOT using convenience functions,
+ * but instead send messages with a custom function.
+ * So we do NOT do this:
+ * #define MAVLINK_USE_CONVENIENCE_FUNCTIONS
+ */
 
 /* use efficient approach, see mavlink_helpers.h */
 #define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes
diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
index 675a6870e..6a2c900af 100644
--- a/src/modules/mavlink/mavlink_ftp.cpp
+++ b/src/modules/mavlink/mavlink_ftp.cpp
@@ -50,16 +50,20 @@ MavlinkFTP::getServer()
 	return _server;
 }
 
-MavlinkFTP::MavlinkFTP()
+MavlinkFTP::MavlinkFTP() :
+	_session_fds{},
+	_workBufs{},
+	_workFree{},
+	_lock{}
 {
 	// initialise the request freelist
 	dq_init(&_workFree);
 	sem_init(&_lock, 0, 1);
-    
-    // initialize session list
-    for (size_t i=0; i<kMaxSession; i++) {
-        _session_fds[i] = -1;
-    }
+
+	// initialize session list
+	for (size_t i=0; i<kMaxSession; i++) {
+		_session_fds[i] = -1;
+	}
 
 	// drop work entries onto the free list
 	for (unsigned i = 0; i < kRequestQueueSize; i++) {
diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
index 59237a4c4..1bd1158fb 100644
--- a/src/modules/mavlink/mavlink_ftp.h
+++ b/src/modules/mavlink/mavlink_ftp.h
@@ -55,6 +55,7 @@
 #include <systemlib/err.h>
 
 #include "mavlink_messages.h"
+#include "mavlink_main.h"
 
 class MavlinkFTP
 {
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index cd0581af4..5589738c1 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -78,10 +78,9 @@
 #include "mavlink_messages.h"
 #include "mavlink_receiver.h"
 #include "mavlink_rate_limiter.h"
-#include "mavlink_commands.h"
 
 #ifndef MAVLINK_CRC_EXTRA
- #error MAVLINK_CRC_EXTRA has to be defined on PX4 systems
+#error MAVLINK_CRC_EXTRA has to be defined on PX4 systems
 #endif
 
 /* oddly, ERROR is not defined for c++ */
@@ -94,11 +93,16 @@ static const int ERROR = -1;
 #define MAX_DATA_RATE	10000	// max data rate in bytes/s
 #define MAIN_LOOP_DELAY 10000	// 100 Hz @ 1000 bytes/s data rate
 
+#define TX_BUFFER_GAP MAVLINK_MAX_PACKET_LEN
+
 static Mavlink *_mavlink_instances = nullptr;
 
 /* TODO: if this is a class member it crashes */
 static struct file_operations fops;
 
+static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
+static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
+
 /**
  * mavlink app start / stop handling function
  *
@@ -108,114 +112,13 @@ extern "C" __EXPORT int mavlink_main(int argc, char *argv[]);
 
 extern mavlink_system_t mavlink_system;
 
-static uint64_t last_write_success_times[6] = {0};
-static uint64_t last_write_try_times[6] = {0};
-
-/*
- * Internal function to send the bytes through the right serial port
- */
-void
-mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length)
-{
-
-	Mavlink *instance;
-
-	switch (channel) {
-	case MAVLINK_COMM_0:
-		instance = Mavlink::get_instance(0);
-		break;
-
-	case MAVLINK_COMM_1:
-		instance = Mavlink::get_instance(1);
-		break;
-
-	case MAVLINK_COMM_2:
-		instance = Mavlink::get_instance(2);
-		break;
-
-	case MAVLINK_COMM_3:
-		instance = Mavlink::get_instance(3);
-		break;
-#ifdef MAVLINK_COMM_4
-
-	case MAVLINK_COMM_4:
-		instance = Mavlink::get_instance(4);
-		break;
-#endif
-#ifdef MAVLINK_COMM_5
-
-	case MAVLINK_COMM_5:
-		instance = Mavlink::get_instance(5);
-		break;
-#endif
-#ifdef MAVLINK_COMM_6
-
-	case MAVLINK_COMM_6:
-		instance = Mavlink::get_instance(6);
-		break;
-#endif
-		default:
-		return;
-	}
-
-	int uart = instance->get_uart_fd();
-
-	ssize_t desired = (sizeof(uint8_t) * length);
-
-	/*
-	 * Check if the OS buffer is full and disable HW
-	 * flow control if it continues to be full
-	 */
-	int buf_free = 0;
-
-	if (instance->get_flow_control_enabled()
-		&& ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
-
-		/* Disable hardware flow control:
-		 * if no successful write since a defined time
-		 * and if the last try was not the last successful write
-		 */
-		if (last_write_try_times[(unsigned)channel] != 0 &&
-			hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL &&
-			last_write_success_times[(unsigned)channel] !=
-			last_write_try_times[(unsigned)channel])
-		{
-			warnx("DISABLING HARDWARE FLOW CONTROL");
-			instance->enable_flow_control(false);
-		}
-
-	}
-
-	/* If the wait until transmit flag is on, only transmit after we've received messages.
-	   Otherwise, transmit all the time. */
-	if (instance->should_transmit()) {
-		last_write_try_times[(unsigned)channel] = hrt_absolute_time();
-
-		/* check if there is space in the buffer, let it overflow else */
-		if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) {
-
-			if (buf_free < desired) {
-				/* we don't want to send anything just in half, so return */
-				instance->count_txerr();
-				return;
-			}
-		}
-
-		ssize_t ret = write(uart, ch, desired);
-		if (ret != desired) {
-			instance->count_txerr();
-		} else {
-			last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel];
-		}
-	}
-}
-
 static void usage(void);
 
 Mavlink::Mavlink() :
 	_device_name(DEFAULT_DEVICE_NAME),
 	_task_should_exit(false),
 	next(nullptr),
+	_instance_id(0),
 	_mavlink_fd(-1),
 	_task_running(false),
 	_hil_enabled(false),
@@ -227,29 +130,48 @@ Mavlink::Mavlink() :
 	_subscriptions(nullptr),
 	_streams(nullptr),
 	_mission_manager(nullptr),
-	_mission_pub(-1),
-	_mission_result_sub(-1),
+	_parameters_manager(nullptr),
 	_mode(MAVLINK_MODE_NORMAL),
-	_total_counter(0),
-	_verbose(false),
-	_forwarding_on(false),
-	_passing_on(false),
-	_ftp_on(false),
-	_uart_fd(-1),
-	_mavlink_param_queue_index(0),
-	_subscribe_to_stream(nullptr),
-	_subscribe_to_stream_rate(0.0f),
-	_flow_control_enabled(true),
-	_message_buffer({}),
-	_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"))
+	_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"))
 {
 	fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl;
 
@@ -416,6 +338,27 @@ Mavlink::destroy_all_instances()
 	return OK;
 }
 
+int
+Mavlink::get_status_all_instances()
+{
+	Mavlink *inst = ::_mavlink_instances;
+
+	unsigned iterations = 0;
+
+	while (inst != nullptr) {
+
+		printf("\ninstance #%u:\n", iterations);
+		inst->display_status();
+
+		/* move on */
+		inst = inst->next;
+		iterations++;
+	}
+
+	/* return an error if there are no instances */
+	return (iterations == 0);
+}
+
 bool
 Mavlink::instance_exists(const char *device_name, Mavlink *self)
 {
@@ -441,7 +384,12 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self)
 	Mavlink *inst;
 	LL_FOREACH(_mavlink_instances, inst) {
 		if (inst != self) {
-			inst->pass_message(msg);
+
+			/* if not in normal mode, we are an onboard link
+			 * onboard links should only pass on messages from the same system ID */
+			if (!(self->_mode != MAVLINK_MODE_NORMAL && msg->sysid != mavlink_system.sysid)) {
+				inst->pass_message(msg);
+			}
 		}
 	}
 }
@@ -489,10 +437,26 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
 	case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: {
 
 			const char *txt = (const char *)arg;
-//		printf("logmsg: %s\n", txt);
 			struct mavlink_logmessage msg;
 			strncpy(msg.text, txt, sizeof(msg.text));
-			msg.severity = (unsigned char)cmd;
+
+			switch (cmd) {
+			case MAVLINK_IOC_SEND_TEXT_INFO:
+				msg.severity = MAV_SEVERITY_INFO;
+				break;
+
+			case MAVLINK_IOC_SEND_TEXT_CRITICAL:
+				msg.severity = MAV_SEVERITY_CRITICAL;
+				break;
+
+			case MAVLINK_IOC_SEND_TEXT_EMERGENCY:
+				msg.severity = MAV_SEVERITY_EMERGENCY;
+				break;
+
+			default:
+				msg.severity = MAV_SEVERITY_INFO;
+				break;
+			}
 
 			Mavlink *inst;
 			LL_FOREACH(_mavlink_instances, inst) {
@@ -517,22 +481,39 @@ void Mavlink::mavlink_update_system(void)
 		_param_component_id = param_find("MAV_COMP_ID");
 		_param_system_type = param_find("MAV_TYPE");
 		_param_use_hil_gps = param_find("MAV_USEHILGPS");
-		_param_initialized = true;
 	}
 
 	/* update system and component id */
 	int32_t system_id;
 	param_get(_param_system_id, &system_id);
 
-	if (system_id > 0 && system_id < 255) {
-		mavlink_system.sysid = system_id;
-	}
-
 	int32_t component_id;
 	param_get(_param_component_id, &component_id);
 
-	if (component_id > 0 && component_id < 255) {
-		mavlink_system.compid = component_id;
+
+	/* only allow system ID and component ID updates
+	 * after reboot - not during operation */
+	if (!_param_initialized) {
+		if (system_id > 0 && system_id < 255) {
+			mavlink_system.sysid = system_id;
+		}
+
+		if (component_id > 0 && component_id < 255) {
+			mavlink_system.compid = component_id;
+		}
+
+		_param_initialized = true;
+	}
+
+	/* warn users that they need to reboot to take this
+	 * into effect
+	 */
+	if (system_id != mavlink_system.sysid) {
+		send_statustext_critical("Save params and reboot to change SYSID");
+	}
+
+	if (component_id != mavlink_system.compid) {
+		send_statustext_critical("Save params and reboot to change COMPID");
 	}
 
 	int32_t system_type;
@@ -607,7 +588,8 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 	case 921600: speed = B921600; break;
 
 	default:
-		warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n", baud);
+		warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n",
+		      baud);
 		return -EINVAL;
 	}
 
@@ -668,6 +650,9 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 		if (enable_flow_control(true)) {
 			warnx("hardware flow control not supported");
 		}
+
+	} else {
+		_flow_control_enabled = false;
 	}
 
 	return _uart_fd;
@@ -709,8 +694,7 @@ Mavlink::set_hil_enabled(bool hil_enabled)
 	/* enable HIL */
 	if (hil_enabled && !_hil_enabled) {
 		_hil_enabled = true;
-		float rate_mult = _datarate / 1000.0f;
-		configure_stream("HIL_CONTROLS", 15.0f * rate_mult);
+		configure_stream("HIL_CONTROLS", 150.0f);
 	}
 
 	/* disable HIL */
@@ -725,241 +709,188 @@ Mavlink::set_hil_enabled(bool hil_enabled)
 	return ret;
 }
 
-void
-Mavlink::send_message(const mavlink_message_t *msg)
+unsigned
+Mavlink::get_free_tx_buf()
 {
-	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
+	/*
+	 * Check if the OS buffer is full and disable HW
+	 * flow control if it continues to be full
+	 */
+	int buf_free = 0;
+	(void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free);
+
+	if (get_flow_control_enabled() && buf_free < TX_BUFFER_GAP) {
+		/* Disable hardware flow control:
+		 * if no successful write since a defined time
+		 * and if the last try was not the last successful write
+		 */
+		if (_last_write_try_time != 0 &&
+		    hrt_elapsed_time(&_last_write_success_time) > 500 * 1000UL &&
+		    _last_write_success_time != _last_write_try_time) {
+			warnx("DISABLING HARDWARE FLOW CONTROL");
+			enable_flow_control(false);
+		}
+	}
 
-	uint16_t len = mavlink_msg_to_send_buffer(buf, msg);
-	mavlink_send_uart_bytes(_channel, buf, len);
+	return buf_free;
 }
 
 void
-Mavlink::handle_message(const mavlink_message_t *msg)
+Mavlink::send_message(const uint8_t msgid, const void *msg)
 {
-	/* handle packet with mission manager */
-	_mission_manager->handle_message(msg);
+	/* If the wait until transmit flag is on, only transmit after we've received messages.
+	   Otherwise, transmit all the time. */
+	if (!should_transmit()) {
+		return;
+	}
 
-	/* handle packet with parameter component */
-	mavlink_pm_message_handler(_channel, msg);
+	pthread_mutex_lock(&_send_mutex);
 
-	if (get_forwarding_on()) {
-		/* forward any messages to other mavlink instances */
-		Mavlink::forward_message(msg, this);
+	int buf_free = get_free_tx_buf();
+
+	uint8_t payload_len = mavlink_message_lengths[msgid];
+	unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+
+	_last_write_try_time = hrt_absolute_time();
+
+	/* check if there is space in the buffer, let it overflow else */
+	if (buf_free < TX_BUFFER_GAP) {
+		/* no enough space in buffer to send */
+		count_txerr();
+		count_txerrbytes(packet_len);
+		pthread_mutex_unlock(&_send_mutex);
+		return;
 	}
-}
 
-int
-Mavlink::mavlink_pm_queued_send()
-{
-	if (_mavlink_param_queue_index < param_count()) {
-		mavlink_pm_send_param(param_for_index(_mavlink_param_queue_index));
-		_mavlink_param_queue_index++;
-		return 0;
+	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
+
+	/* header */
+	buf[0] = MAVLINK_STX;
+	buf[1] = payload_len;
+	/* use mavlink's internal counter for the TX seq */
+	buf[2] = mavlink_get_channel_status(_channel)->current_tx_seq++;
+	buf[3] = mavlink_system.sysid;
+	buf[4] = mavlink_system.compid;
+	buf[5] = msgid;
+
+	/* payload */
+	memcpy(&buf[MAVLINK_NUM_HEADER_BYTES], msg, payload_len);
+
+	/* checksum */
+	uint16_t checksum;
+	crc_init(&checksum);
+	crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len);
+	crc_accumulate(mavlink_message_crcs[msgid], &checksum);
+
+	buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF);
+	buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8);
+
+	/* send message to UART */
+	ssize_t ret = write(_uart_fd, buf, packet_len);
+
+	if (ret != (int) packet_len) {
+		count_txerr();
+		count_txerrbytes(packet_len);
 
 	} else {
-		return 1;
+		_last_write_success_time = _last_write_try_time;
+		count_txbytes(packet_len);
 	}
-}
 
-void Mavlink::mavlink_pm_start_queued_send()
-{
-	_mavlink_param_queue_index = 0;
+	pthread_mutex_unlock(&_send_mutex);
 }
 
-int Mavlink::mavlink_pm_send_param_for_index(uint16_t index)
+void
+Mavlink::resend_message(mavlink_message_t *msg)
 {
-	return mavlink_pm_send_param(param_for_index(index));
-}
+	/* If the wait until transmit flag is on, only transmit after we've received messages.
+	   Otherwise, transmit all the time. */
+	if (!should_transmit()) {
+		return;
+	}
 
-int Mavlink::mavlink_pm_send_param_for_name(const char *name)
-{
-	return mavlink_pm_send_param(param_find(name));
-}
+	pthread_mutex_lock(&_send_mutex);
 
-int Mavlink::mavlink_pm_send_param(param_t param)
-{
-	if (param == PARAM_INVALID) { return 1; }
+	int buf_free = get_free_tx_buf();
 
-	/* buffers for param transmission */
-	char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
-	float val_buf;
-	mavlink_message_t tx_msg;
+	_last_write_try_time = hrt_absolute_time();
 
-	/* query parameter type */
-	param_type_t type = param_type(param);
-	/* copy parameter name */
-	strncpy((char *)name_buf, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+	unsigned packet_len = msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 
-	/*
-	 * Map onboard parameter type to MAVLink type,
-	 * endianess matches (both little endian)
-	 */
-	uint8_t mavlink_type;
+	/* check if there is space in the buffer, let it overflow else */
+	if (buf_free < TX_BUFFER_GAP) {
+		/* no enough space in buffer to send */
+		count_txerr();
+		count_txerrbytes(packet_len);
+		pthread_mutex_unlock(&_send_mutex);
+		return;
+	}
 
-	if (type == PARAM_TYPE_INT32) {
-		mavlink_type = MAVLINK_TYPE_INT32_T;
+	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
 
-	} else if (type == PARAM_TYPE_FLOAT) {
-		mavlink_type = MAVLINK_TYPE_FLOAT;
+	/* header and payload */
+	memcpy(&buf[0], &msg->magic, MAVLINK_NUM_HEADER_BYTES + msg->len);
 
-	} else {
-		mavlink_type = MAVLINK_TYPE_FLOAT;
-	}
+	/* checksum */
+	buf[MAVLINK_NUM_HEADER_BYTES + msg->len] = (uint8_t)(msg->checksum & 0xFF);
+	buf[MAVLINK_NUM_HEADER_BYTES + msg->len + 1] = (uint8_t)(msg->checksum >> 8);
 
-	/*
-	 * get param value, since MAVLink encodes float and int params in the same
-	 * space during transmission, copy param onto float val_buf
-	 */
+	/* send message to UART */
+	ssize_t ret = write(_uart_fd, buf, packet_len);
 
-	int ret;
+	if (ret != (int) packet_len) {
+		count_txerr();
+		count_txerrbytes(packet_len);
 
-	if ((ret = param_get(param, &val_buf)) != OK) {
-		return ret;
+	} else {
+		_last_write_success_time = _last_write_try_time;
+		count_txbytes(packet_len);
 	}
 
-	mavlink_msg_param_value_pack_chan(mavlink_system.sysid,
-					  mavlink_system.compid,
-					  _channel,
-					  &tx_msg,
-					  name_buf,
-					  val_buf,
-					  mavlink_type,
-					  param_count(),
-					  param_get_index(param));
-	send_message(&tx_msg);
-	return OK;
+	pthread_mutex_unlock(&_send_mutex);
 }
 
-void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg)
+void
+Mavlink::handle_message(const mavlink_message_t *msg)
 {
-	switch (msg->msgid) {
-	case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
-			mavlink_param_request_list_t req;
-			mavlink_msg_param_request_list_decode(msg, &req);
-			if (req.target_system == mavlink_system.sysid &&
-					(req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) {
-				/* Start sending parameters */
-				mavlink_pm_start_queued_send();
-				send_statustext_info("[pm] sending list");
-			}
-		} break;
-
-	case MAVLINK_MSG_ID_PARAM_SET: {
-
-			/* Handle parameter setting */
-
-			if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) {
-				mavlink_param_set_t mavlink_param_set;
-				mavlink_msg_param_set_decode(msg, &mavlink_param_set);
-
-				if (mavlink_param_set.target_system == mavlink_system.sysid && ((mavlink_param_set.target_component == mavlink_system.compid) || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) {
-					/* local name buffer to enforce null-terminated string */
-					char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
-					strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
-					/* enforce null termination */
-					name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
-					/* attempt to find parameter, set and send it */
-					param_t param = param_find(name);
-
-					if (param == PARAM_INVALID) {
-						char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
-						sprintf(buf, "[pm] unknown: %s", name);
-						send_statustext_info(buf);
-
-					} else {
-						/* set and send parameter */
-						param_set(param, &(mavlink_param_set.param_value));
-						mavlink_pm_send_param(param);
-					}
-				}
-			}
-		} break;
-
-	case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
-			mavlink_param_request_read_t mavlink_param_request_read;
-			mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read);
-
-			if (mavlink_param_request_read.target_system == mavlink_system.sysid && ((mavlink_param_request_read.target_component == mavlink_system.compid) || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) {
-				/* when no index is given, loop through string ids and compare them */
-				if (mavlink_param_request_read.param_index == -1) {
-					/* local name buffer to enforce null-terminated string */
-					char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
-					strncpy(name, mavlink_param_request_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
-					/* enforce null termination */
-					name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
-					/* attempt to find parameter and send it */
-					mavlink_pm_send_param_for_name(name);
+	/* handle packet with mission manager */
+	_mission_manager->handle_message(msg);
 
-				} else {
-					/* when index is >= 0, send this parameter again */
-					mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index);
-				}
-			}
+	/* handle packet with parameter component */
+	_parameters_manager->handle_message(msg);
 
-		} break;
+	if (get_forwarding_on()) {
+		/* forward any messages to other mavlink instances */
+		Mavlink::forward_message(msg, this);
 	}
 }
 
-int
+void
 Mavlink::send_statustext_info(const char *string)
 {
-	return send_statustext(MAVLINK_IOC_SEND_TEXT_INFO, string);
+	send_statustext(MAV_SEVERITY_INFO, string);
 }
 
-int
+void
 Mavlink::send_statustext_critical(const char *string)
 {
-	return send_statustext(MAVLINK_IOC_SEND_TEXT_CRITICAL, string);
+	send_statustext(MAV_SEVERITY_CRITICAL, string);
 }
 
-int
+void
 Mavlink::send_statustext_emergency(const char *string)
 {
-	return send_statustext(MAVLINK_IOC_SEND_TEXT_EMERGENCY, string);
+	send_statustext(MAV_SEVERITY_EMERGENCY, string);
 }
 
-int
-Mavlink::send_statustext(unsigned severity, const char *string)
+void
+Mavlink::send_statustext(unsigned char severity, const char *string)
 {
-	const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;
-	mavlink_statustext_t statustext;
-
-	int i = 0;
-
-	while (i < len - 1) {
-		statustext.text[i] = string[i];
+	struct mavlink_logmessage logmsg;
+	strncpy(logmsg.text, string, sizeof(logmsg.text));
+	logmsg.severity = severity;
 
-		if (string[i] == '\0') {
-			break;
-		}
-
-		i++;
-	}
-
-	if (i > 1) {
-		/* Enforce null termination */
-		statustext.text[i] = '\0';
-
-		/* Map severity */
-		switch (severity) {
-			case MAVLINK_IOC_SEND_TEXT_INFO:
-				statustext.severity = MAV_SEVERITY_INFO;
-				break;
-			case MAVLINK_IOC_SEND_TEXT_CRITICAL:
-				statustext.severity = MAV_SEVERITY_CRITICAL;
-				break;
-			case MAVLINK_IOC_SEND_TEXT_EMERGENCY:
-				statustext.severity = MAV_SEVERITY_EMERGENCY;
-				break;
-		}
-
-		mavlink_msg_statustext_send(_channel, statustext.severity, statustext.text);
-		return OK;
-
-	} else {
-		return ERROR;
-	}
+	mavlink_logbuffer_write(&_logbuffer, &logmsg);
 }
 
 MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic)
@@ -982,11 +913,17 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic)
 	return sub_new;
 }
 
+unsigned int
+Mavlink::interval_from_rate(float rate)
+{
+	return (rate > 0.0f) ? (1000000.0f / rate) : 0;
+}
+
 int
 Mavlink::configure_stream(const char *stream_name, const float rate)
 {
 	/* calculate interval in us, 0 means disabled stream */
-	unsigned int interval = (rate > 0.0f) ? (1000000.0f / rate) : 0;
+	unsigned int interval = interval_from_rate(rate);
 
 	/* search if stream exists */
 	MavlinkStream *stream;
@@ -1017,10 +954,8 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
 
 		if (strcmp(stream_name, streams_list[i]->get_name()) == 0) {
 			/* create new instance */
-			stream = streams_list[i]->new_instance();
-			stream->set_channel(get_channel());
+			stream = streams_list[i]->new_instance(this);
 			stream->set_interval(interval);
-			stream->subscribe(this);
 			LL_APPEND(_streams, stream);
 
 			return OK;
@@ -1034,6 +969,31 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
 }
 
 void
+Mavlink::adjust_stream_rates(const float multiplier)
+{
+	/* do not allow to push us to zero */
+	if (multiplier < 0.01f) {
+		return;
+	}
+
+	/* search if stream exists */
+	MavlinkStream *stream;
+	LL_FOREACH(_streams, stream) {
+		/* set new interval */
+		unsigned interval = stream->get_interval();
+		interval /= multiplier;
+
+		/* allow max ~600 Hz */
+		if (interval < 1600) {
+			interval = 1600;
+		}
+
+		/* set new interval */
+		stream->set_interval(interval * multiplier);
+	}
+}
+
+void
 Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate)
 {
 	/* orb subscription must be done from the main thread,
@@ -1068,12 +1028,14 @@ Mavlink::message_buffer_init(int size)
 	_message_buffer.size = size;
 	_message_buffer.write_ptr = 0;
 	_message_buffer.read_ptr = 0;
-	_message_buffer.data = (char*)malloc(_message_buffer.size);
+	_message_buffer.data = (char *)malloc(_message_buffer.size);
 
 	int ret;
+
 	if (_message_buffer.data == 0) {
 		ret = ERROR;
 		_message_buffer.size = 0;
+
 	} else {
 		ret = OK;
 	}
@@ -1191,7 +1153,27 @@ Mavlink::pass_message(const mavlink_message_t *msg)
 float
 Mavlink::get_rate_mult()
 {
-	return _datarate / 1000.0f;
+	return _rate_mult;
+}
+
+void
+Mavlink::update_rate_mult()
+{
+	float const_rate = 0.0f;
+	float rate = 0.0f;
+
+	MavlinkStream *stream;
+	LL_FOREACH(_streams, stream) {
+		if (stream->const_rate()) {
+			const_rate += stream->get_size() * 1000000.0f / stream->get_interval();
+
+		} else {
+			rate += stream->get_size() * 1000000.0f / stream->get_interval();
+		}
+	}
+
+	/* don't scale up rates, only scale down if needed */
+	_rate_mult = fminf(1.0f, ((float)_datarate - const_rate) / rate);
 }
 
 int
@@ -1329,6 +1311,9 @@ Mavlink::task_main(int argc, char *argv[])
 		return ERROR;
 	}
 
+	/* initialize send mutex */
+	pthread_mutex_init(&_send_mutex, NULL);
+
 	/* initialize mavlink text message buffering */
 	mavlink_logbuffer_init(&_logbuffer, 5);
 
@@ -1338,7 +1323,7 @@ Mavlink::task_main(int argc, char *argv[])
 		 * make space for two messages plus off-by-one space as we use the empty element
 		 * marker ring buffer approach.
 		 */
-		if (OK != message_buffer_init(2 * MAVLINK_MAX_PACKET_LEN + 2)) {
+		if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) {
 			errx(1, "can't allocate message buffer, exiting");
 		}
 
@@ -1358,12 +1343,6 @@ Mavlink::task_main(int argc, char *argv[])
 	/* start the MAVLink receiver */
 	_receive_thread = MavlinkReceiver::receive_start(this);
 
-	_mission_result_sub = orb_subscribe(ORB_ID(mission_result));
-
-	/* create mission manager */
-	_mission_manager = new MavlinkMissionManager(this);
-	_mission_manager->set_verbose(_verbose);
-
 	_task_running = true;
 
 	MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update));
@@ -1374,34 +1353,50 @@ Mavlink::task_main(int argc, char *argv[])
 	struct vehicle_status_s status;
 	status_sub->update(&status_time, &status);
 
-	MavlinkCommandsStream commands_stream(this, _channel);
-
-	/* add default streams depending on mode and intervals depending on datarate */
-	float rate_mult = get_rate_mult();
+	/* add default streams depending on mode */
 
+	/* HEARTBEAT is constant rate stream, rate never adjusted */
 	configure_stream("HEARTBEAT", 1.0f);
 
+	/* STATUSTEXT stream is like normal stream but gets messages from logbuffer instead of uORB */
+	configure_stream("STATUSTEXT", 20.0f);
+
+	/* COMMAND_LONG stream: use high rate to avoid commands skipping */
+	configure_stream("COMMAND_LONG", 100.0f);
+
+	/* PARAM_VALUE stream */
+	_parameters_manager = (MavlinkParametersManager *) MavlinkParametersManager::new_instance(this);
+	_parameters_manager->set_interval(interval_from_rate(30.0f));
+	LL_APPEND(_streams, _parameters_manager);
+
+	/* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on
+	 * remote requests rate. Rate specified here controls how much bandwidth we will reserve for
+	 * mission messages. */
+	_mission_manager = (MavlinkMissionManager *) MavlinkMissionManager::new_instance(this);
+	_mission_manager->set_interval(interval_from_rate(10.0f));
+	_mission_manager->set_verbose(_verbose);
+	LL_APPEND(_streams, _mission_manager);
+
 	switch (_mode) {
 	case MAVLINK_MODE_NORMAL:
 		configure_stream("SYS_STATUS", 1.0f);
 		configure_stream("GPS_GLOBAL_ORIGIN", 0.5f);
-		configure_stream("HIGHRES_IMU", 1.0f * rate_mult);
-		configure_stream("ATTITUDE", 10.0f * rate_mult);
-		configure_stream("VFR_HUD", 10.0f * rate_mult);
-		configure_stream("GPS_RAW_INT", 1.0f * rate_mult);
-		configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult);
-		configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult);
-		configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult);
-		configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult);
-		configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult);
-		configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult);
+		configure_stream("HIGHRES_IMU", 1.0f);
+		configure_stream("ATTITUDE", 10.0f);
+		configure_stream("VFR_HUD", 8.0f);
+		configure_stream("GPS_RAW_INT", 1.0f);
+		configure_stream("GLOBAL_POSITION_INT", 3.0f);
+		configure_stream("LOCAL_POSITION_NED", 3.0f);
+		configure_stream("RC_CHANNELS_RAW", 1.0f);
+		configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f);
+		configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f);
 		configure_stream("DISTANCE_SENSOR", 0.5f);
 		break;
 
 	case MAVLINK_MODE_CAMERA:
 		configure_stream("SYS_STATUS", 1.0f);
-		configure_stream("ATTITUDE", 15.0f * rate_mult);
-		configure_stream("GLOBAL_POSITION_INT", 15.0f * rate_mult);
+		configure_stream("ATTITUDE", 15.0f);
+		configure_stream("GLOBAL_POSITION_INT", 15.0f);
 		configure_stream("CAMERA_CAPTURE", 1.0f);
 		break;
 
@@ -1409,13 +1404,8 @@ Mavlink::task_main(int argc, char *argv[])
 		break;
 	}
 
-	/* don't send parameters on startup without request */
-	_mavlink_param_queue_index = param_count();
-
-	MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult);
-
 	/* set main loop delay depending on data rate to minimize CPU overhead */
-	_main_loop_delay = MAIN_LOOP_DELAY / rate_mult;
+	_main_loop_delay = MAIN_LOOP_DELAY * 1000 / _datarate;
 
 	/* now the instance is fully initialized and we can bump the instance count */
 	LL_APPEND(_mavlink_instances, this);
@@ -1428,6 +1418,8 @@ Mavlink::task_main(int argc, char *argv[])
 
 		hrt_abstime t = hrt_absolute_time();
 
+		update_rate_mult();
+
 		if (param_sub->update(&param_time, nullptr)) {
 			/* parameters updated */
 			mavlink_update_system();
@@ -1438,14 +1430,12 @@ Mavlink::task_main(int argc, char *argv[])
 			set_hil_enabled(status.hil_state == HIL_STATE_ON);
 		}
 
-		/* update commands stream */
-		commands_stream.update(t);
-
 		/* check for requested subscriptions */
 		if (_subscribe_to_stream != nullptr) {
 			if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) {
 				if (_subscribe_to_stream_rate > 0.0f) {
-					warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, (double)_subscribe_to_stream_rate);
+					warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name,
+					      (double)_subscribe_to_stream_rate);
 
 				} else {
 					warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name);
@@ -1465,72 +1455,72 @@ Mavlink::task_main(int argc, char *argv[])
 			stream->update(t);
 		}
 
-		if (fast_rate_limiter.check(t)) {
-			mavlink_pm_queued_send();
-			_mission_manager->eventloop();
-
-			if (!mavlink_logbuffer_is_empty(&_logbuffer)) {
-				struct mavlink_logmessage msg;
-				int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg);
-
-				if (lb_ret == OK) {
-					send_statustext(msg.severity, msg.text);
-				}
-			}
-		}
-
 		/* pass messages from other UARTs or FTP worker */
 		if (_passing_on || _ftp_on) {
 
 			bool is_part;
 			uint8_t *read_ptr;
-            uint8_t *write_ptr;
+			uint8_t *write_ptr;
 
 			pthread_mutex_lock(&_message_buffer_mutex);
-			int available = message_buffer_get_ptr((void**)&read_ptr, &is_part);
+			int available = message_buffer_get_ptr((void **)&read_ptr, &is_part);
 			pthread_mutex_unlock(&_message_buffer_mutex);
 
 			if (available > 0) {
-                // Reconstruct message from buffer
+				// Reconstruct message from buffer
 
 				mavlink_message_t msg;
-                write_ptr = (uint8_t*)&msg;
-
-                // Pull a single message from the buffer
-                size_t read_count = available;
-                if (read_count > sizeof(mavlink_message_t)) {
-                    read_count = sizeof(mavlink_message_t);
-                }
-                
-                memcpy(write_ptr, read_ptr, read_count);
-                
-                // We hold the mutex until after we complete the second part of the buffer. If we don't
-                // we may end up breaking the empty slot overflow detection semantics when we mark the
-                // possibly partial read below.
-                pthread_mutex_lock(&_message_buffer_mutex);
-                
+				write_ptr = (uint8_t *)&msg;
+
+				// Pull a single message from the buffer
+				size_t read_count = available;
+
+				if (read_count > sizeof(mavlink_message_t)) {
+					read_count = sizeof(mavlink_message_t);
+				}
+
+				memcpy(write_ptr, read_ptr, read_count);
+
+				// We hold the mutex until after we complete the second part of the buffer. If we don't
+				// we may end up breaking the empty slot overflow detection semantics when we mark the
+				// possibly partial read below.
+				pthread_mutex_lock(&_message_buffer_mutex);
+
 				message_buffer_mark_read(read_count);
 
 				/* write second part of buffer if there is some */
 				if (is_part && read_count < sizeof(mavlink_message_t)) {
-                    write_ptr += read_count;
-					available = message_buffer_get_ptr((void**)&read_ptr, &is_part);
-                    read_count = sizeof(mavlink_message_t) - read_count;
-                    memcpy(write_ptr, read_ptr, read_count);
+					write_ptr += read_count;
+					available = message_buffer_get_ptr((void **)&read_ptr, &is_part);
+					read_count = sizeof(mavlink_message_t) - read_count;
+					memcpy(write_ptr, read_ptr, read_count);
 					message_buffer_mark_read(available);
 				}
-                
-                pthread_mutex_unlock(&_message_buffer_mutex);
 
-                _mavlink_resend_uart(_channel, &msg);
+				pthread_mutex_unlock(&_message_buffer_mutex);
+
+				resend_message(&msg);
 			}
 		}
 
+		/* update TX/RX rates*/
+		if (t > _bytes_timestamp + 1000000) {
+			if (_bytes_timestamp != 0) {
+				float dt = (t - _bytes_timestamp) / 1000.0f;
+				_rate_tx = _bytes_tx / dt;
+				_rate_txerr = _bytes_txerr / dt;
+				_rate_rx = _bytes_rx / dt;
+				_bytes_tx = 0;
+				_bytes_txerr = 0;
+				_bytes_rx = 0;
+			}
+
+			_bytes_timestamp = t;
+		}
+
 		perf_end(_loop_perf);
 	}
 
-	delete _mission_manager;
-
 	delete _subscribe_to_stream;
 	_subscribe_to_stream = nullptr;
 
@@ -1576,6 +1566,7 @@ Mavlink::task_main(int argc, char *argv[])
 		message_buffer_destroy();
 		pthread_mutex_destroy(&_message_buffer_mutex);
 	}
+
 	/* destroy log buffer */
 	mavlink_logbuffer_destroy(&_logbuffer);
 
@@ -1597,6 +1588,7 @@ int Mavlink::start_helper(int argc, char *argv[])
 		/* out of memory */
 		res = -ENOMEM;
 		warnx("OUT OF MEM");
+
 	} else {
 		/* this will actually only return once MAVLink exits */
 		res = instance->task_main(argc, argv);
@@ -1656,7 +1648,42 @@ Mavlink::start(int argc, char *argv[])
 void
 Mavlink::display_status()
 {
-	warnx("running");
+
+	if (_rstatus.heartbeat_time > 0) {
+		printf("\tGCS heartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time));
+	}
+
+	if (_rstatus.timestamp > 0) {
+
+		printf("\ttype:\t\t");
+
+		switch (_rstatus.type) {
+		case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO:
+			printf("3DR RADIO\n");
+			break;
+
+		default:
+			printf("UNKNOWN RADIO\n");
+			break;
+		}
+
+		printf("\trssi:\t\t%d\n", _rstatus.rssi);
+		printf("\tremote rssi:\t%u\n", _rstatus.remote_rssi);
+		printf("\ttxbuf:\t\t%u\n", _rstatus.txbuf);
+		printf("\tnoise:\t\t%d\n", _rstatus.noise);
+		printf("\tremote noise:\t%u\n", _rstatus.remote_noise);
+		printf("\trx errors:\t%u\n", _rstatus.rxerrors);
+		printf("\tfixed:\t\t%u\n", _rstatus.fixed);
+
+	} else {
+		printf("\tno telem status.\n");
+	}
+
+	printf("\trates:\n");
+	printf("\ttx: %.3f kB/s\n", (double)_rate_tx);
+	printf("\ttxerr: %.3f kB/s\n", (double)_rate_txerr);
+	printf("\trx: %.3f kB/s\n", (double)_rate_rx);
+	printf("\trate mult: %.3f\n", (double)_rate_mult);
 }
 
 int
@@ -1744,8 +1771,8 @@ int mavlink_main(int argc, char *argv[])
 	} else if (!strcmp(argv[1], "stop-all")) {
 		return Mavlink::destroy_all_instances();
 
-		// } else if (!strcmp(argv[1], "status")) {
-		// 	mavlink::g_mavlink->status();
+	} else if (!strcmp(argv[1], "status")) {
+		return Mavlink::get_status_all_instances();
 
 	} else if (!strcmp(argv[1], "stream")) {
 		return Mavlink::stream_command(argc, argv);
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index acfc8b90e..1e2e94cef 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -51,13 +51,14 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
+#include <uORB/topics/telemetry_status.h>
 
 #include "mavlink_bridge_header.h"
 #include "mavlink_orb_subscription.h"
 #include "mavlink_stream.h"
 #include "mavlink_messages.h"
 #include "mavlink_mission.h"
-
+#include "mavlink_parameters.h"
 
 class Mavlink
 {
@@ -97,6 +98,8 @@ public:
 
 	static int		destroy_all_instances();
 
+	static int		get_status_all_instances();
+
 	static bool		instance_exists(const char *device_name, Mavlink *self);
 
 	static void		forward_message(const mavlink_message_t *msg, Mavlink *self);
@@ -157,7 +160,12 @@ public:
 	 */
 	int			set_hil_enabled(bool hil_enabled);
 
-	void			send_message(const mavlink_message_t *msg);
+	void		send_message(const uint8_t msgid, const void *msg);
+
+	/**
+	 * Resend message as is, don't change sequence number and CRC.
+	 */
+	void		resend_message(mavlink_message_t *msg);
 
 	void			handle_message(const mavlink_message_t *msg);
 
@@ -185,29 +193,33 @@ public:
 	 *
 	 * @param string the message to send (will be capped by mavlink max string length)
 	 */
-	int			send_statustext_info(const char *string);
+	void			send_statustext_info(const char *string);
 
 	/**
 	 * Send a status text with loglevel CRITICAL
 	 *
 	 * @param string the message to send (will be capped by mavlink max string length)
 	 */
-	int			send_statustext_critical(const char *string);
+	void			send_statustext_critical(const char *string);
 
 	/**
 	 * Send a status text with loglevel EMERGENCY
 	 *
 	 * @param string the message to send (will be capped by mavlink max string length)
 	 */
-	int			send_statustext_emergency(const char *string);
+	void			send_statustext_emergency(const char *string);
 
 	/**
-	 * Send a status text with loglevel
+	 * Send a status text with loglevel, the difference from mavlink_log_xxx() is that message sent
+	 * only on this mavlink connection. Useful for reporting communication specific, not system-wide info
+	 * only to client interested in it. Message will be not sent immediately but queued in buffer as
+	 * for mavlink_log_xxx().
 	 *
 	 * @param string the message to send (will be capped by mavlink max string length)
-	 * @param severity the log level, one of 
+	 * @param severity the log level
 	 */
-	int			send_statustext(unsigned severity, const char *string);
+	void			send_statustext(unsigned char severity, const char *string);
+
 	MavlinkStream *		get_streams() const { return _streams; }
 
 	float			get_rate_mult();
@@ -229,6 +241,28 @@ public:
 	 */
 	void			count_txerr();
 
+	/**
+	 * Count transmitted bytes
+	 */
+	void			count_txbytes(unsigned n) { _bytes_tx += n; };
+
+	/**
+	 * Count bytes not transmitted because of errors
+	 */
+	void			count_txerrbytes(unsigned n) { _bytes_txerr += n; };
+
+	/**
+	 * Count received bytes
+	 */
+	void			count_rxbytes(unsigned n) { _bytes_rx += n; };
+
+	/**
+	 * Get the receive status of this MAVLink link
+	 */
+	struct telemetry_status_s&	get_rx_status() { return _rstatus; }
+
+	struct mavlink_logbuffer	*get_logbuffer() { return &_logbuffer; }
+
 protected:
 	Mavlink			*next;
 
@@ -250,13 +284,12 @@ private:
 	MavlinkOrbSubscription	*_subscriptions;
 	MavlinkStream		*_streams;
 
-	MavlinkMissionManager *_mission_manager;
+	MavlinkMissionManager	*_mission_manager;
+	MavlinkParametersManager *_parameters_manager;
 
-	orb_advert_t	_mission_pub;
-	int			_mission_result_sub;
-	MAVLINK_MODE _mode;
+	MAVLINK_MODE 		_mode;
 
-	mavlink_channel_t _channel;
+	mavlink_channel_t	_channel;
 
 	struct mavlink_logbuffer _logbuffer;
 	unsigned int		_total_counter;
@@ -269,7 +302,9 @@ private:
 	bool			_ftp_on;
 	int			_uart_fd;
 	int			_baudrate;
-	int			_datarate;
+	int			_datarate;		///< data rate for normal streams (attitude, position, etc.)
+	int			_datarate_events;	///< data rate for params, waypoints, text messages
+	float		_rate_mult;
 
 	/**
 	 * If the queue index is not at 0, the queue sending
@@ -284,6 +319,18 @@ private:
 	float			_subscribe_to_stream_rate;
 
 	bool			_flow_control_enabled;
+	uint64_t		_last_write_success_time;
+	uint64_t		_last_write_try_time;
+
+	unsigned		_bytes_tx;
+	unsigned		_bytes_txerr;
+	unsigned		_bytes_rx;
+	uint64_t		_bytes_timestamp;
+	float		_rate_tx;
+	float		_rate_txerr;
+	float		_rate_rx;
+
+	struct telemetry_status_s	_rstatus;			///< receive status
 
 	struct mavlink_message_buffer {
 		int write_ptr;
@@ -295,6 +342,7 @@ private:
 	mavlink_message_buffer	_message_buffer;
 
 	pthread_mutex_t		_message_buffer_mutex;
+	pthread_mutex_t		_send_mutex;
 
 	bool			_param_initialized;
 	param_t			_param_system_id;
@@ -305,52 +353,27 @@ private:
 	perf_counter_t		_loop_perf;			/**< loop performance counter */
 	perf_counter_t		_txerr_perf;			/**< TX error counter */
 
-	/**
-	 * Send one parameter.
-	 *
-	 * @param param		The parameter id to send.
-	 * @return		zero on success, nonzero on failure.
-	 */
-	int			mavlink_pm_send_param(param_t param);
+	void			mavlink_update_system();
 
-	/**
-	 * Send one parameter identified by index.
-	 *
-	 * @param index		The index of the parameter to send.
-	 * @return		zero on success, nonzero else.
-	 */
-	int			mavlink_pm_send_param_for_index(uint16_t index);
+	int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
 
 	/**
-	 * Send one parameter identified by name.
+	 * Get the free space in the transmit buffer
 	 *
-	 * @param name		The index of the parameter to send.
-	 * @return		zero on success, nonzero else.
+	 * @return free space in the UART TX buffer
 	 */
-	int			mavlink_pm_send_param_for_name(const char *name);
+	unsigned			get_free_tx_buf();
 
-	/**
-	 * Send a queue of parameters, one parameter per function call.
-	 *
-	 * @return		zero on success, nonzero on failure
-	 */
-	int			mavlink_pm_queued_send(void);
+	static unsigned int interval_from_rate(float rate);
+
+	int configure_stream(const char *stream_name, const float rate);
 
 	/**
-	 * Start sending the parameter queue.
+	 * Adjust the stream rates based on the current rate
 	 *
-	 * This function will not directly send parameters, but instead
-	 * activate the sending of one parameter on each call of
-	 * mavlink_pm_queued_send().
-	 * @see 		mavlink_pm_queued_send()
+	 * @param multiplier if greater than 1, the transmission rate will increase, if smaller than one decrease
 	 */
-	void			mavlink_pm_start_queued_send();
-
-	void			mavlink_update_system();
-
-	int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
-
-	int configure_stream(const char *stream_name, const float rate);
+	void adjust_stream_rates(const float multiplier);
 
 	int message_buffer_init(int size);
 
@@ -366,10 +389,19 @@ private:
 
 	void pass_message(const mavlink_message_t *msg);
 
+	/**
+	 * Update rate mult so total bitrate will be equal to _datarate.
+	 */
+	void update_rate_mult();
+
 	static int	mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
 
 	/**
 	 * Main mavlink task.
 	 */
 	int		task_main(int argc, char *argv[]);
+
+	/* do not allow copying this class */
+	Mavlink(const Mavlink&);
+	Mavlink operator=(const Mavlink&);
 };
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 7c864f127..683f0f1e8 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -40,9 +40,9 @@
  */
 
 #include <stdio.h>
+
 #include <commander/px4_custom_mode.h>
 #include <lib/geo/geo.h>
-
 #include <uORB/uORB.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
@@ -72,11 +72,11 @@
 #include <drivers/drv_rc_input.h>
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_range_finder.h>
-
 #include <systemlib/err.h>
+#include <mavlink/mavlink_log.h>
 
 #include "mavlink_messages.h"
-
+#include "mavlink_main.h"
 
 static uint16_t cm_uint16_from_m_float(float m);
 static void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet,
@@ -249,21 +249,33 @@ public:
 		return MAVLINK_MSG_ID_HEARTBEAT;
 	}
 
-	static MavlinkStream *new_instance()
+	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
-		return new MavlinkStreamHeartbeat();
+		return new MavlinkStreamHeartbeat(mavlink);
+	}
+
+	unsigned get_size()
+	{
+		return MAVLINK_MSG_ID_HEARTBEAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+	}
+
+	bool const_rate() {
+		return true;
 	}
 
 private:
-	MavlinkOrbSubscription *status_sub;
-	MavlinkOrbSubscription *pos_sp_triplet_sub;
+	MavlinkOrbSubscription *_status_sub;
+	MavlinkOrbSubscription *_pos_sp_triplet_sub;
+
+	/* do not allow top copying this class */
+	MavlinkStreamHeartbeat(MavlinkStreamHeartbeat &);
+	MavlinkStreamHeartbeat& operator = (const MavlinkStreamHeartbeat &);
 
 protected:
-	void subscribe(Mavlink *mavlink)
-	{
-		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
-		pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet));
-	}
+	explicit MavlinkStreamHeartbeat(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))),
+		_pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)))
+	{}
 
 	void send(const hrt_abstime t)
 	{
@@ -271,30 +283,150 @@ protected:
 		struct position_setpoint_triplet_s pos_sp_triplet;
 
 		/* always send the heartbeat, independent of the update status of the topics */
-		if (!status_sub->update(&status)) {
+		if (!_status_sub->update(&status)) {
 			/* if topic update failed fill it with defaults */
 			memset(&status, 0, sizeof(status));
 		}
 
-		if (!pos_sp_triplet_sub->update(&pos_sp_triplet)) {
+		if (!_pos_sp_triplet_sub->update(&pos_sp_triplet)) {
 			/* if topic update failed fill it with defaults */
 			memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet));
 		}
 
-		uint8_t mavlink_state = 0;
-		uint8_t mavlink_base_mode = 0;
-		uint32_t mavlink_custom_mode = 0;
-		get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
+		mavlink_heartbeat_t msg;
+
+		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.autopilot = MAV_AUTOPILOT_PX4;
+		msg.mavlink_version = 3;
+
+		_mavlink->send_message(MAVLINK_MSG_ID_HEARTBEAT, &msg);
+	}
+};
+
+class MavlinkStreamStatustext : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamStatustext::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "STATUSTEXT";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_STATUSTEXT;
+	}
+
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamStatustext(mavlink);
+	}
+
+	unsigned get_size() {
+		return mavlink_logbuffer_is_empty(_mavlink->get_logbuffer()) ? 0 : (MAVLINK_MSG_ID_STATUSTEXT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES);
+	}
+
+private:
+	/* do not allow top copying this class */
+	MavlinkStreamStatustext(MavlinkStreamStatustext &);
+	MavlinkStreamStatustext& operator = (const MavlinkStreamStatustext &);
+
+protected:
+	explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink)
+	{}
+
+	void send(const hrt_abstime t)
+	{
+		if (!mavlink_logbuffer_is_empty(_mavlink->get_logbuffer())) {
+			struct mavlink_logmessage logmsg;
+			int lb_ret = mavlink_logbuffer_read(_mavlink->get_logbuffer(), &logmsg);
+
+			if (lb_ret == OK) {
+				mavlink_statustext_t msg;
 
-		mavlink_msg_heartbeat_send(_channel,
-					   mavlink_system.type,
-					   MAV_AUTOPILOT_PX4,
-					   mavlink_base_mode,
-					   mavlink_custom_mode,
-					   mavlink_state);
+				msg.severity = logmsg.severity;
+				strncpy(msg.text, logmsg.text, sizeof(msg.text));
+
+				_mavlink->send_message(MAVLINK_MSG_ID_STATUSTEXT, &msg);
+			}
+		}
 	}
 };
 
+class MavlinkStreamCommandLong : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamCommandLong::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "COMMAND_LONG";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_COMMAND_LONG;
+	}
+
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamCommandLong(mavlink);
+	}
+
+	unsigned get_size() {
+		return 0;	// commands stream is not regular and not predictable
+	}
+
+private:
+	MavlinkOrbSubscription *_cmd_sub;
+	uint64_t _cmd_time;
+
+	/* do not allow top copying this class */
+	MavlinkStreamCommandLong(MavlinkStreamCommandLong &);
+	MavlinkStreamCommandLong& operator = (const MavlinkStreamCommandLong &);
+
+protected:
+	explicit MavlinkStreamCommandLong(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_cmd_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_command))),
+		_cmd_time(0)
+	{}
+
+	void send(const hrt_abstime t)
+	{
+		struct vehicle_command_s cmd;
+
+		if (_cmd_sub->update(&_cmd_time, &cmd)) {
+			/* only send commands for other systems/components */
+			if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) {
+				mavlink_command_long_t msg;
+
+				msg.target_system = cmd.target_system;
+				msg.target_component = cmd.target_component;
+				msg.command = cmd.command;
+				msg.confirmation = cmd.confirmation;
+				msg.param1 = cmd.param1;
+				msg.param2 = cmd.param2;
+				msg.param3 = cmd.param3;
+				msg.param4 = cmd.param4;
+				msg.param5 = cmd.param5;
+				msg.param6 = cmd.param6;
+				msg.param7 = cmd.param7;
+
+				_mavlink->send_message(MAVLINK_MSG_ID_COMMAND_LONG, &msg);
+			}
+		}
+	}
+};
 
 class MavlinkStreamSysStatus : public MavlinkStream
 {
@@ -304,7 +436,7 @@ public:
 		return MavlinkStreamSysStatus::get_name_static();
 	}
 
-	static const char *get_name_static ()
+	static const char *get_name_static()
 	{
 		return "SYS_STATUS";
 	}
@@ -314,39 +446,50 @@ public:
 		return MAVLINK_MSG_ID_SYS_STATUS;
 	}
 
-	static MavlinkStream *new_instance()
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamSysStatus(mavlink);
+	}
+
+	unsigned get_size()
 	{
-		return new MavlinkStreamSysStatus();
+		return MAVLINK_MSG_ID_SYS_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 	}
 
 private:
-	MavlinkOrbSubscription *status_sub;
+	MavlinkOrbSubscription *_status_sub;
+
+	/* do not allow top copying this class */
+	MavlinkStreamSysStatus(MavlinkStreamSysStatus &);
+	MavlinkStreamSysStatus& operator = (const MavlinkStreamSysStatus &);
 
 protected:
-	void subscribe(Mavlink *mavlink)
-	{
-		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
-	}
+	explicit MavlinkStreamSysStatus(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status)))
+	{}
 
 	void send(const hrt_abstime t)
 	{
 		struct vehicle_status_s status;
 
-		if (status_sub->update(&status)) {
-			mavlink_msg_sys_status_send(_channel,
-							status.onboard_control_sensors_present,
-							status.onboard_control_sensors_enabled,
-							status.onboard_control_sensors_health,
-							status.load * 1000.0f,
-							status.battery_voltage * 1000.0f,
-							status.battery_current * 100.0f,
-							status.battery_remaining * 100.0f,
-							status.drop_rate_comm,
-							status.errors_comm,
-							status.errors_count1,
-							status.errors_count2,
-							status.errors_count3,
-							status.errors_count4);
+		if (_status_sub->update(&status)) {
+			mavlink_sys_status_t msg;
+
+			msg.onboard_control_sensors_present = status.onboard_control_sensors_present;
+			msg.onboard_control_sensors_enabled = status.onboard_control_sensors_enabled;
+			msg.onboard_control_sensors_health = status.onboard_control_sensors_health;
+			msg.load = status.load * 1000.0f;
+			msg.voltage_battery = status.battery_voltage * 1000.0f;
+			msg.current_battery = status.battery_current * 100.0f;
+			msg.drop_rate_comm = status.drop_rate_comm;
+			msg.errors_comm = status.errors_comm;
+			msg.errors_count1 = status.errors_count1;
+			msg.errors_count2 = status.errors_count2;
+			msg.errors_count3 = status.errors_count3;
+			msg.errors_count4 = status.errors_count4;
+			msg.battery_remaining = status.battery_remaining * 100.0f;
+
+			_mavlink->send_message(MAVLINK_MSG_ID_SYS_STATUS, &msg);
 		}
 	}
 };
@@ -370,73 +513,89 @@ public:
 		return MAVLINK_MSG_ID_HIGHRES_IMU;
 	}
 
-	static MavlinkStream *new_instance()
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamHighresIMU(mavlink);
+	}
+
+	unsigned get_size()
 	{
-		return new MavlinkStreamHighresIMU();
+		return MAVLINK_MSG_ID_HIGHRES_IMU_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 	}
 
 private:
-	MavlinkOrbSubscription *sensor_sub;
-	uint64_t sensor_time;
+	MavlinkOrbSubscription *_sensor_sub;
+	uint64_t _sensor_time;
 
-	uint64_t accel_timestamp;
-	uint64_t gyro_timestamp;
-	uint64_t mag_timestamp;
-	uint64_t baro_timestamp;
+	uint64_t _accel_timestamp;
+	uint64_t _gyro_timestamp;
+	uint64_t _mag_timestamp;
+	uint64_t _baro_timestamp;
+
+	/* do not allow top copying this class */
+	MavlinkStreamHighresIMU(MavlinkStreamHighresIMU &);
+	MavlinkStreamHighresIMU& operator = (const MavlinkStreamHighresIMU &);
 
 protected:
-	explicit MavlinkStreamHighresIMU() : MavlinkStream(),
-		sensor_time(0),
-		accel_timestamp(0),
-		gyro_timestamp(0),
-		mag_timestamp(0),
-		baro_timestamp(0)
+	explicit MavlinkStreamHighresIMU(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))),
+		_sensor_time(0),
+		_accel_timestamp(0),
+		_gyro_timestamp(0),
+		_mag_timestamp(0),
+		_baro_timestamp(0)
 	{}
 
-	void subscribe(Mavlink *mavlink)
-	{
-		sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined));
-	}
-
 	void send(const hrt_abstime t)
 	{
 		struct sensor_combined_s sensor;
 
-		if (sensor_sub->update(&sensor_time, &sensor)) {
+		if (_sensor_sub->update(&_sensor_time, &sensor)) {
 			uint16_t fields_updated = 0;
 
-			if (accel_timestamp != sensor.accelerometer_timestamp) {
+			if (_accel_timestamp != sensor.accelerometer_timestamp) {
 				/* mark first three dimensions as changed */
 				fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
-				accel_timestamp = sensor.accelerometer_timestamp;
+				_accel_timestamp = sensor.accelerometer_timestamp;
 			}
 
-			if (gyro_timestamp != sensor.timestamp) {
+			if (_gyro_timestamp != sensor.timestamp) {
 				/* mark second group dimensions as changed */
 				fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
-				gyro_timestamp = sensor.timestamp;
+				_gyro_timestamp = sensor.timestamp;
 			}
 
-			if (mag_timestamp != sensor.magnetometer_timestamp) {
+			if (_mag_timestamp != sensor.magnetometer_timestamp) {
 				/* mark third group dimensions as changed */
 				fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
-				mag_timestamp = sensor.magnetometer_timestamp;
+				_mag_timestamp = sensor.magnetometer_timestamp;
 			}
 
-			if (baro_timestamp != sensor.baro_timestamp) {
+			if (_baro_timestamp != sensor.baro_timestamp) {
 				/* mark last group dimensions as changed */
 				fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
-				baro_timestamp = sensor.baro_timestamp;
+				_baro_timestamp = sensor.baro_timestamp;
 			}
 
-			mavlink_msg_highres_imu_send(_channel,
-						     sensor.timestamp,
-						     sensor.accelerometer_m_s2[0], sensor.accelerometer_m_s2[1], sensor.accelerometer_m_s2[2],
-						     sensor.gyro_rad_s[0], sensor.gyro_rad_s[1], sensor.gyro_rad_s[2],
-						     sensor.magnetometer_ga[0], sensor.magnetometer_ga[1], sensor.magnetometer_ga[2],
-						     sensor.baro_pres_mbar, sensor.differential_pressure_pa,
-						     sensor.baro_alt_meter, sensor.baro_temp_celcius,
-						     fields_updated);
+			mavlink_highres_imu_t msg;
+
+			msg.time_usec = sensor.timestamp;
+			msg.xacc = sensor.accelerometer_m_s2[0];
+			msg.yacc = sensor.accelerometer_m_s2[1];
+			msg.zacc = sensor.accelerometer_m_s2[2];
+			msg.xgyro = sensor.gyro_rad_s[0];
+			msg.ygyro = sensor.gyro_rad_s[1];
+			msg.zgyro = sensor.gyro_rad_s[2];
+			msg.xmag = sensor.magnetometer_ga[0];
+			msg.ymag = sensor.magnetometer_ga[1];
+			msg.zmag = sensor.magnetometer_ga[2];
+			msg.abs_pressure = sensor.baro_pres_mbar;
+			msg.diff_pressure = sensor.differential_pressure_pa;
+			msg.pressure_alt = sensor.baro_alt_meter;
+			msg.temperature = sensor.baro_temp_celcius;
+			msg.fields_updated = fields_updated;
+
+			_mavlink->send_message(MAVLINK_MSG_ID_HIGHRES_IMU, &msg);
 		}
 	}
 };
@@ -460,34 +619,47 @@ public:
 		return MAVLINK_MSG_ID_ATTITUDE;
 	}
 
-	static MavlinkStream *new_instance()
+	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
-		return new MavlinkStreamAttitude();
+		return new MavlinkStreamAttitude(mavlink);
+	}
+
+	unsigned get_size()
+	{
+		return MAVLINK_MSG_ID_ATTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 	}
 
 private:
-	MavlinkOrbSubscription *att_sub;
-	uint64_t att_time;
+	MavlinkOrbSubscription *_att_sub;
+	uint64_t _att_time;
+
+	/* do not allow top copying this class */
+	MavlinkStreamAttitude(MavlinkStreamAttitude &);
+	MavlinkStreamAttitude& operator = (const MavlinkStreamAttitude &);
+
 
 protected:
-	explicit MavlinkStreamAttitude() : MavlinkStream(),
-		att_time(0)
+	explicit MavlinkStreamAttitude(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))),
+		_att_time(0)
 	{}
 
-	void subscribe(Mavlink *mavlink)
-	{
-		att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude));
-	}
-
 	void send(const hrt_abstime t)
 	{
 		struct vehicle_attitude_s att;
 
-		if (att_sub->update(&att_time, &att)) {
-			mavlink_msg_attitude_send(_channel,
-						  att.timestamp / 1000,
-						  att.roll, att.pitch, att.yaw,
-						  att.rollspeed, att.pitchspeed, att.yawspeed);
+		if (_att_sub->update(&_att_time, &att)) {
+			mavlink_attitude_t msg;
+
+			msg.time_boot_ms = att.timestamp / 1000;
+			msg.roll = att.roll;
+			msg.pitch = att.pitch;
+			msg.yaw = att.yaw;
+			msg.rollspeed = att.rollspeed;
+			msg.pitchspeed = att.pitchspeed;
+			msg.yawspeed = att.yawspeed;
+
+			_mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE, &msg);
 		}
 	}
 };
@@ -511,39 +683,47 @@ public:
 		return MAVLINK_MSG_ID_ATTITUDE_QUATERNION;
 	}
 
-	static MavlinkStream *new_instance()
+	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
-		return new MavlinkStreamAttitudeQuaternion();
+		return new MavlinkStreamAttitudeQuaternion(mavlink);
+	}
+
+	unsigned get_size()
+	{
+		return MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 	}
 
 private:
-	MavlinkOrbSubscription *att_sub;
-	uint64_t att_time;
+	MavlinkOrbSubscription *_att_sub;
+	uint64_t _att_time;
+
+	/* do not allow top copying this class */
+	MavlinkStreamAttitudeQuaternion(MavlinkStreamAttitudeQuaternion &);
+	MavlinkStreamAttitudeQuaternion& operator = (const MavlinkStreamAttitudeQuaternion &);
 
 protected:
-	explicit MavlinkStreamAttitudeQuaternion() : MavlinkStream(),
-		att_time(0)
+	explicit MavlinkStreamAttitudeQuaternion(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))),
+		_att_time(0)
 	{}
 
-	void subscribe(Mavlink *mavlink)
-	{
-		att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude));
-	}
-
 	void send(const hrt_abstime t)
 	{
 		struct vehicle_attitude_s att;
 
-		if (att_sub->update(&att_time, &att)) {
-			mavlink_msg_attitude_quaternion_send(_channel,
-							     att.timestamp / 1000,
-							     att.q[0],
-							     att.q[1],
-							     att.q[2],
-							     att.q[3],
-							     att.rollspeed,
-							     att.pitchspeed,
-							     att.yawspeed);
+		if (_att_sub->update(&_att_time, &att)) {
+			mavlink_attitude_quaternion_t msg;
+
+			msg.time_boot_ms = att.timestamp / 1000;
+			msg.q1 = att.q[0];
+			msg.q2 = att.q[1];
+			msg.q3 = att.q[2];
+			msg.q4 = att.q[3];
+			msg.rollspeed = att.rollspeed;
+			msg.pitchspeed = att.pitchspeed;
+			msg.yawspeed = att.yawspeed;
+
+			_mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE_QUATERNION, &msg);
 		}
 	}
 };
@@ -568,45 +748,50 @@ public:
 		return MAVLINK_MSG_ID_VFR_HUD;
 	}
 
-	static MavlinkStream *new_instance()
+	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
-		return new MavlinkStreamVFRHUD();
+		return new MavlinkStreamVFRHUD(mavlink);
+	}
+
+	unsigned get_size()
+	{
+		return MAVLINK_MSG_ID_VFR_HUD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 	}
 
 private:
-	MavlinkOrbSubscription *att_sub;
-	uint64_t att_time;
+	MavlinkOrbSubscription *_att_sub;
+	uint64_t _att_time;
+
+	MavlinkOrbSubscription *_pos_sub;
+	uint64_t _pos_time;
 
-	MavlinkOrbSubscription *pos_sub;
-	uint64_t pos_time;
+	MavlinkOrbSubscription *_armed_sub;
+	uint64_t _armed_time;
 
-	MavlinkOrbSubscription *armed_sub;
-	uint64_t armed_time;
+	MavlinkOrbSubscription *_act_sub;
+	uint64_t _act_time;
 
-	MavlinkOrbSubscription *act_sub;
-	uint64_t act_time;
+	MavlinkOrbSubscription *_airspeed_sub;
+	uint64_t _airspeed_time;
 
-	MavlinkOrbSubscription *airspeed_sub;
-	uint64_t airspeed_time;
+	/* do not allow top copying this class */
+	MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &);
+	MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &);
 
 protected:
-	explicit MavlinkStreamVFRHUD() : MavlinkStream(),
-		att_time(0),
-		pos_time(0),
-		armed_time(0),
-		act_time(0),
-		airspeed_time(0)
+	explicit MavlinkStreamVFRHUD(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))),
+		_att_time(0),
+		_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))),
+		_pos_time(0),
+		_armed_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_armed))),
+		_armed_time(0),
+		_act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))),
+		_act_time(0),
+		_airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))),
+		_airspeed_time(0)
 	{}
 
-	void subscribe(Mavlink *mavlink)
-	{
-		att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude));
-		pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position));
-		armed_sub = mavlink->add_orb_subscription(ORB_ID(actuator_armed));
-		act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0));
-		airspeed_sub = mavlink->add_orb_subscription(ORB_ID(airspeed));
-	}
-
 	void send(const hrt_abstime t)
 	{
 		struct vehicle_attitude_s att;
@@ -615,24 +800,23 @@ protected:
 		struct actuator_controls_s act;
 		struct airspeed_s airspeed;
 
-		bool updated = att_sub->update(&att_time, &att);
-		updated |= pos_sub->update(&pos_time, &pos);
-		updated |= armed_sub->update(&armed_time, &armed);
-		updated |= act_sub->update(&act_time, &act);
-		updated |= airspeed_sub->update(&airspeed_time, &airspeed);
+		bool updated = _att_sub->update(&_att_time, &att);
+		updated |= _pos_sub->update(&_pos_time, &pos);
+		updated |= _armed_sub->update(&_armed_time, &armed);
+		updated |= _act_sub->update(&_act_time, &act);
+		updated |= _airspeed_sub->update(&_airspeed_time, &airspeed);
 
 		if (updated) {
-			float groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e);
-			uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
-			float throttle = armed.armed ? act.control[3] * 100.0f : 0.0f;
-
-			mavlink_msg_vfr_hud_send(_channel,
-						 airspeed.true_airspeed_m_s,
-						 groundspeed,
-						 heading,
-						 throttle,
-						 pos.alt,
-						 -pos.vel_d);
+			mavlink_vfr_hud_t msg;
+
+			msg.airspeed = airspeed.true_airspeed_m_s;
+			msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e);
+			msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
+			msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f;
+			msg.alt = pos.alt;
+			msg.climb = -pos.vel_d;
+
+			_mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg);
 		}
 	}
 };
@@ -656,41 +840,49 @@ public:
 		return MAVLINK_MSG_ID_GPS_RAW_INT;
 	}
 
-	static MavlinkStream *new_instance()
+	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
-		return new MavlinkStreamGPSRawInt();
+		return new MavlinkStreamGPSRawInt(mavlink);
+	}
+
+	unsigned get_size()
+	{
+		return MAVLINK_MSG_ID_GPS_RAW_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 	}
 
 private:
-	MavlinkOrbSubscription *gps_sub;
-	uint64_t gps_time;
+	MavlinkOrbSubscription *_gps_sub;
+	uint64_t _gps_time;
+
+	/* do not allow top copying this class */
+	MavlinkStreamGPSRawInt(MavlinkStreamGPSRawInt &);
+	MavlinkStreamGPSRawInt& operator = (const MavlinkStreamGPSRawInt &);
 
 protected:
-	explicit MavlinkStreamGPSRawInt() : MavlinkStream(),
-		gps_time(0)
+	explicit MavlinkStreamGPSRawInt(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_gps_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position))),
+		_gps_time(0)
 	{}
 
-	void subscribe(Mavlink *mavlink)
-	{
-		gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position));
-	}
-
 	void send(const hrt_abstime t)
 	{
 		struct vehicle_gps_position_s gps;
 
-		if (gps_sub->update(&gps_time, &gps)) {
-			mavlink_msg_gps_raw_int_send(_channel,
-						     gps.timestamp_position,
-						     gps.fix_type,
-						     gps.lat,
-						     gps.lon,
-						     gps.alt,
-						     cm_uint16_from_m_float(gps.eph),
-						     cm_uint16_from_m_float(gps.epv),
-						     gps.vel_m_s * 100.0f,
-						     _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
-						     gps.satellites_used);
+		if (_gps_sub->update(&_gps_time, &gps)) {
+			mavlink_gps_raw_int_t msg;
+
+			msg.time_usec = gps.timestamp_position;
+			msg.fix_type = gps.fix_type;
+			msg.lat = gps.lat;
+			msg.lon = gps.lon;
+			msg.alt = gps.alt;
+			msg.eph = cm_uint16_from_m_float(gps.eph);
+			msg.epv = cm_uint16_from_m_float(gps.epv);
+			msg.vel = cm_uint16_from_m_float(gps.vel_m_s),
+		    msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
+		    msg.satellites_visible = gps.satellites_used;
+
+			_mavlink->send_message(MAVLINK_MSG_ID_GPS_RAW_INT, &msg);
 		}
 	}
 };
@@ -714,49 +906,57 @@ public:
 		return MAVLINK_MSG_ID_GLOBAL_POSITION_INT;
 	}
 
-	static MavlinkStream *new_instance()
+	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
-		return new MavlinkStreamGlobalPositionInt();
+		return new MavlinkStreamGlobalPositionInt(mavlink);
+	}
+
+	unsigned get_size()
+	{
+		return MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 	}
 
 private:
-	MavlinkOrbSubscription *pos_sub;
-	uint64_t pos_time;
+	MavlinkOrbSubscription *_pos_sub;
+	uint64_t _pos_time;
+
+	MavlinkOrbSubscription *_home_sub;
+	uint64_t _home_time;
 
-	MavlinkOrbSubscription *home_sub;
-	uint64_t home_time;
+	/* do not allow top copying this class */
+	MavlinkStreamGlobalPositionInt(MavlinkStreamGlobalPositionInt &);
+	MavlinkStreamGlobalPositionInt& operator = (const MavlinkStreamGlobalPositionInt &);
 
 protected:
-	explicit MavlinkStreamGlobalPositionInt() : MavlinkStream(),
-		pos_time(0),
-		home_time(0)
+	explicit MavlinkStreamGlobalPositionInt(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))),
+		_pos_time(0),
+		_home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))),
+		_home_time(0)
 	{}
 
-	void subscribe(Mavlink *mavlink)
-	{
-		pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position));
-		home_sub = mavlink->add_orb_subscription(ORB_ID(home_position));
-	}
-
 	void send(const hrt_abstime t)
 	{
 		struct vehicle_global_position_s pos;
 		struct home_position_s home;
 
-		bool updated = pos_sub->update(&pos_time, &pos);
-		updated |= home_sub->update(&home_time, &home);
+		bool updated = _pos_sub->update(&_pos_time, &pos);
+		updated |= _home_sub->update(&_home_time, &home);
 
 		if (updated) {
-			mavlink_msg_global_position_int_send(_channel,
-							     pos.timestamp / 1000,
-							     pos.lat * 1e7,
-							     pos.lon * 1e7,
-							     pos.alt * 1000.0f,
-							     (pos.alt - home.alt) * 1000.0f,
-							     pos.vel_n * 100.0f,
-							     pos.vel_e * 100.0f,
-							     pos.vel_d * 100.0f,
-							     _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f);
+			mavlink_global_position_int_t msg;
+
+			msg.time_boot_ms = pos.timestamp / 1000;
+			msg.lat = pos.lat * 1e7;
+			msg.lon = pos.lon * 1e7;
+			msg.alt = pos.alt * 1000.0f;
+		    msg.relative_alt = (pos.alt - home.alt) * 1000.0f;
+		    msg.vx = pos.vel_n * 100.0f;
+		    msg.vy = pos.vel_e * 100.0f;
+		    msg.vz = pos.vel_d * 100.0f;
+		    msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f;
+
+			_mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, &msg);
 		}
 	}
 };
@@ -780,44 +980,51 @@ public:
 		return MAVLINK_MSG_ID_LOCAL_POSITION_NED;
 	}
 
-	static MavlinkStream *new_instance()
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamLocalPositionNED(mavlink);
+	}
+
+	unsigned get_size()
 	{
-		return new MavlinkStreamLocalPositionNED();
+		return MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 	}
 
 private:
-	MavlinkOrbSubscription *pos_sub;
-	uint64_t pos_time;
+	MavlinkOrbSubscription *_pos_sub;
+	uint64_t _pos_time;
+
+	/* do not allow top copying this class */
+	MavlinkStreamLocalPositionNED(MavlinkStreamLocalPositionNED &);
+	MavlinkStreamLocalPositionNED& operator = (const MavlinkStreamLocalPositionNED &);
 
 protected:
-	explicit MavlinkStreamLocalPositionNED() : MavlinkStream(),
-		pos_time(0)
+	explicit MavlinkStreamLocalPositionNED(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))),
+		_pos_time(0)
 	{}
 
-	void subscribe(Mavlink *mavlink)
-	{
-		pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position));
-	}
-
 	void send(const hrt_abstime t)
 	{
 		struct vehicle_local_position_s pos;
 
-		if (pos_sub->update(&pos_time, &pos)) {
-			mavlink_msg_local_position_ned_send(_channel,
-							    pos.timestamp / 1000,
-							    pos.x,
-							    pos.y,
-							    pos.z,
-							    pos.vx,
-							    pos.vy,
-							    pos.vz);
+		if (_pos_sub->update(&_pos_time, &pos)) {
+			mavlink_local_position_ned_t msg;
+
+			msg.time_boot_ms = pos.timestamp / 1000;
+			msg.x = pos.x;
+			msg.y = pos.y;
+			msg.z = pos.z;
+		    msg.vx = pos.vx;
+		    msg.vy = pos.vy;
+		    msg.vz = pos.vz;
+
+			_mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED, &msg);
 		}
 	}
 };
 
 
-
 class MavlinkStreamViconPositionEstimate : public MavlinkStream
 {
 public:
@@ -836,38 +1043,46 @@ public:
 		return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE;
 	}
 
-	static MavlinkStream *new_instance()
+	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
-		return new MavlinkStreamViconPositionEstimate();
+		return new MavlinkStreamViconPositionEstimate(mavlink);
+	}
+
+	unsigned get_size()
+	{
+		return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 	}
 
 private:
-	MavlinkOrbSubscription *pos_sub;
-	uint64_t pos_time;
+	MavlinkOrbSubscription *_pos_sub;
+	uint64_t _pos_time;
+
+	/* do not allow top copying this class */
+	MavlinkStreamViconPositionEstimate(MavlinkStreamViconPositionEstimate &);
+	MavlinkStreamViconPositionEstimate& operator = (const MavlinkStreamViconPositionEstimate &);
 
 protected:
-	explicit MavlinkStreamViconPositionEstimate() : MavlinkStream(),
-		pos_time(0)
+	explicit MavlinkStreamViconPositionEstimate(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position))),
+		_pos_time(0)
 	{}
 
-	void subscribe(Mavlink *mavlink)
-	{
-		pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position));
-	}
-
 	void send(const hrt_abstime t)
 	{
 		struct vehicle_vicon_position_s pos;
 
-		if (pos_sub->update(&pos_time, &pos)) {
-			mavlink_msg_vicon_position_estimate_send(_channel,
-								pos.timestamp / 1000,
-								pos.x,
-								pos.y,
-								pos.z,
-								pos.roll,
-								pos.pitch,
-								pos.yaw);
+		if (_pos_sub->update(&_pos_time, &pos)) {
+			mavlink_vicon_position_estimate_t msg;
+
+			msg.usec = pos.timestamp;
+			msg.x = pos.x;
+			msg.y = pos.y;
+			msg.z = pos.z;
+		    msg.roll = pos.roll;
+		    msg.pitch = pos.pitch;
+		    msg.yaw = pos.yaw;
+
+			_mavlink->send_message(MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, &msg);
 		}
 	}
 };
@@ -891,37 +1106,49 @@ public:
 		return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN;
 	}
 
-	static MavlinkStream *new_instance()
+	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
-		return new MavlinkStreamGPSGlobalOrigin();
+		return new MavlinkStreamGPSGlobalOrigin(mavlink);
+	}
+
+	unsigned get_size()
+	{
+		return _home_sub->is_published() ? (MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
 	}
 
 private:
-	MavlinkOrbSubscription *home_sub;
+	MavlinkOrbSubscription *_home_sub;
+
+	/* do not allow top copying this class */
+	MavlinkStreamGPSGlobalOrigin(MavlinkStreamGPSGlobalOrigin &);
+	MavlinkStreamGPSGlobalOrigin& operator = (const MavlinkStreamGPSGlobalOrigin &);
 
 protected:
-	void subscribe(Mavlink *mavlink)
-	{
-		home_sub = mavlink->add_orb_subscription(ORB_ID(home_position));
-	}
+	explicit MavlinkStreamGPSGlobalOrigin(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position)))
+	{}
 
 	void send(const hrt_abstime t)
 	{
 		/* we're sending the GPS home periodically to ensure the
 		 * the GCS does pick it up at one point */
-		if (home_sub->is_published()) {
+		if (_home_sub->is_published()) {
 			struct home_position_s home;
 
-			if (home_sub->update(&home)) {
-				mavlink_msg_gps_global_origin_send(_channel,
-								   (int32_t)(home.lat * 1e7),
-								   (int32_t)(home.lon * 1e7),
-								   (int32_t)(home.alt) * 1000.0f);
+			if (_home_sub->update(&home)) {
+				mavlink_gps_global_origin_t msg;
+
+				msg.latitude = home.lat * 1e7;
+				msg.longitude = home.lon * 1e7;
+				msg.altitude = home.alt * 1e3f;
+
+				_mavlink->send_message(MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, &msg);
 			}
 		}
 	}
 };
 
+
 template <int N>
 class MavlinkStreamServoOutputRaw : public MavlinkStream
 {
@@ -953,21 +1180,28 @@ public:
 		}
 	}
 
-	static MavlinkStream *new_instance()
+	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
-		return new MavlinkStreamServoOutputRaw<N>();
+		return new MavlinkStreamServoOutputRaw<N>(mavlink);
+	}
+
+	unsigned get_size()
+	{
+		return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 	}
 
 private:
-	MavlinkOrbSubscription *act_sub;
-	uint64_t act_time;
+	MavlinkOrbSubscription *_act_sub;
+	uint64_t _act_time;
 
-protected:
-	explicit MavlinkStreamServoOutputRaw() : MavlinkStream(),
-		act_time(0)
-	{}
+	/* do not allow top copying this class */
+	MavlinkStreamServoOutputRaw(MavlinkStreamServoOutputRaw &);
+	MavlinkStreamServoOutputRaw& operator = (const MavlinkStreamServoOutputRaw &);
 
-	void subscribe(Mavlink *mavlink)
+protected:
+	explicit MavlinkStreamServoOutputRaw(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_act_sub(nullptr),
+		_act_time(0)
 	{
 		orb_id_t act_topics[] = {
 			ORB_ID(actuator_outputs_0),
@@ -976,25 +1210,28 @@ protected:
 			ORB_ID(actuator_outputs_3)
 		};
 
-		act_sub = mavlink->add_orb_subscription(act_topics[N]);
+		_act_sub = _mavlink->add_orb_subscription(act_topics[N]);
 	}
 
 	void send(const hrt_abstime t)
 	{
 		struct actuator_outputs_s act;
 
-		if (act_sub->update(&act_time, &act)) {
-			mavlink_msg_servo_output_raw_send(_channel,
-							  act.timestamp / 1000,
-							  N,
-							  act.output[0],
-							  act.output[1],
-							  act.output[2],
-							  act.output[3],
-							  act.output[4],
-							  act.output[5],
-							  act.output[6],
-							  act.output[7]);
+		if (_act_sub->update(&_act_time, &act)) {
+			mavlink_servo_output_raw_t msg;
+
+			msg.time_usec = act.timestamp;
+			msg.port = N;
+			msg.servo1_raw = act.output[0];
+			msg.servo2_raw = act.output[1];
+			msg.servo3_raw = act.output[2];
+			msg.servo4_raw = act.output[3];
+			msg.servo5_raw = act.output[4];
+			msg.servo6_raw = act.output[5];
+			msg.servo7_raw = act.output[6];
+			msg.servo8_raw = act.output[7];
+
+			_mavlink->send_message(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, &msg);
 		}
 	}
 };
@@ -1018,44 +1255,49 @@ public:
 		return MAVLINK_MSG_ID_HIL_CONTROLS;
 	}
 
-	static MavlinkStream *new_instance()
+	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
-		return new MavlinkStreamHILControls();
+		return new MavlinkStreamHILControls(mavlink);
+	}
+
+	unsigned get_size()
+	{
+		return MAVLINK_MSG_ID_HIL_CONTROLS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 	}
 
 private:
-	MavlinkOrbSubscription *status_sub;
-	uint64_t status_time;
+	MavlinkOrbSubscription *_status_sub;
+	uint64_t _status_time;
+
+	MavlinkOrbSubscription *_pos_sp_triplet_sub;
+	uint64_t _pos_sp_triplet_time;
 
-	MavlinkOrbSubscription *pos_sp_triplet_sub;
-	uint64_t pos_sp_triplet_time;
+	MavlinkOrbSubscription *_act_sub;
+	uint64_t _act_time;
 
-	MavlinkOrbSubscription *act_sub;
-	uint64_t act_time;
+	/* do not allow top copying this class */
+	MavlinkStreamHILControls(MavlinkStreamHILControls &);
+	MavlinkStreamHILControls& operator = (const MavlinkStreamHILControls &);
 
 protected:
-	explicit MavlinkStreamHILControls() : MavlinkStream(),
-		status_time(0),
-		pos_sp_triplet_time(0),
-		act_time(0)
+	explicit MavlinkStreamHILControls(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))),
+		_status_time(0),
+		_pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))),
+		_pos_sp_triplet_time(0),
+		_act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0))),
+		_act_time(0)
 	{}
 
-	void subscribe(Mavlink *mavlink)
-	{
-		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
-		pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet));
-		act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0));
-	}
-
 	void send(const hrt_abstime t)
 	{
 		struct vehicle_status_s status;
 		struct position_setpoint_triplet_s pos_sp_triplet;
 		struct actuator_outputs_s act;
 
-		bool updated = act_sub->update(&act_time, &act);
-		updated |= pos_sp_triplet_sub->update(&pos_sp_triplet_time, &pos_sp_triplet);
-		updated |= status_sub->update(&status_time, &status);
+		bool updated = _act_sub->update(&_act_time, &act);
+		updated |= _pos_sp_triplet_sub->update(&_pos_sp_triplet_time, &pos_sp_triplet);
+		updated |= _status_sub->update(&_status_time, &status);
 
 		if (updated && (status.arming_state == ARMING_STATE_ARMED)) {
 			/* translate the current syste state to mavlink state and mode */
@@ -1123,11 +1365,21 @@ protected:
 				}
 			}
 
-			mavlink_msg_hil_controls_send(_channel,
-						      hrt_absolute_time(),
-						      out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
-						      mavlink_base_mode,
-						      0);
+			mavlink_hil_controls_t msg;
+
+			msg.time_usec = hrt_absolute_time();
+			msg.roll_ailerons = out[0];
+			msg.pitch_elevator = out[1];
+			msg.yaw_rudder = out[2];
+			msg.throttle = out[3];
+			msg.aux1 = out[4];
+			msg.aux2 = out[5];
+			msg.aux3 = out[6];
+			msg.aux4 = out[7];
+			msg.mode = mavlink_base_mode;
+			msg.nav_mode = 0;
+
+			_mavlink->send_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg);
 		}
 	}
 };
@@ -1151,31 +1403,42 @@ public:
 		return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT;
 	}
 
-	static MavlinkStream *new_instance()
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamGlobalPositionSetpointInt(mavlink);
+	}
+
+	unsigned get_size()
 	{
-		return new MavlinkStreamGlobalPositionSetpointInt();
+		return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 	}
 
 private:
-	MavlinkOrbSubscription *pos_sp_triplet_sub;
+	MavlinkOrbSubscription *_pos_sp_triplet_sub;
+
+	/* do not allow top copying this class */
+	MavlinkStreamGlobalPositionSetpointInt(MavlinkStreamGlobalPositionSetpointInt &);
+	MavlinkStreamGlobalPositionSetpointInt& operator = (const MavlinkStreamGlobalPositionSetpointInt &);
 
 protected:
-	void subscribe(Mavlink *mavlink)
-	{
-		pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet));
-	}
+	explicit MavlinkStreamGlobalPositionSetpointInt(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)))
+	{}
 
 	void send(const hrt_abstime t)
 	{
 		struct position_setpoint_triplet_s pos_sp_triplet;
 
-		if (pos_sp_triplet_sub->update(&pos_sp_triplet)) {
-			mavlink_msg_global_position_setpoint_int_send(_channel,
-					MAV_FRAME_GLOBAL,
-					(int32_t)(pos_sp_triplet.current.lat * 1e7),
-					(int32_t)(pos_sp_triplet.current.lon * 1e7),
-					(int32_t)(pos_sp_triplet.current.alt * 1000),
-					(int16_t)(pos_sp_triplet.current.yaw * M_RAD_TO_DEG_F * 100.0f));
+		if (_pos_sp_triplet_sub->update(&pos_sp_triplet)) {
+			mavlink_global_position_setpoint_int_t msg;
+
+			msg.coordinate_frame = MAV_FRAME_GLOBAL;
+			msg.latitude = pos_sp_triplet.current.lat * 1e7;
+			msg.longitude = pos_sp_triplet.current.lon * 1e7;
+			msg.altitude = pos_sp_triplet.current.alt * 1000;
+			msg.yaw = pos_sp_triplet.current.yaw * M_RAD_TO_DEG_F * 100.0f;
+
+			_mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, &msg);
 		}
 	}
 };
@@ -1199,36 +1462,44 @@ public:
 		return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT;
 	}
 
-	static MavlinkStream *new_instance()
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamLocalPositionSetpoint(mavlink);
+	}
+
+	unsigned get_size()
 	{
-		return new MavlinkStreamLocalPositionSetpoint();
+		return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 	}
 
 private:
-	MavlinkOrbSubscription *pos_sp_sub;
-	uint64_t pos_sp_time;
+	MavlinkOrbSubscription *_pos_sp_sub;
+	uint64_t _pos_sp_time;
+
+	/* do not allow top copying this class */
+	MavlinkStreamLocalPositionSetpoint(MavlinkStreamLocalPositionSetpoint &);
+	MavlinkStreamLocalPositionSetpoint& operator = (const MavlinkStreamLocalPositionSetpoint &);
 
 protected:
-	explicit MavlinkStreamLocalPositionSetpoint() : MavlinkStream(),
-		pos_sp_time(0)
+	explicit MavlinkStreamLocalPositionSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_pos_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint))),
+		_pos_sp_time(0)
 	{}
 
-	void subscribe(Mavlink *mavlink)
-	{
-		pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint));
-	}
-
 	void send(const hrt_abstime t)
 	{
 		struct vehicle_local_position_setpoint_s pos_sp;
 
-		if (pos_sp_sub->update(&pos_sp_time, &pos_sp)) {
-			mavlink_msg_local_position_setpoint_send(_channel,
-					MAV_FRAME_LOCAL_NED,
-					pos_sp.x,
-					pos_sp.y,
-					pos_sp.z,
-					pos_sp.yaw);
+		if (_pos_sp_sub->update(&_pos_sp_time, &pos_sp)) {
+			mavlink_local_position_setpoint_t msg;
+
+			msg.coordinate_frame = MAV_FRAME_LOCAL_NED;
+			msg.x = pos_sp.x;
+			msg.y = pos_sp.y;
+			msg.z = pos_sp.z;
+			msg.yaw = pos_sp.yaw;
+
+			_mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, &msg);
 		}
 	}
 };
@@ -1252,36 +1523,44 @@ public:
 		return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT;
 	}
 
-	static MavlinkStream *new_instance()
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamRollPitchYawThrustSetpoint(mavlink);
+	}
+
+	unsigned get_size()
 	{
-		return new MavlinkStreamRollPitchYawThrustSetpoint();
+		return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 	}
 
 private:
 	MavlinkOrbSubscription *att_sp_sub;
 	uint64_t att_sp_time;
 
+	/* do not allow top copying this class */
+	MavlinkStreamRollPitchYawThrustSetpoint(MavlinkStreamRollPitchYawThrustSetpoint &);
+	MavlinkStreamRollPitchYawThrustSetpoint& operator = (const MavlinkStreamRollPitchYawThrustSetpoint &);
+
 protected:
-	explicit MavlinkStreamRollPitchYawThrustSetpoint() : MavlinkStream(),
+	explicit MavlinkStreamRollPitchYawThrustSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink),
+		att_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint))),
 		att_sp_time(0)
 	{}
 
-	void subscribe(Mavlink *mavlink)
-	{
-		att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint));
-	}
-
 	void send(const hrt_abstime t)
 	{
 		struct vehicle_attitude_setpoint_s att_sp;
 
 		if (att_sp_sub->update(&att_sp_time, &att_sp)) {
-			mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel,
-					att_sp.timestamp / 1000,
-					att_sp.roll_body,
-					att_sp.pitch_body,
-					att_sp.yaw_body,
-					att_sp.thrust);
+			mavlink_roll_pitch_yaw_thrust_setpoint_t msg;
+
+			msg.time_boot_ms = att_sp.timestamp / 1000;
+			msg.roll = att_sp.roll_body;
+			msg.pitch = att_sp.pitch_body;
+			msg.yaw = att_sp.yaw_body;
+			msg.thrust = att_sp.thrust;
+
+			_mavlink->send_message(MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, &msg);
 		}
 	}
 };
@@ -1305,36 +1584,44 @@ public:
 		return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT;
 	}
 
-	static MavlinkStream *new_instance()
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamRollPitchYawRatesThrustSetpoint(mavlink);
+	}
+
+	unsigned get_size()
 	{
-		return new MavlinkStreamRollPitchYawRatesThrustSetpoint();
+		return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 	}
 
 private:
-	MavlinkOrbSubscription *att_rates_sp_sub;
-	uint64_t att_rates_sp_time;
+	MavlinkOrbSubscription *_att_rates_sp_sub;
+	uint64_t _att_rates_sp_time;
+
+	/* do not allow top copying this class */
+	MavlinkStreamRollPitchYawRatesThrustSetpoint(MavlinkStreamRollPitchYawRatesThrustSetpoint &);
+	MavlinkStreamRollPitchYawRatesThrustSetpoint& operator = (const MavlinkStreamRollPitchYawRatesThrustSetpoint &);
 
 protected:
-	explicit MavlinkStreamRollPitchYawRatesThrustSetpoint() : MavlinkStream(),
-		att_rates_sp_time(0)
+	explicit MavlinkStreamRollPitchYawRatesThrustSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_att_rates_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint))),
+		_att_rates_sp_time(0)
 	{}
 
-	void subscribe(Mavlink *mavlink)
-	{
-		att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint));
-	}
-
 	void send(const hrt_abstime t)
 	{
 		struct vehicle_rates_setpoint_s att_rates_sp;
 
-		if (att_rates_sp_sub->update(&att_rates_sp_time, &att_rates_sp)) {
-			mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel,
-					att_rates_sp.timestamp / 1000,
-					att_rates_sp.roll,
-					att_rates_sp.pitch,
-					att_rates_sp.yaw,
-					att_rates_sp.thrust);
+		if (_att_rates_sp_sub->update(&_att_rates_sp_time, &att_rates_sp)) {
+			mavlink_roll_pitch_yaw_rates_thrust_setpoint_t msg;
+
+			msg.time_boot_ms = att_rates_sp.timestamp / 1000;
+			msg.roll_rate = att_rates_sp.roll;
+			msg.pitch_rate = att_rates_sp.pitch;
+			msg.yaw_rate = att_rates_sp.yaw;
+			msg.thrust = att_rates_sp.thrust;
+
+			_mavlink->send_message(MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, &msg);
 		}
 	}
 };
@@ -1358,72 +1645,84 @@ public:
 		return MAVLINK_MSG_ID_RC_CHANNELS_RAW;
 	}
 
-	static MavlinkStream *new_instance()
+	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
-		return new MavlinkStreamRCChannelsRaw();
+		return new MavlinkStreamRCChannelsRaw(mavlink);
+	}
+
+	unsigned get_size()
+	{
+		return _rc_sub->is_published() ? ((RC_INPUT_MAX_CHANNELS / 8) * (MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) +
+				MAVLINK_MSG_ID_RC_CHANNELS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
 	}
 
 private:
-	MavlinkOrbSubscription *rc_sub;
-	uint64_t rc_time;
+	MavlinkOrbSubscription *_rc_sub;
+	uint64_t _rc_time;
+
+	/* do not allow top copying this class */
+	MavlinkStreamRCChannelsRaw(MavlinkStreamRCChannelsRaw &);
+	MavlinkStreamRCChannelsRaw& operator = (const MavlinkStreamRCChannelsRaw &);
 
 protected:
-	explicit MavlinkStreamRCChannelsRaw() : MavlinkStream(),
-		rc_time(0)
+	explicit MavlinkStreamRCChannelsRaw(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_rc_sub(_mavlink->add_orb_subscription(ORB_ID(input_rc))),
+		_rc_time(0)
 	{}
 
-	void subscribe(Mavlink *mavlink)
-	{
-		rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc));
-	}
-
 	void send(const hrt_abstime t)
 	{
 		struct rc_input_values rc;
 
-		if (rc_sub->update(&rc_time, &rc)) {
+		if (_rc_sub->update(&_rc_time, &rc)) {
 			const unsigned port_width = 8;
 
-			// Deprecated message (but still needed for compatibility!)
+			/* deprecated message (but still needed for compatibility!) */
 			for (unsigned i = 0; (i * port_width) < rc.channel_count; i++) {
-				/* Channels are sent in MAVLink main loop at a fixed interval */
-				mavlink_msg_rc_channels_raw_send(_channel,
-								 rc.timestamp_publication / 1000,
-								 i,
-								 (rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX,
-								 (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX,
-								 (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX,
-								 (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX,
-								 (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX,
-								 (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX,
-								 (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX,
-								 (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX,
-								 rc.rssi);
+				/* channels are sent in MAVLink main loop at a fixed interval */
+				mavlink_rc_channels_raw_t msg;
+
+				msg.time_boot_ms = rc.timestamp_publication / 1000;
+				msg.port = i;
+				msg.chan1_raw = (rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX;
+				msg.chan2_raw = (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX;
+				msg.chan3_raw = (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX;
+				msg.chan4_raw = (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX;
+				msg.chan4_raw = (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX;
+				msg.chan4_raw = (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX;
+				msg.chan4_raw = (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX;
+				msg.chan4_raw = (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX;
+				msg.rssi = rc.rssi;
+
+				_mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS_RAW, &msg);
 			}
 
-			// New message
-			mavlink_msg_rc_channels_send(_channel,
-					rc.timestamp_publication / 1000,
-					rc.channel_count,
-					((rc.channel_count > 0) ? rc.values[0] : UINT16_MAX),
-					((rc.channel_count > 1) ? rc.values[1] : UINT16_MAX),
-					((rc.channel_count > 2) ? rc.values[2] : UINT16_MAX),
-					((rc.channel_count > 3) ? rc.values[3] : UINT16_MAX),
-					((rc.channel_count > 4) ? rc.values[4] : UINT16_MAX),
-					((rc.channel_count > 5) ? rc.values[5] : UINT16_MAX),
-					((rc.channel_count > 6) ? rc.values[6] : UINT16_MAX),
-					((rc.channel_count > 7) ? rc.values[7] : UINT16_MAX),
-					((rc.channel_count > 8) ? rc.values[8] : UINT16_MAX),
-					((rc.channel_count > 9) ? rc.values[9] : UINT16_MAX),
-					((rc.channel_count > 10) ? rc.values[10] : UINT16_MAX),
-					((rc.channel_count > 11) ? rc.values[11] : UINT16_MAX),
-					((rc.channel_count > 12) ? rc.values[12] : UINT16_MAX),
-					((rc.channel_count > 13) ? rc.values[13] : UINT16_MAX),
-					((rc.channel_count > 14) ? rc.values[14] : UINT16_MAX),
-					((rc.channel_count > 15) ? rc.values[15] : UINT16_MAX),
-					((rc.channel_count > 16) ? rc.values[16] : UINT16_MAX),
-					((rc.channel_count > 17) ? rc.values[17] : UINT16_MAX),
-					rc.rssi);
+			/* new message */
+			mavlink_rc_channels_t msg;
+
+			msg.time_boot_ms = rc.timestamp_publication / 1000;
+			msg.chancount = rc.channel_count;
+			msg.chan1_raw = (rc.channel_count > 0) ? rc.values[0] : UINT16_MAX;
+			msg.chan2_raw = (rc.channel_count > 1) ? rc.values[1] : UINT16_MAX;
+			msg.chan3_raw = (rc.channel_count > 2) ? rc.values[2] : UINT16_MAX;
+			msg.chan4_raw = (rc.channel_count > 3) ? rc.values[3] : UINT16_MAX;
+			msg.chan5_raw = (rc.channel_count > 4) ? rc.values[4] : UINT16_MAX;
+			msg.chan6_raw = (rc.channel_count > 5) ? rc.values[5] : UINT16_MAX;
+			msg.chan7_raw = (rc.channel_count > 6) ? rc.values[6] : UINT16_MAX;
+			msg.chan8_raw = (rc.channel_count > 7) ? rc.values[7] : UINT16_MAX;
+			msg.chan9_raw = (rc.channel_count > 8) ? rc.values[8] : UINT16_MAX;
+			msg.chan10_raw = (rc.channel_count > 9) ? rc.values[9] : UINT16_MAX;
+			msg.chan11_raw = (rc.channel_count > 10) ? rc.values[10] : UINT16_MAX;
+			msg.chan12_raw = (rc.channel_count > 11) ? rc.values[11] : UINT16_MAX;
+			msg.chan13_raw = (rc.channel_count > 12) ? rc.values[12] : UINT16_MAX;
+			msg.chan14_raw = (rc.channel_count > 13) ? rc.values[13] : UINT16_MAX;
+			msg.chan15_raw = (rc.channel_count > 14) ? rc.values[14] : UINT16_MAX;
+			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;
+
+			_mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg);
 		}
 	}
 };
@@ -1447,37 +1746,45 @@ public:
 		return MAVLINK_MSG_ID_MANUAL_CONTROL;
 	}
 
-	static MavlinkStream *new_instance()
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamManualControl(mavlink);
+	}
+
+	unsigned get_size()
 	{
-		return new MavlinkStreamManualControl();
+		return _manual_sub->is_published() ? (MAVLINK_MSG_ID_MANUAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
 	}
 
 private:
-	MavlinkOrbSubscription *manual_sub;
-	uint64_t manual_time;
+	MavlinkOrbSubscription *_manual_sub;
+	uint64_t _manual_time;
+
+	/* do not allow top copying this class */
+	MavlinkStreamManualControl(MavlinkStreamManualControl &);
+	MavlinkStreamManualControl& operator = (const MavlinkStreamManualControl &);
 
 protected:
-	explicit MavlinkStreamManualControl() : MavlinkStream(),
-		manual_time(0)
+	explicit MavlinkStreamManualControl(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_manual_sub(_mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint))),
+		_manual_time(0)
 	{}
 
-	void subscribe(Mavlink *mavlink)
-	{
-		manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint));
-	}
-
 	void send(const hrt_abstime t)
 	{
 		struct manual_control_setpoint_s manual;
 
-		if (manual_sub->update(&manual_time, &manual)) {
-			mavlink_msg_manual_control_send(_channel,
-							mavlink_system.sysid,
-							manual.x * 1000,
-							manual.y * 1000,
-							manual.z * 1000,
-							manual.r * 1000,
-							0);
+		if (_manual_sub->update(&_manual_time, &manual)) {
+			mavlink_manual_control_t msg;
+
+			msg.target = mavlink_system.sysid;
+			msg.x = manual.x * 1000;
+			msg.y = manual.y * 1000;
+			msg.z = manual.z * 1000;
+			msg.r = manual.r * 1000;
+			msg.buttons = 0;
+
+			_mavlink->send_message(MAVLINK_MSG_ID_MANUAL_CONTROL, &msg);
 		}
 	}
 };
@@ -1501,37 +1808,47 @@ public:
 		return MAVLINK_MSG_ID_OPTICAL_FLOW;
 	}
 
-	static MavlinkStream *new_instance()
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamOpticalFlow(mavlink);
+	}
+
+	unsigned get_size()
 	{
-		return new MavlinkStreamOpticalFlow();
+		return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
 	}
 
 private:
-	MavlinkOrbSubscription *flow_sub;
-	uint64_t flow_time;
+	MavlinkOrbSubscription *_flow_sub;
+	uint64_t _flow_time;
+
+	/* do not allow top copying this class */
+	MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &);
+	MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &);
 
 protected:
-	explicit MavlinkStreamOpticalFlow() : MavlinkStream(),
-		flow_time(0)
+	explicit MavlinkStreamOpticalFlow(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_flow_sub(_mavlink->add_orb_subscription(ORB_ID(optical_flow))),
+		_flow_time(0)
 	{}
 
-	void subscribe(Mavlink *mavlink)
-	{
-		flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow));
-	}
-
 	void send(const hrt_abstime t)
 	{
 		struct optical_flow_s flow;
 
-		if (flow_sub->update(&flow_time, &flow)) {
-			mavlink_msg_optical_flow_send(_channel,
-						      flow.timestamp,
-						      flow.sensor_id,
-						      flow.flow_raw_x, flow.flow_raw_y,
-						      flow.flow_comp_x_m, flow.flow_comp_y_m,
-						      flow.quality,
-						      flow.ground_distance_m);
+		if (_flow_sub->update(&_flow_time, &flow)) {
+			mavlink_optical_flow_t msg;
+
+			msg.time_usec = flow.timestamp;
+			msg.sensor_id = flow.sensor_id;
+			msg.flow_x = flow.flow_raw_x;
+			msg.flow_y = flow.flow_raw_y;
+			msg.flow_comp_m_x = flow.flow_comp_x_m;
+			msg.flow_comp_m_y = flow.flow_comp_y_m;
+			msg.quality = flow.quality;
+			msg.ground_distance = flow.ground_distance_m;
+
+			_mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW, &msg);
 		}
 	}
 };
@@ -1554,51 +1871,64 @@ public:
 		return 0;
 	}
 
-	static MavlinkStream *new_instance()
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamAttitudeControls(mavlink);
+	}
+
+	unsigned get_size()
 	{
-		return new MavlinkStreamAttitudeControls();
+		return 4 * (MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES);
 	}
 
 private:
-	MavlinkOrbSubscription *att_ctrl_sub;
-	uint64_t att_ctrl_time;
+	MavlinkOrbSubscription *_att_ctrl_sub;
+	uint64_t _att_ctrl_time;
+
+	/* do not allow top copying this class */
+	MavlinkStreamAttitudeControls(MavlinkStreamAttitudeControls &);
+	MavlinkStreamAttitudeControls& operator = (const MavlinkStreamAttitudeControls &);
 
 protected:
-	explicit MavlinkStreamAttitudeControls() : MavlinkStream(),
-		att_ctrl_time(0)
+	explicit MavlinkStreamAttitudeControls(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_att_ctrl_sub(_mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS)),
+		_att_ctrl_time(0)
 	{}
 
-	void subscribe(Mavlink *mavlink)
-	{
-		att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
-	}
-
 	void send(const hrt_abstime t)
 	{
 		struct actuator_controls_s att_ctrl;
 
-		if (att_ctrl_sub->update(&att_ctrl_time, &att_ctrl)) {
+		if (_att_ctrl_sub->update(&_att_ctrl_time, &att_ctrl)) {
 			/* send, add spaces so that string buffer is at least 10 chars long */
-			mavlink_msg_named_value_float_send(_channel,
-							   att_ctrl.timestamp / 1000,
-							   "rll ctrl    ",
-							   att_ctrl.control[0]);
-			mavlink_msg_named_value_float_send(_channel,
-							   att_ctrl.timestamp / 1000,
-							   "ptch ctrl    ",
-							   att_ctrl.control[1]);
-			mavlink_msg_named_value_float_send(_channel,
-							   att_ctrl.timestamp / 1000,
-							   "yaw ctrl     ",
-							   att_ctrl.control[2]);
-			mavlink_msg_named_value_float_send(_channel,
-							   att_ctrl.timestamp / 1000,
-							   "thr ctrl     ",
-							   att_ctrl.control[3]);
+			mavlink_named_value_float_t msg;
+
+			msg.time_boot_ms = att_ctrl.timestamp / 1000;
+
+			snprintf(msg.name, sizeof(msg.name), "rll ctrl");
+			msg.value = att_ctrl.control[0];
+
+			_mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg);
+
+			snprintf(msg.name, sizeof(msg.name), "ptch ctrl");
+			msg.value = att_ctrl.control[1];
+
+			_mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg);
+
+			snprintf(msg.name, sizeof(msg.name), "yaw ctrl");
+			msg.value = att_ctrl.control[2];
+
+			_mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg);
+
+			snprintf(msg.name, sizeof(msg.name), "thr ctrl");
+			msg.value = att_ctrl.control[3];
+
+			_mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg);
 		}
 	}
 };
 
+
 class MavlinkStreamNamedValueFloat : public MavlinkStream
 {
 public:
@@ -1617,41 +1947,49 @@ public:
 		return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT;
 	}
 
-	static MavlinkStream *new_instance()
+	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
-		return new MavlinkStreamNamedValueFloat();
+		return new MavlinkStreamNamedValueFloat(mavlink);
+	}
+
+	unsigned get_size()
+	{
+		return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 	}
 
 private:
-	MavlinkOrbSubscription *debug_sub;
-	uint64_t debug_time;
+	MavlinkOrbSubscription *_debug_sub;
+	uint64_t _debug_time;
+
+	/* do not allow top copying this class */
+	MavlinkStreamNamedValueFloat(MavlinkStreamNamedValueFloat &);
+	MavlinkStreamNamedValueFloat& operator = (const MavlinkStreamNamedValueFloat &);
 
 protected:
-	explicit MavlinkStreamNamedValueFloat() : MavlinkStream(),
-		debug_time(0)
+	explicit MavlinkStreamNamedValueFloat(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_debug_sub(_mavlink->add_orb_subscription(ORB_ID(debug_key_value))),
+		_debug_time(0)
 	{}
 
-	void subscribe(Mavlink *mavlink)
-	{
-		debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value));
-	}
-
 	void send(const hrt_abstime t)
 	{
 		struct debug_key_value_s debug;
 
-		if (debug_sub->update(&debug_time, &debug)) {
+		if (_debug_sub->update(&_debug_time, &debug)) {
+			mavlink_named_value_float_t msg;
+
+			msg.time_boot_ms = debug.timestamp_ms;
+			memcpy(msg.name, debug.key, sizeof(msg.name));
 			/* enforce null termination */
-			debug.key[sizeof(debug.key) - 1] = '\0';
+			msg.name[sizeof(msg.name) - 1] = '\0';
+			msg.value = debug.value;
 
-			mavlink_msg_named_value_float_send(_channel,
-							   debug.timestamp_ms,
-							   debug.key,
-							   debug.value);
+			_mavlink->send_message(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, &msg);
 		}
 	}
 };
 
+
 class MavlinkStreamCameraCapture : public MavlinkStream
 {
 public:
@@ -1670,38 +2008,53 @@ public:
 		return 0;
 	}
 
-	static MavlinkStream *new_instance()
+	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
-		return new MavlinkStreamCameraCapture();
+		return new MavlinkStreamCameraCapture(mavlink);
+	}
+
+	unsigned get_size()
+	{
+		return MAVLINK_MSG_ID_COMMAND_LONG_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 	}
 
 private:
-	MavlinkOrbSubscription *status_sub;
+	MavlinkOrbSubscription *_status_sub;
+
+	/* do not allow top copying this class */
+	MavlinkStreamCameraCapture(MavlinkStreamCameraCapture &);
+	MavlinkStreamCameraCapture& operator = (const MavlinkStreamCameraCapture &);
 
 protected:
-	void subscribe(Mavlink *mavlink)
-	{
-		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
-	}
+	explicit MavlinkStreamCameraCapture(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status)))
+	{}
 
 	void send(const hrt_abstime t)
 	{
 		struct vehicle_status_s status;
-		(void)status_sub->update(&status);
+		(void)_status_sub->update(&status);
 
-		if (status.arming_state == ARMING_STATE_ARMED
-		    || status.arming_state == ARMING_STATE_ARMED_ERROR) {
+		mavlink_command_long_t msg;
 
-			/* send camera capture on */
-			mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0);
+		msg.target_system = mavlink_system.sysid;
+		msg.target_component = MAV_COMP_ID_ALL;
+		msg.command = MAV_CMD_DO_CONTROL_VIDEO;
+		msg.confirmation = 0;
+		msg.param1 = 0;
+		msg.param2 = 0;
+		msg.param3 = 0;
+		/* set camera capture ON/OFF depending on arming state */
+		msg.param4 = (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) ? 1 : 0;
+		msg.param5 = 0;
+		msg.param6 = 0;
+		msg.param7 = 0;
 
-		} else {
-			/* send camera capture off */
-			mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0);
-		}
+		_mavlink->send_message(MAVLINK_MSG_ID_COMMAND_LONG, &msg);
 	}
 };
 
+
 class MavlinkStreamDistanceSensor : public MavlinkStream
 {
 public:
@@ -1720,45 +2073,58 @@ public:
 		return MAVLINK_MSG_ID_DISTANCE_SENSOR;
 	}
 
-	static MavlinkStream *new_instance()
+	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
-		return new MavlinkStreamDistanceSensor();
+		return new MavlinkStreamDistanceSensor(mavlink);
+	}
+
+	unsigned get_size()
+	{
+		return _range_sub->is_published() ? (MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
 	}
 
 private:
-	MavlinkOrbSubscription *range_sub;
-	uint64_t range_time;
+	MavlinkOrbSubscription *_range_sub;
+	uint64_t _range_time;
+
+	/* do not allow top copying this class */
+	MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &);
+	MavlinkStreamDistanceSensor& operator = (const MavlinkStreamDistanceSensor &);
 
 protected:
-	explicit MavlinkStreamDistanceSensor() : MavlinkStream(),
-		range_time(0)
+	explicit MavlinkStreamDistanceSensor(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_range_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_range_finder))),
+		_range_time(0)
 	{}
 
-	void subscribe(Mavlink *mavlink)
-	{
-		range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder));
-	}
-
 	void send(const hrt_abstime t)
 	{
 		struct range_finder_report range;
 
-		if (range_sub->update(&range_time, &range)) {
+		if (_range_sub->update(&_range_time, &range)) {
+
+			mavlink_distance_sensor_t msg;
 
-			uint8_t type;
+			msg.time_boot_ms = range.timestamp / 1000;
 
 			switch (range.type) {
-				case RANGE_FINDER_TYPE_LASER:
-				type = MAV_DISTANCE_SENSOR_LASER;
+			case RANGE_FINDER_TYPE_LASER:
+				msg.type = MAV_DISTANCE_SENSOR_LASER;
+				break;
+
+			default:
+				msg.type = MAV_DISTANCE_SENSOR_LASER;
 				break;
 			}
 
-			uint8_t id = 0;
-			uint8_t orientation = 0;
-			uint8_t covariance = 20;
+			msg.id = 0;
+			msg.orientation = 0;
+			msg.min_distance = range.minimum_distance * 100;
+			msg.max_distance = range.minimum_distance * 100;
+			msg.current_distance = range.distance * 100;
+			msg.covariance = 20;
 
-			mavlink_msg_distance_sensor_send(_channel, range.timestamp / 1000, type, id, orientation,
-				range.minimum_distance*100, range.maximum_distance*100, range.distance*100, covariance);
+			_mavlink->send_message(MAVLINK_MSG_ID_DISTANCE_SENSOR, &msg);
 		}
 	}
 };
@@ -1766,6 +2132,8 @@ protected:
 
 StreamListItem *streams_list[] = {
 	new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static),
+	new StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static),
+	new StreamListItem(&MavlinkStreamCommandLong::new_instance, &MavlinkStreamCommandLong::get_name_static),
 	new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static),
 	new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static),
 	new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static),
@@ -1774,6 +2142,7 @@ StreamListItem *streams_list[] = {
 	new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static),
 	new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static),
 	new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static),
+	new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static),
 	new StreamListItem(&MavlinkStreamGPSGlobalOrigin::new_instance, &MavlinkStreamGPSGlobalOrigin::get_name_static),
 	new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static),
 	new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static),
@@ -1791,6 +2160,5 @@ StreamListItem *streams_list[] = {
 	new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static),
 	new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static),
 	new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static),
-	new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static),
 	nullptr
 };
diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h
index ee64d0e42..7e4416609 100644
--- a/src/modules/mavlink/mavlink_messages.h
+++ b/src/modules/mavlink/mavlink_messages.h
@@ -46,10 +46,10 @@
 class StreamListItem {
 
 public:
-	MavlinkStream* (*new_instance)();
+	MavlinkStream* (*new_instance)(Mavlink *mavlink);
 	const char* (*get_name)();
 
-	StreamListItem(MavlinkStream* (*inst)(), const char* (*name)()) :
+	StreamListItem(MavlinkStream* (*inst)(Mavlink *mavlink), const char* (*name)()) :
 		new_instance(inst),
 		get_name(name) {};
 
diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp
index 068774c47..7a761588a 100644
--- a/src/modules/mavlink/mavlink_mission.cpp
+++ b/src/modules/mavlink/mavlink_mission.cpp
@@ -59,8 +59,7 @@
 static const int ERROR = -1;
 
 
-MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) :
-	_mavlink(mavlink),
+MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(mavlink),
 	_state(MAVLINK_WPM_STATE_IDLE),
 	_time_last_recv(0),
 	_time_last_sent(0),
@@ -79,9 +78,8 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) :
 	_offboard_mission_sub(-1),
 	_mission_result_sub(-1),
 	_offboard_mission_pub(-1),
-	_slow_rate_limiter(2000000.0f / mavlink->get_rate_mult()),
+	_slow_rate_limiter(_interval / 10.0f),
 	_verbose(false),
-	_channel(mavlink->get_channel()),
 	_comp_id(MAV_COMP_ID_MISSIONPLANNER)
 {
 	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
@@ -96,6 +94,20 @@ MavlinkMissionManager::~MavlinkMissionManager()
 	close(_mission_result_sub);
 }
 
+unsigned
+MavlinkMissionManager::get_size()
+{
+	if (_state == MAVLINK_WPM_STATE_SENDLIST) {
+		return MAVLINK_MSG_ID_MISSION_ITEM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+
+	} else if (_state == MAVLINK_WPM_STATE_GETLIST) {
+		return MAVLINK_MSG_ID_MISSION_REQUEST + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+
+	} else {
+		return 0;
+	}
+}
+
 void
 MavlinkMissionManager::init_offboard_mission()
 {
@@ -157,15 +169,13 @@ MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int
 void
 MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type)
 {
-	mavlink_message_t msg;
 	mavlink_mission_ack_t wpa;
 
 	wpa.target_system = sysid;
 	wpa.target_component = compid;
 	wpa.type = type;
 
-	mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpa);
-	_mavlink->send_message(&msg);
+	_mavlink->send_message(MAVLINK_MSG_ID_MISSION_ACK, &wpa);
 
 	if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); }
 }
@@ -175,13 +185,11 @@ void
 MavlinkMissionManager::send_mission_current(uint16_t seq)
 {
 	if (seq < _count) {
-		mavlink_message_t msg;
 		mavlink_mission_current_t wpc;
 
 		wpc.seq = seq;
 
-		mavlink_msg_mission_current_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc);
-		_mavlink->send_message(&msg);
+		_mavlink->send_message(MAVLINK_MSG_ID_MISSION_CURRENT, &wpc);
 
 	} else if (seq == 0 && _count == 0) {
 		/* don't broadcast if no WPs */
@@ -199,15 +207,13 @@ MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_
 {
 	_time_last_sent = hrt_absolute_time();
 
-	mavlink_message_t msg;
 	mavlink_mission_count_t wpc;
 
 	wpc.target_system = sysid;
 	wpc.target_component = compid;
 	wpc.count = _count;
 
-	mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc);
-	_mavlink->send_message(&msg);
+	_mavlink->send_message(MAVLINK_MSG_ID_MISSION_COUNT, &wpc);
 
 	if (_verbose) { warnx("WPM: Send MISSION_COUNT %u to ID %u", wpc.count, wpc.target_system); }
 }
@@ -226,13 +232,12 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
 		mavlink_mission_item_t wp;
 		format_mavlink_mission_item(&mission_item, &wp);
 
-		mavlink_message_t msg;
 		wp.target_system = sysid;
 		wp.target_component = compid;
 		wp.seq = seq;
 		wp.current = (_current_seq == seq) ? 1 : 0;
-		mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp);
-		_mavlink->send_message(&msg);
+
+		_mavlink->send_message(MAVLINK_MSG_ID_MISSION_ITEM, &wp);
 
 		if (_verbose) { warnx("WPM: Send MISSION_ITEM seq %u to ID %u", wp.seq, wp.target_system); }
 
@@ -251,13 +256,12 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1
 	if (seq < _max_count) {
 		_time_last_sent = hrt_absolute_time();
 
-		mavlink_message_t msg;
 		mavlink_mission_request_t wpr;
 		wpr.target_system = sysid;
 		wpr.target_component = compid;
 		wpr.seq = seq;
-		mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpr);
-		_mavlink->send_message(&msg);
+
+		_mavlink->send_message(MAVLINK_MSG_ID_MISSION_REQUEST, &wpr);
 
 		if (_verbose) { warnx("WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); }
 
@@ -272,22 +276,21 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1
 void
 MavlinkMissionManager::send_mission_item_reached(uint16_t seq)
 {
-	mavlink_message_t msg;
 	mavlink_mission_item_reached_t wp_reached;
 
 	wp_reached.seq = seq;
 
-	mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp_reached);
-	_mavlink->send_message(&msg);
+	_mavlink->send_message(MAVLINK_MSG_ID_MISSION_ITEM_REACHED, &wp_reached);
 
 	if (_verbose) { warnx("WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq); }
 }
 
 
 void
-MavlinkMissionManager::eventloop()
+MavlinkMissionManager::send(const hrt_abstime now)
 {
-	hrt_abstime now = hrt_absolute_time();
+	/* update interval for slow rate limiter */
+	_slow_rate_limiter.set_interval(_interval * 10 / _mavlink->get_rate_mult());
 
 	bool updated = false;
 	orb_check(_mission_result_sub, &updated);
diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h
index f63c32f24..8ff27f87d 100644
--- a/src/modules/mavlink/mavlink_mission.h
+++ b/src/modules/mavlink/mavlink_mission.h
@@ -42,11 +42,12 @@
 
 #pragma once
 
+#include <uORB/uORB.h>
+
 #include "mavlink_bridge_header.h"
 #include "mavlink_rate_limiter.h"
-#include <uORB/uORB.h>
+#include "mavlink_stream.h"
 
-// FIXME XXX - TO BE MOVED TO XML
 enum MAVLINK_WPM_STATES {
 	MAVLINK_WPM_STATE_IDLE = 0,
 	MAVLINK_WPM_STATE_SENDLIST,
@@ -66,20 +67,75 @@ enum MAVLINK_WPM_CODES {
 #define MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT 5000000    ///< Protocol communication action timeout in useconds
 #define MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT 500000        ///< Protocol communication retry timeout in useconds
 
-class Mavlink;
-
-class MavlinkMissionManager {
+class MavlinkMissionManager : public MavlinkStream {
 public:
-	MavlinkMissionManager(Mavlink *parent);
-
 	~MavlinkMissionManager();
 
+	const char *get_name() const
+	{
+		return MavlinkMissionManager::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "MISSION_ITEM";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_MISSION_ITEM;
+	}
+
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkMissionManager(mavlink);
+	}
+
+	unsigned get_size();
+
+	void handle_message(const mavlink_message_t *msg);
+
+	void set_verbose(bool v) { _verbose = v; }
+
+private:
+	enum MAVLINK_WPM_STATES _state;					///< Current state
+
+	uint64_t		_time_last_recv;
+	uint64_t		_time_last_sent;
+
+	uint32_t		_action_timeout;
+	uint32_t		_retry_timeout;
+	unsigned		_max_count;				///< Maximum number of mission items
+
+	int			_dataman_id;				///< Dataman storage ID for active mission
+	unsigned		_count;					///< Count of items in active mission
+	int			_current_seq;				///< Current item sequence in active mission
+
+	int			_transfer_dataman_id;			///< Dataman storage ID for current transmission
+	unsigned		_transfer_count;			///< Items count in current transmission
+	unsigned		_transfer_seq;				///< Item sequence in current transmission
+	unsigned		_transfer_current_seq;			///< Current item ID for current transmission (-1 means not initialized)
+	unsigned		_transfer_partner_sysid;		///< Partner system ID for current transmission
+	unsigned		_transfer_partner_compid;		///< Partner component ID for current transmission
+
+	int			_offboard_mission_sub;
+	int			_mission_result_sub;
+	orb_advert_t		_offboard_mission_pub;
+
+	MavlinkRateLimiter	_slow_rate_limiter;
+
+	bool _verbose;
+
+	uint8_t			_comp_id;
+
+	/* do not allow top copying this class */
+	MavlinkMissionManager(MavlinkMissionManager &);
+	MavlinkMissionManager& operator = (const MavlinkMissionManager &);
+
 	void init_offboard_mission();
 
 	int update_active_mission(int dataman_id, unsigned count, int seq);
 
-	void send_message(mavlink_message_t *msg);
-
 	/**
 	 *  @brief Sends an waypoint ack message
 	 */
@@ -111,10 +167,6 @@ public:
 	 */
 	void send_mission_item_reached(uint16_t seq);
 
-	void eventloop();
-
-	void handle_message(const mavlink_message_t *msg);
-
 	void handle_mission_ack(const mavlink_message_t *msg);
 
 	void handle_mission_set_current(const mavlink_message_t *msg);
@@ -139,39 +191,8 @@ public:
 	 */
 	int format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item);
 
-	void set_verbose(bool v) { _verbose = v; }
-
-private:
-	Mavlink*		_mavlink;
-
-	enum MAVLINK_WPM_STATES _state;					///< Current state
-
-	uint64_t		_time_last_recv;
-	uint64_t		_time_last_sent;
-
-	uint32_t		_action_timeout;
-	uint32_t		_retry_timeout;
-	unsigned		_max_count;				///< Maximum number of mission items
+protected:
+	explicit MavlinkMissionManager(Mavlink *mavlink);
 
-	int			_dataman_id;				///< Dataman storage ID for active mission
-	unsigned		_count;					///< Count of items in active mission
-	int			_current_seq;				///< Current item sequence in active mission
-
-	int			_transfer_dataman_id;			///< Dataman storage ID for current transmission
-	unsigned		_transfer_count;			///< Items count in current transmission
-	unsigned		_transfer_seq;				///< Item sequence in current transmission
-	unsigned		_transfer_current_seq;			///< Current item ID for current transmission (-1 means not initialized)
-	unsigned		_transfer_partner_sysid;		///< Partner system ID for current transmission
-	unsigned		_transfer_partner_compid;		///< Partner component ID for current transmission
-
-	int			_offboard_mission_sub;
-	int			_mission_result_sub;
-	orb_advert_t		_offboard_mission_pub;
-
-	MavlinkRateLimiter	_slow_rate_limiter;
-
-	bool _verbose;
-
-	mavlink_channel_t	_channel;
-	uint8_t			_comp_id;
+	void send(const hrt_abstime t);
 };
diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h
index 71efb43af..7af454df6 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.h
+++ b/src/modules/mavlink/mavlink_orb_subscription.h
@@ -82,6 +82,10 @@ private:
 	const orb_id_t _topic;		///< topic metadata
 	int _fd;			///< subscription handle
 	bool _published;		///< topic was ever published
+
+	/* do not allow copying this class */
+	MavlinkOrbSubscription(const MavlinkOrbSubscription&);
+	MavlinkOrbSubscription operator=(const MavlinkOrbSubscription&);
 };
 
 
diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp
new file mode 100644
index 000000000..cd5f53d88
--- /dev/null
+++ b/src/modules/mavlink/mavlink_parameters.cpp
@@ -0,0 +1,197 @@
+/****************************************************************************
+ *
+ *   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 mavlink_parameters.cpp
+ * Mavlink parameters manager implementation.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <stdio.h>
+
+#include "mavlink_parameters.h"
+#include "mavlink_main.h"
+
+MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink),
+	_send_all_index(-1)
+{
+}
+
+unsigned
+MavlinkParametersManager::get_size()
+{
+	if (_send_all_index >= 0) {
+		return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+
+	} else {
+		return 0;
+	}
+}
+
+void
+MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
+{
+	switch (msg->msgid) {
+	case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
+			/* request all parameters */
+			mavlink_param_request_list_t req_list;
+			mavlink_msg_param_request_list_decode(msg, &req_list);
+
+			if (req_list.target_system == mavlink_system.sysid &&
+			    (req_list.target_component == mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) {
+
+				_send_all_index = 0;
+				_mavlink->send_statustext_info("[pm] sending list");
+			}
+			break;
+		}
+
+	case MAVLINK_MSG_ID_PARAM_SET: {
+			/* set parameter */
+			if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) {
+				mavlink_param_set_t set;
+				mavlink_msg_param_set_decode(msg, &set);
+
+				if (set.target_system == mavlink_system.sysid &&
+				    (set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) {
+
+					/* local name buffer to enforce null-terminated string */
+					char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
+					strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+					/* enforce null termination */
+					name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
+					/* attempt to find parameter, set and send it */
+					param_t param = param_find(name);
+
+					if (param == PARAM_INVALID) {
+						char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
+						sprintf(buf, "[pm] unknown param: %s", name);
+						_mavlink->send_statustext_info(buf);
+
+					} else {
+						/* set and send parameter */
+						param_set(param, &(set.param_value));
+						send_param(param);
+					}
+				}
+			}
+			break;
+		}
+
+	case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
+			/* request one parameter */
+			mavlink_param_request_read_t req_read;
+			mavlink_msg_param_request_read_decode(msg, &req_read);
+
+			if (req_read.target_system == mavlink_system.sysid &&
+			    (req_read.target_component == mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) {
+
+				/* when no index is given, loop through string ids and compare them */
+				if (req_read.param_index < 0) {
+					/* local name buffer to enforce null-terminated string */
+					char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
+					strncpy(name, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+					/* enforce null termination */
+					name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
+					/* attempt to find parameter and send it */
+					send_param(param_find(name));
+
+				} else {
+					/* when index is >= 0, send this parameter again */
+					send_param(param_for_index(req_read.param_index));
+				}
+			}
+			break;
+		}
+
+	default:
+		break;
+	}
+}
+
+void
+MavlinkParametersManager::send(const hrt_abstime t)
+{
+	/* send all parameters if requested */
+	if (_send_all_index >= 0) {
+		send_param(param_for_index(_send_all_index));
+		_send_all_index++;
+		if (_send_all_index >= (int) param_count()) {
+			_send_all_index = -1;
+		}
+	}
+}
+
+void
+MavlinkParametersManager::send_param(param_t param)
+{
+	if (param == PARAM_INVALID) {
+		return;
+	}
+
+	mavlink_param_value_t msg;
+
+	/*
+	 * get param value, since MAVLink encodes float and int params in the same
+	 * space during transmission, copy param onto float val_buf
+	 */
+	if (param_get(param, &msg.param_value) != OK) {
+		return;
+	}
+
+	msg.param_count = param_count();
+	msg.param_index = param_get_index(param);
+
+	/* copy parameter name */
+	strncpy(msg.param_id, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+
+	/* query parameter type */
+	param_type_t type = param_type(param);
+
+	/*
+	 * Map onboard parameter type to MAVLink type,
+	 * endianess matches (both little endian)
+	 */
+	if (type == PARAM_TYPE_INT32) {
+		msg.param_type = MAVLINK_TYPE_INT32_T;
+
+	} else if (type == PARAM_TYPE_FLOAT) {
+		msg.param_type = MAVLINK_TYPE_FLOAT;
+
+	} else {
+		msg.param_type = MAVLINK_TYPE_FLOAT;
+	}
+
+	_mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg);
+}
diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h
new file mode 100644
index 000000000..5576e6b84
--- /dev/null
+++ b/src/modules/mavlink/mavlink_parameters.h
@@ -0,0 +1,115 @@
+/****************************************************************************
+ *
+ *   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 mavlink_parameters.h
+ * Mavlink parameters manager definition.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#pragma once
+
+#include <systemlib/param/param.h>
+
+#include "mavlink_bridge_header.h"
+#include "mavlink_stream.h"
+
+class MavlinkParametersManager : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkParametersManager::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "PARAM_VALUE";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_PARAM_VALUE;
+	}
+
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkParametersManager(mavlink);
+	}
+
+	unsigned get_size();
+
+	void handle_message(const mavlink_message_t *msg);
+
+	/**
+	 * Send one parameter identified by index.
+	 *
+	 * @param index		The index of the parameter to send.
+	 * @return		zero on success, nonzero else.
+	 */
+	void		start_send_one(int index);
+
+
+	/**
+	 * Send one parameter identified by name.
+	 *
+	 * @param name		The index of the parameter to send.
+	 * @return		zero on success, nonzero else.
+	 */
+	int			start_send_for_name(const char *name);
+
+	/**
+	 * Start sending the parameter queue.
+	 *
+	 * This function will not directly send parameters, but instead
+	 * activate the sending of one parameter on each call of
+	 * mavlink_pm_queued_send().
+	 * @see 		mavlink_pm_queued_send()
+	 */
+	void		start_send_all();
+
+private:
+	int		_send_all_index;
+
+	/* do not allow top copying this class */
+	MavlinkParametersManager(MavlinkParametersManager &);
+	MavlinkParametersManager& operator = (const MavlinkParametersManager &);
+
+protected:
+	explicit MavlinkParametersManager(Mavlink *mavlink);
+
+	void send(const hrt_abstime t);
+
+	void send_param(param_t param);
+};
diff --git a/src/modules/mavlink/mavlink_rate_limiter.cpp b/src/modules/mavlink/mavlink_rate_limiter.cpp
index 9cdda8b17..d3b3e1cde 100644
--- a/src/modules/mavlink/mavlink_rate_limiter.cpp
+++ b/src/modules/mavlink/mavlink_rate_limiter.cpp
@@ -64,7 +64,7 @@ MavlinkRateLimiter::check(hrt_abstime t)
 	uint64_t dt = t - _last_sent;
 
 	if (dt > 0 && dt >= _interval) {
-		_last_sent = (t / _interval) * _interval;
+		_last_sent = t;
 		return true;
 	}
 
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 43a7abdda..54a7fb065 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -86,7 +86,9 @@ static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f;
 
 MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_mavlink(parent),
-
+	status{},
+	hil_local_pos{},
+	_control_mode{},
 	_global_pos_pub(-1),
 	_local_pos_pub(-1),
 	_attitude_pub(-1),
@@ -110,17 +112,14 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_telemetry_status_pub(-1),
 	_rc_pub(-1),
 	_manual_pub(-1),
-	_telemetry_heartbeat_time(0),
 	_radio_status_available(false),
-	_control_mode_sub(-1),
+	_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
 	_hil_frames(0),
 	_old_timestamp(0),
 	_hil_local_proj_inited(0),
-	_hil_local_alt0(0.0)
+	_hil_local_alt0(0.0f),
+	_hil_local_proj_ref{}
 {
-	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
-	memset(&hil_local_pos, 0, sizeof(hil_local_pos));
-	memset(&_control_mode, 0, sizeof(_control_mode));
 
 	// make sure the FTP server is started
 	(void)MavlinkFTP::getServer();
@@ -247,7 +246,8 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
 		} else {
 
 			if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
-				warnx("ignoring CMD spoofed with same SYS/COMP ID");
+				warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID",
+				      mavlink_system.sysid, mavlink_system.compid);
 				return;
 			}
 
@@ -444,11 +444,11 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 		mavlink_radio_status_t rstatus;
 		mavlink_msg_radio_status_decode(msg, &rstatus);
 
-		struct telemetry_status_s tstatus;
-		memset(&tstatus, 0, sizeof(tstatus));
+		struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
 
 		tstatus.timestamp = hrt_absolute_time();
-		tstatus.heartbeat_time = _telemetry_heartbeat_time;
+		tstatus.telem_time = tstatus.timestamp;
+		/* tstatus.heartbeat_time is set by system heartbeats */
 		tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
 		tstatus.rssi = rstatus.rssi;
 		tstatus.remote_rssi = rstatus.remrssi;
@@ -505,16 +505,20 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
 
 		/* ignore own heartbeats, accept only heartbeats from GCS */
 		if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) {
-			_telemetry_heartbeat_time = hrt_absolute_time();
+
+			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) {
 
-				struct telemetry_status_s tstatus;
-				memset(&tstatus, 0, sizeof(tstatus));
-
-				tstatus.timestamp = _telemetry_heartbeat_time;
-				tstatus.heartbeat_time = _telemetry_heartbeat_time;
+				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) {
@@ -590,10 +594,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		gyro.temperature = imu.temperature;
 
 		if (_gyro_pub < 0) {
-			_gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro);
+			_gyro_pub = orb_advertise(ORB_ID(sensor_gyro0), &gyro);
 
 		} else {
-			orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro);
+			orb_publish(ORB_ID(sensor_gyro0), _gyro_pub, &gyro);
 		}
 	}
 
@@ -612,10 +616,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		accel.temperature = imu.temperature;
 
 		if (_accel_pub < 0) {
-			_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
+			_accel_pub = orb_advertise(ORB_ID(sensor_accel0), &accel);
 
 		} else {
-			orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel);
+			orb_publish(ORB_ID(sensor_accel0), _accel_pub, &accel);
 		}
 	}
 
@@ -633,10 +637,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		mag.z = imu.zmag;
 
 		if (_mag_pub < 0) {
-			_mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag);
+			_mag_pub = orb_advertise(ORB_ID(sensor_mag0), &mag);
 
 		} else {
-			orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag);
+			orb_publish(ORB_ID(sensor_mag0), _mag_pub, &mag);
 		}
 	}
 
@@ -651,10 +655,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		baro.temperature = imu.temperature;
 
 		if (_baro_pub < 0) {
-			_baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro);
+			_baro_pub = orb_advertise(ORB_ID(sensor_baro0), &baro);
 
 		} else {
-			orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro);
+			orb_publish(ORB_ID(sensor_baro0), _baro_pub, &baro);
 		}
 	}
 
@@ -928,10 +932,10 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		accel.temperature = 25.0f;
 
 		if (_accel_pub < 0) {
-			_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
+			_accel_pub = orb_advertise(ORB_ID(sensor_accel0), &accel);
 
 		} else {
-			orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel);
+			orb_publish(ORB_ID(sensor_accel0), _accel_pub, &accel);
 		}
 	}
 
@@ -999,6 +1003,9 @@ MavlinkReceiver::receive_thread(void *arg)
 					_mavlink->handle_message(&msg);
 				}
 			}
+
+			/* count received bytes */
+			_mavlink->count_rxbytes(nread);
 		}
 	}
 
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index a94307705..014193100 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -151,7 +151,6 @@ private:
 	orb_advert_t _telemetry_status_pub;
 	orb_advert_t _rc_pub;
 	orb_advert_t _manual_pub;
-	hrt_abstime _telemetry_heartbeat_time;
 	bool _radio_status_available;
 	int _control_mode_sub;
 	int _hil_frames;
@@ -159,4 +158,8 @@ private:
 	bool _hil_local_proj_inited;
 	float _hil_local_alt0;
 	struct map_projection_reference_s _hil_local_proj_ref;
+
+	/* do not allow copying this class */
+	MavlinkReceiver(const MavlinkReceiver&);
+	MavlinkReceiver operator=(const MavlinkReceiver&);
 };
diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp
index 7279206db..5b9e45d3e 100644
--- a/src/modules/mavlink/mavlink_stream.cpp
+++ b/src/modules/mavlink/mavlink_stream.cpp
@@ -43,9 +43,9 @@
 #include "mavlink_stream.h"
 #include "mavlink_main.h"
 
-MavlinkStream::MavlinkStream() :
+MavlinkStream::MavlinkStream(Mavlink *mavlink) :
 	next(nullptr),
-	_channel(MAVLINK_COMM_0),
+	_mavlink(mavlink),
 	_interval(1000000),
 	_last_sent(0)
 {
@@ -65,26 +65,27 @@ MavlinkStream::set_interval(const unsigned int interval)
 }
 
 /**
- * Set mavlink channel
- */
-void
-MavlinkStream::set_channel(mavlink_channel_t channel)
-{
-	_channel = channel;
-}
-
-/**
  * Update subscriptions and send message if necessary
  */
 int
 MavlinkStream::update(const hrt_abstime t)
 {
 	uint64_t dt = t - _last_sent;
+	unsigned int interval = _interval;
+
+	if (!const_rate()) {
+		interval /= _mavlink->get_rate_mult();
+	}
 
-	if (dt > 0 && dt >= _interval) {
+	if (dt > 0 && dt >= interval) {
 		/* interval expired, send message */
 		send(t);
-		_last_sent = (t / _interval) * _interval;
+		if (const_rate()) {
+			_last_sent = (t / _interval) * _interval;
+
+		} else {
+			_last_sent = t;
+		}
 
 		return 0;
 	}
diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h
index 69809a386..ef22dc443 100644
--- a/src/modules/mavlink/mavlink_stream.h
+++ b/src/modules/mavlink/mavlink_stream.h
@@ -46,37 +46,60 @@
 class Mavlink;
 class MavlinkStream;
 
-#include "mavlink_main.h"
-
 class MavlinkStream
 {
 
 public:
 	MavlinkStream *next;
 
-	MavlinkStream();
+	MavlinkStream(Mavlink *mavlink);
 	virtual ~MavlinkStream();
+
+	/**
+	 * Get the interval
+	 *
+	 * @param interval the inveral in microseconds (us) between messages
+	 */
 	void set_interval(const unsigned int interval);
-	void set_channel(mavlink_channel_t channel);
+
+	/**
+	 * Get the interval
+	 *
+	 * @return the inveral in microseconds (us) between messages
+	 */
+	unsigned get_interval() { return _interval; }
 
 	/**
 	 * @return 0 if updated / sent, -1 if unchanged
 	 */
 	int update(const hrt_abstime t);
-	static MavlinkStream *new_instance();
+	static MavlinkStream *new_instance(const Mavlink *mavlink);
 	static const char *get_name_static();
-	virtual void subscribe(Mavlink *mavlink) = 0;
 	virtual const char *get_name() const = 0;
 	virtual uint8_t get_id() = 0;
 
+	/**
+	 * @return true if steam rate shouldn't be adjusted
+	 */
+	virtual bool const_rate() { return false; }
+
+	/**
+	 * Get maximal total messages size on update
+	 */
+	virtual unsigned get_size() = 0;
+
 protected:
-	mavlink_channel_t _channel;
+	Mavlink *    _mavlink;
 	unsigned int _interval;
 
 	virtual void send(const hrt_abstime t) = 0;
 
 private:
 	hrt_abstime _last_sent;
+
+	/* do not allow top copying this class */
+	MavlinkStream(const MavlinkStream&);
+	MavlinkStream& operator=(const MavlinkStream&);
 };
 
 
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index d49bbb7f7..91fdd6154 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -40,11 +40,11 @@ SRCS		 += mavlink_main.cpp \
 			mavlink.c \
 			mavlink_receiver.cpp \
 			mavlink_mission.cpp \
+			mavlink_parameters.cpp \
 			mavlink_orb_subscription.cpp \
 			mavlink_messages.cpp \
 			mavlink_stream.cpp \
 			mavlink_rate_limiter.cpp \
-			mavlink_commands.cpp \
 			mavlink_ftp.cpp
 
 INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
@@ -52,3 +52,5 @@ INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
 MAXOPTIMIZATION	 = -Os
 
 MODULE_STACKSIZE = 1024
+
+EXTRACXXFLAGS	= -Weffc++
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index ba766cd10..12f6b9c21 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -314,30 +314,36 @@ Mission::set_mission_items()
 	/* set previous position setpoint to current */
 	set_previous_pos_setpoint();
 
+	/* get home distance state */
+	bool home_dist_ok = check_dist_1wp();
+	/* the home dist check provides user feedback, so we initialize it to this */
+	bool user_feedback_done = !home_dist_ok;
+
 	/* try setting onboard mission item */
 	if (_param_onboard_enabled.get() && read_mission_item(true, true, &_mission_item)) {
 		/* if mission type changed, notify */
 		if (_mission_type != MISSION_TYPE_ONBOARD) {
-			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: onboard mission running");
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "onboard mission now running");
 		}
 		_mission_type = MISSION_TYPE_ONBOARD;
 
 	/* try setting offboard mission item */
-	} else if (check_dist_1wp() && read_mission_item(false, true, &_mission_item)) {
+	} else if (home_dist_ok && read_mission_item(false, true, &_mission_item)) {
 		/* if mission type changed, notify */
 		if (_mission_type != MISSION_TYPE_OFFBOARD) {
-			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: offboard mission running");
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "offboard mission now running");
 		}
 		_mission_type = MISSION_TYPE_OFFBOARD;
 
 	} else {
-		/* no mission available, switch to loiter */
+		/* no mission available or mission finished, switch to loiter */
 		if (_mission_type != MISSION_TYPE_NONE) {
-			mavlink_log_info(_navigator->get_mavlink_fd(),
-					"#audio: mission finished");
-		} else {
-			mavlink_log_info(_navigator->get_mavlink_fd(),
-					"#audio: no mission available");
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
+		} else if (!user_feedback_done) {
+			/* only tell users that we got no mission if there has not been any
+			 * better, more specific feedback yet
+			 */
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available");
 		}
 		_mission_type = MISSION_TYPE_NONE;
 
@@ -397,7 +403,7 @@ Mission::set_mission_items()
 			takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get());
 		}
 
-		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: takeoff to %.1fm above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
+		mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
 
 		_mission_item.lat = _navigator->get_global_position()->lat;
 		_mission_item.lon = _navigator->get_global_position()->lon;
@@ -483,7 +489,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 		if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) {
 			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
 			mavlink_log_critical(_navigator->get_mavlink_fd(),
-			                     "#audio: ERROR waypoint could not be read");
+			                     "ERROR waypoint could not be read");
 			return false;
 		}
 
@@ -502,7 +508,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 						/* not supposed to happen unless the datamanager can't access the
 						 * dataman */
 						mavlink_log_critical(_navigator->get_mavlink_fd(),
-								"#audio: ERROR DO JUMP waypoint could not be written");
+								"ERROR DO JUMP waypoint could not be written");
 						return false;
 					}
 				}
@@ -511,8 +517,8 @@ 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_info(_navigator->get_mavlink_fd(),
-						 "#audio: DO JUMP repetitions completed");
+				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)++;
 			}
@@ -526,7 +532,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 
 	/* we have given up, we don't want to cycle forever */
 	mavlink_log_critical(_navigator->get_mavlink_fd(),
-			     "#audio: ERROR DO JUMP is cycling, giving up");
+			     "ERROR DO JUMP is cycling, giving up");
 	return false;
 }
 
diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp
index dd7f4c801..606521f20 100644
--- a/src/modules/navigator/mission_feasibility_checker.cpp
+++ b/src/modules/navigator/mission_feasibility_checker.cpp
@@ -135,12 +135,15 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
 			}
 		}
 
-		if (home_alt > missionitem.altitude) {
+		/* calculate the global waypoint altitude */
+		float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude;
+
+		if (home_alt > wp_alt) {
 			if (throw_error) {
-				mavlink_log_info(_mavlink_fd, "Waypoint %d below home", i);
+				mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Waypoint %d below home", i);
 				return false;
 			} else	{
-				mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i);
+				mavlink_log_critical(_mavlink_fd, "Warning: Waypoint %d below home", i);
 				return true;
 			}
 		}
diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk
index 637eaae59..b50198996 100644
--- a/src/modules/navigator/module.mk
+++ b/src/modules/navigator/module.mk
@@ -54,3 +54,5 @@ SRCS		= navigator_main.cpp \
 INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
 
 MODULE_STACKSIZE = 1200
+
+EXTRACXXFLAGS	= -Weffc++
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index bf6e2ea0e..8edbb63b3 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -222,5 +222,11 @@ private:
 	 * Publish a new position setpoint triplet for position controllers
 	 */
 	void		publish_position_setpoint_triplet();
+
+	/* this class has ptr data members, so it should not be copied,
+	 * consequently the copy constructors are private.
+	 */
+	Navigator(const Navigator&);
+	Navigator operator=(const Navigator&);
 };
 #endif
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 1a5ba4c1a..331a9a728 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -105,17 +105,18 @@ Navigator::Navigator() :
 	_control_mode_sub(-1),
 	_onboard_mission_sub(-1),
 	_offboard_mission_sub(-1),
+	_param_update_sub(-1),
 	_pos_sp_triplet_pub(-1),
-	_vstatus({}),
-	_control_mode({}),
-	_global_pos({}),
-	_home_pos({}),
-	_mission_item({}),
-	_nav_caps({}),
-	_pos_sp_triplet({}),
+	_vstatus{},
+	_control_mode{},
+	_global_pos{},
+	_home_pos{},
+	_mission_item{},
+	_nav_caps{},
+	_pos_sp_triplet{},
 	_mission_item_valid(false),
 	_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
-	_geofence({}),
+	_geofence{},
 	_geofence_violation_warning_sent(false),
 	_fence_valid(false),
 	_inside_fence(true),
@@ -124,6 +125,8 @@ Navigator::Navigator() :
 	_loiter(this, "LOI"),
 	_rtl(this, "RTL"),
 	_offboard(this, "OFF"),
+	_can_loiter_at_sp(false),
+	_pos_sp_triplet_updated(false),
 	_param_loiter_radius(this, "LOITER_RAD"),
 	_param_acceptance_radius(this, "ACC_RAD")
 {
@@ -455,7 +458,7 @@ void
 Navigator::publish_position_setpoint_triplet()
 {
 	/* update navigation state */
-	/* TODO: set nav_state */
+	_pos_sp_triplet.nav_state = _vstatus.nav_state;
 
 	/* lazily publish the position setpoint triplet only once available */
 	if (_pos_sp_triplet_pub > 0) {
diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h
index a7ba79bba..de5545dcb 100644
--- a/src/modules/navigator/navigator_mode.h
+++ b/src/modules/navigator/navigator_mode.h
@@ -88,6 +88,12 @@ protected:
 
 private:
 	bool _first_run;
+
+	/* this class has ptr data members, so it should not be copied,
+	 * consequently the copy constructors are private.
+	 */
+	NavigatorMode(const NavigatorMode&);
+	NavigatorMode operator=(const NavigatorMode&);
 };
 
 #endif
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c
index 90d8d2b50..dff2768c7 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c
@@ -41,28 +41,242 @@
 
 #include "position_estimator_inav_params.h"
 
+/**
+ * Z axis weight for barometer
+ *
+ * Weight (cutoff frequency) for barometer altitude measurements.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f);
+
+/**
+ * Z axis weight for GPS
+ *
+ * Weight (cutoff frequency) for GPS altitude measurements. GPS altitude data is very noisy and should be used only as slow correction for baro offset.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f);
+
+/**
+ * Z axis weight for vision
+ *
+ * Weight (cutoff frequency) for vision altitude measurements. vision altitude data is very noisy and should be used only as slow correction for baro offset.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_W_Z_VIS_P, 0.5f);
+
+/**
+ * Z axis weight for sonar
+ *
+ * Weight (cutoff frequency) for sonar measurements.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f);
+
+/**
+ * XY axis weight for GPS position
+ *
+ * Weight (cutoff frequency) for GPS position measurements.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f);
+
+/**
+ * XY axis weight for GPS velocity
+ *
+ * Weight (cutoff frequency) for GPS velocity measurements.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f);
+
+/**
+ * XY axis weight for vision position
+ *
+ * Weight (cutoff frequency) for vision position measurements.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_P, 5.0f);
+
+/**
+ * XY axis weight for vision velocity
+ *
+ * Weight (cutoff frequency) for vision velocity measurements.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_V, 0.0f);
+
+/**
+ * XY axis weight for optical flow
+ *
+ * Weight (cutoff frequency) for optical flow (velocity) measurements.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f);
+
+/**
+ * XY axis weight for resetting velocity
+ *
+ * When velocity sources lost slowly decrease estimated horizontal velocity with this weight.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_W_XY_RES_V, 0.5f);
+
+/**
+ * XY axis weight factor for GPS when optical flow available
+ *
+ * When optical flow data available, multiply GPS weights (for position and velocity) by this factor.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f);
+
+/**
+ * Accelerometer bias estimation weight
+ *
+ * Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable.
+ *
+ * @min 0.0
+ * @max 0.1
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f);
+
+/**
+ * Optical flow scale factor
+ *
+ * Factor to convert raw optical flow (in pixels) to radians [rad/px].
+ *
+ * @min 0.0
+ * @max 1.0
+ * @unit rad/px
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f);
+
+/**
+ * Minimal acceptable optical flow quality
+ *
+ * 0 - lowest quality, 1 - best quality.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.5f);
+
+/**
+ * Weight for sonar filter
+ *
+ * Sonar filter detects spikes on sonar measurements and used to detect new surface level.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f);
+
+/**
+ * Sonar maximal error for new surface
+ *
+ * If sonar measurement error is larger than this value it skiped (spike) or accepted as new surface level (if offset is stable).
+ *
+ * @min 0.0
+ * @max 1.0
+ * @unit m
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f);
+
+/**
+ * Land detector time
+ *
+ * Vehicle assumed landed if no altitude changes happened during this time on low throttle.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @unit s
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f);
+
+/**
+ * Land detector altitude dispersion threshold
+ *
+ * Dispersion threshold for triggering land detector.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @unit m
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f);
+
+/**
+ * Land detector throttle threshold
+ *
+ * Value should be lower than minimal hovering thrust. Half of it is good choice.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f);
-PARAM_DEFINE_INT32(CBRK_NO_VISION, 0);
+
+/**
+ * GPS delay
+ *
+ * GPS delay compensation
+ *
+ * @min 0.0
+ * @max 1.0
+ * @unit s
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f);
 
+/**
+ * Disable vision input
+ *
+ * Set to the appropriate key to disable input
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Position Estimator INAV
+ */
+PARAM_DEFINE_INT32(CBRK_NO_VISION, 0);
+
 int parameters_init(struct position_estimator_inav_param_handles *h)
 {
 	h->w_z_baro = param_find("INAV_W_Z_BARO");
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 2f721bf1e..606c639f9 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -111,7 +111,7 @@ mixer_tick(void)
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
 	}
 
-	/* default to failsafe mixing */
+	/* default to failsafe mixing - it will be forced below if flag is set */
 	source = MIX_FAILSAFE;
 
 	/*
@@ -155,6 +155,13 @@ mixer_tick(void)
 	}
 
 	/*
+	 * Check if we should force failsafe - and do it if we have to
+	 */
+	if (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) {
+		source = MIX_FAILSAFE;
+	}
+
+	/*
 	 * Set failsafe status flag depending on mixing source
 	 */
 	if (source == MIX_FAILSAFE) {
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index d5a6b1ec4..050783687 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -179,6 +179,7 @@
 #define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE	(1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */
 #define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED	(1 << 6) /* Disable the IO-internal evaluation of the RC */
 #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_PWM_RATES			2	/* bitmask, 0 = low rate, 1 = high rate */
 #define PX4IO_P_SETUP_PWM_DEFAULTRATE		3	/* 'low' PWM frame output rate in Hz */
@@ -253,6 +254,10 @@ enum {							/* DSM bind states */
 /* PWM failsafe values - zero disables the output */
 #define PX4IO_PAGE_FAILSAFE_PWM			55		/**< 0..CONFIG_ACTUATOR_COUNT-1 */
 
+/* PWM failsafe values - zero disables the output */
+#define PX4IO_PAGE_SENSORS			56		/**< Sensors connected to PX4IO */
+#define PX4IO_P_SENSORS_ALTITUDE		0		/**< Altitude of an external sensor (HoTT or S.BUS2) */
+
 /* Debug and test page - not used in normal operation */
 #define PX4IO_PAGE_TEST				127
 #define PX4IO_P_TEST_LED			0		/**< set the amber LED on/off */
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index b37259997..43161aa70 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -189,7 +189,8 @@ volatile uint16_t	r_page_setup[] =
 					 PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \
 					 PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \
 					 PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \
-					 PX4IO_P_SETUP_ARMING_LOCKDOWN)
+					 PX4IO_P_SETUP_ARMING_LOCKDOWN | \
+					 PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE)
 #define PX4IO_P_SETUP_RATES_VALID	((1 << PX4IO_SERVO_COUNT) - 1)
 #define PX4IO_P_SETUP_RELAYS_VALID	((1 << PX4IO_RELAY_CHANNELS) - 1)
 
@@ -738,30 +739,19 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 		{
 			/*
 			 * Coefficients here derived by measurement of the 5-16V
-			 * range on one unit:
+			 * range on one unit, validated on sample points of another unit
 			 *
-			 * V   counts
-			 *  5  1001
-			 *  6  1219
-			 *  7  1436
-			 *  8  1653
-			 *  9  1870
-			 * 10  2086
-			 * 11  2303
-			 * 12  2522
-			 * 13  2738
-			 * 14  2956
-			 * 15  3172
-			 * 16  3389
+			 * Data in Tools/tests-host/data folder.
 			 *
-			 * slope = 0.0046067
-			 * intercept = 0.3863
+			 * measured slope = 0.004585267878277 (int: 4585)
+			 * nominal theoretic slope: 0.00459340659 (int: 4593)
+			 * intercept = 0.016646394188076 (int: 16646)
+			 * nominal theoretic intercept: 0.00 (int: 0)
 			 *
-			 * Intercept corrected for best results @ 12V.
 			 */
 			unsigned counts = adc_measure(ADC_VBATT);
 			if (counts != 0xffff) {
-				unsigned mV = (4150 + (counts * 46)) / 10 - 200;
+				unsigned mV = (166460 + (counts * 45934)) / 10000;
 				unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000;
 
 				r_page_status[PX4IO_P_STATUS_VBATT] = corrected;
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 0d36fa2c6..31861c3fc 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1073,6 +1073,12 @@ int sdlog2_thread_main(int argc, char *argv[])
 	hrt_abstime magnetometer_timestamp = 0;
 	hrt_abstime barometer_timestamp = 0;
 	hrt_abstime differential_pressure_timestamp = 0;
+	hrt_abstime gyro1_timestamp = 0;
+	hrt_abstime accelerometer1_timestamp = 0;
+	hrt_abstime magnetometer1_timestamp = 0;
+	hrt_abstime gyro2_timestamp = 0;
+	hrt_abstime accelerometer2_timestamp = 0;
+	hrt_abstime magnetometer2_timestamp = 0;
 
 	/* initialize calculated mean SNR */
 	float snr_mean = 0.0f;
@@ -1209,6 +1215,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 		/* --- SENSOR COMBINED --- */
 		if (copy_if_updated(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor)) {
 			bool write_IMU = false;
+			bool write_IMU1 = false;
+			bool write_IMU2 = false;
 			bool write_SENS = false;
 
 			if (buf.sensor.timestamp != gyro_timestamp) {
@@ -1260,6 +1268,64 @@ int sdlog2_thread_main(int argc, char *argv[])
 				LOGBUFFER_WRITE_AND_COUNT(SENS);
 			}
 
+			if (buf.sensor.accelerometer1_timestamp != accelerometer1_timestamp) {
+				accelerometer1_timestamp = buf.sensor.accelerometer1_timestamp;
+				write_IMU1 = true;
+			}
+
+			if (buf.sensor.gyro1_timestamp != gyro1_timestamp) {
+				gyro1_timestamp = buf.sensor.gyro1_timestamp;
+				write_IMU1 = true;
+			}
+
+			if (buf.sensor.magnetometer1_timestamp != magnetometer1_timestamp) {
+				magnetometer1_timestamp = buf.sensor.magnetometer1_timestamp;
+				write_IMU1 = true;
+			}
+
+			if (write_IMU1) {
+				log_msg.msg_type = LOG_IMU1_MSG;
+				log_msg.body.log_IMU.gyro_x = buf.sensor.gyro1_rad_s[0];
+				log_msg.body.log_IMU.gyro_y = buf.sensor.gyro1_rad_s[1];
+				log_msg.body.log_IMU.gyro_z = buf.sensor.gyro1_rad_s[2];
+				log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer1_m_s2[0];
+				log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer1_m_s2[1];
+				log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer1_m_s2[2];
+				log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer1_ga[0];
+				log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer1_ga[1];
+				log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer1_ga[2];
+				LOGBUFFER_WRITE_AND_COUNT(IMU);
+			}
+
+			if (buf.sensor.accelerometer2_timestamp != accelerometer2_timestamp) {
+				accelerometer2_timestamp = buf.sensor.accelerometer2_timestamp;
+				write_IMU2 = true;
+			}
+
+			if (buf.sensor.gyro2_timestamp != gyro2_timestamp) {
+				gyro2_timestamp = buf.sensor.gyro2_timestamp;
+				write_IMU2 = true;
+			}
+
+			if (buf.sensor.magnetometer2_timestamp != magnetometer2_timestamp) {
+				magnetometer2_timestamp = buf.sensor.magnetometer2_timestamp;
+				write_IMU2 = true;
+			}
+
+			if (write_IMU2) {
+				log_msg.msg_type = LOG_IMU2_MSG;
+				log_msg.body.log_IMU.gyro_x = buf.sensor.gyro2_rad_s[0];
+				log_msg.body.log_IMU.gyro_y = buf.sensor.gyro2_rad_s[1];
+				log_msg.body.log_IMU.gyro_z = buf.sensor.gyro2_rad_s[2];
+				log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer2_m_s2[0];
+				log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer2_m_s2[1];
+				log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer2_m_s2[2];
+				log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer2_ga[0];
+				log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer2_ga[1];
+				log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer2_ga[2];
+				LOGBUFFER_WRITE_AND_COUNT(IMU);
+			}
+
 		}
 
 		/* --- ATTITUDE --- */
@@ -1366,17 +1432,20 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 		/* --- GLOBAL POSITION SETPOINT --- */
 		if (copy_if_updated(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet)) {
-			log_msg.msg_type = LOG_GPSP_MSG;
-			log_msg.body.log_GPSP.nav_state = 0;  /* TODO: Fix this */
-			log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d);
-			log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d);
-			log_msg.body.log_GPSP.alt = buf.triplet.current.alt;
-			log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw;
-			log_msg.body.log_GPSP.type = buf.triplet.current.type;
-			log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius;
-			log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction;
-			log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min;
-			LOGBUFFER_WRITE_AND_COUNT(GPSP);
+
+			if (buf.triplet.current.valid) {
+				log_msg.msg_type = LOG_GPSP_MSG;
+				log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state;
+				log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d);
+				log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d);
+				log_msg.body.log_GPSP.alt = buf.triplet.current.alt;
+				log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw;
+				log_msg.body.log_GPSP.type = buf.triplet.current.type;
+				log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius;
+				log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction;
+				log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min;
+				LOGBUFFER_WRITE_AND_COUNT(GPSP);
+			}
 		}
 
 		/* --- VICON POSITION --- */
@@ -1529,6 +1598,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.msg_type = LOG_TECS_MSG;
 			log_msg.body.log_TECS.altitudeSp = buf.tecs_status.altitudeSp;
 			log_msg.body.log_TECS.altitude = buf.tecs_status.altitude;
+			log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitudeFiltered;
 			log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp;
 			log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle;
 			log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered;
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 87f7f718f..853a3811f 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -73,6 +73,8 @@ struct log_ATSP_s {
 
 /* --- IMU - IMU SENSORS --- */
 #define LOG_IMU_MSG 4
+#define LOG_IMU1_MSG 22
+#define LOG_IMU2_MSG 23
 struct log_IMU_s {
 	float acc_x;
 	float acc_y;
@@ -276,8 +278,8 @@ struct log_DIST_s {
 	uint8_t flags;
 };
 
-// ID 22 available
-// ID 23 available
+/* LOG IMU1 and IMU2 MSGs consume IDs 22 and 23 */
+
 
 /* --- PWR - ONBOARD POWER SYSTEM --- */
 #define LOG_PWR_MSG 24
@@ -332,6 +334,7 @@ struct log_GS1B_s {
 struct log_TECS_s {
 	float altitudeSp;
 	float altitude;
+	float altitudeFiltered;
 	float flightPathAngleSp;
 	float flightPathAngle;
 	float flightPathAngleFiltered;
@@ -420,7 +423,9 @@ static const struct log_format_s log_formats[] = {
 	/* business-level messages, ID < 0x80 */
 	LOG_FORMAT(ATT, "fffffffff",		"Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
 	LOG_FORMAT(ATSP, "ffff",		"RollSP,PitchSP,YawSP,ThrustSP"),
-	LOG_FORMAT(IMU, "fffffffff",		"AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
+	LOG_FORMAT_S(IMU, IMU, "fffffffff",		"AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
+	LOG_FORMAT_S(IMU1, IMU, "fffffffff",		"AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
+	LOG_FORMAT_S(IMU2, IMU, "fffffffff",		"AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
 	LOG_FORMAT(SENS, "fffff",		"BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"),
 	LOG_FORMAT(LPOS, "ffffffffLLfBBBff",	"X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,LFlg,GFlg,EPH,EPV"),
 	LOG_FORMAT(LPSP, "ffff",		"X,Y,Z,Yaw"),
@@ -450,7 +455,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
-	LOG_FORMAT(TECS, "ffffffffffffffB",	"AltSP,Alt,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
+	LOG_FORMAT(TECS, "fffffffffffffffB",	"ASP,A,AF,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
 	LOG_FORMAT(WIND, "ffff",	"X,Y,CovX,CovY"),
 
 	/* system-level messages, ID >= 0x80 */
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 38b190761..7ce6ef5ef 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -292,6 +292,19 @@ PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f);
  */
 PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0);
 
+/**
+* Set usage of external magnetometer
+*
+*  * Set to 0 (default) to auto-detect (will try to get the external as primary)
+*  * Set to 1 to force the external magnetometer as primary
+*  * Set to 2 to force the internal magnetometer as primary
+*
+* @min 0
+* @max 2
+* @group Sensor Calibration
+*/
+PARAM_DEFINE_INT32(SENS_EXT_MAG, 0);
+
 
 /**
  * RC Channel 1 Minimum
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 6bafb9ba6..4e8a8c01d 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -199,9 +199,15 @@ private:
 	bool		_hil_enabled;			/**< if true, HIL is active */
 	bool		_publishing;			/**< if true, we are publishing sensor data */
 
-	int		_gyro_sub;			/**< raw gyro data subscription */
-	int		_accel_sub;			/**< raw accel data subscription */
-	int		_mag_sub;			/**< raw mag data subscription */
+	int		_gyro_sub;			/**< raw gyro0 data subscription */
+	int		_accel_sub;			/**< raw accel0 data subscription */
+	int		_mag_sub;			/**< raw mag0 data subscription */
+	int		_gyro1_sub;			/**< raw gyro1 data subscription */
+	int		_accel1_sub;			/**< raw accel1 data subscription */
+	int		_mag1_sub;			/**< raw mag1 data subscription */
+	int		_gyro2_sub;			/**< raw gyro2 data subscription */
+	int		_accel2_sub;			/**< raw accel2 data subscription */
+	int		_mag2_sub;			/**< raw mag2 data subscription */
 	int 		_rc_sub;			/**< raw rc channels data subscription */
 	int		_baro_sub;			/**< raw baro data subscription */
 	int		_airspeed_sub;			/**< airspeed subscription */
@@ -478,6 +484,12 @@ Sensors::Sensors() :
 	_gyro_sub(-1),
 	_accel_sub(-1),
 	_mag_sub(-1),
+	_gyro1_sub(-1),
+	_accel1_sub(-1),
+	_mag1_sub(-1),
+	_gyro2_sub(-1),
+	_accel2_sub(-1),
+	_mag2_sub(-1),
 	_rc_sub(-1),
 	_baro_sub(-1),
 	_vcontrol_mode_sub(-1),
@@ -1004,7 +1016,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 	if (accel_updated) {
 		struct accel_report	accel_report;
 
-		orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
+		orb_copy(ORB_ID(sensor_accel0), _accel_sub, &accel_report);
 
 		math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
 		vect = _board_rotation * vect;
@@ -1019,6 +1031,48 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 
 		raw.accelerometer_timestamp = accel_report.timestamp;
 	}
+
+	orb_check(_accel1_sub, &accel_updated);
+
+	if (accel_updated) {
+		struct accel_report	accel_report;
+
+		orb_copy(ORB_ID(sensor_accel1), _accel_sub, &accel_report);
+
+		math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
+		vect = _board_rotation * vect;
+
+		raw.accelerometer1_m_s2[0] = vect(0);
+		raw.accelerometer1_m_s2[1] = vect(1);
+		raw.accelerometer1_m_s2[2] = vect(2);
+
+		raw.accelerometer1_raw[0] = accel_report.x_raw;
+		raw.accelerometer1_raw[1] = accel_report.y_raw;
+		raw.accelerometer1_raw[2] = accel_report.z_raw;
+
+		raw.accelerometer1_timestamp = accel_report.timestamp;
+	}
+
+	orb_check(_accel2_sub, &accel_updated);
+
+	if (accel_updated) {
+		struct accel_report	accel_report;
+
+		orb_copy(ORB_ID(sensor_accel2), _accel_sub, &accel_report);
+
+		math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
+		vect = _board_rotation * vect;
+
+		raw.accelerometer2_m_s2[0] = vect(0);
+		raw.accelerometer2_m_s2[1] = vect(1);
+		raw.accelerometer2_m_s2[2] = vect(2);
+
+		raw.accelerometer2_raw[0] = accel_report.x_raw;
+		raw.accelerometer2_raw[1] = accel_report.y_raw;
+		raw.accelerometer2_raw[2] = accel_report.z_raw;
+
+		raw.accelerometer2_timestamp = accel_report.timestamp;
+	}
 }
 
 void
@@ -1030,7 +1084,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 	if (gyro_updated) {
 		struct gyro_report	gyro_report;
 
-		orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
+		orb_copy(ORB_ID(sensor_gyro0), _gyro_sub, &gyro_report);
 
 		math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
 		vect = _board_rotation * vect;
@@ -1045,6 +1099,48 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 
 		raw.timestamp = gyro_report.timestamp;
 	}
+
+	orb_check(_gyro1_sub, &gyro_updated);
+
+	if (gyro_updated) {
+		struct gyro_report	gyro_report;
+
+		orb_copy(ORB_ID(sensor_gyro1), _gyro1_sub, &gyro_report);
+
+		math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
+		vect = _board_rotation * vect;
+
+		raw.gyro1_rad_s[0] = vect(0);
+		raw.gyro1_rad_s[1] = vect(1);
+		raw.gyro1_rad_s[2] = vect(2);
+
+		raw.gyro1_raw[0] = gyro_report.x_raw;
+		raw.gyro1_raw[1] = gyro_report.y_raw;
+		raw.gyro1_raw[2] = gyro_report.z_raw;
+
+		raw.gyro1_timestamp = gyro_report.timestamp;
+	}
+
+	orb_check(_gyro2_sub, &gyro_updated);
+
+	if (gyro_updated) {
+		struct gyro_report	gyro_report;
+
+		orb_copy(ORB_ID(sensor_gyro2), _gyro_sub, &gyro_report);
+
+		math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
+		vect = _board_rotation * vect;
+
+		raw.gyro2_rad_s[0] = vect(0);
+		raw.gyro2_rad_s[1] = vect(1);
+		raw.gyro2_rad_s[2] = vect(2);
+
+		raw.gyro2_raw[0] = gyro_report.x_raw;
+		raw.gyro2_raw[1] = gyro_report.y_raw;
+		raw.gyro2_raw[2] = gyro_report.z_raw;
+
+		raw.gyro2_timestamp = gyro_report.timestamp;
+	}
 }
 
 void
@@ -1056,10 +1152,12 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 	if (mag_updated) {
 		struct mag_report	mag_report;
 
-		orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
+		orb_copy(ORB_ID(sensor_mag0), _mag_sub, &mag_report);
 
 		math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
 
+		// XXX we need device-id based handling here
+
 		if (_mag_is_external) {
 			vect = _external_mag_rotation * vect;
 
@@ -1087,7 +1185,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw)
 
 	if (baro_updated) {
 
-		orb_copy(ORB_ID(sensor_baro), _baro_sub, &_barometer);
+		orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_barometer);
 
 		raw.baro_pres_mbar = _barometer.pressure; // Pressure in mbar
 		raw.baro_alt_meter = _barometer.altitude; // Altitude in meters
@@ -1618,11 +1716,17 @@ Sensors::task_main()
 	/*
 	 * do subscriptions
 	 */
-	_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
-	_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
-	_mag_sub = orb_subscribe(ORB_ID(sensor_mag));
+	_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro0));
+	_accel_sub = orb_subscribe(ORB_ID(sensor_accel0));
+	_mag_sub = orb_subscribe(ORB_ID(sensor_mag0));
+	_gyro1_sub = orb_subscribe(ORB_ID(sensor_gyro1));
+	_accel1_sub = orb_subscribe(ORB_ID(sensor_accel1));
+	_mag1_sub = orb_subscribe(ORB_ID(sensor_mag1));
+	_gyro2_sub = orb_subscribe(ORB_ID(sensor_gyro2));
+	_accel2_sub = orb_subscribe(ORB_ID(sensor_accel2));
+	_mag2_sub = orb_subscribe(ORB_ID(sensor_mag2));
 	_rc_sub = orb_subscribe(ORB_ID(input_rc));
-	_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
+	_baro_sub = orb_subscribe(ORB_ID(sensor_baro0));
 	_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
 	_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c
index 8f697749e..64317a18a 100644
--- a/src/modules/systemlib/circuit_breaker.c
+++ b/src/modules/systemlib/circuit_breaker.c
@@ -83,6 +83,18 @@ PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0);
  */
 PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0);
 
+/**
+ * Circuit breaker for airspeed sensor
+ *
+ * Setting this parameter to 162128 will disable the check for an airspeed sensor.
+ * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
+ *
+ * @min 0
+ * @max 162128
+ * @group Circuit Breaker
+ */
+PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0);
+
 bool circuit_breaker_enabled(const char* breaker, int32_t magic)
 {
 	int32_t val;
diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h
index 1175dbce8..b60584608 100644
--- a/src/modules/systemlib/circuit_breaker.h
+++ b/src/modules/systemlib/circuit_breaker.h
@@ -52,6 +52,7 @@
 #define CBRK_SUPPLY_CHK_KEY	894281
 #define CBRK_RATE_CTRL_KEY	140253
 #define CBRK_IO_SAFETY_KEY	22027
+#define CBRK_AIRSPD_CHK_KEY	162128
 
 #include <stdbool.h>
 
diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h
index 225570fa4..cdf60febc 100644
--- a/src/modules/systemlib/mixer/mixer.h
+++ b/src/modules/systemlib/mixer/mixer.h
@@ -231,6 +231,10 @@ protected:
 	static const char *		skipline(const char *buf, unsigned &buflen);
 
 private:
+
+	/* do not allow to copy due to prt data members */
+	Mixer(const Mixer&);
+	Mixer& operator=(const Mixer&);
 };
 
 /**
@@ -307,6 +311,10 @@ public:
 
 private:
 	Mixer				*_first;	/**< linked list of mixers */
+
+	/* do not allow to copy due to pointer data members */
+	MixerGroup(const MixerGroup&);
+	MixerGroup operator=(const MixerGroup&);
 };
 
 /**
@@ -424,6 +432,10 @@ private:
 			mixer_scaler_s &scaler,
 			uint8_t &control_group,
 			uint8_t &control_index);
+
+	/* do not allow to copy due to ptr data members */
+	SimpleMixer(const SimpleMixer&);
+	SimpleMixer operator=(const SimpleMixer&);
 };
 
 /**
@@ -522,6 +534,9 @@ private:
 	unsigned			_rotor_count;
 	const Rotor			*_rotors;
 
+	/* do not allow to copy due to ptr data members */
+	MultirotorMixer(const MultirotorMixer&);
+	MultirotorMixer operator=(const MultirotorMixer&);
 };
 
 #endif
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index 7b1b162a0..f7d5f8737 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -46,16 +46,23 @@
 #include <drivers/drv_orb_dev.h>
 
 #include <drivers/drv_mag.h>
-ORB_DEFINE(sensor_mag, struct mag_report);
+ORB_DEFINE(sensor_mag0, struct mag_report);
+ORB_DEFINE(sensor_mag1, struct mag_report);
+ORB_DEFINE(sensor_mag2, struct mag_report);
 
 #include <drivers/drv_accel.h>
-ORB_DEFINE(sensor_accel, struct accel_report);
+ORB_DEFINE(sensor_accel0, struct accel_report);
+ORB_DEFINE(sensor_accel1, struct accel_report);
+ORB_DEFINE(sensor_accel2, struct accel_report);
 
 #include <drivers/drv_gyro.h>
-ORB_DEFINE(sensor_gyro, struct gyro_report);
+ORB_DEFINE(sensor_gyro0, struct gyro_report);
+ORB_DEFINE(sensor_gyro1, struct gyro_report);
+ORB_DEFINE(sensor_gyro2, struct gyro_report);
 
 #include <drivers/drv_baro.h>
-ORB_DEFINE(sensor_baro, struct baro_report);
+ORB_DEFINE(sensor_baro0, struct baro_report);
+ORB_DEFINE(sensor_baro1, struct baro_report);
 
 #include <drivers/drv_range_finder.h>
 ORB_DEFINE(sensor_range_finder, struct range_finder_report);
diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/modules/uORB/topics/actuator_armed.h
index a98d3fc3a..0f6c9aca1 100644
--- a/src/modules/uORB/topics/actuator_armed.h
+++ b/src/modules/uORB/topics/actuator_armed.h
@@ -56,6 +56,7 @@ struct actuator_armed_s {
 	bool		armed;		/**< Set to true if system is armed */
 	bool		ready_to_arm;	/**< Set to true if system is ready to be armed */
 	bool		lockdown;	/**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
+	bool		force_failsafe; /**< Set to true if the actuators are forced to the failsafe position */
 };
 
 /**
@@ -65,4 +66,4 @@ struct actuator_armed_s {
 /* register this as object request broker structure */
 ORB_DECLARE(actuator_armed);
 
-#endif
\ No newline at end of file
+#endif
diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h
index 673c0e491..4a1932180 100644
--- a/src/modules/uORB/topics/position_setpoint_triplet.h
+++ b/src/modules/uORB/topics/position_setpoint_triplet.h
@@ -95,6 +95,8 @@ struct position_setpoint_triplet_s
 	struct position_setpoint_s previous;
 	struct position_setpoint_s current;
 	struct position_setpoint_s next;
+
+	unsigned nav_state;				/**< report the navigation state */
 };
 
 /**
diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h
index fa3367de9..06dfcdab3 100644
--- a/src/modules/uORB/topics/sensor_combined.h
+++ b/src/modules/uORB/topics/sensor_combined.h
@@ -95,6 +95,30 @@ struct sensor_combined_s {
 	float magnetometer_cuttoff_freq_hz;	/**< Internal analog low pass frequency of sensor */
 	uint64_t magnetometer_timestamp;	/**< Magnetometer timestamp         */
 
+	int16_t	gyro1_raw[3];			/**< Raw sensor values of angular velocity        */
+	float gyro1_rad_s[3];			/**< Angular velocity in radian per seconds       */
+	uint64_t gyro1_timestamp;		/**< Gyro timestamp */
+
+	int16_t accelerometer1_raw[3];		/**< Raw acceleration in NED body frame           */
+	float accelerometer1_m_s2[3];		/**< Acceleration in NED body frame, in m/s^2     */
+	uint64_t accelerometer1_timestamp;	/**< Accelerometer timestamp        */
+
+	int16_t	magnetometer1_raw[3];		/**< Raw magnetic field in NED body frame         */
+	float magnetometer1_ga[3];		/**< Magnetic field in NED body frame, in Gauss   */
+	uint64_t magnetometer1_timestamp;	/**< Magnetometer timestamp         */
+
+	int16_t	gyro2_raw[3];			/**< Raw sensor values of angular velocity        */
+	float gyro2_rad_s[3];			/**< Angular velocity in radian per seconds       */
+	uint64_t gyro2_timestamp;		/**< Gyro timestamp */
+
+	int16_t accelerometer2_raw[3];		/**< Raw acceleration in NED body frame           */
+	float accelerometer2_m_s2[3];		/**< Acceleration in NED body frame, in m/s^2     */
+	uint64_t accelerometer2_timestamp;	/**< Accelerometer timestamp        */
+
+	int16_t	magnetometer2_raw[3];		/**< Raw magnetic field in NED body frame         */
+	float magnetometer2_ga[3];		/**< Magnetic field in NED body frame, in Gauss   */
+	uint64_t magnetometer2_timestamp;	/**< Magnetometer timestamp         */
+
 	float baro_pres_mbar;			/**< Barometric pressure, already temp. comp.     */
 	float baro_alt_meter;			/**< Altitude, already temp. comp.                */
 	float baro_temp_celcius;		/**< Temperature in degrees celsius               */
diff --git a/src/modules/uORB/topics/tecs_status.h b/src/modules/uORB/topics/tecs_status.h
index c4d0c1874..33055018c 100644
--- a/src/modules/uORB/topics/tecs_status.h
+++ b/src/modules/uORB/topics/tecs_status.h
@@ -66,6 +66,7 @@ struct tecs_status_s {
 
 	float altitudeSp;
 	float altitude;
+	float altitudeFiltered;
 	float flightPathAngleSp;
 	float flightPathAngle;
 	float flightPathAngleFiltered;
diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h
index 3347724a5..1297c1a9d 100644
--- a/src/modules/uORB/topics/telemetry_status.h
+++ b/src/modules/uORB/topics/telemetry_status.h
@@ -57,7 +57,8 @@ enum TELEMETRY_STATUS_RADIO_TYPE {
 
 struct telemetry_status_s {
 	uint64_t timestamp;
-	uint64_t heartbeat_time;	/**< Time of last received heartbeat from remote system */
+	uint64_t heartbeat_time;		/**< Time of last received heartbeat from remote system */
+	uint64_t telem_time;			/**< Time of last received telemetry status packet, 0 for none */
 	enum TELEMETRY_STATUS_RADIO_TYPE type;	/**< type of the radio hardware     */
 	uint8_t rssi;				/**< local signal strength                      */
 	uint8_t remote_rssi;			/**< remote signal strength                     */
diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h
index c21a29b13..7db33d98b 100644
--- a/src/modules/uORB/topics/vehicle_command.h
+++ b/src/modules/uORB/topics/vehicle_command.h
@@ -76,6 +76,7 @@ enum VEHICLE_CMD {
 	VEHICLE_CMD_DO_REPEAT_RELAY = 182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty|  */
 	VEHICLE_CMD_DO_SET_SERVO = 183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty|  */
 	VEHICLE_CMD_DO_REPEAT_SERVO = 184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty|  */
+	VEHICLE_CMD_DO_FLIGHTTERMINATION=185, /* Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty|  */
 	VEHICLE_CMD_DO_CONTROL_VIDEO = 200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty|  */
 	VEHICLE_CMD_DO_LAST = 240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
 	VEHICLE_CMD_PREFLIGHT_CALIBRATION = 241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty|  */
diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h
index e32529cb4..25137c1c6 100644
--- a/src/modules/uORB/topics/vehicle_global_position.h
+++ b/src/modules/uORB/topics/vehicle_global_position.h
@@ -70,8 +70,8 @@ struct vehicle_global_position_s {
 	float vel_e;			/**< Ground east velocity, m/s							   */
 	float vel_d;			/**< Ground downside velocity, m/s						   */
 	float yaw; 			/**< Yaw in radians -PI..+PI.							   */
-	float eph;
-	float epv;
+	float eph;			/**< Standard deviation of position estimate horizontally */
+	float epv;			/**< Standard deviation of position vertically */
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index b46c00b75..b683bf98a 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -186,7 +186,7 @@ struct vehicle_status_s {
 	bool condition_system_sensors_initialized;
 	bool condition_system_returned_to_home;
 	bool condition_auto_mission_available;
-	bool condition_global_position_valid;		/**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */
+	bool condition_global_position_valid;		/**< set to true by the commander app if the quality of the position estimate is good enough to use it for navigation */
 	bool condition_launch_position_valid;		/**< indicates a valid launch position */
 	bool condition_home_position_valid;		/**< indicates a valid home position (a valid home position is not always a valid launch) */
 	bool condition_local_position_valid;
@@ -226,6 +226,7 @@ struct vehicle_status_s {
 	uint16_t errors_count4;
 
 	bool circuit_breaker_engaged_power_check;
+	bool circuit_breaker_engaged_airspd_check;
 };
 
 /**
diff --git a/src/modules/uavcan/.gitignore b/src/modules/uavcan/.gitignore
new file mode 100644
index 000000000..24fbf171f
--- /dev/null
+++ b/src/modules/uavcan/.gitignore
@@ -0,0 +1 @@
+./dsdlc_generated/
diff --git a/src/modules/uavcan/esc_controller.cpp b/src/modules/uavcan/esc_controller.cpp
new file mode 100644
index 000000000..406eba88c
--- /dev/null
+++ b/src/modules/uavcan/esc_controller.cpp
@@ -0,0 +1,138 @@
+/****************************************************************************
+ *
+ *   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 esc_controller.cpp
+ *
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include "esc_controller.hpp"
+#include <systemlib/err.h>
+
+UavcanEscController::UavcanEscController(uavcan::INode &node) :
+	_node(node),
+	_uavcan_pub_raw_cmd(node),
+	_uavcan_sub_status(node),
+	_orb_timer(node)
+{
+}
+
+UavcanEscController::~UavcanEscController()
+{
+	perf_free(_perfcnt_invalid_input);
+	perf_free(_perfcnt_scaling_error);
+}
+
+int UavcanEscController::init()
+{
+	// ESC status subscription
+	int res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb));
+	if (res < 0)
+	{
+		warnx("ESC status sub failed %i", res);
+		return res;
+	}
+
+	// ESC status will be relayed from UAVCAN bus into ORB at this rate
+	_orb_timer.setCallback(TimerCbBinder(this, &UavcanEscController::orb_pub_timer_cb));
+	_orb_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / ESC_STATUS_UPDATE_RATE_HZ));
+
+	return res;
+}
+
+void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
+{
+	if ((outputs == nullptr) || (num_outputs > MAX_ESCS)) {
+		perf_count(_perfcnt_invalid_input);
+		return;
+	}
+
+	/*
+	 * Rate limiting - we don't want to congest the bus
+	 */
+	const auto timestamp = _node.getMonotonicTime();
+	if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) {
+		return;
+	}
+	_prev_cmd_pub = timestamp;
+
+	/*
+	 * Fill the command message
+	 * If unarmed, we publish an empty message anyway
+	 */
+	uavcan::equipment::esc::RawCommand msg;
+
+	if (_armed) {
+		for (unsigned i = 0; i < num_outputs; i++) {
+
+			float scaled = (outputs[i] + 1.0F) * 0.5F * uavcan::equipment::esc::RawCommand::CMD_MAX;
+			if (scaled < 1.0F) {
+				scaled = 1.0F;  // Since we're armed, we don't want to stop it completely
+			}
+
+			if (scaled < uavcan::equipment::esc::RawCommand::CMD_MIN) {
+				scaled = uavcan::equipment::esc::RawCommand::CMD_MIN;
+				perf_count(_perfcnt_scaling_error);
+			} else if (scaled > uavcan::equipment::esc::RawCommand::CMD_MAX) {
+				scaled = uavcan::equipment::esc::RawCommand::CMD_MAX;
+				perf_count(_perfcnt_scaling_error);
+			} else {
+				; // Correct value
+			}
+
+			msg.cmd.push_back(static_cast<unsigned>(scaled));
+		}
+	}
+
+	/*
+	 * Publish the command message to the bus
+	 * Note that for a quadrotor it takes one CAN frame
+	 */
+	(void)_uavcan_pub_raw_cmd.broadcast(msg);
+}
+
+void UavcanEscController::arm_esc(bool arm)
+{
+	_armed = arm;
+}
+
+void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg)
+{
+	// TODO save status into a local storage; publish to ORB later from orb_pub_timer_cb()
+}
+
+void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent&)
+{
+	// TODO publish to ORB
+}
diff --git a/src/modules/uavcan/esc_controller.hpp b/src/modules/uavcan/esc_controller.hpp
new file mode 100644
index 000000000..559ede561
--- /dev/null
+++ b/src/modules/uavcan/esc_controller.hpp
@@ -0,0 +1,107 @@
+/****************************************************************************
+ *
+ *   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 esc_controller.hpp
+ *
+ * UAVCAN <--> ORB bridge for ESC messages:
+ *     uavcan.equipment.esc.RawCommand
+ *     uavcan.equipment.esc.RPMCommand
+ *     uavcan.equipment.esc.Status
+ *
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <uavcan/uavcan.hpp>
+#include <uavcan/equipment/esc/RawCommand.hpp>
+#include <uavcan/equipment/esc/Status.hpp>
+#include <systemlib/perf_counter.h>
+
+class UavcanEscController
+{
+public:
+	UavcanEscController(uavcan::INode& node);
+	~UavcanEscController();
+
+	int init();
+
+	void update_outputs(float *outputs, unsigned num_outputs);
+
+	void arm_esc(bool arm);
+
+private:
+	/**
+	 * ESC status message reception will be reported via this callback.
+	 */
+	void esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg);
+
+	/**
+	 * ESC status will be published to ORB from this callback (fixed rate).
+	 */
+	void orb_pub_timer_cb(const uavcan::TimerEvent &event);
+
+
+	static constexpr unsigned MAX_RATE_HZ = 100;			///< XXX make this configurable
+	static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 5;
+	static constexpr unsigned MAX_ESCS = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize;
+
+	typedef uavcan::MethodBinder<UavcanEscController*,
+		void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)>
+		StatusCbBinder;
+
+	typedef uavcan::MethodBinder<UavcanEscController*, void (UavcanEscController::*)(const uavcan::TimerEvent&)>
+		TimerCbBinder;
+
+	/*
+	 * libuavcan related things
+	 */
+	uavcan::MonotonicTime							_prev_cmd_pub;   ///< rate limiting
+	uavcan::INode								&_node;
+	uavcan::Publisher<uavcan::equipment::esc::RawCommand>			_uavcan_pub_raw_cmd;
+	uavcan::Subscriber<uavcan::equipment::esc::Status, StatusCbBinder>	_uavcan_sub_status;
+	uavcan::TimerEventForwarder<TimerCbBinder>				_orb_timer;
+
+	/*
+	 * ESC states
+	 */
+	bool 				_armed = false;
+	uavcan::equipment::esc::Status	_states[MAX_ESCS];
+
+	/*
+	 * Perf counters
+	 */
+	perf_counter_t _perfcnt_invalid_input = perf_alloc(PC_COUNT, "uavcan_esc_invalid_input");
+	perf_counter_t _perfcnt_scaling_error = perf_alloc(PC_COUNT, "uavcan_esc_scaling_error");
+};
diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp
new file mode 100644
index 000000000..ba1fe5e49
--- /dev/null
+++ b/src/modules/uavcan/gnss_receiver.cpp
@@ -0,0 +1,153 @@
+/****************************************************************************
+ *
+ *   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 gnss_receiver.cpp
+ *
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ * @author Andrew Chambers <achamber@gmail.com>
+ *
+ */
+
+#include "gnss_receiver.hpp"
+#include <systemlib/err.h>
+#include <mathlib/mathlib.h>
+
+#define MM_PER_CM 			10	// Millimeters per centimeter
+
+UavcanGnssReceiver::UavcanGnssReceiver(uavcan::INode &node) :
+_node(node),
+_uavcan_sub_status(node),
+_report_pub(-1)
+{
+}
+
+int UavcanGnssReceiver::init()
+{
+	int res = -1;
+
+	// GNSS fix subscription
+	res = _uavcan_sub_status.start(FixCbBinder(this, &UavcanGnssReceiver::gnss_fix_sub_cb));
+	if (res < 0)
+	{
+		warnx("GNSS fix sub failed %i", res);
+		return res;
+	}
+
+	// Clear the uORB GPS report
+	memset(&_report, 0, sizeof(_report));
+
+	return res;
+}
+
+void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg)
+{
+	_report.timestamp_position = hrt_absolute_time();
+	_report.lat = msg.lat_1e7;
+	_report.lon = msg.lon_1e7;
+	_report.alt = msg.alt_1e2 * MM_PER_CM;	// Convert from centimeter (1e2) to millimeters (1e3)
+
+	_report.timestamp_variance = _report.timestamp_position;
+
+
+	// Check if the msg contains valid covariance information
+	const bool valid_position_covariance = !msg.position_covariance.empty();
+	const bool valid_velocity_covariance = !msg.velocity_covariance.empty();
+
+	if (valid_position_covariance) {
+		float pos_cov[9];
+		msg.position_covariance.unpackSquareMatrix(pos_cov);
+
+		// Horizontal position uncertainty
+		const float horizontal_pos_variance = math::max(pos_cov[0], pos_cov[4]);
+		_report.eph = (horizontal_pos_variance > 0) ? sqrtf(horizontal_pos_variance) : -1.0F;
+
+		// Vertical position uncertainty
+		_report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F;
+	} else {
+		_report.eph = -1.0F;
+		_report.epv = -1.0F;
+	}
+
+	if (valid_velocity_covariance) {
+	    float vel_cov[9];
+	    msg.velocity_covariance.unpackSquareMatrix(vel_cov);
+		_report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]);
+
+		/* There is a nonlinear relationship between the velocity vector and the heading.
+		 * Use Jacobian to transform velocity covariance to heading covariance
+		 *
+		 * Nonlinear equation:
+		 * heading = atan2(vel_e_m_s, vel_n_m_s)
+		 * For math, see http://en.wikipedia.org/wiki/Atan2#Derivative
+		 *
+		 * To calculate the variance of heading from the variance of velocity,
+		 * cov(heading) = J(velocity)*cov(velocity)*J(velocity)^T
+		 */
+		float vel_n = msg.ned_velocity[0];
+		float vel_e = msg.ned_velocity[1];
+		float vel_n_sq = vel_n * vel_n;
+		float vel_e_sq = vel_e * vel_e;
+		_report.c_variance_rad =
+				(vel_e_sq * vel_cov[0] +
+						-2 * vel_n * vel_e * vel_cov[1] +	// Covariance matrix is symmetric
+						vel_n_sq* vel_cov[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq));
+
+	} else {
+		_report.s_variance_m_s = -1.0F;
+		_report.c_variance_rad = -1.0F;
+	}
+
+	_report.fix_type = msg.status;
+
+	_report.timestamp_velocity = _report.timestamp_position;
+	_report.vel_n_m_s = msg.ned_velocity[0];
+	_report.vel_e_m_s = msg.ned_velocity[1];
+	_report.vel_d_m_s = msg.ned_velocity[2];
+	_report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s);
+	_report.cog_rad = atan2f(_report.vel_e_m_s, _report.vel_n_m_s);
+	_report.vel_ned_valid = true;
+
+	_report.timestamp_time = _report.timestamp_position;
+	_report.time_gps_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec();	// Convert to microseconds
+
+	_report.satellites_used = msg.sats_used;
+
+	if (_report_pub > 0) {
+		orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
+
+	} else {
+		_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
+	}
+
+}
diff --git a/src/modules/mavlink/mavlink_commands.cpp b/src/modules/uavcan/gnss_receiver.hpp
index fccd4d9a5..18df8da2f 100644
--- a/src/modules/mavlink/mavlink_commands.cpp
+++ b/src/modules/uavcan/gnss_receiver.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *   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
@@ -32,39 +32,53 @@
  ****************************************************************************/
 
 /**
- * @file mavlink_commands.cpp
- * Mavlink commands stream implementation.
+ * @file gnss_receiver.hpp
  *
- * @author Anton Babushkin <anton.babushkin@me.com>
+ * UAVCAN --> ORB bridge for GNSS messages:
+ *     uavcan.equipment.gnss.Fix
+ *
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ * @author Andrew Chambers <achamber@gmail.com>
  */
 
-#include "mavlink_commands.h"
+#pragma once
 
-MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel), _cmd_time(0)
-{
-	_cmd_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command));
-}
+#include <drivers/drv_hrt.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_gps_position.h>
 
-void
-MavlinkCommandsStream::update(const hrt_abstime t)
+#include <uavcan/uavcan.hpp>
+#include <uavcan/equipment/gnss/Fix.hpp>
+
+class UavcanGnssReceiver
 {
-	struct vehicle_command_s cmd;
+public:
+	UavcanGnssReceiver(uavcan::INode& node);
+
+	int init();
+
+private:
+	/**
+	 * GNSS fix message will be reported via this callback.
+	 */
+	void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg);
+
+
+	typedef uavcan::MethodBinder<UavcanGnssReceiver*,
+		void (UavcanGnssReceiver::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>&)>
+		FixCbBinder;
+
+	/*
+	 * libuavcan related things
+	 */
+	uavcan::INode													&_node;
+	uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder>	_uavcan_sub_status;
+
+	/*
+	 * uORB
+	 */
+	struct vehicle_gps_position_s 	_report;					///< uORB topic for gnss position
+	orb_advert_t			_report_pub;					///< uORB pub for gnss position
 
-	if (_cmd_sub->update(&_cmd_time, &cmd)) {
-		/* only send commands for other systems/components */
-		if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) {
-			mavlink_msg_command_long_send(_channel,
-						      cmd.target_system,
-						      cmd.target_component,
-						      cmd.command,
-						      cmd.confirmation,
-						      cmd.param1,
-						      cmd.param2,
-						      cmd.param3,
-						      cmd.param4,
-						      cmd.param5,
-						      cmd.param6,
-						      cmd.param7);
-		}
-	}
-}
+};
diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk
new file mode 100644
index 000000000..1ef6f0cfa
--- /dev/null
+++ b/src/modules/uavcan/module.mk
@@ -0,0 +1,74 @@
+############################################################################
+#
+#   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+#   Author: Pavel Kirienko <pavel.kirienko@gmail.com>
+#
+# 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.
+#
+############################################################################
+
+#
+# UAVCAN <--> uORB bridge
+#
+
+MODULE_COMMAND = uavcan
+
+MAXOPTIMIZATION = -Os
+
+SRCS += uavcan_main.cpp         \
+        uavcan_clock.cpp        \
+        esc_controller.cpp      \
+        gnss_receiver.cpp
+
+#
+# libuavcan
+#
+include $(UAVCAN_DIR)/libuavcan/include.mk
+SRCS += $(LIBUAVCAN_SRC)
+INCLUDE_DIRS += $(LIBUAVCAN_INC)
+# Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile
+# because this platform lacks most of the standard library and STL. Hence we need to force C++03 mode.
+override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_NO_ASSERTIONS
+
+#
+# libuavcan drivers for STM32
+#
+include $(UAVCAN_DIR)/libuavcan_drivers/stm32/driver/include.mk
+SRCS += $(LIBUAVCAN_STM32_SRC)
+INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC)
+override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=2
+
+#
+# Invoke DSDL compiler
+# TODO: Add make target for this, or invoke dsdlc manually.
+#       The second option assumes that the generated headers shall be saved
+#       under the version control, which may be undesirable.
+#       The first option requires any Python and the Python Mako library for the sources to be built.
+#
+$(info $(shell $(LIBUAVCAN_DSDLC) $(UAVCAN_DSDL_DIR)))
+INCLUDE_DIRS += dsdlc_generated
diff --git a/src/modules/mavlink/mavlink_commands.h b/src/modules/uavcan/uavcan_clock.cpp
index abdba34b9..e41d5f953 100644
--- a/src/modules/mavlink/mavlink_commands.h
+++ b/src/modules/uavcan/uavcan_clock.cpp
@@ -31,35 +31,49 @@
  *
  ****************************************************************************/
 
+#include <uavcan_stm32/uavcan_stm32.hpp>
+#include <drivers/drv_hrt.h>
+
 /**
- * @file mavlink_commands.h
- * Mavlink commands stream definition.
+ * @file uavcan_clock.cpp
+ *
+ * Implements a clock for the CAN node.
  *
- * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
  */
 
-#ifndef MAVLINK_COMMANDS_H_
-#define MAVLINK_COMMANDS_H_
+namespace uavcan_stm32
+{
+namespace clock
+{
+
+uavcan::MonotonicTime getMonotonic()
+{
+	return uavcan::MonotonicTime::fromUSec(hrt_absolute_time());
+}
 
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_command.h>
+uavcan::UtcTime getUtc()
+{
+	return uavcan::UtcTime();
+}
 
-class Mavlink;
-class MavlinkCommansStream;
+void adjustUtc(uavcan::UtcDuration adjustment)
+{
+	(void)adjustment;
+}
+
+uavcan::uint64_t getUtcUSecFromCanInterrupt()
+{
+	return 0;
+}
 
-#include "mavlink_main.h"
+} // namespace clock
 
-class MavlinkCommandsStream
+SystemClock &SystemClock::instance()
 {
-private:
-	MavlinkOrbSubscription *_cmd_sub;
-	struct vehicle_command_s *_cmd;
-	mavlink_channel_t _channel;
-	uint64_t _cmd_time;
+	static SystemClock inst;
+	return inst;
+}
 
-public:
-	MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel);
-	void update(const hrt_abstime t);
-};
+}
 
-#endif /* MAVLINK_COMMANDS_H_ */
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
new file mode 100644
index 000000000..27e77e9c5
--- /dev/null
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -0,0 +1,582 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <cstdlib>
+#include <cstring>
+#include <fcntl.h>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/mixer/mixer.h>
+#include <arch/board/board.h>
+#include <arch/chip/chip.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_pwm_output.h>
+
+#include "uavcan_main.hpp"
+
+/**
+ * @file uavcan_main.cpp
+ *
+ * Implements basic functinality of UAVCAN node.
+ *
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+/*
+ * UavcanNode
+ */
+UavcanNode *UavcanNode::_instance;
+
+UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
+	CDev("uavcan", UAVCAN_DEVICE_PATH),
+	_node(can_driver, system_clock),
+	_esc_controller(_node),
+	_gnss_receiver(_node)
+{
+	_control_topics[0] = ORB_ID(actuator_controls_0);
+	_control_topics[1] = ORB_ID(actuator_controls_1);
+	_control_topics[2] = ORB_ID(actuator_controls_2);
+	_control_topics[3] = ORB_ID(actuator_controls_3);
+
+	// memset(_controls, 0, sizeof(_controls));
+	// memset(_poll_fds, 0, sizeof(_poll_fds));
+}
+
+UavcanNode::~UavcanNode()
+{
+	if (_task != -1) {
+		/* tell the task we want it to go away */
+		_task_should_exit = true;
+
+		unsigned i = 10;
+
+		do {
+			/* wait 5ms - it should wake every 10ms or so worst-case */
+			usleep(5000);
+
+			/* if we have given up, kill it */
+			if (--i == 0) {
+				task_delete(_task);
+				break;
+			}
+
+		} while (_task != -1);
+	}
+
+	/* clean up the alternate device node */
+		// unregister_driver(PWM_OUTPUT_DEVICE_PATH);
+
+	::close(_armed_sub);
+
+	_instance = nullptr;
+}
+
+int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
+{
+	if (_instance != nullptr) {
+		warnx("Already started");
+		return -1;
+	}
+
+	/*
+	 * GPIO config.
+	 * Forced pull up on CAN2 is required for Pixhawk v1 where the second interface lacks a transceiver.
+	 * If no transceiver is connected, the RX pin will float, occasionally causing CAN controller to
+	 * fail during initialization.
+	 */
+	stm32_configgpio(GPIO_CAN1_RX);
+	stm32_configgpio(GPIO_CAN1_TX);
+	stm32_configgpio(GPIO_CAN2_RX | GPIO_PULLUP);
+	stm32_configgpio(GPIO_CAN2_TX);
+
+	/*
+	 * CAN driver init
+	 */
+	static CanInitHelper can;
+	static bool can_initialized = false;
+
+	if (!can_initialized) {
+		const int can_init_res = can.init(bitrate);
+
+		if (can_init_res < 0) {
+			warnx("CAN driver init failed %i", can_init_res);
+			return can_init_res;
+		}
+
+		can_initialized = true;
+	}
+
+	/*
+	 * Node init
+	 */
+	_instance = new UavcanNode(can.driver, uavcan_stm32::SystemClock::instance());
+
+	if (_instance == nullptr) {
+		warnx("Out of memory");
+		return -1;
+	}
+
+	const int node_init_res = _instance->init(node_id);
+
+	if (node_init_res < 0) {
+		delete _instance;
+		_instance = nullptr;
+		warnx("Node init failed %i", node_init_res);
+		return node_init_res;
+	}
+
+	/*
+	 * Start the task. Normally it should never exit.
+	 */
+	static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();};
+	_instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, StackSize,
+			      static_cast<main_t>(run_trampoline), nullptr);
+
+	if (_instance->_task < 0) {
+		warnx("start failed: %d", errno);
+		return -errno;
+	}
+
+	return OK;
+}
+
+int UavcanNode::init(uavcan::NodeID node_id)
+{
+	int ret = -1;
+
+	/* do regular cdev init */
+	ret = CDev::init();
+
+	if (ret != OK)
+		return ret;
+
+	ret = _esc_controller.init();
+	if (ret < 0)
+		return ret;
+
+	ret = _gnss_receiver.init();
+	if (ret < 0)
+		return ret;
+
+	uavcan::protocol::SoftwareVersion swver;
+	swver.major = 12;                        // TODO fill version info
+	swver.minor = 34;
+	_node.setSoftwareVersion(swver);
+
+	uavcan::protocol::HardwareVersion hwver;
+	hwver.major = 42;                        // TODO fill version info
+	hwver.minor = 42;
+	_node.setHardwareVersion(hwver);
+
+	_node.setName("org.pixhawk"); // Huh?
+
+	_node.setNodeID(node_id);
+
+	return _node.start();
+}
+
+void UavcanNode::node_spin_once()
+{
+	const int spin_res = _node.spin(uavcan::MonotonicTime());
+	if (spin_res < 0) {
+		warnx("node spin error %i", spin_res);
+	}
+}
+
+int UavcanNode::run()
+{
+	const unsigned PollTimeoutMs = 50;
+
+	// XXX figure out the output count
+	_output_count = 2;
+
+
+	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+
+	actuator_outputs_s outputs;
+	memset(&outputs, 0, sizeof(outputs));
+
+	const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0);
+	if (busevent_fd < 0)
+	{
+		warnx("Failed to open %s", uavcan_stm32::BusEvent::DevName);
+		_task_should_exit = true;
+	}
+
+	/*
+	 * XXX Mixing logic/subscriptions shall be moved into UavcanEscController::update();
+	 *     IO multiplexing shall be done here.
+	 */
+
+	_node.setStatusOk();
+
+	while (!_task_should_exit) {
+
+		if (_groups_subscribed != _groups_required) {
+			subscribe();
+			_groups_subscribed = _groups_required;
+			/*
+			 * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error).
+			 * Please note that with such multiplexing it is no longer possible to rely only on
+			 * the value returned from poll() to detect whether actuator control has timed out or not.
+			 * Instead, all ORB events need to be checked individually (see below).
+			 */
+			_poll_fds[_poll_fds_num] = ::pollfd();
+			_poll_fds[_poll_fds_num].fd = busevent_fd;
+			_poll_fds[_poll_fds_num].events = POLLIN;
+			_poll_fds_num += 1;
+		}
+
+		const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs);
+
+		node_spin_once();  // Non-blocking
+
+		// this would be bad...
+		if (poll_ret < 0) {
+			log("poll error %d", errno);
+			continue;
+		} else {
+			// get controls for required topics
+			bool controls_updated = false;
+			unsigned poll_id = 0;
+			for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+				if (_control_subs[i] > 0) {
+					if (_poll_fds[poll_id].revents & POLLIN) {
+						controls_updated = true;
+						orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
+					}
+					poll_id++;
+				}
+			}
+
+			if (!controls_updated) {
+				// timeout: no control data, switch to failsafe values
+				// XXX trigger failsafe
+			}
+
+			//can we mix?
+			if (controls_updated && (_mixers != nullptr)) {
+
+				// XXX one output group has 8 outputs max,
+				// but this driver could well serve multiple groups.
+				unsigned num_outputs_max = 8;
+
+				// Do mixing
+				outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs_max);
+				outputs.timestamp = hrt_absolute_time();
+
+				// iterate actuators
+				for (unsigned i = 0; i < outputs.noutputs; i++) {
+					// last resort: catch NaN, INF and out-of-band errors
+					if (!isfinite(outputs.output[i])) {
+						/*
+						 * Value is NaN, INF or out of band - set to the minimum value.
+						 * This will be clearly visible on the servo status and will limit the risk of accidentally
+						 * spinning motors. It would be deadly in flight.
+						 */
+						outputs.output[i] = -1.0f;
+					}
+
+					// limit outputs to valid range
+
+					// never go below min
+					if (outputs.output[i] < -1.0f) {
+						outputs.output[i] = -1.0f;
+					}
+
+					// never go below max
+					if (outputs.output[i] > 1.0f) {
+						outputs.output[i] = 1.0f;
+					}
+				}
+
+				// Output to the bus
+				_esc_controller.update_outputs(outputs.output, outputs.noutputs);
+			}
+
+		}
+
+		// Check arming state
+		bool updated = false;
+		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;
+
+			arm_actuators(set_armed);
+		}
+	}
+
+	teardown();
+	warnx("exiting.");
+
+	exit(0);
+}
+
+int
+UavcanNode::control_callback(uintptr_t handle,
+			 uint8_t control_group,
+			 uint8_t control_index,
+			 float &input)
+{
+	const actuator_controls_s *controls = (actuator_controls_s *)handle;
+
+	input = controls[control_group].control[control_index];
+	return 0;
+}
+
+int
+UavcanNode::teardown()
+{
+	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+		if (_control_subs[i] > 0) {
+			::close(_control_subs[i]);
+			_control_subs[i] = -1;
+		}
+	}
+	return ::close(_armed_sub);
+}
+
+int
+UavcanNode::arm_actuators(bool arm)
+{
+	_is_armed = arm;
+	_esc_controller.arm_esc(arm);
+	return OK;
+}
+
+void
+UavcanNode::subscribe()
+{
+	// Subscribe/unsubscribe to required actuator control groups
+	uint32_t sub_groups = _groups_required & ~_groups_subscribed;
+	uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
+	_poll_fds_num = 0;
+	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+		if (sub_groups & (1 << i)) {
+			warnx("subscribe to actuator_controls_%d", i);
+			_control_subs[i] = orb_subscribe(_control_topics[i]);
+		}
+		if (unsub_groups & (1 << i)) {
+			warnx("unsubscribe from actuator_controls_%d", i);
+			::close(_control_subs[i]);
+			_control_subs[i] = -1;
+		}
+
+		if (_control_subs[i] > 0) {
+			_poll_fds[_poll_fds_num].fd = _control_subs[i];
+			_poll_fds[_poll_fds_num].events = POLLIN;
+			_poll_fds_num++;
+		}
+	}
+}
+
+int
+UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
+{
+	int ret = OK;
+
+	lock();
+
+	switch (cmd) {
+	case PWM_SERVO_ARM:
+		arm_actuators(true);
+		break;
+
+	case PWM_SERVO_SET_ARM_OK:
+	case PWM_SERVO_CLEAR_ARM_OK:
+	case PWM_SERVO_SET_FORCE_SAFETY_OFF:
+		// these are no-ops, as no safety switch
+		break;
+
+	case PWM_SERVO_DISARM:
+		arm_actuators(false);
+		break;
+
+	case MIXERIOCGETOUTPUTCOUNT:
+		*(unsigned *)arg = _output_count;
+		break;
+
+	case MIXERIOCRESET:
+		if (_mixers != nullptr) {
+			delete _mixers;
+			_mixers = nullptr;
+			_groups_required = 0;
+		}
+
+		break;
+
+	case MIXERIOCLOADBUF: {
+			const char *buf = (const char *)arg;
+			unsigned buflen = strnlen(buf, 1024);
+
+			if (_mixers == nullptr)
+				_mixers = new MixerGroup(control_callback, (uintptr_t)_controls);
+
+			if (_mixers == nullptr) {
+				_groups_required = 0;
+				ret = -ENOMEM;
+
+			} else {
+
+				ret = _mixers->load_from_buf(buf, buflen);
+
+				if (ret != 0) {
+					warnx("mixer load failed with %d", ret);
+					delete _mixers;
+					_mixers = nullptr;
+					_groups_required = 0;
+					ret = -EINVAL;
+				} else {
+
+					_mixers->groups_required(_groups_required);
+				}
+			}
+
+			break;
+		}
+
+	default:
+		ret = -ENOTTY;
+		break;
+	}
+
+	unlock();
+
+	if (ret == -ENOTTY) {
+		ret = CDev::ioctl(filp, cmd, arg);
+	}
+
+	return ret;
+}
+
+void
+UavcanNode::print_info()
+{
+	if (!_instance) {
+		warnx("not running, start first");
+	}
+
+	warnx("groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
+	warnx("mixer: %s", (_mixers == nullptr) ? "FAIL" : "OK");
+}
+
+/*
+ * App entry point
+ */
+static void print_usage()
+{
+	warnx("usage: uavcan start <node_id> [can_bitrate]");
+}
+
+extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
+
+int uavcan_main(int argc, char *argv[])
+{
+	constexpr unsigned DEFAULT_CAN_BITRATE = 1000000;
+
+	if (argc < 2) {
+		print_usage();
+		::exit(1);
+	}
+
+	if (!std::strcmp(argv[1], "start")) {
+		if (argc < 3) {
+			print_usage();
+			::exit(1);
+		}
+
+		/*
+		 * Node ID
+		 */
+		const int node_id = atoi(argv[2]);
+
+		if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) {
+			warnx("Invalid Node ID %i", node_id);
+			::exit(1);
+		}
+
+		/*
+		 * CAN bitrate
+		 */
+		unsigned bitrate = 0;
+
+		if (argc > 3) {
+			bitrate = atol(argv[3]);
+		}
+
+		if (bitrate <= 0) {
+			bitrate = DEFAULT_CAN_BITRATE;
+		}
+
+		if (UavcanNode::instance()) {
+			errx(1, "already started");
+		}
+
+		/*
+		 * Start
+		 */
+		warnx("Node ID %u, bitrate %u", node_id, bitrate);
+		return UavcanNode::start(node_id, bitrate);
+
+	}
+
+	/* commands below require the app to be started */
+	UavcanNode *inst = UavcanNode::instance();
+
+	if (!inst) {
+		errx(1, "application not running");
+	}
+
+	if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) {
+		
+			inst->print_info();
+			return OK;
+	}
+
+	if (!std::strcmp(argv[1], "stop")) {
+		
+			delete inst;
+			inst = nullptr;
+			return OK;
+	}
+
+	print_usage();
+	::exit(1);
+}
diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp
new file mode 100644
index 000000000..443525379
--- /dev/null
+++ b/src/modules/uavcan/uavcan_main.hpp
@@ -0,0 +1,123 @@
+/****************************************************************************
+ *
+ *   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
+
+#include <nuttx/config.h>
+
+#include <uavcan_stm32/uavcan_stm32.hpp>
+#include <drivers/device/device.h>
+
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_armed.h>
+
+#include "esc_controller.hpp"
+#include "gnss_receiver.hpp"
+
+/**
+ * @file uavcan_main.hpp
+ *
+ * Defines basic functinality of UAVCAN node.
+ *
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN	4
+#define UAVCAN_DEVICE_PATH	"/dev/uavcan/esc"
+
+/**
+ * A UAVCAN node.
+ */
+class UavcanNode : public device::CDev
+{
+	static constexpr unsigned MemPoolSize        = 10752;
+	static constexpr unsigned RxQueueLenPerIface = 64;
+	static constexpr unsigned StackSize          = 3000;
+
+public:
+	typedef uavcan::Node<MemPoolSize> Node;
+	typedef uavcan_stm32::CanInitHelper<RxQueueLenPerIface> CanInitHelper;
+
+	UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock);
+
+	virtual		~UavcanNode();
+
+	virtual int	ioctl(file *filp, int cmd, unsigned long arg);
+
+	static int	start(uavcan::NodeID node_id, uint32_t bitrate);
+
+	Node&		getNode() { return _node; }
+
+	static int	control_callback(uintptr_t handle,
+					 uint8_t control_group,
+					 uint8_t control_index,
+					 float &input);
+
+	void		subscribe();
+
+	int		teardown();
+	int		arm_actuators(bool arm);
+
+	void		print_info();
+
+	static UavcanNode* instance() { return _instance; }
+
+private:
+	int		init(uavcan::NodeID node_id);
+	void		node_spin_once();
+	int		run();
+
+	int			_task = -1;			///< handle to the OS task
+	bool			_task_should_exit = false;	///< flag to indicate to tear down the CAN driver
+	int			_armed_sub = -1;		///< uORB subscription of the arming status
+	actuator_armed_s	_armed;				///< the arming request of the system
+	bool			_is_armed = false;		///< the arming status of the actuators on the bus
+
+	unsigned		_output_count = 0;		///< number of actuators currently available
+
+	static UavcanNode	*_instance;			///< singleton pointer
+	Node			_node;				///< library instance
+	UavcanEscController	_esc_controller;
+	UavcanGnssReceiver 	_gnss_receiver;
+
+	MixerGroup		*_mixers = nullptr;
+
+	uint32_t		_groups_required = 0;
+	uint32_t		_groups_subscribed = 0;
+	int			_control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
+	actuator_controls_s 	_controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
+	orb_id_t		_control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
+	pollfd			_poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN + 1] = {};	///< +1 for /dev/uavcan/busevent
+	unsigned		_poll_fds_num = 0;
+};
diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk
index b22b446da..e2fa0ff80 100644
--- a/src/systemcmds/nshterm/module.mk
+++ b/src/systemcmds/nshterm/module.mk
@@ -38,4 +38,4 @@
 MODULE_COMMAND	 = nshterm
 SRCS		 = nshterm.c
 
-MODULE_STACKSIZE = 1200
+MODULE_STACKSIZE = 1400
diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c
index e0e6ca537..c8d698b86 100644
--- a/src/systemcmds/pwm/pwm.c
+++ b/src/systemcmds/pwm/pwm.c
@@ -70,7 +70,7 @@ usage(const char *reason)
 {
 	if (reason != NULL)
 		warnx("%s", reason);
-	errx(1, 
+	errx(1,
 		"usage:\n"
 		"pwm arm|disarm|rate|failsafe|disarmed|min|max|test|info  ...\n"
 		"\n"
@@ -635,8 +635,28 @@ pwm_main(int argc, char *argv[])
 		}
 		exit(0);
 
+	} else if (!strcmp(argv[1], "forcefail")) {
+
+		if (argc < 3) {
+			errx(1, "arg missing [on|off]");
+		} else {
+
+			if (!strcmp(argv[2], "on")) {
+				/* force failsafe */
+				ret = ioctl(fd, PWM_SERVO_SET_FORCE_FAILSAFE, 1);
+			} else {
+				/* force failsafe */
+				ret = ioctl(fd, PWM_SERVO_SET_FORCE_FAILSAFE, 0);
+			}
+
+			if (ret != OK) {
+				warnx("FAILED setting forcefail %s", argv[2]);
+			}
+		}
+		exit(0);
 	}
-	usage("specify arm|disarm|rate|failsafe|disarmed|min|max|test|info");
+
+	usage("specify arm|disarm|rate|failsafe|disarmed|min|max|test|info|forcefail");
 	return 0;
 }