9 files changed, 443 insertions, 361 deletions

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 1563a257b..33d81729c 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -152,12 +152,6 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
 
 static void usage(void);
 
-namespace mavlink
-{
-
-	Mavlink	*g_mavlink;
-}
-
 Mavlink::Mavlink() :
 	device_name("/dev/ttyS1"),
 	_task_should_exit(false),
@@ -166,14 +160,13 @@ Mavlink::Mavlink() :
 	thread_running(false),
 	_mavlink_task(-1),
 	_mavlink_incoming_fd(-1),
-
-/* performance counters */
-	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink")),
 	_mavlink_hil_enabled(false),
 	_subscriptions(nullptr),
 	_streams(nullptr),
+	mission_pub(-1),
 
-	mission_pub(-1)
+/* performance counters */
+	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink"))
 {
 	wpm = &wpm_s;
 	fops.ioctl = (int (*)(file*, int, long unsigned int))&mavlink_dev_ioctl;
@@ -204,12 +197,14 @@ Mavlink::~Mavlink()
 	}
 }
 
-void Mavlink::set_mode(enum MAVLINK_MODE mode)
+void
+Mavlink::set_mode(enum MAVLINK_MODE mode)
 {
 	_mode = mode;
 }
 
-int Mavlink::instance_count()
+int
+Mavlink::instance_count()
 {
 	/* note: a local buffer count will help if this ever is called often */
 	Mavlink* inst = ::_head;
@@ -222,10 +217,11 @@ int Mavlink::instance_count()
 	return inst_index;
 }
 
-Mavlink* Mavlink::new_instance()
+Mavlink *
+Mavlink::new_instance()
 {
-	Mavlink* inst = new Mavlink();
-	Mavlink* next = ::_head;
+	Mavlink *inst = new Mavlink();
+	Mavlink *next = ::_head;
 
 	/* create the first instance at _head */
 	if (::_head == nullptr) {
@@ -241,9 +237,10 @@ Mavlink* Mavlink::new_instance()
 	return inst;
 }
 
-Mavlink* Mavlink::get_instance(unsigned instance)
+Mavlink *
+Mavlink::get_instance(unsigned instance)
 {
-	Mavlink* inst = ::_head;
+	Mavlink *inst = ::_head;
 	unsigned inst_index = 0;
 	while (inst->_next != nullptr && inst_index < instance) {
 		inst = inst->_next;
@@ -257,7 +254,8 @@ Mavlink* Mavlink::get_instance(unsigned instance)
 	return inst;
 }
 
-int Mavlink::destroy_all_instances()
+int
+Mavlink::destroy_all_instances()
 {
 	/* start deleting from the end */
 	Mavlink *inst_to_del = nullptr;
@@ -294,7 +292,8 @@ int Mavlink::destroy_all_instances()
 	return OK;
 }
 
-bool Mavlink::instance_exists(const char *device_name, Mavlink *self)
+bool
+Mavlink::instance_exists(const char *device_name, Mavlink *self)
 {
 	Mavlink* inst = ::_head;
 	while (inst != nullptr) {
@@ -307,7 +306,8 @@ bool Mavlink::instance_exists(const char *device_name, Mavlink *self)
 	return false;
 }
 
-int Mavlink::get_uart_fd(unsigned index)
+int
+Mavlink::get_uart_fd(unsigned index)
 {
 	Mavlink* inst = get_instance(index);
 	if (inst)
@@ -316,14 +316,16 @@ int Mavlink::get_uart_fd(unsigned index)
 	return -1;
 }
 
-int Mavlink::get_uart_fd()
+int
+Mavlink::get_uart_fd()
 {
 	return _uart;
 }
 
-int Mavlink::get_channel()
+mavlink_channel_t
+Mavlink::get_channel()
 {
-	return (int)_chan;
+	return _chan;
 }
 
 /****************************************************************************
@@ -1364,7 +1366,7 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const struct orb_metadata
 	MavlinkOrbSubscription *sub;
 
 	LL_FOREACH(_subscriptions, sub) {
-		if (sub->topic == topic) {
+		if (sub->get_topic() == topic) {
 			/* already subscribed */
 			return sub;
 		}
@@ -1379,26 +1381,19 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const struct orb_metadata
 }
 
 int
-Mavlink::add_stream(const char *stream_name, const unsigned int interval)
+Mavlink::add_stream(const char *stream_name, const float rate)
 {
-	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);
+	/* search for stream with specified name */
+	for (unsigned int i = 0; streams_list[i] != nullptr; i++) {
+		if (strcmp(stream_name, streams_list[i]->get_name()) == 0) {
+			/* create stream copy for each mavlink instance */
+			MavlinkStream *stream = streams_list[i]->new_instance();
+			stream->set_channel(get_channel());
+			stream->set_interval(1000.0f / rate);
+			stream->subscribe(this);
 			LL_APPEND(_streams, stream);
 			return OK;
 		}
-		i++;
 	}
 	return ERROR;
 }
@@ -1407,7 +1402,7 @@ int
 Mavlink::task_main(int argc, char *argv[])
 {
 	/* inform about start */
-	warnx("Initializing..");
+	warnx("start");
 	fflush(stdout);
 
 	/* initialize mavlink text message buffering */
@@ -1463,9 +1458,6 @@ Mavlink::task_main(int argc, char *argv[])
 
 	bool usb_uart;
 
-	/* print welcome text */
-	warnx("MAVLink v1.0 serial interface starting...");
-
 	/* inform about mode */
 	switch (_mode) {
 		case MODE_TX_HEARTBEAT_ONLY:
@@ -1510,13 +1502,13 @@ Mavlink::task_main(int argc, char *argv[])
 		err(1, "could not open %s", device_name);
 
 	/* create the device node that's used for sending text log messages, etc. */
-	register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL);
+	if (instance_count() == 1) {
+		register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL);
+	}
 
 	/* initialize logging device */
 	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 
-	mavlink_log_info(_mavlink_fd, "[mavlink] started");
-
 	/* Initialize system properties */
 	mavlink_update_system();
 
@@ -1537,8 +1529,10 @@ Mavlink::task_main(int argc, char *argv[])
 	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));
 
-	struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->data;
+	struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->get_data();
 
+	warnx("started");
+	mavlink_log_info(_mavlink_fd, "[mavlink] started");
 
 	/* add default streams, intervals depend on baud rate */
 //	if (_baudrate >= 230400) {
@@ -1546,15 +1540,15 @@ Mavlink::task_main(int argc, char *argv[])
 //	} else if (_baudrate >= 57600) {
 //	}
 
-	add_stream("HEARTBEAT", 1000);
-	add_stream("SYS_STATUS", 1000);
-	add_stream("HIGHRES_IMU", 300);
-	add_stream("RAW_IMU", 300);
-	add_stream("ATTITUDE", 200);
-	add_stream("NAMED_VALUE_FLOAT", 200);
-	add_stream("SERVO_OUTPUT_RAW", 500);
-	add_stream("GPS_RAW_INT", 500);
-	add_stream("MANUAL_CONTROL", 500);
+	add_stream("HEARTBEAT", 1.0f);
+	add_stream("SYS_STATUS", 1.0f);
+	add_stream("HIGHRES_IMU", 20.0f);
+//	add_stream("RAW_IMU", 10.0f);
+	add_stream("ATTITUDE", 20.0f);
+//	add_stream("NAMED_VALUE_FLOAT", 5.0f);
+//	add_stream("SERVO_OUTPUT_RAW", 2.0f);
+//	add_stream("GPS_RAW_INT", 2.0f);
+//	add_stream("MANUAL_CONTROL", 2.0f);
 
 	while (!_task_should_exit) {
 		/* main loop */
@@ -1604,31 +1598,24 @@ Mavlink::task_main(int argc, char *argv[])
 			mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission);
 		}
 
-		mavlink_waypoint_eventloop(hrt_absolute_time());
-
-		/* 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());
 
-			mavlink_waypoint_eventloop(hrt_absolute_time());
-
 			if (_baudrate > 57600) {
 				mavlink_pm_queued_send();
 			}
 
-			 /* send one string at 10 Hz */
-			 if (!mavlink_logbuffer_is_empty(&lb)) {
+			 /* send one string at 20 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);
 				}
-			 }
+			}
 		}
 
 		perf_end(_loop_perf);
@@ -1636,7 +1623,6 @@ Mavlink::task_main(int argc, char *argv[])
 
 	/* wait for threads to complete */
 	pthread_join(receive_thread, NULL);
-	pthread_join(uorb_receive_thread, NULL);
 
 	/* Reset the UART flags to original state */
 	tcsetattr(_uart, TCSANOW, &uart_config_original);
@@ -1654,15 +1640,50 @@ Mavlink::task_main(int argc, char *argv[])
 
 int Mavlink::start_helper(int argc, char *argv[])
 {
-	// Create the instance in task context
+	/* create the instance in task context */
 	Mavlink *instance = Mavlink::new_instance();
-	// This will actually only return once MAVLink exits
+	/* this will actually only return once MAVLink exits */
 	return instance->task_main(argc, argv);
 }
 
 int
-Mavlink::start()
+Mavlink::start(int argc, char *argv[])
 {
+	// Instantiate thread
+	char buf[32];
+	sprintf(buf, "mavlink_if%d", Mavlink::instance_count());
+
+	/*mavlink->_mavlink_task = */task_spawn_cmd(buf,
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_DEFAULT,
+					 4096,
+					 (main_t)&Mavlink::start_helper,
+					 (const char **)argv);
+
+	// while (!this->is_running()) {
+	// 		usleep(200);
+	// }
+
+	// if (mavlink->_mavlink_task < 0) {
+	// 	warn("task start failed");
+	// 	return -errno;
+	// }
+
+	// if (mavlink::g_mavlink != nullptr) {
+	// 	errx(1, "already running");
+	// }
+
+	// mavlink::g_mavlink = new Mavlink;
+
+	// if (mavlink::g_mavlink == nullptr) {
+	// 	errx(1, "alloc failed");
+	// }
+
+	// if (OK != mavlink::g_mavlink->start()) {
+	// 	delete mavlink::g_mavlink;
+	// 	mavlink::g_mavlink = nullptr;
+	// 	err(1, "start failed");
+	// }
 
 	return OK;
 }
@@ -1685,44 +1706,7 @@ int mavlink_main(int argc, char *argv[])
 	}
 
 	if (!strcmp(argv[1], "start")) {
-
-		// Instantiate thread
-		char buf[32];
-		sprintf(buf, "mavlink_if%d", Mavlink::instance_count());
-
-		/*mavlink->_mavlink_task = */task_spawn_cmd(buf,
-						 SCHED_DEFAULT,
-						 SCHED_PRIORITY_DEFAULT,
-						 2048,
-						 (main_t)&Mavlink::start_helper,
-						 (const char **)argv);
-
-		// while (!this->is_running()) {
-		// 		usleep(200);
-		// }
-
-		// if (mavlink->_mavlink_task < 0) {
-		// 	warn("task start failed");
-		// 	return -errno;
-		// }
-
-		// if (mavlink::g_mavlink != nullptr) {
-		// 	errx(1, "already running");
-		// }
-
-		// mavlink::g_mavlink = new Mavlink;
-
-		// if (mavlink::g_mavlink == nullptr) {
-		// 	errx(1, "alloc failed");
-		// }
-
-		// if (OK != mavlink::g_mavlink->start()) {
-		// 	delete mavlink::g_mavlink;
-		// 	mavlink::g_mavlink = nullptr;
-		// 	err(1, "start failed");
-		// }
-
-		return 0;
+		return Mavlink::start(argc, argv);
 
 	} else if (!strcmp(argv[1], "stop")) {
 		warnx("mavlink stop is deprecated, use stop-all instead");
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 56d262000..e7f3486da 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -142,7 +142,7 @@ public:
 	 *
 	 * @return		OK on success.
 	 */
-	static int		start();
+	static int		start(int argc, char *argv[]);
 
 	/**
 	 * Display the mavlink status.
@@ -163,8 +163,6 @@ public:
 
 	int get_uart_fd();
 
-	int get_channel();
-
 	const char *device_name;
 
 	enum MAVLINK_MODE {
@@ -205,16 +203,11 @@ public:
 
 	MavlinkOrbSubscription *add_orb_subscription(const struct orb_metadata *topic, size_t size);
 
-	mavlink_channel_t get_chan() { return _chan; }
+	mavlink_channel_t get_channel();
 
 	bool		_task_should_exit;		/**< if true, mavlink task should exit */
 
 protected:
-	/**
-	 * Pointer to the default cdev file operations table; useful for
-	 * registering clone devices etc.
-	 */
-
 	Mavlink*	_next;
 
 private:
@@ -234,7 +227,7 @@ private:
 
 	orb_advert_t	mission_pub;
 	struct mission_s mission;
-	uint8_t missionlib_msg_buf[300]; //XXX MAGIC NUMBER
+	uint8_t missionlib_msg_buf[sizeof(mavlink_message_t)];
 	MAVLINK_MODE _mode;
 
 	uint8_t _mavlink_wpm_comp_id;
@@ -247,7 +240,6 @@ private:
 	unsigned int total_counter;
 
 	pthread_t receive_thread;
-	pthread_t uorb_receive_thread;
 
 	/* Allocate storage space for waypoints */
 	mavlink_wpm_storage wpm_s;
@@ -326,7 +318,7 @@ private:
 
 	int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
 
-	int add_stream(const char *stream_name, const unsigned int interval);
+	int add_stream(const char *stream_name, const float rate);
 
 	static int	mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
 
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 7a56c36a5..df73581f0 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -12,195 +12,289 @@
 
 #include "mavlink_messages.h"
 
+class MavlinkStreamHeartbeat : public MavlinkStream {
+public:
+	const char *get_name()
+	{
+		return "HEARTBEAT";
+	}
 
-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 }
-};
+	MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamHeartbeat();
+	}
 
-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;
+private:
+	MavlinkOrbSubscription *status_sub;
+	MavlinkOrbSubscription *pos_sp_triplet_sub;
 
-	uint8_t mavlink_state = 0;
-	uint8_t mavlink_base_mode = 0;
-	uint32_t mavlink_custom_mode = 0;
+	struct vehicle_status_s *status;
+	struct position_setpoint_triplet_s *pos_sp_triplet;
 
-	/* HIL */
-	if (status->hil_state == HIL_STATE_ON) {
-		mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
-	}
+protected:
+	void subscribe(Mavlink *mavlink)
+	{
+		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status), sizeof(struct vehicle_status_s));
+		status = (struct vehicle_status_s *)status_sub->get_data();
 
-	/* arming state */
-	if (status->arming_state == ARMING_STATE_ARMED
-			|| status->arming_state == ARMING_STATE_ARMED_ERROR) {
-		mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
+		pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet), sizeof(position_setpoint_triplet_s));
+		pos_sp_triplet = (struct position_setpoint_triplet_s *)status_sub->get_data();
 	}
 
-	/* 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) {
+	void send(const hrt_abstime t) {
+		status_sub->update(t);
+		pos_sp_triplet_sub->update(t);
+
+		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;
+		}
+
+		/* arming state */
+		if (status->arming_state == ARMING_STATE_ARMED
+				|| 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 (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;
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
+			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;
+			}
 		}
-	} 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;
 		}
+
+		mavlink_msg_heartbeat_send(_channel,
+					   mavlink_system.type,
+					   MAV_AUTOPILOT_PX4,
+					   mavlink_base_mode,
+					   mavlink_custom_mode,
+					   mavlink_state);
+
 	}
-	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;
+};
+
+
+class MavlinkStreamSysStatus : public MavlinkStream {
+public:
+	const char *get_name()
+	{
+		return "SYS_STATUS";
 	}
 
-	/* 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
-msg_sys_status(const MavlinkStream *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
-msg_highres_imu(const MavlinkStream *stream)
-{
-	struct sensor_combined_s *sensor = (struct sensor_combined_s *)stream->subscriptions[0]->data;
-
-	uint16_t fields_updated = 0;
-
-	if (stream->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
-		mavlink_msg_highres_imu_send(stream->mavlink->get_chan(), sensor->timestamp,
-					     sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2],
-					     sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2],
-					     sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2],
-					     sensor->baro_pres_mbar, sensor->differential_pressure_pa,
-					     sensor->baro_alt_meter, sensor->baro_temp_celcius,
-					     fields_updated);
-}
+	MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamSysStatus();
+	}
+
+private:
+	MavlinkOrbSubscription *status_sub;
+
+	struct vehicle_status_s *status;
+
+protected:
+
+	void subscribe(Mavlink *mavlink)
+	{
+		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status), sizeof(struct vehicle_status_s));
+		status = (struct vehicle_status_s *)status_sub->get_data();
+	}
+
+	void send(const hrt_abstime t) {
+		status_sub->update(t);
+
+		mavlink_msg_sys_status_send(_channel,
+				status->onboard_control_sensors_present,
+				status->onboard_control_sensors_enabled,
+				status->onboard_control_sensors_health,
+				status->load * 1000.0f,
+				status->battery_voltage * 1000.0f,
+				status->battery_current * 1000.0f,
+				status->battery_remaining,
+				status->drop_rate_comm,
+				status->errors_comm,
+				status->errors_count1,
+				status->errors_count2,
+				status->errors_count3,
+				status->errors_count4);
+	}
+};
+
+
+class MavlinkStreamHighresIMU : public MavlinkStream {
+public:
+	const char *get_name()
+	{
+		return "HIGHRES_IMU";
+	}
+
+	MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamHighresIMU();
+	}
+
+private:
+	MavlinkOrbSubscription *sensor_sub;
+
+	struct sensor_combined_s *sensor;
+
+	uint32_t accel_counter = 0;
+	uint32_t gyro_counter = 0;
+	uint32_t mag_counter = 0;
+	uint32_t baro_counter = 0;
+
+protected:
+
+	void subscribe(Mavlink *mavlink)
+	{
+		sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined), sizeof(struct sensor_combined_s));
+		sensor = (struct sensor_combined_s *)sensor_sub->get_data();
+	}
+
+	void send(const hrt_abstime t) {
+		sensor_sub->update(t);
+
+		uint16_t fields_updated = 0;
+
+		if (accel_counter != sensor->accelerometer_counter) {
+			/* mark first three dimensions as changed */
+			fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
+			accel_counter = sensor->accelerometer_counter;
+		}
+
+		if (gyro_counter != sensor->gyro_counter) {
+			/* mark second group dimensions as changed */
+			fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
+			gyro_counter = sensor->gyro_counter;
+		}
+
+		if (mag_counter != sensor->magnetometer_counter) {
+			/* mark third group dimensions as changed */
+			fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
+			mag_counter = sensor->magnetometer_counter;
+		}
+
+		if (baro_counter != sensor->baro_counter) {
+			/* mark last group dimensions as changed */
+			fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
+			baro_counter = sensor->baro_counter;
+		}
+
+		mavlink_msg_highres_imu_send(_channel,
+				sensor->timestamp,
+				sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2],
+				sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2],
+				sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2],
+				sensor->baro_pres_mbar, sensor->differential_pressure_pa,
+				sensor->baro_alt_meter, sensor->baro_temp_celcius,
+				fields_updated);
+	}
+};
+
+
+class MavlinkStreamAttitude : public MavlinkStream {
+public:
+	const char *get_name()
+	{
+		return "ATTITUDE";
+	}
+
+	MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamAttitude();
+	}
+
+private:
+	MavlinkOrbSubscription *att_sub;
+
+	struct vehicle_attitude_s *att;
+
+protected:
+
+	void subscribe(Mavlink *mavlink)
+	{
+		att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude), sizeof(struct vehicle_attitude_s));
+		att = (struct vehicle_attitude_s *)att_sub->get_data();
+	}
+
+	void send(const hrt_abstime t) {
+		att_sub->update(t);
+
+		mavlink_msg_attitude_send(_channel,
+				att->timestamp / 1000,
+				att->roll, att->pitch, att->yaw,
+				att->rollspeed, att->pitchspeed, att->yawspeed);
+	}
+};
+
+
+MavlinkStream *streams_list[] = {
+		new MavlinkStreamHeartbeat(),
+		new MavlinkStreamSysStatus(),
+		new MavlinkStreamHighresIMU(),
+		new MavlinkStreamAttitude(),
+		nullptr
+};
+
+
+
+
+
+
+
+
 
-//void
-//MavlinkOrbListener::l_sensor_combined(const struct listener *l)
-//{
-//	struct sensor_combined_s raw;
-//
-//	/* copy sensors raw data into local buffer */
-//	orb_copy(ORB_ID(sensor_combined), l->mavlink->get_subs()->sensor_sub, &raw);
-//
-//	/* mark individual fields as _mavlink->get_chan()ged */
-//	uint16_t fields_updated = 0;
-//
-//	// if (accel_counter != raw.accelerometer_counter) {
-//	// 	/* mark first three dimensions as _mavlink->get_chan()ged */
-//	// 	fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
-//	// 	accel_counter = raw.accelerometer_counter;
-//	// }
-//
-//	// if (gyro_counter != raw.gyro_counter) {
-//	// 	/* mark second group dimensions as _mavlink->get_chan()ged */
-//	// 	fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
-//	// 	gyro_counter = raw.gyro_counter;
-//	// }
-//
-//	// if (mag_counter != raw.magnetometer_counter) {
-//	// 	/* mark third group dimensions as _mavlink->get_chan()ged */
-//	// 	fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
-//	// 	mag_counter = raw.magnetometer_counter;
-//	// }
-//
-//	// if (baro_counter != raw.baro_counter) {
-//	// 	/* mark last group dimensions as _mavlink->get_chan()ged */
-//	// 	fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
-//	// 	baro_counter = raw.baro_counter;
-//	// }
-//
-//	if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
-//		mavlink_msg_highres_imu_send(l->mavlink->get_chan(), l->listener->last_sensor_timestamp,
-//					     raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1],
-//					     raw.accelerometer_m_s2[2], raw.gyro_rad_s[0],
-//					     raw.gyro_rad_s[1], raw.gyro_rad_s[2],
-//					     raw.magnetometer_ga[0],
-//					     raw.magnetometer_ga[1], raw.magnetometer_ga[2],
-//					     raw.baro_pres_mbar, raw.differential_pressure_pa,
-//					     raw.baro_alt_meter, raw.baro_temp_celcius,
-//					     fields_updated);
-//
-//	l->listener->sensors_raw_counter++;
-//}
-//
 //void
 //MavlinkOrbListener::l_vehicle_attitude(const struct listener *l)
 //{
diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h
index a6326bad1..3dc6cb699 100644
--- a/src/modules/mavlink/mavlink_messages.h
+++ b/src/modules/mavlink/mavlink_messages.h
@@ -10,19 +10,6 @@
 
 #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);
-static void msg_highres_imu(const MavlinkStream *stream);
+extern MavlinkStream *streams_list[];
 
 #endif /* MAVLINK_MESSAGES_H_ */
diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp
index 84ac11483..b504b6955 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.cpp
+++ b/src/modules/mavlink/mavlink_orb_subscription.cpp
@@ -13,28 +13,40 @@
 
 #include "mavlink_orb_subscription.h"
 
-MavlinkOrbSubscription::MavlinkOrbSubscription(const struct orb_metadata *topic, size_t size)
+MavlinkOrbSubscription::MavlinkOrbSubscription(const struct orb_metadata *topic, size_t size) : _topic(topic), _last_check(0), next(nullptr)
 {
-	this->topic = topic;
-	this->data = malloc(size);
-	memset(this->data, 0, size);
-	this->fd = orb_subscribe(topic);
-	this->last_update = 0;
+	_data = malloc(size);
+	memset(_data, 0, size);
+	_fd = orb_subscribe(_topic);
 }
 
 MavlinkOrbSubscription::~MavlinkOrbSubscription()
 {
-	close(fd);
-	free(data);
+	close(_fd);
+	free(_data);
 }
 
-bool MavlinkOrbSubscription::update(const hrt_abstime t)
+const struct orb_metadata *
+MavlinkOrbSubscription::get_topic()
 {
-	if (last_update != t) {
+	return _topic;
+}
+
+void *
+MavlinkOrbSubscription::get_data()
+{
+	return _data;
+}
+
+bool
+MavlinkOrbSubscription::update(const hrt_abstime t)
+{
+	if (_last_check != t) {
+		_last_check = t;
 		bool updated;
-		orb_check(fd, &updated);
+		orb_check(_fd, &updated);
 		if (updated) {
-			orb_copy(topic, fd, data);
+			orb_copy(_topic, _fd, _data);
 			return true;
 		}
 	}
diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h
index 9a7340e9b..c38a9cc43 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.h
+++ b/src/modules/mavlink/mavlink_orb_subscription.h
@@ -11,18 +11,23 @@
 #include <systemlib/uthash/utlist.h>
 #include <drivers/drv_hrt.h>
 
+
 class MavlinkOrbSubscription {
 public:
-	MavlinkOrbSubscription(const struct orb_metadata *meta, size_t size);
+	MavlinkOrbSubscription *next;
+
+	MavlinkOrbSubscription(const struct orb_metadata *topic, size_t size);
 	~MavlinkOrbSubscription();
 
 	bool update(const hrt_abstime t);
+	void *get_data();
+	const struct orb_metadata *get_topic();
 
-	const struct orb_metadata *topic;
-	int fd;
-	void *data;
-	hrt_abstime last_update;
-	MavlinkOrbSubscription *next;
+private:
+	const struct orb_metadata *_topic;
+	int _fd;
+	void *_data;
+	hrt_abstime _last_check;
 };
 
 
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index d07de0f22..b828420e6 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -818,7 +818,7 @@ MavlinkReceiver::receive_thread(void *arg)
 
 			/* if read failed, this loop won't execute */
 			for (ssize_t i = 0; i < nread; i++) {
-				if (mavlink_parse_char(_mavlink->get_chan(), buf[i], &msg, &status)) {
+				if (mavlink_parse_char(_mavlink->get_channel(), buf[i], &msg, &status)) {
 					/* handle generic messages and commands */
 					handle_message(&msg);
 
diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp
index 9df4263ee..16407366e 100644
--- a/src/modules/mavlink/mavlink_stream.cpp
+++ b/src/modules/mavlink/mavlink_stream.cpp
@@ -10,39 +10,42 @@
 #include "mavlink_stream.h"
 #include "mavlink_main.h"
 
-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)
+MavlinkStream::MavlinkStream() : _interval(1000), _last_sent(0), _channel(MAVLINK_COMM_0), next(nullptr)
 {
-	this->callback = callback;
-	this->arg = arg;
-	this->interval = interval * 1000;
-	this->mavlink = mavlink;
-	this->subscriptions_n = subs_n;
-	this->subscriptions = (MavlinkOrbSubscription **) malloc(subs_n * sizeof(MavlinkOrbSubscription *));
-
-	for (int i = 0; i < subs_n; i++) {
-		this->subscriptions[i] = mavlink->add_orb_subscription(topics[i], sizes[i]);
-	}
 }
 
 MavlinkStream::~MavlinkStream()
 {
-	free(subscriptions);
 }
 
 /**
- * Update mavlink stream, i.e. update subscriptions and send message if necessary
+ * Set messages interval in ms
  */
-int MavlinkStream::update(const hrt_abstime t)
+void
+MavlinkStream::set_interval(const unsigned int interval)
 {
-	uint64_t dt = t - last_sent;
-	if (dt > 0 && dt >= interval) {
-		/* interval expired, update all subscriptions */
-		for (unsigned int i = 0; i < subscriptions_n; i++) {
-			subscriptions[i]->update(t);
-		}
+	_interval = interval * 1000;
+}
 
-		/* format and send mavlink message */
-		callback(this);
-		last_sent = t;
+/**
+ * Set mavlink channel
+ */
+void
+MavlinkStream::set_channel(mavlink_channel_t channel)
+{
+	_channel = channel;
+}
+
+/**
+ * Update subscriptions and send message if necessary
+ */
+int
+MavlinkStream::update(const hrt_abstime t)
+{
+	uint64_t dt = t - _last_sent;
+	if (dt > 0 && dt >= _interval) {
+		/* interval expired, send message */
+		send(t);
+		_last_sent = (t / _interval) * _interval;
 	}
 }
diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h
index c3e60917e..9f175adbe 100644
--- a/src/modules/mavlink/mavlink_stream.h
+++ b/src/modules/mavlink/mavlink_stream.h
@@ -15,22 +15,27 @@ class MavlinkStream;
 
 #include "mavlink_main.h"
 
-class MavlinkOrbSubscription;
-
 class MavlinkStream {
+private:
+	hrt_abstime _last_sent;
+
+protected:
+	mavlink_channel_t _channel;
+	unsigned int _interval;
+
+	virtual void send(const hrt_abstime t) = 0;
+
 public:
-	void	(*callback)(const MavlinkStream *);
-	uintptr_t	arg;
-	unsigned int subscriptions_n;
-	MavlinkOrbSubscription **subscriptions;
-	hrt_abstime last_sent;
-	unsigned int interval;
 	MavlinkStream *next;
-	Mavlink		*mavlink;
 
-	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();
 	~MavlinkStream();
+	void set_interval(const unsigned int interval);
+	void set_channel(mavlink_channel_t channel);
 	int update(const hrt_abstime t);
+	virtual MavlinkStream *new_instance() = 0;
+	virtual void subscribe(Mavlink *mavlink) = 0;
+	virtual const char *get_name() = 0;
 };