Not building yet, things are coming together slowly on mavlink app

src/modules/mavlink/mavlink_main.cpp

@@ -56,7 +56,6 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/home_position.h>
-#include <uORB/topics/mission_item_triplet.h>
 #include <uORB/topics/mission_result.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_control_mode.h>
@@ -196,6 +195,11 @@ Mavlink::~Mavlink()
 	}
 }
 
+void Mavlink::set_mode(enum MAVLINK_MODE mode)
+{
+	_mode = mode;
+}
+
 int Mavlink::instance_count()
 {
 	/* note: a local buffer count will help if this ever is called often */
@@ -1506,7 +1510,7 @@ Mavlink::task_main(int argc, char *argv[])
 			break;
 
 		case 'o':
-			mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD;
+			_mode = MODE_ONBOARD;
 			break;
 
 		default:
@@ -1523,7 +1527,7 @@ Mavlink::task_main(int argc, char *argv[])
 	warnx("MAVLink v1.0 serial interface starting...");
 
 	/* inform about mode */
-	warnx((mavlink_link_mode == MAVLINK_INTERFACE_MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE");
+	warnx((_mode == MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE");
 
 	/* Flush stdout in case MAVLink is about to take it over */
 	fflush(stdout);
@@ -1541,12 +1545,12 @@ Mavlink::task_main(int argc, char *argv[])
 	mavlink_update_system();
 
 	/* start the MAVLink receiver */
-	MavlinkReceiver rcv(this);
-	receive_thread = rcv.receive_start(uart);
+	// MavlinkReceiver rcv(this);
+	receive_thread = MavlinkReceiver::receive_start(this);
 
 	/* start the ORB receiver */
-	MavlinkOrbListener listener(this);
-	uorb_receive_thread = listener.uorb_receive_start();
+	//MavlinkOrbListener listener(this);
+	uorb_receive_thread = MavlinkOrbListener::uorb_receive_start(this);
 
 	/* initialize waypoint manager */
 	mavlink_wpm_init(wpm);

src/modules/mavlink/mavlink_main.h

@@ -54,9 +54,7 @@
 #include <uORB/topics/offboard_control_setpoint.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
-#include <uORB/topics/mission_item_triplet.h>
 #include <uORB/topics/vehicle_vicon_position.h>
-#include <uORB/topics/mission_item_triplet.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_control_mode.h>
@@ -72,7 +70,8 @@
 #include <uORB/topics/battery_status.h>
 #include <drivers/drv_rc_input.h>
 #include <uORB/topics/navigation_capabilities.h>
-
+#include <uORB/topics/mission.h>
+#include <systemlib/param/param.h>
 
 #include <nuttx/fs/fs.h>
 
@@ -139,6 +138,7 @@ struct mavlink_wpm_storage {
 
 class Mavlink
 {
+
 public:
 	/**
 	 * Constructor
@@ -170,6 +170,31 @@ public:
 
 	static int get_uart_fd(unsigned index);
 
+	int get_uart_fd() { return _mavlink_fd; }
+
+	enum MAVLINK_MODE {
+		MODE_TX_HEARTBEAT_ONLY=0,
+		MODE_OFFBOARD,
+		MODE_ONBOARD,
+		MODE_HIL
+	};
+
+	void		set_mode(enum MAVLINK_MODE);
+	enum MAVLINK_MODE		get_mode();
+
+	bool		hil_enabled() { return _mavlink_hil_enabled; };
+
+	/**
+	 * Handle waypoint related messages.
+	 */
+	void mavlink_wpm_message_handler(const mavlink_message_t *msg);
+
+	/**
+	 * Handle parameter related messages.
+	 */
+	void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
+
+
 	struct mavlink_subscriptions {
 		int sensor_sub;
 		int att_sub;
@@ -196,10 +221,15 @@ public:
 		int airspeed_sub;
 		int navigation_capabilities_sub;
 		int control_mode_sub;
+		int rc_sub;
+		int status_sub;
 	};
 
 	struct mavlink_subscriptions subs;
 
+	struct mavlink_subscriptions* get_subs() { return &subs; }
+	mavlink_channel_t get_chan() { return chan; }
+
 	/** Global position */
 	struct vehicle_global_position_s global_pos;
 	/** Local position */
@@ -243,6 +273,7 @@ private:
 	uint8_t missionlib_msg_buf[300]; //XXX MAGIC NUMBER
 	bool thread_running;
 	bool thread_should_exit;
+	MAVLINK_MODE _mode;
 
 	uint8_t mavlink_wpm_comp_id;
 	mavlink_channel_t chan;
@@ -269,12 +300,6 @@ private:
 	 */
 	unsigned int mavlink_param_queue_index;
 
-	/* interface mode */
-	enum {
-		MAVLINK_INTERFACE_MODE_OFFBOARD,
-		MAVLINK_INTERFACE_MODE_ONBOARD
-	} mavlink_link_mode;
-
 	struct mavlink_logbuffer lb;
 	bool mavlink_link_termination_allowed;
 	
@@ -293,12 +318,6 @@ private:
 	 */
 	int		set_hil_on_off(bool hil_enabled);
 
-
-	/**
-	 * Handle parameter related messages.
-	 */
-	void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
-
 	/**
 	 * Send all parameters at once.
 	 *
@@ -352,7 +371,6 @@ private:
 
 	void mavlink_update_system();
 
-	void mavlink_wpm_message_handler(const mavlink_message_t *msg);
 	void mavlink_waypoint_eventloop(uint64_t now);
 	void mavlink_wpm_send_waypoint_reached(uint16_t seq);
 	void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq);

src/modules/mavlink/mavlink_orb_listener.cpp

@@ -62,11 +62,7 @@
 
 	void		*uorb_receive_thread(void *arg);
 
-struct listener {
-	void	(*callback)(const struct listener *l);
-	int		*subp;
-	uintptr_t	arg;
-};
+
 
 uint16_t cm_uint16_from_m_float(float m);
 
@@ -86,41 +82,58 @@ cm_uint16_from_m_float(float m)
 
 MavlinkOrbListener::MavlinkOrbListener(Mavlink* parent) : 
 
-	_task_should_exit(false),
+	thread_should_exit(false),
 	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink orb")),
 	_mavlink(parent),
+	_listeners(nullptr),
 	_n_listeners(0)
 {
-	static const struct listener listeners[] = {
-	{l_sensor_combined,		&mavlink_subs.sensor_sub,	0},
-	{l_vehicle_attitude,		&mavlink_subs.att_sub,		0},
-	{l_vehicle_gps_position,	&mavlink_subs.gps_sub,		0},
-	{l_vehicle_status,		&status_sub,			0},
-	{l_rc_channels,			&rc_sub,			0},
-	{l_input_rc,			&mavlink_subs.input_rc_sub,	0},
-	{l_global_position,		&mavlink_subs.global_pos_sub,	0},
-	{l_local_position,		&mavlink_subs.local_pos_sub,	0},
-	{l_global_position_setpoint,	&mavlink_subs.triplet_sub,	0},
-	{l_local_position_setpoint,	&mavlink_subs.spl_sub,		0},
-	{l_attitude_setpoint,		&mavlink_subs.spa_sub,		0},
-	{l_actuator_outputs,		&mavlink_subs.act_0_sub,	0},
-	{l_actuator_outputs,		&mavlink_subs.act_1_sub,	1},
-	{l_actuator_outputs,		&mavlink_subs.act_2_sub,	2},
-	{l_actuator_outputs,		&mavlink_subs.act_3_sub,	3},
-	{l_actuator_armed,		&mavlink_subs.armed_sub,	0},
-	{l_manual_control_setpoint,	&mavlink_subs.man_control_sp_sub, 0},
-	{l_vehicle_attitude_controls,	&mavlink_subs.actuators_sub,	0},
-	{l_debug_key_value,		&mavlink_subs.debug_key_value,	0},
-	{l_optical_flow,		&mavlink_subs.optical_flow,	0},
-	{l_vehicle_rates_setpoint,	&mavlink_subs.rates_setpoint_sub,	0},
-	{l_home,			&mavlink_subs.home_sub,		0},
-	{l_airspeed,			&mavlink_subs.airspeed_sub,		0},
-	{l_nav_cap,			&mavlink_subs.navigation_capabilities_sub,		0},
-	{l_control_mode,			&mavlink_subs.control_mode_sub,		0},
-};
+	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,		&status_sub,						0);
+	add_listener(MavlinkOrbListener::l_rc_channels,			&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_home,			&_mavlink->get_subs()->home_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);
+	add_listener(MavlinkOrbListener::l_control_mode,		&_mavlink->get_subs()->control_mode_sub,		0);
 
-	_n_listeners = sizeof(listeners) / sizeof(listeners[0]);
+}
 
+void MavlinkOrbListener::add_listener(void (*callback)(const struct listener *l), int *subp, uintptr_t arg)
+{
+	struct listener *nl = new listener;
+
+	nl->callback = callback;
+	nl->subp = subp;
+	nl->arg = arg;
+	nl->next = nullptr;
+
+	// Register it
+	struct listener *next = _listeners;
+	while (next->next != nullptr) {
+		next = next->next;
+	}
+
+	// Attach
+	next->next = nl;
+	_n_listeners++;
 }
 
 void
@@ -129,43 +142,37 @@ 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), mavlink_subs.sensor_sub, &raw);
+	orb_copy(ORB_ID(sensor_combined), l->mavlink->get_subs()->sensor_sub, &raw);
 
-	last_sensor_timestamp = raw.timestamp;
-
-	/* mark individual fields as changed */
+	/* mark individual fields as _mavlink->get_chan()ged */
 	uint16_t fields_updated = 0;
-	static unsigned accel_counter = 0;
-	static unsigned gyro_counter = 0;
-	static unsigned mag_counter = 0;
-	static unsigned baro_counter = 0;
 
-	if (accel_counter != raw.accelerometer_counter) {
-		/* mark first three dimensions as changed */
-		fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
-		accel_counter = raw.accelerometer_counter;
-	}
+	// 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 changed */
-		fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
-		gyro_counter = raw.gyro_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 changed */
-		fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
-		mag_counter = raw.magnetometer_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 changed */
-		fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
-		baro_counter = raw.baro_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 (gcs_link)
-		mavlink_msg_highres_imu_send(_mavlink->get_mavlink_chan(), last_sensor_timestamp,
+	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],
@@ -175,51 +182,51 @@ MavlinkOrbListener::l_sensor_combined(const struct listener *l)
 					     raw.baro_alt_meter, raw.baro_temp_celcius,
 					     fields_updated);
 
-	sensors_raw_counter++;
+	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), mavlink_subs.att_sub, &att);
+	orb_copy(ORB_ID(vehicle_attitude), l->mavlink->get_subs()->att_sub, &l->listener->att);
 
-	if (gcs_link) {
+	if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) {
 		/* send sensor values */
-		mavlink_msg_attitude_send(_mavlink->get_mavlink_chan(),
-					  last_sensor_timestamp / 1000,
-					  att.roll,
-					  att.pitch,
-					  att.yaw,
-					  att.rollspeed,
-					  att.pitchspeed,
-					  att.yawspeed);
+		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 >= last_sent_vfr + 100000) {
-			last_sent_vfr = t;
-			float groundspeed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e);
-			uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
-			float throttle = armed.armed ? actuators_0.control[3] * 100.0f : 0.0f;
-			mavlink_msg_vfr_hud_send(_mavlink->get_mavlink_chan(), airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vel_d);
+		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(att.q_valid) {
-			mavlink_msg_attitude_quaternion_send(_mavlink->get_mavlink_chan(),
-												last_sensor_timestamp / 1000,
-												att.q[0],
-												att.q[1],
-												att.q[2],
-												att.q[3],
-												att.rollspeed,
-												att.pitchspeed,
-												att.yawspeed);
+		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);
 		}
 	}
 
-	attitude_counter++;
+	l->listener->attitude_counter++;
 }
 
 void
@@ -228,13 +235,13 @@ 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), mavlink_subs.gps_sub, &gps);
+	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(_mavlink->get_mavlink_chan(),
+	mavlink_msg_gps_raw_int_send(l->mavlink->get_chan(),
 				     gps.timestamp_position,
 				     gps.fix_type,
 				     gps.lat,
@@ -247,8 +254,8 @@ MavlinkOrbListener::l_vehicle_gps_position(const struct listener *l)
 				     gps.satellites_visible);
 
 	/* update SAT info every 10 seconds */
-	if (gps.satellite_info_available && (gps_counter % 50 == 0)) {
-		mavlink_msg_gps_status_send(_mavlink->get_mavlink_chan(),
+	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,
@@ -257,30 +264,30 @@ MavlinkOrbListener::l_vehicle_gps_position(const struct listener *l)
 					    gps.satellite_snr);
 	}
 
-	gps_counter++;
+	l->listener->gps_counter++;
 }
 
 void
 MavlinkOrbListener::l_vehicle_status(const struct listener *l)
 {
-	/* immediately communicate state changes back to user */
-	orb_copy(ORB_ID(vehicle_status), status_sub, &v_status);
-	orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
+	/* immediately communicate state _mavlink->get_chan()ges back to user */
+	orb_copy(ORB_ID(vehicle_status), l->listener->status_sub, &l->listener->v_status);
+	orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed);
 
 	/* enable or disable HIL */
