7 files changed, 721 insertions, 735 deletions

diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp
index fdc196371..ea1e3a8bb 100644
--- a/src/modules/mavlink/mavlink_orb_listener.cpp
+++ b/src/modules/mavlink/mavlink_orb_listener.cpp
@@ -57,17 +57,14 @@
 #include <lib/geo/geo.h>
 #include <systemlib/err.h>
 
-
 #include <mavlink/mavlink_log.h>
 
 #include "mavlink_orb_listener.h"
 #include "mavlink_main.h"
 
 
-
 uint16_t cm_uint16_from_m_float(float m);
 
-
 uint16_t
 cm_uint16_from_m_float(float m)
 {
@@ -85,212 +82,195 @@ MavlinkOrbListener::MavlinkOrbListener(Mavlink* parent) :
 
 	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink orb")),
 	_mavlink(parent),
-	_listeners(nullptr),
-	_n_listeners(0)
+	_subscriptions(nullptr),
+	_streams(nullptr)
 {
-	add_listener(MavlinkOrbListener::l_sensor_combined,		&(_mavlink->get_subs()->sensor_sub),			0);
-	add_listener(MavlinkOrbListener::l_vehicle_attitude,		&(_mavlink->get_subs()->att_sub),			0);
-	add_listener(MavlinkOrbListener::l_vehicle_gps_position,	&(_mavlink->get_subs()->gps_sub),			0);
-	add_listener(MavlinkOrbListener::l_vehicle_status,		&(_mavlink->get_subs()->status_sub),			0);
-	add_listener(MavlinkOrbListener::l_home,			&(_mavlink->get_subs()->home_sub),			0);
-	add_listener(MavlinkOrbListener::l_rc_channels,			&(_mavlink->get_subs()->rc_sub),			0);
-	add_listener(MavlinkOrbListener::l_input_rc,			&(_mavlink->get_subs()->input_rc_sub),			0);
-	add_listener(MavlinkOrbListener::l_global_position,		&(_mavlink->get_subs()->global_pos_sub),		0);
-	add_listener(MavlinkOrbListener::l_local_position,		&(_mavlink->get_subs()->local_pos_sub),			0);
-	add_listener(MavlinkOrbListener::l_global_position_setpoint,	&(_mavlink->get_subs()->triplet_sub),			0);
-	add_listener(MavlinkOrbListener::l_local_position_setpoint,	&(_mavlink->get_subs()->spl_sub),			0);
-	add_listener(MavlinkOrbListener::l_attitude_setpoint,		&(_mavlink->get_subs()->spa_sub),			0);
-	add_listener(MavlinkOrbListener::l_actuator_outputs,		&(_mavlink->get_subs()->act_0_sub),			0);
-	add_listener(MavlinkOrbListener::l_actuator_outputs,		&(_mavlink->get_subs()->act_1_sub),			1);
-	add_listener(MavlinkOrbListener::l_actuator_outputs,		&(_mavlink->get_subs()->act_2_sub),			2);
-	add_listener(MavlinkOrbListener::l_actuator_outputs,		&(_mavlink->get_subs()->act_3_sub),			3);
-	add_listener(MavlinkOrbListener::l_actuator_armed,		&(_mavlink->get_subs()->armed_sub),			0);
-	add_listener(MavlinkOrbListener::l_manual_control_setpoint,	&(_mavlink->get_subs()->man_control_sp_sub), 		0);
-	add_listener(MavlinkOrbListener::l_vehicle_attitude_controls,	&(_mavlink->get_subs()->actuators_sub),			0);
-	add_listener(MavlinkOrbListener::l_debug_key_value,		&(_mavlink->get_subs()->debug_key_value),		0);
-	add_listener(MavlinkOrbListener::l_optical_flow,		&(_mavlink->get_subs()->optical_flow),			0);
-	add_listener(MavlinkOrbListener::l_vehicle_rates_setpoint,	&(_mavlink->get_subs()->rates_setpoint_sub),		0);
-	add_listener(MavlinkOrbListener::l_airspeed,			&(_mavlink->get_subs()->airspeed_sub),			0);
-	add_listener(MavlinkOrbListener::l_nav_cap,			&(_mavlink->get_subs()->navigation_capabilities_sub),	0);
 
 }
 
-void MavlinkOrbListener::add_listener(void (*callback)(const struct listener *l), int *subp, uintptr_t arg)
+MavlinkOrbListener::~MavlinkOrbListener()
 {
-	struct listener *nl = new listener;
-
-	nl->callback = callback;
-	nl->subp = subp;
-	nl->arg = arg;
-	nl->next = nullptr;
-	nl->mavlink = _mavlink;
-	nl->listener = this;
-
-	if (_listeners == nullptr) {
-		_listeners = nl;
-	} else {
-		struct listener *next = _listeners;
-		while (next->next != nullptr) {
-			next = next->next;
-		}
-		next->next = nl;
-	}
-	_n_listeners++;
-}
 
-void
-MavlinkOrbListener::l_sensor_combined(const struct listener *l)
-{
-	struct sensor_combined_s raw;
-
-	/* copy sensors raw data into local buffer */
-	orb_copy(ORB_ID(sensor_combined), l->mavlink->get_subs()->sensor_sub, &raw);
-
-	/* mark individual fields as _mavlink->get_chan()ged */
-	uint16_t fields_updated = 0;
-
-	// if (accel_counter != raw.accelerometer_counter) {
-	// 	/* mark first three dimensions as _mavlink->get_chan()ged */
-	// 	fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
-	// 	accel_counter = raw.accelerometer_counter;
-	// }
-
-	// if (gyro_counter != raw.gyro_counter) {
-	// 	/* mark second group dimensions as _mavlink->get_chan()ged */
-	// 	fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
-	// 	gyro_counter = raw.gyro_counter;
-	// }
-
-	// if (mag_counter != raw.magnetometer_counter) {
-	// 	/* mark third group dimensions as _mavlink->get_chan()ged */
-	// 	fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
-	// 	mag_counter = raw.magnetometer_counter;
-	// }
-
-	// if (baro_counter != raw.baro_counter) {
-	// 	/* mark last group dimensions as _mavlink->get_chan()ged */
-	// 	fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
-	// 	baro_counter = raw.baro_counter;
-	// }
-
-	if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
-		mavlink_msg_highres_imu_send(l->mavlink->get_chan(), l->listener->last_sensor_timestamp,
-					     raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1],
-					     raw.accelerometer_m_s2[2], raw.gyro_rad_s[0],
-					     raw.gyro_rad_s[1], raw.gyro_rad_s[2],
-					     raw.magnetometer_ga[0],
-					     raw.magnetometer_ga[1], raw.magnetometer_ga[2],
-					     raw.baro_pres_mbar, raw.differential_pressure_pa,
-					     raw.baro_alt_meter, raw.baro_temp_celcius,
-					     fields_updated);
-
-	l->listener->sensors_raw_counter++;
 }
 
-void
-MavlinkOrbListener::l_vehicle_attitude(const struct listener *l)
+MavlinkOrbSubscription *MavlinkOrbListener::add_subscription(const struct orb_metadata *meta, const size_t size, const MavlinkStream *stream, const unsigned int interval)
 {
-	/* copy attitude data into local buffer */
-	orb_copy(ORB_ID(vehicle_attitude), l->mavlink->get_subs()->att_sub, &l->listener->att);
-
-	if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) {
-		/* send sensor values */
-		mavlink_msg_attitude_send(l->mavlink->get_chan(),
-					  l->listener->last_sensor_timestamp / 1000,
-					  l->listener->att.roll,
-					  l->listener->att.pitch,
-					  l->listener->att.yaw,
-					  l->listener->att.rollspeed,
-					  l->listener->att.pitchspeed,
-					  l->listener->att.yawspeed);
-					  	
-		/* limit VFR message rate to 10Hz */
-		hrt_abstime t = hrt_absolute_time();
-		if (t >= l->listener->last_sent_vfr + 100000) {
-			l->listener->last_sent_vfr = t;
-			float groundspeed = sqrtf(l->listener->global_pos.vel_n * l->listener->global_pos.vel_n + l->listener->global_pos.vel_e * l->listener->global_pos.vel_e);
-			uint16_t heading = _wrap_2pi(l->listener->att.yaw) * M_RAD_TO_DEG_F;
-			float throttle = l->listener->armed.armed ? l->listener->actuators_0.control[3] * 100.0f : 0.0f;
-			mavlink_msg_vfr_hud_send(l->mavlink->get_chan(), l->listener->airspeed.true_airspeed_m_s, groundspeed, heading, throttle, l->listener->global_pos.alt, -l->listener->global_pos.vel_d);
-		}
-		
-		/* send quaternion values if it exists */
-		if(l->listener->att.q_valid) {
-			mavlink_msg_attitude_quaternion_send(l->mavlink->get_chan(),
-												l->listener->last_sensor_timestamp / 1000,
-												l->listener->att.q[0],
-												l->listener->att.q[1],
-												l->listener->att.q[2],
-												l->listener->att.q[3],
-												l->listener->att.rollspeed,
-												l->listener->att.pitchspeed,
-												l->listener->att.yawspeed);
+	/* check if already subscribed to this topic */
+	MavlinkOrbSubscription *sub;
+
+	LL_FOREACH(_subscriptions, sub) {
+		if (sub->meta == meta) {
+			/* already subscribed */
+			if (sub->interval > interval) {
+				/* subscribed with bigger interval, change interval */
+				sub->set_interval(interval);
+			}
+			return sub;
 		}
 	}
 
-	l->listener->attitude_counter++;
+	/* add new subscription */
+	MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(meta, size);
+
+	sub_new->set_interval(interval);
+
+	LL_APPEND(_subscriptions, sub_new);
+
+	return sub_new;
 }
 
-void
-MavlinkOrbListener::l_vehicle_gps_position(const struct listener *l)
+void MavlinkOrbListener::add_stream(void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **metas, const size_t *sizes, const uintptr_t arg, const unsigned int interval)
 {
-	struct vehicle_gps_position_s gps;
-
-	/* copy gps data into local buffer */
-	orb_copy(ORB_ID(vehicle_gps_position), l->mavlink->get_subs()->gps_sub, &gps);
-
-	/* GPS COG is 0..2PI in degrees * 1e2 */
-	float cog_deg = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F;
-
-	/* GPS position */
-	mavlink_msg_gps_raw_int_send(l->mavlink->get_chan(),
-				     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 * 1e2f, // from m/s to cm/s
-				     cog_deg * 1e2f, // from deg to deg * 100
-				     gps.satellites_visible);
-
-	/* update SAT info every 10 seconds */
-	if (gps.satellite_info_available && (l->listener->gps_counter % 50 == 0)) {
-		mavlink_msg_gps_status_send(l->mavlink->get_chan(),
-					    gps.satellites_visible,
-					    gps.satellite_prn,
-					    gps.satellite_used,
-					    gps.satellite_elevation,
-					    gps.satellite_azimuth,
-					    gps.satellite_snr);
-	}
+	MavlinkStream *stream = new MavlinkStream(this, callback, subs_n, metas, sizes, arg, interval);
 
-	l->listener->gps_counter++;
+	stream->mavlink = _mavlink;
+
+	LL_APPEND(_streams, stream);
 }
 
+//void
+//MavlinkOrbListener::l_sensor_combined(const struct listener *l)
+//{
+//	struct sensor_combined_s raw;
+//
+//	/* copy sensors raw data into local buffer */
+//	orb_copy(ORB_ID(sensor_combined), l->mavlink->get_subs()->sensor_sub, &raw);
+//
+//	/* mark individual fields as _mavlink->get_chan()ged */
+//	uint16_t fields_updated = 0;
+//
+//	// if (accel_counter != raw.accelerometer_counter) {
+//	// 	/* mark first three dimensions as _mavlink->get_chan()ged */
+//	// 	fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
+//	// 	accel_counter = raw.accelerometer_counter;
+//	// }
+//
+//	// if (gyro_counter != raw.gyro_counter) {
+//	// 	/* mark second group dimensions as _mavlink->get_chan()ged */
+//	// 	fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
+//	// 	gyro_counter = raw.gyro_counter;
+//	// }
+//
+//	// if (mag_counter != raw.magnetometer_counter) {
+//	// 	/* mark third group dimensions as _mavlink->get_chan()ged */
+//	// 	fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
+//	// 	mag_counter = raw.magnetometer_counter;
+//	// }
+//
+//	// if (baro_counter != raw.baro_counter) {
+//	// 	/* mark last group dimensions as _mavlink->get_chan()ged */
+//	// 	fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
+//	// 	baro_counter = raw.baro_counter;
+//	// }
+//
+//	if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
+//		mavlink_msg_highres_imu_send(l->mavlink->get_chan(), l->listener->last_sensor_timestamp,
+//					     raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1],
+//					     raw.accelerometer_m_s2[2], raw.gyro_rad_s[0],
+//					     raw.gyro_rad_s[1], raw.gyro_rad_s[2],
+//					     raw.magnetometer_ga[0],
+//					     raw.magnetometer_ga[1], raw.magnetometer_ga[2],
+//					     raw.baro_pres_mbar, raw.differential_pressure_pa,
+//					     raw.baro_alt_meter, raw.baro_temp_celcius,
+//					     fields_updated);
+//
+//	l->listener->sensors_raw_counter++;
+//}
+//
+//void
+//MavlinkOrbListener::l_vehicle_attitude(const struct listener *l)
+//{
+//	/* copy attitude data into local buffer */
+//	orb_copy(ORB_ID(vehicle_attitude), l->mavlink->get_subs()->att_sub, &l->listener->att);
+//
+//	if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) {
+//		/* send sensor values */
+//		mavlink_msg_attitude_send(l->mavlink->get_chan(),
+//					  l->listener->last_sensor_timestamp / 1000,
+//					  l->listener->att.roll,
+//					  l->listener->att.pitch,
+//					  l->listener->att.yaw,
+//					  l->listener->att.rollspeed,
+//					  l->listener->att.pitchspeed,
+//					  l->listener->att.yawspeed);
+//
+//		/* limit VFR message rate to 10Hz */
+//		hrt_abstime t = hrt_absolute_time();
+//		if (t >= l->listener->last_sent_vfr + 100000) {
+//			l->listener->last_sent_vfr = t;
+//			float groundspeed = sqrtf(l->listener->global_pos.vel_n * l->listener->global_pos.vel_n + l->listener->global_pos.vel_e * l->listener->global_pos.vel_e);
+//			uint16_t heading = _wrap_2pi(l->listener->att.yaw) * M_RAD_TO_DEG_F;
+//			float throttle = l->listener->armed.armed ? l->listener->actuators_0.control[3] * 100.0f : 0.0f;
+//			mavlink_msg_vfr_hud_send(l->mavlink->get_chan(), l->listener->airspeed.true_airspeed_m_s, groundspeed, heading, throttle, l->listener->global_pos.alt, -l->listener->global_pos.vel_d);
+//		}
+//
+//		/* send quaternion values if it exists */
+//		if(l->listener->att.q_valid) {
+//			mavlink_msg_attitude_quaternion_send(l->mavlink->get_chan(),
+//												l->listener->last_sensor_timestamp / 1000,
+//												l->listener->att.q[0],
+//												l->listener->att.q[1],
+//												l->listener->att.q[2],
+//												l->listener->att.q[3],
+//												l->listener->att.rollspeed,
+//												l->listener->att.pitchspeed,
+//												l->listener->att.yawspeed);
+//		}
+//	}
+//
+//	l->listener->attitude_counter++;
+//}
+//
+//void
+//MavlinkOrbListener::l_vehicle_gps_position(const struct listener *l)
+//{
+//	struct vehicle_gps_position_s gps;
+//
+//	/* copy gps data into local buffer */
+//	orb_copy(ORB_ID(vehicle_gps_position), l->mavlink->get_subs()->gps_sub, &gps);
+//
+//	/* GPS COG is 0..2PI in degrees * 1e2 */
+//	float cog_deg = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F;
+//
+//	/* GPS position */
+//	mavlink_msg_gps_raw_int_send(l->mavlink->get_chan(),
+//				     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 * 1e2f, // from m/s to cm/s
+//				     cog_deg * 1e2f, // from deg to deg * 100
+//				     gps.satellites_visible);
+//
+//	/* update SAT info every 10 seconds */
+//	if (gps.satellite_info_available && (l->listener->gps_counter % 50 == 0)) {
+//		mavlink_msg_gps_status_send(l->mavlink->get_chan(),
+//					    gps.satellites_visible,
+//					    gps.satellite_prn,
+//					    gps.satellite_used,
+//					    gps.satellite_elevation,
+//					    gps.satellite_azimuth,
+//					    gps.satellite_snr);
+//	}
+//
+//	l->listener->gps_counter++;
+//}
+//
+
 void
-MavlinkOrbListener::l_vehicle_status(const struct listener *l)
+MavlinkOrbListener::msg_heartbeat(const MavlinkStream *stream)
 {
-	/* immediately communicate state _mavlink->get_chan()ges back to user */
-	orb_copy(ORB_ID(vehicle_status), l->mavlink->get_subs()->status_sub, &(l->mavlink->v_status));
-	orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed);
-	orb_copy(ORB_ID(position_setpoint_triplet), l->mavlink->get_subs()->position_setpoint_triplet_sub, &(l->mavlink->pos_sp_triplet));
-
-	/* enable or disable HIL */
-	if (l->listener->v_status.hil_state == HIL_STATE_ON)
-		l->mavlink->set_hil_enabled(true);
-	else if (l->listener->v_status.hil_state == HIL_STATE_OFF)
-		l->mavlink->set_hil_enabled(false);
-
 	/* translate the current syste state to mavlink state and mode */
 	uint8_t mavlink_state = 0;
 	uint8_t mavlink_base_mode = 0;
 	uint32_t mavlink_custom_mode = 0;
-	l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
+	//l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
 
 	/* send heartbeat */
-	mavlink_msg_heartbeat_send(l->mavlink->get_chan(),
+	mavlink_msg_heartbeat_send(stream->mavlink->get_chan(),
 				   mavlink_system.type,
 				   MAV_AUTOPILOT_PX4,
 				   mavlink_base_mode,
@@ -298,409 +278,378 @@ MavlinkOrbListener::l_vehicle_status(const struct listener *l)
 				   mavlink_state);
 }
 
-void
-MavlinkOrbListener::l_rc_channels(const struct listener *l)
-{
-	/* copy rc channels into local buffer */
-	orb_copy(ORB_ID(rc_channels), l->mavlink->get_subs()->rc_sub, &l->listener->rc);
-	// XXX Add RC channels scaled message here
-}
-
-void
-MavlinkOrbListener::l_input_rc(const struct listener *l)
-{
-	/* copy rc _mavlink->get_chan()nels into local buffer */
-	orb_copy(ORB_ID(input_rc), l->mavlink->get_subs()->input_rc_sub, &l->listener->rc_raw);
-
-	if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) {
-
-		const unsigned port_width = 8;
-
-		for (unsigned i = 0; (i * port_width) < l->listener->rc_raw.channel_count; i++) {
-			/* Channels are sent in MAVLink main loop at a fixed interval */
-			mavlink_msg_rc_channels_raw_send(l->mavlink->get_chan(),
-							 l->listener->rc_raw.timestamp_publication / 1000,
-							 i,
-							 (l->listener->rc_raw.channel_count > (i * port_width) + 0) ? l->listener->rc_raw.values[(i * port_width) + 0] : UINT16_MAX,
-							 (l->listener->rc_raw.channel_count > (i * port_width) + 1) ? l->listener->rc_raw.values[(i * port_width) + 1] : UINT16_MAX,
-							 (l->listener->rc_raw.channel_count > (i * port_width) + 2) ? l->listener->rc_raw.values[(i * port_width) + 2] : UINT16_MAX,
-							 (l->listener->rc_raw.channel_count > (i * port_width) + 3) ? l->listener->rc_raw.values[(i * port_width) + 3] : UINT16_MAX,
-							 (l->listener->rc_raw.channel_count > (i * port_width) + 4) ? l->listener->rc_raw.values[(i * port_width) + 4] : UINT16_MAX,
-							 (l->listener->rc_raw.channel_count > (i * port_width) + 5) ? l->listener->rc_raw.values[(i * port_width) + 5] : UINT16_MAX,
-							 (l->listener->rc_raw.channel_count > (i * port_width) + 6) ? l->listener->rc_raw.values[(i * port_width) + 6] : UINT16_MAX,
-							 (l->listener->rc_raw.channel_count > (i * port_width) + 7) ? l->listener->rc_raw.values[(i * port_width) + 7] : UINT16_MAX,
-							  l->listener->rc_raw.rssi);
-		}
-	}
-}
-
-void
-MavlinkOrbListener::l_global_position(const struct listener *l)
-{
-	/* copy global position data into local buffer */
-	orb_copy(ORB_ID(vehicle_global_position), l->mavlink->get_subs()->global_pos_sub, &l->listener->global_pos);
-
-	mavlink_msg_global_position_int_send(l->mavlink->get_chan(),
-						 l->listener->global_pos.timestamp / 1000,
-					     l->listener->global_pos.lat * 1e7,
-					     l->listener->global_pos.lon * 1e7,
-					     l->listener->global_pos.alt * 1000.0f,
-					     (l->listener->global_pos.alt - l->listener->home.alt) * 1000.0f,
-					     l->listener->global_pos.vel_n * 100.0f,
-					     l->listener->global_pos.vel_e * 100.0f,
-					     l->listener->global_pos.vel_d * 100.0f,
-					     _wrap_2pi(l->listener->global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f);
-}
-
-void
-MavlinkOrbListener::l_local_position(const struct listener *l)
-{
-	/* copy local position data into local buffer */
-	orb_copy(ORB_ID(vehicle_local_position), l->mavlink->get_subs()->local_pos_sub, &l->listener->local_pos);
-
-	if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
-		mavlink_msg_local_position_ned_send(l->mavlink->get_chan(),
-						    l->listener->local_pos.timestamp / 1000,
-						    l->listener->local_pos.x,
-						    l->listener->local_pos.y,
-						    l->listener->local_pos.z,
-						    l->listener->local_pos.vx,
-						    l->listener->local_pos.vy,
-						    l->listener->local_pos.vz);
-}
-
-void
-MavlinkOrbListener::l_global_position_setpoint(const struct listener *l)
-{
-	struct position_setpoint_triplet_s triplet;
-	orb_copy(ORB_ID(position_setpoint_triplet), l->mavlink->get_subs()->triplet_sub, &triplet);
-
-	if (!triplet.current.valid)
-		return;
-
-	if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
-		mavlink_msg_global_position_setpoint_int_send(l->mavlink->get_chan(),
-				MAV_FRAME_GLOBAL,
-				(int32_t)(triplet.current.lat * 1e7d),
-				(int32_t)(triplet.current.lon * 1e7d),
-				(int32_t)(triplet.current.alt * 1e3f),
-				(int16_t)(triplet.current.yaw * M_RAD_TO_DEG_F * 1e2f));
-}
-
-void
-MavlinkOrbListener::l_local_position_setpoint(const struct listener *l)
-{
-	struct vehicle_local_position_setpoint_s local_sp;
-
-	/* copy local position data into local buffer */
-	orb_copy(ORB_ID(vehicle_local_position_setpoint), l->mavlink->get_subs()->spl_sub, &local_sp);
-
-	if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
-		mavlink_msg_local_position_setpoint_send(l->mavlink->get_chan(),
-				MAV_FRAME_LOCAL_NED,
-				local_sp.x,
-				local_sp.y,
-				local_sp.z,
-				local_sp.yaw);
-}
-
-void
-MavlinkOrbListener::l_attitude_setpoint(const struct listener *l)
-{
-	struct vehicle_attitude_setpoint_s att_sp;
-
-	/* copy local position data into local buffer */
-	orb_copy(ORB_ID(vehicle_attitude_setpoint), l->mavlink->get_subs()->spa_sub, &att_sp);
-
-	if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
-		mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(l->mavlink->get_chan(),
-				att_sp.timestamp / 1000,
-				att_sp.roll_body,
-				att_sp.pitch_body,
-				att_sp.yaw_body,
-				att_sp.thrust);
-}
-
-void
-MavlinkOrbListener::l_vehicle_rates_setpoint(const struct listener *l)
-{
-	struct vehicle_rates_setpoint_s rates_sp;
-
-	/* copy local position data into local buffer */
-	orb_copy(ORB_ID(vehicle_rates_setpoint), l->mavlink->get_subs()->rates_setpoint_sub, &rates_sp);
-
-	if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
-		mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(l->mavlink->get_chan(),
-				rates_sp.timestamp / 1000,
-				rates_sp.roll,
-				rates_sp.pitch,
-				rates_sp.yaw,
-				rates_sp.thrust);
-}
-
-void
-MavlinkOrbListener::l_actuator_outputs(const struct listener *l)
-{
-	struct actuator_outputs_s act_outputs;
-
-	orb_id_t ids[] = {
-		ORB_ID(actuator_outputs_0),
-		ORB_ID(actuator_outputs_1),
-		ORB_ID(actuator_outputs_2),
-		ORB_ID(actuator_outputs_3)
-	};
-
-	/* copy actuator data into local buffer */
-	orb_copy(ids[l->arg], *l->subp, &act_outputs);
-
-	if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) {
-		mavlink_msg_servo_output_raw_send(l->mavlink->get_chan(), l->listener->last_sensor_timestamp / 1000,
-						  l->arg /* port number - needs GCS support */,
-							 /* QGC has port number support already */
-						  act_outputs.output[0],
-						  act_outputs.output[1],
-						  act_outputs.output[2],
-						  act_outputs.output[3],
-						  act_outputs.output[4],
-						  act_outputs.output[5],
-						  act_outputs.output[6],
-						  act_outputs.output[7]);
-
-		/* only send in HIL mode and only send first group for HIL */
-		if (l->mavlink->get_hil_enabled() && l->listener->armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) {
-
-			/* translate the current syste state to mavlink state and mode */
-			uint8_t mavlink_state = 0;
-			uint8_t mavlink_base_mode = 0;
-			uint32_t mavlink_custom_mode = 0;
-			l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
-
-			/* HIL message as per MAVLink spec */
-
-			/* scale / assign outputs depending on system type */
-
-			if (mavlink_system.type == MAV_TYPE_QUADROTOR) {
-				mavlink_msg_hil_controls_send(l->mavlink->get_chan(),
-							      hrt_absolute_time(),
-							      ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
-							      -1,
-							      -1,
-							      -1,
-							      -1,
-							      mavlink_base_mode,
-							      0);
-
-			} else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
-				mavlink_msg_hil_controls_send(l->mavlink->get_chan(),
-							      hrt_absolute_time(),
-							      ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
-							      -1,
-							      -1,
-							      mavlink_base_mode,
-							      0);
-
-			} else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
-				mavlink_msg_hil_controls_send(l->mavlink->get_chan(),
-							      hrt_absolute_time(),
-							      ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f,
-							      mavlink_base_mode,
-							      0);
-
-			} else {
-				mavlink_msg_hil_controls_send(l->mavlink->get_chan(),
-							      hrt_absolute_time(),
-							      (act_outputs.output[0] - 1500.0f) / 500.0f,
-							      (act_outputs.output[1] - 1500.0f) / 500.0f,
-							      (act_outputs.output[2] - 1500.0f) / 500.0f,
-							      (act_outputs.output[3] - 1000.0f) / 1000.0f,
-							      (act_outputs.output[4] - 1500.0f) / 500.0f,
-							      (act_outputs.output[5] - 1500.0f) / 500.0f,
-							      (act_outputs.output[6] - 1500.0f) / 500.0f,
-							      (act_outputs.output[7] - 1500.0f) / 500.0f,
-							      mavlink_base_mode,
-							      0);
-			}
-		}
-	}
-}
-
-void
-MavlinkOrbListener::l_actuator_armed(const struct listener *l)
-{
-	orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed);
-}
-
-void
-MavlinkOrbListener::l_manual_control_setpoint(const struct listener *l)
-{
-	struct manual_control_setpoint_s man_control;
-
-	/* copy manual control data into local buffer */
-	orb_copy(ORB_ID(manual_control_setpoint), l->mavlink->get_subs()->man_control_sp_sub, &man_control);
-
-	if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
-		mavlink_msg_manual_control_send(l->mavlink->get_chan(),
-						mavlink_system.sysid,
-						man_control.roll * 1000,
-						man_control.pitch * 1000,
-						man_control.yaw * 1000,
-						man_control.throttle * 1000,
-						0);
-}
-
-void
-MavlinkOrbListener::l_vehicle_attitude_controls(const struct listener *l)
-{
-	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, l->mavlink->get_subs()->actuators_sub, &l->listener->actuators_0);
-
-	if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) {
-		/* send, add spaces so that string buffer is at least 10 chars long */
-		mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
-						   l->listener->last_sensor_timestamp / 1000,
-						   "ctrl0    ",
-						   l->listener->actuators_0.control[0]);
-		mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
-						   l->listener->last_sensor_timestamp / 1000,
-						   "ctrl1    ",
-						   l->listener->actuators_0.control[1]);
-		mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
-						   l->listener->last_sensor_timestamp / 1000,
-						   "ctrl2     ",
-						   l->listener->actuators_0.control[2]);
-		mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
-						   l->listener->last_sensor_timestamp / 1000,
-						   "ctrl3     ",
-						   l->listener->actuators_0.control[3]);
-	}
-}
-
-void
-MavlinkOrbListener::l_debug_key_value(const struct listener *l)
-{
-	struct debug_key_value_s debug;
-
-	orb_copy(ORB_ID(debug_key_value), l->mavlink->get_subs()->debug_key_value, &debug);
-
-	/* Enforce null termination */
-	debug.key[sizeof(debug.key) - 1] = '\0';
-
-	mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
-					   l->listener->last_sensor_timestamp / 1000,
-					   debug.key,
-					   debug.value);
-}
-
-void
-MavlinkOrbListener::l_optical_flow(const struct listener *l)
-{
-	struct optical_flow_s flow;
-
-	orb_copy(ORB_ID(optical_flow), l->mavlink->get_subs()->optical_flow, &flow);
-
-	mavlink_msg_optical_flow_send(l->mavlink->get_chan(), 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);
-}
-
-void
-MavlinkOrbListener::l_home(const struct listener *l)
-{
-	orb_copy(ORB_ID(home_position), l->mavlink->get_subs()->home_sub, &l->listener->home);
-
-	mavlink_msg_gps_global_origin_send(l->mavlink->get_chan(), (int32_t)(l->listener->home.lat*1e7d), (int32_t)(l->listener->home.lon*1e7d), (int32_t)(l->listener->home.alt)*1e3f);
-}
-
-void
-MavlinkOrbListener::l_airspeed(const struct listener *l)
-{
-	orb_copy(ORB_ID(airspeed), l->mavlink->get_subs()->airspeed_sub, &l->listener->airspeed);
-}
-
-void
-MavlinkOrbListener::l_nav_cap(const struct listener *l)
-{
-
-	orb_copy(ORB_ID(navigation_capabilities), l->mavlink->get_subs()->navigation_capabilities_sub, &l->listener->nav_cap);
-
-	mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
-				   hrt_absolute_time() / 1000,
-				   "turn dist",
-				   l->listener->nav_cap.turn_distance);
-
-}
+//
+//void
+//MavlinkOrbListener::l_rc_channels(const struct listener *l)
+//{
+//	/* copy rc channels into local buffer */
+//	orb_copy(ORB_ID(rc_channels), l->mavlink->get_subs()->rc_sub, &l->listener->rc);
+//	// XXX Add RC channels scaled message here
+//}
+//
+//void
+//MavlinkOrbListener::l_input_rc(const struct listener *l)
+//{
+//	/* copy rc _mavlink->get_chan()nels into local buffer */
+//	orb_copy(ORB_ID(input_rc), l->mavlink->get_subs()->input_rc_sub, &l->listener->rc_raw);
+//
+//	if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) {
+//
+//		const unsigned port_width = 8;
+//
+//		for (unsigned i = 0; (i * port_width) < l->listener->rc_raw.channel_count; i++) {
+//			/* Channels are sent in MAVLink main loop at a fixed interval */
+//			mavlink_msg_rc_channels_raw_send(l->mavlink->get_chan(),
+//							 l->listener->rc_raw.timestamp_publication / 1000,
+//							 i,
+//							 (l->listener->rc_raw.channel_count > (i * port_width) + 0) ? l->listener->rc_raw.values[(i * port_width) + 0] : UINT16_MAX,
+//							 (l->listener->rc_raw.channel_count > (i * port_width) + 1) ? l->listener->rc_raw.values[(i * port_width) + 1] : UINT16_MAX,
+//							 (l->listener->rc_raw.channel_count > (i * port_width) + 2) ? l->listener->rc_raw.values[(i * port_width) + 2] : UINT16_MAX,
+//							 (l->listener->rc_raw.channel_count > (i * port_width) + 3) ? l->listener->rc_raw.values[(i * port_width) + 3] : UINT16_MAX,
+//							 (l->listener->rc_raw.channel_count > (i * port_width) + 4) ? l->listener->rc_raw.values[(i * port_width) + 4] : UINT16_MAX,
+//							 (l->listener->rc_raw.channel_count > (i * port_width) + 5) ? l->listener->rc_raw.values[(i * port_width) + 5] : UINT16_MAX,
+//							 (l->listener->rc_raw.channel_count > (i * port_width) + 6) ? l->listener->rc_raw.values[(i * port_width) + 6] : UINT16_MAX,
+//							 (l->listener->rc_raw.channel_count > (i * port_width) + 7) ? l->listener->rc_raw.values[(i * port_width) + 7] : UINT16_MAX,
+//							  l->listener->rc_raw.rssi);
+//		}
+//	}
+//}
+//
+//void
+//MavlinkOrbListener::l_global_position(const struct listener *l)
+//{
+//	/* copy global position data into local buffer */
+//	orb_copy(ORB_ID(vehicle_global_position), l->mavlink->get_subs()->global_pos_sub, &l->listener->global_pos);
+//
+//	mavlink_msg_global_position_int_send(l->mavlink->get_chan(),
+//						 l->listener->global_pos.timestamp / 1000,
+//					     l->listener->global_pos.lat * 1e7,
+//					     l->listener->global_pos.lon * 1e7,
+//					     l->listener->global_pos.alt * 1000.0f,
+//					     (l->listener->global_pos.alt - l->listener->home.alt) * 1000.0f,
+//					     l->listener->global_pos.vel_n * 100.0f,
+//					     l->listener->global_pos.vel_e * 100.0f,
+//					     l->listener->global_pos.vel_d * 100.0f,
+//					     _wrap_2pi(l->listener->global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f);
+//}
+//
+//void
+//MavlinkOrbListener::l_local_position(const struct listener *l)
+//{
+//	/* copy local position data into local buffer */
+//	orb_copy(ORB_ID(vehicle_local_position), l->mavlink->get_subs()->local_pos_sub, &l->listener->local_pos);
+//
+//	if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
+//		mavlink_msg_local_position_ned_send(l->mavlink->get_chan(),
+//						    l->listener->local_pos.timestamp / 1000,
+//						    l->listener->local_pos.x,
+//						    l->listener->local_pos.y,
+//						    l->listener->local_pos.z,
+//						    l->listener->local_pos.vx,
+//						    l->listener->local_pos.vy,
+//						    l->listener->local_pos.vz);
+//}
+//
+//void
+//MavlinkOrbListener::l_global_position_setpoint(const struct listener *l)
+//{
+//	struct position_setpoint_triplet_s triplet;
+//	orb_copy(ORB_ID(position_setpoint_triplet), l->mavlink->get_subs()->triplet_sub, &triplet);
+//
+//	if (!triplet.current.valid)
+//		return;
+//
+//	if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
+//		mavlink_msg_global_position_setpoint_int_send(l->mavlink->get_chan(),
+//				MAV_FRAME_GLOBAL,
+//				(int32_t)(triplet.current.lat * 1e7d),
+//				(int32_t)(triplet.current.lon * 1e7d),
+//				(int32_t)(triplet.current.alt * 1e3f),
+//				(int16_t)(triplet.current.yaw * M_RAD_TO_DEG_F * 1e2f));
+//}
+//
+//void
+//MavlinkOrbListener::l_local_position_setpoint(const struct listener *l)
+//{
+//	struct vehicle_local_position_setpoint_s local_sp;
+//
+//	/* copy local position data into local buffer */
+//	orb_copy(ORB_ID(vehicle_local_position_setpoint), l->mavlink->get_subs()->spl_sub, &local_sp);
+//
+//	if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
+//		mavlink_msg_local_position_setpoint_send(l->mavlink->get_chan(),
+//				MAV_FRAME_LOCAL_NED,
+//				local_sp.x,
+//				local_sp.y,
+//				local_sp.z,
+//				local_sp.yaw);
+//}
+//
+//void
+//MavlinkOrbListener::l_attitude_setpoint(const struct listener *l)
+//{
+//	struct vehicle_attitude_setpoint_s att_sp;
+//
+//	/* copy local position data into local buffer */
+//	orb_copy(ORB_ID(vehicle_attitude_setpoint), l->mavlink->get_subs()->spa_sub, &att_sp);
+//
+//	if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
+//		mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(l->mavlink->get_chan(),
+//				att_sp.timestamp / 1000,
+//				att_sp.roll_body,
+//				att_sp.pitch_body,
+//				att_sp.yaw_body,
+//				att_sp.thrust);
+//}
+//
+//void
+//MavlinkOrbListener::l_vehicle_rates_setpoint(const struct listener *l)
+//{
+//	struct vehicle_rates_setpoint_s rates_sp;
+//
+//	/* copy local position data into local buffer */
+//	orb_copy(ORB_ID(vehicle_rates_setpoint), l->mavlink->get_subs()->rates_setpoint_sub, &rates_sp);
+//
+//	if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
+//		mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(l->mavlink->get_chan(),
+//				rates_sp.timestamp / 1000,
+//				rates_sp.roll,
+//				rates_sp.pitch,
+//				rates_sp.yaw,
+//				rates_sp.thrust);
+//}
+//
+//void
+//MavlinkOrbListener::l_actuator_outputs(const struct listener *l)
+//{
+//	struct actuator_outputs_s act_outputs;
+//
+//	orb_id_t ids[] = {
+//		ORB_ID(actuator_outputs_0),
+//		ORB_ID(actuator_outputs_1),
+//		ORB_ID(actuator_outputs_2),
+//		ORB_ID(actuator_outputs_3)
+//	};
+//
+//	/* copy actuator data into local buffer */
+//	orb_copy(ids[l->arg], *l->subp, &act_outputs);
+//
+//	if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) {
+//		mavlink_msg_servo_output_raw_send(l->mavlink->get_chan(), l->listener->last_sensor_timestamp / 1000,
+//						  l->arg /* port number - needs GCS support */,
+//							 /* QGC has port number support already */
+//						  act_outputs.output[0],
+//						  act_outputs.output[1],
+//						  act_outputs.output[2],
+//						  act_outputs.output[3],
+//						  act_outputs.output[4],
+//						  act_outputs.output[5],
+//						  act_outputs.output[6],
+//						  act_outputs.output[7]);
+//
+//		/* only send in HIL mode and only send first group for HIL */
+//		if (l->mavlink->get_hil_enabled() && l->listener->armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) {
+//
+//			/* translate the current syste state to mavlink state and mode */
+//			uint8_t mavlink_state = 0;
+//			uint8_t mavlink_base_mode = 0;
+//			uint32_t mavlink_custom_mode = 0;
+//			l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
+//
+//			/* HIL message as per MAVLink spec */
+//
+//			/* scale / assign outputs depending on system type */
+//
+//			if (mavlink_system.type == MAV_TYPE_QUADROTOR) {
+//				mavlink_msg_hil_controls_send(l->mavlink->get_chan(),
+//							      hrt_absolute_time(),
+//							      ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
+//							      ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
+//							      ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
+//							      ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
+//							      -1,
+//							      -1,
+//							      -1,
+//							      -1,
+//							      mavlink_base_mode,
+//							      0);
+//
+//			} else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
+//				mavlink_msg_hil_controls_send(l->mavlink->get_chan(),
+//							      hrt_absolute_time(),
+//							      ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
+//							      ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
+//							      ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
+//							      ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
+//							      ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f,
+//							      ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
+//							      -1,
+//							      -1,
+//							      mavlink_base_mode,
+//							      0);
+//
+//			} else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
+//				mavlink_msg_hil_controls_send(l->mavlink->get_chan(),
+//							      hrt_absolute_time(),
+//							      ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
+//							      ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
+//							      ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
+//							      ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
+//							      ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f,
+//							      ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
+//							      ((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f,
+//							      ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f,
+//							      mavlink_base_mode,
+//							      0);
+//
+//			} else {
+//				mavlink_msg_hil_controls_send(l->mavlink->get_chan(),
+//							      hrt_absolute_time(),
+//							      (act_outputs.output[0] - 1500.0f) / 500.0f,
+//							      (act_outputs.output[1] - 1500.0f) / 500.0f,
+//							      (act_outputs.output[2] - 1500.0f) / 500.0f,
+//							      (act_outputs.output[3] - 1000.0f) / 1000.0f,
+//							      (act_outputs.output[4] - 1500.0f) / 500.0f,
+//							      (act_outputs.output[5] - 1500.0f) / 500.0f,
+//							      (act_outputs.output[6] - 1500.0f) / 500.0f,
+//							      (act_outputs.output[7] - 1500.0f) / 500.0f,
+//							      mavlink_base_mode,
+//							      0);
+//			}
+//		}
+//	}
+//}
+//
+//void
+//MavlinkOrbListener::l_actuator_armed(const struct listener *l)
+//{
+//	orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed);
+//}
+//
+//void
+//MavlinkOrbListener::l_manual_control_setpoint(const struct listener *l)
+//{
+//	struct manual_control_setpoint_s man_control;
+//
+//	/* copy manual control data into local buffer */
+//	orb_copy(ORB_ID(manual_control_setpoint), l->mavlink->get_subs()->man_control_sp_sub, &man_control);
+//
+//	if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
+//		mavlink_msg_manual_control_send(l->mavlink->get_chan(),
+//						mavlink_system.sysid,
+//						man_control.roll * 1000,
+//						man_control.pitch * 1000,
+//						man_control.yaw * 1000,
+//						man_control.throttle * 1000,
+//						0);
+//}
+//
+//void
+//MavlinkOrbListener::l_vehicle_attitude_controls(const struct listener *l)
+//{
+//	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, l->mavlink->get_subs()->actuators_sub, &l->listener->actuators_0);
+//
+//	if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) {
+//		/* send, add spaces so that string buffer is at least 10 chars long */
+//		mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
+//						   l->listener->last_sensor_timestamp / 1000,
+//						   "ctrl0    ",
+//						   l->listener->actuators_0.control[0]);
+//		mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
+//						   l->listener->last_sensor_timestamp / 1000,
+//						   "ctrl1    ",
+//						   l->listener->actuators_0.control[1]);
+//		mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
+//						   l->listener->last_sensor_timestamp / 1000,
+//						   "ctrl2     ",
+//						   l->listener->actuators_0.control[2]);
+//		mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
+//						   l->listener->last_sensor_timestamp / 1000,
+//						   "ctrl3     ",
+//						   l->listener->actuators_0.control[3]);
+//	}
+//}
+//
+//void
+//MavlinkOrbListener::l_debug_key_value(const struct listener *l)
+//{
+//	struct debug_key_value_s debug;
+//
+//	orb_copy(ORB_ID(debug_key_value), l->mavlink->get_subs()->debug_key_value, &debug);
+//
+//	/* Enforce null termination */
+//	debug.key[sizeof(debug.key) - 1] = '\0';
+//
+//	mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
+//					   l->listener->last_sensor_timestamp / 1000,
+//					   debug.key,
+//					   debug.value);
+//}
+//
+//void
+//MavlinkOrbListener::l_optical_flow(const struct listener *l)
+//{
+//	struct optical_flow_s flow;
+//
+//	orb_copy(ORB_ID(optical_flow), l->mavlink->get_subs()->optical_flow, &flow);
+//
+//	mavlink_msg_optical_flow_send(l->mavlink->get_chan(), 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);
+//}
+//
+//void
+//MavlinkOrbListener::l_home(const struct listener *l)
+//{
+//	orb_copy(ORB_ID(home_position), l->mavlink->get_subs()->home_sub, &l->listener->home);
+//
+//	mavlink_msg_gps_global_origin_send(l->mavlink->get_chan(), (int32_t)(l->listener->home.lat*1e7d), (int32_t)(l->listener->home.lon*1e7d), (int32_t)(l->listener->home.alt)*1e3f);
+//}
+//
+//void
+//MavlinkOrbListener::l_airspeed(const struct listener *l)
+//{
+//	orb_copy(ORB_ID(airspeed), l->mavlink->get_subs()->airspeed_sub, &l->listener->airspeed);
+//}
+//
+//void
+//MavlinkOrbListener::l_nav_cap(const struct listener *l)
+//{
+//
+//	orb_copy(ORB_ID(navigation_capabilities), l->mavlink->get_subs()->navigation_capabilities_sub, &l->listener->nav_cap);
+//
+//	mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
+//				   hrt_absolute_time() / 1000,
+//				   "turn dist",
+//				   l->listener->nav_cap.turn_distance);
+//
+//}
 
 void *
 MavlinkOrbListener::uorb_receive_thread(void *arg)
 {
-	/* Set thread name */
+	/* set thread name */
 	char thread_name[18];
 	sprintf(thread_name, "mavlink_uorb_rcv_%d", _mavlink->get_channel());
 	prctl(PR_SET_NAME, thread_name, getpid());
 
-	/*
-	 * set up poll to block for new data,
-	 * wait for a maximum of 1000 ms (1 second)
-	 */
-	const int timeout = 1000;
-
-	/*
-	 * Initialise listener array.
-	 *
-	 * Might want to invoke each listener once to set initial state.
-	 */
-	struct pollfd fds[_max_listeners];
+	/* add mavlink streams */
+	/* common buffer for topics and data sizes */
+	const struct orb_metadata *topics[1];
+	size_t sizes[1];
 
-	struct listener* next = _listeners;
-	unsigned i = 0;
-	while (next != nullptr) {
-
-		fds[i].fd = *next->subp;
-		fds[i].events = POLLIN;
-
-		next = next->next;
-		i++;
-	}
-		/* Invoke callback to set initial state */
-		//listeners[i].callback(&listener[i]);
+	/* --- HEARTBEAT --- */
+	topics[0] = ORB_ID(vehicle_status);
+	sizes[0] = sizeof(vehicle_status_s);
+	add_stream(msg_heartbeat, 1, topics, sizes, 0, 500);
 
 	while (!_mavlink->_task_should_exit) {
-
-		int poll_ret = poll(fds, _n_listeners, timeout);
-
-		/* handle the poll result */
-		if (poll_ret == 0) {
-			/* silent */
-
-		} else if (poll_ret < 0) {
-			//mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data");
-
-		} else {
-
-			i = 0;
-			struct listener* cb = _listeners;
-			while (cb != nullptr) {
-
-				if (fds[i].revents & POLLIN)
-					cb->callback(cb);
-
-				cb = cb->next;
-				i++;
-			}
+		/* check all streams each 1ms */
+		hrt_abstime t = hrt_absolute_time();
+		MavlinkStream *stream;
+		LL_FOREACH(_streams, stream) {
+			stream->update(t);
 		}
+		usleep(1000);
 	}
 
 	return NULL;
@@ -715,105 +664,6 @@ void * MavlinkOrbListener::uorb_start_helper(void *context)
 pthread_t
 MavlinkOrbListener::uorb_receive_start(Mavlink* mavlink)
 {
-
-	/* --- SENSORS RAW VALUE --- */
-	mavlink->get_subs()->sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
-	/* rate limit set externally based on interface speed, set a basic default here */
-	orb_set_interval(mavlink->get_subs()->sensor_sub, 200);	/* 5Hz updates */
-
-	/* --- ATTITUDE VALUE --- */
-	mavlink->get_subs()->att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	/* rate limit set externally based on interface speed, set a basic default here */
-	orb_set_interval(mavlink->get_subs()->att_sub, 200);	/* 5Hz updates */
-
-	/* --- GPS VALUE --- */
-	mavlink->get_subs()->gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
-	orb_set_interval(mavlink->get_subs()->gps_sub, 200);	/* 5Hz updates */
-
-	/* --- HOME POSITION --- */
-	mavlink->get_subs()->home_sub = orb_subscribe(ORB_ID(home_position));
-	orb_set_interval(mavlink->get_subs()->home_sub, 1000);	/* 1Hz updates */
-
-	/* --- SYSTEM STATE --- */
-	mavlink->get_subs()->status_sub = orb_subscribe(ORB_ID(vehicle_status));
-	orb_set_interval(mavlink->get_subs()->status_sub, 300);		/* max 3.33 Hz updates */
-
-	/* --- POSITION SETPOINT TRIPLET --- */
-	mavlink->get_subs()->position_setpoint_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
-	orb_set_interval(mavlink->get_subs()->position_setpoint_triplet_sub, 0);		/* not polled, don't limit */
-
-	/* --- RC CHANNELS VALUE --- */
-	mavlink->get_subs()->rc_sub = orb_subscribe(ORB_ID(rc_channels));
-	orb_set_interval(mavlink->get_subs()->rc_sub, 100);			/* 10Hz updates */
-
-	/* --- RC RAW VALUE --- */
-	mavlink->get_subs()->input_rc_sub = orb_subscribe(ORB_ID(input_rc));
-	orb_set_interval(mavlink->get_subs()->input_rc_sub, 100);
-
-	/* --- GLOBAL POS VALUE --- */
-	mavlink->get_subs()->global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	orb_set_interval(mavlink->get_subs()->global_pos_sub, 100);	/* 10 Hz active updates */
-
-	/* --- LOCAL POS VALUE --- */
-	mavlink->get_subs()->local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
-	orb_set_interval(mavlink->get_subs()->local_pos_sub, 1000);	/* 1Hz active updates */
-
-	/* --- GLOBAL SETPOINT VALUE --- */
-	mavlink->get_subs()->triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
-	orb_set_interval(mavlink->get_subs()->triplet_sub, 2000);	/* 0.5 Hz updates */
-	
-	/* --- LOCAL SETPOINT VALUE --- */
-	mavlink->get_subs()->spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
-	orb_set_interval(mavlink->get_subs()->spl_sub, 2000);	/* 0.5 Hz updates */
-
-	/* --- ATTITUDE SETPOINT VALUE --- */
-	mavlink->get_subs()->spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
-	orb_set_interval(mavlink->get_subs()->spa_sub, 2000);	/* 0.5 Hz updates */
-
-	/* --- RATES SETPOINT VALUE --- */
-	mavlink->get_subs()->rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
-	orb_set_interval(mavlink->get_subs()->rates_setpoint_sub, 2000);  /* 0.5 Hz updates */
-
-	/* --- ACTUATOR OUTPUTS --- */
-	mavlink->get_subs()->act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0));
-	mavlink->get_subs()->act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1));
-	mavlink->get_subs()->act_2_sub = orb_subscribe(ORB_ID(actuator_outputs_2));
-	mavlink->get_subs()->act_3_sub = orb_subscribe(ORB_ID(actuator_outputs_3));
-	/* rate limits set externally based on interface speed, set a basic default here */
-	orb_set_interval(mavlink->get_subs()->act_0_sub, 100);	/* 10Hz updates */
-	orb_set_interval(mavlink->get_subs()->act_1_sub, 100);	/* 10Hz updates */
-	orb_set_interval(mavlink->get_subs()->act_2_sub, 100);	/* 10Hz updates */
-	orb_set_interval(mavlink->get_subs()->act_3_sub, 100);	/* 10Hz updates */
-
-	/* --- ACTUATOR ARMED VALUE --- */
-	mavlink->get_subs()->armed_sub = orb_subscribe(ORB_ID(actuator_armed));
-	orb_set_interval(mavlink->get_subs()->armed_sub, 100);	/* 10Hz updates */
-
-	/* --- MAPPED MANUAL CONTROL INPUTS --- */
-	mavlink->get_subs()->man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	/* rate limits set externally based on interface speed, set a basic default here */
-	orb_set_interval(mavlink->get_subs()->man_control_sp_sub, 100);	/* 10Hz updates */
-
-	/* --- ACTUATOR CONTROL VALUE --- */
-	mavlink->get_subs()->actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
-	orb_set_interval(mavlink->get_subs()->actuators_sub, 100);	/* 10Hz updates */
-
-	/* --- DEBUG VALUE OUTPUT --- */
-	mavlink->get_subs()->debug_key_value = orb_subscribe(ORB_ID(debug_key_value));
-	orb_set_interval(mavlink->get_subs()->debug_key_value, 100);	/* 10Hz updates */
-
-	/* --- FLOW SENSOR --- */
-	mavlink->get_subs()->optical_flow = orb_subscribe(ORB_ID(optical_flow));
-	orb_set_interval(mavlink->get_subs()->optical_flow, 200); 	/* 5Hz updates */
-
-	/* --- AIRSPEED --- */
-	mavlink->get_subs()->airspeed_sub = orb_subscribe(ORB_ID(airspeed));
-	orb_set_interval(mavlink->get_subs()->airspeed_sub, 200); 	/* 5Hz updates */
-
-	/* --- NAVIGATION CAPABILITIES --- */
-	mavlink->get_subs()->navigation_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
-	orb_set_interval(mavlink->get_subs()->navigation_capabilities_sub, 500); 	/* 2Hz updates */
-
 	/* start the listener loop */
 	pthread_attr_t uorb_attr;
 	pthread_attr_init(&uorb_attr);
diff --git a/src/modules/mavlink/mavlink_orb_listener.h b/src/modules/mavlink/mavlink_orb_listener.h
index 26a2e832f..1a103e43d 100644
--- a/src/modules/mavlink/mavlink_orb_listener.h
+++ b/src/modules/mavlink/mavlink_orb_listener.h
@@ -40,9 +40,12 @@
  */
 
 #include <systemlib/perf_counter.h>
+#include <pthread.h>
 
 #pragma once
 
+#include <drivers/drv_rc_input.h>
+
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/rc_channels.h>
 #include <uORB/topics/vehicle_attitude.h>
@@ -68,11 +71,14 @@
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/position_setpoint_triplet.h>
-#include <drivers/drv_rc_input.h>
 #include <uORB/topics/navigation_capabilities.h>
 #include <uORB/topics/mission.h>
 
+#include "mavlink_orb_subscription.h"
+#include "mavlink_stream.h"
+
 class Mavlink;
+class MavlinkStream;
 
 class MavlinkOrbListener
 {
@@ -83,22 +89,14 @@ public:
 	MavlinkOrbListener(Mavlink* parent);
 
 	/**
-	 * Destructor, also kills the mavlinks task.
+	 * Destructor, closes all subscriptions.
 	 */
 	~MavlinkOrbListener();
 
 	static pthread_t uorb_receive_start(Mavlink *mavlink);
 
-	struct listener {
-		void	(*callback)(const struct listener *l);
-		int		*subp;
-		uintptr_t	arg;
-		struct listener *next;
-		Mavlink		*mavlink;
-		MavlinkOrbListener* listener;
-	};
-
-	void add_listener(void (*callback)(const struct listener *l), int *subp, uintptr_t arg);
+	MavlinkOrbSubscription *add_subscription(const struct orb_metadata *meta, size_t size, const MavlinkStream *stream, const unsigned int interval);
+	void add_stream(void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **metas, const size_t *sizes, const uintptr_t arg, const unsigned int interval);
 	static void * uorb_start_helper(void *context);
 
 private:
@@ -107,57 +105,34 @@ private:
 
 	Mavlink*	_mavlink;
 
-	struct listener *_listeners;
-	unsigned	_n_listeners;
-	static const unsigned _max_listeners = 32;
+	MavlinkOrbSubscription *_subscriptions;
+	static const unsigned _max_subscriptions = 32;
+	MavlinkStream *_streams;
 
 	void *uorb_receive_thread(void *arg);
 
-	static void		l_sensor_combined(const struct listener *l);
-	static void		l_vehicle_attitude(const struct listener *l);
-	static void		l_vehicle_gps_position(const struct listener *l);
-	static void		l_vehicle_status(const struct listener *l);
-	static void		l_rc_channels(const struct listener *l);
-	static void		l_input_rc(const struct listener *l);
-	static void		l_global_position(const struct listener *l);
-	static void		l_local_position(const struct listener *l);
-	static void		l_global_position_setpoint(const struct listener *l);
-	static void		l_local_position_setpoint(const struct listener *l);
-	static void		l_attitude_setpoint(const struct listener *l);
-	static void		l_actuator_outputs(const struct listener *l);
-	static void		l_actuator_armed(const struct listener *l);
-	static void		l_manual_control_setpoint(const struct listener *l);
-	static void		l_vehicle_attitude_controls(const struct listener *l);
-	static void		l_debug_key_value(const struct listener *l);
-	static void		l_optical_flow(const struct listener *l);
-	static void		l_vehicle_rates_setpoint(const struct listener *l);
-	static void		l_home(const struct listener *l);
-	static void		l_airspeed(const struct listener *l);
-	static void		l_nav_cap(const struct listener *l);
-
-	struct vehicle_global_position_s global_pos;
-	struct vehicle_local_position_s local_pos;
-	struct navigation_capabilities_s nav_cap;
-	struct vehicle_status_s v_status;
-	struct rc_channels_s rc;
-	struct rc_input_values rc_raw;
-	struct actuator_armed_s armed;
-	struct actuator_controls_s actuators_0;
-	struct vehicle_attitude_s att;
-	struct airspeed_s airspeed;
-	struct home_position_s home;
-
-	unsigned int sensors_raw_counter;
-	unsigned int attitude_counter;
-	unsigned int gps_counter;
-
-	/*
-	 * Last sensor loop time
-	 * some outputs are better timestamped
-	 * with this "global" reference.
-	 */
-	uint64_t last_sensor_timestamp;
-
-	hrt_abstime last_sent_vfr;
+	static void msg_heartbeat(const MavlinkStream *stream);
+
+//	static void		l_sensor_combined(const struct listener *l);
+//	static void		l_vehicle_attitude(const struct listener *l);
+//	static void		l_vehicle_gps_position(const struct listener *l);
+//	static void		l_vehicle_status(const struct listener *l);
+//	static void		l_rc_channels(const struct listener *l);
+//	static void		l_input_rc(const struct listener *l);
+//	static void		l_global_position(const struct listener *l);
+//	static void		l_local_position(const struct listener *l);
+//	static void		l_global_position_setpoint(const struct listener *l);
+//	static void		l_local_position_setpoint(const struct listener *l);
+//	static void		l_attitude_setpoint(const struct listener *l);
+//	static void		l_actuator_outputs(const struct listener *l);
+//	static void		l_actuator_armed(const struct listener *l);
+//	static void		l_manual_control_setpoint(const struct listener *l);
+//	static void		l_vehicle_attitude_controls(const struct listener *l);
+//	static void		l_debug_key_value(const struct listener *l);
+//	static void		l_optical_flow(const struct listener *l);
+//	static void		l_vehicle_rates_setpoint(const struct listener *l);
+//	static void		l_home(const struct listener *l);
+//	static void		l_airspeed(const struct listener *l);
+//	static void		l_nav_cap(const struct listener *l);
 
 };
diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp
new file mode 100644
index 000000000..182407a5e
--- /dev/null
+++ b/src/modules/mavlink/mavlink_orb_subscription.cpp
@@ -0,0 +1,47 @@
+/*
+ * mavlink_orb_subscription.cpp
+ *
+ *  Created on: 23.02.2014
+ *      Author: ton
+ */
+
+
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <uORB/uORB.h>
+
+#include "mavlink_orb_subscription.h"
+
+MavlinkOrbSubscription::MavlinkOrbSubscription(const struct orb_metadata *meta, size_t size)
+{
+	this->meta = meta;
+	this->data = malloc(size);
+	memset(this->data, 0, size);
+	this->fd = orb_subscribe(meta);
+	this->last_update = 0;
+	this->interval = 0;
+}
+
+MavlinkOrbSubscription::~MavlinkOrbSubscription()
+{
+	close(fd);
+	free(data);
+}
+
+int MavlinkOrbSubscription::set_interval(const unsigned int interval)
+{
+	this->interval = interval;
+	return orb_set_interval(fd, interval);
+}
+
+int MavlinkOrbSubscription::update(const hrt_abstime t)
+{
+	if (last_update != t) {
+		bool updated;
+		orb_check(fd, &updated);
+		if (updated)
+			return orb_copy(meta, fd, &data);
+	}
+	return OK;
+}
diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h
new file mode 100644
index 000000000..2c72abaef
--- /dev/null
+++ b/src/modules/mavlink/mavlink_orb_subscription.h
@@ -0,0 +1,31 @@
+/*
+ * mavlink_orb_subscription.h
+ *
+ *  Created on: 23.02.2014
+ *      Author: ton
+ */
+
+#ifndef MAVLINK_ORB_SUBSCRIPTION_H_
+#define MAVLINK_ORB_SUBSCRIPTION_H_
+
+#include <systemlib/uthash/utlist.h>
+#include <drivers/drv_hrt.h>
+
+class MavlinkOrbSubscription {
+public:
+	MavlinkOrbSubscription(const struct orb_metadata *meta, size_t size);
+	~MavlinkOrbSubscription();
+
+	int set_interval(const unsigned int interval);
+	int update(const hrt_abstime t);
+
+	const struct orb_metadata *meta;
+	int fd;
+	void *data;
+	hrt_abstime last_update;
+	unsigned int interval;
+	MavlinkOrbSubscription *next;
+};
+
+
+#endif /* MAVLINK_ORB_SUBSCRIPTION_H_ */
diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp
new file mode 100644
index 000000000..2a6728fdb
--- /dev/null
+++ b/src/modules/mavlink/mavlink_stream.cpp
@@ -0,0 +1,48 @@
+/*
+ * mavlink_stream.cpp
+ *
+ *  Created on: 24.02.2014
+ *      Author: ton
+ */
+
+#include <stdlib.h>
+
+#include "mavlink_orb_listener.h"
+
+MavlinkStream::MavlinkStream(MavlinkOrbListener *listener, void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **metas, const size_t *sizes, const uintptr_t arg, const unsigned int interval)
+{
+	this->callback = callback;
+	this->arg = arg;
+	this->interval = interval * 1000;
+	this->mavlink = mavlink;
+	this->listener = listener;
+	this->subscriptions_n = subs_n;
+	this->subscriptions = (MavlinkOrbSubscription **) malloc(subs_n * sizeof(MavlinkOrbSubscription *));
+
+	for (int i = 0; i < subs_n; i++) {
+		this->subscriptions[i] = listener->add_subscription(metas[i], sizes[i], this, interval);
+	}
+}
+
+MavlinkStream::~MavlinkStream()
+{
+	free(subscriptions);
+}
+
+/**
+ * Update mavlink stream, i.e. update subscriptions and send message if necessary
+ */
+int MavlinkStream::update(const hrt_abstime t)
+{
+	uint64_t dt = t - last_sent;
+	if (dt > 0 && dt >= interval) {
+		/* interval expired, update all subscriptions */
+		for (unsigned int i = 0; i < subscriptions_n; i++) {
+			subscriptions[i]->update(t);
+		}
+
+		/* format and send mavlink message */
+		callback(this);
+		last_sent = t;
+	}
+}
diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h
new file mode 100644
index 000000000..0f959d720
--- /dev/null
+++ b/src/modules/mavlink/mavlink_stream.h
@@ -0,0 +1,33 @@
+/*
+ * mavlink_stream.h
+ *
+ *  Created on: 24.02.2014
+ *      Author: ton
+ */
+
+#ifndef MAVLINK_STREAM_H_
+#define MAVLINK_STREAM_H_
+
+class Mavlink;
+class MavlinkOrbListener;
+class MavlinkOrbSubscription;
+
+class MavlinkStream {
+public:
+	void	(*callback)(const MavlinkStream *);
+	uintptr_t	arg;
+	unsigned int subscriptions_n;
+	MavlinkOrbSubscription **subscriptions;
+	hrt_abstime last_sent;
+	unsigned int interval;
+	MavlinkStream *next;
+	Mavlink		*mavlink;
+	MavlinkOrbListener* listener;
+
+	MavlinkStream(MavlinkOrbListener *listener, void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **metas, const size_t *sizes, const uintptr_t arg, const unsigned int interval);
+	~MavlinkStream();
+	int update(const hrt_abstime t);
+};
+
+
+#endif /* MAVLINK_STREAM_H_ */
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index 76798eb12..dc8d5586f 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -39,6 +39,8 @@ MODULE_COMMAND	 = mavlink
 SRCS		 += mavlink_main.cpp \
 			mavlink.c \
 			mavlink_receiver.cpp \
-			mavlink_orb_listener.cpp
+			mavlink_orb_listener.cpp \
+			mavlink_orb_subscription.cpp \
+			mavlink_stream.cpp
 
 INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink