mavlink: adding message stream by name implemnted, mavlink streams definitions and formatters moved to mavlink_messages.h/cpp, mavlink_orb_listener class and thread removed

10 files changed, 325 insertions, 712 deletions

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 1ce467a7b..4a75b00ab 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -79,11 +79,9 @@
 #include <mathlib/mathlib.h>
 #include <mavlink/mavlink_log.h>
 
-#include <commander/px4_custom_mode.h>
-
 #include "mavlink_bridge_header.h"
 #include "mavlink_main.h"
-#include "mavlink_orb_listener.h"
+#include "mavlink_messages.h"
 #include "mavlink_receiver.h"
 
 /* oddly, ERROR is not defined for c++ */
@@ -92,6 +90,8 @@
 #endif
 static const int ERROR = -1;
 
+#define MAIN_LOOP_DELAY 10000	// 100 Hz
+
 static Mavlink* _head = nullptr;
 
 /* TODO: if this is a class member it crashes */
@@ -170,7 +170,8 @@ Mavlink::Mavlink() :
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink")),
 	_mavlink_hil_enabled(false),
-	// _params_sub(-1)
+	_subscriptions(nullptr),
+	_streams(nullptr),
 
 	mission_pub(-1)
 {
@@ -325,17 +326,6 @@ int Mavlink::get_channel()
 	return (int)_chan;
 }
 
-void
-Mavlink::parameters_update()
-{
-	/* read from param to clear updated flag */
-	struct parameter_update_s update;
-	orb_copy(ORB_ID(parameter_update), _params_sub, &update);
-
-	// param_get(_parameter_handles.min_altitude, &(_parameters.min_altitude));
-
-}
-
 /****************************************************************************
  * MAVLink text message logger
  ****************************************************************************/
@@ -532,13 +522,13 @@ Mavlink::set_hil_enabled(bool hil_enabled)
 			hil_rate_interval = 5;
 		}
 
-		orb_set_interval(subs.spa_sub, hil_rate_interval);
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval);
+//		orb_set_interval(subs.spa_sub, hil_rate_interval);
+//		set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval);
 	}
 
 	if (!hil_enabled && _mavlink_hil_enabled) {
 		_mavlink_hil_enabled = false;
-		orb_set_interval(subs.spa_sub, 200);
+//		orb_set_interval(subs.spa_sub, 200);
 
 	} else {
 		ret = ERROR;
@@ -547,159 +537,8 @@ Mavlink::set_hil_enabled(bool hil_enabled)
 	return ret;
 }
 
-void
-Mavlink::get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode)
-{
-	/* reset MAVLink mode bitfield */
-	*mavlink_base_mode = 0;
-	*mavlink_custom_mode = 0;
-
-	/**
-	 * Set mode flags
-	 **/
-
-	/* HIL */
-	if (v_status.hil_state == HIL_STATE_ON) {
-		*mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
-	}
-
-	/* arming state */
-	if (v_status.arming_state == ARMING_STATE_ARMED
-			|| v_status.arming_state == ARMING_STATE_ARMED_ERROR) {
-		*mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
-	}
-
-	/* main state */
-	*mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
-	union px4_custom_mode custom_mode;
-	custom_mode.data = 0;
-	if (pos_sp_triplet.nav_state == NAV_STATE_NONE) {
-	/* use main state when navigator is not active */
-		if (v_status.main_state == MAIN_STATE_MANUAL) {
-			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
-			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
-		} else if (v_status.main_state == MAIN_STATE_SEATBELT) {
-			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
-			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
-		} else if (v_status.main_state == MAIN_STATE_EASY) {
-			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
-			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY;
-		} else if (v_status.main_state == MAIN_STATE_AUTO) {
-			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
-			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
-		}
-	} else {
-		/* use navigation state when navigator is active */
-		*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
-		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
-		if (pos_sp_triplet.nav_state == NAV_STATE_READY) {
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
-		} else if (pos_sp_triplet.nav_state == NAV_STATE_LOITER) {
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
-		} else if (pos_sp_triplet.nav_state == NAV_STATE_MISSION) {
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
-		} else if (pos_sp_triplet.nav_state == NAV_STATE_RTL) {
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
-		} else if (pos_sp_triplet.nav_state == NAV_STATE_LAND) {
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
-		}
-	}
-	*mavlink_custom_mode = custom_mode.data;
-
-	/**
-	 * Set mavlink state
-	 **/
-
-	/* set calibration state */
-	if (v_status.arming_state == ARMING_STATE_INIT
-			|| v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE
-			|| v_status.arming_state == ARMING_STATE_STANDBY_ERROR) {	// TODO review
-		*mavlink_state = MAV_STATE_UNINIT;
-	} else if (v_status.arming_state == ARMING_STATE_ARMED) {
-		*mavlink_state = MAV_STATE_ACTIVE;
-	} else if (v_status.arming_state == ARMING_STATE_ARMED_ERROR) {
-		*mavlink_state = MAV_STATE_CRITICAL;
-	} else if (v_status.arming_state == ARMING_STATE_STANDBY) {
-		*mavlink_state = MAV_STATE_STANDBY;
-	} else if (v_status.arming_state == ARMING_STATE_REBOOT) {
-		*mavlink_state = MAV_STATE_POWEROFF;
-	} else {
-		warnx("Unknown mavlink state");
-		*mavlink_state = MAV_STATE_CRITICAL;
-	}
-}
-
-
-int Mavlink::set_mavlink_interval_limit(int mavlink_msg_id, int min_interval)
-{
-	int ret = OK;
-
-	switch (mavlink_msg_id) {
-	case MAVLINK_MSG_ID_SCALED_IMU:
-		/* sensor sub triggers scaled IMU */
-		orb_set_interval(subs.sensor_sub, min_interval);
-		break;
-
-	case MAVLINK_MSG_ID_HIGHRES_IMU:
-		/* sensor sub triggers highres IMU */
-		orb_set_interval(subs.sensor_sub, min_interval);
-		break;
-
-	case MAVLINK_MSG_ID_RAW_IMU:
-		/* sensor sub triggers RAW IMU */
-		orb_set_interval(subs.sensor_sub, min_interval);
-		break;
-
-	case MAVLINK_MSG_ID_ATTITUDE:
-		/* attitude sub triggers attitude */
-		orb_set_interval(subs.att_sub, min_interval);
-		break;
-
-	case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
-		/* actuator_outputs triggers this message */
-		orb_set_interval(subs.act_0_sub, min_interval);
-		orb_set_interval(subs.act_1_sub, min_interval);
-		orb_set_interval(subs.act_2_sub, min_interval);
-		orb_set_interval(subs.act_3_sub, min_interval);
-		orb_set_interval(subs.actuators_sub, min_interval);
-		orb_set_interval(subs.actuators_effective_sub, min_interval);
-		orb_set_interval(subs.spa_sub, min_interval);
-		orb_set_interval(subs.rates_setpoint_sub, min_interval);
-		break;
-
-	case MAVLINK_MSG_ID_MANUAL_CONTROL:
-		/* manual_control_setpoint triggers this message */
-		orb_set_interval(subs.man_control_sp_sub, min_interval);
-		break;
-
-	case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT:
-		orb_set_interval(subs.debug_key_value, min_interval);
-		break;
-
-	default:
-		/* not found */
-		ret = ERROR;
-		break;
-	}
-
-	return ret;
-}
-
 extern mavlink_system_t mavlink_system;
 
-void Mavlink::mavlink_pm_callback(void *arg, param_t param)
-{
-	//mavlink_pm_send_param(param);
-	usleep(*(unsigned int *)arg);
-}
-
-void Mavlink::mavlink_pm_send_all_params(unsigned int delay)
-{
-	unsigned int dbuf = delay;
-	param_foreach(&mavlink_pm_callback, &dbuf, false);
-}
-
 int Mavlink::mavlink_pm_queued_send()
 {
 	if (mavlink_param_queue_index < param_count()) {
@@ -1519,6 +1358,51 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string)
 	}
 }
 