-	if (v_status.hil_state == HIL_STATE_ON)
-		set_hil_on_off(true);
-	else if (v_status.hil_state == HIL_STATE_OFF)
-		set_hil_on_off(false);
+	if (l->listener->v_status.hil_state == HIL_STATE_ON)
+		l->mavlink->set_hil_on_off(true);
+	else if (l->listener->v_status.hil_state == HIL_STATE_OFF)
+		l->mavlink->set_hil_on_off(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;
-	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(chan,
+	mavlink_msg_heartbeat_send(l->mavlink->get_chan(),
 				   mavlink_system.type,
 				   MAV_AUTOPILOT_PX4,
 				   mavlink_base_mode,
@@ -289,37 +296,37 @@ MavlinkOrbListener::l_vehicle_status(const struct listener *l)
 }
 
 void
-MavlinkOrbListener::l_rc_channels(const struct listener *l)
+MavlinkOrbListener::l_rc__mavlink->get_chan()nels(const struct listener *l)
 {
-	/* copy rc channels into local buffer */
-	orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
-	// XXX Add RC channels scaled message here
+	/* copy rc _mavlink->get_chan()nels into local buffer */
+	orb_copy(ORB_ID(rc__mavlink->get_chan()nels), rc_sub, &rc);
+	// XXX Add RC _mavlink->get_chan()nels scaled message here
 }
 
 void
 MavlinkOrbListener::l_input_rc(const struct listener *l)
 {
-	/* copy rc channels into local buffer */
-	orb_copy(ORB_ID(input_rc), mavlink_subs.input_rc_sub, &rc_raw);
+	/* 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 (gcs_link) {
+	if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) {
 
 		const unsigned port_width = 8;
 
-		for (unsigned i = 0; (i * port_width) < (rc_raw.channel_count + port_width); i++) {
+		for (unsigned i = 0; (i * port_width) < (l->listener->rc_raw.channel_count + port_width); i++) {
 			/* Channels are sent in MAVLink main loop at a fixed interval */
-			mavlink_msg_rc_channels_raw_send(chan,
-							 rc_raw.timestamp / 1000,
+			mavlink_msg_rc_channels_raw_send(_mavlink->get_chan(),
+							 l->listener->rc_raw.timestamp / 1000,
 							 i,
-							 (rc_raw.channel_count > (i * port_width) + 0) ? rc_raw.values[(i * port_width) + 0] : UINT16_MAX,
-							 (rc_raw.channel_count > (i * port_width) + 1) ? rc_raw.values[(i * port_width) + 1] : UINT16_MAX,
-							 (rc_raw.channel_count > (i * port_width) + 2) ? rc_raw.values[(i * port_width) + 2] : UINT16_MAX,
-							 (rc_raw.channel_count > (i * port_width) + 3) ? rc_raw.values[(i * port_width) + 3] : UINT16_MAX,
-							 (rc_raw.channel_count > (i * port_width) + 4) ? rc_raw.values[(i * port_width) + 4] : UINT16_MAX,
-							 (rc_raw.channel_count > (i * port_width) + 5) ? rc_raw.values[(i * port_width) + 5] : UINT16_MAX,
-							 (rc_raw.channel_count > (i * port_width) + 6) ? rc_raw.values[(i * port_width) + 6] : UINT16_MAX,
-							 (rc_raw.channel_count > (i * port_width) + 7) ? rc_raw.values[(i * port_width) + 7] : UINT16_MAX,
-							 rc_raw.rssi);
+							 (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);
 		}
 	}
 }
@@ -328,48 +335,48 @@ void
 MavlinkOrbListener::l_global_position(const struct listener *l)
 {
 	/* copy global position data into local buffer */
-	orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos);
+	orb_copy(ORB_ID(vehicle_global_position), _mavlink->get_subs()->global_pos_sub, l->listener->global_pos);
 
-	mavlink_msg_global_position_int_send(_mavlink->get_mavlink_chan(),
-						 global_pos.timestamp / 1000,
-					     global_pos.lat * 1e7,
-					     global_pos.lon * 1e7,
-					     global_pos.alt * 1000.0f,
-					     (global_pos.alt - home.alt) * 1000.0f,
-					     global_pos.vel_n * 100.0f,
-					     global_pos.vel_e * 100.0f,
-					     global_pos.vel_d * 100.0f,
-					     _wrap_2pi(global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f);
+	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), mavlink_subs.local_pos_sub, &local_pos);
+	orb_copy(ORB_ID(vehicle_local_position), _mavlink->get_subs()->local_pos_sub, l->listener->local_pos);
 
