mavlink app: Clean up allocations

9 files changed, 1080 insertions, 1074 deletions

diff --git a/src/modules/mavlink/mavlink_commands.cpp b/src/modules/mavlink/mavlink_commands.cpp
index 1c1e097a4..5760d7512 100644
--- a/src/modules/mavlink/mavlink_commands.cpp
+++ b/src/modules/mavlink/mavlink_commands.cpp
@@ -43,7 +43,6 @@
 MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel)
 {
 	_cmd_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command));
-	_cmd = (struct vehicle_command_s *)_cmd_sub->get_data();
 }
 
 MavlinkCommandsStream::~MavlinkCommandsStream()
@@ -53,21 +52,23 @@ MavlinkCommandsStream::~MavlinkCommandsStream()
 void
 MavlinkCommandsStream::update(const hrt_abstime t)
 {
-	if (_cmd_sub->update(t)) {
+	struct vehicle_command_s cmd;
+
+	if (_cmd_sub->update(t, &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_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 6c97bfca7..340b20e1b 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -234,6 +234,11 @@ 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"))
@@ -493,44 +498,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;
 }
@@ -791,7 +791,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
 	case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
 			/* Start sending parameters */
 			mavlink_pm_start_queued_send();
-			mavlink_missionlib_send_gcs_string("[mavlink pm] sending list");
+			mavlink_missionlib_send_gcs_string("[pm] sending list");
 		} break;
 
 	case MAVLINK_MSG_ID_PARAM_SET: {
@@ -813,7 +813,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 {
@@ -1001,8 +1001,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"); }
 	}
 }
 
@@ -1073,8 +1071,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"); }
 	}
 }
 
@@ -1105,8 +1101,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;
@@ -1137,8 +1131,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;
@@ -1162,21 +1154,14 @@ 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"); }
-
 				}
 
 			} else {
 				mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
-
-				if (_verbose) { warnx("REJ. WP CMD: target id mismatch"); }
 			}
 
 			break;
@@ -1206,14 +1191,10 @@ 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"); }
 				}
 
 			} else {
 				mavlink_missionlib_send_gcs_string("REJ. REQUEST LIST: target id mismatch");
-
-				if (_verbose) { warnx("REJ. REQUEST LIST: target id mismatch"); }
 			}
 
 			break;
@@ -1230,8 +1211,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;
 				}
 
@@ -1242,15 +1221,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;
 					}
 
@@ -1258,17 +1235,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;
 					}
 
@@ -1276,8 +1251,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;
 				}
 
@@ -1291,7 +1264,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");
 				}
 
 
@@ -1301,13 +1274,9 @@ 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 from ID %u because i'm already talking to ID %u.", msg->sysid, _wpm->current_partner_sysid); }
-
 				} else {
 
 					mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
-
-					if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); }
 				}
 			}
 
@@ -1331,15 +1300,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;
@@ -1353,25 +1318,17 @@ 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"); }
 				}
 
 			} else {
 
 				mavlink_missionlib_send_gcs_string("REJ. WP COUNT CMD: target id mismatch");
-
-				if (_verbose) { warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); }
 			}
 		}
 		break;
@@ -1393,7 +1350,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;
 					}
 
@@ -1401,12 +1357,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;
 					}
@@ -1473,8 +1428,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 			} else {
 				mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
-
-				if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); }
 			}
 
 			break;
@@ -1515,8 +1468,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 			} else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
 
 				mavlink_missionlib_send_gcs_string("REJ. WP CLERR CMD: target id mismatch");
-
-				if (_verbose) { warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); }
 			}
 
 			break;
@@ -1535,8 +1486,7 @@ Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg)
 	uint8_t missionlib_msg_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);
+	mavlink_send_uart_bytes(_channel, buf, len);
 }
 
 
@@ -1619,6 +1569,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
 	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();
@@ -1924,7 +1875,7 @@ Mavlink::task_main(int argc, char *argv[])
 	/* if we are passing on mavlink messages, we need to prepare a buffer for this instance */
 	if (_passing_on) {
 		/* initialize message buffer if multiplexing is on */
-		if (OK != message_buffer_init(500)) {
+		if (OK != message_buffer_init(300)) {
 			errx(1, "can't allocate message buffer, exiting");
 		}
 
@@ -1956,7 +1907,8 @@ Mavlink::task_main(int argc, char *argv[])
 	MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update));
 	MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status));
 
-	struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->get_data();
+	struct vehicle_status_s status;
+	status_sub->update(0, &status);
 
 	MavlinkCommandsStream commands_stream(this, _channel);
 
@@ -2013,14 +1965,14 @@ Mavlink::task_main(int argc, char *argv[])
 
 		hrt_abstime t = hrt_absolute_time();
 
