Merge branch 'master' into mpc_local_pos

9 files changed, 555 insertions, 66 deletions

diff --git a/src/modules/mavlink/mavlink_commands.cpp b/src/modules/mavlink/mavlink_commands.cpp
new file mode 100644
index 000000000..1c1e097a4
--- /dev/null
+++ b/src/modules/mavlink/mavlink_commands.cpp
@@ -0,0 +1,73 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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_commands.cpp
+ * Mavlink commands stream implementation.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include "mavlink_commands.h"
+
+MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel)
+{
+	_cmd_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command));
+	_cmd = (struct vehicle_command_s *)_cmd_sub->get_data();
+}
+
+MavlinkCommandsStream::~MavlinkCommandsStream()
+{
+}
+
+void
+MavlinkCommandsStream::update(const hrt_abstime t)
+{
+	if (_cmd_sub->update(t)) {
+		/* only send commands for other systems/components */
+		if (_cmd->target_system != mavlink_system.sysid || _cmd->target_component != mavlink_system.compid) {
+			mavlink_msg_command_long_send(_channel,
+						      _cmd->target_system,
+						      _cmd->target_component,
+						      _cmd->command,
+						      _cmd->confirmation,
+						      _cmd->param1,
+						      _cmd->param2,
+						      _cmd->param3,
+						      _cmd->param4,
+						      _cmd->param5,
+						      _cmd->param6,
+						      _cmd->param7);
+		}
+	}
+}
diff --git a/src/modules/mavlink/mavlink_commands.h b/src/modules/mavlink/mavlink_commands.h
new file mode 100644
index 000000000..6255d65df
--- /dev/null
+++ b/src/modules/mavlink/mavlink_commands.h
@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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_commands.h
+ * Mavlink commands stream definition.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#ifndef MAVLINK_COMMANDS_H_
+#define MAVLINK_COMMANDS_H_
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_command.h>
+
+class Mavlink;
+class MavlinkCommansStream;
+
+#include "mavlink_main.h"
+
+class MavlinkCommandsStream
+{
+private:
+	MavlinkOrbSubscription *_cmd_sub;
+	struct vehicle_command_s *_cmd;
+	mavlink_channel_t _channel;
+
+public:
+	MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel);
+	~MavlinkCommandsStream();
+	void update(const hrt_abstime t);
+};
+
+#endif /* MAVLINK_COMMANDS_H_ */
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 18df577fe..a9f5f4de7 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -81,6 +81,7 @@
 #include "mavlink_messages.h"
 #include "mavlink_receiver.h"
 #include "mavlink_rate_limiter.h"
+#include "mavlink_commands.h"
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -166,12 +167,12 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
 	int buf_free = 0;
 
 	if (instance->get_flow_control_enabled()
-	    && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
+		&& ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
 
 		if (buf_free == 0) {
 
 			if (last_write_times[(unsigned)channel] != 0 &&
-			    hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) {
+				hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) {
 
 				warnx("DISABLING HARDWARE FLOW CONTROL");
 				instance->enable_flow_control(false);
@@ -185,12 +186,17 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
 		}
 	}
 
-	ssize_t ret = write(uart, ch, desired);
-
-	if (ret != desired) {
-		// XXX do something here, but change to using FIONWRITE and OS buf size for detection
+	/* If the wait until transmit flag is on, only transmit after we've received messages.
+	   Otherwise, transmit all the time. */
+	if (instance->should_transmit()) {
+	   ssize_t ret = write(uart, ch, desired);
+		if (ret != desired) {
+			// XXX do something here, but change to using FIONWRITE and OS buf size for detection
+		}
 	}
 
+
+
 }
 
 static void usage(void);
@@ -203,14 +209,21 @@ Mavlink::Mavlink() :
 	_task_running(false),
 	_hil_enabled(false),
 	_is_usb_uart(false),
+	_wait_to_transmit(false),
+	_received_messages(false),
 	_main_loop_delay(1000),
 	_subscriptions(nullptr),
 	_streams(nullptr),
 	_mission_pub(-1),
+	_verbose(false),
+	_forwarding_on(false),
+	_passing_on(false),
+	_uart_fd(-1),
 	_mavlink_param_queue_index(0),
 	_subscribe_to_stream(nullptr),
 	_subscribe_to_stream_rate(0.0f),
 	_flow_control_enabled(true),
+	_message_buffer({}),
 
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink"))
@@ -261,7 +274,6 @@ Mavlink::Mavlink() :
 		errx(1, "instance ID is out of range");
 		break;
 	}