-	if (gcs_link)
-		mavlink_msg_local_position_ned_send(_mavlink->get_mavlink_chan(),
-						    local_pos.timestamp / 1000,
-						    local_pos.x,
-						    local_pos.y,
-						    local_pos.z,
-						    local_pos.vx,
-						    local_pos.vy,
-						    local_pos.vz);
+	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), mavlink_subs.triplet_sub, &triplet);
+	orb_copy(ORB_ID(position_setpoint_triplet), l->mavlink->get_subs()->triplet_sub, &triplet);
 
 	if (!triplet.current.valid)
 		return;
 
-	if (gcs_link)
-		mavlink_msg_global_position_setpoint_int_send(_mavlink->get_mavlink_chan(),
+	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),
@@ -383,10 +390,10 @@ 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), mavlink_subs.spl_sub, &local_sp);
+	orb_copy(ORB_ID(vehicle_local_position_setpoint), l->mavlink->get_subs()->spl_sub, &local_sp);
 
-	if (gcs_link)
-		mavlink_msg_local_position_setpoint_send(_mavlink->get_mavlink_chan(),
+	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,
@@ -400,10 +407,10 @@ 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), mavlink_subs.spa_sub, &att_sp);
+	orb_copy(ORB_ID(vehicle_attitude_setpoint), _mavlink->get_subs()->spa_sub, &att_sp);
 
-	if (gcs_link)
-		mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_mavlink->get_mavlink_chan(),
+	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,
@@ -417,10 +424,10 @@ 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), mavlink_subs.rates_setpoint_sub, &rates_sp);
+	orb_copy(ORB_ID(vehicle_rates_setpoint), l->mavlink->get_subs()->rates_setpoint_sub, &rates_sp);
 
-	if (gcs_link)
-		mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_mavlink->get_mavlink_chan(),
+	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,
@@ -443,8 +450,8 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l)
 	/* copy actuator data into local buffer */
 	orb_copy(ids[l->arg], *l->subp, &act_outputs);
 
-	if (gcs_link) {
-		mavlink_msg_servo_output_raw_send(_mavlink->get_mavlink_chan(), last_sensor_timestamp / 1000,
+	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],
@@ -457,7 +464,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l)
 						  act_outputs.output[7]);
 
 		/* only send in HIL mode and only send first group for HIL */
-		if (mavlink_hil_enabled && armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) {
+		if (l->listener->mavlink_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;
@@ -470,7 +477,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l)
 			/* scale / assign outputs depending on system type */
 
 			if (mavlink_system.type == MAV_TYPE_QUADROTOR) {
-				mavlink_msg_hil_controls_send(chan,
+				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,
@@ -484,7 +491,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l)
 							      0);
 
 			} else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
-				mavlink_msg_hil_controls_send(chan,
+				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,
@@ -498,7 +505,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l)
 							      0);
 
 			} else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
-				mavlink_msg_hil_controls_send(chan,
+				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,
@@ -512,7 +519,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l)
 							      0);
 
 			} else {
-				mavlink_msg_hil_controls_send(chan,
+				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,
@@ -532,7 +539,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l)
 void
 MavlinkOrbListener::l_actuator_armed(const struct listener *l)
 {
-	orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
+	orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed);
 }
 
 void
@@ -541,10 +548,10 @@ 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), mavlink_subs.man_control_sp_sub, &man_control);
+	orb_copy(ORB_ID(manual_control_setpoint), l->mavlink->get_subs()->man_control_sp_sub, &man_control);
 
-	if (gcs_link)
-		mavlink_msg_manual_control_send(_mavlink->get_mavlink_chan(),
+	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,
@@ -556,26 +563,26 @@ MavlinkOrbListener::l_manual_control_setpoint(const struct listener *l)
 void
 MavlinkOrbListener::l_vehicle_attitude_controls(const struct listener *l)
 {
-	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators_0);
+	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, l->mavlink->get_subs()->actuators_sub, &l->listener->actuators_0);
 
-	if (gcs_link) {
+	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(_mavlink->get_mavlink_chan(),
-						   last_sensor_timestamp / 1000,
+		mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
+						   l->listener->last_sensor_timestamp / 1000,
 						   "ctrl0    ",
-						   actuators_0.control[0]);
-		mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(),
-						   last_sensor_timestamp / 1000,
+						   l->listener->actuators_0.control[0]);
+		mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
+						   l->listener->last_sensor_timestamp / 1000,
 						   "ctrl1    ",
-						   actuators_0.control[1]);
-		mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(),
-						   last_sensor_timestamp / 1000,
+						   l->listener->actuators_0.control[1]);
+		mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
+						   l->listener->last_sensor_timestamp / 1000,
 						   "ctrl2     ",
-						   actuators_0.control[2]);
-		mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(),
-						   last_sensor_timestamp / 1000,
+						   l->listener->actuators_0.control[2]);
+		mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
+						   l->listener->last_sensor_timestamp / 1000,
 						   "ctrl3     ",
-						   actuators_0.control[3]);
+						   l->listener->actuators_0.control[3]);
 	}
 }
 
@@ -584,13 +591,13 @@ MavlinkOrbListener::l_debug_key_value(const struct listener *l)
 {
 	struct debug_key_value_s debug;
 
-	orb_copy(ORB_ID(debug_key_value), mavlink_subs.debug_key_value, &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(_mavlink->get_mavlink_chan(),
-					   last_sensor_timestamp / 1000,
+	mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
+					   l->listener->last_sensor_timestamp / 1000,
 					   debug.key,
 					   debug.value);
 }
@@ -600,52 +607,52 @@ MavlinkOrbListener::l_optical_flow(const struct listener *l)
 {
 	struct optical_flow_s flow;
 
-	orb_copy(ORB_ID(optical_flow), mavlink_subs.optical_flow, &flow);
+	orb_copy(ORB_ID(optical_flow), l->mavlink->get_subs()->optical_flow, &flow);
 
-	mavlink_msg_optical_flow_send(_mavlink->get_mavlink_chan(), flow.timestamp, flow.sensor_id, flow.flow_raw_x, flow.flow_raw_y,
+	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), mavlink_subs.home_sub, &home);
+	orb_copy(ORB_ID(home_position), l->mavlink->get_subs()->home_sub, &home);
 
-	mavlink_msg_gps_global_origin_send(_mavlink->get_mavlink_chan(), (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.alt)*1e3f);
+	mavlink_msg_gps_global_origin_send(l->mavlink->get_chan(), (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.alt)*1e3f);
 }
 
 void
 MavlinkOrbListener::l_airspeed(const struct listener *l)
 {
-	orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed);
+	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), mavlink_subs.navigation_capabilities_sub, &nav_cap);
+	orb_copy(ORB_ID(navigation_capabilities), l->mavlink->get_subs()->navigation_capabilities_sub, &l->listener->nav_cap);
 
-	mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(),
+	mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
 				   hrt_absolute_time() / 1000,
 				   "turn dist",
-				   nav_cap.turn_distance);
+				   l->listener->nav_cap.turn_distance);
 
 }
 
 void
 MavlinkOrbListener::l_control_mode(const struct listener *l)
 {
-	orb_copy(ORB_ID(vehicle_control_mode), mavlink_subs.control_mode_sub, &control_mode);
+	orb_copy(ORB_ID(vehicle_control_mode), l->mavlink->get_subs()->control_mode_sub, &l->listener->control_mode);
 
 	/* 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;
-	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(chan,
+	mavlink_msg_heartbeat_send(l->mavlink->get_chan(),
 				   mavlink_system.type,
 				   MAV_AUTOPILOT_PX4,
 				   mavlink_base_mode,
@@ -657,7 +664,9 @@ void *
 MavlinkOrbListener::uorb_receive_thread(void *arg)
 {
 	/* Set thread name */
-	prctl(PR_SET_NAME, "mavlink_orb_rcv", getpid());
+	char buf[32];
+	sprintf(buf, "mavlink rcv%d", Mavlink::instance_count());
+	prctl(PR_SET_NAME, buf, getpid());
 
 	/*
 	 * set up poll to block for new data,
@@ -670,15 +679,21 @@ MavlinkOrbListener::uorb_receive_thread(void *arg)
 	 *
 	 * Might want to invoke each listener once to set initial state.
 	 */
-	struct pollfd fds[_n_listeners];
+	struct pollfd fds[_max_listeners];
 
-	for (unsigned i = 0; i < _n_listeners; i++) {
-		fds[i].fd = *listeners[i].subp;
+	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]);
-	}
 
 	while (!thread_should_exit) {
 
@@ -689,13 +704,19 @@ MavlinkOrbListener::uorb_receive_thread(void *arg)
 			/* silent */
 
 		} else if (poll_ret < 0) {
-			mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data");
+			//mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data");
 
 		} else {
 
-			for (unsigned i = 0; i < _n_listeners; i++) {
+			unsigned i = 0;
+			struct listener* cb = _listeners;
+			while (cb != nullptr) {
+
 				if (fds[i].revents & POLLIN)
-					listeners[i].callback(&listeners[i]);
+					cb->callback(cb);
+
+				cb = cb->next;
+				i++;
 			}
 		}
 	}
@@ -703,107 +724,113 @@ MavlinkOrbListener::uorb_receive_thread(void *arg)
 	return NULL;
 }
 
-pthread_t
-MavlinkOrbListener::uorb_receive_start(void)
+void * MavlinkOrbListener::uorb_start_helper(void *context)
 {
+	return ((MavlinkOrbListener *)context)->uorb_receive_thread(NULL);
+}
+
+pthread_t
+MavlinkOrbListener::uorb_receive_start(Mavlink* mavlink)
+{
+	MavlinkOrbListener* urcv = new MavlinkOrbListener(mavlink);
+
 	/* --- SENSORS RAW VALUE --- */
-	mavlink_subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+	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_subs.sensor_sub, 200);	/* 5Hz updates */
+	orb_set_interval(mavlink->get_subs()->sensor_sub, 200);	/* 5Hz updates */
 
 	/* --- ATTITUDE VALUE --- */
-	mavlink_subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	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_subs.att_sub, 200);	/* 5Hz updates */
+	orb_set_interval(mavlink->get_subs()->att_sub, 200);	/* 5Hz updates */
 
 	/* --- GPS VALUE --- */
-	mavlink_subs.gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
-	orb_set_interval(mavlink_subs.gps_sub, 200);	/* 5Hz updates */
+	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_subs.home_sub = orb_subscribe(ORB_ID(home_position));
-	orb_set_interval(mavlink_subs.home_sub, 1000);	/* 1Hz updates */
+	mavlink->get_subs()->home_sub = orb_subscribe(ORB_ID(home_position));
+	orb_set_interval(mavlink->get_subs()->home_sub, 1000);	/* 1Hz updates */
 
 	/* --- SYSTEM STATE --- */
-	status_sub = orb_subscribe(ORB_ID(vehicle_status));
-	orb_set_interval(status_sub, 300);		/* max 3.33 Hz updates */
+	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 */
 
 	/* --- CONTROL MODE --- */
-	mavlink_subs.control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
-	orb_set_interval(mavlink_subs.control_mode_sub, 300);		/* max 3.33 Hz updates */
+	mavlink->get_subs()->control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
+	orb_set_interval(mavlink->get_subs()->control_mode_sub, 300);		/* max 3.33 Hz updates */
 
 	/* --- RC CHANNELS VALUE --- */
-	rc_sub = orb_subscribe(ORB_ID(rc_channels));
-	orb_set_interval(rc_sub, 100);			/* 10Hz updates */
+	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_subs.input_rc_sub = orb_subscribe(ORB_ID(input_rc));
-	orb_set_interval(mavlink_subs.input_rc_sub, 100);
+	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_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	orb_set_interval(mavlink_subs.global_pos_sub, 100);	/* 10 Hz active updates */
+	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_subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
-	orb_set_interval(mavlink_subs.local_pos_sub, 1000);	/* 1Hz active updates */
+	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_subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
-	orb_set_interval(mavlink_subs.triplet_sub, 2000);	/* 0.5 Hz updates */
+	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_subs.spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
-	orb_set_interval(mavlink_subs.spl_sub, 2000);	/* 0.5 Hz updates */
+	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_subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
-	orb_set_interval(mavlink_subs.spa_sub, 2000);	/* 0.5 Hz updates */
+	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_subs.rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
-	orb_set_interval(mavlink_subs.rates_setpoint_sub, 2000);  /* 0.5 Hz updates */
+	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_subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0));
-	mavlink_subs.act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1));
-	mavlink_subs.act_2_sub = orb_subscribe(ORB_ID(actuator_outputs_2));
-	mavlink_subs.act_3_sub = orb_subscribe(ORB_ID(actuator_outputs_3));
+	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_subs.act_0_sub, 100);	/* 10Hz updates */
-	orb_set_interval(mavlink_subs.act_1_sub, 100);	/* 10Hz updates */
-	orb_set_interval(mavlink_subs.act_2_sub, 100);	/* 10Hz updates */
-	orb_set_interval(mavlink_subs.act_3_sub, 100);	/* 10Hz updates */
+	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_subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed));
-	orb_set_interval(mavlink_subs.armed_sub, 100);	/* 10Hz updates */
+	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_subs.man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	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_subs.man_control_sp_sub, 100);	/* 10Hz updates */
+	orb_set_interval(mavlink->get_subs()->man_control_sp_sub, 100);	/* 10Hz updates */
 
 	/* --- ACTUATOR CONTROL VALUE --- */
-	mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
-	orb_set_interval(mavlink_subs.actuators_sub, 100);	/* 10Hz updates */
+	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_subs.debug_key_value = orb_subscribe(ORB_ID(debug_key_value));
-	orb_set_interval(mavlink_subs.debug_key_value, 100);	/* 10Hz updates */
+	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_subs.optical_flow = orb_subscribe(ORB_ID(optical_flow));
-	orb_set_interval(mavlink_subs.optical_flow, 200); 	/* 5Hz updates */
+	mavlink->get_subs()->optical_flow = orb_subscribe(ORB_ID(optical_flow));
+	orb_set_interval(mavlink->get_subs()->optical_flow, 200); 	/* 5Hz updates */
 
 	/* --- AIRSPEED --- */
-	mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
-	orb_set_interval(mavlink_subs.airspeed_sub, 200); 	/* 5Hz updates */
+	mavlink->get_subs()->airspeed_sub = orb_subscribe(ORB_ID(airspeed));
+	orb_set_interval(mavlink->get_subs()->airspeed_sub, 200); 	/* 5Hz updates */
 
 	/* --- NAVIGATION CAPABILITIES --- */
-	mavlink_subs.navigation_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
-	orb_set_interval(mavlink_subs.navigation_capabilities_sub, 500); 	/* 2Hz updates */
-	nav_cap.turn_distance = 0.0f;
+	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;
@@ -813,7 +840,7 @@ MavlinkOrbListener::uorb_receive_start(void)
 	pthread_attr_setstacksize(&uorb_attr, 2048);
 
 	pthread_t thread;
-	pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL);
+	pthread_create(&thread, &uorb_attr, MavlinkOrbListener::uorb_start_helper, urcv);
 
 	pthread_attr_destroy(&uorb_attr);
 	return thread;

src/modules/mavlink/mavlink_orb_listener.h

@@ -38,8 +38,40 @@
  * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
+#include <systemlib/perf_counter.h>
+
 #pragma once
 
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/rc_channels.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/offboard_control_setpoint.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/vehicle_vicon_position.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_controls_effective.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/telemetry_status.h>
+#include <uORB/topics/debug_key_value.h>
+#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>
+
 class Mavlink;
 
 class MavlinkOrbListener
@@ -67,18 +99,32 @@ public:
 	 */
 	void		status();
 
-	pthread_t uorb_receive_start(void);
+	static pthread_t uorb_receive_start(Mavlink *mavlink);
 	void * uorb_receive_thread(void *arg);
 
+	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);
+	static void * uorb_start_helper(void *context);
+
 private:
 