-		if (param_sub->update(t)) {
+		if (param_sub->update(t, nullptr)) {
 			/* parameters updated */
 			mavlink_update_system();
 		}
 
-		if (status_sub->update(t)) {
+		if (status_sub->update(t, &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 c7a7d32f8..1f0445cb6 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -278,11 +278,15 @@ private:
 		int size;
 		char *data;
 	};
-	mavlink_message_buffer _message_buffer;
-
-	pthread_mutex_t _message_buffer_mutex;
+	mavlink_message_buffer	_message_buffer;
 
+	pthread_mutex_t		_message_buffer_mutex;
 
+	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 79dd88657..4bb827116 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"
 
 
@@ -189,42 +191,51 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 class MavlinkStreamHeartbeat : public MavlinkStream
 {
 public:
-	const char *get_name()
+
+	~MavlinkStreamHeartbeat() {};
+
+	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:
+
+	explicit MavlinkStreamHeartbeat() {};
+
 	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);
+		struct vehicle_status_s status;
+		struct position_setpoint_triplet_s pos_sp_triplet;
+
+		(void)status_sub->update(t, &status);
+		(void)pos_sp_triplet_sub->update(t, &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);
+		get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
 
 		mavlink_msg_heartbeat_send(_channel,
 					   mavlink_system.type,
@@ -240,12 +251,19 @@ protected:
 class MavlinkStreamSysStatus : public MavlinkStream
 {
 public:
-	const char *get_name()
+	~MavlinkStreamSysStatus() {};
+
+	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();
 	}
@@ -255,29 +273,31 @@ private:
 	struct vehicle_status_s *status;
 
 protected:
+	explicit MavlinkStreamSysStatus() {};
+
 	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);
+		struct vehicle_status_s status;
+		(void)status_sub->update(t, &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 * 1000.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);
+						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 * 1000.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);
 	}
 };
 
@@ -285,23 +305,25 @@ protected:
 class MavlinkStreamHighresIMU : public MavlinkStream
 {
 public:
-	MavlinkStreamHighresIMU() : MavlinkStream(), accel_timestamp(0), gyro_timestamp(0), mag_timestamp(0), baro_timestamp(0)
+	~MavlinkStreamHighresIMU();
+
+	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 accel_timestamp;
 	uint64_t gyro_timestamp;
@@ -309,48 +331,52 @@ private:
 	uint64_t baro_timestamp;
 
 protected:
+	explicit MavlinkStreamHighresIMU() : MavlinkStream(), 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(t, &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);
 		}
 	}
@@ -360,12 +386,17 @@ 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();
 	}
@@ -378,16 +409,17 @@ protected:
 	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(t, &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);
 		}
 	}
 };
@@ -396,39 +428,44 @@ 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;
 
 protected:
 	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(t, &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);
 		}
 	}
 };
@@ -437,12 +474,18 @@ 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();
 	}
@@ -467,41 +510,38 @@ protected:
 	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(t, &att);
+		updated |= pos_sub->update(t, &pos);
+		updated |= armed_sub->update(t, &armed);
+		updated |= act_sub->update(t, &act);
+		updated |= airspeed_sub->update(t, &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);
 		}
 	}
 };
@@ -510,12 +550,17 @@ 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();
 	}
@@ -528,864 +573,865 @@ protected:
 	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)) {
-			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);
-		}
-	}
-};
-
-
-class MavlinkStreamGlobalPositionInt : public MavlinkStream
-{
-public:
-	const char *get_name()
-	{
-		return "GLOBAL_POSITION_INT";
-	}
-
-	MavlinkStream *new_instance()
-	{
-		return new MavlinkStreamGlobalPositionInt();
-	}
-
-private:
-	MavlinkOrbSubscription *pos_sub;
-	struct vehicle_global_position_s *pos;
-
-	MavlinkOrbSubscription *home_sub;
-	struct home_position_s *home;
-
-protected:
-	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);
-
-		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);
-		}
-	}
-};
-
-
-class MavlinkStreamLocalPositionNED : public MavlinkStream
-{
-public:
-	const char *get_name()
-	{
-		return "LOCAL_POSITION_NED";
-	}
-
-	MavlinkStream *new_instance()
-	{
-		return new MavlinkStreamLocalPositionNED();
-	}
-
-private:
-	MavlinkOrbSubscription *pos_sub;
-	struct vehicle_local_position_s *pos;
-
-protected:
-	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)) {
-			mavlink_msg_local_position_ned_send(_channel,
-							    pos->timestamp / 1000,
-							    pos->x,
-							    pos->y,
-							    pos->z,
-							    pos->vx,
-							    pos->vy,
-							    pos->vz);
-		}
-	}
-};
-
-
-
-class MavlinkStreamViconPositionEstimate : public MavlinkStream
-{
-public:
-	const char *get_name()
-	{
-		return "VICON_POSITION_ESTIMATE";
-	}
-
-	MavlinkStream *new_instance()
-	{
-		return new MavlinkStreamViconPositionEstimate();
-	}
-
-private:
-	MavlinkOrbSubscription *pos_sub;
-	struct vehicle_vicon_position_s *pos;
-
-protected:
-	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)) {
-			mavlink_msg_vicon_position_estimate_send(_channel,
-								pos->timestamp / 1000,
-								pos->x,
-								pos->y,
-								pos->z,
-								pos->roll,
-								pos->pitch,
-								pos->yaw);
-		}
-	}
-};
-
-
-class MavlinkStreamGPSGlobalOrigin : public MavlinkStream
-{
-public:
-	const char *get_name()
-	{
-		return "GPS_GLOBAL_ORIGIN";
-	}
-
-	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);
-
-			mavlink_msg_gps_global_origin_send(_channel,
-							   (int32_t)(home->lat * 1e7),
-							   (int32_t)(home->lon * 1e7),
-							   (int32_t)(home->alt) * 1000.0f);
-		}
-	}
-};
-
-
-class MavlinkStreamServoOutputRaw : public MavlinkStream
-{
-public:
-	MavlinkStreamServoOutputRaw(unsigned int n) : MavlinkStream(), _n(n)
-	{
-		sprintf(_name, "SERVO_OUTPUT_RAW_%d", _n);
-	}
-
-	const char *get_name()
-	{
-		return _name;
-	}
-
-	MavlinkStream *new_instance()
-	{
-		return new MavlinkStreamServoOutputRaw(_n);
-	}
-
-private:
-	MavlinkOrbSubscription *act_sub;
-	struct actuator_outputs_s *act;
-
-	char _name[20];
-	unsigned int _n;
-
-protected:
-	void subscribe(Mavlink *mavlink)
-	{
-		orb_id_t act_topics[] = {
-			ORB_ID(actuator_outputs_0),
-			ORB_ID(actuator_outputs_1),
-			ORB_ID(actuator_outputs_2),
-			ORB_ID(actuator_outputs_3)
-		};
-
-		act_sub = mavlink->add_orb_subscription(act_topics[_n]);
-		act = (struct actuator_outputs_s *)act_sub->get_data();
-	}
-
-	void send(const hrt_abstime t)
-	{
-		if (act_sub->update(t)) {
-			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]);
-		}
-	}
-};
-
-
-class MavlinkStreamHILControls : public MavlinkStream
-{
-public:
-	const char *get_name()
-	{
-		return "HIL_CONTROLS";
-	}
-
-	MavlinkStream *new_instance()
-	{
-		return new MavlinkStreamHILControls();
-	}
-
-private:
-	MavlinkOrbSubscription *status_sub;
-	struct vehicle_status_s *status;
-
-	MavlinkOrbSubscription *pos_sp_triplet_sub;
-	struct position_setpoint_triplet_s *pos_sp_triplet;
-
-	MavlinkOrbSubscription *act_sub;
-	struct actuator_outputs_s *act;
-
-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();
-
-		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);
-
-		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);
-
-			if (mavlink_system.type == MAV_TYPE_QUADROTOR ||
-				mavlink_system.type == MAV_TYPE_HEXAROTOR ||
-				mavlink_system.type == MAV_TYPE_OCTOROTOR) {
-				/* set number of valid outputs depending on vehicle type */
-				unsigned n;
-
-				switch (mavlink_system.type) {
-				case MAV_TYPE_QUADROTOR:
-					n = 4;
-					break;
-
-				case MAV_TYPE_HEXAROTOR:
-					n = 6;
-					break;
-
-				default:
-					n = 8;
-					break;
-				}
-
-				/* scale / assign outputs depending on system type */
-				float out[8];
-
-				for (unsigned i = 0; i < 8; i++) {
-					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);
-
-						} else {
-							/* send 0 when disarmed */
-							out[i] = 0.0f;
-						}
-
-					} else {
-						out[i] = -1.0f;
-					}
-				}
-
-				mavlink_msg_hil_controls_send(_channel,
-							      hrt_absolute_time(),
-							      out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
-							      mavlink_base_mode,
-							      0);
-			} else {
-
-				/* fixed wing: scale all channels except throttle -1 .. 1
-				 * because we know that we set the mixers up this way
-				 */
-
-				float out[8];
-
-				const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2;
-
-				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);
-
-					} 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);
-					}
-
-				}
-
-				mavlink_msg_hil_controls_send(_channel,
-							      hrt_absolute_time(),
-							      out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
-							      mavlink_base_mode,
-							      0);
-			}
-		}
-	}
-};
-
-
-class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream
-{
-public:
-	const char *get_name()
-	{
-		return "GLOBAL_POSITION_SETPOINT_INT";
-	}
-
-	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)) {
-			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));
-		}
-	}
-};
-
-
-class MavlinkStreamLocalPositionSetpoint : public MavlinkStream
-{
-public:
-	const char *get_name()
-	{
-		return "LOCAL_POSITION_SETPOINT";
-	}
-
-	MavlinkStream *new_instance()
-	{
-		return new MavlinkStreamLocalPositionSetpoint();
-	}
-
-private:
-	MavlinkOrbSubscription *pos_sp_sub;
-	struct vehicle_local_position_setpoint_s *pos_sp;
-
-protected:
-	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)) {
-			mavlink_msg_local_position_setpoint_send(_channel,
-					MAV_FRAME_LOCAL_NED,
-					pos_sp->x,
-					pos_sp->y,
-					pos_sp->z,
-					pos_sp->yaw);
-		}
-	}
-};
-
-
-class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream
-{
-public:
-	const char *get_name()
-	{
-		return "ROLL_PITCH_YAW_THRUST_SETPOINT";
-	}
-
-	MavlinkStream *new_instance()
-	{
-		return new MavlinkStreamRollPitchYawThrustSetpoint();
-	}
-
-private:
-	MavlinkOrbSubscription *att_sp_sub;
-	struct vehicle_attitude_setpoint_s *att_sp;
-
-protected:
-	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)) {
-			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);
-		}
-	}
-};
-
-
-class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream
-{
-public:
-	const char *get_name()
-	{
-		return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT";
-	}
-
-	MavlinkStream *new_instance()
-	{
-		return new MavlinkStreamRollPitchYawRatesThrustSetpoint();
-	}
-
-private:
-	MavlinkOrbSubscription *att_rates_sp_sub;
-	struct vehicle_rates_setpoint_s *att_rates_sp;
-
-protected:
-	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)) {
-			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);
-		}
-	}
-};
-
-
-class MavlinkStreamRCChannelsRaw : public MavlinkStream
-{
-public:
-	const char *get_name()
-	{
-		return "RC_CHANNELS_RAW";
-	}
-
-	MavlinkStream *new_instance()
-	{
-		return new MavlinkStreamRCChannelsRaw();
-	}
-
-private:
-	MavlinkOrbSubscription *rc_sub;
-	struct rc_input_values *rc;
-
-protected:
-	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)) {
-			const unsigned port_width = 8;
-
-			for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) {
-				/* Channels are sent in MAVLink main loop at a fixed interval */
-				mavlink_msg_rc_channels_raw_send(_channel,
-								 rc->timestamp_publication / 1000,
-								 i,
-								 (rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX,
-								 (rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX,
-								 (rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX,
-								 (rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX,
-								 (rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX,
-								 (rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX,
-								 (rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX,
-								 (rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX,
-								 rc->rssi);
-			}
-		}
-	}
-};
-
-
-class MavlinkStreamManualControl : public MavlinkStream
-{
-public:
-	const char *get_name()
-	{
-		return "MANUAL_CONTROL";
-	}
-
-	MavlinkStream *new_instance()
-	{
-		return new MavlinkStreamManualControl();
-	}
-
-private:
-	MavlinkOrbSubscription *manual_sub;
-	struct manual_control_setpoint_s *manual;
-
-protected:
-	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)) {
-			mavlink_msg_manual_control_send(_channel,
-							mavlink_system.sysid,
-							manual->x * 1000,
-							manual->y * 1000,
-							manual->z * 1000,
-							manual->r * 1000,
-							0);
-		}
-	}
-};
+		struct vehicle_gps_position_s gps;
 
-
-class MavlinkStreamOpticalFlow : public MavlinkStream
-{
-public:
-	const char *get_name()
-	{
-		return "OPTICAL_FLOW";
-	}
-
-	MavlinkStream *new_instance()
-	{
-		return new MavlinkStreamOpticalFlow();
-	}
-
-private:
-	MavlinkOrbSubscription *flow_sub;
-	struct optical_flow_s *flow;
-
-protected:
-	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)) {
-			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);
-		}
-	}
-};
-
-class MavlinkStreamAttitudeControls : public MavlinkStream
-{
-public:
-	const char *get_name()
-	{
-		return "ATTITUDE_CONTROLS";
-	}
-
-	MavlinkStream *new_instance()
-	{
-		return new MavlinkStreamAttitudeControls();
-	}
-
-private:
-	MavlinkOrbSubscription *att_ctrl_sub;
-	struct actuator_controls_s *att_ctrl;
-
-protected:
-	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)) {
-			/* send, add spaces so that string buffer is at least 10 chars long */
-			mavlink_msg_named_value_float_send(_channel,
-							   att_ctrl->timestamp / 1000,
-							   "rll ctrl    ",
-							   att_ctrl->control[0]);
-			mavlink_msg_named_value_float_send(_channel,
-							   att_ctrl->timestamp / 1000,
-							   "ptch ctrl    ",
-							   att_ctrl->control[1]);
-			mavlink_msg_named_value_float_send(_channel,
-							   att_ctrl->timestamp / 1000,
-							   "yaw ctrl     ",
-							   att_ctrl->control[2]);
-			mavlink_msg_named_value_float_send(_channel,
-							   att_ctrl->timestamp / 1000,
-							   "thr ctrl     ",
-							   att_ctrl->control[3]);
-		}
-	}
-};
-
-class MavlinkStreamNamedValueFloat : public MavlinkStream
-{
-public:
-	const char *get_name()
-	{
-		return "NAMED_VALUE_FLOAT";
-	}
-
-	MavlinkStream *new_instance()
-	{
-		return new MavlinkStreamNamedValueFloat();
-	}
-
-private:
-	MavlinkOrbSubscription *debug_sub;
-	struct debug_key_value_s *debug;
-
-protected:
-	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)) {
-			/* enforce null termination */
-			debug->key[sizeof(debug->key) - 1] = '\0';
-
-			mavlink_msg_named_value_float_send(_channel,
-							   debug->timestamp_ms,
-							   debug->key,
-							   debug->value);
-		}
-	}
-};
-
-class MavlinkStreamCameraCapture : public MavlinkStream
-{
-public:
-	const char *get_name()
-	{
-		return "CAMERA_CAPTURE";
-	}
-
-	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);
-
-		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);
-
-		} else {
-			/* send camera capture off */
-			mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0);
+		if (gps_sub->update(t, &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);
 		}
 	}
 };
 
-class MavlinkStreamDistanceSensor : public MavlinkStream
-{
-public:
-	const char *get_name()
-	{
-		return "DISTANCE_SENSOR";
-	}
-
-	MavlinkStream *new_instance()
-	{
-		return new MavlinkStreamDistanceSensor();
-	}
-
-private:
-	MavlinkOrbSubscription *range_sub;
-	struct range_finder_report *range;
-
-protected:
-	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)) {
-
-			uint8_t type;
-
-			switch (range->type) {
-				case RANGE_FINDER_TYPE_LASER:
-				type = MAV_DISTANCE_SENSOR_LASER;
-				break;
-			}
-
-			uint8_t id = 0;
-			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);
-		}
-	}
-};
 
-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(),
+// class MavlinkStreamGlobalPositionInt : public MavlinkStream
+// {
+// public:
+// 	const char *get_name()
+// 	{
+// 		return "GLOBAL_POSITION_INT";
+// 	}
+
+// 	MavlinkStream *new_instance()
+// 	{
+// 		return new MavlinkStreamGlobalPositionInt();
+// 	}
+
+// private:
+// 	MavlinkOrbSubscription *pos_sub;
+// 	struct vehicle_global_position_s *pos;
+
+// 	MavlinkOrbSubscription *home_sub;
+// 	struct home_position_s *home;
+
+// protected:
+// 	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);
+
+// 		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);
+// 		}
+// 	}
+// };
+
+
+// class MavlinkStreamLocalPositionNED : public MavlinkStream
+// {
+// public:
+// 	const char *get_name()
+// 	{
+// 		return "LOCAL_POSITION_NED";
+// 	}
+
+// 	MavlinkStream *new_instance()
+// 	{
+// 		return new MavlinkStreamLocalPositionNED();
+// 	}
+
+// private:
+// 	MavlinkOrbSubscription *pos_sub;
+// 	struct vehicle_local_position_s *pos;
+
+// protected:
+// 	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)) {
+// 			mavlink_msg_local_position_ned_send(_channel,
+// 							    pos->timestamp / 1000,
+// 							    pos->x,
+// 							    pos->y,
+// 							    pos->z,
+// 							    pos->vx,
+// 							    pos->vy,
+// 							    pos->vz);
+// 		}
+// 	}
+// };
+
+
+
+// class MavlinkStreamViconPositionEstimate : public MavlinkStream
+// {
+// public:
+// 	const char *get_name()
+// 	{
+// 		return "VICON_POSITION_ESTIMATE";
+// 	}
+
+// 	MavlinkStream *new_instance()
+// 	{
+// 		return new MavlinkStreamViconPositionEstimate();
+// 	}
+
+// private:
+// 	MavlinkOrbSubscription *pos_sub;
+// 	struct vehicle_vicon_position_s *pos;
+
+// protected:
+// 	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)) {
+// 			mavlink_msg_vicon_position_estimate_send(_channel,
+// 								pos->timestamp / 1000,
+// 								pos->x,
+// 								pos->y,
+// 								pos->z,
+// 								pos->roll,
+// 								pos->pitch,
+// 								pos->yaw);
+// 		}
+// 	}
+// };
+
+
+// class MavlinkStreamGPSGlobalOrigin : public MavlinkStream
+// {
+// public:
+// 	const char *get_name()
+// 	{
+// 		return "GPS_GLOBAL_ORIGIN";
+// 	}
+
+// 	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);
+
+// 			mavlink_msg_gps_global_origin_send(_channel,
+// 							   (int32_t)(home->lat * 1e7),
+// 							   (int32_t)(home->lon * 1e7),
+// 							   (int32_t)(home->alt) * 1000.0f);
+// 		}
+// 	}
+// };
+
+
+// class MavlinkStreamServoOutputRaw : public MavlinkStream
+// {
+// public:
+// 	MavlinkStreamServoOutputRaw(unsigned int n) : MavlinkStream(), _n(n)
+// 	{
+// 		sprintf(_name, "SERVO_OUTPUT_RAW_%d", _n);
+// 	}
+
+// 	const char *get_name()
+// 	{
+// 		return _name;
+// 	}
+
+// 	MavlinkStream *new_instance()
+// 	{
+// 		return new MavlinkStreamServoOutputRaw(_n);
+// 	}
+
+// private:
+// 	MavlinkOrbSubscription *act_sub;
+// 	struct actuator_outputs_s *act;
+
+// 	char _name[20];
+// 	unsigned int _n;
+
+// protected:
+// 	void subscribe(Mavlink *mavlink)
+// 	{
+// 		orb_id_t act_topics[] = {
+// 			ORB_ID(actuator_outputs_0),
+// 			ORB_ID(actuator_outputs_1),
+// 			ORB_ID(actuator_outputs_2),
+// 			ORB_ID(actuator_outputs_3)
+// 		};
+
+// 		act_sub = mavlink->add_orb_subscription(act_topics[_n]);
+// 		act = (struct actuator_outputs_s *)act_sub->get_data();
+// 	}
+
+// 	void send(const hrt_abstime t)
+// 	{
+// 		if (act_sub->update(t)) {
+// 			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]);
+// 		}
+// 	}
+// };
+
+
+// class MavlinkStreamHILControls : public MavlinkStream
+// {
+// public:
+// 	const char *get_name()
+// 	{
+// 		return "HIL_CONTROLS";
+// 	}
+
+// 	MavlinkStream *new_instance()
+// 	{
+// 		return new MavlinkStreamHILControls();
+// 	}
+
+// private:
+// 	MavlinkOrbSubscription *status_sub;
+// 	struct vehicle_status_s *status;
+
+// 	MavlinkOrbSubscription *pos_sp_triplet_sub;
+// 	struct position_setpoint_triplet_s *pos_sp_triplet;
+
+// 	MavlinkOrbSubscription *act_sub;
+// 	struct actuator_outputs_s *act;
+
+// 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();
+
+// 		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);
+
+// 		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);
+
+// 			if (mavlink_system.type == MAV_TYPE_QUADROTOR ||
+// 				mavlink_system.type == MAV_TYPE_HEXAROTOR ||
+// 				mavlink_system.type == MAV_TYPE_OCTOROTOR) {
+// 				/* set number of valid outputs depending on vehicle type */
+// 				unsigned n;
+
+// 				switch (mavlink_system.type) {
+// 				case MAV_TYPE_QUADROTOR:
+// 					n = 4;
+// 					break;
+
+// 				case MAV_TYPE_HEXAROTOR:
+// 					n = 6;
+// 					break;
+
+// 				default:
+// 					n = 8;
+// 					break;
+// 				}
+
+// 				/* scale / assign outputs depending on system type */
+// 				float out[8];
+
+// 				for (unsigned i = 0; i < 8; i++) {
+// 					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);
+
+// 						} else {
+// 							/* send 0 when disarmed */
+// 							out[i] = 0.0f;
+// 						}
+
+// 					} else {
+// 						out[i] = -1.0f;
+// 					}
+// 				}
+
+// 				mavlink_msg_hil_controls_send(_channel,
+// 							      hrt_absolute_time(),
+// 							      out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
+// 							      mavlink_base_mode,
+// 							      0);
+// 			} else {
+
+// 				/* fixed wing: scale all channels except throttle -1 .. 1
+// 				 * because we know that we set the mixers up this way
+// 				 */
+
+// 				float out[8];
+
+// 				const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2;
+
+// 				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);
+
+// 					} 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);
+// 					}
+
+// 				}
+
+// 				mavlink_msg_hil_controls_send(_channel,
+// 							      hrt_absolute_time(),
+// 							      out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
+// 							      mavlink_base_mode,
+// 							      0);
+// 			}
+// 		}
+// 	}
+// };
+
+
+// class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream
+// {
+// public:
+// 	const char *get_name()
+// 	{
+// 		return "GLOBAL_POSITION_SETPOINT_INT";
+// 	}
+
+// 	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)) {
+// 			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));
+// 		}
+// 	}
+// };
+
+
+// class MavlinkStreamLocalPositionSetpoint : public MavlinkStream
+// {
+// public:
+// 	const char *get_name()
+// 	{
+// 		return "LOCAL_POSITION_SETPOINT";
+// 	}
+
+// 	MavlinkStream *new_instance()
+// 	{
+// 		return new MavlinkStreamLocalPositionSetpoint();
+// 	}
+
+// private:
+// 	MavlinkOrbSubscription *pos_sp_sub;
+// 	struct vehicle_local_position_setpoint_s *pos_sp;
+
+// protected:
+// 	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)) {
+// 			mavlink_msg_local_position_setpoint_send(_channel,
+// 					MAV_FRAME_LOCAL_NED,
+// 					pos_sp->x,
+// 					pos_sp->y,
+// 					pos_sp->z,
+// 					pos_sp->yaw);
+// 		}
+// 	}
+// };
+
+
+// class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream
+// {
+// public:
+// 	const char *get_name()
+// 	{
+// 		return "ROLL_PITCH_YAW_THRUST_SETPOINT";
+// 	}
+
+// 	MavlinkStream *new_instance()
+// 	{
+// 		return new MavlinkStreamRollPitchYawThrustSetpoint();
+// 	}
+
+// private:
+// 	MavlinkOrbSubscription *att_sp_sub;
+// 	struct vehicle_attitude_setpoint_s *att_sp;
+
+// protected:
+// 	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)) {
+// 			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);
+// 		}
+// 	}
+// };
+
+
+// class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream
+// {
+// public:
+// 	const char *get_name()
+// 	{
+// 		return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT";
+// 	}
+
+// 	MavlinkStream *new_instance()
+// 	{
+// 		return new MavlinkStreamRollPitchYawRatesThrustSetpoint();
+// 	}
+
+// private:
+// 	MavlinkOrbSubscription *att_rates_sp_sub;
+// 	struct vehicle_rates_setpoint_s *att_rates_sp;
+
+// protected:
+// 	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)) {
+// 			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);
+// 		}
+// 	}
+// };
+
+
+// class MavlinkStreamRCChannelsRaw : public MavlinkStream
+// {
+// public:
+// 	const char *get_name()
+// 	{
+// 		return "RC_CHANNELS_RAW";
+// 	}
+
+// 	MavlinkStream *new_instance()
+// 	{
+// 		return new MavlinkStreamRCChannelsRaw();
+// 	}
+
+// private:
+// 	MavlinkOrbSubscription *rc_sub;
+// 	struct rc_input_values *rc;
+
+// protected:
+// 	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)) {
+// 			const unsigned port_width = 8;
+
+// 			for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) {
+// 				/* Channels are sent in MAVLink main loop at a fixed interval */
+// 				mavlink_msg_rc_channels_raw_send(_channel,
+// 								 rc->timestamp_publication / 1000,
+// 								 i,
+// 								 (rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX,
+// 								 (rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX,
+// 								 (rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX,
+// 								 (rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX,
+// 								 (rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX,
+// 								 (rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX,
+// 								 (rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX,
+// 								 (rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX,
+// 								 rc->rssi);
+// 			}
+// 		}
+// 	}
+// };
+
+
+// class MavlinkStreamManualControl : public MavlinkStream
+// {
+// public:
+// 	const char *get_name()
+// 	{
+// 		return "MANUAL_CONTROL";
+// 	}
+
+// 	MavlinkStream *new_instance()
+// 	{
+// 		return new MavlinkStreamManualControl();
+// 	}
+
+// private:
+// 	MavlinkOrbSubscription *manual_sub;
+// 	struct manual_control_setpoint_s *manual;
+
+// protected:
+// 	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)) {
+// 			mavlink_msg_manual_control_send(_channel,
+// 							mavlink_system.sysid,
+// 							manual->x * 1000,
+// 							manual->y * 1000,
+// 							manual->z * 1000,
+// 							manual->r * 1000,
+// 							0);
+// 		}
+// 	}
+// };
+
+
+// class MavlinkStreamOpticalFlow : public MavlinkStream
+// {
+// public:
+// 	const char *get_name()
+// 	{
+// 		return "OPTICAL_FLOW";
+// 	}
+
+// 	MavlinkStream *new_instance()
+// 	{
+// 		return new MavlinkStreamOpticalFlow();
+// 	}
+
+// private:
+// 	MavlinkOrbSubscription *flow_sub;
+// 	struct optical_flow_s *flow;
+
+// protected:
+// 	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)) {
+// 			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);
+// 		}
+// 	}
+// };
+
+// class MavlinkStreamAttitudeControls : public MavlinkStream
+// {
+// public:
+// 	const char *get_name()
+// 	{
+// 		return "ATTITUDE_CONTROLS";
+// 	}
+
+// 	MavlinkStream *new_instance()
+// 	{
+// 		return new MavlinkStreamAttitudeControls();
+// 	}
+
+// private:
+// 	MavlinkOrbSubscription *att_ctrl_sub;
+// 	struct actuator_controls_s *att_ctrl;
+
+// protected:
+// 	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)) {
+// 			/* send, add spaces so that string buffer is at least 10 chars long */
+// 			mavlink_msg_named_value_float_send(_channel,
+// 							   att_ctrl->timestamp / 1000,
+// 							   "rll ctrl    ",
+// 							   att_ctrl->control[0]);
+// 			mavlink_msg_named_value_float_send(_channel,
+// 							   att_ctrl->timestamp / 1000,
+// 							   "ptch ctrl    ",
+// 							   att_ctrl->control[1]);
+// 			mavlink_msg_named_value_float_send(_channel,
+// 							   att_ctrl->timestamp / 1000,
+// 							   "yaw ctrl     ",
+// 							   att_ctrl->control[2]);
+// 			mavlink_msg_named_value_float_send(_channel,
+// 							   att_ctrl->timestamp / 1000,
+// 							   "thr ctrl     ",
+// 							   att_ctrl->control[3]);
+// 		}
+// 	}
+// };
+
+// class MavlinkStreamNamedValueFloat : public MavlinkStream
+// {
+// public:
+// 	const char *get_name()
+// 	{
+// 		return "NAMED_VALUE_FLOAT";
+// 	}
+
+// 	MavlinkStream *new_instance()
+// 	{
+// 		return new MavlinkStreamNamedValueFloat();
+// 	}
+
+// private:
+// 	MavlinkOrbSubscription *debug_sub;
+// 	struct debug_key_value_s *debug;
+
+// protected:
+// 	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)) {
+// 			/* enforce null termination */
+// 			debug->key[sizeof(debug->key) - 1] = '\0';
+
+// 			mavlink_msg_named_value_float_send(_channel,
+// 							   debug->timestamp_ms,
+// 							   debug->key,
+// 							   debug->value);
+// 		}
+// 	}
+// };
+
+// class MavlinkStreamCameraCapture : public MavlinkStream
+// {
+// public:
+// 	const char *get_name()
+// 	{
+// 		return "CAMERA_CAPTURE";
+// 	}
+
+// 	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);
+
+// 		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);
+
+// 		} else {
+// 			/* send camera capture off */
+// 			mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0);
+// 		}
+// 	}
+// };
+
+// class MavlinkStreamDistanceSensor : public MavlinkStream
+// {
+// public:
+// 	const char *get_name()
+// 	{
+// 		return "DISTANCE_SENSOR";
+// 	}
+
+// 	MavlinkStream *new_instance()
+// 	{
+// 		return new MavlinkStreamDistanceSensor();
+// 	}
+
+// private:
+// 	MavlinkOrbSubscription *range_sub;
+// 	struct range_finder_report *range;
+
+// protected:
+// 	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)) {
+
+// 			uint8_t type;
+
+// 			switch (range->type) {
+// 				case RANGE_FINDER_TYPE_LASER:
+// 				type = MAV_DISTANCE_SENSOR_LASER;
+// 				break;
+// 			}
+
+// 			uint8_t id = 0;
+// 			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);
+// 		}
+// 	}
+// };
+
+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 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(),
 	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 d432edd2b..0a23fb01e 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.cpp
+++ b/src/modules/mavlink/mavlink_orb_subscription.cpp
@@ -53,30 +53,21 @@ MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) :
 	_last_check(0),
 	next(nullptr)
 {
-	_data = malloc(topic->o_size);
-	memset(_data, 0, topic->o_size);
 }
 
 MavlinkOrbSubscription::~MavlinkOrbSubscription()
 {
 	close(_fd);
-	free(_data);
 }
 
-const orb_id_t
-MavlinkOrbSubscription::get_topic()
+orb_id_t
+MavlinkOrbSubscription::get_topic() const
 {
 	return _topic;
 }
 
-void *
-MavlinkOrbSubscription::get_data()
-{
-	return _data;
-}
-
 bool
-MavlinkOrbSubscription::update(const hrt_abstime t)
+MavlinkOrbSubscription::update(const hrt_abstime t, void* data)
 {
 	if (_last_check == t) {
 		/* already checked right now, return result of the check */
@@ -86,8 +77,8 @@ MavlinkOrbSubscription::update(const hrt_abstime t)
 		_last_check = t;
 		orb_check(_fd, &_updated);
 
-		if (_updated) {
-			orb_copy(_topic, _fd, _data);
+		if (_updated && data) {
+			orb_copy(_topic, _fd, data);
 			_published = true;
 			return true;
 		}
diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h
index 5c6543e81..abd4031bd 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.h
+++ b/src/modules/mavlink/mavlink_orb_subscription.h
@@ -48,12 +48,12 @@
 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);
+	bool update(const hrt_abstime t, void* data);
 
 	/**
 	 * Check if the topic has been published.
@@ -62,16 +62,14 @@ public:
 	 * @return true if the topic has been published at least once.
 	 */
 	bool is_published();
-	void *get_data();
-	const 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
+	hrt_abstime _last_check;	///< time of last check
+	bool _updated;			///< updated on last check
 };
 
 
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 72b9ee83a..666b3a8cd 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -950,7 +950,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 def40d9ad..eb881edd7 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;
@@ -67,9 +59,19 @@ public:
 	void set_interval(const unsigned int interval);
 	void set_channel(mavlink_channel_t channel);
 	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;
 };