+MavlinkOrbSubscription *Mavlink::add_orb_subscription(const struct orb_metadata *topic, const size_t size)
+{
+	/* check if already subscribed to this topic */
+	MavlinkOrbSubscription *sub;
+
+	LL_FOREACH(_subscriptions, sub) {
+		if (sub->topic == topic) {
+			/* already subscribed */
+			return sub;
+		}
+	}
+
+	/* add new subscription */
+	MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic, size);
+
+	LL_APPEND(_subscriptions, sub_new);
+
+	return sub_new;
+}
+
+int
+Mavlink::add_stream(const char *stream_name, const unsigned int interval)
+{
+	uintptr_t arg = 0;
+
+	unsigned int i = 0;
+	/* search for message with specified name */
+	while (msgs_list[i].name != nullptr) {
+		if (strcmp(stream_name, msgs_list[i].name) == 0) {
+			/* count topics, array is nullptr-terminated */
+			unsigned int topics_n;
+			for (topics_n = 0; topics_n < MAX_TOPICS_PER_MAVLINK_STREAM; topics_n++) {
+				if (msgs_list[i].topics[topics_n] == nullptr) {
+					break;
+				}
+			}
+			MavlinkStream *stream = new MavlinkStream(this, msgs_list[i].callback, topics_n, msgs_list[i].topics, msgs_list[i].sizes, arg, interval);
+			LL_APPEND(_streams, stream);
+			return OK;
+		}
+		i++;
+	}
+	return ERROR;
+}
+
 int
 Mavlink::task_main(int argc, char *argv[])
 {
@@ -1639,67 +1523,64 @@ Mavlink::task_main(int argc, char *argv[])
 	/* start the MAVLink receiver */
 	receive_thread = MavlinkReceiver::receive_start(this);
 
-	/* start the ORB receiver */
-	uorb_receive_thread = MavlinkOrbListener::uorb_receive_start(this);
-
 	/* initialize waypoint manager */
 	mavlink_wpm_init(wpm);
 
 	/* all subscriptions are now active, set up initial guess about rate limits */
-	if (_baudrate >= 230400) {
-		/* 200 Hz / 5 ms */
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 20);
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 20);
-		/* 50 Hz / 20 ms */
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 30);
-		/* 20 Hz / 50 ms */
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10);
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50);
-		/* 10 Hz */
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 100);
-		/* 10 Hz */
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 100);
-
-	} else if (_baudrate >= 115200) {
-		/* 20 Hz / 50 ms */
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 50);
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 50);
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 50);
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50);
-		/* 5 Hz / 200 ms */
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200);
-		/* 5 Hz / 200 ms */
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 200);
-		/* 2 Hz */
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
-
-	} else if (_baudrate >= 57600) {
-		/* 10 Hz / 100 ms */
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 300);
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 300);
-		/* 10 Hz / 100 ms ATTITUDE */
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 200);
-		/* 5 Hz / 200 ms */
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200);
-		/* 5 Hz / 200 ms */
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500);
-		/* 2 Hz */
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
-		/* 2 Hz */
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 500);
-
-	} else {
-		/* very low baud rate, limit to 1 Hz / 1000 ms */
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 1000);
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 1000);
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 1000);
-		/* 1 Hz / 1000 ms */
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000);
-		/* 0.5 Hz */
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000);
-		/* 0.1 Hz */
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 10000);
-	}
+//	if (_baudrate >= 230400) {
+//		/* 200 Hz / 5 ms */
+//		set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 20);
+//		set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 20);
+//		/* 50 Hz / 20 ms */
+//		set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 30);
+//		/* 20 Hz / 50 ms */
+//		set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10);
+//		set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50);
+//		/* 10 Hz */
+//		set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 100);
+//		/* 10 Hz */
+//		set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 100);
+//
+//	} else if (_baudrate >= 115200) {
+//		/* 20 Hz / 50 ms */
+//		set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 50);
+//		set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 50);
+//		set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 50);
+//		set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50);
+//		/* 5 Hz / 200 ms */
+//		set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200);
+//		/* 5 Hz / 200 ms */
+//		set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 200);
+//		/* 2 Hz */
+//		set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
+//
+//	} else if (_baudrate >= 57600) {
+//		/* 10 Hz / 100 ms */
+//		set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 300);
+//		set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 300);
+//		/* 10 Hz / 100 ms ATTITUDE */
+//		set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 200);
+//		/* 5 Hz / 200 ms */
+//		set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200);
+//		/* 5 Hz / 200 ms */
+//		set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500);
+//		/* 2 Hz */
+//		set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
+//		/* 2 Hz */
+//		set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 500);
+//
+//	} else {
+//		/* very low baud rate, limit to 1 Hz / 1000 ms */
+//		set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 1000);
+//		set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 1000);
+//		set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 1000);
+//		/* 1 Hz / 1000 ms */
+//		set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000);
+//		/* 0.5 Hz */
+//		set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000);
+//		/* 0.1 Hz */
+//		set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 10000);
+//	}
 
 	int mission_result_sub = orb_subscribe(ORB_ID(mission_result));
 	struct mission_result_s mission_result;
@@ -1709,72 +1590,42 @@ Mavlink::task_main(int argc, char *argv[])
 
 	unsigned lowspeed_counter = 0;
 
-	/* wakeup source(s) */
-	struct pollfd fds[1];
+	MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update), sizeof(parameter_update_s));
+	MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status), sizeof(vehicle_status_s));
 
-	_params_sub = orb_subscribe(ORB_ID(parameter_update));
+	struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->data;
 
-	/* Setup of loop */
-	fds[0].fd = _params_sub;
-	fds[0].events = POLLIN;
+	add_stream("HEARTBEAT", 1000);
+	add_stream("SYS_STATUS", 100);
 
 	while (!_task_should_exit) {
-
-		/* wait for up to 100ms for data */
-		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
-
-		/* this is undesirable but not much we can do - might want to flag unhappy status */
-		if (pret < 0) {
-			warn("poll error %d, %d", pret, errno);
-			continue;
-		}
+		/* main loop */
+		usleep(MAIN_LOOP_DELAY);
 
 		perf_begin(_loop_perf);
 
-		/* parameters updated */
-		if (fds[0].revents & POLLIN) {
-			parameters_update();
-		}
+		hrt_abstime t = hrt_absolute_time();
 
-		/* 1 Hz */
-		if (lowspeed_counter % 10 == 0) {
+		if (param_sub->update(t)) {
+			/* parameters updated */
 			mavlink_update_system();
+		}
 
-			/* translate the current system 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);
-
-			/* send heartbeat */
-			mavlink_msg_heartbeat_send(_chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, mavlink_custom_mode, mavlink_state);
-
+		if (status_sub->update(t)) {
 			/* switch HIL mode if required */
-			if (v_status.hil_state == HIL_STATE_ON)
+			if (status->hil_state == HIL_STATE_ON)
 				set_hil_enabled(true);
-			else if (v_status.hil_state == HIL_STATE_OFF)
+			else if (status->hil_state == HIL_STATE_OFF)
 				set_hil_enabled(false);
+		}
 
-			/* send status (values already copied in the section above) */
-			mavlink_msg_sys_status_send(_chan,
-						    v_status.onboard_control_sensors_present,
-						    v_status.onboard_control_sensors_enabled,
-						    v_status.onboard_control_sensors_health,
-						    v_status.load * 1000.0f,
-						    v_status.battery_voltage * 1000.0f,
-						    v_status.battery_current * 1000.0f,
-						    v_status.battery_remaining,
-						    v_status.drop_rate_comm,
-						    v_status.errors_comm,
-						    v_status.errors_count1,
-						    v_status.errors_count2,
-						    v_status.errors_count3,
-						    v_status.errors_count4);
+		MavlinkStream *stream;
+		LL_FOREACH(_streams, stream) {
+			stream->update(t);
 		}
 
 		/* 0.5 Hz */
-		if (lowspeed_counter % 20 == 0) {
-
+		if (lowspeed_counter % (2000000 / MAIN_LOOP_DELAY) == 0) {
 			mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission);
 		}
 
@@ -1800,26 +1651,27 @@ Mavlink::task_main(int argc, char *argv[])
 		/* check if waypoint has been reached against the last positions */
 		mavlink_waypoint_eventloop(hrt_absolute_time());
 
+		if (lowspeed_counter % (50000 / MAIN_LOOP_DELAY) == 0) {
+			/* send parameters at 20 Hz (if queued for sending) */
+			mavlink_pm_queued_send();
+			mavlink_waypoint_eventloop(hrt_absolute_time());
 
-		/* send parameters at 20 Hz (if queued for sending) */
-		mavlink_pm_queued_send();
-		mavlink_waypoint_eventloop(hrt_absolute_time());
-
-		mavlink_waypoint_eventloop(hrt_absolute_time());
+			mavlink_waypoint_eventloop(hrt_absolute_time());
 
-		if (_baudrate > 57600) {
-			mavlink_pm_queued_send();
-		}
+			if (_baudrate > 57600) {
+				mavlink_pm_queued_send();
+			}
 
-		 /* send one string at 10 Hz */
-		 if (!mavlink_logbuffer_is_empty(&lb)) {
-		 	struct mavlink_logmessage msg;
-		 	int lb_ret = mavlink_logbuffer_read(&lb, &msg);
+			 /* send one string at 10 Hz */
+			 if (!mavlink_logbuffer_is_empty(&lb)) {
+				struct mavlink_logmessage msg;
+				int lb_ret = mavlink_logbuffer_read(&lb, &msg);
 
-		 	if (lb_ret == OK) {
-		 		mavlink_missionlib_send_gcs_string(msg.text);
-		 	}
-		 }
+				if (lb_ret == OK) {
+					mavlink_missionlib_send_gcs_string(msg.text);
+				}
+			 }
+		}
 
 		perf_end(_loop_perf);
 	}
@@ -1878,7 +1730,7 @@ int mavlink_main(int argc, char *argv[])
 
 		// Instantiate thread
 		char buf[32];
-		sprintf(buf, "mavlink if%d", Mavlink::instance_count());
+		sprintf(buf, "mavlink_if%d", Mavlink::instance_count());
 
 		/*mavlink->_mavlink_task = */task_spawn_cmd(buf,
 						 SCHED_DEFAULT,
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 9461a51f8..56d262000 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -41,6 +41,12 @@
 #pragma once
 
 #include <stdbool.h>
+#include <nuttx/fs/fs.h>
+#include <drivers/drv_rc_input.h>
+#include <systemlib/param/param.h>
+#include <systemlib/perf_counter.h>
+#include <pthread.h>
+#include <mavlink/mavlink_log.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/sensor_combined.h>
@@ -68,12 +74,12 @@
 #include <uORB/topics/debug_key_value.h>
 #include <uORB/topics/airspeed.h>
 #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>
+#include "mavlink_bridge_header.h"
+#include "mavlink_orb_subscription.h"
+#include "mavlink_stream.h"
 
  // FIXME XXX - TO BE MOVED TO XML
 enum MAVLINK_WPM_STATES {
@@ -185,7 +191,7 @@ public:
 	 */
 	void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
 
-	void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
+	void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
 
 	/**
 	 * Enable / disable Hardware in the Loop simulation mode.
@@ -197,57 +203,10 @@ public:
 	 */
 	int		set_hil_enabled(bool hil_enabled);
 
-	struct mavlink_subscriptions {
-		int sensor_sub;
-		int att_sub;
-		int global_pos_sub;
-		int act_0_sub;
-		int act_1_sub;
-		int act_2_sub;
-		int act_3_sub;
-		int gps_sub;
-		int man_control_sp_sub;
-		int safety_sub;
-		int actuators_sub;
-		int armed_sub;
-		int actuators_effective_sub;
-		int local_pos_sub;
-		int spa_sub;
-		int spl_sub;
-		int triplet_sub;
-		int debug_key_value;
-		int input_rc_sub;
-		int optical_flow;
-		int rates_setpoint_sub;
-		int get_sub;
-		int airspeed_sub;
-		int navigation_capabilities_sub;
-		int position_setpoint_triplet_sub;
-		int rc_sub;
-		int status_sub;
-		int home_sub;
-	};
-
-	struct mavlink_subscriptions subs;
+	MavlinkOrbSubscription *add_orb_subscription(const struct orb_metadata *topic, size_t size);
 
-	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 */
-	struct vehicle_local_position_s local_pos;
-	/** navigation capabilities */
-	struct navigation_capabilities_s nav_cap;
-	/** Vehicle status */
-	struct vehicle_status_s v_status;
-	/** RC channels */
-	struct rc_channels_s rc;
-	/** Actuator armed state */
-	struct actuator_armed_s armed;
-	/** Position setpoint triplet */
-	struct position_setpoint_triplet_s pos_sp_triplet;
-
 	bool		_task_should_exit;		/**< if true, mavlink task should exit */
 
 protected:
@@ -270,7 +229,8 @@ private:
 	/* states */
 	bool		_mavlink_hil_enabled;		/**< Hardware in the loop mode */
 
-	int		_params_sub;
+	MavlinkOrbSubscription *_subscriptions;
+	MavlinkStream *_streams;
 
 	orb_advert_t	mission_pub;
 	struct mission_s mission;
@@ -307,21 +267,6 @@ private:
 	bool mavlink_link_termination_allowed;
 	
 	/**
-	 * Update our local parameter cache.
-	 */
-	void		parameters_update();
-
-	/**
-	 * Send all parameters at once.
-	 *
-	 * This function blocks until all parameters have been sent.
-	 * it delays each parameter by the passed amount of microseconds.
-	 *
-	 * @param delay		The delay in us between sending all parameters.
-	 */
-	void mavlink_pm_send_all_params(unsigned int delay);
-
-	/**
 	 * Send one parameter.
 	 *
 	 * @param param		The parameter id to send.
@@ -381,12 +326,7 @@ private:
 
 	int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
 
-	int set_mavlink_interval_limit(int mavlink_msg_id, int min_interval);
-
-	/**
-	 * Callback for param interface.
-	 */
-	static void mavlink_pm_callback(void *arg, param_t param);
+	int add_stream(const char *stream_name, const unsigned int interval);
 
 	static int	mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
 
diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_messages.cpp
index ea1e3a8bb..77ec344dc 100644
--- a/src/modules/mavlink/mavlink_orb_listener.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1,131 +1,138 @@
-/****************************************************************************
+/*
+ * mavlink_messages.cpp
  *
- *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file orb_listener.cpp
- * Monitors ORB topics and sends update messages as appropriate.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
+ *  Created on: 25.02.2014
+ *      Author: ton
  */
 
-// XXX trim includes
-#include <nuttx/config.h>
-#include <stdio.h>
-#include <math.h>
-#include <stdbool.h>
-#include <fcntl.h>
-#include <string.h>
-#include "mavlink_bridge_header.h"
-#include <drivers/drv_hrt.h>
-#include <time.h>
-#include <float.h>
-#include <unistd.h>
-#include <sys/prctl.h>
-#include <stdlib.h>
-#include <poll.h>
-#include <lib/geo/geo.h>
-#include <systemlib/err.h>
-
-#include <mavlink/mavlink_log.h>
+#include <commander/px4_custom_mode.h>
 
-#include "mavlink_orb_listener.h"
-#include "mavlink_main.h"
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/position_setpoint_triplet.h>
 
+#include "mavlink_messages.h"
 
-uint16_t cm_uint16_from_m_float(float m);
 
-uint16_t
-cm_uint16_from_m_float(float m)
-{
-	if (m < 0.0f) {
-		return 0;
-
-	} else if (m > 655.35f) {
-		return 65535;
-	}
+struct msgs_list_s msgs_list[] = {
+		{
+				.name = "HEARTBEAT",
+				.callback = msg_heartbeat,
+				.topics = { ORB_ID(vehicle_status), ORB_ID(position_setpoint_triplet), nullptr },
+				.sizes = { sizeof(struct vehicle_status_s), sizeof(struct position_setpoint_triplet_s) }
+		},
+		{
+				.name = "SYS_STATUS",
+				.callback = msg_sys_status,
+				.topics = { ORB_ID(vehicle_status), nullptr },
+				.sizes = { sizeof(struct vehicle_status_s) }
+		},
+		{ .name = nullptr }
+};
 
-	return (uint16_t)(m * 100.0f);
-}
-
-MavlinkOrbListener::MavlinkOrbListener(Mavlink* parent) :
-
-	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink orb")),
-	_mavlink(parent),
-	_subscriptions(nullptr),
-	_streams(nullptr)
+void
+msg_heartbeat(const MavlinkStream *stream)
 {
+	struct vehicle_status_s *status = (struct vehicle_status_s *)stream->subscriptions[0]->data;
+	struct position_setpoint_triplet_s *pos_sp_triplet = (struct position_setpoint_triplet_s *)stream->subscriptions[1]->data;
 
-}
-
-MavlinkOrbListener::~MavlinkOrbListener()
-{
+	uint8_t mavlink_state = 0;
+	uint8_t mavlink_base_mode = 0;
+	uint32_t mavlink_custom_mode = 0;
 
-}
+	/* HIL */
+	if (status->hil_state == HIL_STATE_ON) {
+		mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
+	}
 
-MavlinkOrbSubscription *MavlinkOrbListener::add_subscription(const struct orb_metadata *meta, const size_t size, const MavlinkStream *stream, const unsigned int interval)
-{
-	/* check if already subscribed to this topic */
-	MavlinkOrbSubscription *sub;
+	/* arming state */
+	if (status->arming_state == ARMING_STATE_ARMED
+			|| status->arming_state == ARMING_STATE_ARMED_ERROR) {
+		mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
+	}
 
-	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;
+	/* main state */
+	mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
+	union px4_custom_mode custom_mode;
+	custom_mode.data = 0;
+	if (pos_sp_triplet->nav_state == NAV_STATE_NONE) {
+	/* use main state when navigator is not active */
+		if (status->main_state == MAIN_STATE_MANUAL) {
+			mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
+		} else if (status->main_state == MAIN_STATE_SEATBELT) {
+			mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
+		} else if (status->main_state == MAIN_STATE_EASY) {
+			mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY;
+		} else if (status->main_state == MAIN_STATE_AUTO) {
+			mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
+		}
+	} else {
+		/* use navigation state when navigator is active */
+		mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		if (pos_sp_triplet->nav_state == NAV_STATE_READY) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
+		} else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
+		} else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
+		} else if (pos_sp_triplet->nav_state == NAV_STATE_RTL) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
+		} else if (pos_sp_triplet->nav_state == NAV_STATE_LAND) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
 		}
 	}
+	mavlink_custom_mode = custom_mode.data;
+
+	/* set system state */
+	if (status->arming_state == ARMING_STATE_INIT
+			|| status->arming_state == ARMING_STATE_IN_AIR_RESTORE
+			|| status->arming_state == ARMING_STATE_STANDBY_ERROR) {	// TODO review
+		mavlink_state = MAV_STATE_UNINIT;
+	} else if (status->arming_state == ARMING_STATE_ARMED) {
+		mavlink_state = MAV_STATE_ACTIVE;
+	} else if (status->arming_state == ARMING_STATE_ARMED_ERROR) {
+		mavlink_state = MAV_STATE_CRITICAL;
+	} else if (status->arming_state == ARMING_STATE_STANDBY) {
+		mavlink_state = MAV_STATE_STANDBY;
+	} else if (status->arming_state == ARMING_STATE_REBOOT) {
+		mavlink_state = MAV_STATE_POWEROFF;
+	} else {
+		mavlink_state = MAV_STATE_CRITICAL;
+	}
 
-	/* add new subscription */
-	MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(meta, size);
-
-	sub_new->set_interval(interval);
-
-	LL_APPEND(_subscriptions, sub_new);
-
-	return sub_new;
+	/* send heartbeat */
+	mavlink_msg_heartbeat_send(stream->mavlink->get_chan(),
+				   mavlink_system.type,
+				   MAV_AUTOPILOT_PX4,
+				   mavlink_base_mode,
+				   mavlink_custom_mode,
+				   mavlink_state);
 }
 
-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)
+void
+msg_sys_status(const MavlinkStream *stream)
 {
-	MavlinkStream *stream = new MavlinkStream(this, callback, subs_n, metas, sizes, arg, interval);
-
-	stream->mavlink = _mavlink;
-
-	LL_APPEND(_streams, stream);
+	struct vehicle_status_s *status = (struct vehicle_status_s *)stream->subscriptions[0]->data;
+
+	mavlink_msg_sys_status_send(stream->mavlink->get_chan(),
+				    status->onboard_control_sensors_present,
+				    status->onboard_control_sensors_enabled,
+				    status->onboard_control_sensors_health,
+				    status->load * 1000.0f,
+				    status->battery_voltage * 1000.0f,
+				    status->battery_current * 1000.0f,
+				    status->battery_remaining,
+				    status->drop_rate_comm,
+				    status->errors_comm,
+				    status->errors_count1,
+				    status->errors_count2,
+				    status->errors_count3,
+				    status->errors_count4);
 }
 
 //void
@@ -259,25 +266,6 @@ void MavlinkOrbListener::add_stream(void (*callback)(const MavlinkStream *), con
 //	l->listener->gps_counter++;
 //}
 //
-
-void
-MavlinkOrbListener::msg_heartbeat(const MavlinkStream *stream)
-{
-	/* 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);
-
-	/* send heartbeat */
-	mavlink_msg_heartbeat_send(stream->mavlink->get_chan(),
-				   mavlink_system.type,
-				   MAV_AUTOPILOT_PX4,
-				   mavlink_base_mode,
-				   mavlink_custom_mode,
-				   mavlink_state);
-}
-
 //
 //void
 //MavlinkOrbListener::l_rc_channels(const struct listener *l)
@@ -623,57 +611,3 @@ MavlinkOrbListener::msg_heartbeat(const MavlinkStream *stream)
 //				   l->listener->nav_cap.turn_distance);
 //
 //}
-
-void *
-MavlinkOrbListener::uorb_receive_thread(void *arg)
-{
-	/* set thread name */
-	char thread_name[18];
-	sprintf(thread_name, "mavlink_uorb_rcv_%d", _mavlink->get_channel());
-	prctl(PR_SET_NAME, thread_name, getpid());
-
-	/* add mavlink streams */
-	/* common buffer for topics and data sizes */
-	const struct orb_metadata *topics[1];
-	size_t sizes[1];
-
-	/* --- 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) {
-		/* check all streams each 1ms */
-		hrt_abstime t = hrt_absolute_time();
-		MavlinkStream *stream;
-		LL_FOREACH(_streams, stream) {
-			stream->update(t);
-		}
-		usleep(1000);
-	}
-
-	return NULL;
-}
-
-void * MavlinkOrbListener::uorb_start_helper(void *context)
-{
-	MavlinkOrbListener* urcv = new MavlinkOrbListener(((Mavlink *)context));
-	return urcv->uorb_receive_thread(NULL);
-}
-
-pthread_t
-MavlinkOrbListener::uorb_receive_start(Mavlink* mavlink)
-{
-	/* start the listener loop */
-	pthread_attr_t uorb_attr;
-	pthread_attr_init(&uorb_attr);
-
-	/* Set stack size, needs less than 2k */
-	pthread_attr_setstacksize(&uorb_attr, 2048);
-
-	pthread_t thread;
-	pthread_create(&thread, &uorb_attr, MavlinkOrbListener::uorb_start_helper, (void*)mavlink);
-
-	pthread_attr_destroy(&uorb_attr);
-	return thread;
-}
diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h
new file mode 100644
index 000000000..78279c08f
--- /dev/null
+++ b/src/modules/mavlink/mavlink_messages.h
@@ -0,0 +1,28 @@
+/*
+ * mavlink_messages.h
+ *
+ *  Created on: 25.02.2014
+ *      Author: ton
+ */
+
+#ifndef MAVLINK_MESSAGES_H_
+#define MAVLINK_MESSAGES_H_
+
+#include "mavlink_stream.h"
+
+#define MAX_TOPICS_PER_MAVLINK_STREAM 4
+
+struct msgs_list_s {
+	char *name;
+	void (*callback)(const MavlinkStream *);
+	const struct orb_metadata *topics[MAX_TOPICS_PER_MAVLINK_STREAM+1];
+	size_t sizes[MAX_TOPICS_PER_MAVLINK_STREAM+1];
+};
+
+extern struct msgs_list_s msgs_list[];
+
+static void msg_heartbeat(const MavlinkStream *stream);
+static void msg_sys_status(const MavlinkStream *stream);
+
+
+#endif /* MAVLINK_MESSAGES_H_ */
diff --git a/src/modules/mavlink/mavlink_orb_listener.h b/src/modules/mavlink/mavlink_orb_listener.h
deleted file mode 100644
index 1a103e43d..000000000
--- a/src/modules/mavlink/mavlink_orb_listener.h
+++ /dev/null
@@ -1,138 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file mavlink_orb_listener.h
- * MAVLink 1.0 protocol interface definition.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
- */
-
-#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>
-#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/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 <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
-{
-public:
-	/**
-	 * Constructor
-	 */
-	MavlinkOrbListener(Mavlink* parent);
-
-	/**
-	 * Destructor, closes all subscriptions.
-	 */
-	~MavlinkOrbListener();
-
-	static pthread_t uorb_receive_start(Mavlink *mavlink);
-
-	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:
-
-	perf_counter_t	_loop_perf;			/**< loop performance counter */
-
-	Mavlink*	_mavlink;
-
-	MavlinkOrbSubscription *_subscriptions;
-	static const unsigned _max_subscriptions = 32;
-	MavlinkStream *_streams;
-
-	void *uorb_receive_thread(void *arg);
-
-	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
index 16044ef72..84ac11483 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.cpp
+++ b/src/modules/mavlink/mavlink_orb_subscription.cpp
@@ -13,14 +13,13 @@
 
 #include "mavlink_orb_subscription.h"
 
-MavlinkOrbSubscription::MavlinkOrbSubscription(const struct orb_metadata *meta, size_t size)
+MavlinkOrbSubscription::MavlinkOrbSubscription(const struct orb_metadata *topic, size_t size)
 {
-	this->meta = meta;
+	this->topic = topic;
 	this->data = malloc(size);
 	memset(this->data, 0, size);
-	this->fd = orb_subscribe(meta);
+	this->fd = orb_subscribe(topic);
 	this->last_update = 0;
-	this->interval = 0;
 }
 
 MavlinkOrbSubscription::~MavlinkOrbSubscription()
@@ -29,19 +28,15 @@ MavlinkOrbSubscription::~MavlinkOrbSubscription()
 	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)
+bool MavlinkOrbSubscription::update(const hrt_abstime t)
 {
 	if (last_update != t) {
 		bool updated;
 		orb_check(fd, &updated);
-		if (updated)
-			return orb_copy(meta, fd, data);
+		if (updated) {
+			orb_copy(topic, fd, data);
+			return true;
+		}
 	}
-	return OK;
+	return false;
 }
diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h
index 2c72abaef..9a7340e9b 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.h
+++ b/src/modules/mavlink/mavlink_orb_subscription.h
@@ -16,14 +16,12 @@ public:
 	MavlinkOrbSubscription(const struct orb_metadata *meta, size_t size);
 	~MavlinkOrbSubscription();
 
-	int set_interval(const unsigned int interval);
-	int update(const hrt_abstime t);
+	bool update(const hrt_abstime t);
 
-	const struct orb_metadata *meta;
+	const struct orb_metadata *topic;
 	int fd;
 	void *data;
 	hrt_abstime last_update;
-	unsigned int interval;
 	MavlinkOrbSubscription *next;
 };
 
diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp
index 2a6728fdb..9df4263ee 100644
--- a/src/modules/mavlink/mavlink_stream.cpp
+++ b/src/modules/mavlink/mavlink_stream.cpp
@@ -7,20 +7,20 @@
 
 #include <stdlib.h>
 
-#include "mavlink_orb_listener.h"
+#include "mavlink_stream.h"
+#include "mavlink_main.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)
+MavlinkStream::MavlinkStream(Mavlink *mavlink, void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **topics, 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);
+		this->subscriptions[i] = mavlink->add_orb_subscription(topics[i], sizes[i]);
 	}
 }
 
diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h
index 0f959d720..c3e60917e 100644
--- a/src/modules/mavlink/mavlink_stream.h
+++ b/src/modules/mavlink/mavlink_stream.h
@@ -8,8 +8,13 @@
 #ifndef MAVLINK_STREAM_H_
 #define MAVLINK_STREAM_H_
 
+#include <drivers/drv_hrt.h>
+
 class Mavlink;
-class MavlinkOrbListener;
+class MavlinkStream;
+
+#include "mavlink_main.h"
+
 class MavlinkOrbSubscription;
 
 class MavlinkStream {
@@ -22,9 +27,8 @@ public:
 	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(Mavlink *mavlink, void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **topics, const size_t *sizes, const uintptr_t arg, const unsigned int interval);
 	~MavlinkStream();
 	int update(const hrt_abstime t);
 };
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index dc8d5586f..5ea6c0f46 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -39,8 +39,8 @@ MODULE_COMMAND	 = mavlink
 SRCS		 += mavlink_main.cpp \
 			mavlink.c \
 			mavlink_receiver.cpp \
-			mavlink_orb_listener.cpp \
 			mavlink_orb_subscription.cpp \
+			mavlink_messages.cpp \
 			mavlink_stream.cpp
 
 INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink