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

Conflicts:
	src/modules/mavlink/mavlink_main.cpp
	src/modules/mavlink/mavlink_main.h
	src/modules/mavlink/mavlink_receiver.cpp

57 files changed, 2559 insertions, 1733 deletions

diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h
index b7981e9c4..307f7dbc7 100644
--- a/src/drivers/drv_tone_alarm.h
+++ b/src/drivers/drv_tone_alarm.h
@@ -149,6 +149,8 @@ enum {
 	TONE_GPS_WARNING_TUNE,
 	TONE_ARMING_FAILURE_TUNE,
 	TONE_PARACHUTE_RELEASE_TUNE,
+	TONE_EKF_WARNING_TUNE,
+	TONE_BARO_WARNING_TUNE,
 	TONE_NUMBER_OF_TUNES
 };
 
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index 6ab437716..159706278 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.2f
+#define CONVERSION_INTERVAL	(1000000 / MEAS_RATE)	/* microseconds */
 
 class MEASAirspeed : public Airspeed
 {
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 bf6893a7e..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)
 {
 }
 
@@ -245,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];
 
@@ -262,19 +263,19 @@ 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;
+	int ret = OK;
 	while (count--) {
-		ret = recv(*p++, 5000);
+		ret = recv_byte_with_timeout(p++, 5000);
 
 		if (ret != OK)
 			break;
@@ -289,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) {
@@ -331,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;
@@ -372,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;
@@ -513,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);
@@ -600,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 3e2142cf2..e17523413 100644
--- a/src/drivers/px4io/uploader.h
+++ b/src/drivers/px4io/uploader.h
@@ -74,19 +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 */
+		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/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
index 810f4aacc..8f523b390 100644
--- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp
+++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
@@ -336,6 +336,8 @@ 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
+	_default_tunes[TONE_BARO_WARNING_TUNE] = "MFT255L4gf#fed#d";				// baro warning
 
 	_tune_names[TONE_STARTUP_TUNE] = "startup";			// startup tune
 	_tune_names[TONE_ERROR_TUNE] = "error";				// ERROR tone
@@ -348,6 +350,8 @@ 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
+	_tune_names[TONE_BARO_WARNING_TUNE] = "baro_warning";			// baro warning
 }
 
 ToneAlarm::~ToneAlarm()
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/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp
index 6386e37a0..a57a0481a 100644
--- a/src/lib/external_lgpl/tecs/tecs.cpp
+++ b/src/lib/external_lgpl/tecs/tecs.cpp
@@ -298,7 +298,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
 
 	} else {
 		// Calculate gain scaler from specific energy error to throttle
-		float K_STE2Thr = 1 / (_timeConst * (_STEdot_max - _STEdot_min));
+		float K_STE2Thr = 1 / (_timeConstThrot * (_STEdot_max - _STEdot_min));
 
 		// Calculate feed-forward throttle
 		float ff_throttle = 0;
@@ -327,9 +327,12 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
 			_throttle_dem = constrain(_throttle_dem,
 						  _last_throttle_dem - thrRateIncr,
 						  _last_throttle_dem + thrRateIncr);
-			_last_throttle_dem = _throttle_dem;
 		}
 
+		// Ensure _last_throttle_dem is always initialized properly
+		// Also: The throttle_slewrate limit is only applied to throttle_dem, but does not limit the integrator!!
+		_last_throttle_dem = _throttle_dem;
+
 
 		// Calculate integrator state upper and lower limits
 		// Set to a value thqat will allow 0.1 (10%) throttle saturation to allow for noise on the demand
@@ -551,18 +554,30 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f
 	// Calculate pitch demand
 	_update_pitch();
 
-//    // Write internal variables to the log_tuning structure. This
-//    // structure will be logged in dataflash at 10Hz
-	// log_tuning.hgt_dem  = _hgt_dem_adj;
-	// log_tuning.hgt      = _integ3_state;
-	// log_tuning.dhgt_dem = _hgt_rate_dem;
-	// log_tuning.dhgt     = _integ2_state;
-	// log_tuning.spd_dem  = _TAS_dem_adj;
-	// log_tuning.spd      = _integ5_state;
-	// log_tuning.dspd     = _vel_dot;
-	// log_tuning.ithr     = _integ6_state;
-	// log_tuning.iptch    = _integ7_state;
-	// log_tuning.thr      = _throttle_dem;
-	// log_tuning.ptch     = _pitch_dem;
-	// log_tuning.dspd_dem = _TAS_rate_dem;
+	_tecs_state.timestamp = now;
+
+	if (_underspeed) {
+		_tecs_state.mode = ECL_TECS_MODE_UNDERSPEED;
+	} else if (_badDescent) {
+		_tecs_state.mode = ECL_TECS_MODE_BAD_DESCENT;
+	} else if (_climbOutDem) {
+		_tecs_state.mode = ECL_TECS_MODE_CLIMBOUT;
+	} else {
+		// If no error flag applies, conclude normal
+		_tecs_state.mode = ECL_TECS_MODE_NORMAL;
+	}
+
+	_tecs_state.hgt_dem  = _hgt_dem_adj;
+	_tecs_state.hgt      = _integ3_state;
+	_tecs_state.dhgt_dem = _hgt_rate_dem;
+	_tecs_state.dhgt     = _integ2_state;
+	_tecs_state.spd_dem  = _TAS_dem_adj;
+	_tecs_state.spd      = _integ5_state;
+	_tecs_state.dspd     = _vel_dot;
+	_tecs_state.ithr     = _integ6_state;
+	_tecs_state.iptch    = _integ7_state;
+	_tecs_state.thr      = _throttle_dem;
+	_tecs_state.ptch     = _pitch_dem;
+	_tecs_state.dspd_dem = _TAS_rate_dem;
+
 }
diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h
index 5cafb1c79..bcc2d90e5 100644
--- a/src/lib/external_lgpl/tecs/tecs.h
+++ b/src/lib/external_lgpl/tecs/tecs.h
@@ -28,6 +28,27 @@ class __EXPORT TECS
 {
 public:
 	TECS() :
+		_tecs_state {},
+		_update_50hz_last_usec(0),
+		_update_speed_last_usec(0),
+		_update_pitch_throttle_last_usec(0),
+		// TECS tuning parameters
+		_hgtCompFiltOmega(0.0f),
+		_spdCompFiltOmega(0.0f),
+		_maxClimbRate(2.0f),
+		_minSinkRate(1.0f),
+		_maxSinkRate(2.0f),
+		_timeConst(5.0f),
+		_timeConstThrot(8.0f),
+		_ptchDamp(0.0f),
+		_thrDamp(0.0f),
+		_integGain(0.0f),
+		_vertAccLim(0.0f),
+		_rollComp(0.0f),
+		_spdWeight(0.5f),
+		_heightrate_p(0.0f),
+		_speedrate_p(0.0f),
+		_throttle_dem(0.0f),
 		_pitch_dem(0.0f),
 		_integ1_state(0.0f),
 		_integ2_state(0.0f),
@@ -100,29 +121,42 @@ public:
 		return _spdWeight;
 	}
 
-	// log data on internal state of the controller. Called at 10Hz
-	// void log_data(DataFlash_Class &dataflash, uint8_t msgid);
-
-	// struct PACKED log_TECS_Tuning {
-	// 	LOG_PACKET_HEADER;
-	// 	float hgt;
-	// 	float dhgt;
-	// 	float hgt_dem;
-	// 	float dhgt_dem;
-	// 	float spd_dem;
-	// 	float spd;
-	// 	float dspd;
-	// 	float ithr;
-	// 	float iptch;
-	// 	float thr;
-	// 	float ptch;
-	// 	float dspd_dem;
-	// } log_tuning;
+	enum ECL_TECS_MODE {
+		ECL_TECS_MODE_NORMAL = 0,
+		ECL_TECS_MODE_UNDERSPEED,
+		ECL_TECS_MODE_BAD_DESCENT,
+		ECL_TECS_MODE_CLIMBOUT
+	};
+
+	struct tecs_state {
+		uint64_t timestamp;
+		float hgt;
+		float dhgt;
+		float hgt_dem;
+		float dhgt_dem;
+		float spd_dem;
+		float spd;
+		float dspd;
+		float ithr;
+		float iptch;
+		float thr;
+		float ptch;
+		float dspd_dem;
+		enum ECL_TECS_MODE mode;
+	};
+
+	void get_tecs_state(struct tecs_state& state) {
+		state = _tecs_state;
+	}
 
 	void set_time_const(float time_const) {
 		_timeConst = time_const;
 	}
 
+	void set_time_const_throt(float time_const_throt) {
+		_timeConstThrot = time_const_throt;
+	}
+
 	void set_min_sink_rate(float rate) {
 		_minSinkRate = rate;
 	}
@@ -188,6 +222,9 @@ public:
 	}
 
 private:
+
+	struct tecs_state _tecs_state;
+
 	// Last time update_50Hz was called
 	uint64_t _update_50hz_last_usec;
 
@@ -204,6 +241,7 @@ private:
 	float _minSinkRate;
 	float _maxSinkRate;
 	float _timeConst;
+	float _timeConstThrot;
 	float _ptchDamp;
 	float _thrDamp;
 	float _integGain;
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index eac8b0afc..2d2aeecfa 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));
@@ -391,7 +393,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
 
 	// Transition the armed state. By passing mavlink_fd to arming_state_transition it will
 	// output appropriate error messages if the state cannot transition.
-	arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd_local);
+	arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd_local);
 
 	if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
 		mavlink_log_info(mavlink_fd_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
@@ -758,6 +760,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);
@@ -999,6 +1002,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;
 
@@ -1103,7 +1107,7 @@ int commander_thread_main(int argc, char *argv[])
 			if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) {
 				arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
 
-				if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) {
+				if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd)) {
 					mavlink_log_info(mavlink_fd, "DISARMED by safety switch");
 					arming_state_changed = true;
 				}
@@ -1128,32 +1132,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;
@@ -1183,8 +1187,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 {
@@ -1192,7 +1196,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 {
@@ -1306,14 +1310,14 @@ int commander_thread_main(int argc, char *argv[])
 			status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
 
 			if (armed.armed) {
-				arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, mavlink_fd);
+				arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd);
 
 				if (arming_ret == TRANSITION_CHANGED) {
 					arming_state_changed = true;
 				}
 
 			} else {
-				arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, mavlink_fd);
+				arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd);
 
 				if (arming_ret == TRANSITION_CHANGED) {
 					arming_state_changed = true;
@@ -1327,7 +1331,7 @@ int commander_thread_main(int argc, char *argv[])
 		/* If in INIT state, try to proceed to STANDBY state */
 		if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
 			/* TODO: check for sensors */
-			arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd);
+			arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
 
 			if (arming_ret == TRANSITION_CHANGED) {
 				arming_state_changed = true;
@@ -1386,7 +1390,7 @@ int commander_thread_main(int argc, char *argv[])
 				if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
 					/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
 					arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
-					arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd);
+					arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd);
 					if (arming_ret == TRANSITION_CHANGED) {
 						arming_state_changed = true;
 					}
@@ -1412,7 +1416,7 @@ int commander_thread_main(int argc, char *argv[])
 					if (status.main_state != MAIN_STATE_MANUAL) {
 						print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
 					} else {
-						arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd);
+						arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, mavlink_fd);
 						if (arming_ret == TRANSITION_CHANGED) {
 							arming_state_changed = true;
 						}
@@ -1524,7 +1528,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;
@@ -2184,7 +2188,7 @@ void *commander_low_prio_loop(void *arg)
 				int calib_ret = ERROR;
 
 				/* try to go to INIT/PREFLIGHT arming state */
-				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, mavlink_fd)) {
+				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, true /* fRunPreArmChecks */, mavlink_fd)) {
 					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
 					break;
 				}
@@ -2247,7 +2251,7 @@ void *commander_low_prio_loop(void *arg)
 					tune_negative(true);
 				}
 
-				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd);
+				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
 
 				break;
 			}
diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
index 2e18c4284..08dda2fab 100644
--- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
@@ -286,7 +286,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
         armed.ready_to_arm = test->current_state.ready_to_arm;
         
         // Attempt transition
-        transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, 0 /* no mavlink_fd */);
+        transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, false /* no pre-arm checks */, 0 /* no mavlink_fd */);
         
         // Validate result of transition
         ut_assert(test->assertMsg, test->expected_transition_result == result);
@@ -335,12 +335,12 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
 			MTT_ALL_NOT_VALID,
 			MAIN_STATE_ACRO, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
 
-		{ "transition: MANUAL to AUTO_MISSION - global position valid",
-			MTT_GLOBAL_POS_VALID,
+		{ "transition: MANUAL to AUTO_MISSION - global position valid, home position valid",
+			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
 			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED },
 
-		{ "transition: AUTO_MISSION to MANUAL - global position valid",
-			MTT_GLOBAL_POS_VALID,
+		{ "transition: AUTO_MISSION to MANUAL - global position valid, home position valid",
+			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
 			MAIN_STATE_AUTO_MISSION, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
 
 		{ "transition: MANUAL to AUTO_LOITER - global position valid",
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 7b26e3e8c..f8589d24b 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -99,11 +99,12 @@ static const char * const state_names[ARMING_STATE_MAX] = {
 };
 
 transition_result_t
-arming_state_transition(struct vehicle_status_s *status,            /// current vehicle status
-			const struct safety_s   *safety,            /// current safety settings
-			arming_state_t          new_arming_state,   /// arming state requested
-			struct actuator_armed_s *armed,             /// current armed status
-			const int               mavlink_fd)         /// mavlink fd for error reporting, 0 for none
+arming_state_transition(struct vehicle_status_s *status,		///< current vehicle status
+			const struct safety_s   *safety,		///< current safety settings
+			arming_state_t          new_arming_state,	///< arming state requested
+			struct actuator_armed_s *armed,			///< current armed status
+			bool			fRunPreArmChecks,	///< true: run the pre-arm checks, false: no pre-arm checks, for unit testing
+			const int               mavlink_fd)		///< mavlink fd for error reporting, 0 for none
 {
 	// Double check that our static arrays are still valid
 	ASSERT(ARMING_STATE_INIT == 0);
@@ -125,7 +126,7 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 		int prearm_ret = OK;
 
 		/* only perform the check if we have to */
-		if (new_arming_state == ARMING_STATE_ARMED) {
+		if (fRunPreArmChecks && new_arming_state == ARMING_STATE_ARMED) {
 			prearm_ret = prearm_check(status, mavlink_fd);
 		}
 
@@ -669,7 +670,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/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index bb1b87e71..69ce8bbce 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -57,7 +57,7 @@ typedef enum {
 bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
 
 transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
-		arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd);
+		arming_state_t new_arming_state, struct actuator_armed_s *armed, bool fRunPreArmChecks, const int mavlink_fd);
 
 transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state);
 
diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp
index 0175acda9..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());
diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp
index 37d7832b3..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() {};
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 ccc497323..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
@@ -630,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);
@@ -1469,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..c7c7305b2 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
@@ -1032,10 +1032,16 @@ void AttPosEKF::FuseVelposNED()
             // apply a 5-sigma threshold
             current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0f * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]);
             current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime;
-            if (current_ekf_state.velHealth || current_ekf_state.velTimeout)
-            {
+            if (current_ekf_state.velHealth || staticMode) {
                 current_ekf_state.velHealth = true;
                 current_ekf_state.velFailTime = millis();
+            } else if (current_ekf_state.velTimeout || !current_ekf_state.posHealth) {
+                // XXX check
+                current_ekf_state.velHealth = true;
+                ResetVelocity();
+                ResetStoredStates();
+                // do not fuse bad data
+                fuseVelData = false;
             }
             else
             {
@@ -1056,6 +1062,17 @@ void AttPosEKF::FuseVelposNED()
             {
                 current_ekf_state.posHealth = true;
                 current_ekf_state.posFailTime = millis();
+
+                if (current_ekf_state.posTimeout) {
+                    ResetPosition();
+                    
+                    // XXX cross-check the state reset
+                    ResetStoredStates();
+
+                    // do not fuse position data on this time
+                    // step
+                    fusePosData = false;
+                }
             }
             else
             {
@@ -1070,10 +1087,18 @@ void AttPosEKF::FuseVelposNED()
             // apply a 10-sigma threshold
             current_ekf_state.hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5];
             current_ekf_state.hgtTimeout = (millis() - current_ekf_state.hgtFailTime) > hgtRetryTime;
-            if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout)
+            if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout || staticMode)
             {
                 current_ekf_state.hgtHealth = true;
                 current_ekf_state.hgtFailTime = millis();
+
+                // if we just reset from a timeout, do not fuse
+                // the height data, but reset height and stored states
+                if (current_ekf_state.hgtTimeout) {
+                    ResetHeight();
+                    ResetStoredStates();
+                    fuseHgtData = false;
+                }
             }
             else
             {
@@ -2773,7 +2798,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 +3053,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.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/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 0b111f7bd..350ce6dec 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,7 +134,6 @@ 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;
@@ -144,6 +146,7 @@ private:
 	int		_range_finder_sub;		/**< range finder subscription */
 
 	orb_advert_t	_attitude_sp_pub;		/**< attitude setpoint */
+	orb_advert_t	_tecs_status_pub;		/**< TECS status publication */
 	orb_advert_t	_nav_capabilities_pub;		/**< navigation capabilities publication */
 
 	struct vehicle_attitude_s			_att;				/**< vehicle attitude */
@@ -192,6 +195,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;
 
@@ -199,6 +203,23 @@ private:
 		float l1_period;
 		float l1_damping;
 
+		float time_const;
+		float time_const_throt;
+		float min_sink_rate;
+		float max_sink_rate;
+		float max_climb_rate;
+		float climbout_diff;
+		float heightrate_p;
+		float speedrate_p;
+		float throttle_damp;
+		float integrator_gain;
+		float vertical_accel_limit;
+		float height_comp_filter_omega;
+		float speed_comp_filter_omega;
+		float roll_throttle_compensation;
+		float speed_weight;
+		float pitch_damping;
+
 		float airspeed_min;
 		float airspeed_trim;
 		float airspeed_max;
@@ -209,6 +230,7 @@ private:
 		float throttle_min;
 		float throttle_max;
 		float throttle_cruise;
+		float throttle_slew_max;
 
 		float throttle_land_max;
 
@@ -226,6 +248,23 @@ private:
 		param_t l1_period;
 		param_t l1_damping;
 
+		param_t time_const;
+		param_t time_const_throt;
+		param_t min_sink_rate;
+		param_t max_sink_rate;
+		param_t max_climb_rate;
+		param_t climbout_diff;
+		param_t heightrate_p;
+		param_t speedrate_p;
+		param_t throttle_damp;
+		param_t integrator_gain;
+		param_t vertical_accel_limit;
+		param_t height_comp_filter_omega;
+		param_t speed_comp_filter_omega;
+		param_t roll_throttle_compensation;
+		param_t speed_weight;
+		param_t pitch_damping;
+
 		param_t airspeed_min;
 		param_t airspeed_trim;
 		param_t airspeed_max;
@@ -236,6 +275,7 @@ private:
 		param_t throttle_min;
 		param_t throttle_max;
 		param_t throttle_cruise;
+		param_t throttle_slew_max;
 
 		param_t throttle_land_max;
 
@@ -361,7 +401,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_mavlink_fd(-1),
 	_task_should_exit(false),
 	_task_running(false),
-	_control_task(-1),
 
 /* subscriptions */
 	_global_pos_sub(-1),
@@ -376,6 +415,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 
 /* publications */
 	_attitude_sp_pub(-1),
+	_tecs_status_pub(-1),
 	_nav_capabilities_pub(-1),
 
 /* states */
@@ -428,6 +468,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.roll_limit = param_find("FW_R_LIM");
 	_parameter_handles.throttle_min = param_find("FW_THR_MIN");
 	_parameter_handles.throttle_max = param_find("FW_THR_MAX");
+	_parameter_handles.throttle_slew_max = param_find("FW_THR_SLEW_MAX");
 	_parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE");
 	_parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX");
 
@@ -438,6 +479,23 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST");
 	_parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT");
 
+	_parameter_handles.time_const = 			param_find("FW_T_TIME_CONST");
+	_parameter_handles.time_const_throt = 			param_find("FW_T_THRO_CONST");
+	_parameter_handles.min_sink_rate = 			param_find("FW_T_SINK_MIN");
+	_parameter_handles.max_sink_rate =			param_find("FW_T_SINK_MAX");
+	_parameter_handles.max_climb_rate =			param_find("FW_T_CLMB_MAX");
+	_parameter_handles.climbout_diff =			param_find("FW_CLMBOUT_DIFF");
+	_parameter_handles.throttle_damp = 			param_find("FW_T_THR_DAMP");
+	_parameter_handles.integrator_gain =			param_find("FW_T_INTEG_GAIN");
+	_parameter_handles.vertical_accel_limit =		param_find("FW_T_VERT_ACC");
+	_parameter_handles.height_comp_filter_omega =		param_find("FW_T_HGT_OMEGA");
+	_parameter_handles.speed_comp_filter_omega =		param_find("FW_T_SPD_OMEGA");
+	_parameter_handles.roll_throttle_compensation = 	param_find("FW_T_RLL2THR");
+	_parameter_handles.speed_weight = 			param_find("FW_T_SPDWEIGHT");
+	_parameter_handles.pitch_damping = 			param_find("FW_T_PTCH_DAMP");
+	_parameter_handles.heightrate_p =			param_find("FW_T_HRATE_P");
+	_parameter_handles.speedrate_p =			param_find("FW_T_SRATE_P");
+
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -485,9 +543,28 @@ FixedwingPositionControl::parameters_update()
 	param_get(_parameter_handles.throttle_min, &(_parameters.throttle_min));
 	param_get(_parameter_handles.throttle_max, &(_parameters.throttle_max));
 	param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise));
+	param_get(_parameter_handles.throttle_slew_max, &(_parameters.throttle_slew_max));
 
 	param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max));
 
+	param_get(_parameter_handles.time_const, &(_parameters.time_const));
+	param_get(_parameter_handles.time_const_throt, &(_parameters.time_const_throt));
+	param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate));
+	param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate));
+	param_get(_parameter_handles.throttle_damp, &(_parameters.throttle_damp));
+	param_get(_parameter_handles.integrator_gain, &(_parameters.integrator_gain));
+	param_get(_parameter_handles.vertical_accel_limit, &(_parameters.vertical_accel_limit));
+	param_get(_parameter_handles.height_comp_filter_omega, &(_parameters.height_comp_filter_omega));
+	param_get(_parameter_handles.speed_comp_filter_omega, &(_parameters.speed_comp_filter_omega));
+	param_get(_parameter_handles.roll_throttle_compensation, &(_parameters.roll_throttle_compensation));
+	param_get(_parameter_handles.speed_weight, &(_parameters.speed_weight));
+	param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping));
+	param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate));
+	param_get(_parameter_handles.climbout_diff, &(_parameters.climbout_diff));
+
+	param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p));
+	param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p));
+
 	param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle));
 	param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt));
 	param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative));
@@ -500,6 +577,25 @@ 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_time_const_throt(_parameters.time_const_throt);
+	_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_throttle_slewrate(_parameters.throttle_slew_max);
+	_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 ||
@@ -561,6 +657,9 @@ FixedwingPositionControl::vehicle_airspeed_poll()
 		}
 	}
 
+	/* update TECS state */
+	_tecs.enable_airspeed(_airspeed_valid);
+
 	return airspeed_updated;
 }
 
@@ -621,7 +720,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
@@ -733,6 +842,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 	float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
 
+	/* 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;
 
@@ -758,6 +875,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);
 
@@ -991,7 +1111,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				usePreTakeoffThrust = false;
 
 				/* apply minimum pitch and limit roll if target altitude is not within 10 meters */
-				if (altitude_error > 15.0f) {
+				if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) {
 
 					/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
 					tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
@@ -1066,9 +1186,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;
@@ -1084,10 +1204,6 @@ void
 FixedwingPositionControl::task_main()
 {
 
-	/* inform about start */
-	warnx("Initializing..");
-	fflush(stdout);
-
 	/*
 	 * do subscriptions
 	 */
@@ -1248,20 +1364,74 @@ 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);
+
+		struct TECS::tecs_state s;
+		_tecs.get_tecs_state(s);
+
+		struct tecs_status_s t;
+
+		t.timestamp = s.timestamp;
+
+		switch (s.mode) {
+			case TECS::ECL_TECS_MODE_NORMAL:
+				t.mode = TECS_MODE_NORMAL;
+				break;
+			case TECS::ECL_TECS_MODE_UNDERSPEED:
+				t.mode = TECS_MODE_UNDERSPEED;
+				break;
+			case TECS::ECL_TECS_MODE_BAD_DESCENT:
+				t.mode = TECS_MODE_BAD_DESCENT;
+				break;
+			case TECS::ECL_TECS_MODE_CLIMBOUT:
+				t.mode = TECS_MODE_CLIMBOUT;
+				break;
+		}
+
+		t.altitudeSp			= s.hgt_dem;
+		t.altitude_filtered		= s.hgt;
+		t.airspeedSp			= s.spd_dem;
+		t.airspeed_filtered		= s.spd;
+
+		t.flightPathAngleSp		= s.dhgt_dem;
+		t.flightPathAngle		= s.dhgt;
+		t.flightPathAngleFiltered	= s.dhgt;
+
+		t.airspeedDerivativeSp		= s.dspd_dem;
+		t.airspeedDerivative		= s.dspd;
+
+		t.totalEnergyRateSp		= s.thr;
+		t.totalEnergyRate		= s.ithr;
+		t.energyDistributionRateSp	= s.ptch;
+		t.energyDistributionRate	= s.iptch;
+
+		if (_tecs_status_pub > 0) {
+			orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t);
+		} else {
+			_tecs_status_pub = orb_advertise(ORB_ID(tecs_status), &t);
+		}
 	}
-	_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
-			limitOverride);
 }
 
 int
@@ -1295,14 +1465,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/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
index 27039ff51..41c374407 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
@@ -83,6 +83,17 @@ PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f);
 PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.7f);
 
 /**
+ * Throttle max slew rate
+ *
+ * Maximum slew rate for the commanded throttle
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_THR_SLEW_MAX, 0.0f);
+
+/**
  * Negative pitch limit
  *
  * The minimum negative pitch the controller will output.
@@ -155,6 +166,205 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
 PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f);
 
 /**
+ * Climbout Altitude difference
+ *
+ * If the altitude error exceeds this parameter, the system will climb out
+ * with maximum throttle and minimum airspeed until it is closer than this
+ * distance to the desired altitude. Mostly used for takeoff waypoints / modes.
+ * Set to zero to disable climbout mode (not recommended).
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.0f);
+
+/**
+ * Maximum climb rate
+ *
+ * This is the best climb rate that the aircraft can achieve with 
+ * the throttle set to THR_MAX and the airspeed set to the 
+ * default value. For electric aircraft make sure this number can be 
+ * achieved towards the end of flight when the battery voltage has reduced. 
+ * The setting of this parameter can be checked by commanding a positive 
+ * altitude change of 100m in loiter, RTL or guided mode. If the throttle 
+ * required to climb is close to THR_MAX and the aircraft is maintaining 
+ * airspeed, then this parameter is set correctly. If the airspeed starts 
+ * to reduce, then the parameter is set to high, and if the throttle 
+ * demand required to climb and maintain speed is noticeably less than 
+ * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or 
+ * FW_THR_MAX reduced.
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f);
+
+/**
+ * Minimum descent rate
+ *
+ * This is the sink rate of the aircraft with the throttle 
+ * set to THR_MIN and flown at the same airspeed as used 
+ * to measure FW_T_CLMB_MAX.
+ *
+ * @group Fixed Wing TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f);
+
+/**
+ * Maximum descent rate
+ *
+ * This sets the maximum descent rate that the controller will use. 
+ * If this value is too large, the aircraft can over-speed on descent. 
+ * This should be set to a value that can be achieved without 
+ * exceeding the lower pitch angle limit and without over-speeding 
+ * the aircraft.
+ *
+ * @group Fixed Wing TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
+
+/**
+ * TECS time constant
+ *
+ * This is the time constant of the TECS control algorithm (in seconds). 
+ * Smaller values make it faster to respond, larger values make it slower
+ * to respond.
+ *
+ * @group Fixed Wing TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f);
+
+/**
+ * TECS Throttle time constant
+ *
+ * This is the time constant of the TECS throttle control algorithm (in seconds). 
+ * Smaller values make it faster to respond, larger values make it slower
+ * to respond.
+ *
+ * @group Fixed Wing TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_THRO_CONST, 8.0f);
+
+/**
+ * Throttle damping factor
+ *
+ * This is the damping gain for the throttle demand loop. 
+ * Increase to add damping to correct for oscillations in speed and height.
+ *
+ * @group Fixed Wing TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f);
+
+/**
+ * Integrator gain
+ *
+ * This is the integrator gain on the control loop. 
+ * Increasing this gain increases the speed at which speed 
+ * and height offsets are trimmed out, but reduces damping and 
+ * increases overshoot.
+ *
+ * @group Fixed Wing TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f);
+
+/**
+ * Maximum vertical acceleration
+ *
+ * This is the maximum vertical acceleration (in metres/second square)
+ * either up or down that the controller will use to correct speed 
+ * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) 
+ * allows for reasonably aggressive pitch changes if required to recover 
+ * from under-speed conditions.
+ *
+ * @group Fixed Wing TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
+
+/**
+ * Complementary filter "omega" parameter for height
+ *
+ * This is the cross-over frequency (in radians/second) of the complementary 
+ * filter used to fuse vertical acceleration and barometric height to obtain 
+ * an estimate of height rate and height. Increasing this frequency weights 
+ * the solution more towards use of the barometer, whilst reducing it weights 
+ * the solution more towards use of the accelerometer data.
+ *
+ * @group Fixed Wing TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f);
+
+/**
+ * Complementary filter "omega" parameter for speed
+ *
+ * This is the cross-over frequency (in radians/second) of the complementary 
+ * filter used to fuse longitudinal acceleration and airspeed to obtain an 
+ * improved airspeed estimate. Increasing this frequency weights the solution
+ * more towards use of the arispeed sensor, whilst reducing it weights the 
+ * solution more towards use of the accelerometer data.
+ *
+ * @group Fixed Wing TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
+
+/**
+ * Roll -> Throttle feedforward
+ *
+ * Increasing this gain turn increases the amount of throttle that will 
+ * be used to compensate for the additional drag created by turning. 
+ * Ideally this should be set to  approximately 10 x the extra sink rate 
+ * in m/s created by a 45 degree bank turn. Increase this gain if 
+ * the aircraft initially loses energy in turns and reduce if the 
+ * aircraft initially gains energy in turns. Efficient high aspect-ratio 
+ * aircraft (eg powered sailplanes) can use a lower value, whereas 
+ * inefficient low aspect-ratio models (eg delta wings) can use a higher value.
+ *
+ * @group Fixed Wing TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f);
+
+/**
+ * Speed <--> Altitude priority
+ *
+ * This parameter adjusts the amount of weighting that the pitch control 
+ * applies to speed vs height errors. Setting it to 0.0 will cause the 
+ * pitch control to control height and ignore speed errors. This will 
+ * normally improve height accuracy but give larger airspeed errors. 
+ * Setting it to 2.0 will cause the pitch control loop to control speed 
+ * and ignore height errors. This will normally reduce airspeed errors, 
+ * but give larger height errors. The default value of 1.0 allows the pitch 
+ * control to simultaneously control height and speed. 
+ * Note to Glider Pilots - set this parameter to 2.0 (The glider will 
+ * adjust its pitch angle to maintain airspeed, ignoring changes in height).
+ *
+ * @group Fixed Wing TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f);
+
+/**
+ * Pitch damping factor
+ *
+ * This is the damping gain for the pitch demand loop. Increase to add 
+ * damping to correct for oscillations in height. The default value of 0.0 
+ * will work well provided the pitch to servo controller has been tuned 
+ * properly.
+ *
+ * @group Fixed Wing TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
+
+/**
+ * Height rate P factor
+ *
+ * @group Fixed Wing TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f);
+
+/**
+ * Speed rate P factor
+ *
+ * @group Fixed Wing TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f);
+
+/**
  * Landing slope angle
  *
  * @group L1 Control
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
index 749f57a2b..bffeefc1f 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
@@ -109,8 +109,7 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti
 
 	/* Write part of the status message */
 	_status.altitudeSp = altitudeSp;
-	_status.altitude = altitude;
-	_status.altitudeFiltered = altitudeFiltered;
+	_status.altitude_filtered = altitudeFiltered;
 
 
 	/* use flightpath angle setpoint for total energy control */
@@ -146,8 +145,7 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
 
 	/* Write part of the status message */
 	_status.airspeedSp = airspeedSp;
-	_status.airspeed = airspeed;
-	_status.airspeedFiltered = airspeedFiltered;
+	_status.airspeed_filtered = airspeedFiltered;
 
 
 	/* use longitudinal acceleration setpoint for total energy control */
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 4ca31fe20..58a1e9f6b 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
@@ -55,7 +55,7 @@
  * @max 1
  * @group mTECS
  */
-PARAM_DEFINE_INT32(MT_ENABLED, 1);
+PARAM_DEFINE_INT32(MT_ENABLED, 0);
 
 /**
  * Total Energy Rate Control Feedforward
@@ -241,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
@@ -268,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)
@@ -287,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_commands.cpp b/src/modules/mavlink/mavlink_commands.cpp
deleted file mode 100644
index b502c3c86..000000000
--- a/src/modules/mavlink/mavlink_commands.cpp
+++ /dev/null
@@ -1,73 +0,0 @@
-/****************************************************************************
- *
- *   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_commands.cpp
- * Mavlink commands stream implementation.
- *
- * @author Anton Babushkin <anton.babushkin@me.com>
- */
-
-#include "mavlink_commands.h"
-
-MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) :
-	_cmd_sub(mavlink->add_orb_subscription(ORB_ID(vehicle_command))),
-	_cmd{},
-	_channel(channel),
-	_cmd_time(0)
-{
-}
-
-void
-MavlinkCommandsStream::update(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_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/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
index 6a2c900af..17d1babff 100644
--- a/src/modules/mavlink/mavlink_ftp.cpp
+++ b/src/modules/mavlink/mavlink_ftp.cpp
@@ -35,6 +35,7 @@
 #include <unistd.h>
 #include <stdio.h>
 #include <fcntl.h>
+#include <sys/stat.h>
 
 #include "mavlink_ftp.h"
 
@@ -190,6 +191,8 @@ void
 MavlinkFTP::_reply(Request *req)
 {
 	auto hdr = req->header();
+	
+	hdr->magic = kProtocolMagic;
 
 	// message is assumed to be already constructed in the request buffer, so generate the CRC
 	hdr->crc32 = 0;
@@ -242,15 +245,18 @@ MavlinkFTP::_workList(Request *req)
 			break;
 		}
 
-		// name too big to fit?
-		if ((strlen(entry.d_name) + offset + 2) > kMaxDataLength) {
-			break;
-		}
+		uint32_t fileSize = 0;
+		char buf[256];
 
 		// store the type marker
 		switch (entry.d_type) {
 		case DTYPE_FILE:
 			hdr->data[offset++] = kDirentFile;
+			snprintf(buf, sizeof(buf), "%s/%s", dirPath, entry.d_name);
+			struct stat st;
+			if (stat(buf, &st) == 0) {
+				fileSize = st.st_size;
+			}
 			break;
 		case DTYPE_DIRECTORY:
 			hdr->data[offset++] = kDirentDir;
@@ -259,11 +265,24 @@ MavlinkFTP::_workList(Request *req)
 			hdr->data[offset++] = kDirentUnknown;
 			break;
 		}
+		
+		if (entry.d_type == DTYPE_FILE) {
+			snprintf(buf, sizeof(buf), "%s\t%d", entry.d_name, fileSize);
+		} else {
+			strncpy(buf, entry.d_name, sizeof(buf));
+			buf[sizeof(buf)-1] = 0;
+		}
+		size_t nameLen = strlen(buf);
 
+		// name too big to fit?
+		if ((nameLen + offset + 2) > kMaxDataLength) {
+			break;
+		}
+		
 		// copy the name, which we know will fit
-		strcpy((char *)&hdr->data[offset], entry.d_name);
+		strcpy((char *)&hdr->data[offset], buf);
 		//printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]);
-		offset += strlen(entry.d_name) + 1;
+		offset += nameLen + 1;
 	}
 
 	closedir(dp);
@@ -282,6 +301,16 @@ MavlinkFTP::_workOpen(Request *req, bool create)
 		return kErrNoSession;
 	}
 
+	
+	uint32_t fileSize = 0;
+	if (!create) {
+		struct stat st;
+		if (stat(req->dataAsCString(), &st) != 0) {
+			return kErrNotFile;
+		}
+		fileSize = st.st_size;
+	}
+
 	int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY;
     
 	int fd = ::open(req->dataAsCString(), oflag);
@@ -291,7 +320,12 @@ MavlinkFTP::_workOpen(Request *req, bool create)
 	_session_fds[session_index] = fd;
 
 	hdr->session = session_index;
-	hdr->size = 0;
+	if (create) {
+		hdr->size = 0;
+	} else {
+		hdr->size = sizeof(uint32_t);
+		*((uint32_t*)hdr->data) = fileSize;
+	}
 
 	return kErrNone;
 }
diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
index 59237a4c4..800c98b69 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
 {
@@ -145,7 +146,7 @@ private:
 			mavlink_message_t msg;
 			msg.checksum = 0;
 			unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(),
-				_mavlink->get_channel(), &msg, sequence(), rawData());
+				_mavlink->get_channel(), &msg, sequence()+1, rawData());
 
 			_mavlink->lockMessageBufferMutex();
 			bool success = _mavlink->message_buffer_write(&msg, len);
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 3f64f30ed..e2931ce6d 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -78,7 +78,6 @@
 #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
@@ -91,14 +90,19 @@
 static const int ERROR = -1;
 
 #define DEFAULT_DEVICE_NAME	"/dev/ttyS1"
-#define MAX_DATA_RATE	10000	// max data rate in bytes/s
+#define MAX_DATA_RATE	20000	// 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,113 +112,6 @@ 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();
-				instance->count_txerrbytes(desired);
-				return;
-			}
-		}
-
-		ssize_t ret = write(uart, ch, desired);
-
-		if (ret != desired) {
-			instance->count_txerr();
-			instance->count_txerrbytes(desired);
-
-		} else {
-			last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel];
-			instance->count_txbytes(desired);
-		}
-	}
-}
-
 static void usage(void);
 
 Mavlink::Mavlink() :
@@ -234,8 +131,7 @@ Mavlink::Mavlink() :
 	_subscriptions(nullptr),
 	_streams(nullptr),
 	_mission_manager(nullptr),
-	_mission_pub(-1),
-	_mission_result_sub(-1),
+	_parameters_manager(nullptr),
 	_mode(MAVLINK_MODE_NORMAL),
 	_channel(MAVLINK_COMM_0),
 	_logbuffer {},
@@ -247,12 +143,16 @@ Mavlink::Mavlink() :
 	_ftp_on(false),
 	_uart_fd(-1),
 	_baudrate(57600),
-	_datarate(10000),
+	_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),
@@ -263,6 +163,7 @@ Mavlink::Mavlink() :
 	_rstatus {},
 	_message_buffer {},
 	_message_buffer_mutex {},
+	_send_mutex {},
 	_param_initialized(false),
 	_param_system_id(0),
 	_param_component_id(0),
@@ -485,7 +386,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);
+			}
 		}
 	}
 }
@@ -533,10 +439,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) {
@@ -562,22 +484,39 @@ void Mavlink::mavlink_update_system(void)
 		_param_system_type = param_find("MAV_TYPE");
 		_param_use_hil_gps = param_find("MAV_USEHILGPS");
 		_param_forward_externalsp = param_find("MAV_FWDEXTSP");
-		_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;
@@ -719,6 +658,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;
@@ -760,8 +702,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", 200.0f);
 	}
 
 	/* disable HIL */
@@ -776,248 +717,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];
-
-		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;
+	struct mavlink_logmessage logmsg;
+	strncpy(logmsg.text, string, sizeof(logmsg.text));
+	logmsg.severity = severity;
 
-		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)
@@ -1040,11 +921,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;
@@ -1075,10 +962,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;
@@ -1092,6 +977,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,
@@ -1251,7 +1161,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
@@ -1389,6 +1319,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);
 
@@ -1398,7 +1331,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");
 		}
 
@@ -1418,12 +1351,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));
@@ -1434,34 +1361,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", 8.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;
 
@@ -1469,13 +1412,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);
@@ -1488,6 +1426,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();
@@ -1498,9 +1438,6 @@ 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)) {
@@ -1526,20 +1463,6 @@ 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) {
 
@@ -1584,7 +1507,7 @@ Mavlink::task_main(int argc, char *argv[])
 
 				pthread_mutex_unlock(&_message_buffer_mutex);
 
-				_mavlink_resend_uart(_channel, &msg);
+				resend_message(&msg);
 			}
 		}
 
@@ -1599,14 +1522,13 @@ Mavlink::task_main(int argc, char *argv[])
 				_bytes_txerr = 0;
 				_bytes_rx = 0;
 			}
+
 			_bytes_timestamp = t;
 		}
 
 		perf_end(_loop_perf);
 	}
 
-	delete _mission_manager;
-
 	delete _subscribe_to_stream;
 	_subscribe_to_stream = nullptr;
 
@@ -1764,10 +1686,12 @@ Mavlink::display_status()
 	} 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
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 260a7a224..d4d15d81f 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -58,7 +58,7 @@
 #include "mavlink_stream.h"
 #include "mavlink_messages.h"
 #include "mavlink_mission.h"
-
+#include "mavlink_parameters.h"
 
 class Mavlink
 {
@@ -162,7 +162,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);
 
@@ -190,29 +195,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();
@@ -254,6 +263,8 @@ public:
 	 */
 	struct telemetry_status_s&	get_rx_status() { return _rstatus; }
 
+	struct mavlink_logbuffer	*get_logbuffer() { return &_logbuffer; }
+
 protected:
 	Mavlink			*next;
 
@@ -277,9 +288,8 @@ private:
 	MavlinkStream		*_streams;
 
 	MavlinkMissionManager	*_mission_manager;
+	MavlinkParametersManager *_parameters_manager;
 
-	orb_advert_t		_mission_pub;
-	int			_mission_result_sub;
 	MAVLINK_MODE 		_mode;
 
 	mavlink_channel_t	_channel;
@@ -295,7 +305,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
@@ -310,6 +322,8 @@ 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;
@@ -331,6 +345,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;
@@ -342,52 +357,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);
 
@@ -403,6 +393,11 @@ 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);
 
 	/**
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 6885bebde..c10be77b5 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,61 +249,184 @@ 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:
-	explicit MavlinkStreamHeartbeat() : MavlinkStream(),
-		status_sub(nullptr),
-		pos_sp_triplet_sub(nullptr)
+	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 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));
-	}
-
 	void send(const hrt_abstime t)
 	{
 		struct vehicle_status_s status;
 		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;
 
-		mavlink_msg_heartbeat_send(_channel,
-					   mavlink_system.type,
-					   MAV_AUTOPILOT_PX4,
-					   mavlink_base_mode,
-					   mavlink_custom_mode,
-					   mavlink_state);
+		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;
+
+				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
 {
@@ -313,7 +436,7 @@ public:
 		return MavlinkStreamSysStatus::get_name_static();
 	}
 
-	static const char *get_name_static ()
+	static const char *get_name_static()
 	{
 		return "SYS_STATUS";
 	}
@@ -323,47 +446,50 @@ public:
 		return MAVLINK_MSG_ID_SYS_STATUS;
 	}
 
-	static MavlinkStream *new_instance()
+	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
-		return new MavlinkStreamSysStatus();
+		return new MavlinkStreamSysStatus(mavlink);
+	}
+
+	unsigned get_size()
+	{
+		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:
-	explicit MavlinkStreamSysStatus() : MavlinkStream(),
-		status_sub(nullptr)
+	explicit MavlinkStreamSysStatus(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status)))
 	{}
 
-	void subscribe(Mavlink *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);
 		}
 	}
 };
@@ -387,78 +513,89 @@ public:
 		return MAVLINK_MSG_ID_HIGHRES_IMU;
 	}
 
-	static MavlinkStream *new_instance()
+	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
-		return new MavlinkStreamHighresIMU();
+		return new MavlinkStreamHighresIMU(mavlink);
+	}
+
+	unsigned get_size()
+	{
+		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_sub(nullptr),
-		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);
 		}
 	}
 };
@@ -482,14 +619,19 @@ 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 &);
@@ -497,25 +639,27 @@ private:
 
 
 protected:
-	explicit MavlinkStreamAttitude() : MavlinkStream(),
-		att_sub(nullptr),
-		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);
 		}
 	}
 };
@@ -539,44 +683,47 @@ public:
 		return MAVLINK_MSG_ID_ATTITUDE_QUATERNION;
 	}
 
-	static MavlinkStream *new_instance()
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamAttitudeQuaternion(mavlink);
+	}
+
+	unsigned get_size()
 	{
-		return new MavlinkStreamAttitudeQuaternion();
+		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_sub(nullptr),
-		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);
 		}
 	}
 };
@@ -601,54 +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_sub(nullptr),
-		att_time(0),
-		pos_sub(nullptr),
-		pos_time(0),
-		armed_sub(nullptr),
-		armed_time(0),
-		act_sub(nullptr),
-		act_time(0),
-		airspeed_sub(nullptr),
-		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;
@@ -657,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);
 		}
 	}
 };
@@ -698,46 +840,49 @@ public:
 		return MAVLINK_MSG_ID_GPS_RAW_INT;
 	}
 
-	static MavlinkStream *new_instance()
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamGPSRawInt(mavlink);
+	}
+
+	unsigned get_size()
 	{
-		return new MavlinkStreamGPSRawInt();
+		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_sub(nullptr),
-		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);
 		}
 	}
 };
@@ -761,55 +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_sub(nullptr),
-		pos_time(0),
-		home_sub(nullptr),
-		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);
 		}
 	}
 };
@@ -833,49 +980,51 @@ public:
 		return MAVLINK_MSG_ID_LOCAL_POSITION_NED;
 	}
 
-	static MavlinkStream *new_instance()
+	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
-		return new MavlinkStreamLocalPositionNED();
+		return new MavlinkStreamLocalPositionNED(mavlink);
+	}
+
+	unsigned get_size()
+	{
+		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_sub(nullptr),
-		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:
@@ -894,43 +1043,46 @@ public:
 		return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE;
 	}
 
-	static MavlinkStream *new_instance()
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamViconPositionEstimate(mavlink);
+	}
+
+	unsigned get_size()
 	{
-		return new MavlinkStreamViconPositionEstimate();
+		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_sub(nullptr),
-		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);
 		}
 	}
 };
@@ -954,45 +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:
-	explicit MavlinkStreamGPSGlobalOrigin() : MavlinkStream(),
-		home_sub(nullptr)
+	explicit MavlinkStreamGPSGlobalOrigin(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position)))
 	{}
 
-	void subscribe(Mavlink *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
 {
@@ -1024,26 +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;
 
 	/* do not allow top copying this class */
 	MavlinkStreamServoOutputRaw(MavlinkStreamServoOutputRaw &);
 	MavlinkStreamServoOutputRaw& operator = (const MavlinkStreamServoOutputRaw &);
 
 protected:
-	explicit MavlinkStreamServoOutputRaw() : MavlinkStream(),
-		act_sub(nullptr),
-		act_time(0)
-	{}
-
-	void subscribe(Mavlink *mavlink)
+	explicit MavlinkStreamServoOutputRaw(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_act_sub(nullptr),
+		_act_time(0)
 	{
 		orb_id_t act_topics[] = {
 			ORB_ID(actuator_outputs_0),
@@ -1052,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);
 		}
 	}
 };
@@ -1094,51 +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_sub(nullptr),
-		status_time(0),
-		pos_sp_triplet_sub(nullptr),
-		pos_sp_triplet_time(0),
-		act_sub(nullptr),
-		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 */
@@ -1206,67 +1365,79 @@ 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);
 		}
 	}
 };
 
 
-class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream
+class MavlinkStreamPositionTargetGlobalInt : public MavlinkStream
 {
 public:
 	const char *get_name() const
 	{
-		return MavlinkStreamGlobalPositionSetpointInt::get_name_static();
+		return MavlinkStreamPositionTargetGlobalInt::get_name_static();
 	}
 
 	static const char *get_name_static()
 	{
-		return "GLOBAL_POSITION_SETPOINT_INT";
+		return "POSITION_TARGET_GLOBAL_INT";
 	}
 
 	uint8_t get_id()
 	{
-		return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT;
+		return MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT;
+	}
+
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamPositionTargetGlobalInt(mavlink);
 	}
 
-	static MavlinkStream *new_instance()
+	unsigned get_size()
 	{
-		return new MavlinkStreamGlobalPositionSetpointInt();
+		return MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_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 &);
+	MavlinkStreamPositionTargetGlobalInt(MavlinkStreamPositionTargetGlobalInt &);
+	MavlinkStreamPositionTargetGlobalInt& operator = (const MavlinkStreamPositionTargetGlobalInt &);
 
 protected:
-	explicit MavlinkStreamGlobalPositionSetpointInt() : MavlinkStream(),
-		pos_sp_triplet_sub(nullptr)
+	explicit MavlinkStreamPositionTargetGlobalInt(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)))
 	{}
 
-	void subscribe(Mavlink *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_position_target_global_int_t msg{};
+
+			msg.coordinate_frame = MAV_FRAME_GLOBAL;
+			msg.lat_int = pos_sp_triplet.current.lat * 1e7;
+			msg.lon_int = pos_sp_triplet.current.lon * 1e7;
+			msg.alt = pos_sp_triplet.current.alt;
+
+			_mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT, &msg);
 		}
 	}
 };
@@ -1282,165 +1453,124 @@ public:
 
 	static const char *get_name_static()
 	{
-		return "LOCAL_POSITION_SETPOINT";
+		return "POSITION_TARGET_LOCAL_NED";
 	}
 
 	uint8_t get_id()
 	{
-		return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT;
+		return MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED;
+	}
+
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamLocalPositionSetpoint(mavlink);
 	}
 
-	static MavlinkStream *new_instance()
+	unsigned get_size()
 	{
-		return new MavlinkStreamLocalPositionSetpoint();
+		return MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_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_sub(nullptr),
-		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_position_target_local_ned_t msg{};
+
+			msg.time_boot_ms = pos_sp.timestamp / 1000;
+			msg.coordinate_frame = MAV_FRAME_LOCAL_NED;
+			msg.x = pos_sp.x;
+			msg.y = pos_sp.y;
+			msg.z = pos_sp.z;
+
+			_mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, &msg);
 		}
 	}
 };
 
 
-class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream
+class MavlinkStreamAttitudeTarget : public MavlinkStream
 {
 public:
 	const char *get_name() const
 	{
-		return MavlinkStreamRollPitchYawThrustSetpoint::get_name_static();
+		return MavlinkStreamAttitudeTarget::get_name_static();
 	}
 
 	static const char *get_name_static()
 	{
-		return "ROLL_PITCH_YAW_THRUST_SETPOINT";
+		return "ATTITUDE_TARGET";
 	}
 
 	uint8_t get_id()
 	{
-		return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT;
+		return MAVLINK_MSG_ID_ATTITUDE_TARGET;
 	}
 
-	static MavlinkStream *new_instance()
+	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
-		return new MavlinkStreamRollPitchYawThrustSetpoint();
+		return new MavlinkStreamAttitudeTarget(mavlink);
+	}
+
+	unsigned get_size()
+	{
+		return MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 	}
 
 private:
-	MavlinkOrbSubscription *att_sp_sub;
-	uint64_t att_sp_time;
+	MavlinkOrbSubscription *_att_sp_sub;
+	MavlinkOrbSubscription *_att_rates_sp_sub;
+	uint64_t _att_sp_time;
+	uint64_t _att_rates_sp_time;
 
 	/* do not allow top copying this class */
-	MavlinkStreamRollPitchYawThrustSetpoint(MavlinkStreamRollPitchYawThrustSetpoint &);
-	MavlinkStreamRollPitchYawThrustSetpoint& operator = (const MavlinkStreamRollPitchYawThrustSetpoint &);
+	MavlinkStreamAttitudeTarget(MavlinkStreamAttitudeTarget &);
+	MavlinkStreamAttitudeTarget& operator = (const MavlinkStreamAttitudeTarget &);
 
 protected:
-	explicit MavlinkStreamRollPitchYawThrustSetpoint() : MavlinkStream(),
-		att_sp_sub(nullptr),
-		att_sp_time(0)
+	explicit MavlinkStreamAttitudeTarget(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_att_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint))),
+		_att_rates_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint))),
+		_att_sp_time(0),
+		_att_rates_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);
-		}
-	}
-};
+		if (_att_sp_sub->update(&_att_sp_time, &att_sp)) {
 
+			struct vehicle_rates_setpoint_s att_rates_sp;
+			(void)_att_rates_sp_sub->update(&_att_rates_sp_time, &att_rates_sp);
 
-class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream
-{
-public:
-	const char *get_name() const
-	{
-		return MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static();
-	}
+			mavlink_attitude_target_t msg{};
 
-	static const char *get_name_static()
-	{
-		return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT";
-	}
+			msg.time_boot_ms = att_sp.timestamp / 1000;
+			mavlink_euler_to_quaternion(att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, msg.q);
 
-	uint8_t get_id()
-	{
-		return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT;
-	}
+			msg.body_roll_rate = att_rates_sp.roll;
+			msg.body_pitch_rate = att_rates_sp.pitch;
+			msg.body_yaw_rate = att_rates_sp.yaw;
 
-	static MavlinkStream *new_instance()
-	{
-		return new MavlinkStreamRollPitchYawRatesThrustSetpoint();
-	}
+			msg.thrust = att_sp.thrust;
 
-private:
-	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_sub(nullptr),
-		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);
+			_mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE_TARGET, &msg);
 		}
 	}
 };
@@ -1464,77 +1594,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_sub(nullptr),
-		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);
 		}
 	}
 };
@@ -1558,42 +1695,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_sub(nullptr),
-		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);
 		}
 	}
 };
@@ -1617,42 +1757,47 @@ public:
 		return MAVLINK_MSG_ID_OPTICAL_FLOW;
 	}
 
-	static MavlinkStream *new_instance()
+	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
-		return new MavlinkStreamOpticalFlow();
+		return new MavlinkStreamOpticalFlow(mavlink);
+	}
+
+	unsigned get_size()
+	{
+		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_sub(nullptr),
-		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);
 		}
 	}
 };
@@ -1675,56 +1820,64 @@ public:
 		return 0;
 	}
 
-	static MavlinkStream *new_instance()
+	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
-		return new MavlinkStreamAttitudeControls();
+		return new MavlinkStreamAttitudeControls(mavlink);
+	}
+
+	unsigned get_size()
+	{
+		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_sub(nullptr),
-		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:
@@ -1743,46 +1896,49 @@ public:
 		return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT;
 	}
 
-	static MavlinkStream *new_instance()
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamNamedValueFloat(mavlink);
+	}
+
+	unsigned get_size()
 	{
-		return new MavlinkStreamNamedValueFloat();
+		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_sub(nullptr),
-		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:
@@ -1801,46 +1957,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:
-	explicit MavlinkStreamCameraCapture() : MavlinkStream(),
-		status_sub(nullptr)
+	explicit MavlinkStreamCameraCapture(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status)))
 	{}
 
-	void subscribe(Mavlink *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:
@@ -1859,50 +2022,58 @@ public:
 		return MAVLINK_MSG_ID_DISTANCE_SENSOR;
 	}
 
-	static MavlinkStream *new_instance()
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamDistanceSensor(mavlink);
+	}
+
+	unsigned get_size()
 	{
-		return new MavlinkStreamDistanceSensor();
+		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_sub(nullptr),
-		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);
 		}
 	}
 };
@@ -1910,6 +2081,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),
@@ -1918,16 +2091,16 @@ 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),
 	new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static),
 	new StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static),
 	new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static),
-	new StreamListItem(&MavlinkStreamGlobalPositionSetpointInt::new_instance, &MavlinkStreamGlobalPositionSetpointInt::get_name_static),
+	new StreamListItem(&MavlinkStreamPositionTargetGlobalInt::new_instance, &MavlinkStreamPositionTargetGlobalInt::get_name_static),
 	new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static),
-	new StreamListItem(&MavlinkStreamRollPitchYawThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawThrustSetpoint::get_name_static),
-	new StreamListItem(&MavlinkStreamRollPitchYawRatesThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static),
+	new StreamListItem(&MavlinkStreamAttitudeTarget::new_instance, &MavlinkStreamAttitudeTarget::get_name_static),
 	new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static),
 	new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static),
 	new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static),
@@ -1935,6 +2108,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 b792b9aaf..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,43 +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
-
-	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;
+protected:
+	explicit MavlinkMissionManager(Mavlink *mavlink);
 
-	/* do not allow copying this class */
-	MavlinkMissionManager(const MavlinkMissionManager&);
-	MavlinkMissionManager operator=(const MavlinkMissionManager&);
+	void send(const hrt_abstime t);
 };
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_commands.h b/src/modules/mavlink/mavlink_parameters.h
index abdba34b9..5576e6b84 100644
--- a/src/modules/mavlink/mavlink_commands.h
+++ b/src/modules/mavlink/mavlink_parameters.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -32,34 +32,84 @@
  ****************************************************************************/
 
 /**
- * @file mavlink_commands.h
- * Mavlink commands stream definition.
+ * @file mavlink_parameters.h
+ * Mavlink parameters manager definition.
  *
  * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
-#ifndef MAVLINK_COMMANDS_H_
-#define MAVLINK_COMMANDS_H_
+#pragma once
 
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_command.h>
+#include <systemlib/param/param.h>
 
-class Mavlink;
-class MavlinkCommansStream;
+#include "mavlink_bridge_header.h"
+#include "mavlink_stream.h"
 
-#include "mavlink_main.h"
-
-class MavlinkCommandsStream
+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:
-	MavlinkOrbSubscription *_cmd_sub;
-	struct vehicle_command_s *_cmd;
-	mavlink_channel_t _channel;
-	uint64_t _cmd_time;
+	int		_send_all_index;
 
-public:
-	MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel);
-	void update(const hrt_abstime t);
-};
+	/* do not allow top copying this class */
+	MavlinkParametersManager(MavlinkParametersManager &);
+	MavlinkParametersManager& operator = (const MavlinkParametersManager &);
+
+protected:
+	explicit MavlinkParametersManager(Mavlink *mavlink);
 
-#endif /* MAVLINK_COMMANDS_H_ */
+	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 b7cddf523..dfc5ddc91 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -150,18 +150,13 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_vicon_position_estimate(msg);
 		break;
 
-	case MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST:
-		handle_message_quad_swarm_roll_pitch_yaw_thrust(msg);
+	case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
+		handle_message_set_position_target_local_ned(msg);
 		break;
 
-	case MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL:
-		handle_message_local_ned_position_setpoint_external(msg);
+	case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
+		handle_message_set_attitude_target(msg);
 		break;
-
-	case MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL:
-		handle_message_attitude_setpoint_external(msg);
-		break;
-
 	case MAVLINK_MSG_ID_RADIO_STATUS:
 		handle_message_radio_status(msg);
 		break;
@@ -251,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;
 			}
 
@@ -372,53 +368,22 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg)
 }
 
 void
-MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg)
+MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t *msg)
 {
-	mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t swarm_offboard_control;
-	mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &swarm_offboard_control);
-
-	/* Only accept system IDs from 1 to 4 */
-	if (mavlink_system.sysid >= 1 && mavlink_system.sysid <= 4) {
-		struct offboard_control_setpoint_s offboard_control_sp;
-		memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));
-
-		/* Convert values * 1000 back */
-		//XXX: convert to quaternion
-		//offboard_control_sp.p1 = (float)swarm_offboard_control.roll[mavlink_system.sysid - 1] / 1000.0f;
-		//offboard_control_sp.p2 = (float)swarm_offboard_control.pitch[mavlink_system.sysid - 1] / 1000.0f;
-		//offboard_control_sp.p3 = (float)swarm_offboard_control.yaw[mavlink_system.sysid - 1] / 1000.0f;
-		//offboard_control_sp.p4 = (float)swarm_offboard_control.thrust[mavlink_system.sysid - 1] / 1000.0f;
-
-		offboard_control_sp.mode = (enum OFFBOARD_CONTROL_MODE)swarm_offboard_control.mode;
-
-		offboard_control_sp.timestamp = hrt_absolute_time();
-
-		if (_offboard_control_sp_pub < 0) {
-			_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
-
-		} else {
-			orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp);
-		}
-	}
-}
-
-void
-MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_message_t *msg)
-{
-	mavlink_local_ned_position_setpoint_external_t local_ned_position_setpoint_external;
-	mavlink_msg_local_ned_position_setpoint_external_decode(msg, &local_ned_position_setpoint_external);
+	mavlink_set_position_target_local_ned_t set_position_target_local_ned;
+	mavlink_msg_set_position_target_local_ned_decode(msg, &set_position_target_local_ned);
 
 	struct offboard_control_setpoint_s offboard_control_sp;
 	memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));//XXX breaks compatibility with multiple setpoints
 
 	/* Only accept messages which are intended for this system */
-	if ((mavlink_system.sysid == local_ned_position_setpoint_external.target_system ||
-				local_ned_position_setpoint_external.target_system == 0) &&
-			(mavlink_system.compid == local_ned_position_setpoint_external.target_component ||
-			 local_ned_position_setpoint_external.target_component == 0)) {
+	if ((mavlink_system.sysid == set_position_target_local_ned.target_system ||
+				set_position_target_local_ned.target_system == 0) &&
+			(mavlink_system.compid == set_position_target_local_ned.target_component ||
+			 set_position_target_local_ned.target_component == 0)) {
 
 		/* convert mavlink type (local, NED) to uORB offboard control struct */
-		switch (local_ned_position_setpoint_external.coordinate_frame) {
+		switch (set_position_target_local_ned.coordinate_frame) {
 			case MAV_FRAME_LOCAL_NED:
 				offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED;
 				break;
@@ -435,16 +400,16 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes
 				/* invalid setpoint, avoid publishing */
 				return;
 		}
-		offboard_control_sp.position[0] = local_ned_position_setpoint_external.x;
-		offboard_control_sp.position[1] = local_ned_position_setpoint_external.y;
-		offboard_control_sp.position[2] = local_ned_position_setpoint_external.z;
-		offboard_control_sp.velocity[0] = local_ned_position_setpoint_external.vx;
-		offboard_control_sp.velocity[1] = local_ned_position_setpoint_external.vy;
-		offboard_control_sp.velocity[2] = local_ned_position_setpoint_external.vz;
-		offboard_control_sp.acceleration[0] = local_ned_position_setpoint_external.afx;
-		offboard_control_sp.acceleration[1] = local_ned_position_setpoint_external.afy;
-		offboard_control_sp.acceleration[2] = local_ned_position_setpoint_external.afz;
-		offboard_control_sp.isForceSetpoint = (bool)(local_ned_position_setpoint_external.type_mask & (1 << 9));
+		offboard_control_sp.position[0] = set_position_target_local_ned.x;
+		offboard_control_sp.position[1] = set_position_target_local_ned.y;
+		offboard_control_sp.position[2] = set_position_target_local_ned.z;
+		offboard_control_sp.velocity[0] = set_position_target_local_ned.vx;
+		offboard_control_sp.velocity[1] = set_position_target_local_ned.vy;
+		offboard_control_sp.velocity[2] = set_position_target_local_ned.vz;
+		offboard_control_sp.acceleration[0] = set_position_target_local_ned.afx;
+		offboard_control_sp.acceleration[1] = set_position_target_local_ned.afy;
+		offboard_control_sp.acceleration[2] = set_position_target_local_ned.afz;
+		offboard_control_sp.isForceSetpoint = (bool)(set_position_target_local_ned.type_mask & (1 << 9));
 
 		/* If we are in force control mode, for now set offboard mode to force control */
 		if (offboard_control_sp.isForceSetpoint) {
@@ -454,7 +419,7 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes
 		/* set ignore flags */
 		for (int i = 0; i < 9; i++) {
 			offboard_control_sp.ignore &=  ~(1 << i);
-			offboard_control_sp.ignore |=  (local_ned_position_setpoint_external.type_mask & (1 << i));
+			offboard_control_sp.ignore |=  (set_position_target_local_ned.type_mask & (1 << i));
 		}
 
 
@@ -540,32 +505,32 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes
 }
 
 void
-MavlinkReceiver::handle_message_attitude_setpoint_external(mavlink_message_t *msg)
+MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 {
-	mavlink_attitude_setpoint_external_t attitude_setpoint_external;
-	mavlink_msg_attitude_setpoint_external_decode(msg, &attitude_setpoint_external);
+	mavlink_set_attitude_target_t set_attitude_target;
+	mavlink_msg_set_attitude_target_decode(msg, &set_attitude_target);
 
 	struct offboard_control_setpoint_s offboard_control_sp;
 	memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); //XXX breaks compatibility with multiple setpoints
 
 	/* Only accept messages which are intended for this system */
-	if ((mavlink_system.sysid == attitude_setpoint_external.target_system ||
-				attitude_setpoint_external.target_system == 0) &&
-			(mavlink_system.compid == attitude_setpoint_external.target_component ||
-			 attitude_setpoint_external.target_component == 0)) {
+	if ((mavlink_system.sysid == set_attitude_target.target_system ||
+				set_attitude_target.target_system == 0) &&
+			(mavlink_system.compid == set_attitude_target.target_component ||
+			 set_attitude_target.target_component == 0)) {
 		for (int i = 0; i < 4; i++) {
-			offboard_control_sp.attitude[i] = attitude_setpoint_external.q[i];
+			offboard_control_sp.attitude[i] = set_attitude_target.q[i];
 		}
-		offboard_control_sp.attitude_rate[0] = attitude_setpoint_external.body_roll_rate;
-		offboard_control_sp.attitude_rate[1] = attitude_setpoint_external.body_pitch_rate;
-		offboard_control_sp.attitude_rate[2] = attitude_setpoint_external.body_yaw_rate;
+		offboard_control_sp.attitude_rate[0] = set_attitude_target.body_roll_rate;
+		offboard_control_sp.attitude_rate[1] = set_attitude_target.body_pitch_rate;
+		offboard_control_sp.attitude_rate[2] = set_attitude_target.body_yaw_rate;
 
 		for (int i = 0; i < 3; i++) {
 			offboard_control_sp.ignore &=  ~(1 << (i + 9));
-			offboard_control_sp.ignore |=  (attitude_setpoint_external.type_mask & (1 << i)) << 9;
+			offboard_control_sp.ignore |=  (set_attitude_target.type_mask & (1 << i)) << 9;
 		}
 		offboard_control_sp.ignore &= ~(1 << 10);
-		offboard_control_sp.ignore |= ((attitude_setpoint_external.type_mask & (1 << 7)) << 3);
+		offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 7)) << 3);
 
 
 		offboard_control_sp.timestamp = hrt_absolute_time();
@@ -590,14 +555,14 @@ MavlinkReceiver::handle_message_attitude_setpoint_external(mavlink_message_t *ms
 			if (_control_mode.flag_control_offboard_enabled) {
 
 				/* Publish attitude setpoint if ignore bit is not set */
-				if (!(attitude_setpoint_external.type_mask & (1 << 7))) {
+				if (!(set_attitude_target.type_mask & (1 << 7))) {
 					struct vehicle_attitude_setpoint_s att_sp;
 					att_sp.timestamp = hrt_absolute_time();
-					mavlink_quaternion_to_euler(attitude_setpoint_external.q,
+					mavlink_quaternion_to_euler(set_attitude_target.q,
 							&att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body);
-					att_sp.thrust = attitude_setpoint_external.thrust;
+					att_sp.thrust = set_attitude_target.thrust;
 					att_sp.q_d_valid = true;
-					memcpy(att_sp.q_d, attitude_setpoint_external.q, sizeof(att_sp.q_d));
+					memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d));
 					if (_att_sp_pub < 0) {
 						_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
 					} else {
@@ -607,13 +572,13 @@ MavlinkReceiver::handle_message_attitude_setpoint_external(mavlink_message_t *ms
 
 				/* Publish attitude rate setpoint if ignore bit are not set */
 				///XXX add support for ignoring individual axes
-				if (!(attitude_setpoint_external.type_mask & (0b111))) {
+				if (!(set_attitude_target.type_mask & (0b111))) {
 					struct vehicle_rates_setpoint_s rates_sp;
 					rates_sp.timestamp = hrt_absolute_time();
-					rates_sp.roll = attitude_setpoint_external.body_roll_rate;
-					rates_sp.pitch = attitude_setpoint_external.body_pitch_rate;
-					rates_sp.yaw = attitude_setpoint_external.body_yaw_rate;
-					rates_sp.thrust = attitude_setpoint_external.thrust;
+					rates_sp.roll = set_attitude_target.body_roll_rate;
+					rates_sp.pitch = set_attitude_target.body_pitch_rate;
+					rates_sp.yaw = set_attitude_target.body_yaw_rate;
+					rates_sp.thrust = set_attitude_target.thrust;
 
 					if (_att_sp_pub < 0) {
 						_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
@@ -626,7 +591,6 @@ MavlinkReceiver::handle_message_attitude_setpoint_external(mavlink_message_t *ms
 		}
 	}
 }
-
 void
 MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 {
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index a2ed4264f..e3b635dd2 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -114,8 +114,8 @@ private:
 	void handle_message_set_mode(mavlink_message_t *msg);
 	void handle_message_vicon_position_estimate(mavlink_message_t *msg);
 	void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg);
-	void handle_message_local_ned_position_setpoint_external(mavlink_message_t *msg);
-	void handle_message_attitude_setpoint_external(mavlink_message_t *msg);
+	void handle_message_set_position_target_local_ned(mavlink_message_t *msg);
+	void handle_message_set_attitude_target(mavlink_message_t *msg);
 	void handle_message_radio_status(mavlink_message_t *msg);
 	void handle_message_manual_control(mavlink_message_t *msg);
 	void handle_message_heartbeat(mavlink_message_t *msg);
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 20e1c7c44..ef22dc443 100644
--- a/src/modules/mavlink/mavlink_stream.h
+++ b/src/modules/mavlink/mavlink_stream.h
@@ -46,31 +46,50 @@
 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;
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index 1986ae3c8..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
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 9b0f69226..ecc40428d 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -105,6 +105,7 @@ public:
 	int		start();
 
 private:
+	const float alt_ctl_dz = 0.2f;
 
 	bool		_task_should_exit;		/**< if true, task should exit */
 	int		_control_task;			/**< task handle for task */
@@ -184,6 +185,8 @@ private:
 	math::Vector<3> _vel;
 	math::Vector<3> _vel_sp;
 	math::Vector<3> _vel_prev;			/**< velocity on previous step */
+	math::Vector<3> _vel_ff;
+	math::Vector<3> _sp_move_rate;
 
 	/**
 	 * Update our local parameter cache.
@@ -217,6 +220,16 @@ private:
 	void		reset_alt_sp();
 
 	/**
+	 * Set position setpoint using manual control
+	 */
+	void		control_manual(float dt);
+
+	/**
+	 * Set position setpoint using offboard control
+	 */
+	void		control_offboard(float dt);
+
+	/**
 	 * Select between barometric and global (AMSL) altitudes
 	 */
 	void		select_alt(bool global);
@@ -297,6 +310,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_vel.zero();
 	_vel_sp.zero();
 	_vel_prev.zero();
+	_vel_ff.zero();
+	_sp_move_rate.zero();
 
 	_params_handles.thr_min		= param_find("MPC_THR_MIN");
 	_params_handles.thr_max		= param_find("MPC_THR_MAX");
@@ -392,9 +407,11 @@ MulticopterPositionControl::parameters_update(bool force)
 		param_get(_params_handles.z_vel_max, &v);
 		_params.vel_max(2) = v;
 		param_get(_params_handles.xy_ff, &v);
+		v = math::constrain(v, 0.0f, 1.0f);
 		_params.vel_ff(0) = v;
 		_params.vel_ff(1) = v;
 		param_get(_params_handles.z_ff, &v);
+		v = math::constrain(v, 0.0f, 1.0f);
 		_params.vel_ff(2) = v;
 
 		_params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f;
@@ -497,8 +514,11 @@ MulticopterPositionControl::reset_pos_sp()
 {
 	if (_reset_pos_sp) {
 		_reset_pos_sp = false;
-		_pos_sp(0) = _pos(0);
-		_pos_sp(1) = _pos(1);
+		/* shift position setpoint to make attitude setpoint continuous */
+		_pos_sp(0) = _pos(0) + (_vel(0) - _att_sp.R_body[0][2] * _att_sp.thrust / _params.vel_p(0)
+				- _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0);
+		_pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1)
+				- _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1);
 		mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1));
 	}
 }
@@ -508,12 +528,126 @@ MulticopterPositionControl::reset_alt_sp()
 {
 	if (_reset_alt_sp) {
 		_reset_alt_sp = false;
-		_pos_sp(2) = _pos(2);
+		_pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2);
 		mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2));
 	}
 }
 
 void
+MulticopterPositionControl::control_manual(float dt)
+{
+	_sp_move_rate.zero();
+
+	if (_control_mode.flag_control_altitude_enabled) {
+		/* move altitude setpoint with throttle stick */
+		_sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz);
+	}
+
+	if (_control_mode.flag_control_position_enabled) {
+		/* move position setpoint with roll/pitch stick */
+		_sp_move_rate(0) = _manual.x;
+		_sp_move_rate(1) = _manual.y;
+	}
+
+	/* limit setpoint move rate */
+	float sp_move_norm = _sp_move_rate.length();
+
+	if (sp_move_norm > 1.0f) {
+		_sp_move_rate /= sp_move_norm;
+	}
+
+	/* _sp_move_rate scaled to 0..1, scale it to max speed and rotate around yaw */
+	math::Matrix<3, 3> R_yaw_sp;
+	R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
+	_sp_move_rate = R_yaw_sp * _sp_move_rate.emult(_params.vel_max);
+
+	if (_control_mode.flag_control_altitude_enabled) {
+		/* reset alt setpoint to current altitude if needed */
+		reset_alt_sp();
+	}
+
+	if (_control_mode.flag_control_position_enabled) {
+		/* reset position setpoint to current position if needed */
+		reset_pos_sp();
+	}
+
+	/* feed forward setpoint move rate with weight vel_ff */
+	_vel_ff = _sp_move_rate.emult(_params.vel_ff);
+
+	/* move position setpoint */
+	_pos_sp += _sp_move_rate * dt;
+
+	/* check if position setpoint is too far from actual position */
+	math::Vector<3> pos_sp_offs;
+	pos_sp_offs.zero();
+
+	if (_control_mode.flag_control_position_enabled) {
+		pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
+		pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
+	}
+
+	if (_control_mode.flag_control_altitude_enabled) {
+		pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
+	}
+
+	float pos_sp_offs_norm = pos_sp_offs.length();
+
+	if (pos_sp_offs_norm > 1.0f) {
+		pos_sp_offs /= pos_sp_offs_norm;
+		_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
+	}
+}
+
+void
+MulticopterPositionControl::control_offboard(float dt)
+{
+	bool updated;
+	orb_check(_pos_sp_triplet_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
+	}
+
+	if (_pos_sp_triplet.current.valid) {
+		if (_control_mode.flag_control_position_enabled && _pos_sp_triplet.current.position_valid) {
+			/* control position */
+			_pos_sp(0) = _pos_sp_triplet.current.x;
+			_pos_sp(1) = _pos_sp_triplet.current.y;
+			_pos_sp(2) = _pos_sp_triplet.current.z;
+			_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
+
+		} else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) {
+			/* control velocity */
+			/* reset position setpoint to current position if needed */
+			reset_pos_sp();
+
+			/* set position setpoint move rate */
+			_sp_move_rate(0) = _pos_sp_triplet.current.vx;
+			_sp_move_rate(1) = _pos_sp_triplet.current.vy;
+			_att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt;
+		}
+
+		if (_control_mode.flag_control_altitude_enabled) {
+			/* reset alt setpoint to current altitude if needed */
+			reset_alt_sp();
+
+			/* set altitude setpoint move rate */
+			_sp_move_rate(2) = _pos_sp_triplet.current.vz;
+		}
+
+		/* feed forward setpoint move rate with weight vel_ff */
+		_vel_ff = _sp_move_rate.emult(_params.vel_ff);
+
+		/* move position setpoint */
+		_pos_sp += _sp_move_rate * dt;
+
+	} else {
+		reset_pos_sp();
+		reset_alt_sp();
+	}
+}
+
+void
 MulticopterPositionControl::task_main()
 {
 	warnx("started");
@@ -551,13 +685,6 @@ MulticopterPositionControl::task_main()
 
 	hrt_abstime t_prev = 0;
 
-	const float alt_ctl_dz = 0.2f;
-
-	math::Vector<3> sp_move_rate;
-	sp_move_rate.zero();
-
-	float yaw_sp_move_rate;
-
 	math::Vector<3> thrust_int;
 	thrust_int.zero();
 	math::Matrix<3, 3> R;
@@ -616,138 +743,17 @@ MulticopterPositionControl::task_main()
 			_vel(1) = _local_pos.vy;
 			_vel(2) = _local_pos.vz;
 
-			sp_move_rate.zero();
+			_vel_ff.zero();
+			_sp_move_rate.zero();
 
 			/* select control source */
 			if (_control_mode.flag_control_manual_enabled) {
 				/* manual control */
-				if (_control_mode.flag_control_altitude_enabled) {
-					/* reset alt setpoint to current altitude if needed */
-					reset_alt_sp();
-
-					/* move altitude setpoint with throttle stick */
-					sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz);
-				}
-
-				if (_control_mode.flag_control_position_enabled) {
-					/* reset position setpoint to current position if needed */
-					reset_pos_sp();
-
-					/* move position setpoint with roll/pitch stick */
-					sp_move_rate(0) = _manual.x;
-					sp_move_rate(1) = _manual.y;
-				}
-
-				/* limit setpoint move rate */
-				float sp_move_norm = sp_move_rate.length();
-
-				if (sp_move_norm > 1.0f) {
-					sp_move_rate /= sp_move_norm;
-				}
-
-				/* scale to max speed and rotate around yaw */
-				math::Matrix<3, 3> R_yaw_sp;
-				R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
-				sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max);
-
-				/* move position setpoint */
-				_pos_sp += sp_move_rate * dt;
-
-				/* check if position setpoint is too far from actual position */
-				math::Vector<3> pos_sp_offs;
-				pos_sp_offs.zero();
-
-				if (_control_mode.flag_control_position_enabled) {
-					pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
-					pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
-				}
-
-				if (_control_mode.flag_control_altitude_enabled) {
-					pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
-				}
-
-				float pos_sp_offs_norm = pos_sp_offs.length();
-
-				if (pos_sp_offs_norm > 1.0f) {
-					pos_sp_offs /= pos_sp_offs_norm;
-					_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
-				}
+				control_manual(dt);
 
 			} else if (_control_mode.flag_control_offboard_enabled) {
-				/* Offboard control */
-				bool updated;
-				orb_check(_pos_sp_triplet_sub, &updated);
-
-				if (updated) {
-					orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
-				}
-
-				if (_pos_sp_triplet.current.valid) {
-
-					if (_control_mode.flag_control_position_enabled && _pos_sp_triplet.current.position_valid) {
-
-						_pos_sp(0) = _pos_sp_triplet.current.x;
-						_pos_sp(1) = _pos_sp_triplet.current.y;
-						_pos_sp(2) = _pos_sp_triplet.current.z;
-						_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
-
-					} else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) {
-						/* reset position setpoint to current position if needed */
-						reset_pos_sp();
-						/* move position setpoint with roll/pitch stick */
-						sp_move_rate(0) = _pos_sp_triplet.current.vx;
-						sp_move_rate(1) = _pos_sp_triplet.current.vy;
-						yaw_sp_move_rate = _pos_sp_triplet.current.yawspeed;
-						_att_sp.yaw_body = _att.yaw + yaw_sp_move_rate * dt;
-					}
-
-					if (_control_mode.flag_control_altitude_enabled) {
-						/* reset alt setpoint to current altitude if needed */
-						reset_alt_sp();
-
-						/* move altitude setpoint with throttle stick */
-						sp_move_rate(2) = -scale_control(_pos_sp_triplet.current.vz - 0.5f, 0.5f, alt_ctl_dz);;
-					}
-
-					/* limit setpoint move rate */
-					float sp_move_norm = sp_move_rate.length();
-
-					if (sp_move_norm > 1.0f) {
-						sp_move_rate /= sp_move_norm;
-					}
-
-					/* scale to max speed and rotate around yaw */
-					math::Matrix<3, 3> R_yaw_sp;
-					R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
-					sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max);
-
-					/* move position setpoint */
-					_pos_sp += sp_move_rate * dt;
-
-					/* check if position setpoint is too far from actual position */
-					math::Vector<3> pos_sp_offs;
-					pos_sp_offs.zero();
-
-					if (_control_mode.flag_control_position_enabled) {
-						pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
-						pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
-					}
-
-					if (_control_mode.flag_control_altitude_enabled) {
-						pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
-					}
-
-					float pos_sp_offs_norm = pos_sp_offs.length();
-
-					if (pos_sp_offs_norm > 1.0f) {
-						pos_sp_offs /= pos_sp_offs_norm;
-						_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
-					}
-
-				} else {
-					reset_pos_sp();
-					reset_alt_sp();
-				}
+				/* offboard control */
+				control_offboard(dt);
 
 			} else {
 				/* AUTO */
@@ -782,6 +788,7 @@ MulticopterPositionControl::task_main()
 			}
 
 			/* fill local position setpoint */
+			_local_pos_sp.timestamp = hrt_absolute_time();
 			_local_pos_sp.x = _pos_sp(0);
 			_local_pos_sp.y = _pos_sp(1);
 			_local_pos_sp.z = _pos_sp(2);
@@ -821,7 +828,7 @@ MulticopterPositionControl::task_main()
 				/* run position & altitude controllers, calculate velocity setpoint */
 				math::Vector<3> pos_err = _pos_sp - _pos;
 
-				_vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff);
+				_vel_sp = pos_err.emult(_params.pos_p) + _vel_ff;
 
 				if (!_control_mode.flag_control_altitude_enabled) {
 					_reset_alt_sp = true;
@@ -901,7 +908,7 @@ MulticopterPositionControl::task_main()
 					math::Vector<3> vel_err = _vel_sp - _vel;
 
 					/* derivative of velocity error, not includes setpoint acceleration */
-					math::Vector<3> vel_err_d = (sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt;
+					math::Vector<3> vel_err_d = (_sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt;
 					_vel_prev = _vel;
 
 					/* thrust vector in NED frame */
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index ba766cd10..c0e37a3ed 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -273,6 +273,10 @@ Mission::check_dist_1wp()
 
 						if (dist_to_1wp < _param_dist_1wp.get()) {
 							_dist_1wp_ok = true;
+							if (dist_to_1wp > ((_param_dist_1wp.get() * 3) / 2)) {
+								/* allow at 2/3 distance, but warn */
+								mavlink_log_critical(_navigator->get_mavlink_fd(), "Warning: First waypoint very far: %d m", (int)dist_to_1wp);
+							}
 							return true;
 
 						} else {
@@ -314,30 +318,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 +407,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 +493,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 +512,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 +521,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 +536,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_params.c b/src/modules/navigator/mission_params.c
index 881caa24e..d15e1e771 100644
--- a/src/modules/navigator/mission_params.c
+++ b/src/modules/navigator/mission_params.c
@@ -78,7 +78,7 @@ PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1);
  * waypoint is more distant than MIS_DIS_1WP from the current position.
  *
  * @min 0
- * @max 250
+ * @max 1000
  * @group Mission
  */
-PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 175);
+PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500);
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 31861c3fc..6c4b49452 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1597,14 +1597,12 @@ int sdlog2_thread_main(int argc, char *argv[])
 		if (copy_if_updated(ORB_ID(tecs_status), subs.tecs_status_sub, &buf.tecs_status)) {
 			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.altitudeFiltered = buf.tecs_status.altitude_filtered;
 			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;
 			log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp;
-			log_msg.body.log_TECS.airspeed = buf.tecs_status.airspeed;
-			log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeedFiltered;
+			log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeed_filtered;
 			log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp;
 			log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative;
 			log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp;
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 853a3811f..fb7609435 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -333,13 +333,11 @@ struct log_GS1B_s {
 #define LOG_TECS_MSG 30
 struct log_TECS_s {
 	float altitudeSp;
-	float altitude;
 	float altitudeFiltered;
 	float flightPathAngleSp;
 	float flightPathAngle;
 	float flightPathAngleFiltered;
 	float airspeedSp;
-	float airspeed;
 	float airspeedFiltered;
 	float airspeedDerivativeSp;
 	float airspeedDerivative;
@@ -455,7 +453,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, "fffffffffffffffB",	"ASP,A,AF,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
+	LOG_FORMAT(TECS, "fffffffffffffB",	"ASP,AF,FSP,F,FF,AsSP,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/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/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h
index 4a1932180..ec2131abd 100644
--- a/src/modules/uORB/topics/position_setpoint_triplet.h
+++ b/src/modules/uORB/topics/position_setpoint_triplet.h
@@ -60,7 +60,7 @@ enum SETPOINT_TYPE
 	SETPOINT_TYPE_TAKEOFF,		/**< takeoff setpoint */
 	SETPOINT_TYPE_LAND,		/**< land setpoint, altitude must be ignored, descend until landing */
 	SETPOINT_TYPE_IDLE,		/**< do nothing, switch off motors or keep at idle speed (MC) */
-	SETPOINT_TYPE_OFFBOARD, 	/**< setpoint set by offboard */
+	SETPOINT_TYPE_OFFBOARD, 	/**< setpoint in NED frame (x, y, z, vx, vy, vz) set by offboard */
 };
 
 struct position_setpoint_s
@@ -71,9 +71,9 @@ struct position_setpoint_s
 	float y;			/**< local position setpoint in m in NED */
 	float z;			/**< local position setpoint in m in NED */
 	bool position_valid;	/**< true if local position setpoint valid */
-	float vx;			/**< local velocity setpoint in m in NED */
-	float vy;			/**< local velocity setpoint in m in NED */
-	float vz;			/**< local velocity setpoint in m in NED */
+	float vx;			/**< local velocity setpoint in m/s in NED */
+	float vy;			/**< local velocity setpoint in m/s in NED */
+	float vz;			/**< local velocity setpoint in m/s in NED */
 	bool velocity_valid;		/**< true if local velocity setpoint valid */
 	double lat;			/**< latitude, in deg */
 	double lon;			/**< longitude, in deg */
diff --git a/src/modules/uORB/topics/tecs_status.h b/src/modules/uORB/topics/tecs_status.h
index 33055018c..ddca19d61 100644
--- a/src/modules/uORB/topics/tecs_status.h
+++ b/src/modules/uORB/topics/tecs_status.h
@@ -51,11 +51,13 @@
  */
 
 typedef enum {
-	TECS_MODE_NORMAL,
+	TECS_MODE_NORMAL = 0,
 	TECS_MODE_UNDERSPEED,
 	TECS_MODE_TAKEOFF,
 	TECS_MODE_LAND,
-	TECS_MODE_LAND_THROTTLELIM
+	TECS_MODE_LAND_THROTTLELIM,
+	TECS_MODE_BAD_DESCENT,
+	TECS_MODE_CLIMBOUT
 } tecs_mode;
 
  /**
@@ -65,14 +67,12 @@ struct tecs_status_s {
 	uint64_t timestamp;		/**< timestamp, in microseconds since system start */
 
 	float altitudeSp;
-	float altitude;
-	float altitudeFiltered;
+	float altitude_filtered;
 	float flightPathAngleSp;
 	float flightPathAngle;
 	float flightPathAngleFiltered;
 	float airspeedSp;
-	float airspeed;
-	float airspeedFiltered;
+	float airspeed_filtered;
 	float airspeedDerivativeSp;
 	float airspeedDerivative;
 
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_local_position_setpoint.h b/src/modules/uORB/topics/vehicle_local_position_setpoint.h
index 8988a0330..6766bb58a 100644
--- a/src/modules/uORB/topics/vehicle_local_position_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_local_position_setpoint.h
@@ -50,11 +50,12 @@
  */
 
 struct vehicle_local_position_setpoint_s {
+	uint64_t timestamp;	/**< timestamp of the setpoint */
 	float x;		/**< in meters NED			  		*/
 	float y;		/**< in meters NED			  		*/
 	float z;		/**< in meters NED			  		*/
 	float yaw;		/**< in radians NED -PI..+PI  		*/
-}; /**< Local position in NED frame to go to */
+}; /**< Local position in NED frame */
 
 /**
  * @}
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 7a40ac636..d9220fe14 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;
@@ -228,6 +228,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/module.mk b/src/modules/uavcan/module.mk
index 1ef6f0cfa..3865f2468 100644
--- a/src/modules/uavcan/module.mk
+++ b/src/modules/uavcan/module.mk
@@ -65,10 +65,6 @@ override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM
 
 #
 # 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/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index 27e77e9c5..4535b6d6a 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -39,6 +39,8 @@
 #include <systemlib/err.h>
 #include <systemlib/systemlib.h>
 #include <systemlib/mixer/mixer.h>
+#include <systemlib/board_serial.h>
+#include <version/version.h>
 #include <arch/board/board.h>
 #include <arch/chip/chip.h>
 
@@ -173,6 +175,41 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
 	return OK;
 }
 
+void UavcanNode::fill_node_info()
+{
+	/* software version */
+	uavcan::protocol::SoftwareVersion swver;
+
+	// Extracting the first 8 hex digits of FW_GIT and converting them to int
+	char fw_git_short[9] = {};
+	std::memmove(fw_git_short, FW_GIT, 8);
+	assert(fw_git_short[8] == '\0');
+	char *end = nullptr;
+	swver.vcs_commit = std::strtol(fw_git_short, &end, 16);
+	swver.optional_field_mask |= swver.OPTIONAL_FIELD_MASK_VCS_COMMIT;
+
+	warnx("SW version vcs_commit: 0x%08x", unsigned(swver.vcs_commit));
+
+	_node.setSoftwareVersion(swver);
+
+	/* hardware version */
+	uavcan::protocol::HardwareVersion hwver;
+
+	if (!std::strncmp(HW_ARCH, "PX4FMU_V1", 9)) {
+		hwver.major = 1;
+	} else if (!std::strncmp(HW_ARCH, "PX4FMU_V2", 9)) {
+		hwver.major = 2;
+	} else {
+		; // All other values of HW_ARCH resolve to zero
+	}
+
+	uint8_t udid[12] = {};  // Someone seems to love magic numbers
+	get_board_serial(udid);
+	uavcan::copy(udid, udid + sizeof(udid), hwver.unique_id.begin());
+
+	_node.setHardwareVersion(hwver);
+}
+
 int UavcanNode::init(uavcan::NodeID node_id)
 {
 	int ret = -1;
@@ -183,6 +220,13 @@ int UavcanNode::init(uavcan::NodeID node_id)
 	if (ret != OK)
 		return ret;
 
+	_node.setName("org.pixhawk.pixhawk");
+
+	_node.setNodeID(node_id);
+
+	fill_node_info();
+
+	/* initializing the bridges UAVCAN <--> uORB */
 	ret = _esc_controller.init();
 	if (ret < 0)
 		return ret;
@@ -191,20 +235,6 @@ int UavcanNode::init(uavcan::NodeID node_id)
 	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();
 }
 
@@ -223,7 +253,6 @@ int UavcanNode::run()
 	// XXX figure out the output count
 	_output_count = 2;
 
-
 	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
 
 	actuator_outputs_s outputs;
@@ -243,21 +272,23 @@ int UavcanNode::run()
 
 	_node.setStatusOk();
 
-	while (!_task_should_exit) {
+	/*
+	 * 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_num = 0;
+	_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;
 
+	while (!_task_should_exit) {
+		// update actuator controls subscriptions if needed
 		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);
@@ -271,7 +302,7 @@ int UavcanNode::run()
 		} else {
 			// get controls for required topics
 			bool controls_updated = false;
-			unsigned poll_id = 0;
+			unsigned poll_id = 1;
 			for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 				if (_control_subs[i] > 0) {
 					if (_poll_fds[poll_id].revents & POLLIN) {
@@ -282,12 +313,7 @@ int UavcanNode::run()
 				}
 			}
 
-			if (!controls_updated) {
-				// timeout: no control data, switch to failsafe values
-				// XXX trigger failsafe
-			}
-
-			//can we mix?
+			// can we mix?
 			if (controls_updated && (_mixers != nullptr)) {
 
 				// XXX one output group has 8 outputs max,
@@ -387,7 +413,8 @@ 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;
+	// the first fd used by CAN
+	_poll_fds_num = 1;
 	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 		if (sub_groups & (1 << i)) {
 			warnx("subscribe to actuator_controls_%d", i);
@@ -493,8 +520,8 @@ UavcanNode::print_info()
 		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");
+	warnx("actuators control groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
+	warnx("mixer: %s", (_mixers == nullptr) ? "NONE" : "OK");
 }
 
 /*
diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp
index 443525379..05b66fd7b 100644
--- a/src/modules/uavcan/uavcan_main.hpp
+++ b/src/modules/uavcan/uavcan_main.hpp
@@ -94,6 +94,7 @@ public:
 	static UavcanNode* instance() { return _instance; }
 
 private:
+	void		fill_node_info();
 	int		init(uavcan::NodeID node_id);
 	void		node_spin_once();
 	int		run();
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