-	bool		_task_should_exit;		/**< if true, sensor task should exit */
+	bool		thread_should_exit;		/**< if true, sensor task should exit */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
 	Mavlink*	_mavlink;
 
+	struct listener *_listeners;
 	unsigned	_n_listeners;
+	static const unsigned _max_listeners = 32;
 
 	/**
 	 * Shim for calling task_main from task_create.
@@ -90,28 +136,28 @@ private:
 	 */
 	void		task_main() __attribute__((noreturn));
 
-	void		l_sensor_combined(const struct listener *l);
-	void		l_vehicle_attitude(const struct listener *l);
-	void		l_vehicle_gps_position(const struct listener *l);
-	void		l_vehicle_status(const struct listener *l);
-	void		l_rc_channels(const struct listener *l);
-	void		l_input_rc(const struct listener *l);
-	void		l_global_position(const struct listener *l);
-	void		l_local_position(const struct listener *l);
-	void		l_global_position_setpoint(const struct listener *l);
-	void		l_local_position_setpoint(const struct listener *l);
-	void		l_attitude_setpoint(const struct listener *l);
-	void		l_actuator_outputs(const struct listener *l);
-	void		l_actuator_armed(const struct listener *l);
-	void		l_manual_control_setpoint(const struct listener *l);
-	void		l_vehicle_attitude_controls(const struct listener *l);
-	void		l_debug_key_value(const struct listener *l);
-	void		l_optical_flow(const struct listener *l);
-	void		l_vehicle_rates_setpoint(const struct listener *l);
-	void		l_home(const struct listener *l);
-	void		l_airspeed(const struct listener *l);
-	void		l_nav_cap(const struct listener *l);
-	void		l_control_mode(const struct listener *l);
+	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);
+	static void		l_control_mode(const struct listener *l);
 
 	struct vehicle_global_position_s global_pos;
 	struct vehicle_local_position_s local_pos;

src/modules/mavlink/mavlink_receiver.cpp

@@ -83,7 +83,8 @@ __BEGIN_DECLS
 
 __END_DECLS
 
-void MavlinkReceiver::MavlinkReceiver() :
+MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
+	_mavlink(parent),
 	pub_hil_global_pos(-1),
 	pub_hil_local_pos(-1),
 	pub_hil_attitude(-1),
@@ -790,7 +791,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 void *
 MavlinkReceiver::receive_thread(void *arg)
 {
-	int uart_fd = *((int *)arg);
+	int uart_fd = _mavlink->get_uart_fd();
 
 	const int timeout = 1000;
 	uint8_t buf[32];
@@ -822,10 +823,10 @@ MavlinkReceiver::receive_thread(void *arg)
 					handle_message(&msg);
 
 					/* handle packet with waypoint component */
-					mavlink_wpm_message_handler(&msg);
+					_mavlink->mavlink_wpm_message_handler(&msg);
 
 					/* handle packet with parameter component */
-					mavlink_pm_message_handler(MAVLINK_COMM_0, &msg);
+					_mavlink->mavlink_pm_message_handler(MAVLINK_COMM_0, &msg);
 				}
 			}
 		}
@@ -839,15 +840,22 @@ void MavlinkReceiver::print_status()
 
 }
 
-pthread_t
-MavlinkReceiver::receive_start(int uart)
+void * MavlinkReceiver::start_helper(void *context)
 {
+	return ((MavlinkReceiver *)context)->receive_thread(NULL);
+}
+
+pthread_t
+MavlinkReceiver::receive_start(Mavlink *mavlink)
+{
+	MavlinkReceiver *rcv = new MavlinkReceiver(mavlink);
+
 	pthread_attr_t receiveloop_attr;
 	pthread_attr_init(&receiveloop_attr);
 
 	// set to non-blocking read
-	int flags = fcntl(uart, F_GETFL, 0);
-	fcntl(uart, F_SETFL, flags | O_NONBLOCK);
+	int flags = fcntl(mavlink->get_uart_fd(), F_GETFL, 0);
+	fcntl(mavlink->get_uart_fd(), F_SETFL, flags | O_NONBLOCK);
 
 	struct sched_param param;
 	(void)pthread_attr_getschedparam(&receiveloop_attr, &param);
@@ -857,7 +865,7 @@ MavlinkReceiver::receive_start(int uart)
 	pthread_attr_setstacksize(&receiveloop_attr, 3000);
 
 	pthread_t thread;
-	pthread_create(&thread, &receiveloop_attr, receive_thread, &uart);
+	pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, rcv);
 
 	pthread_attr_destroy(&receiveloop_attr);
 	return thread;

src/modules/mavlink/mavlink_receiver.h

@@ -96,7 +96,9 @@ public:
 	 */
 	void		print_status();
 
-	pthread_t receive_start(int uart);
+	static pthread_t receive_start(Mavlink* mavlink);
+
+	static void * start_helper(void *context);
 
 private: