merged master

26 files changed, 1115 insertions, 650 deletions

diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom
index 24372bddc..d05c3174f 100644
--- a/ROMFS/px4fmu_common/init.d/3031_phantom
+++ b/ROMFS/px4fmu_common/init.d/3031_phantom
@@ -37,3 +37,7 @@ then
 fi
 
 set MIXER FMU_Q
+
+# Provide ESC a constant 1000 us pulse
+set PWM_OUTPUTS 4
+set PWM_DISARMED 1000
diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb
index 02002b55f..1f8d8b862 100644
--- a/ROMFS/px4fmu_common/init.d/rc.usb
+++ b/ROMFS/px4fmu_common/init.d/rc.usb
@@ -17,6 +17,8 @@ mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROLS -r 30
 usleep 100000
 mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20
 usleep 100000
+mavlink stream -d /dev/ttyACM0 -s GLOBAL_POSITION_SETPOINT_INT -r 20
+usleep 100000
 
 # Exit shell to make it available to MAVLink
 exit
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 972f45148..992ab9623 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -197,8 +197,10 @@ public:
 	 * Print IO status.
 	 *
 	 * Print all relevant IO status information
+	 *
+	 * @param extended_status Shows more verbose information (in particular RC config)
 	 */
-	void			print_status();
+	void			print_status(bool extended_status);
 
 	/**
 	 * Fetch and print debug console output.
@@ -1850,7 +1852,7 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
 }
 
 void
-PX4IO::print_status()
+PX4IO::print_status(bool extended_status)
 {
 	/* basic configuration */
 	printf("protocol %u hardware %u bootloader %u buffer %uB crc 0x%04x%04x\n",
@@ -2013,19 +2015,21 @@ PX4IO::print_status()
 		printf("\n");
 	}
 
-	for (unsigned i = 0; i < _max_rc_input; i++) {
-		unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
-		uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS);
-		printf("input %u min %u center %u max %u deadzone %u assigned %u options 0x%04x%s%s\n",
-		       i,
-		       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN),
-		       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER),
-		       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX),
-		       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE),
-		       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT),
-		       options,
-		       ((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""),
-		       ((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : ""));
+	if (extended_status) {
+		for (unsigned i = 0; i < _max_rc_input; i++) {
+			unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
+			uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS);
+			printf("input %u min %u center %u max %u deadzone %u assigned %u options 0x%04x%s%s\n",
+			       i,
+			       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN),
+			       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER),
+			       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX),
+			       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE),
+			       io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT),
+			       options,
+			       ((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""),
+			       ((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : ""));
+		}
 	}
 
 	printf("failsafe");
@@ -2853,7 +2857,7 @@ monitor(void)
 		if (g_dev != nullptr) {
 
 			printf("\033[2J\033[H"); /* move cursor home and clear screen */
-			(void)g_dev->print_status();
+			(void)g_dev->print_status(false);
 			(void)g_dev->print_debug();
 			printf("\n\n\n[ Use 'px4io debug <N>' for more output. Hit <enter> three times to exit monitor mode ]\n");
 
@@ -3119,7 +3123,7 @@ px4io_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "status")) {
 
 		printf("[px4io] loaded\n");
-		g_dev->print_status();
+		g_dev->print_status(true);
 
 		exit(0);
 	}
diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp
index 28ec62356..7b6361a7c 100644
--- a/src/drivers/px4io/px4io_uploader.cpp
+++ b/src/drivers/px4io/px4io_uploader.cpp
@@ -240,9 +240,9 @@ PX4IO_Uploader::upload(const char *filenames[])
 	close(_io_fd);
 	_io_fd = -1;
 
-        // sleep for enough time for the IO chip to boot. This makes
-        // forceupdate more reliably startup IO again after update
-        up_udelay(100*1000);
+	// sleep for enough time for the IO chip to boot. This makes
+	// forceupdate more reliably startup IO again after update
+	up_udelay(100*1000);
 
 	return ret;
 }
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
index 0a909d02f..a0a18bc2e 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
@@ -61,9 +61,9 @@ ECL_PitchController::ECL_PitchController() :
 	_integrator(0.0f),
 	_rate_error(0.0f),
 	_rate_setpoint(0.0f),
-	_bodyrate_setpoint(0.0f)
+	_bodyrate_setpoint(0.0f),
+	_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control pitch nonfinite input"))
 {
-	perf_alloc(PC_COUNT, "fw att control pitch nonfinite input");
 }
 
 ECL_PitchController::~ECL_PitchController()
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
index 82903ef5a..d2a231694 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
@@ -59,9 +59,9 @@ ECL_RollController::ECL_RollController() :
 	_integrator(0.0f),
 	_rate_error(0.0f),
 	_rate_setpoint(0.0f),
-	_bodyrate_setpoint(0.0f)
+	_bodyrate_setpoint(0.0f),
+	_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control roll nonfinite input"))
 {
-	perf_alloc(PC_COUNT, "fw att control roll nonfinite input");
 }
 
 ECL_RollController::~ECL_RollController()
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
index e53ffc644..79184e2cd 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
@@ -58,9 +58,9 @@ ECL_YawController::ECL_YawController() :
 	_rate_error(0.0f),
 	_rate_setpoint(0.0f),
 	_bodyrate_setpoint(0.0f),
-	_coordinated_min_speed(1.0f)
+	_coordinated_min_speed(1.0f),
+	_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control yaw nonfinite input"))
 {
-	perf_alloc(PC_COUNT, "fw att control yaw nonfinite input");
 }
 
 ECL_YawController::~ECL_YawController()
diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h
index e2f3da6f8..87c1cf460 100644
--- a/src/lib/geo/geo.h
+++ b/src/lib/geo/geo.h
@@ -50,7 +50,7 @@
 
 __BEGIN_DECLS
 
-#include "geo/geo_mag_declination.h"
+#include "geo_mag_declination.h"
 
 #include <stdbool.h>
 
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index 66a949af1..5bcce1b4d 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -65,6 +65,7 @@
 #include <drivers/drv_hrt.h>
 
 #include <lib/mathlib/mathlib.h>
+#include <lib/geo/geo.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/perf_counter.h>
@@ -287,6 +288,9 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
 	float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
 
+	/* magnetic declination, in radians */
+	float mag_decl = 0.0f;
+
 	/* rotation matrix for magnetic declination */
 	math::Matrix<3, 3> R_decl;
 	R_decl.identity();
@@ -325,9 +329,6 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
 				/* update parameters */
 				parameters_update(&ekf_param_handles, &ekf_params);
-
-				/* update mag declination rotation matrix */
-				R_decl.from_euler(0.0f, 0.0f, ekf_params.mag_decl);
 			}
 
 			/* only run filter if sensor values changed */
@@ -340,6 +341,13 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 				orb_check(sub_gps, &gps_updated);
 				if (gps_updated) {
 					orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps);
+
+					if (gps.eph_m < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) {
+						mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f));
+
+						/* update mag declination rotation matrix */
+						R_decl.from_euler(0.0f, 0.0f, mag_decl);
+					}
 				}
 
 				bool global_pos_updated;
@@ -469,7 +477,15 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 						parameters_update(&ekf_param_handles, &ekf_params);
 
 						/* update mag declination rotation matrix */
-						R_decl.from_euler(0.0f, 0.0f, ekf_params.mag_decl);
+						if (gps.eph_m < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) {
+							mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f));
+
+						} else {
+							mag_decl = ekf_params.mag_decl;
+						}
+
+						/* update mag declination rotation matrix */
+						R_decl.from_euler(0.0f, 0.0f, mag_decl);
 
 						x_aposteriori_k[0] = z_k[0];
 						x_aposteriori_k[1] = z_k[1];
@@ -515,7 +531,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
 					att.roll = euler[0];
 					att.pitch = euler[1];
-					att.yaw = euler[2] + ekf_params.mag_decl;
+					att.yaw = euler[2] + mag_decl;
 
 					att.rollspeed = x_aposteriori[0];
 					att.pitchspeed = x_aposteriori[1];
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 65922b2a5..588f48225 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -412,7 +412,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			/* if HIL got enabled, reset battery status state */
 			if (hil_ret == OK && status->hil_state == HIL_STATE_ON) {
 				/* reset the arming mode to disarmed */
-				arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed);
+				arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed, mavlink_fd);
 
 				if (arming_res != TRANSITION_DENIED) {
 					mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby");
@@ -945,8 +945,8 @@ 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_log_info(mavlink_fd, "[cmd] DISARMED by safety switch");
+				if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) {
+					mavlink_log_info(mavlink_fd, "#audio DISARMED by safety switch");
 				}
 			}
 		}
@@ -1149,10 +1149,10 @@ int commander_thread_main(int argc, char *argv[])
 			status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
 
 			if (armed.armed) {
-				arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
+				arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, mavlink_fd);
 
 			} else {
-				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
+				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, mavlink_fd);
 			}
 
 			status_changed = true;
@@ -1163,7 +1163,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) {
 			// XXX check for sensors
-			arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
+			arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd);
 
 		} else {
 			// XXX: Add emergency stuff if sensors are lost
@@ -1217,7 +1217,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_res = arming_state_transition(&status, &safety, new_arming_state, &armed);
+					arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd);
 					stick_off_counter = 0;
 
 				} else {
@@ -1239,7 +1239,7 @@ int commander_thread_main(int argc, char *argv[])
 						print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first.");
 
 					} else {
-						arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
+						arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd);
 					}
 
 					stick_on_counter = 0;
@@ -1941,7 +1941,7 @@ void *commander_low_prio_loop(void *arg)
 				/* try to go to INIT/PREFLIGHT arming state */
 
 				// XXX disable interrupts in arming_state_transition
-				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) {
+				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, mavlink_fd)) {
 					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
 					break;
 				}
@@ -2004,7 +2004,7 @@ void *commander_low_prio_loop(void *arg)
 					tune_negative(true);
 				}
 
-				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
+				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd);
 
 				break;
 			}
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index 0ddd4f05a..abb917873 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -57,7 +57,7 @@ typedef enum {
 } transition_result_t;
 
 transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
-		arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd = 0);
+		arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd);
 
 bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
 
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index 178b590ae..3cd4ce928 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -707,14 +707,21 @@ FixedwingAttitudeControl::task_main()
 				float throttle_sp = 0.0f;
 
 				if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) {
+					/* read in attitude setpoint from attitude setpoint uorb topic */
 					roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
 					pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
 					throttle_sp = _att_sp.thrust;
 
 					/* reset integrals where needed */
-					if (_att_sp.roll_reset_integral)
+					if (_att_sp.roll_reset_integral) {
 						_roll_ctrl.reset_integrator();
-
+					}
+					if (_att_sp.pitch_reset_integral) {
+						_pitch_ctrl.reset_integrator();
+					}
+					if (_att_sp.yaw_reset_integral) {
+						_yaw_ctrl.reset_integrator();
+					}
 				} else {
 					/*
 					 * Scale down roll and pitch as the setpoints are radians
diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c
index aa637e207..7cae84678 100644
--- a/src/modules/fw_att_control/fw_att_control_params.c
+++ b/src/modules/fw_att_control/fw_att_control_params.c
@@ -50,149 +50,322 @@ f *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
  * Controller parameters, accessible via MAVLink
  *
  */
-// @DisplayName		Attitude Time Constant
-// @Description		This defines the latency between a step input and the achieved setpoint (inverse to a P gain). Half a second is a good start value and fits for most average systems. Smaller systems may require smaller values, but as this will wear out servos faster, the value should only be decreased as needed.
-// @Range		0.4 to 1.0 seconds, in tens of seconds
+
+/**
+ * Attitude Time Constant
+ *
+ * This defines the latency between a step input and the achieved setpoint
+ * (inverse to a P gain). Half a second is a good start value and fits for
+ * most average systems. Smaller systems may require smaller values, but as
+ * this will wear out servos faster, the value should only be decreased as
+ * needed.
+ *
+ * @unit seconds
+ * @min 0.4
+ * @max 1.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f);
 
-// @DisplayName		Pitch rate proportional gain.
-// @Description		This defines how much the elevator input will be commanded depending on the current body angular rate error.
-// @Range		10 to 200, 1 increments
+/**
+ * Pitch rate proportional gain.
+ *
+ * This defines how much the elevator input will be commanded depending on the
+ * current body angular rate error.
+ *
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_PR_P, 0.05f);
 
-// @DisplayName		Pitch rate integrator gain.
-// @Description		This gain defines how much control response will result out of a steady state error. It trims any constant error.
-// @Range		0 to 50.0
+/**
+ * Pitch rate integrator gain.
+ *
+ * This gain defines how much control response will result out of a steady
+ * state error. It trims any constant error.
+ *
+ * @min 0.0
+ * @max 50.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_PR_I, 0.0f);
 
-// @DisplayName		Maximum positive / up pitch rate.
-// @Description		This limits the maximum pitch up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
-// @Range		0 to 90.0 degrees per seconds, in 1 increments
+/**
+ * Maximum positive / up pitch rate.
+ *
+ * This limits the maximum pitch up angular rate the controller will output (in
+ * degrees per second). Setting a value of zero disables the limit.
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 90.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 0.0f);
 
-// @DisplayName		Maximum negative / down pitch rate.
-// @Description		This limits the maximum pitch down up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
-// @Range		0 to 90.0 degrees per seconds, in 1 increments
+/**
+ * Maximum negative / down pitch rate.
+ *
+ * This limits the maximum pitch down up angular rate the controller will
+ * output (in degrees per second). Setting a value of zero disables the limit.
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 90.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f);
 
-// @DisplayName		Pitch rate integrator limit
-// @Description		The portion of the integrator part in the control surface deflection is limited to this value
-// @Range		0.0 to 1
-// @Increment		0.1
+/**
+ * Pitch rate integrator limit
+ *
+ * The portion of the integrator part in the control surface deflection is
+ * limited to this value
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.2f);
 
-// @DisplayName		Roll to Pitch feedforward gain.
-// @Description		This compensates during turns and ensures the nose stays level.
-// @Range		0.5 2.0
-// @Increment		0.05
-// @User		User
+/**
+ * Roll to Pitch feedforward gain.
+ *
+ * This compensates during turns and ensures the nose stays level.
+ *
+ * @min 0.0
+ * @max 2.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 0.0f); //xxx: set to 0 as default, see comment in ECL_PitchController::control_attitude (float turn_offset = ...)
 
-// @DisplayName		Roll rate proportional Gain.
-// @Description		This defines how much the aileron input will be commanded depending on the current body angular rate error.
-// @Range		10.0 200.0
-// @Increment		10.0
-// @User		User
+/**
+ * Roll rate proportional Gain
+ *
+ * This defines how much the aileron input will be commanded depending on the
+ * current body angular rate error.
+ *
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f);
 
-// @DisplayName		Roll rate integrator Gain
-// @Description		This gain defines how much control response will result out of a steady state error. It trims any constant error.
-// @Range		0.0 100.0
-// @Increment		5.0
-// @User		User
+/**
+ * Roll rate integrator Gain
+ *
+ * This gain defines how much control response will result out of a steady
+ * state error. It trims any constant error.
+ *
+ * @min 0.0
+ * @max 100.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_RR_I, 0.0f);
 
-// @DisplayName		Roll Integrator Anti-Windup
-// @Description		The portion of the integrator part in the control surface deflection is limited to this value.
-// @Range		0.0 to 1.0
-// @Increment		0.1
+/**
+ * Roll Integrator Anti-Windup
+ *
+ * The portion of the integrator part in the control surface deflection is limited to this value.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.2f);
 
-// @DisplayName		Maximum Roll Rate
-// @Description		This limits the maximum roll rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
-// @Range		0 to 90.0 degrees per seconds
-// @Increment		1.0
-PARAM_DEFINE_FLOAT(FW_R_RMAX, 0);
+/**
+ * Maximum Roll Rate
+ *
+ * This limits the maximum roll rate the controller will output (in degrees per
+ * second). Setting a value of zero disables the limit.
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 90.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_R_RMAX, 0.0f);
 
-// @DisplayName		Yaw rate proportional gain.
-// @Description		This defines how much the rudder input will be commanded depending on the current body angular rate error.
-// @Range		10 to 200, 1 increments
-PARAM_DEFINE_FLOAT(FW_YR_P, 0.05);
+/**
+ * Yaw rate proportional gain
+ *
+ * This defines how much the rudder input will be commanded depending on the
+ * current body angular rate error.
+ *
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_YR_P, 0.05f);
 
-// @DisplayName		Yaw rate integrator gain.
-// @Description		This gain defines how much control response will result out of a steady state error. It trims any constant error.
-// @Range		0 to 50.0
+/**
+ * Yaw rate integrator gain
+ *
+ * This gain defines how much control response will result out of a steady
+ * state error. It trims any constant error.
+ *
+ * @min 0.0
+ * @max 50.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_YR_I, 0.0f);
 
-// @DisplayName		Yaw rate integrator limit
-// @Description		The portion of the integrator part in the control surface deflection is limited to this value
-// @Range		0.0 to 1
-// @Increment		0.1
+/**
+ * Yaw rate integrator limit
+ *
+ * The portion of the integrator part in the control surface deflection is
+ * limited to this value
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f);
 
-// @DisplayName		Maximum Yaw Rate
-// @Description		This limits the maximum yaw rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
-// @Range		0 to 90.0 degrees per seconds
-// @Increment		1.0
-PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0);
+/**
+ * Maximum Yaw Rate
+ *
+ * This limits the maximum yaw rate the controller will output (in degrees per
+ * second). Setting a value of zero disables the limit.
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 90.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f);
 
-// @DisplayName		Roll rate feed forward
-// @Description		Direct feed forward from rate setpoint to control surface output
-// @Range		0 to 10
-// @Increment		0.1
+/**
+ * Roll rate feed forward
+ *
+ * Direct feed forward from rate setpoint to control surface output
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_RR_FF, 0.3f);
 
-// @DisplayName		Pitch rate feed forward
-// @Description		Direct feed forward from rate setpoint to control surface output
-// @Range		0 to 10
-// @Increment		0.1
+/**
+ * Pitch rate feed forward
+ *
+ * Direct feed forward from rate setpoint to control surface output
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_PR_FF, 0.4f);
 
-// @DisplayName		Yaw rate feed forward
-// @Description		Direct feed forward from rate setpoint to control surface output
-// @Range		0 to 10
-// @Increment		0.1
+/**
+ * Yaw rate feed forward
+ *
+ * Direct feed forward from rate setpoint to control surface output
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f);
 
-// @DisplayName		Minimal speed for yaw coordination
-// @Description		For airspeeds above this value the yaw rate is calculated for a coordinated turn. Set to a very high value to disable.
-// @Range		0 to 90.0 degrees per seconds
-// @Increment		1.0
+/**
+ * Minimal speed for yaw coordination
+ *
+ * For airspeeds above this value, the yaw rate is calculated for a coordinated
+ * turn. Set to a very high value to disable.
+ *
+ * @unit m/s
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_YCO_VMIN, 1000.0f);
 
-/* Airspeed parameters: the following parameters about airspeed are used by the attitude and the positon controller */
+/* Airspeed parameters:
+ * The following parameters about airspeed are used by the attitude and the
+ * position controller.
+ * */
 
-// @DisplayName		Minimum Airspeed
-// @Description		If the airspeed falls below this value the TECS controller will try to increase airspeed more aggressively
-// @Range		0.0 to 30
+/**
+ * Minimum Airspeed
+ *
+ * If the airspeed falls below this value, the TECS controller will try to
+ * increase airspeed more aggressively.
+ *
+ * @unit m/s
+ * @min 0.0
+ * @max 30.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 13.0f);
 
-// @DisplayName		Trim Airspeed
-// @Description		The TECS controller tries to fly at this airspeed
-// @Range		0.0 to 30
+/**
+ * Trim Airspeed
+ *
+ * The TECS controller tries to fly at this airspeed.
+ *
+ * @unit m/s
+ * @min 0.0
+ * @max 30.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 20.0f);
 
-// @DisplayName		Maximum Airspeed
-// @Description		If the airspeed is above this value the TECS controller will try to decrease airspeed more aggressively
-// @Range		0.0 to 30
+/**
+ * Maximum Airspeed
+ *
+ * If the airspeed is above this value, the TECS controller will try to decrease
+ * airspeed more aggressively.
+ *
+ * @unit m/s
+ * @min 0.0
+ * @max 30.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 50.0f);
 
-// @DisplayName		Roll Setpoint Offset
-// @Description		An airframe specific offset of the roll setpoint in degrees, the value is added to the roll setpoint and should correspond to the typical cruise speed of the airframe
-// @Range		-90.0 to 90.0
+/**
+ * Roll Setpoint Offset
+ *
+ * An airframe specific offset of the roll setpoint in degrees, the value is
+ * added to the roll setpoint and should correspond to the typical cruise speed
+ * of the airframe.
+ *
+ * @unit deg
+ * @min -90.0
+ * @max 90.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_RSP_OFF, 0.0f);
 
-// @DisplayName		Pitch Setpoint Offset
-// @Description		An airframe specific offset of the pitch setpoint in degrees, the value is added to the pitch setpoint and should correspond to the typical cruise speed of the airframe
-// @Range		-90.0 to 90.0
+/**
+ * Pitch Setpoint Offset
+ *
+ * An airframe specific offset of the pitch setpoint in degrees, the value is
+ * added to the pitch setpoint and should correspond to the typical cruise
+ * speed of the airframe.
+ *
+ * @unit deg
+ * @min -90.0
+ * @max 90.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f);
 
-// @DisplayName		Max Manual Roll
-// @Description		Max roll for manual control in attitude stabilized mode
-// @Range		0.0 to 90.0
+/**
+ * Max Manual Roll
+ *
+ * Max roll for manual control in attitude stabilized mode
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 90.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f);
 
-// @DisplayName		Max Manual Pitch
-// @Description		Max pitch for manual control in attitude stabilized mode
-// @Range		0.0 to 90.0
+/**
+ * Max Manual Pitch
+ *
+ * Max pitch for manual control in attitude stabilized mode
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 90.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 45.0f);
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 116d3cc63..0e065211e 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
@@ -841,6 +841,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		/* current waypoint (the one currently heading for) */
 		math::Vector<2> curr_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon);
 
+		/* Initialize attitude controller integrator reset flags to 0 */
+		_att_sp.roll_reset_integral = false;
+		_att_sp.pitch_reset_integral = false;
+		_att_sp.yaw_reset_integral = false;
 
 		/* previous waypoint */
 		math::Vector<2> prev_wp;
@@ -1020,15 +1024,21 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) {
 
 			/* Perform launch detection */
-//			warnx("Launch detection running");
 			if(!launch_detected) { //do not do further checks once a launch was detected
 				if (launchDetector.launchDetectionEnabled()) {
 					static hrt_abstime last_sent = 0;
 					if(hrt_absolute_time() - last_sent > 4e6) {
-//						warnx("Launch detection running");
 						mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running");
 						last_sent = hrt_absolute_time();
 					}
+
+					/* Tell the attitude controller to stop integrating while we are waiting
+					 * for the launch */
+					_att_sp.roll_reset_integral = true;
+					_att_sp.pitch_reset_integral = true;
+					_att_sp.yaw_reset_integral = true;
+
+					/* Detect launch */
 					launchDetector.update(_sensor_combined.accelerometer_m_s2[0]);
 					if (launchDetector.getLaunchDetected()) {
 						launch_detected = true;
diff --git a/src/modules/mavlink/mavlink_commands.cpp b/src/modules/mavlink/mavlink_commands.cpp
index 1c1e097a4..fccd4d9a5 100644
--- a/src/modules/mavlink/mavlink_commands.cpp
+++ b/src/modules/mavlink/mavlink_commands.cpp
@@ -40,34 +40,31 @@
 
 #include "mavlink_commands.h"
 
-MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel)
+MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel), _cmd_time(0)
 {
 	_cmd_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command));
-	_cmd = (struct vehicle_command_s *)_cmd_sub->get_data();
-}
-
-MavlinkCommandsStream::~MavlinkCommandsStream()
-{
 }
 
 void
 MavlinkCommandsStream::update(const hrt_abstime t)
 {
-	if (_cmd_sub->update(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) {
+		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);
+						      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_commands.h b/src/modules/mavlink/mavlink_commands.h
index 6255d65df..abdba34b9 100644
--- a/src/modules/mavlink/mavlink_commands.h
+++ b/src/modules/mavlink/mavlink_commands.h
@@ -55,10 +55,10 @@ private:
 	MavlinkOrbSubscription *_cmd_sub;
 	struct vehicle_command_s *_cmd;
 	mavlink_channel_t _channel;
+	uint64_t _cmd_time;
 
 public:
 	MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel);
-	~MavlinkCommandsStream();
 	void update(const hrt_abstime t);
 };
 
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 066d25bf6..647519ea5 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -242,6 +242,12 @@ Mavlink::Mavlink() :
 	_subscribe_to_stream_rate(0.0f),
 	_flow_control_enabled(true),
 	_message_buffer({}),
+	_param_initialized(false),
+	_param_system_id(0),
+	_param_component_id(0),
+	_param_system_type(0),
+	_param_use_hil_gps(0),
+
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink"))
 {
@@ -500,44 +506,39 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
 
 void Mavlink::mavlink_update_system(void)
 {
-	static bool initialized = false;
-	static param_t param_system_id;
-	static param_t param_component_id;
-	static param_t param_system_type;
-	static param_t param_use_hil_gps;
 
-	if (!initialized) {
-		param_system_id = param_find("MAV_SYS_ID");
-		param_component_id = param_find("MAV_COMP_ID");
-		param_system_type = param_find("MAV_TYPE");
-		param_use_hil_gps = param_find("MAV_USEHILGPS");
-		initialized = true;
+	if (!_param_initialized) {
+		_param_system_id = param_find("MAV_SYS_ID");
+		_param_component_id = param_find("MAV_COMP_ID");
+		_param_system_type = param_find("MAV_TYPE");
+		_param_use_hil_gps = param_find("MAV_USEHILGPS");
+		_param_initialized = true;
 	}
 
 	/* update system and component id */
 	int32_t system_id;
-	param_get(param_system_id, &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);
+	param_get(_param_component_id, &component_id);
 
 	if (component_id > 0 && component_id < 255) {
 		mavlink_system.compid = component_id;
 	}
 
 	int32_t system_type;
-	param_get(param_system_type, &system_type);
+	param_get(_param_system_type, &system_type);
 
 	if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
 		mavlink_system.type = system_type;
 	}
 
 	int32_t use_hil_gps;
-	param_get(param_use_hil_gps, &use_hil_gps);
+	param_get(_param_use_hil_gps, &use_hil_gps);
 
 	_use_hil_gps = (bool)use_hil_gps;
 }
@@ -835,7 +836,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
 
 					if (param == PARAM_INVALID) {
 						char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
-						sprintf(buf, "[mavlink pm] unknown: %s", name);
+						sprintf(buf, "[pm] unknown: %s", name);
 						mavlink_missionlib_send_gcs_string(buf);
 
 					} else {
@@ -1024,8 +1025,6 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq)
 
 	} else {
 		mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds");
-
-		if (_verbose) { warnx("ERROR: index out of bounds"); }
 	}
 }
 
@@ -1098,8 +1097,6 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u
 
 	} else {
 		mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity");
-
-		if (_verbose) { warnx("ERROR: Waypoint index exceeds list capacity"); }
 	}
 }
 
@@ -1130,8 +1127,6 @@ void Mavlink::mavlink_waypoint_eventloop(uint64_t now)
 
 		mavlink_missionlib_send_gcs_string("Operation timeout");
 
-		if (_verbose) { warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_state); }
-
 		_wpm->current_state = MAVLINK_WPM_STATE_IDLE;
 		_wpm->current_partner_sysid = 0;
 		_wpm->current_partner_compid = 0;
@@ -1166,8 +1161,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 			} else {
 				mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch");
-
-				if (_verbose) { warnx("REJ. WP CMD: curr partner id mismatch"); }
 			}
 
 			break;
@@ -1191,15 +1184,10 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 					} else {
 						mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list");
-
-						if (_verbose) { warnx("IGN WP CURR CMD: Not in list"); }
 					}
 
 				} else {
 					mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy");
-
-					if (_verbose) { warnx("IGN WP CURR CMD: Busy"); }
-
 				}
 			}
 
@@ -1230,8 +1218,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 				} else {
 					mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy");
-
-					if (_verbose) { warnx("IGN REQUEST LIST: Busy"); }
 				}
 			}
 
@@ -1249,8 +1235,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 					mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list");
 
-					if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq); }
-
 					break;
 				}
 
@@ -1261,15 +1245,13 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 				if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) {
 
 					if (wpr.seq == 0) {
-						if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
+						if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u from %u changing to STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
 
 						_wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
 
 					} else {
 						mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0");
 
-						if (_verbose) { warnx("REJ. WP CMD: First id != 0"); }
-
 						break;
 					}
 
@@ -1277,17 +1259,15 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 					if (wpr.seq == _wpm->current_wp_id) {
 
-						if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
+						if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u (again) from %u staying in STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
 
 					} else if (wpr.seq == _wpm->current_wp_id + 1) {
 
-						if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
+						if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u from %u staying in STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
 
 					} else {
 						mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected");
 
-						if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, _wpm->current_wp_id, _wpm->current_wp_id + 1); }
-
 						break;
 					}
 
@@ -1295,8 +1275,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 					mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
 
-					if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", _wpm->current_state); }
-
 					break;
 				}
 
@@ -1310,7 +1288,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 				} else {
 					mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
 
-					if (_verbose) { warnx("ERROR: Waypoint %u out of bounds", wpr.seq); }
+					mavlink_missionlib_send_gcs_string("ERROR: Waypoint out of bounds");
 				}
 
 
@@ -1344,15 +1322,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 					}
 
 					if (wpc.count == 0) {
-						mavlink_missionlib_send_gcs_string("COUNT 0");
-
-						if (_verbose) { warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE"); }
+						mavlink_missionlib_send_gcs_string("WP COUNT 0");
 
 						break;
 					}
 
-					if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); }
-
 					_wpm->current_state = MAVLINK_WPM_STATE_GETLIST;
 					_wpm->current_wp_id = 0;
 					_wpm->current_partner_sysid = msg->sysid;
@@ -1366,18 +1340,12 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 					if (_wpm->current_wp_id == 0) {
 						mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN");
 
-						if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid); }
-
 					} else {
-						mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
-
-						if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", _wpm->current_wp_id); }
+						mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy with WP");
 					}
 
 				} else {
 					mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy");
-
-					if (_verbose) { warnx("IGN MISSION_COUNT CMD: Busy"); }
 				}
 			}
 		}
@@ -1400,7 +1368,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 					if (wp.seq != 0) {
 						mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0");
-						warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.", wp.seq);
 						break;
 					}
 
@@ -1408,12 +1375,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 					if (wp.seq >= _wpm->current_count) {
 						mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP out of bounds");
-						warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.", wp.seq);
 						break;
 					}
 
 					if (wp.seq != _wpm->current_wp_id) {
-						warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.", wp.seq, _wpm->current_wp_id);
+						mavlink_missionlib_send_gcs_string("IGN: waypoint ID mismatch");
 						mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
 						break;
 					}
@@ -1528,11 +1494,10 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 void
 Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg)
 {
-	uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
+	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
 
-	uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg);
-
-	mavlink_send_uart_bytes(_channel, missionlib_msg_buf, len);
+	uint16_t len = mavlink_msg_to_send_buffer(buf, msg);
+	mavlink_send_uart_bytes(_channel, buf, len);
 }
 
 
@@ -1606,31 +1571,36 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
 				/* delete stream */
 				LL_DELETE(_streams, stream);
 				delete stream;
+				warnx("deleted stream %s", stream->get_name());
 			}
 
 			return OK;
 		}
 	}
 
-	if (interval > 0) {
-		/* search for stream with specified name in supported streams list */
-		for (unsigned int i = 0; streams_list[i] != nullptr; i++) {
-			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->set_interval(interval);
-				stream->subscribe(this);
-				LL_APPEND(_streams, stream);
-				return OK;
-			}
-		}
-
-	} else {
-		/* stream not found, nothing to disable */
+	if (interval == 0) {
+		/* stream was not active and is requested to be disabled, do nothing */
 		return OK;
 	}
 
+	/* search for stream with specified name in supported streams list */
+	for (unsigned int i = 0; streams_list[i] != nullptr; i++) {
+
+		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->set_interval(interval);
+			stream->subscribe(this);
+			LL_APPEND(_streams, stream);
+
+			return OK;
+		}
+	}
+
+	/* if we reach here, the stream list does not contain the stream */
+	warnx("stream %s not found", stream_name);
+
 	return ERROR;
 }
 
@@ -1964,9 +1934,12 @@ Mavlink::task_main(int argc, char *argv[])
 	_task_running = true;
 
 	MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update));
+	uint64_t param_time = 0;
 	MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status));
+	uint64_t status_time = 0;
 
-	struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->get_data();
+	struct vehicle_status_s status;
+	status_sub->update(&status_time, &status);
 
 	MavlinkCommandsStream commands_stream(this, _channel);
 
@@ -2023,14 +1996,14 @@ Mavlink::task_main(int argc, char *argv[])
 
 		hrt_abstime t = hrt_absolute_time();
 
-		if (param_sub->update(t)) {
+		if (param_sub->update(&param_time, nullptr)) {
 			/* parameters updated */
 			mavlink_update_system();
 		}
 
-		if (status_sub->update(t)) {
+		if (status_sub->update(&status_time, &status)) {
 			/* switch HIL mode if required */
-			set_hil_enabled(status->hil_state == HIL_STATE_ON);
+			set_hil_enabled(status.hil_state == HIL_STATE_ON);
 		}
 
 		/* update commands stream */
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index f0ca028a8..76ce42943 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -301,6 +301,12 @@ private:
 
 	perf_counter_t		_loop_perf;			/**< loop performance counter */
 
+	bool			_param_initialized;
+	param_t			_param_system_id;
+	param_t			_param_component_id;
+	param_t			_param_system_type;
+	param_t			_param_use_hil_gps;
+
 	/**
 	 * Send one parameter.
 	 *
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index fd41b723a..4433377c6 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -74,6 +74,8 @@
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_range_finder.h>
 
+#include <systemlib/err.h>
+
 #include "mavlink_messages.h"
 
 
@@ -193,50 +195,50 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 class MavlinkStreamHeartbeat : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamHeartbeat::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "HEARTBEAT";
 	}
 
-	MavlinkStream *new_instance()
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamHeartbeat();
 	}
 
 private:
 	MavlinkOrbSubscription *status_sub;
-	struct vehicle_status_s *status;
-
 	MavlinkOrbSubscription *pos_sp_triplet_sub;
-	struct position_setpoint_triplet_s *pos_sp_triplet;
 
 protected:
 	void subscribe(Mavlink *mavlink)
 	{
 		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
-		status = (struct vehicle_status_s *)status_sub->get_data();
-
 		pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet));
-		pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		(void)status_sub->update(t);
-		(void)pos_sp_triplet_sub->update(t);
-
-		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_msg_heartbeat_send(_channel,
-					   mavlink_system.type,
-					   MAV_AUTOPILOT_PX4,
-					   mavlink_base_mode,
-					   mavlink_custom_mode,
-					   mavlink_state);
-
+		struct vehicle_status_s status;
+		struct position_setpoint_triplet_s pos_sp_triplet;
+
+		if (status_sub->update(&status) && pos_sp_triplet_sub->update(&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_msg_heartbeat_send(_channel,
+						   mavlink_system.type,
+						   MAV_AUTOPILOT_PX4,
+						   mavlink_base_mode,
+						   mavlink_custom_mode,
+						   mavlink_state);
+		}
 	}
 };
 
@@ -244,44 +246,50 @@ protected:
 class MavlinkStreamSysStatus : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamSysStatus::get_name_static();
+	}
+
+	static const char *get_name_static ()
 	{
 		return "SYS_STATUS";
 	}
 
-	MavlinkStream *new_instance()
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamSysStatus();
 	}
 
 private:
 	MavlinkOrbSubscription *status_sub;
-	struct vehicle_status_s *status;
 
 protected:
 	void subscribe(Mavlink *mavlink)
 	{
 		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
-		status = (struct vehicle_status_s *)status_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		status_sub->update(t);
-		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);
+		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);
+		}
 	}
 };
 
@@ -289,23 +297,24 @@ protected:
 class MavlinkStreamHighresIMU : public MavlinkStream
 {
 public:
-	MavlinkStreamHighresIMU() : MavlinkStream(), accel_timestamp(0), gyro_timestamp(0), mag_timestamp(0), baro_timestamp(0)
+	const char *get_name() const
 	{
+		return MavlinkStreamHighresIMU::get_name_static();
 	}
 
-	const char *get_name()
+	static const char *get_name_static()
 	{
 		return "HIGHRES_IMU";
 	}
 
-	MavlinkStream *new_instance()
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamHighresIMU();
 	}
 
 private:
 	MavlinkOrbSubscription *sensor_sub;
-	struct sensor_combined_s *sensor;
+	uint64_t sensor_time;
 
 	uint64_t accel_timestamp;
 	uint64_t gyro_timestamp;
@@ -313,48 +322,57 @@ private:
 	uint64_t baro_timestamp;
 
 protected:
+	explicit MavlinkStreamHighresIMU() : MavlinkStream(),
+		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));
-		sensor = (struct sensor_combined_s *)sensor_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		if (sensor_sub->update(t)) {
+		struct sensor_combined_s 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,
+						     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);
 		}
 	}
@@ -364,34 +382,44 @@ protected:
 class MavlinkStreamAttitude : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamAttitude::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "ATTITUDE";
 	}
 
-	MavlinkStream *new_instance()
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamAttitude();
 	}
 
 private:
 	MavlinkOrbSubscription *att_sub;
-	struct vehicle_attitude_s *att;
+	uint64_t att_time;
 
 protected:
+	explicit MavlinkStreamAttitude() : MavlinkStream(),
+		att_time(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude));
-		att = (struct vehicle_attitude_s *)att_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		if (att_sub->update(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);
+						  att.timestamp / 1000,
+						  att.roll, att.pitch, att.yaw,
+						  att.rollspeed, att.pitchspeed, att.yawspeed);
 		}
 	}
 };
@@ -400,39 +428,49 @@ protected:
 class MavlinkStreamAttitudeQuaternion : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamAttitudeQuaternion::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "ATTITUDE_QUATERNION";
 	}
 
-	MavlinkStream *new_instance()
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamAttitudeQuaternion();
 	}
 
 private:
 	MavlinkOrbSubscription *att_sub;
-	struct vehicle_attitude_s *att;
+	uint64_t att_time;
 
 protected:
+	explicit MavlinkStreamAttitudeQuaternion() : MavlinkStream(),
+		att_time(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude));
-		att = (struct vehicle_attitude_s *)att_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		if (att_sub->update(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);
+							     att.timestamp / 1000,
+							     att.q[0],
+							     att.q[1],
+							     att.q[2],
+							     att.q[3],
+							     att.rollspeed,
+							     att.pitchspeed,
+							     att.yawspeed);
 		}
 	}
 };
@@ -441,71 +479,82 @@ protected:
 class MavlinkStreamVFRHUD : public MavlinkStream
 {
 public:
-	const char *get_name()
+
+	const char *get_name() const
+	{
+		return MavlinkStreamVFRHUD::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "VFR_HUD";
 	}
 
-	MavlinkStream *new_instance()
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamVFRHUD();
 	}
 
 private:
 	MavlinkOrbSubscription *att_sub;
-	struct vehicle_attitude_s *att;
+	uint64_t att_time;
 
 	MavlinkOrbSubscription *pos_sub;
-	struct vehicle_global_position_s *pos;
+	uint64_t pos_time;
 
 	MavlinkOrbSubscription *armed_sub;
-	struct actuator_armed_s *armed;
+	uint64_t armed_time;
 
 	MavlinkOrbSubscription *act_sub;
-	struct actuator_controls_s *act;
+	uint64_t act_time;
 
 	MavlinkOrbSubscription *airspeed_sub;
-	struct airspeed_s *airspeed;
+	uint64_t airspeed_time;
 
 protected:
+	explicit MavlinkStreamVFRHUD() : MavlinkStream(),
+		att_time(0),
+		pos_time(0),
+		armed_time(0),
+		act_time(0),
+		airspeed_time(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude));
-		att = (struct vehicle_attitude_s *)att_sub->get_data();
-
 		pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position));
-		pos = (struct vehicle_global_position_s *)pos_sub->get_data();
-
 		armed_sub = mavlink->add_orb_subscription(ORB_ID(actuator_armed));
-		armed = (struct actuator_armed_s *)armed_sub->get_data();
-
 		act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0));
-		act = (struct actuator_controls_s *)act_sub->get_data();
-
 		airspeed_sub = mavlink->add_orb_subscription(ORB_ID(airspeed));
-		airspeed = (struct airspeed_s *)airspeed_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		bool updated = att_sub->update(t);
-		updated |= pos_sub->update(t);
-		updated |= armed_sub->update(t);
-		updated |= act_sub->update(t);
-		updated |= airspeed_sub->update(t);
+		struct vehicle_attitude_s att;
+		struct vehicle_global_position_s pos;
+		struct actuator_armed_s armed;
+		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);
 
 		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;
+			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,
+						 airspeed.true_airspeed_m_s,
 						 groundspeed,
 						 heading,
 						 throttle,
-						 pos->alt,
-						 -pos->vel_d);
+						 pos.alt,
+						 -pos.vel_d);
 		}
 	}
 };
@@ -514,41 +563,51 @@ protected:
 class MavlinkStreamGPSRawInt : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamGPSRawInt::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "GPS_RAW_INT";
 	}
 
-	MavlinkStream *new_instance()
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamGPSRawInt();
 	}
 
 private:
 	MavlinkOrbSubscription *gps_sub;
-	struct vehicle_gps_position_s *gps;
+	uint64_t gps_time;
 
 protected:
+	explicit MavlinkStreamGPSRawInt() : MavlinkStream(),
+		gps_time(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position));
-		gps = (struct vehicle_gps_position_s *)gps_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		if (gps_sub->update(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_m),
-						     cm_uint16_from_m_float(gps->epv_m),
-						     gps->vel_m_s * 100.0f,
-						     _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f,
-						     gps->satellites_visible);
+						     gps.timestamp_position,
+						     gps.fix_type,
+						     gps.lat,
+						     gps.lon,
+						     gps.alt,
+						     cm_uint16_from_m_float(gps.eph_m),
+						     cm_uint16_from_m_float(gps.epv_m),
+						     gps.vel_m_s * 100.0f,
+						     _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
+						     gps.satellites_visible);
 		}
 	}
 };
@@ -557,49 +616,59 @@ protected:
 class MavlinkStreamGlobalPositionInt : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamGlobalPositionInt::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "GLOBAL_POSITION_INT";
 	}
 
-	MavlinkStream *new_instance()
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamGlobalPositionInt();
 	}
 
 private:
 	MavlinkOrbSubscription *pos_sub;
-	struct vehicle_global_position_s *pos;
+	uint64_t pos_time;
 
 	MavlinkOrbSubscription *home_sub;
-	struct home_position_s *home;
+	uint64_t home_time;
 
 protected:
+	explicit MavlinkStreamGlobalPositionInt() : MavlinkStream(),
+		pos_time(0),
+		home_time(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position));
-		pos = (struct vehicle_global_position_s *)pos_sub->get_data();
-
 		home_sub = mavlink->add_orb_subscription(ORB_ID(home_position));
-		home = (struct home_position_s *)home_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		bool updated = pos_sub->update(t);
-		updated |= home_sub->update(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);
 
 		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);
+							     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);
 		}
 	}
 };
@@ -608,38 +677,48 @@ protected:
 class MavlinkStreamLocalPositionNED : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamLocalPositionNED::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "LOCAL_POSITION_NED";
 	}
 
-	MavlinkStream *new_instance()
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamLocalPositionNED();
 	}
 
 private:
 	MavlinkOrbSubscription *pos_sub;
-	struct vehicle_local_position_s *pos;
+	uint64_t pos_time;
 
 protected:
+	explicit MavlinkStreamLocalPositionNED() : MavlinkStream(),
+		pos_time(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position));
-		pos = (struct vehicle_local_position_s *)pos_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		if (pos_sub->update(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);
+							    pos.timestamp / 1000,
+							    pos.x,
+							    pos.y,
+							    pos.z,
+							    pos.vx,
+							    pos.vy,
+							    pos.vz);
 		}
 	}
 };
@@ -649,38 +728,48 @@ protected:
 class MavlinkStreamViconPositionEstimate : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamViconPositionEstimate::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "VICON_POSITION_ESTIMATE";
 	}
 
-	MavlinkStream *new_instance()
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamViconPositionEstimate();
 	}
 
 private:
 	MavlinkOrbSubscription *pos_sub;
-	struct vehicle_vicon_position_s *pos;
+	uint64_t pos_time;
 
 protected:
+	explicit MavlinkStreamViconPositionEstimate() : MavlinkStream(),
+		pos_time(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position));
-		pos = (struct vehicle_vicon_position_s *)pos_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		if (pos_sub->update(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);
+								pos.timestamp / 1000,
+								pos.x,
+								pos.y,
+								pos.z,
+								pos.roll,
+								pos.pitch,
+								pos.yaw);
 		}
 	}
 };
@@ -689,70 +778,87 @@ protected:
 class MavlinkStreamGPSGlobalOrigin : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamGPSGlobalOrigin::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "GPS_GLOBAL_ORIGIN";
 	}
 
-	MavlinkStream *new_instance()
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamGPSGlobalOrigin();
 	}
 
 private:
 	MavlinkOrbSubscription *home_sub;
-	struct home_position_s *home;
 
 protected:
 	void subscribe(Mavlink *mavlink)
 	{
 		home_sub = mavlink->add_orb_subscription(ORB_ID(home_position));
-		home = (struct home_position_s *)home_sub->get_data();
 	}
 
 	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()) {
-			home_sub->update(t);
+			struct home_position_s 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_msg_gps_global_origin_send(_channel,
+								   (int32_t)(home.lat * 1e7),
+								   (int32_t)(home.lon * 1e7),
+								   (int32_t)(home.alt) * 1000.0f);
+			}
 		}
 	}
 };
 
-
+template <int N>
 class MavlinkStreamServoOutputRaw : public MavlinkStream
 {
 public:
-	MavlinkStreamServoOutputRaw(unsigned int n) : MavlinkStream(), _n(n)
+	const char *get_name() const
 	{
-		sprintf(_name, "SERVO_OUTPUT_RAW_%d", _n);
+		return MavlinkStreamServoOutputRaw<N>::get_name_static();
 	}
 
-	const char *get_name()
+	static const char *get_name_static()
 	{
-		return _name;
+		switch (N) {
+			case 0:
+			return "SERVO_OUTPUT_RAW_0";
+
+			case 1:
+			return "SERVO_OUTPUT_RAW_1";
+
+			case 2:
+			return "SERVO_OUTPUT_RAW_2";
+
+			case 3:
+			return "SERVO_OUTPUT_RAW_3";
+		}
 	}
 
-	MavlinkStream *new_instance()
+	static MavlinkStream *new_instance()
 	{
-		return new MavlinkStreamServoOutputRaw(_n);
+		return new MavlinkStreamServoOutputRaw<N>();
 	}
 
 private:
 	MavlinkOrbSubscription *act_sub;
-	struct actuator_outputs_s *act;
-
-	char _name[20];
-	unsigned int _n;
+	uint64_t act_time;
 
 protected:
+	explicit MavlinkStreamServoOutputRaw() : MavlinkStream(),
+		act_time(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		orb_id_t act_topics[] = {
@@ -762,24 +868,25 @@ protected:
 			ORB_ID(actuator_outputs_3)
 		};
 
-		act_sub = mavlink->add_orb_subscription(act_topics[_n]);
-		act = (struct actuator_outputs_s *)act_sub->get_data();
+		act_sub = mavlink->add_orb_subscription(act_topics[N]);
 	}
 
 	void send(const hrt_abstime t)
 	{
-		if (act_sub->update(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]);
+							  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]);
 		}
 	}
 };
@@ -788,51 +895,61 @@ protected:
 class MavlinkStreamHILControls : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamHILControls::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "HIL_CONTROLS";
 	}
 
-	MavlinkStream *new_instance()
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamHILControls();
 	}
 
 private:
 	MavlinkOrbSubscription *status_sub;
-	struct vehicle_status_s *status;
+	uint64_t status_time;
 
 	MavlinkOrbSubscription *pos_sp_triplet_sub;
-	struct position_setpoint_triplet_s *pos_sp_triplet;
+	uint64_t pos_sp_triplet_time;
 
 	MavlinkOrbSubscription *act_sub;
-	struct actuator_outputs_s *act;
+	uint64_t act_time;
 
 protected:
+	explicit MavlinkStreamHILControls() : MavlinkStream(),
+		status_time(0),
+		pos_sp_triplet_time(0),
+		act_time(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
-		status = (struct vehicle_status_s *)status_sub->get_data();
-
 		pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet));
-		pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data();
-
 		act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0));
-		act = (struct actuator_outputs_s *)act_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		bool updated = act_sub->update(t);
-		(void)pos_sp_triplet_sub->update(t);
-		(void)status_sub->update(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);
 
-		if (updated && (status->arming_state == ARMING_STATE_ARMED)) {
+		if (updated && (status.arming_state == ARMING_STATE_ARMED)) {
 			/* translate the current syste state to mavlink state and mode */
 			uint8_t mavlink_state;
 			uint8_t mavlink_base_mode;
 			uint32_t mavlink_custom_mode;
-			get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
+			get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
 
 			if (mavlink_system.type == MAV_TYPE_QUADROTOR ||
 				mavlink_system.type == MAV_TYPE_HEXAROTOR ||
@@ -861,7 +978,7 @@ protected:
 					if (i < n) {
 						if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
 							/* scale fake PWM out 900..2100 us to 0..1 for normal multirotors */
-							out[i] = (act->output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
+							out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
 
 						} else {
 							/* send 0 when disarmed */
@@ -891,12 +1008,12 @@ protected:
 				for (unsigned i = 0; i < 8; i++) {
 					if (i != 3) {
 						/* scale fake PWM out 900..2100 us to -1..+1 for normal channels */
-						out[i] = (act->output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
+						out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
 
 					} else {
 
 						/* scale fake PWM out 900..2100 us to 0..1 for throttle */
-						out[i] = (act->output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
+						out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
 					}
 
 				}
@@ -915,36 +1032,41 @@ protected:
 class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamGlobalPositionSetpointInt::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "GLOBAL_POSITION_SETPOINT_INT";
 	}
 
-	MavlinkStream *new_instance()
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamGlobalPositionSetpointInt();
 	}
 
 private:
 	MavlinkOrbSubscription *pos_sp_triplet_sub;
-	struct position_setpoint_triplet_s *pos_sp_triplet;
 
 protected:
 	void subscribe(Mavlink *mavlink)
 	{
 		pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet));
-		pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		if (pos_sp_triplet_sub->update(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));
+					(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));
 		}
 	}
 };
@@ -953,36 +1075,46 @@ protected:
 class MavlinkStreamLocalPositionSetpoint : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamLocalPositionSetpoint::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "LOCAL_POSITION_SETPOINT";
 	}
 
-	MavlinkStream *new_instance()
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamLocalPositionSetpoint();
 	}
 
 private:
 	MavlinkOrbSubscription *pos_sp_sub;
-	struct vehicle_local_position_setpoint_s *pos_sp;
+	uint64_t pos_sp_time;
 
 protected:
+	explicit MavlinkStreamLocalPositionSetpoint() : MavlinkStream(),
+		pos_sp_time(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint));
-		pos_sp = (struct vehicle_local_position_setpoint_s *)pos_sp_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		if (pos_sp_sub->update(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);
+					pos_sp.x,
+					pos_sp.y,
+					pos_sp.z,
+					pos_sp.yaw);
 		}
 	}
 };
@@ -991,36 +1123,46 @@ protected:
 class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamRollPitchYawThrustSetpoint::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "ROLL_PITCH_YAW_THRUST_SETPOINT";
 	}
 
-	MavlinkStream *new_instance()
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamRollPitchYawThrustSetpoint();
 	}
 
 private:
 	MavlinkOrbSubscription *att_sp_sub;
-	struct vehicle_attitude_setpoint_s *att_sp;
+	uint64_t att_sp_time;
 
 protected:
+	explicit MavlinkStreamRollPitchYawThrustSetpoint() : MavlinkStream(),
+		att_sp_time(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint));
-		att_sp = (struct vehicle_attitude_setpoint_s *)att_sp_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		if (att_sp_sub->update(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);
+					att_sp.timestamp / 1000,
+					att_sp.roll_body,
+					att_sp.pitch_body,
+					att_sp.yaw_body,
+					att_sp.thrust);
 		}
 	}
 };
@@ -1029,36 +1171,46 @@ protected:
 class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT";
 	}
 
-	MavlinkStream *new_instance()
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamRollPitchYawRatesThrustSetpoint();
 	}
 
 private:
 	MavlinkOrbSubscription *att_rates_sp_sub;
-	struct vehicle_rates_setpoint_s *att_rates_sp;
+	uint64_t att_rates_sp_time;
 
 protected:
+	explicit MavlinkStreamRollPitchYawRatesThrustSetpoint() : MavlinkStream(),
+		att_rates_sp_time(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint));
-		att_rates_sp = (struct vehicle_rates_setpoint_s *)att_rates_sp_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		if (att_rates_sp_sub->update(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);
+					att_rates_sp.timestamp / 1000,
+					att_rates_sp.roll,
+					att_rates_sp.pitch,
+					att_rates_sp.yaw,
+					att_rates_sp.thrust);
 		}
 	}
 };
@@ -1067,47 +1219,82 @@ protected:
 class MavlinkStreamRCChannelsRaw : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamRCChannelsRaw::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "RC_CHANNELS_RAW";
 	}
 
-	MavlinkStream *new_instance()
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamRCChannelsRaw();
 	}
 
 private:
 	MavlinkOrbSubscription *rc_sub;
-	struct rc_input_values *rc;
+	uint64_t rc_time;
 
 protected:
+	explicit MavlinkStreamRCChannelsRaw() : MavlinkStream(),
+		rc_time(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc));
-		rc = (struct rc_input_values *)rc_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		if (rc_sub->update(t)) {
+		struct rc_input_values rc;
+
+		if (rc_sub->update(&rc_time, &rc)) {
 			const unsigned port_width = 8;
 
-			for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) {
+			// 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,
+								 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);
+								 (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);
 			}
+
+			// 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);
 		}
 	}
 };
@@ -1116,36 +1303,46 @@ protected:
 class MavlinkStreamManualControl : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamManualControl::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "MANUAL_CONTROL";
 	}
 
-	MavlinkStream *new_instance()
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamManualControl();
 	}
 
 private:
 	MavlinkOrbSubscription *manual_sub;
-	struct manual_control_setpoint_s *manual;
+	uint64_t manual_time;
 
 protected:
+	explicit MavlinkStreamManualControl() : MavlinkStream(),
+		manual_time(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint));
-		manual = (struct manual_control_setpoint_s *)manual_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		if (manual_sub->update(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,
+							manual.x * 1000,
+							manual.y * 1000,
+							manual.z * 1000,
+							manual.r * 1000,
 							0);
 		}
 	}
@@ -1155,37 +1352,47 @@ protected:
 class MavlinkStreamOpticalFlow : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamOpticalFlow::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "OPTICAL_FLOW";
 	}
 
-	MavlinkStream *new_instance()
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamOpticalFlow();
 	}
 
 private:
 	MavlinkOrbSubscription *flow_sub;
-	struct optical_flow_s *flow;
+	uint64_t flow_time;
 
 protected:
+	explicit MavlinkStreamOpticalFlow() : MavlinkStream(),
+		flow_time(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow));
-		flow = (struct optical_flow_s *)flow_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		if (flow_sub->update(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);
+						      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);
 		}
 	}
 };
@@ -1193,47 +1400,57 @@ protected:
 class MavlinkStreamAttitudeControls : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamAttitudeControls::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "ATTITUDE_CONTROLS";
 	}
 
-	MavlinkStream *new_instance()
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamAttitudeControls();
 	}
 
 private:
 	MavlinkOrbSubscription *att_ctrl_sub;
-	struct actuator_controls_s *att_ctrl;
+	uint64_t att_ctrl_time;
 
 protected:
+	explicit MavlinkStreamAttitudeControls() : MavlinkStream(),
+		att_ctrl_time(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
-		att_ctrl = (struct actuator_controls_s *)att_ctrl_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		if (att_ctrl_sub->update(t)) {
+		struct actuator_controls_s 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,
+							   att_ctrl.timestamp / 1000,
 							   "rll ctrl    ",
-							   att_ctrl->control[0]);
+							   att_ctrl.control[0]);
 			mavlink_msg_named_value_float_send(_channel,
-							   att_ctrl->timestamp / 1000,
+							   att_ctrl.timestamp / 1000,
 							   "ptch ctrl    ",
-							   att_ctrl->control[1]);
+							   att_ctrl.control[1]);
 			mavlink_msg_named_value_float_send(_channel,
-							   att_ctrl->timestamp / 1000,
+							   att_ctrl.timestamp / 1000,
 							   "yaw ctrl     ",
-							   att_ctrl->control[2]);
+							   att_ctrl.control[2]);
 			mavlink_msg_named_value_float_send(_channel,
-							   att_ctrl->timestamp / 1000,
+							   att_ctrl.timestamp / 1000,
 							   "thr ctrl     ",
-							   att_ctrl->control[3]);
+							   att_ctrl.control[3]);
 		}
 	}
 };
@@ -1241,37 +1458,47 @@ protected:
 class MavlinkStreamNamedValueFloat : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamNamedValueFloat::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "NAMED_VALUE_FLOAT";
 	}
 
-	MavlinkStream *new_instance()
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamNamedValueFloat();
 	}
 
 private:
 	MavlinkOrbSubscription *debug_sub;
-	struct debug_key_value_s *debug;
+	uint64_t debug_time;
 
 protected:
+	explicit MavlinkStreamNamedValueFloat() : MavlinkStream(),
+		debug_time(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value));
-		debug = (struct debug_key_value_s *)debug_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		if (debug_sub->update(t)) {
+		struct debug_key_value_s debug;
+
+		if (debug_sub->update(&debug_time, &debug)) {
 			/* enforce null termination */
-			debug->key[sizeof(debug->key) - 1] = '\0';
+			debug.key[sizeof(debug.key) - 1] = '\0';
 
 			mavlink_msg_named_value_float_send(_channel,
-							   debug->timestamp_ms,
-							   debug->key,
-							   debug->value);
+							   debug.timestamp_ms,
+							   debug.key,
+							   debug.value);
 		}
 	}
 };
@@ -1279,33 +1506,37 @@ protected:
 class MavlinkStreamCameraCapture : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamCameraCapture::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "CAMERA_CAPTURE";
 	}
 
-	MavlinkStream *new_instance()
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamCameraCapture();
 	}
 
 private:
 	MavlinkOrbSubscription *status_sub;
-	struct vehicle_status_s *status;
 
 protected:
 	void subscribe(Mavlink *mavlink)
 	{
 		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
-		status = (struct vehicle_status_s *)status_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		(void)status_sub->update(t);
+		struct vehicle_status_s status;
+		(void)status_sub->update(&status);
 
-		if (status->arming_state == ARMING_STATE_ARMED
-		    || status->arming_state == ARMING_STATE_ARMED_ERROR) {
+		if (status.arming_state == ARMING_STATE_ARMED
+		    || status.arming_state == ARMING_STATE_ARMED_ERROR) {
 
 			/* 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);
@@ -1320,34 +1551,44 @@ protected:
 class MavlinkStreamDistanceSensor : public MavlinkStream
 {
 public:
-	const char *get_name()
+	const char *get_name() const
+	{
+		return MavlinkStreamDistanceSensor::get_name_static();
+	}
+
+	static const char *get_name_static()
 	{
 		return "DISTANCE_SENSOR";
 	}
 
-	MavlinkStream *new_instance()
+	static MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamDistanceSensor();
 	}
 
 private:
 	MavlinkOrbSubscription *range_sub;
-	struct range_finder_report *range;
+	uint64_t range_time;
 
 protected:
+	explicit MavlinkStreamDistanceSensor() : MavlinkStream(),
+		range_time(0)
+	{}
+
 	void subscribe(Mavlink *mavlink)
 	{
 		range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder));
-		range = (struct range_finder_report *)range_sub->get_data();
 	}
 
 	void send(const hrt_abstime t)
 	{
-		if (range_sub->update(t)) {
+		struct range_finder_report range;
+
+		if (range_sub->update(&range_time, &range)) {
 
 			uint8_t type;
 
-			switch (range->type) {
+			switch (range.type) {
 				case RANGE_FINDER_TYPE_LASER:
 				type = MAV_DISTANCE_SENSOR_LASER;
 				break;
@@ -1357,39 +1598,40 @@ protected:
 			uint8_t orientation = 0;
 			uint8_t covariance = 20;
 
-			mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation,
-				range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance);
+			mavlink_msg_distance_sensor_send(_channel, range.timestamp / 1000, type, id, orientation,
+				range.minimum_distance*100, range.maximum_distance*100, range.distance*100, covariance);
 		}
 	}
 };
 
-MavlinkStream *streams_list[] = {
-	new MavlinkStreamHeartbeat(),
-	new MavlinkStreamSysStatus(),
-	new MavlinkStreamHighresIMU(),
-	new MavlinkStreamAttitude(),
-	new MavlinkStreamAttitudeQuaternion(),
-	new MavlinkStreamVFRHUD(),
-	new MavlinkStreamGPSRawInt(),
-	new MavlinkStreamGlobalPositionInt(),
-	new MavlinkStreamLocalPositionNED(),
-	new MavlinkStreamGPSGlobalOrigin(),
-	new MavlinkStreamServoOutputRaw(0),
-	new MavlinkStreamServoOutputRaw(1),
-	new MavlinkStreamServoOutputRaw(2),
-	new MavlinkStreamServoOutputRaw(3),
-	new MavlinkStreamHILControls(),
-	new MavlinkStreamGlobalPositionSetpointInt(),
-	new MavlinkStreamLocalPositionSetpoint(),
-	new MavlinkStreamRollPitchYawThrustSetpoint(),
-	new MavlinkStreamRollPitchYawRatesThrustSetpoint(),
-	new MavlinkStreamRCChannelsRaw(),
-	new MavlinkStreamManualControl(),
-	new MavlinkStreamOpticalFlow(),
-	new MavlinkStreamAttitudeControls(),
-	new MavlinkStreamNamedValueFloat(),
-	new MavlinkStreamCameraCapture(),
-	new MavlinkStreamDistanceSensor(),
-	new MavlinkStreamViconPositionEstimate(),
+
+StreamListItem *streams_list[] = {
+	new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::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),
+	new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static),
+	new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static),
+	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(&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(&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(&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),
+	new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static),
+	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 b8823263a..ee64d0e42 100644
--- a/src/modules/mavlink/mavlink_messages.h
+++ b/src/modules/mavlink/mavlink_messages.h
@@ -43,6 +43,19 @@
 
 #include "mavlink_stream.h"
 
-extern MavlinkStream *streams_list[];
+class StreamListItem {
+
+public:
+	MavlinkStream* (*new_instance)();
+	const char* (*get_name)();
+
+	StreamListItem(MavlinkStream* (*inst)(), const char* (*name)()) :
+		new_instance(inst),
+		get_name(name) {};
+
+	~StreamListItem() {};
+};
+
+extern StreamListItem *streams_list[];
 
 #endif /* MAVLINK_MESSAGES_H_ */
diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp
index 21d5219d3..901fa8f05 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.cpp
+++ b/src/modules/mavlink/mavlink_orb_subscription.cpp
@@ -50,50 +50,55 @@ MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) :
 	_fd(orb_subscribe(_topic)),
 	_published(false),
 	_topic(topic),
-	_last_check(0),
 	next(nullptr)
 {
-	_data = malloc(topic->o_size);
-	memset(_data, 0, topic->o_size);
 }
 
 MavlinkOrbSubscription::~MavlinkOrbSubscription()
 {
 	close(_fd);
-	free(_data);
 }
 
 orb_id_t
-MavlinkOrbSubscription::get_topic()
+MavlinkOrbSubscription::get_topic() const
 {
 	return _topic;
 }
 
-void *
-MavlinkOrbSubscription::get_data()
-{
-	return _data;
-}
-
 bool
-MavlinkOrbSubscription::update(const hrt_abstime t)
+MavlinkOrbSubscription::update(uint64_t *time, void* data)
 {
-	if (_last_check == t) {
-		/* already checked right now, return result of the check */
-		return _updated;
+	// TODO this is NOT atomic operation, we can get data newer than time
+	// if topic was published between orb_stat and orb_copy calls.
 
-	} else {
-		_last_check = t;
-		orb_check(_fd, &_updated);
+	uint64_t time_topic;
+	if (orb_stat(_fd, &time_topic)) {
+		/* error getting last topic publication time */
+		time_topic = 0;
+	}
+
+	if (orb_copy(_topic, _fd, data)) {
+		/* error copying topic data */
+		memset(data, 0, _topic->o_size);
+		return false;
 
-		if (_updated) {
-			orb_copy(_topic, _fd, _data);
-			_published = true;
+	} else {
+		/* data copied successfully */
+		_published = true;
+		if (time_topic != *time) {
+			*time = time_topic;
 			return true;
+
+		} else {
+			return false;
 		}
 	}
+}
 
-	return false;
+bool
+MavlinkOrbSubscription::update(void* data)
+{
+	return !orb_copy(_topic, _fd, data);
 }
 
 bool
diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h
index 8c09772c8..71efb43af 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.h
+++ b/src/modules/mavlink/mavlink_orb_subscription.h
@@ -48,12 +48,26 @@
 class MavlinkOrbSubscription
 {
 public:
-	MavlinkOrbSubscription *next;	/*< pointer to next subscription in list */
+	MavlinkOrbSubscription *next;	///< pointer to next subscription in list
 
 	MavlinkOrbSubscription(const orb_id_t topic);
 	~MavlinkOrbSubscription();
 
-	bool update(const hrt_abstime t);
+	/**
+	 * Check if subscription updated and get data.
+	 *
+	 * @return true only if topic was updated and data copied to buffer successfully.
+	 * If topic was not updated since last check it will return false but still copy the data.
+	 * If no data available data buffer will be filled with zeroes.
+	 */
+	bool update(uint64_t *time, void* data);
+
+	/**
+	 * Copy topic data to given buffer.
+	 *
+	 * @return true only if topic data copied successfully.
+	 */
+	bool update(void* data);
 
 	/**
 	 * Check if the topic has been published.
@@ -62,16 +76,12 @@ public:
 	 * @return true if the topic has been published at least once.
 	 */
 	bool is_published();
-	void *get_data();
-	orb_id_t get_topic();
+	orb_id_t get_topic() const;
 
 private:
-	const orb_id_t _topic;		/*< topic metadata */
-	int _fd;					/*< subscription handle */
-	bool _published;			/*< topic was ever published */
-	void *_data;				/*< pointer to data buffer */
-	hrt_abstime _last_check;	/*< time of last check */
-	bool _updated;				/*< updated on last check */
+	const orb_id_t _topic;		///< topic metadata
+	int _fd;			///< subscription handle
+	bool _published;		///< topic was ever published
 };
 
 
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 4a244815a..251d862e3 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -959,7 +959,6 @@ MavlinkReceiver::receive_start(Mavlink *parent)
 	(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
 
 	pthread_attr_setstacksize(&receiveloop_attr, 2900);
-
 	pthread_t thread;
 	pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent);
 
diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h
index 2979d20de..a41ace48e 100644
--- a/src/modules/mavlink/mavlink_stream.h
+++ b/src/modules/mavlink/mavlink_stream.h
@@ -50,14 +50,6 @@ class MavlinkStream;
 
 class MavlinkStream
 {
-private:
-	hrt_abstime _last_sent;
-
-protected:
-	mavlink_channel_t _channel;
-	unsigned int _interval;
-
-	virtual void send(const hrt_abstime t) = 0;
 
 public:
 	MavlinkStream *next;
@@ -71,9 +63,19 @@ public:
 	 * @return 0 if updated / sent, -1 if unchanged
 	 */
 	int update(const hrt_abstime t);
-	virtual MavlinkStream *new_instance() = 0;
+	static MavlinkStream *new_instance();
+	static const char *get_name_static();
 	virtual void subscribe(Mavlink *mavlink) = 0;
-	virtual const char *get_name() = 0;
+	virtual const char *get_name() const = 0;
+
+protected:
+	mavlink_channel_t _channel;
+	unsigned int _interval;
+
+	virtual void send(const hrt_abstime t) = 0;
+
+private:
+	hrt_abstime _last_sent;
 };
 
 
diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c
index 22182e39e..d6d8284d2 100644
--- a/src/modules/systemlib/perf_counter.c
+++ b/src/modules/systemlib/perf_counter.c
@@ -301,7 +301,7 @@ perf_print_counter_fd(int fd, perf_counter_t handle)
 	case PC_ELAPSED: {
 		struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
 
-		dprintf(fd, "%s: %llu events, %lluus elapsed, %llu avg, min %lluus max %lluus\n",
+		dprintf(fd, "%s: %llu events, %lluus elapsed, %lluus avg, min %lluus max %lluus\n",
 		       handle->name,
 		       pce->event_count,
 		       pce->time_total,
@@ -314,7 +314,7 @@ perf_print_counter_fd(int fd, perf_counter_t handle)
 	case PC_INTERVAL: {
 		struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
 
-		dprintf(fd, "%s: %llu events, %llu avg, min %lluus max %lluus\n",
+		dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus\n",
 		       handle->name,
 		       pci->event_count,
 		       (pci->time_last - pci->time_first) / pci->event_count,
diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
index d526a8ff2..8446e9c6e 100644
--- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
@@ -72,6 +72,8 @@ struct vehicle_attitude_setpoint_s {
 	float thrust;					/**< Thrust in Newton the power system should generate */
 
 	bool	roll_reset_integral;			/**< Reset roll integral part (navigation logic change) */
+	bool	pitch_reset_integral;			/**< Reset pitch integral part (navigation logic change) */
+	bool	yaw_reset_integral;			/**< Reset yaw integral part (navigation logic change) */
 
 };