-
 }
 
 Mavlink::~Mavlink()
@@ -394,6 +406,18 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self)
 	return false;
 }
 
+void
+Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self)
+{
+	
+	Mavlink *inst;
+	LL_FOREACH(_mavlink_instances, inst) {
+		if (inst != self) {
+			inst->pass_message(msg);
+		}
+	}
+}
+
 int
 Mavlink::get_uart_fd(unsigned index)
 {
@@ -808,10 +832,10 @@ void Mavlink::publish_mission()
 {
 	/* Initialize mission publication if necessary */
 	if (_mission_pub < 0) {
-		_mission_pub = orb_advertise(ORB_ID(mission), &mission);
+		_mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission);
 
 	} else {
-		orb_publish(ORB_ID(mission), _mission_pub, &mission);
+		orb_publish(ORB_ID(offboard_mission), _mission_pub, &mission);
 	}
 }
 
@@ -1617,6 +1641,125 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate)
 }
 
 int
+Mavlink::message_buffer_init(int size)
+{
+	_message_buffer.size = size;
+	_message_buffer.write_ptr = 0;
+	_message_buffer.read_ptr = 0;
+	_message_buffer.data = (char*)malloc(_message_buffer.size);
+	return (_message_buffer.data == 0) ? ERROR : OK;
+}
+
+void
+Mavlink::message_buffer_destroy()
+{
+	_message_buffer.size = 0;
+	_message_buffer.write_ptr = 0;
+	_message_buffer.read_ptr = 0;
+	free(_message_buffer.data);
+}
+
+int
+Mavlink::message_buffer_count()
+{
+	int n = _message_buffer.write_ptr - _message_buffer.read_ptr;
+
+	if (n < 0) {
+		n += _message_buffer.size;
+	}
+
+	return n;
+}
+
+int
+Mavlink::message_buffer_is_empty()
+{
+	return _message_buffer.read_ptr == _message_buffer.write_ptr;
+}
+
+
+bool
+Mavlink::message_buffer_write(void *ptr, int size)
+{
+	// bytes available to write
+	int available = _message_buffer.read_ptr - _message_buffer.write_ptr - 1;
+
+	if (available < 0) {
+		available += _message_buffer.size;
+	}
+
+	if (size > available) {
+		// buffer overflow
+		return false;
+	}
+
+	char *c = (char *) ptr;
+	int n = _message_buffer.size - _message_buffer.write_ptr;	// bytes to end of the buffer
+
+	if (n < size) {
+		// message goes over end of the buffer
+		memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), c, n);
+		_message_buffer.write_ptr = 0;
+
+	} else {
+		n = 0;
+	}
+
+	// now: n = bytes already written
+	int p = size - n;	// number of bytes to write
+	memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), &(c[n]), p);
+	_message_buffer.write_ptr = (_message_buffer.write_ptr + p) % _message_buffer.size;
+	return true;
+}
+
+int
+Mavlink::message_buffer_get_ptr(void **ptr, bool *is_part)
+{
+	// bytes available to read
+	int available = _message_buffer.write_ptr - _message_buffer.read_ptr;
+
+	if (available == 0) {
+		return 0;	// buffer is empty
+	}
+
+	int n = 0;
+
+	if (available > 0) {
+		// read pointer is before write pointer, all available bytes can be read
+		n = available;
+		*is_part = false;
+
+	} else {
+		// read pointer is after write pointer, read bytes from read_ptr to end of the buffer
+		n = _message_buffer.size - _message_buffer.read_ptr;
+		*is_part = _message_buffer.write_ptr > 0;
+	}
+
+	*ptr = &(_message_buffer.data[_message_buffer.read_ptr]);
+	return n;
+}
+
+void
+Mavlink::message_buffer_mark_read(int n)
+{
+	_message_buffer.read_ptr = (_message_buffer.read_ptr + n) % _message_buffer.size;
+}
+
+void
+Mavlink::pass_message(mavlink_message_t *msg)
+{
+	if (_passing_on) {
+		/* size is 8 bytes plus variable payload */
+		int size = MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len;
+		pthread_mutex_lock(&_message_buffer_mutex);
+		message_buffer_write(msg, size);
+		pthread_mutex_unlock(&_message_buffer_mutex);
+	}
+}
+
+
+
+int
 Mavlink::task_main(int argc, char *argv[])
 {
 	int ch;
@@ -1632,7 +1775,7 @@ Mavlink::task_main(int argc, char *argv[])
 	 * set error flag instead */
 	bool err_flag = false;
 
-	while ((ch = getopt(argc, argv, "b:r:d:m:v")) != EOF) {
+	while ((ch = getopt(argc, argv, "b:r:d:m:fpvw")) != EOF) {
 		switch (ch) {
 		case 'b':
 			_baudrate = strtoul(optarg, NULL, 10);
@@ -1672,10 +1815,22 @@ Mavlink::task_main(int argc, char *argv[])
 
 			break;
 
+		case 'f':
+			_forwarding_on = true;
+			break;
+
+		case 'p':
+			_passing_on = true;
+			break;
+
 		case 'v':
 			_verbose = true;
 			break;
 
+		case 'w':
+			_wait_to_transmit = true;
+			break;
+
 		default:
 			err_flag = true;
 			break;
@@ -1740,6 +1895,17 @@ Mavlink::task_main(int argc, char *argv[])
 	/* initialize mavlink text message buffering */
 	mavlink_logbuffer_init(&_logbuffer, 5);
 
+	/* if we are passing on mavlink messages, we need to prepare a buffer for this instance */
+	if (_passing_on) {
+		/* initialize message buffer if multiplexing is on */
+		if (OK != message_buffer_init(500)) {
+			errx(1, "can't allocate message buffer, exiting");
+		}
+
+		/* initialize message buffer mutex */
+		pthread_mutex_init(&_message_buffer_mutex, NULL);
+	}
+
 	/* create the device node that's used for sending text log messages, etc. */
 	register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL);
 
@@ -1766,6 +1932,8 @@ Mavlink::task_main(int argc, char *argv[])
 
 	struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->get_data();
 
+	MavlinkCommandsStream commands_stream(this, _channel);
+
 	/* add default streams depending on mode and intervals depending on datarate */
 	float rate_mult = _datarate / 1000.0f;
 
@@ -1783,6 +1951,9 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult);
 		configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult);
 		configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult);
+		configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult);
+		configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult);
+		configure_stream("DISTANCE_SENSOR", 0.5f);
 		break;
 
 	case MAVLINK_MODE_CAMERA:
@@ -1826,6 +1997,9 @@ Mavlink::task_main(int argc, char *argv[])
 			set_hil_enabled(status->hil_state == HIL_STATE_ON);
 		}
 
+		/* update commands stream */
+		commands_stream.update(t);
+
 		/* check for requested subscriptions */
 		if (_subscribe_to_stream != nullptr) {
 			if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) {
@@ -1884,6 +2058,37 @@ Mavlink::task_main(int argc, char *argv[])
 			}
 		}
 
+		/* pass messages from other UARTs */
+		if (_passing_on) {
+
+			bool is_part;
+			void *read_ptr;
+
+			/* guard get ptr by mutex */
+			pthread_mutex_lock(&_message_buffer_mutex);
+			int available = message_buffer_get_ptr(&read_ptr, &is_part);
+			pthread_mutex_unlock(&_message_buffer_mutex);
+
+			if (available > 0) {
+				/* write first part of buffer */
+				_mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr);
+				message_buffer_mark_read(available);
+
+				/* write second part of buffer if there is some */
+				if (is_part) {
+					/* guard get ptr by mutex */
+					pthread_mutex_lock(&_message_buffer_mutex);
+					available = message_buffer_get_ptr(&read_ptr, &is_part);
+					pthread_mutex_unlock(&_message_buffer_mutex);
+
+					_mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr);
+					message_buffer_mark_read(available);
+				}
+			}
+		}
+
+
+
 		perf_end(_loop_perf);
 	}
 
@@ -1928,6 +2133,10 @@ Mavlink::task_main(int argc, char *argv[])
 	/* close mavlink logging device */
 	close(_mavlink_fd);
 
+	if (_passing_on) {
+		message_buffer_destroy();
+		pthread_mutex_destroy(&_message_buffer_mutex);
+	}
 	/* destroy log buffer */
 	mavlink_logbuffer_destroy(&_logbuffer);
 
@@ -2067,7 +2276,7 @@ Mavlink::stream(int argc, char *argv[])
 
 static void usage()
 {
-	warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-v]");
+	warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v] [-w]");
 }
 
 int mavlink_main(int argc, char *argv[])
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 5a118a0ad..66d82b471 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -138,6 +138,8 @@ public:
 
 	static bool	instance_exists(const char *device_name, Mavlink *self);
 
+	static void	forward_message(mavlink_message_t *msg, Mavlink *self);
+
 	static int get_uart_fd(unsigned index);
 
 	int get_uart_fd();
@@ -153,10 +155,12 @@ public:
 	void		set_mode(enum MAVLINK_MODE);
 	enum MAVLINK_MODE		get_mode() { return _mode; }
 
-	bool		get_hil_enabled() { return _hil_enabled; };
+	bool		get_hil_enabled() { return _hil_enabled; }
 
 	bool		get_flow_control_enabled() { return _flow_control_enabled; }
 
+	bool		get_forwarding_on() { return _forwarding_on; }
+
 	/**
 	 * Handle waypoint related messages.
 	 */
@@ -196,6 +200,16 @@ public:
 
 	bool		_task_should_exit;		/**< if true, mavlink task should exit */
 
+	int get_mavlink_fd() { return _mavlink_fd; }
+
+
+	/* Functions for waiting to start transmission until message received. */
+	void set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
+	bool get_has_received_messages() { return _received_messages; }
+	void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; }
+	bool get_wait_to_transmit() { return _wait_to_transmit; }
+	bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
+
 protected:
 	Mavlink	*next;
 
@@ -210,6 +224,8 @@ private:
 	/* states */
 	bool		_hil_enabled;		/**< Hardware In the Loop mode */
 	bool		_is_usb_uart;		/**< Port is USB */
+	bool        _wait_to_transmit;  /**< Wait to transmit until received messages. */
+	bool        _received_messages; /**< Whether we've received valid mavlink messages. */
 
 	unsigned	_main_loop_delay;		/**< mainloop delay, depends on data rate */
 
@@ -234,6 +250,8 @@ private:
 	mavlink_wpm_storage *_wpm;
 
 	bool _verbose;
+	bool _forwarding_on;
+	bool _passing_on;
 	int _uart_fd;
 	int _baudrate;
 	int _datarate;
@@ -252,6 +270,18 @@ private:
 
 	bool		_flow_control_enabled;
 
+	struct mavlink_message_buffer {
+		int write_ptr;
+		int read_ptr;
+		int size;
+		char *data;
+	};
+	mavlink_message_buffer _message_buffer;
+
+	pthread_mutex_t _message_buffer_mutex;
+
+
+
 	/**
 	 * Send one parameter.
 	 *
@@ -315,6 +345,22 @@ private:
 	int configure_stream(const char *stream_name, const float rate);
 	void configure_stream_threadsafe(const char *stream_name, const float rate);
 
+	int message_buffer_init(int size);
+
+	void message_buffer_destroy();
+
+	int message_buffer_count();
+
+	int message_buffer_is_empty();
+
+	bool message_buffer_write(void *ptr, int size);
+
+	int message_buffer_get_ptr(void **ptr, bool *is_part);
+
+	void message_buffer_mark_read(int n);
+
+	void pass_message(mavlink_message_t *msg);
+
 	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 4ca3840d4..678ce1645 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -72,6 +72,7 @@
 #include <uORB/topics/navigation_capabilities.h>
 #include <drivers/drv_rc_input.h>
 #include <drivers/drv_pwm_output.h>
+#include <drivers/drv_range_finder.h>
 
 #include "mavlink_messages.h"
 
@@ -262,22 +263,21 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		if (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);
-		}
+		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 * 100.0f,
+						status->drop_rate_comm,
+						status->errors_comm,
+						status->errors_count1,
+						status->errors_count2,
+						status->errors_count3,
+						status->errors_count4);
 	}
 };
 
@@ -641,6 +641,47 @@ protected:
 };
 
 
+
+class MavlinkStreamViconPositionEstimate : public MavlinkStream
+{
+public:
+	const char *get_name()
+	{
+		return "VICON_POSITION_ESTIMATE";
+	}
+
+	MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamViconPositionEstimate();
+	}
+
+private:
+	MavlinkOrbSubscription *pos_sub;
+	struct vehicle_vicon_position_s *pos;
+
+protected:
+	void subscribe(Mavlink *mavlink)
+	{
+		pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position));
+		pos = (struct vehicle_vicon_position_s *)pos_sub->get_data();
+	}
+
+	void send(const hrt_abstime t)
+	{
+		if (pos_sub->update(t)) {
+			mavlink_msg_vicon_position_estimate_send(_channel,
+								pos->timestamp / 1000,
+								pos->x,
+								pos->y,
+								pos->z,
+								pos->roll,
+								pos->pitch,
+								pos->yaw);
+		}
+	}
+};
+
+
 class MavlinkStreamGPSGlobalOrigin : public MavlinkStream
 {
 public:
@@ -1253,8 +1294,6 @@ protected:
 	{
 		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
 		status = (struct vehicle_status_s *)status_sub->get_data();
-
-
 	}
 
 	void send(const hrt_abstime t)
@@ -1265,12 +1304,57 @@ protected:
 		    || status->arming_state == ARMING_STATE_ARMED_ERROR) {
 
 			/* send camera capture on */
-			mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0);
+			mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0);
 
 		} else {
 			/* send camera capture off */
-			mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0);
+			mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0);
+		}
+	}
+};
+
+class MavlinkStreamDistanceSensor : public MavlinkStream
+{
+public:
+	const char *get_name()
+	{
+		return "DISTANCE_SENSOR";
+	}
+
+	MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamDistanceSensor();
+	}
+
+private:
+	MavlinkOrbSubscription *range_sub;
+	struct range_finder_report *range;
+
+protected:
+	void subscribe(Mavlink *mavlink)
+	{
+		range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder));
+		range = (struct range_finder_report *)range_sub->get_data();
+	}
+
+	void send(const hrt_abstime t)
+	{
+		(void)range_sub->update(t);
+
+		uint8_t type;
+
+		switch (range->type) {
+			case RANGE_FINDER_TYPE_LASER:
+			type = MAV_DISTANCE_SENSOR_LASER;
+			break;
 		}
+
+		uint8_t id = 0;
+		uint8_t orientation = 0;
+		uint8_t covariance = 20;
+
+		mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation, 
+			range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance);
 	}
 };
 
@@ -1300,5 +1384,7 @@ MavlinkStream *streams_list[] = {
 	new MavlinkStreamAttitudeControls(),
 	new MavlinkStreamNamedValueFloat(),
 	new MavlinkStreamCameraCapture(),
+	new MavlinkStreamDistanceSensor(),
+	new MavlinkStreamViconPositionEstimate(),
 	nullptr
 };
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index dc6236a51..adafdd139 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -181,6 +181,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 			break;
 		}
 	}
+
+	/* If we've received a valid message, mark the flag indicating so.
+	   This is used in the '-w' command-line flag. */
+	_mavlink->set_has_received_messages(true);
 }
 
 void
@@ -222,12 +226,10 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
 			vcmd.source_component = msg->compid;
 			vcmd.confirmation =  cmd_mavlink.confirmation;
 
-			/* check if topic is advertised */
-			if (_cmd_pub <= 0) {
+			if (_cmd_pub < 0) {
 				_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
 
 			} else {
-				/* publish */
 				orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd);
 			}
 		}
@@ -254,7 +256,7 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
 	f.quality = flow.quality;
 	f.sensor_id = flow.sensor_id;
 
-	if (_flow_pub <= 0) {
+	if (_flow_pub < 0) {
 		_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
 
 	} else {
@@ -288,7 +290,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
 	vcmd.source_component = msg->compid;
 	vcmd.confirmation = 1;
 
-	if (_cmd_pub <= 0) {
+	if (_cmd_pub < 0) {
 		_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
 
 	} else {
@@ -313,7 +315,7 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg)
 	vicon_position.pitch = pos.pitch;
 	vicon_position.yaw = pos.yaw;
 
-	if (_vicon_position_pub <= 0) {
+	if (_vicon_position_pub < 0) {
 		_vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
 
 	} else {
@@ -374,7 +376,7 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message
 
 		offboard_control_sp.timestamp = hrt_absolute_time();
 
-		if (_offboard_control_sp_pub <= 0) {
+		if (_offboard_control_sp_pub < 0) {
 			_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
 
 		} else {
@@ -402,7 +404,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 	tstatus.rxerrors = rstatus.rxerrors;
 	tstatus.fixed = rstatus.fixed;
 
-	if (_telemetry_status_pub <= 0) {
+	if (_telemetry_status_pub < 0) {
 		_telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
 
 	} else {
@@ -429,7 +431,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 		rc.chan[2].scaled = man.r / 1000.0f;
 		rc.chan[3].scaled = man.z / 1000.0f;
 
-		if (_rc_pub == 0) {
+		if (_rc_pub < 0) {
 			_rc_pub = orb_advertise(ORB_ID(rc_channels), &rc);
 
 		} else {
@@ -451,7 +453,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 		manual.yaw = man.r / 1000.0f;
 		manual.throttle = man.z / 1000.0f;
 
-		if (_manual_pub == 0) {
+		if (_manual_pub < 0) {
 			_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
 
 		} else {
@@ -620,11 +622,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		hil_sensors.differential_pressure_timestamp = timestamp;
 
 		/* publish combined sensor topic */
-		if (_sensors_pub > 0) {
-			orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors);
+		if (_sensors_pub < 0) {
+			_sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
 
 		} else {
-			_sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
+			orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors);
 		}
 	}
 
@@ -639,11 +641,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		hil_battery_status.current_a = 10.0f;
 		hil_battery_status.discharged_mah = -1.0f;
 
-		if (_battery_pub > 0) {
-			orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
+		if (_battery_pub < 0) {
+			_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
 
 		} else {
-			_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
+			orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
 		}
 	}
 
@@ -695,11 +697,11 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
 	hil_gps.fix_type = gps.fix_type;
 	hil_gps.satellites_visible = gps.satellites_visible;
 
-	if (_gps_pub > 0) {
-		orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps);
+	if (_gps_pub < 0) {
+		_gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
 
 	} else {
-		_gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
+		orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps);
 	}
 }
 
@@ -753,11 +755,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		hil_attitude.pitchspeed = hil_state.pitchspeed;
 		hil_attitude.yawspeed = hil_state.yawspeed;
 
-		if (_attitude_pub > 0) {
-			orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude);
+		if (_attitude_pub < 0) {
+			_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
 
 		} else {
-			_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
+			orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude);
 		}
 	}
 
@@ -777,11 +779,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		hil_global_pos.eph = 2.0f;
 		hil_global_pos.epv = 4.0f;
 
-		if (_global_pos_pub > 0) {
-			orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos);
+		if (_global_pos_pub < 0) {
+			_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
 
 		} else {
-			_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
+			orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos);
 		}
 	}
 
@@ -821,11 +823,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve?
 		hil_local_pos.landed = landed;
 
-		if (_local_pos_pub > 0) {
-			orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos);
+		if (_local_pos_pub < 0) {
+			_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
 
 		} else {
-			_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
+			orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos);
 		}
 	}
 
@@ -862,11 +864,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		hil_battery_status.current_a = 10.0f;
 		hil_battery_status.discharged_mah = -1.0f;
 
-		if (_battery_pub > 0) {
-			orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
+		if (_battery_pub < 0) {
+			_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
 
 		} else {
-			_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
+			orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
 		}
 	}
 }
@@ -918,6 +920,11 @@ MavlinkReceiver::receive_thread(void *arg)
 
 					/* handle packet with parameter component */
 					_mavlink->mavlink_pm_message_handler(_mavlink->get_channel(), &msg);
+
+					if (_mavlink->get_forwarding_on()) {
+						/* forward any messages to other mavlink instances */
+						Mavlink::forward_message(&msg, _mavlink);
+					}
 				}
 			}
 		}
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 796c09722..dd9912338 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -120,7 +120,6 @@ private:
 
 	mavlink_status_t status;
 	struct vehicle_local_position_s hil_local_pos;
-	int _manual_sub;
 	orb_advert_t _global_pos_pub;
 	orb_advert_t _local_pos_pub;
 	orb_advert_t _attitude_pub;
@@ -139,6 +138,7 @@ private:
 	orb_advert_t _telemetry_status_pub;
 	orb_advert_t _rc_pub;
 	orb_advert_t _manual_pub;
+	int _manual_sub;
 	int _hil_frames;
 	uint64_t _old_timestamp;
 	bool _hil_local_proj_inited;
diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h
index 135e1bce0..def40d9ad 100644
--- a/src/modules/mavlink/mavlink_stream.h
+++ b/src/modules/mavlink/mavlink_stream.h
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file mavlink_stream.cpp
+ * @file mavlink_stream.h
  * Mavlink messages stream definition.
  *
  * @author Anton Babushkin <anton.babushkin@me.com>
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index f09efa2e6..dcca11977 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -42,6 +42,9 @@ SRCS		 += mavlink_main.cpp \
 			mavlink_orb_subscription.cpp \
 			mavlink_messages.cpp \
 			mavlink_stream.cpp \
-			mavlink_rate_limiter.cpp
+			mavlink_rate_limiter.cpp \
+			mavlink_commands.cpp
 
 INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
+
+MAXOPTIMIZATION	 = -Os