From 2ac0e1fc547683a092eef85678652c6a538a870e Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Mon, 31 Mar 2014 10:10:50 +0400
Subject: [PATCH 01/97] rcS: removed unnecessary sleeps, minor code style fixes

---
 ROMFS/px4fmu_common/init.d/rcS | 10 ++++------
 1 file changed, 4 insertions(+), 6 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 503dbb83e1..0cb6d281a2 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -294,6 +294,7 @@ then
 				tone_alarm $TUNE_OUT_ERROR
 			fi
 		fi
+		
 		if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == ardrone ]
 		then
 			echo "[init] Use FMU as primary output"
@@ -317,6 +318,7 @@ then
 				fi
 			fi
 		fi
+		
 		if [ $OUTPUT_MODE == mkblctrl ]
 		then
 			echo "[init] Use MKBLCTRL as primary output"
@@ -338,7 +340,8 @@ then
 				tone_alarm $TUNE_OUT_ERROR
 			fi
 			
-		fi		
+		fi
+				
 		if [ $OUTPUT_MODE == hil ]
 		then
 			echo "[init] Use HIL as primary output"
@@ -396,34 +399,29 @@ then
 	#
 	# MAVLink
 	#
-	
 	if [ $MAVLINK_FLAGS == default ]
 	then
 		if [ $HIL == yes ]
 		then
 			sleep 1
 			set MAVLINK_FLAGS "-r 10000 -d /dev/ttyACM0"
-			usleep 5000
 		else
 			# Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s
 			if [ $TTYS1_BUSY == yes ]
 			then
 				# Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else
 				set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0"
-				usleep 5000
 				
 				# Exit from nsh to free port for mavlink
 				set EXIT_ON_END yes
 			else
 				# Start MAVLink on default port: ttyS1
 				set MAVLINK_FLAGS "-r 1000"
-				usleep 5000
 			fi
 		fi
 	fi
 
 	mavlink start $MAVLINK_FLAGS
-	usleep 5000
 	
 	#
 	# Start the datamanager

From 3fa82675e79013fedf9a787ca21e06e5bd15e5ca Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 3 Apr 2014 21:13:03 +0200
Subject: [PATCH 02/97] commander: don't beep if message is not understood

---
 src/modules/commander/commander.cpp | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index d4567c4f11..27d5e42608 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -591,11 +591,11 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
 		break;
 	}
-
-	if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
-		/* already warned about unsupported commands in "default" case */
-		answer_command(*cmd, result);
-	}
+	/* silently ignore unsupported commands, maybe they are passed on over mavlink */
+//	if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
+//		/* already warned about unsupported commands in "default" case */
+//		answer_command(*cmd, result);
+//	}
 
 	/* send any requested ACKs */
 	if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) {

From 15defeb0f1e6ce396562685316a8734fcd867081 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 3 Apr 2014 21:15:47 +0200
Subject: [PATCH 03/97] mavlink: implemented multicasting between mavlink
 instances (two options: forwarding: forward received messages from self to
 other mavlink instances, passing: send out messages received from other
 mavlink intances over serial

---
 src/modules/mavlink/mavlink_main.cpp     | 197 ++++++++++++++++++++++-
 src/modules/mavlink/mavlink_main.h       |  34 +++-
 src/modules/mavlink/mavlink_messages.cpp |   6 +-
 src/modules/mavlink/mavlink_receiver.cpp |   5 +
 4 files changed, 236 insertions(+), 6 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 18df577fea..c5055939ec 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -207,10 +207,15 @@ Mavlink::Mavlink() :
 	_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 +266,6 @@ Mavlink::Mavlink() :
 		errx(1, "instance ID is out of range");
 		break;
 	}
-
 }
 
 Mavlink::~Mavlink()
@@ -394,6 +398,20 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self)
 	return false;
 }
 
+void
+Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self)
+{
+	Mavlink *inst = ::_mavlink_instances;
+
+	while (inst != nullptr) {
+		/* don't broadcast to itself */
+		if (inst != self) {
+			inst->pass_message(msg);
+		}
+		inst = inst->next;
+	}
+}
+
 int
 Mavlink::get_uart_fd(unsigned index)
 {
@@ -1616,6 +1634,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[])
 {
@@ -1632,7 +1769,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:fpv")) != EOF) {
 		switch (ch) {
 		case 'b':
 			_baudrate = strtoul(optarg, NULL, 10);
@@ -1672,6 +1809,14 @@ 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;
@@ -1740,6 +1885,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);
 
@@ -1884,6 +2040,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 +2115,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 +2258,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]");
 }
 
 int mavlink_main(int argc, char *argv[])
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 5a118a0ad9..4f9a53a5ba 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.
 	 */
@@ -234,6 +238,8 @@ private:
 	mavlink_wpm_storage *_wpm;
 
 	bool _verbose;
+	bool _forwarding_on;
+	bool _passing_on;
 	int _uart_fd;
 	int _baudrate;
 	int _datarate;
@@ -252,6 +258,16 @@ 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 +331,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 4ca3840d47..2b5d650807 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1265,11 +1265,13 @@ 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);
+			/* XXX TODO: get param for system ID */
+			mavlink_msg_command_long_send(_channel, 42, 250, 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);
+			/* XXX TODO: get param for system ID */
+			mavlink_msg_command_long_send(_channel, 42, 250, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0);
 		}
 	}
 };
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index f6f5e4848c..8e4e5f0d4e 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -880,6 +880,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);
+					}
 				}
 			}
 		}

From 2cd4648a8e6df65ad953ffe06426c83476b7f835 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 7 Apr 2014 14:38:16 +0200
Subject: [PATCH 04/97] mavlink: in normal mode transmit position setpoint and
 roll-pitch-yaw-thrust setpoint

---
 src/modules/mavlink/mavlink_main.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index c5055939ec..bb73b80887 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1939,6 +1939,8 @@ 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);
 		break;
 
 	case MAVLINK_MODE_CAMERA:

From a66dcbf7e92a1028765f6c5c9a545945ce1a5dc3 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Mon, 7 Apr 2014 17:17:56 +0400
Subject: [PATCH 05/97] mavlink: publish SYS_STATUS at constant rate, don't
 look at update() result

---
 src/modules/mavlink/mavlink_messages.cpp | 31 ++++++++++++------------
 1 file changed, 15 insertions(+), 16 deletions(-)

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 2b5d650807..6b543d44f8 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -262,22 +262,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,
+						status->drop_rate_comm,
+						status->errors_comm,
+						status->errors_count1,
+						status->errors_count2,
+						status->errors_count3,
+						status->errors_count4);
 	}
 };
 

From 3dd64086e49192aabc020b50f48d68233bad392f Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 7 Apr 2014 21:42:19 +0200
Subject: [PATCH 06/97] commander: put unsupported warning back in place

---
 src/modules/commander/commander.cpp | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 27d5e42608..662b98b35c 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -587,15 +587,15 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		break;
 
 	default:
-		/* warn about unsupported commands */
+		/* Warn about unsupported commands, this makes sense because only commands
+		 * to this component ID (or all) are passed by mavlink. */
 		answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
 		break;
 	}
-	/* silently ignore unsupported commands, maybe they are passed on over mavlink */
-//	if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
-//		/* already warned about unsupported commands in "default" case */
-//		answer_command(*cmd, result);
-//	}
+	if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
+		/* already warned about unsupported commands in "default" case */
+		answer_command(*cmd, result);
+	}
 
 	/* send any requested ACKs */
 	if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) {

From 01d9d482c3237250420e182e0aedd409c365f848 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 7 Apr 2014 21:42:48 +0200
Subject: [PATCH 07/97] mavlink: use LL_FOREACH

---
 src/modules/mavlink/mavlink_main.cpp | 8 +++-----
 1 file changed, 3 insertions(+), 5 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index bb73b80887..3d897431a1 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -401,14 +401,12 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self)
 void
 Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self)
 {
-	Mavlink *inst = ::_mavlink_instances;
-
-	while (inst != nullptr) {
-		/* don't broadcast to itself */
+	
+	Mavlink *inst;
+	LL_FOREACH(_mavlink_instances, inst) {
 		if (inst != self) {
 			inst->pass_message(msg);
 		}
-		inst = inst->next;
 	}
 }
 

From d662fa4c14676aac26603e55c71c04a2dd29503a Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 7 Apr 2014 21:44:01 +0200
Subject: [PATCH 08/97] Send camera command to all, use own sysid

---
 src/modules/mavlink/mavlink_messages.cpp | 6 ++----
 1 file changed, 2 insertions(+), 4 deletions(-)

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 6b543d44f8..45b86a497b 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1264,13 +1264,11 @@ protected:
 		    || status->arming_state == ARMING_STATE_ARMED_ERROR) {
 
 			/* send camera capture on */
-			/* XXX TODO: get param for system ID */
-			mavlink_msg_command_long_send(_channel, 42, 250, 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 */
-			/* XXX TODO: get param for system ID */
-			mavlink_msg_command_long_send(_channel, 42, 250, 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);
 		}
 	}
 };

From fefe19a7b9d053265a55862cbbbf16978ca38846 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 8 Apr 2014 11:03:23 +0200
Subject: [PATCH 09/97] gps driver: fake mode: lower eph and epv values in
 order to convince the commander that the gps signal is valid

---
 src/drivers/gps/gps.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index c72f692d73..a902bdf2f5 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -282,8 +282,8 @@ GPS::task_main()
 			_report.p_variance_m = 10.0f;
 			_report.c_variance_rad = 0.1f;
 			_report.fix_type = 3;
-			_report.eph_m = 10.0f;
-			_report.epv_m = 10.0f;
+			_report.eph_m = 3.0f;
+			_report.epv_m = 7.0f;
 			_report.timestamp_velocity = hrt_absolute_time();
 			_report.vel_n_m_s = 0.0f;
 			_report.vel_e_m_s = 0.0f;

From 1c49f132a43cc8521fc0a9395b03911400c91435 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 8 Apr 2014 15:15:06 +0200
Subject: [PATCH 10/97] mavlink: accessor for _mavlink_fd

---
 src/modules/mavlink/mavlink_main.h | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 4f9a53a5ba..9941a5f993 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -200,6 +200,8 @@ public:
 
 	bool		_task_should_exit;		/**< if true, mavlink task should exit */
 
+	int get_mavlink_fd() { return _mavlink_fd; }
+
 protected:
 	Mavlink	*next;
 

From 03a3b1d67127af8681275a50e5604ea1b60814ea Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 8 Apr 2014 17:06:52 +0200
Subject: [PATCH 11/97] commander: handle_command: filter commands by sysid and
 componentid

---
 src/modules/commander/commander.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 662b98b35c..e6de58563e 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -395,6 +395,11 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 	enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED;
 	bool ret = false;
 
+	/* only handle commands that are meant to be handled by this system and component */
+	if (cmd->target_system != status->system_id || cmd->target_component != status->component_id) {
+		return false;
+	}
+
 	/* only handle high-priority commands here */
 
 	/* request to set different system mode */

From b2bc8c1f08fb728ca2b80e7df6c3288b3fda4e3b Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 8 Apr 2014 17:08:35 +0200
Subject: [PATCH 12/97] mavlink: COMMAND_LONG stream: listen to vehicle_command
 uorb topic and send mavlink_msg_command_long

---
 src/modules/mavlink/mavlink_messages.cpp | 46 ++++++++++++++++++++++++
 1 file changed, 46 insertions(+)

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 45b86a497b..4d7b9160d3 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1273,6 +1273,51 @@ protected:
 	}
 };
 
+class MavlinkStreamCommandLong : public MavlinkStream
+{
+public:
+	const char *get_name()
+	{
+		return "COMMAND_LONG";
+	}
+
+	MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamCommandLong();
+	}
+
+private:
+	MavlinkOrbSubscription *vehicle_command_sub;
+	struct vehicle_command_s *vehicle_command;
+
+protected:
+	void subscribe(Mavlink *mavlink)
+	{
+		vehicle_command_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command));
+		vehicle_command = (struct vehicle_command_s *)vehicle_command_sub->get_data();
+	}
+
+	void send(const hrt_abstime t)
+	{
+		if (vehicle_command_sub->update(t)) {
+			if (!((vehicle_command->target_system == mavlink_system.sysid) && (vehicle_command->target_component == mavlink_system.compid))) {
+				mavlink_msg_command_long_send(_channel,
+						vehicle_command->target_system,
+						vehicle_command->target_component,
+						vehicle_command->command,
+						vehicle_command->confirmation,
+						vehicle_command->param1,
+						vehicle_command->param2,
+						vehicle_command->param3,
+						vehicle_command->param4,
+						vehicle_command->param5,
+						vehicle_command->param6,
+						vehicle_command->param7);
+			}
+		}
+	}
+};
+
 MavlinkStream *streams_list[] = {
 	new MavlinkStreamHeartbeat(),
 	new MavlinkStreamSysStatus(),
@@ -1299,5 +1344,6 @@ MavlinkStream *streams_list[] = {
 	new MavlinkStreamAttitudeControls(),
 	new MavlinkStreamNamedValueFloat(),
 	new MavlinkStreamCameraCapture(),
+	new MavlinkStreamCommandLong(),
 	nullptr
 };

From 93227f9200ee3369f0d2a3aa2fa7bb2a738e18c9 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 8 Apr 2014 17:29:11 +0200
Subject: [PATCH 13/97] commander: handle_command: do not filter command if
 componentid == 0

---
 src/modules/commander/commander.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index e6de58563e..e7cf2b3fac 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -396,7 +396,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 	bool ret = false;
 
 	/* only handle commands that are meant to be handled by this system and component */
-	if (cmd->target_system != status->system_id || cmd->target_component != status->component_id) {
+	if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components
 		return false;
 	}
 

From b603b002bf4d26c35b0fb10f5192532e43fa57a0 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Tue, 8 Apr 2014 22:23:18 +0400
Subject: [PATCH 14/97] position_estimator_inav: make land detector more
 sensitive to LANDED -> IN AIR transitions

---
 .../position_estimator_inav/position_estimator_inav_main.c      | 2 +-
 .../position_estimator_inav/position_estimator_inav_params.c    | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 368fa6ee25..763b875635 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -763,7 +763,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		float thrust = armed.armed ? actuator.control[3] : 0.0f;
 
 		if (landed) {
-			if (alt_disp2 > land_disp2 && thrust > params.land_thr) {
+			if (alt_disp2 > land_disp2 || thrust > params.land_thr) {
 				landed = false;
 				landed_time = 0;
 			}
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c
index b71f9472f5..dcad5c03b6 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c
@@ -56,7 +56,7 @@ PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f);
 PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f);
 PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f);
 PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f);
-PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.3f);
+PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f);
 
 int parameters_init(struct position_estimator_inav_param_handles *h)
 {

From 839fa1371de96b4647d4ace6c4dfab49d4d97af1 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Tue, 8 Apr 2014 23:28:52 +0400
Subject: [PATCH 15/97] mavlink: commands stream implemented

---
 src/modules/mavlink/mavlink_commands.cpp | 73 ++++++++++++++++++++++++
 src/modules/mavlink/mavlink_commands.h   | 65 +++++++++++++++++++++
 src/modules/mavlink/mavlink_main.cpp     |  6 ++
 src/modules/mavlink/module.mk            |  3 +-
 4 files changed, 146 insertions(+), 1 deletion(-)
 create mode 100644 src/modules/mavlink/mavlink_commands.cpp
 create mode 100644 src/modules/mavlink/mavlink_commands.h

diff --git a/src/modules/mavlink/mavlink_commands.cpp b/src/modules/mavlink/mavlink_commands.cpp
new file mode 100644
index 0000000000..1c1e097a43
--- /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 0000000000..6255d65dff
--- /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 3d897431a1..1ed3f40014 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
@@ -1920,6 +1921,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;
 
@@ -1982,6 +1985,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)) {
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index f09efa2e6c..515fbfadce 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -42,6 +42,7 @@ 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

From 84aa96cf235e591fee13b55ec295db2b8b667c4d Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Tue, 8 Apr 2014 23:29:40 +0400
Subject: [PATCH 16/97] mavlink: minor comments and formatting fixes

---
 src/modules/mavlink/mavlink_messages.cpp | 2 --
 src/modules/mavlink/mavlink_stream.h     | 2 +-
 2 files changed, 1 insertion(+), 3 deletions(-)

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 4d7b9160d3..bb8bba0370 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1252,8 +1252,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)
diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h
index 135e1bce08..def40d9adc 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>

From a96d83e4ec48dd11634709d449e8df2ea30146d4 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 8 Apr 2014 21:40:05 +0200
Subject: [PATCH 17/97] Revert "Merge pull request #816 from
 PX4/mavlink_commandlongstream"

This reverts commit 00ef10f307d3c4a262a15ab5747d854eb4c568d5, reversing
changes made to d55e64d1e54542762510387a22897f504c68a5a6.
---
 src/modules/mavlink/mavlink_messages.cpp | 46 ------------------------
 1 file changed, 46 deletions(-)

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index bb8bba0370..47603b3903 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1271,51 +1271,6 @@ protected:
 	}
 };
 
-class MavlinkStreamCommandLong : public MavlinkStream
-{
-public:
-	const char *get_name()
-	{
-		return "COMMAND_LONG";
-	}
-
-	MavlinkStream *new_instance()
-	{
-		return new MavlinkStreamCommandLong();
-	}
-
-private:
-	MavlinkOrbSubscription *vehicle_command_sub;
-	struct vehicle_command_s *vehicle_command;
-
-protected:
-	void subscribe(Mavlink *mavlink)
-	{
-		vehicle_command_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command));
-		vehicle_command = (struct vehicle_command_s *)vehicle_command_sub->get_data();
-	}
-
-	void send(const hrt_abstime t)
-	{
-		if (vehicle_command_sub->update(t)) {
-			if (!((vehicle_command->target_system == mavlink_system.sysid) && (vehicle_command->target_component == mavlink_system.compid))) {
-				mavlink_msg_command_long_send(_channel,
-						vehicle_command->target_system,
-						vehicle_command->target_component,
-						vehicle_command->command,
-						vehicle_command->confirmation,
-						vehicle_command->param1,
-						vehicle_command->param2,
-						vehicle_command->param3,
-						vehicle_command->param4,
-						vehicle_command->param5,
-						vehicle_command->param6,
-						vehicle_command->param7);
-			}
-		}
-	}
-};
-
 MavlinkStream *streams_list[] = {
 	new MavlinkStreamHeartbeat(),
 	new MavlinkStreamSysStatus(),
@@ -1342,6 +1297,5 @@ MavlinkStream *streams_list[] = {
 	new MavlinkStreamAttitudeControls(),
 	new MavlinkStreamNamedValueFloat(),
 	new MavlinkStreamCameraCapture(),
-	new MavlinkStreamCommandLong(),
 	nullptr
 };

From 746c5086b03c642cc7d5047c9928c26fc8aac5f2 Mon Sep 17 00:00:00 2001
From: Helen Oleynikova <helenoleynikova@gmail.com>
Date: Wed, 9 Apr 2014 17:11:27 +0200
Subject: [PATCH 18/97] Added VICON logging, finally.

---
 src/modules/sdlog2/sdlog2.c          | 30 ++++++++++++++++++----------
 src/modules/sdlog2/sdlog2_messages.h | 12 +++++++++++
 2 files changed, 31 insertions(+), 11 deletions(-)

diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 4b2d01c713..a664c94670 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -226,11 +226,11 @@ sdlog2_usage(const char *reason)
 	}
 
 	errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a -t\n"
-	     "\t-r\tLog rate in Hz, 0 means unlimited rate\n"
-	     "\t-b\tLog buffer size in KiB, default is 8\n"
-	     "\t-e\tEnable logging by default (if not, can be started by command)\n"
-	     "\t-a\tLog only when armed (can be still overriden by command)\n"
-	     "\t-t\tUse date/time for naming log directories and files\n");
+		 "\t-r\tLog rate in Hz, 0 means unlimited rate\n"
+		 "\t-b\tLog buffer size in KiB, default is 8\n"
+		 "\t-e\tEnable logging by default (if not, can be started by command)\n"
+		 "\t-a\tLog only when armed (can be still overriden by command)\n"
+		 "\t-t\tUse date/time for naming log directories and files\n");
 }
 
 /**
@@ -257,11 +257,11 @@ int sdlog2_main(int argc, char *argv[])
 
 		main_thread_should_exit = false;
 		deamon_task = task_spawn_cmd("sdlog2",
-					     SCHED_DEFAULT,
-					     SCHED_PRIORITY_DEFAULT - 30,
-					     3000,
-					     sdlog2_thread_main,
-					     (const char **)argv);
+						 SCHED_DEFAULT,
+						 SCHED_PRIORITY_DEFAULT - 30,
+						 3000,
+						 sdlog2_thread_main,
+						 (const char **)argv);
 		exit(0);
 	}
 
@@ -833,6 +833,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_TELE_s log_TELE;
 			struct log_ESTM_s log_ESTM;
 			struct log_PWR_s log_PWR;
+			struct log_VICON_s log_VICON;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -1163,7 +1164,14 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 		/* --- VICON POSITION --- */
 		if (copy_if_updated(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos)) {
-			// TODO not implemented yet
+            log_msg.msg_type = LOG_VICON_MSG;
+            log_msg.body.log_VICON.x = buf.vicon_pos.x;
+            log_msg.body.log_VICON.y = buf.vicon_pos.y;
+            log_msg.body.log_VICON.z = buf.vicon_pos.z;
+            log_msg.body.log_VICON.pitch = buf.vicon_pos.pitch;
+            log_msg.body.log_VICON.roll = buf.vicon_pos.roll;
+            log_msg.body.log_VICON.yaw = buf.vicon_pos.yaw;
+            LOGBUFFER_WRITE_AND_COUNT(VICON);
 		}
 
 		/* --- FLOW --- */
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 4b70e55c92..ca4346a2ec 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -302,6 +302,17 @@ struct log_PWR_s {
 	uint8_t high_power_rail_overcurrent;
 };
 
+/* --- VICON - VICON POSITION --- */
+#define LOG_VICON_MSG 25
+struct log_VICON_s {
+	float x;
+	float y;
+	float z;
+	float roll;
+	float pitch;
+	float yaw;
+};
+
 /********** SYSTEM MESSAGES, ID > 0x80 **********/
 
 /* --- TIME - TIME STAMP --- */
@@ -352,6 +363,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"),
 	LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"),
 	LOG_FORMAT(PWR, "fffBBBBB", "Periph_5V,Servo_5V,RSSI,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"),
+	LOG_FORMAT(VICON, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"),
 
 	/* system-level messages, ID >= 0x80 */
 	/* FMT: don't write format of format message, it's useless */

From 18e47c0945562a7599aabce4469cb3c41874e055 Mon Sep 17 00:00:00 2001
From: Helen Oleynikova <helenoleynikova@gmail.com>
Date: Wed, 9 Apr 2014 17:21:07 +0200
Subject: [PATCH 19/97] Tabs tabs tabs

---
 src/modules/sdlog2/sdlog2.c | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index a664c94670..12cd90d210 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1164,14 +1164,14 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 		/* --- VICON POSITION --- */
 		if (copy_if_updated(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos)) {
-            log_msg.msg_type = LOG_VICON_MSG;
-            log_msg.body.log_VICON.x = buf.vicon_pos.x;
-            log_msg.body.log_VICON.y = buf.vicon_pos.y;
-            log_msg.body.log_VICON.z = buf.vicon_pos.z;
-            log_msg.body.log_VICON.pitch = buf.vicon_pos.pitch;
-            log_msg.body.log_VICON.roll = buf.vicon_pos.roll;
-            log_msg.body.log_VICON.yaw = buf.vicon_pos.yaw;
-            LOGBUFFER_WRITE_AND_COUNT(VICON);
+			log_msg.msg_type = LOG_VICON_MSG;
+			log_msg.body.log_VICON.x = buf.vicon_pos.x;
+			log_msg.body.log_VICON.y = buf.vicon_pos.y;
+			log_msg.body.log_VICON.z = buf.vicon_pos.z;
+			log_msg.body.log_VICON.pitch = buf.vicon_pos.pitch;
+			log_msg.body.log_VICON.roll = buf.vicon_pos.roll;
+			log_msg.body.log_VICON.yaw = buf.vicon_pos.yaw;
+			LOGBUFFER_WRITE_AND_COUNT(VICON);
 		}
 
 		/* --- FLOW --- */

From df0ab0ecd0bd462a63f6958354e96d65346db4ce Mon Sep 17 00:00:00 2001
From: Helen Oleynikova <helenoleynikova@gmail.com>
Date: Wed, 9 Apr 2014 17:25:33 +0200
Subject: [PATCH 20/97] VICON -> VICN

---
 src/modules/sdlog2/sdlog2_messages.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index ca4346a2ec..a229f860c7 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -363,7 +363,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"),
 	LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"),
 	LOG_FORMAT(PWR, "fffBBBBB", "Periph_5V,Servo_5V,RSSI,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"),
-	LOG_FORMAT(VICON, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"),
+	LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"),
 
 	/* system-level messages, ID >= 0x80 */
 	/* FMT: don't write format of format message, it's useless */

From 99ab001a5a0d7f6cebcfaa5acc86ab1dba57c876 Mon Sep 17 00:00:00 2001
From: Helen Oleynikova <helenoleynikova@gmail.com>
Date: Wed, 9 Apr 2014 17:28:00 +0200
Subject: [PATCH 21/97] VICON -> VICN really this time.

---
 src/modules/sdlog2/sdlog2.c          | 18 +++++++++---------
 src/modules/sdlog2/sdlog2_messages.h |  6 +++---
 2 files changed, 12 insertions(+), 12 deletions(-)

diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 12cd90d210..cba39c5b1d 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -833,7 +833,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_TELE_s log_TELE;
 			struct log_ESTM_s log_ESTM;
 			struct log_PWR_s log_PWR;
-			struct log_VICON_s log_VICON;
+			struct log_VICN_s log_VICN;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -1164,14 +1164,14 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 		/* --- VICON POSITION --- */
 		if (copy_if_updated(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos)) {
-			log_msg.msg_type = LOG_VICON_MSG;
-			log_msg.body.log_VICON.x = buf.vicon_pos.x;
-			log_msg.body.log_VICON.y = buf.vicon_pos.y;
-			log_msg.body.log_VICON.z = buf.vicon_pos.z;
-			log_msg.body.log_VICON.pitch = buf.vicon_pos.pitch;
-			log_msg.body.log_VICON.roll = buf.vicon_pos.roll;
-			log_msg.body.log_VICON.yaw = buf.vicon_pos.yaw;
-			LOGBUFFER_WRITE_AND_COUNT(VICON);
+			log_msg.msg_type = LOG_VICN_MSG;
+			log_msg.body.log_VICN.x = buf.vicon_pos.x;
+			log_msg.body.log_VICN.y = buf.vicon_pos.y;
+			log_msg.body.log_VICN.z = buf.vicon_pos.z;
+			log_msg.body.log_VICN.pitch = buf.vicon_pos.pitch;
+			log_msg.body.log_VICN.roll = buf.vicon_pos.roll;
+			log_msg.body.log_VICN.yaw = buf.vicon_pos.yaw;
+			LOGBUFFER_WRITE_AND_COUNT(VICN);
 		}
 
 		/* --- FLOW --- */
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index a229f860c7..e38681a398 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -302,9 +302,9 @@ struct log_PWR_s {
 	uint8_t high_power_rail_overcurrent;
 };
 
-/* --- VICON - VICON POSITION --- */
-#define LOG_VICON_MSG 25
-struct log_VICON_s {
+/* --- VICN - VICON POSITION --- */
+#define LOG_VICN_MSG 25
+struct log_VICN_s {
 	float x;
 	float y;
 	float z;

From b96669a5314198b88e406fc02940ed2ced291f2b Mon Sep 17 00:00:00 2001
From: Helen Oleynikova <helenoleynikova@gmail.com>
Date: Wed, 9 Apr 2014 17:58:18 +0200
Subject: [PATCH 22/97] Logging changes, baud rate changes.

---
 ROMFS/px4fmu_common/init.d/rc.logging | 2 +-
 ROMFS/px4fmu_common/init.d/rcS        | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging
index c5aef8273c..d6fbe8c4fa 100644
--- a/ROMFS/px4fmu_common/init.d/rc.logging
+++ b/ROMFS/px4fmu_common/init.d/rc.logging
@@ -8,7 +8,7 @@ then
 	if hw_ver compare PX4FMU_V1
 	then
 		echo "Start sdlog2 at 50Hz"
-		sdlog2 start -r 50 -a -b 8 -t
+		sdlog2 start -r 50 -a -b 1 -t -e
 	else
 		echo "Start sdlog2 at 200Hz"
 		sdlog2 start -r 200 -a -b 16 -t
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 0cb6d281a2..a62f67718b 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -410,7 +410,7 @@ then
 			if [ $TTYS1_BUSY == yes ]
 			then
 				# Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else
-				set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0"
+				set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0 -b 115200"
 				
 				# Exit from nsh to free port for mavlink
 				set EXIT_ON_END yes

From 665a2d6a92827e27213d596df7383061636c5dde Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 9 Apr 2014 18:39:23 +0200
Subject: [PATCH 23/97] mavlink and mission topic: added reading in DO_JUMP
 mission items

---
 src/modules/mavlink/mavlink_main.cpp | 10 +++++++++-
 src/modules/uORB/topics/mission.h    |  4 +++-
 2 files changed, 12 insertions(+), 2 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 1ed3f40014..2dba567b04 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -863,7 +863,10 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item
 	case MAV_CMD_NAV_TAKEOFF:
 		mission_item->pitch_min = mavlink_mission_item->param2;
 		break;
-
+	case MAV_CMD_DO_JUMP:
+		mission_item->do_jump_mission_index = mavlink_mission_item->param1;
+		/* TODO: also save param2 (repeat count) */
+		break;
 	default:
 		mission_item->acceptance_radius = mavlink_mission_item->param2;
 		break;
@@ -896,6 +899,11 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_
 		mavlink_mission_item->param2 = mission_item->pitch_min;
 		break;
 
+	case NAV_CMD_DO_JUMP:
+		mavlink_mission_item->param1 = mission_item->do_jump_mission_index;
+		/* TODO: also save param2 (repeat count) */
+		break;
+
 	default:
 		mavlink_mission_item->param2 = mission_item->acceptance_radius;
 		break;
diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h
index 9594c4c6a2..1ccdb7181e 100644
--- a/src/modules/uORB/topics/mission.h
+++ b/src/modules/uORB/topics/mission.h
@@ -58,7 +58,8 @@ enum NAV_CMD {
 	NAV_CMD_LAND=21,
 	NAV_CMD_TAKEOFF=22,
 	NAV_CMD_ROI=80,
-	NAV_CMD_PATHPLANNING=81
+	NAV_CMD_PATHPLANNING=81,
+	NAV_CMD_DO_JUMP=177
 };
 
 enum ORIGIN {
@@ -91,6 +92,7 @@ struct mission_item_s {
 	float pitch_min;		/**< minimal pitch angle for fixed wing takeoff waypoints */
 	bool autocontinue;		/**< true if next waypoint should follow after this one */
 	enum ORIGIN origin;		/**< where the waypoint has been generated		*/
+	int do_jump_mission_index;	/**< the mission index that we want to jump to		*/
 };
 
 struct mission_s

From 705b10fcafcb04440edc306cc07c7430e89ad788 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 9 Apr 2014 18:39:42 +0200
Subject: [PATCH 24/97] navigator: support DO_JUMP misison items

---
 src/modules/navigator/navigator_mission.cpp | 94 ++++++++++++---------
 src/modules/navigator/navigator_mission.h   |  2 +
 2 files changed, 56 insertions(+), 40 deletions(-)

diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/navigator_mission.cpp
index 72dddebfee..ac7e604efe 100644
--- a/src/modules/navigator/navigator_mission.cpp
+++ b/src/modules/navigator/navigator_mission.cpp
@@ -142,22 +142,19 @@ Mission::next_mission_available()
 int
 Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, unsigned *index)
 {
+	int ret = ERROR;
+
 	/* try onboard mission first */
 	if (current_onboard_mission_available()) {
 
-		const ssize_t len = sizeof(struct mission_item_s);
-
-		if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index, new_mission_item, len) != len) {
-			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
-			return ERROR;
+		ret = get_mission_item(DM_KEY_WAYPOINTS_ONBOARD, &_current_onboard_mission_index, new_mission_item);
+		if (ret == OK) {
+			_current_mission_type = MISSION_TYPE_ONBOARD;
+			*onboard = true;
+			*index = _current_onboard_mission_index;
 		}
 
-		_current_mission_type = MISSION_TYPE_ONBOARD;
-		*onboard = true;
-		*index = _current_onboard_mission_index;
-
-		/* otherwise fallback to offboard */
-
+	/* otherwise fallback to offboard */
 	} else if (current_offboard_mission_available()) {
 
 		dm_item_t dm_current;
@@ -169,42 +166,34 @@ Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool
 			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
 		}
 
-		const ssize_t len = sizeof(struct mission_item_s);
-
-		if (dm_read(dm_current, _current_offboard_mission_index, new_mission_item, len) != len) {
-			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
-			_current_mission_type = MISSION_TYPE_NONE;
-			return ERROR;
+		ret = get_mission_item(dm_current, &_current_offboard_mission_index, new_mission_item);
+		if (ret == OK) {
+			_current_mission_type = MISSION_TYPE_OFFBOARD;
+			*onboard = false;
+			*index = _current_offboard_mission_index;
 		}
 
-		_current_mission_type = MISSION_TYPE_OFFBOARD;
-		*onboard = false;
-		*index = _current_offboard_mission_index;
-
 	} else {
 		/* happens when no more mission items can be added as a next item */
 		_current_mission_type = MISSION_TYPE_NONE;
-		return ERROR;
+		ret == ERROR;
 	}
 
-	return OK;
+	return ret;
 }
 
 int
 Mission::get_next_mission_item(struct mission_item_s *new_mission_item)
 {
+	int ret = ERROR;
+
 	/* try onboard mission first */
 	if (next_onboard_mission_available()) {
 
-		const ssize_t len = sizeof(struct mission_item_s);
-
-		if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, new_mission_item, len) != len) {
-			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
-			return ERROR;
-		}
-
-		/* otherwise fallback to offboard */
+		unsigned next_onboard_mission_index = _current_onboard_mission_index + 1;
+		ret = get_mission_item(DM_KEY_WAYPOINTS_ONBOARD, &next_onboard_mission_index, new_mission_item);
 
+	/* otherwise fallback to offboard */
 	} else if (next_offboard_mission_available()) {
 
 		dm_item_t dm_current;
@@ -216,19 +205,14 @@ Mission::get_next_mission_item(struct mission_item_s *new_mission_item)
 			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
 		}
 
-		const ssize_t len = sizeof(struct mission_item_s);
-
-		if (dm_read(dm_current, _current_offboard_mission_index + 1, new_mission_item, len) != len) {
-			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
-			return ERROR;
-		}
+		unsigned next_offboard_mission_index = _current_offboard_mission_index + 1;
+		ret = get_mission_item(dm_current, &next_offboard_mission_index, new_mission_item);
 
 	} else {
 		/* happens when no more mission items can be added as a next item */
-		return ERROR;
+		ret = ERROR;
 	}
-
-	return OK;
+	return ret;
 }
 
 
@@ -316,3 +300,33 @@ Mission::publish_mission_result()
 	/* reset reached bool */
 	_mission_result.mission_reached = false;
 }
+
+int
+Mission::get_mission_item(const dm_item_t dm_item, unsigned *mission_index, struct mission_item_s *new_mission_item)
+{
+	/* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */
+	for (int i=0; i<10; i++) {
+
+		const ssize_t len = sizeof(struct mission_item_s);
+
+		/* read mission item from datamanager */
+		if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) {
+			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+			return ERROR;
+		}
+
+		/* check for DO_JUMP item */
+		if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) {
+			/* set new mission item index and repeat
+			 * we don't have to validate here, if it's invalid, we should realize this later .*/
+			*mission_index = new_mission_item->do_jump_mission_index;
+		} else {
+			/* if it's not a DO_JUMP, then we were successful */
+			return OK;
+		}
+	}
+
+	/* we have given up, we don't want to cycle forever */
+	warnx("ERROR: cycling through mission items without success");
+	return ERROR;
+}
diff --git a/src/modules/navigator/navigator_mission.h b/src/modules/navigator/navigator_mission.h
index 2bd4da82eb..fef1454102 100644
--- a/src/modules/navigator/navigator_mission.h
+++ b/src/modules/navigator/navigator_mission.h
@@ -77,6 +77,8 @@ public:
 	void		publish_mission_result();
 
 private:
+	int		get_mission_item(const dm_item_t dm_item, unsigned *mission_index, struct mission_item_s *new_mission_item);
+
 	bool		current_onboard_mission_available();
 	bool		current_offboard_mission_available();
 	bool		next_onboard_mission_available();

From 5c996065e2ddbbfa3776390163435afa52a4b37d Mon Sep 17 00:00:00 2001
From: Helen Oleynikova <helenoleynikova@gmail.com>
Date: Thu, 10 Apr 2014 09:08:37 +0200
Subject: [PATCH 25/97] Added vicon stream.

---
 src/modules/mavlink/mavlink_messages.cpp | 42 ++++++++++++++++++++++++
 1 file changed, 42 insertions(+)

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 47603b3903..37929edac6 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -640,6 +640,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:
@@ -1297,5 +1338,6 @@ MavlinkStream *streams_list[] = {
 	new MavlinkStreamAttitudeControls(),
 	new MavlinkStreamNamedValueFloat(),
 	new MavlinkStreamCameraCapture(),
+    new MavlinkStreamViconPositionEstimate(),
 	nullptr
 };

From 32c57294d80980c88dfd611cf8a9c34aefc6dcab Mon Sep 17 00:00:00 2001
From: Helen Oleynikova <helenoleynikova@gmail.com>
Date: Thu, 10 Apr 2014 09:08:47 +0200
Subject: [PATCH 26/97] Set startup script to do mavlinks.

---
 ROMFS/px4fmu_common/init.d/rcS | 12 ++++++++++--
 1 file changed, 10 insertions(+), 2 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index a62f67718b..f0bdf32f86 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -410,7 +410,7 @@ then
 			if [ $TTYS1_BUSY == yes ]
 			then
 				# Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else
-				set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0 -b 115200"
+				set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0 -b 57600"
 				
 				# Exit from nsh to free port for mavlink
 				set EXIT_ON_END yes
@@ -421,8 +421,16 @@ then
 		fi
 	fi
 
+    # Main mavlink
 	mavlink start $MAVLINK_FLAGS
-	
+    # Optical Flow
+    mavlink start -d /dev/ttyS3 -m custom -b 115200
+    # Android board
+    #mavlink start -d /dev/ttyS2 -m custom -b 115200
+    #mavlink stream -d /dev/ttyS2 -s LOCAL_POSITION_NED -r 5 
+    #mavlink stream -d /dev/ttyS2 -s ATTITUDE -r 50
+    #mavlink stream -d /dev/ttyS2 -s VICON_POSITION_ESTIMATE -r 50
+
 	#
 	# Start the datamanager
 	#

From e09aed917d0f301658cb7ed97d04c19a08a5482a Mon Sep 17 00:00:00 2001
From: Helen Oleynikova <helenoleynikova@gmail.com>
Date: Thu, 10 Apr 2014 10:26:15 +0200
Subject: [PATCH 27/97] Finished adding a '-w' option.

---
 src/modules/mavlink/mavlink_main.cpp     | 37 +++++++++++++++---------
 src/modules/mavlink/mavlink_main.h       | 12 ++++++++
 src/modules/mavlink/mavlink_receiver.cpp |  4 +++
 3 files changed, 40 insertions(+), 13 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 2dba567b04..10d781a56a 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -167,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);
@@ -186,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);
@@ -204,6 +209,8 @@ 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),
@@ -1776,7 +1783,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:fpv")) != EOF) {
+	while ((ch = getopt(argc, argv, "b:r:d:m:fpvw")) != EOF) {
 		switch (ch) {
 		case 'b':
 			_baudrate = strtoul(optarg, NULL, 10);
@@ -1828,6 +1835,10 @@ Mavlink::task_main(int argc, char *argv[])
 			_verbose = true;
 			break;
 
+		case 'w':
+			_wait_to_transmit = true;
+			break;
+
 		default:
 			err_flag = true;
 			break;
@@ -2172,11 +2183,11 @@ Mavlink::start(int argc, char *argv[])
 	// task - start_helper() only returns
 	// when the started task exits.
 	task_spawn_cmd(buf,
-		       SCHED_DEFAULT,
-		       SCHED_PRIORITY_DEFAULT,
-		       2048,
-		       (main_t)&Mavlink::start_helper,
-		       (const char **)argv);
+			   SCHED_DEFAULT,
+			   SCHED_PRIORITY_DEFAULT,
+			   2048,
+			   (main_t)&Mavlink::start_helper,
+			   (const char **)argv);
 
 	// Ensure that this shell command
 	// does not return before the instance
@@ -2272,7 +2283,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] [-f] [-p] [-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 9941a5f993..2c18261399 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -202,6 +202,14 @@ public:
 
 	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;
 
@@ -216,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 */
 
@@ -270,6 +280,8 @@ private:
 
 	pthread_mutex_t _message_buffer_mutex;
 
+
+
 	/**
 	 * Send one parameter.
 	 *
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 8e4e5f0d4e..9180ec5e60 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -179,6 +179,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

From 7d38af81d7c3f93dc5a03d1fe8082b8a9ac7f109 Mon Sep 17 00:00:00 2001
From: Helen Oleynikova <helenoleynikova@gmail.com>
Date: Thu, 10 Apr 2014 10:26:33 +0200
Subject: [PATCH 28/97] Fixed final parameters in rcS

---
 ROMFS/px4fmu_common/init.d/rcS | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index f0bdf32f86..e37cab04c6 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -422,14 +422,14 @@ then
 	fi
 
     # Main mavlink
-	mavlink start $MAVLINK_FLAGS
+	mavlink start -d /dev/ttyS0 -b 57600
+    mavlink stream -d /dev/ttyS0 -s OPTICAL_FLOW -r 10
     # Optical Flow
     mavlink start -d /dev/ttyS3 -m custom -b 115200
     # Android board
-    #mavlink start -d /dev/ttyS2 -m custom -b 115200
-    #mavlink stream -d /dev/ttyS2 -s LOCAL_POSITION_NED -r 5 
-    #mavlink stream -d /dev/ttyS2 -s ATTITUDE -r 50
-    #mavlink stream -d /dev/ttyS2 -s VICON_POSITION_ESTIMATE -r 50
+    mavlink start -d /dev/ttyS2 -m custom -b 115200 -w
+    mavlink stream -d /dev/ttyS2 -s ATTITUDE -r 50
+    #mavlink stream -d /dev/ttyS2 -s VICON_POSITION_ESTIMATE -r 10
 
 	#
 	# Start the datamanager
@@ -453,7 +453,7 @@ then
 		sh /etc/init.d/rc.logging
 		
 		echo "[init] Start GPS"
-		gps start
+		#gps start
 	fi
 	
 	#

From 0ed8a6d2da0ffd7f32d86be462ed3fea6fc7dcb3 Mon Sep 17 00:00:00 2001
From: Helen Oleynikova <helenoleynikova@gmail.com>
Date: Thu, 10 Apr 2014 10:27:35 +0200
Subject: [PATCH 29/97] Reverted rcS

---
 ROMFS/px4fmu_common/init.d/rcS | 16 ++++------------
 1 file changed, 4 insertions(+), 12 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index e37cab04c6..0cb6d281a2 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -410,7 +410,7 @@ then
 			if [ $TTYS1_BUSY == yes ]
 			then
 				# Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else
-				set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0 -b 57600"
+				set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0"
 				
 				# Exit from nsh to free port for mavlink
 				set EXIT_ON_END yes
@@ -421,16 +421,8 @@ then
 		fi
 	fi
 
-    # Main mavlink
-	mavlink start -d /dev/ttyS0 -b 57600
-    mavlink stream -d /dev/ttyS0 -s OPTICAL_FLOW -r 10
-    # Optical Flow
-    mavlink start -d /dev/ttyS3 -m custom -b 115200
-    # Android board
-    mavlink start -d /dev/ttyS2 -m custom -b 115200 -w
-    mavlink stream -d /dev/ttyS2 -s ATTITUDE -r 50
-    #mavlink stream -d /dev/ttyS2 -s VICON_POSITION_ESTIMATE -r 10
-
+	mavlink start $MAVLINK_FLAGS
+	
 	#
 	# Start the datamanager
 	#
@@ -453,7 +445,7 @@ then
 		sh /etc/init.d/rc.logging
 		
 		echo "[init] Start GPS"
-		#gps start
+		gps start
 	fi
 	
 	#

From ff2e4732a0dd03121a0f6106fd9ad1b2dbeea527 Mon Sep 17 00:00:00 2001
From: Helen Oleynikova <helenoleynikova@gmail.com>
Date: Thu, 10 Apr 2014 10:30:00 +0200
Subject: [PATCH 30/97] Indentation.

---
 src/modules/mavlink/mavlink_main.h | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 2c18261399..66d82b4718 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -203,12 +203,12 @@ public:
 	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)); }
+	/* 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;
@@ -224,8 +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. */
+	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 */
 

From e35d4cd49181eef2c336cd0596f723a47bbf54ba Mon Sep 17 00:00:00 2001
From: Helen Oleynikova <helenoleynikova@gmail.com>
Date: Thu, 10 Apr 2014 10:33:10 +0200
Subject: [PATCH 31/97] Reverted logging.

---
 ROMFS/px4fmu_common/init.d/rc.logging |  2 +-
 src/modules/mavlink/mavlink_main.cpp  | 10 +++++-----
 2 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging
index d6fbe8c4fa..c5aef8273c 100644
--- a/ROMFS/px4fmu_common/init.d/rc.logging
+++ b/ROMFS/px4fmu_common/init.d/rc.logging
@@ -8,7 +8,7 @@ then
 	if hw_ver compare PX4FMU_V1
 	then
 		echo "Start sdlog2 at 50Hz"
-		sdlog2 start -r 50 -a -b 1 -t -e
+		sdlog2 start -r 50 -a -b 8 -t
 	else
 		echo "Start sdlog2 at 200Hz"
 		sdlog2 start -r 200 -a -b 16 -t
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 10d781a56a..c3b59f9408 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -2183,11 +2183,11 @@ Mavlink::start(int argc, char *argv[])
 	// task - start_helper() only returns
 	// when the started task exits.
 	task_spawn_cmd(buf,
-			   SCHED_DEFAULT,
-			   SCHED_PRIORITY_DEFAULT,
-			   2048,
-			   (main_t)&Mavlink::start_helper,
-			   (const char **)argv);
+				   SCHED_DEFAULT,
+				   SCHED_PRIORITY_DEFAULT,
+				   2048,
+				   (main_t)&Mavlink::start_helper,
+				   (const char **)argv);
 
 	// Ensure that this shell command
 	// does not return before the instance

From 7accde9b5bce299722384ebd62ab767331d8b73e Mon Sep 17 00:00:00 2001
From: Helen Oleynikova <helenoleynikova@gmail.com>
Date: Thu, 10 Apr 2014 10:35:30 +0200
Subject: [PATCH 32/97] Whtespace.

---
 src/modules/mavlink/mavlink_main.cpp | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index c3b59f9408..072f16aaef 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -2183,11 +2183,11 @@ Mavlink::start(int argc, char *argv[])
 	// task - start_helper() only returns
 	// when the started task exits.
 	task_spawn_cmd(buf,
-				   SCHED_DEFAULT,
-				   SCHED_PRIORITY_DEFAULT,
-				   2048,
-				   (main_t)&Mavlink::start_helper,
-				   (const char **)argv);
+		       SCHED_DEFAULT,
+		       SCHED_PRIORITY_DEFAULT,
+		       2048,
+		       (main_t)&Mavlink::start_helper,
+		       (const char **)argv);
 
 	// Ensure that this shell command
 	// does not return before the instance

From d6647d841a20574c5e30e1c976b6a4247616d382 Mon Sep 17 00:00:00 2001
From: Helen Oleynikova <helenoleynikova@gmail.com>
Date: Thu, 10 Apr 2014 10:37:58 +0200
Subject: [PATCH 33/97] More whitespace all the time.

---
 src/modules/mavlink/mavlink_messages.cpp | 178 +++++++++++------------
 src/modules/mavlink/mavlink_receiver.cpp |   6 +-
 2 files changed, 92 insertions(+), 92 deletions(-)

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 37929edac6..2d1d92243c 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -94,7 +94,7 @@ cm_uint16_from_m_float(float m)
 }
 
 void get_mavlink_mode_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)
+				uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode)
 {
 	*mavlink_state = 0;
 	*mavlink_base_mode = 0;
@@ -107,7 +107,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 
 	/* arming state */
 	if (status->arming_state == ARMING_STATE_ARMED
-	    || status->arming_state == ARMING_STATE_ARMED_ERROR) {
+		|| status->arming_state == ARMING_STATE_ARMED_ERROR) {
 		*mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
 	}
 
@@ -163,8 +163,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 
 	/* 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
+		|| 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) {
@@ -344,13 +344,13 @@ protected:
 			}
 
 			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);
+							 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);
 		}
 	}
 };
@@ -420,14 +420,14 @@ protected:
 	{
 		if (att_sub->update(t)) {
 			mavlink_msg_attitude_quaternion_send(_channel,
-							     att->timestamp / 1000,
-							     att->q[0],
-							     att->q[1],
-							     att->q[2],
-							     att->q[3],
-							     att->rollspeed,
-							     att->pitchspeed,
-							     att->yawspeed);
+								 att->timestamp / 1000,
+								 att->q[0],
+								 att->q[1],
+								 att->q[2],
+								 att->q[3],
+								 att->rollspeed,
+								 att->pitchspeed,
+								 att->yawspeed);
 		}
 	}
 };
@@ -534,16 +534,16 @@ protected:
 	{
 		if (gps_sub->update(t)) {
 			mavlink_msg_gps_raw_int_send(_channel,
-						     gps->timestamp_position,
-						     gps->fix_type,
-						     gps->lat,
-						     gps->lon,
-						     gps->alt,
-						     cm_uint16_from_m_float(gps->eph_m),
-						     cm_uint16_from_m_float(gps->epv_m),
-						     gps->vel_m_s * 100.0f,
-						     _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f,
-						     gps->satellites_visible);
+							 gps->timestamp_position,
+							 gps->fix_type,
+							 gps->lat,
+							 gps->lon,
+							 gps->alt,
+							 cm_uint16_from_m_float(gps->eph_m),
+							 cm_uint16_from_m_float(gps->epv_m),
+							 gps->vel_m_s * 100.0f,
+							 _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f,
+							 gps->satellites_visible);
 		}
 	}
 };
@@ -586,15 +586,15 @@ protected:
 
 		if (updated) {
 			mavlink_msg_global_position_int_send(_channel,
-							     pos->timestamp / 1000,
-							     pos->lat * 1e7,
-							     pos->lon * 1e7,
-							     pos->alt * 1000.0f,
-							     (pos->alt - home->alt) * 1000.0f,
-							     pos->vel_n * 100.0f,
-							     pos->vel_e * 100.0f,
-							     pos->vel_d * 100.0f,
-							     _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f);
+								 pos->timestamp / 1000,
+								 pos->lat * 1e7,
+								 pos->lon * 1e7,
+								 pos->alt * 1000.0f,
+								 (pos->alt - home->alt) * 1000.0f,
+								 pos->vel_n * 100.0f,
+								 pos->vel_e * 100.0f,
+								 pos->vel_d * 100.0f,
+								 _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f);
 		}
 	}
 };
@@ -628,13 +628,13 @@ protected:
 	{
 		if (pos_sub->update(t)) {
 			mavlink_msg_local_position_ned_send(_channel,
-							    pos->timestamp / 1000,
-							    pos->x,
-							    pos->y,
-							    pos->z,
-							    pos->vx,
-							    pos->vy,
-							    pos->vz);
+								pos->timestamp / 1000,
+								pos->x,
+								pos->y,
+								pos->z,
+								pos->vx,
+								pos->vy,
+								pos->vz);
 		}
 	}
 };
@@ -644,40 +644,40 @@ protected:
 class MavlinkStreamViconPositionEstimate : public MavlinkStream
 {
 public:
-    const char *get_name()
-    {
-        return "VICON_POSITION_ESTIMATE";
-    }
+	const char *get_name()
+	{
+		return "VICON_POSITION_ESTIMATE";
+	}
 
-    MavlinkStream *new_instance()
-    {
-        return new MavlinkStreamViconPositionEstimate();
-    }
+	MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamViconPositionEstimate();
+	}
 
 private:
-    MavlinkOrbSubscription *pos_sub;
-    struct vehicle_vicon_position_s *pos;
+	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 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);
-        }
-    }
+	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);
+		}
+	}
 };
 
 
@@ -869,10 +869,10 @@ protected:
 				}
 
 				mavlink_msg_hil_controls_send(_channel,
-							      hrt_absolute_time(),
-							      out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
-							      mavlink_base_mode,
-							      0);
+								  hrt_absolute_time(),
+								  out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
+								  mavlink_base_mode,
+								  0);
 			} else {
 
 				/* fixed wing: scale all channels except throttle -1 .. 1
@@ -897,10 +897,10 @@ protected:
 				}
 
 				mavlink_msg_hil_controls_send(_channel,
-							      hrt_absolute_time(),
-							      out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
-							      mavlink_base_mode,
-							      0);
+								  hrt_absolute_time(),
+								  out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
+								  mavlink_base_mode,
+								  0);
 			}
 		}
 	}
@@ -1175,12 +1175,12 @@ protected:
 	{
 		if (flow_sub->update(t)) {
 			mavlink_msg_optical_flow_send(_channel,
-						      flow->timestamp,
-						      flow->sensor_id,
-						      flow->flow_raw_x, flow->flow_raw_y,
-						      flow->flow_comp_x_m, flow->flow_comp_y_m,
-						      flow->quality,
-						      flow->ground_distance_m);
+							  flow->timestamp,
+							  flow->sensor_id,
+							  flow->flow_raw_x, flow->flow_raw_y,
+							  flow->flow_comp_x_m, flow->flow_comp_y_m,
+							  flow->quality,
+							  flow->ground_distance_m);
 		}
 	}
 };
@@ -1300,7 +1300,7 @@ protected:
 		(void)status_sub->update(t);
 
 		if (status->arming_state == ARMING_STATE_ARMED
-		    || status->arming_state == ARMING_STATE_ARMED_ERROR) {
+			|| status->arming_state == ARMING_STATE_ARMED_ERROR) {
 
 			/* send camera capture on */
 			mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0);
@@ -1338,6 +1338,6 @@ MavlinkStream *streams_list[] = {
 	new MavlinkStreamAttitudeControls(),
 	new MavlinkStreamNamedValueFloat(),
 	new MavlinkStreamCameraCapture(),
-    new MavlinkStreamViconPositionEstimate(),
+	new MavlinkStreamViconPositionEstimate(),
 	nullptr
 };
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 9180ec5e60..f5029652d1 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -180,9 +180,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		}
 	}
 
-    /* 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);
+	/* 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

From ebf0678333a91b405d8d26803a97ba18b64666dd Mon Sep 17 00:00:00 2001
From: Helen Oleynikova <helenoleynikova@gmail.com>
Date: Thu, 10 Apr 2014 10:41:00 +0200
Subject: [PATCH 34/97] More whitespace all the time.

---
 src/modules/mavlink/mavlink_messages.cpp | 134 +++++++++++------------
 1 file changed, 67 insertions(+), 67 deletions(-)

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 2d1d92243c..648228e3be 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -94,7 +94,7 @@ cm_uint16_from_m_float(float m)
 }
 
 void get_mavlink_mode_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)
+			    uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode)
 {
 	*mavlink_state = 0;
 	*mavlink_base_mode = 0;
@@ -107,7 +107,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 
 	/* arming state */
 	if (status->arming_state == ARMING_STATE_ARMED
-		|| status->arming_state == ARMING_STATE_ARMED_ERROR) {
+	    || status->arming_state == ARMING_STATE_ARMED_ERROR) {
 		*mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
 	}
 
@@ -163,8 +163,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 
 	/* 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
+	    || 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) {
@@ -344,13 +344,13 @@ protected:
 			}
 
 			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);
+						     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);
 		}
 	}
 };
@@ -420,14 +420,14 @@ protected:
 	{
 		if (att_sub->update(t)) {
 			mavlink_msg_attitude_quaternion_send(_channel,
-								 att->timestamp / 1000,
-								 att->q[0],
-								 att->q[1],
-								 att->q[2],
-								 att->q[3],
-								 att->rollspeed,
-								 att->pitchspeed,
-								 att->yawspeed);
+							     att->timestamp / 1000,
+							     att->q[0],
+							     att->q[1],
+							     att->q[2],
+							     att->q[3],
+							     att->rollspeed,
+							     att->pitchspeed,
+							     att->yawspeed);
 		}
 	}
 };
@@ -534,16 +534,16 @@ protected:
 	{
 		if (gps_sub->update(t)) {
 			mavlink_msg_gps_raw_int_send(_channel,
-							 gps->timestamp_position,
-							 gps->fix_type,
-							 gps->lat,
-							 gps->lon,
-							 gps->alt,
-							 cm_uint16_from_m_float(gps->eph_m),
-							 cm_uint16_from_m_float(gps->epv_m),
-							 gps->vel_m_s * 100.0f,
-							 _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f,
-							 gps->satellites_visible);
+						     gps->timestamp_position,
+						     gps->fix_type,
+						     gps->lat,
+						     gps->lon,
+						     gps->alt,
+						     cm_uint16_from_m_float(gps->eph_m),
+						     cm_uint16_from_m_float(gps->epv_m),
+						     gps->vel_m_s * 100.0f,
+						     _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f,
+						     gps->satellites_visible);
 		}
 	}
 };
@@ -586,15 +586,15 @@ protected:
 
 		if (updated) {
 			mavlink_msg_global_position_int_send(_channel,
-								 pos->timestamp / 1000,
-								 pos->lat * 1e7,
-								 pos->lon * 1e7,
-								 pos->alt * 1000.0f,
-								 (pos->alt - home->alt) * 1000.0f,
-								 pos->vel_n * 100.0f,
-								 pos->vel_e * 100.0f,
-								 pos->vel_d * 100.0f,
-								 _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f);
+							     pos->timestamp / 1000,
+							     pos->lat * 1e7,
+							     pos->lon * 1e7,
+							     pos->alt * 1000.0f,
+							     (pos->alt - home->alt) * 1000.0f,
+							     pos->vel_n * 100.0f,
+							     pos->vel_e * 100.0f,
+							     pos->vel_d * 100.0f,
+							     _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f);
 		}
 	}
 };
@@ -628,13 +628,13 @@ protected:
 	{
 		if (pos_sub->update(t)) {
 			mavlink_msg_local_position_ned_send(_channel,
-								pos->timestamp / 1000,
-								pos->x,
-								pos->y,
-								pos->z,
-								pos->vx,
-								pos->vy,
-								pos->vz);
+							    pos->timestamp / 1000,
+							    pos->x,
+							    pos->y,
+							    pos->z,
+							    pos->vx,
+							    pos->vy,
+							    pos->vz);
 		}
 	}
 };
@@ -669,13 +669,13 @@ protected:
 	{
 		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);
+								pos->timestamp / 1000,
+								pos->x,
+								pos->y,
+								pos->z,
+								pos->roll,
+								pos->pitch,
+								pos->yaw);
 		}
 	}
 };
@@ -869,10 +869,10 @@ protected:
 				}
 
 				mavlink_msg_hil_controls_send(_channel,
-								  hrt_absolute_time(),
-								  out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
-								  mavlink_base_mode,
-								  0);
+							      hrt_absolute_time(),
+							      out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
+							      mavlink_base_mode,
+							      0);
 			} else {
 
 				/* fixed wing: scale all channels except throttle -1 .. 1
@@ -897,10 +897,10 @@ protected:
 				}
 
 				mavlink_msg_hil_controls_send(_channel,
-								  hrt_absolute_time(),
-								  out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
-								  mavlink_base_mode,
-								  0);
+							      hrt_absolute_time(),
+							      out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
+							      mavlink_base_mode,
+							      0);
 			}
 		}
 	}
@@ -1175,12 +1175,12 @@ protected:
 	{
 		if (flow_sub->update(t)) {
 			mavlink_msg_optical_flow_send(_channel,
-							  flow->timestamp,
-							  flow->sensor_id,
-							  flow->flow_raw_x, flow->flow_raw_y,
-							  flow->flow_comp_x_m, flow->flow_comp_y_m,
-							  flow->quality,
-							  flow->ground_distance_m);
+						      flow->timestamp,
+						      flow->sensor_id,
+						      flow->flow_raw_x, flow->flow_raw_y,
+						      flow->flow_comp_x_m, flow->flow_comp_y_m,
+						      flow->quality,
+						      flow->ground_distance_m);
 		}
 	}
 };
@@ -1300,7 +1300,7 @@ protected:
 		(void)status_sub->update(t);
 
 		if (status->arming_state == ARMING_STATE_ARMED
-			|| status->arming_state == ARMING_STATE_ARMED_ERROR) {
+		    || status->arming_state == ARMING_STATE_ARMED_ERROR) {
 
 			/* send camera capture on */
 			mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0);

From fde2878413141953e30e41aa9689c924c83e207f Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 13 Apr 2014 20:45:03 +0200
Subject: [PATCH 35/97] mavlink: Change to size optimization

---
 src/modules/mavlink/module.mk | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index 515fbfadce..c44354ff04 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -46,3 +46,5 @@ SRCS		 += mavlink_main.cpp \
 			mavlink_commands.cpp
 
 INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
+
+MAXOPTIMIZATION		 = -Os

From d7c4867898184c5a884bcf144d7222e84dd00f28 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 13 Apr 2014 09:41:23 +0200
Subject: [PATCH 36/97] mavlink: Updated protocol files

---
 .../v1.0/ardupilotmega/ardupilotmega.h        | 108 +--
 .../v1.0/ardupilotmega/mavlink_msg_ahrs.h     |  44 ++
 .../v1.0/ardupilotmega/mavlink_msg_ahrs2.h    |  42 ++
 .../mavlink_msg_airspeed_autocal.h            |  54 ++
 .../v1.0/ardupilotmega/mavlink_msg_ap_adc.h   |  42 ++
 .../mavlink_msg_compassmot_status.h           | 329 +++++++++
 .../v1.0/ardupilotmega/mavlink_msg_data16.h   |  34 +
 .../v1.0/ardupilotmega/mavlink_msg_data32.h   |  34 +
 .../v1.0/ardupilotmega/mavlink_msg_data64.h   |  34 +
 .../v1.0/ardupilotmega/mavlink_msg_data96.h   |  34 +
 .../mavlink_msg_digicam_configure.h           |  52 ++
 .../mavlink_msg_digicam_control.h             |  50 ++
 .../mavlink_msg_fence_fetch_point.h           |  36 +
 .../ardupilotmega/mavlink_msg_fence_point.h   |  42 ++
 .../ardupilotmega/mavlink_msg_fence_status.h  |  38 +
 .../v1.0/ardupilotmega/mavlink_msg_hwstatus.h |  34 +
 .../ardupilotmega/mavlink_msg_limits_status.h |  48 ++
 .../v1.0/ardupilotmega/mavlink_msg_meminfo.h  |  34 +
 .../mavlink_msg_mount_configure.h             |  42 ++
 .../ardupilotmega/mavlink_msg_mount_control.h |  42 ++
 .../ardupilotmega/mavlink_msg_mount_status.h  |  40 +
 .../v1.0/ardupilotmega/mavlink_msg_radio.h    |  44 ++
 .../mavlink_msg_rally_fetch_point.h           |  36 +
 .../ardupilotmega/mavlink_msg_rally_point.h   |  50 ++
 .../ardupilotmega/mavlink_msg_rangefinder.h   |  34 +
 .../mavlink_msg_sensor_offsets.h              |  54 ++
 .../mavlink_msg_set_mag_offsets.h             |  40 +
 .../v1.0/ardupilotmega/mavlink_msg_simstate.h |  52 ++
 .../v1.0/ardupilotmega/mavlink_msg_wind.h     |  36 +
 .../mavlink/v1.0/ardupilotmega/testsuite.h    |  54 ++
 .../mavlink/v1.0/ardupilotmega/version.h      |   2 +-
 .../include/mavlink/v1.0/autoquad/autoquad.h  |  17 +-
 .../autoquad/mavlink_msg_aq_telemetry_f.h     |  72 ++
 .../include/mavlink/v1.0/autoquad/version.h   |   2 +-
 mavlink/include/mavlink/v1.0/common/common.h  | 124 +++-
 .../v1.0/common/mavlink_msg_attitude.h        |  44 ++
 .../common/mavlink_msg_attitude_quaternion.h  |  46 ++
 .../v1.0/common/mavlink_msg_auth_key.h        |  32 +
 .../v1.0/common/mavlink_msg_battery_status.h  |  52 ++
 .../mavlink_msg_change_operator_control.h     |  36 +
 .../mavlink_msg_change_operator_control_ack.h |  36 +
 .../v1.0/common/mavlink_msg_command_ack.h     |  34 +
 .../v1.0/common/mavlink_msg_command_long.h    |  52 ++
 .../v1.0/common/mavlink_msg_data_stream.h     |  36 +
 .../mavlink_msg_data_transmission_handshake.h |  44 ++
 .../mavlink/v1.0/common/mavlink_msg_debug.h   |  36 +
 .../v1.0/common/mavlink_msg_debug_vect.h      |  38 +
 .../v1.0/common/mavlink_msg_distance_sensor.h | 377 ++++++++++
 .../common/mavlink_msg_encapsulated_data.h    |  32 +
 .../mavlink_msg_file_transfer_dir_list.h      |  34 +
 .../common/mavlink_msg_file_transfer_res.h    |  34 +
 .../common/mavlink_msg_file_transfer_start.h  |  38 +
 .../common/mavlink_msg_global_position_int.h  |  48 ++
 ...mavlink_msg_global_position_setpoint_int.h |  40 +
 ...link_msg_global_vision_position_estimate.h |  44 ++
 .../v1.0/common/mavlink_msg_gps2_raw.h        |  54 ++
 .../common/mavlink_msg_gps_global_origin.h    |  36 +
 .../v1.0/common/mavlink_msg_gps_inject_data.h |  36 +
 .../v1.0/common/mavlink_msg_gps_raw_int.h     |  50 ++
 .../v1.0/common/mavlink_msg_gps_status.h      |  40 +
 .../v1.0/common/mavlink_msg_heartbeat.h       |  42 ++
 .../v1.0/common/mavlink_msg_highres_imu.h     |  60 ++
 .../v1.0/common/mavlink_msg_hil_controls.h    |  52 ++
 .../mavlink/v1.0/common/mavlink_msg_hil_gps.h |  56 ++
 .../common/mavlink_msg_hil_optical_flow.h     |  46 ++
 .../common/mavlink_msg_hil_rc_inputs_raw.h    |  58 ++
 .../v1.0/common/mavlink_msg_hil_sensor.h      |  60 ++
 .../v1.0/common/mavlink_msg_hil_state.h       |  62 ++
 .../common/mavlink_msg_hil_state_quaternion.h |  60 ++
 .../common/mavlink_msg_local_position_ned.h   |  44 ++
 ..._local_position_ned_system_global_offset.h |  44 ++
 .../mavlink_msg_local_position_setpoint.h     |  40 +
 .../v1.0/common/mavlink_msg_log_data.h        |  36 +
 .../v1.0/common/mavlink_msg_log_entry.h       |  40 +
 .../v1.0/common/mavlink_msg_log_erase.h       |  34 +
 .../common/mavlink_msg_log_request_data.h     |  40 +
 .../v1.0/common/mavlink_msg_log_request_end.h |  34 +
 .../common/mavlink_msg_log_request_list.h     |  38 +
 .../v1.0/common/mavlink_msg_manual_control.h  |  42 ++
 .../v1.0/common/mavlink_msg_manual_setpoint.h |  44 ++
 .../v1.0/common/mavlink_msg_memory_vect.h     |  36 +
 .../v1.0/common/mavlink_msg_mission_ack.h     |  36 +
 .../common/mavlink_msg_mission_clear_all.h    |  34 +
 .../v1.0/common/mavlink_msg_mission_count.h   |  36 +
 .../v1.0/common/mavlink_msg_mission_current.h |  32 +
 .../v1.0/common/mavlink_msg_mission_item.h    |  58 ++
 .../common/mavlink_msg_mission_item_reached.h |  32 +
 .../v1.0/common/mavlink_msg_mission_request.h |  36 +
 .../common/mavlink_msg_mission_request_list.h |  34 +
 ...mavlink_msg_mission_request_partial_list.h |  38 +
 .../common/mavlink_msg_mission_set_current.h  |  36 +
 .../mavlink_msg_mission_write_partial_list.h  |  38 +
 .../common/mavlink_msg_named_value_float.h    |  34 +
 .../v1.0/common/mavlink_msg_named_value_int.h |  34 +
 .../mavlink_msg_nav_controller_output.h       |  46 ++
 .../common/mavlink_msg_omnidirectional_flow.h |  40 +
 .../v1.0/common/mavlink_msg_optical_flow.h    |  46 ++
 .../common/mavlink_msg_param_request_list.h   |  34 +
 .../common/mavlink_msg_param_request_read.h   |  36 +
 .../v1.0/common/mavlink_msg_param_set.h       |  38 +
 .../v1.0/common/mavlink_msg_param_value.h     |  38 +
 .../mavlink/v1.0/common/mavlink_msg_ping.h    |  38 +
 .../v1.0/common/mavlink_msg_power_status.h    | 257 +++++++
 .../v1.0/common/mavlink_msg_radio_status.h    |  44 ++
 .../mavlink/v1.0/common/mavlink_msg_raw_imu.h |  50 ++
 .../v1.0/common/mavlink_msg_raw_pressure.h    |  40 +
 .../v1.0/common/mavlink_msg_rc_channels.h     | 689 ++++++++++++++++++
 .../common/mavlink_msg_rc_channels_override.h |  50 ++
 .../v1.0/common/mavlink_msg_rc_channels_raw.h |  52 ++
 .../common/mavlink_msg_rc_channels_scaled.h   |  52 ++
 .../common/mavlink_msg_request_data_stream.h  |  40 +
 ...msg_roll_pitch_yaw_rates_thrust_setpoint.h |  40 +
 ...msg_roll_pitch_yaw_speed_thrust_setpoint.h |  40 +
 ...vlink_msg_roll_pitch_yaw_thrust_setpoint.h |  40 +
 .../common/mavlink_msg_safety_allowed_area.h  |  44 ++
 .../mavlink_msg_safety_set_allowed_area.h     |  48 ++
 .../v1.0/common/mavlink_msg_scaled_imu.h      |  50 ++
 .../v1.0/common/mavlink_msg_scaled_imu2.h     |  50 ++
 .../v1.0/common/mavlink_msg_scaled_pressure.h |  38 +
 .../v1.0/common/mavlink_msg_serial_control.h  | 321 ++++++++
 .../common/mavlink_msg_servo_output_raw.h     |  50 ++
 ...ink_msg_set_global_position_setpoint_int.h |  40 +
 .../mavlink_msg_set_gps_global_origin.h       |  38 +
 .../mavlink_msg_set_local_position_setpoint.h |  44 ++
 .../v1.0/common/mavlink_msg_set_mode.h        |  36 +
 .../mavlink_msg_set_quad_motors_setpoint.h    |  40 +
 ...set_quad_swarm_led_roll_pitch_yaw_thrust.h |  46 ++
 ...msg_set_quad_swarm_roll_pitch_yaw_thrust.h |  40 +
 ...link_msg_set_roll_pitch_yaw_speed_thrust.h |  42 ++
 .../mavlink_msg_set_roll_pitch_yaw_thrust.h   |  42 ++
 .../v1.0/common/mavlink_msg_setpoint_6dof.h   |  44 ++
 .../v1.0/common/mavlink_msg_setpoint_8dof.h   |  48 ++
 .../v1.0/common/mavlink_msg_sim_state.h       |  72 ++
 .../common/mavlink_msg_state_correction.h     |  48 ++
 .../v1.0/common/mavlink_msg_statustext.h      |  32 +
 .../v1.0/common/mavlink_msg_sys_status.h      |  56 ++
 .../v1.0/common/mavlink_msg_system_time.h     |  34 +
 .../mavlink/v1.0/common/mavlink_msg_vfr_hud.h |  42 ++
 .../mavlink_msg_vicon_position_estimate.h     |  44 ++
 .../mavlink_msg_vision_position_estimate.h    |  44 ++
 .../mavlink_msg_vision_speed_estimate.h       |  38 +
 .../include/mavlink/v1.0/common/testsuite.h   | 244 +++++++
 mavlink/include/mavlink/v1.0/common/version.h |   2 +-
 .../mavlink/v1.0/matrixpilot/matrixpilot.h    |  17 +-
 .../v1.0/matrixpilot/mavlink_msg_airspeeds.h  |  44 ++
 .../v1.0/matrixpilot/mavlink_msg_altitudes.h  |  44 ++
 ...avlink_msg_flexifunction_buffer_function.h |  42 ++
 ...nk_msg_flexifunction_buffer_function_ack.h |  38 +
 .../mavlink_msg_flexifunction_command.h       |  36 +
 .../mavlink_msg_flexifunction_command_ack.h   |  34 +
 .../mavlink_msg_flexifunction_directory.h     |  40 +
 .../mavlink_msg_flexifunction_directory_ack.h |  42 ++
 .../mavlink_msg_flexifunction_read_req.h      |  38 +
 .../mavlink_msg_flexifunction_set.h           |  34 +
 .../mavlink_msg_serial_udb_extra_f13.h        |  38 +
 .../mavlink_msg_serial_udb_extra_f14.h        |  52 ++
 .../mavlink_msg_serial_udb_extra_f15.h        |  34 +
 .../mavlink_msg_serial_udb_extra_f16.h        |  34 +
 .../mavlink_msg_serial_udb_extra_f2_a.h       |  86 +++
 .../mavlink_msg_serial_udb_extra_f2_b.h       |  96 +++
 .../mavlink_msg_serial_udb_extra_f4.h         |  50 ++
 .../mavlink_msg_serial_udb_extra_f5.h         |  42 ++
 .../mavlink_msg_serial_udb_extra_f6.h         |  40 +
 .../mavlink_msg_serial_udb_extra_f7.h         |  42 ++
 .../mavlink_msg_serial_udb_extra_f8.h         |  44 ++
 .../mavlink/v1.0/matrixpilot/version.h        |   2 +-
 .../pixhawk/mavlink_msg_attitude_control.h    |  48 ++
 .../v1.0/pixhawk/mavlink_msg_brief_feature.h  |  44 ++
 .../pixhawk/mavlink_msg_image_available.h     |  76 ++
 .../mavlink_msg_image_trigger_control.h       |  32 +
 .../pixhawk/mavlink_msg_image_triggered.h     |  54 ++
 .../mavlink/v1.0/pixhawk/mavlink_msg_marker.h |  44 ++
 .../pixhawk/mavlink_msg_pattern_detected.h    |  36 +
 .../pixhawk/mavlink_msg_point_of_interest.h   |  44 ++
 ...mavlink_msg_point_of_interest_connection.h |  50 ++
 .../mavlink_msg_position_control_setpoint.h   |  40 +
 .../v1.0/pixhawk/mavlink_msg_raw_aux.h        |  44 ++
 .../pixhawk/mavlink_msg_set_cam_shutter.h     |  42 ++
 .../mavlink_msg_set_position_control_offset.h |  42 ++
 .../pixhawk/mavlink_msg_watchdog_command.h    |  38 +
 .../pixhawk/mavlink_msg_watchdog_heartbeat.h  |  34 +
 .../mavlink_msg_watchdog_process_info.h       |  38 +
 .../mavlink_msg_watchdog_process_status.h     |  42 ++
 .../include/mavlink/v1.0/pixhawk/pixhawk.h    |  17 +-
 .../include/mavlink/v1.0/pixhawk/version.h    |   2 +-
 .../sensesoar/mavlink_msg_cmd_airspeed_ack.h  |  34 +
 .../sensesoar/mavlink_msg_cmd_airspeed_chng.h |  34 +
 .../v1.0/sensesoar/mavlink_msg_filt_rot_vel.h |  32 +
 .../v1.0/sensesoar/mavlink_msg_llc_out.h      |  34 +
 .../v1.0/sensesoar/mavlink_msg_obs_air_temp.h |  32 +
 .../sensesoar/mavlink_msg_obs_air_velocity.h  |  36 +
 .../v1.0/sensesoar/mavlink_msg_obs_attitude.h |  32 +
 .../v1.0/sensesoar/mavlink_msg_obs_bias.h     |  34 +
 .../v1.0/sensesoar/mavlink_msg_obs_position.h |  36 +
 .../v1.0/sensesoar/mavlink_msg_obs_qff.h      |  32 +
 .../v1.0/sensesoar/mavlink_msg_obs_velocity.h |  32 +
 .../v1.0/sensesoar/mavlink_msg_obs_wind.h     |  32 +
 .../v1.0/sensesoar/mavlink_msg_pm_elec.h      |  34 +
 .../v1.0/sensesoar/mavlink_msg_sys_stat.h     |  38 +
 .../mavlink/v1.0/sensesoar/sensesoar.h        |   6 +-
 .../include/mavlink/v1.0/sensesoar/version.h  |   2 +-
 201 files changed, 10148 insertions(+), 120 deletions(-)
 create mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_compassmot_status.h
 create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_distance_sensor.h
 create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_power_status.h
 create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels.h
 create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_serial_control.h

diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h b/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h
index 2acb7965ca..c887dc68a1 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h
@@ -12,15 +12,15 @@ extern "C" {
 // MESSAGE LENGTHS AND CRCS
 
 #ifndef MAVLINK_MESSAGE_LENGTHS
-#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 0, 0, 0, 0, 0, 13, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 44, 3, 9, 22, 12, 18, 34, 66, 98, 8, 48, 19, 3, 0, 24, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0}
+#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 42, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 0, 0, 0, 13, 255, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 44, 3, 9, 22, 12, 18, 34, 66, 98, 8, 48, 19, 3, 20, 24, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0}
 #endif
 
 #ifndef MAVLINK_MESSAGE_CRCS
-#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 0, 0, 0, 0, 0, 29, 223, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 154, 21, 21, 144, 1, 234, 73, 181, 22, 83, 167, 138, 234, 0, 47, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0}
+#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 85, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 154, 21, 21, 144, 1, 234, 73, 181, 22, 83, 167, 138, 234, 240, 47, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0}
 #endif
 
 #ifndef MAVLINK_MESSAGE_INFO
-#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, MAVLINK_MESSAGE_INFO_RADIO, MAVLINK_MESSAGE_INFO_LIMITS_STATUS, MAVLINK_MESSAGE_INFO_WIND, MAVLINK_MESSAGE_INFO_DATA16, MAVLINK_MESSAGE_INFO_DATA32, MAVLINK_MESSAGE_INFO_DATA64, MAVLINK_MESSAGE_INFO_DATA96, MAVLINK_MESSAGE_INFO_RANGEFINDER, MAVLINK_MESSAGE_INFO_AIRSPEED_AUTOCAL, MAVLINK_MESSAGE_INFO_RALLY_POINT, MAVLINK_MESSAGE_INFO_RALLY_FETCH_POINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_AHRS2, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}}
+#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, MAVLINK_MESSAGE_INFO_RADIO, MAVLINK_MESSAGE_INFO_LIMITS_STATUS, MAVLINK_MESSAGE_INFO_WIND, MAVLINK_MESSAGE_INFO_DATA16, MAVLINK_MESSAGE_INFO_DATA32, MAVLINK_MESSAGE_INFO_DATA64, MAVLINK_MESSAGE_INFO_DATA96, MAVLINK_MESSAGE_INFO_RANGEFINDER, MAVLINK_MESSAGE_INFO_AIRSPEED_AUTOCAL, MAVLINK_MESSAGE_INFO_RALLY_POINT, MAVLINK_MESSAGE_INFO_RALLY_FETCH_POINT, MAVLINK_MESSAGE_INFO_COMPASSMOT_STATUS, MAVLINK_MESSAGE_INFO_AHRS2, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}}
 #endif
 
 #include "../protocol.h"
@@ -30,95 +30,6 @@ extern "C" {
 // ENUM DEFINITIONS
 
 
-/** @brief Enumeration of possible mount operation modes */
-#ifndef HAVE_ENUM_MAV_MOUNT_MODE
-#define HAVE_ENUM_MAV_MOUNT_MODE
-enum MAV_MOUNT_MODE
-{
-	MAV_MOUNT_MODE_RETRACT=0, /* Load and keep safe position (Roll,Pitch,Yaw) from EEPROM and stop stabilization | */
-	MAV_MOUNT_MODE_NEUTRAL=1, /* Load and keep neutral position (Roll,Pitch,Yaw) from EEPROM. | */
-	MAV_MOUNT_MODE_MAVLINK_TARGETING=2, /* Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization | */
-	MAV_MOUNT_MODE_RC_TARGETING=3, /* Load neutral position and start RC Roll,Pitch,Yaw control with stabilization | */
-	MAV_MOUNT_MODE_GPS_POINT=4, /* Load neutral position and start to point to Lat,Lon,Alt | */
-	MAV_MOUNT_MODE_ENUM_END=5, /*  | */
-};
-#endif
-
-/** @brief  */
-#ifndef HAVE_ENUM_MAV_CMD
-#define HAVE_ENUM_MAV_CMD
-enum MAV_CMD
-{
-	MAV_CMD_NAV_WAYPOINT=16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude|  */
-	MAV_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude|  */
-	MAV_CMD_NAV_LOITER_TURNS=18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude|  */
-	MAV_CMD_NAV_LOITER_TIME=19, /* Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude|  */
-	MAV_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_NAV_LAND=21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude|  */
-	MAV_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude|  */
-	MAV_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|  */
-	MAV_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal|  */
-	MAV_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate.  Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude|  */
-	MAV_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty|  */
-	MAV_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list.  Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed  (m/s, -1 indicates no change)| Throttle  ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude|  */
-	MAV_CMD_DO_SET_PARAMETER=180, /* Set a system parameter.  Caution!  Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_DO_SET_RELAY=181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty|  */
-	MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty|  */
-	MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|  */
-	MAV_CMD_DO_DIGICAM_CONFIGURE=202, /* Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)|  */
-	MAV_CMD_DO_DIGICAM_CONTROL=203, /* Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty|  */
-	MAV_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty|  */
-	MAV_CMD_DO_MOUNT_CONTROL=205, /* Mission command to control a camera or antenna mount |pitch(deg*100) or lat, depending on mount mode.| roll(deg*100) or lon depending on mount mode| yaw(deg*100) or alt (in cm) depending on mount mode| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_DO_SET_CAM_TRIGG_DIST=206, /* Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty|  */
-	MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units|  */
-	MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty|  */
-	MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty|  */
-	MAV_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position|  */
-	MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item:  the last mission item to run (after this item is run, the mission ends)|  */
-	MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm|  */
-	MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX|  */
-	MAV_CMD_ENUM_END=501, /*  | */
-};
-#endif
-
-/** @brief  */
-#ifndef HAVE_ENUM_FENCE_ACTION
-#define HAVE_ENUM_FENCE_ACTION
-enum FENCE_ACTION
-{
-	FENCE_ACTION_NONE=0, /* Disable fenced mode | */
-	FENCE_ACTION_GUIDED=1, /* Switched to guided mode to return point (fence point 0) | */
-	FENCE_ACTION_REPORT=2, /* Report fence breach, but don't take action | */
-	FENCE_ACTION_GUIDED_THR_PASS=3, /* Switched to guided mode to return point (fence point 0) with manual throttle control | */
-	FENCE_ACTION_ENUM_END=4, /*  | */
-};
-#endif
-
-/** @brief  */
-#ifndef HAVE_ENUM_FENCE_BREACH
-#define HAVE_ENUM_FENCE_BREACH
-enum FENCE_BREACH
-{
-	FENCE_BREACH_NONE=0, /* No last fence breach | */
-	FENCE_BREACH_MINALT=1, /* Breached minimum altitude | */
-	FENCE_BREACH_MAXALT=2, /* Breached maximum altitude | */
-	FENCE_BREACH_BOUNDARY=3, /* Breached fence boundary | */
-	FENCE_BREACH_ENUM_END=4, /*  | */
-};
-#endif
-
 /** @brief  */
 #ifndef HAVE_ENUM_LIMITS_STATE
 #define HAVE_ENUM_LIMITS_STATE
@@ -157,6 +68,18 @@ enum RALLY_FLAGS
 };
 #endif
 
+/** @brief  */
+#ifndef HAVE_ENUM_PARACHUTE_ACTION
+#define HAVE_ENUM_PARACHUTE_ACTION
+enum PARACHUTE_ACTION
+{
+	PARACHUTE_DISABLE=0, /* Disable parachute release | */
+	PARACHUTE_ENABLE=1, /* Enable parachute release | */
+	PARACHUTE_RELEASE=2, /* Release parachute | */
+	PARACHUTE_ACTION_ENUM_END=3, /*  | */
+};
+#endif
+
 #include "../common/common.h"
 
 // MAVLINK VERSION
@@ -197,6 +120,7 @@ enum RALLY_FLAGS
 #include "./mavlink_msg_airspeed_autocal.h"
 #include "./mavlink_msg_rally_point.h"
 #include "./mavlink_msg_rally_fetch_point.h"
+#include "./mavlink_msg_compassmot_status.h"
 #include "./mavlink_msg_ahrs2.h"
 
 #ifdef __cplusplus
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h
index c3ead11401..50ddc79e5a 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h
@@ -212,6 +212,50 @@ static inline void mavlink_msg_ahrs_send(mavlink_channel_t chan, float omegaIx,
 #endif
 }
 
+#if MAVLINK_MSG_ID_AHRS_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_ahrs_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  float omegaIx, float omegaIy, float omegaIz, float accel_weight, float renorm_val, float error_rp, float error_yaw)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, omegaIx);
+	_mav_put_float(buf, 4, omegaIy);
+	_mav_put_float(buf, 8, omegaIz);
+	_mav_put_float(buf, 12, accel_weight);
+	_mav_put_float(buf, 16, renorm_val);
+	_mav_put_float(buf, 20, error_rp);
+	_mav_put_float(buf, 24, error_yaw);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, buf, MAVLINK_MSG_ID_AHRS_LEN, MAVLINK_MSG_ID_AHRS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, buf, MAVLINK_MSG_ID_AHRS_LEN);
+#endif
+#else
+	mavlink_ahrs_t *packet = (mavlink_ahrs_t *)msgbuf;
+	packet->omegaIx = omegaIx;
+	packet->omegaIy = omegaIy;
+	packet->omegaIz = omegaIz;
+	packet->accel_weight = accel_weight;
+	packet->renorm_val = renorm_val;
+	packet->error_rp = error_rp;
+	packet->error_yaw = error_yaw;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, (const char *)packet, MAVLINK_MSG_ID_AHRS_LEN, MAVLINK_MSG_ID_AHRS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, (const char *)packet, MAVLINK_MSG_ID_AHRS_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE AHRS UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs2.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs2.h
index f6fde9590c..33e0066882 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs2.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs2.h
@@ -201,6 +201,48 @@ static inline void mavlink_msg_ahrs2_send(mavlink_channel_t chan, float roll, fl
 #endif
 }
 
+#if MAVLINK_MSG_ID_AHRS2_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_ahrs2_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  float roll, float pitch, float yaw, float altitude, int32_t lat, int32_t lng)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, roll);
+	_mav_put_float(buf, 4, pitch);
+	_mav_put_float(buf, 8, yaw);
+	_mav_put_float(buf, 12, altitude);
+	_mav_put_int32_t(buf, 16, lat);
+	_mav_put_int32_t(buf, 20, lng);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, buf, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, buf, MAVLINK_MSG_ID_AHRS2_LEN);
+#endif
+#else
+	mavlink_ahrs2_t *packet = (mavlink_ahrs2_t *)msgbuf;
+	packet->roll = roll;
+	packet->pitch = pitch;
+	packet->yaw = yaw;
+	packet->altitude = altitude;
+	packet->lat = lat;
+	packet->lng = lng;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, (const char *)packet, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, (const char *)packet, MAVLINK_MSG_ID_AHRS2_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE AHRS2 UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_airspeed_autocal.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_airspeed_autocal.h
index d046f2ad09..521831c8ff 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_airspeed_autocal.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_airspeed_autocal.h
@@ -267,6 +267,60 @@ static inline void mavlink_msg_airspeed_autocal_send(mavlink_channel_t chan, flo
 #endif
 }
 
+#if MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_airspeed_autocal_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  float vx, float vy, float vz, float diff_pressure, float EAS2TAS, float ratio, float state_x, float state_y, float state_z, float Pax, float Pby, float Pcz)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, vx);
+	_mav_put_float(buf, 4, vy);
+	_mav_put_float(buf, 8, vz);
+	_mav_put_float(buf, 12, diff_pressure);
+	_mav_put_float(buf, 16, EAS2TAS);
+	_mav_put_float(buf, 20, ratio);
+	_mav_put_float(buf, 24, state_x);
+	_mav_put_float(buf, 28, state_y);
+	_mav_put_float(buf, 32, state_z);
+	_mav_put_float(buf, 36, Pax);
+	_mav_put_float(buf, 40, Pby);
+	_mav_put_float(buf, 44, Pcz);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, buf, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, buf, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN);
+#endif
+#else
+	mavlink_airspeed_autocal_t *packet = (mavlink_airspeed_autocal_t *)msgbuf;
+	packet->vx = vx;
+	packet->vy = vy;
+	packet->vz = vz;
+	packet->diff_pressure = diff_pressure;
+	packet->EAS2TAS = EAS2TAS;
+	packet->ratio = ratio;
+	packet->state_x = state_x;
+	packet->state_y = state_y;
+	packet->state_z = state_z;
+	packet->Pax = Pax;
+	packet->Pby = Pby;
+	packet->Pcz = Pcz;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, (const char *)packet, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, (const char *)packet, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE AIRSPEED_AUTOCAL UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ap_adc.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ap_adc.h
index 821ce73e47..3c18e644ef 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ap_adc.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ap_adc.h
@@ -201,6 +201,48 @@ static inline void mavlink_msg_ap_adc_send(mavlink_channel_t chan, uint16_t adc1
 #endif
 }
 
+#if MAVLINK_MSG_ID_AP_ADC_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_ap_adc_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t adc5, uint16_t adc6)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint16_t(buf, 0, adc1);
+	_mav_put_uint16_t(buf, 2, adc2);
+	_mav_put_uint16_t(buf, 4, adc3);
+	_mav_put_uint16_t(buf, 6, adc4);
+	_mav_put_uint16_t(buf, 8, adc5);
+	_mav_put_uint16_t(buf, 10, adc6);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, buf, MAVLINK_MSG_ID_AP_ADC_LEN, MAVLINK_MSG_ID_AP_ADC_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, buf, MAVLINK_MSG_ID_AP_ADC_LEN);
+#endif
+#else
+	mavlink_ap_adc_t *packet = (mavlink_ap_adc_t *)msgbuf;
+	packet->adc1 = adc1;
+	packet->adc2 = adc2;
+	packet->adc3 = adc3;
+	packet->adc4 = adc4;
+	packet->adc5 = adc5;
+	packet->adc6 = adc6;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, (const char *)packet, MAVLINK_MSG_ID_AP_ADC_LEN, MAVLINK_MSG_ID_AP_ADC_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, (const char *)packet, MAVLINK_MSG_ID_AP_ADC_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE AP_ADC UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_compassmot_status.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_compassmot_status.h
new file mode 100644
index 0000000000..9655459889
--- /dev/null
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_compassmot_status.h
@@ -0,0 +1,329 @@
+// MESSAGE COMPASSMOT_STATUS PACKING
+
+#define MAVLINK_MSG_ID_COMPASSMOT_STATUS 177
+
+typedef struct __mavlink_compassmot_status_t
+{
+ float current; ///< current (amps)
+ float CompensationX; ///< Motor Compensation X
+ float CompensationY; ///< Motor Compensation Y
+ float CompensationZ; ///< Motor Compensation Z
+ uint16_t throttle; ///< throttle (percent*10)
+ uint16_t interference; ///< interference (percent)
+} mavlink_compassmot_status_t;
+
+#define MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN 20
+#define MAVLINK_MSG_ID_177_LEN 20
+
+#define MAVLINK_MSG_ID_COMPASSMOT_STATUS_CRC 240
+#define MAVLINK_MSG_ID_177_CRC 240
+
+
+
+#define MAVLINK_MESSAGE_INFO_COMPASSMOT_STATUS { \
+	"COMPASSMOT_STATUS", \
+	6, \
+	{  { "current", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_compassmot_status_t, current) }, \
+         { "CompensationX", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_compassmot_status_t, CompensationX) }, \
+         { "CompensationY", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_compassmot_status_t, CompensationY) }, \
+         { "CompensationZ", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_compassmot_status_t, CompensationZ) }, \
+         { "throttle", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_compassmot_status_t, throttle) }, \
+         { "interference", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_compassmot_status_t, interference) }, \
+         } \
+}
+
+
+/**
+ * @brief Pack a compassmot_status message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ *
+ * @param throttle throttle (percent*10)
+ * @param current current (amps)
+ * @param interference interference (percent)
+ * @param CompensationX Motor Compensation X
+ * @param CompensationY Motor Compensation Y
+ * @param CompensationZ Motor Compensation Z
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_compassmot_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
+						       uint16_t throttle, float current, uint16_t interference, float CompensationX, float CompensationY, float CompensationZ)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char buf[MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN];
+	_mav_put_float(buf, 0, current);
+	_mav_put_float(buf, 4, CompensationX);
+	_mav_put_float(buf, 8, CompensationY);
+	_mav_put_float(buf, 12, CompensationZ);
+	_mav_put_uint16_t(buf, 16, throttle);
+	_mav_put_uint16_t(buf, 18, interference);
+
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN);
+#else
+	mavlink_compassmot_status_t packet;
+	packet.current = current;
+	packet.CompensationX = CompensationX;
+	packet.CompensationY = CompensationY;
+	packet.CompensationZ = CompensationZ;
+	packet.throttle = throttle;
+	packet.interference = interference;
+
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN);
+#endif
+
+	msg->msgid = MAVLINK_MSG_ID_COMPASSMOT_STATUS;
+#if MAVLINK_CRC_EXTRA
+    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN, MAVLINK_MSG_ID_COMPASSMOT_STATUS_CRC);
+#else
+    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN);
+#endif
+}
+
+/**
+ * @brief Pack a compassmot_status message on a channel
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message will be sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param throttle throttle (percent*10)
+ * @param current current (amps)
+ * @param interference interference (percent)
+ * @param CompensationX Motor Compensation X
+ * @param CompensationY Motor Compensation Y
+ * @param CompensationZ Motor Compensation Z
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_compassmot_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
+							   mavlink_message_t* msg,
+						           uint16_t throttle,float current,uint16_t interference,float CompensationX,float CompensationY,float CompensationZ)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char buf[MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN];
+	_mav_put_float(buf, 0, current);
+	_mav_put_float(buf, 4, CompensationX);
+	_mav_put_float(buf, 8, CompensationY);
+	_mav_put_float(buf, 12, CompensationZ);
+	_mav_put_uint16_t(buf, 16, throttle);
+	_mav_put_uint16_t(buf, 18, interference);
+
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN);
+#else
+	mavlink_compassmot_status_t packet;
+	packet.current = current;
+	packet.CompensationX = CompensationX;
+	packet.CompensationY = CompensationY;
+	packet.CompensationZ = CompensationZ;
+	packet.throttle = throttle;
+	packet.interference = interference;
+
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN);
+#endif
+
+	msg->msgid = MAVLINK_MSG_ID_COMPASSMOT_STATUS;
+#if MAVLINK_CRC_EXTRA
+    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN, MAVLINK_MSG_ID_COMPASSMOT_STATUS_CRC);
+#else
+    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN);
+#endif
+}
+
+/**
+ * @brief Encode a compassmot_status struct
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ * @param compassmot_status C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_compassmot_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_compassmot_status_t* compassmot_status)
+{
+	return mavlink_msg_compassmot_status_pack(system_id, component_id, msg, compassmot_status->throttle, compassmot_status->current, compassmot_status->interference, compassmot_status->CompensationX, compassmot_status->CompensationY, compassmot_status->CompensationZ);
+}
+
+/**
+ * @brief Encode a compassmot_status struct on a channel
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message will be sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param compassmot_status C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_compassmot_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_compassmot_status_t* compassmot_status)
+{
+	return mavlink_msg_compassmot_status_pack_chan(system_id, component_id, chan, msg, compassmot_status->throttle, compassmot_status->current, compassmot_status->interference, compassmot_status->CompensationX, compassmot_status->CompensationY, compassmot_status->CompensationZ);
+}
+
+/**
+ * @brief Send a compassmot_status message
+ * @param chan MAVLink channel to send the message
+ *
+ * @param throttle throttle (percent*10)
+ * @param current current (amps)
+ * @param interference interference (percent)
+ * @param CompensationX Motor Compensation X
+ * @param CompensationY Motor Compensation Y
+ * @param CompensationZ Motor Compensation Z
+ */
+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
+
+static inline void mavlink_msg_compassmot_status_send(mavlink_channel_t chan, uint16_t throttle, float current, uint16_t interference, float CompensationX, float CompensationY, float CompensationZ)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char buf[MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN];
+	_mav_put_float(buf, 0, current);
+	_mav_put_float(buf, 4, CompensationX);
+	_mav_put_float(buf, 8, CompensationY);
+	_mav_put_float(buf, 12, CompensationZ);
+	_mav_put_uint16_t(buf, 16, throttle);
+	_mav_put_uint16_t(buf, 18, interference);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, buf, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN, MAVLINK_MSG_ID_COMPASSMOT_STATUS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, buf, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN);
+#endif
+#else
+	mavlink_compassmot_status_t packet;
+	packet.current = current;
+	packet.CompensationX = CompensationX;
+	packet.CompensationY = CompensationY;
+	packet.CompensationZ = CompensationZ;
+	packet.throttle = throttle;
+	packet.interference = interference;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, (const char *)&packet, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN, MAVLINK_MSG_ID_COMPASSMOT_STATUS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, (const char *)&packet, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN);
+#endif
+#endif
+}
+
+#if MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_compassmot_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint16_t throttle, float current, uint16_t interference, float CompensationX, float CompensationY, float CompensationZ)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, current);
+	_mav_put_float(buf, 4, CompensationX);
+	_mav_put_float(buf, 8, CompensationY);
+	_mav_put_float(buf, 12, CompensationZ);
+	_mav_put_uint16_t(buf, 16, throttle);
+	_mav_put_uint16_t(buf, 18, interference);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, buf, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN, MAVLINK_MSG_ID_COMPASSMOT_STATUS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, buf, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN);
+#endif
+#else
+	mavlink_compassmot_status_t *packet = (mavlink_compassmot_status_t *)msgbuf;
+	packet->current = current;
+	packet->CompensationX = CompensationX;
+	packet->CompensationY = CompensationY;
+	packet->CompensationZ = CompensationZ;
+	packet->throttle = throttle;
+	packet->interference = interference;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, (const char *)packet, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN, MAVLINK_MSG_ID_COMPASSMOT_STATUS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, (const char *)packet, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN);
+#endif
+#endif
+}
+#endif
+
+#endif
+
+// MESSAGE COMPASSMOT_STATUS UNPACKING
+
+
+/**
+ * @brief Get field throttle from compassmot_status message
+ *
+ * @return throttle (percent*10)
+ */
+static inline uint16_t mavlink_msg_compassmot_status_get_throttle(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint16_t(msg,  16);
+}
+
+/**
+ * @brief Get field current from compassmot_status message
+ *
+ * @return current (amps)
+ */
+static inline float mavlink_msg_compassmot_status_get_current(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_float(msg,  0);
+}
+
+/**
+ * @brief Get field interference from compassmot_status message
+ *
+ * @return interference (percent)
+ */
+static inline uint16_t mavlink_msg_compassmot_status_get_interference(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint16_t(msg,  18);
+}
+
+/**
+ * @brief Get field CompensationX from compassmot_status message
+ *
+ * @return Motor Compensation X
+ */
+static inline float mavlink_msg_compassmot_status_get_CompensationX(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_float(msg,  4);
+}
+
+/**
+ * @brief Get field CompensationY from compassmot_status message
+ *
+ * @return Motor Compensation Y
+ */
+static inline float mavlink_msg_compassmot_status_get_CompensationY(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_float(msg,  8);
+}
+
+/**
+ * @brief Get field CompensationZ from compassmot_status message
+ *
+ * @return Motor Compensation Z
+ */
+static inline float mavlink_msg_compassmot_status_get_CompensationZ(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_float(msg,  12);
+}
+
+/**
+ * @brief Decode a compassmot_status message into a struct
+ *
+ * @param msg The message to decode
+ * @param compassmot_status C-struct to decode the message contents into
+ */
+static inline void mavlink_msg_compassmot_status_decode(const mavlink_message_t* msg, mavlink_compassmot_status_t* compassmot_status)
+{
+#if MAVLINK_NEED_BYTE_SWAP
+	compassmot_status->current = mavlink_msg_compassmot_status_get_current(msg);
+	compassmot_status->CompensationX = mavlink_msg_compassmot_status_get_CompensationX(msg);
+	compassmot_status->CompensationY = mavlink_msg_compassmot_status_get_CompensationY(msg);
+	compassmot_status->CompensationZ = mavlink_msg_compassmot_status_get_CompensationZ(msg);
+	compassmot_status->throttle = mavlink_msg_compassmot_status_get_throttle(msg);
+	compassmot_status->interference = mavlink_msg_compassmot_status_get_interference(msg);
+#else
+	memcpy(compassmot_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN);
+#endif
+}
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data16.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data16.h
index 9200eefa0d..7bad1b23de 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data16.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data16.h
@@ -162,6 +162,40 @@ static inline void mavlink_msg_data16_send(mavlink_channel_t chan, uint8_t type,
 #endif
 }
 
+#if MAVLINK_MSG_ID_DATA16_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_data16_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t type, uint8_t len, const uint8_t *data)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint8_t(buf, 0, type);
+	_mav_put_uint8_t(buf, 1, len);
+	_mav_put_uint8_t_array(buf, 2, data, 16);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, buf, MAVLINK_MSG_ID_DATA16_LEN, MAVLINK_MSG_ID_DATA16_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, buf, MAVLINK_MSG_ID_DATA16_LEN);
+#endif
+#else
+	mavlink_data16_t *packet = (mavlink_data16_t *)msgbuf;
+	packet->type = type;
+	packet->len = len;
+	mav_array_memcpy(packet->data, data, sizeof(uint8_t)*16);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, (const char *)packet, MAVLINK_MSG_ID_DATA16_LEN, MAVLINK_MSG_ID_DATA16_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, (const char *)packet, MAVLINK_MSG_ID_DATA16_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE DATA16 UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h
index 3afedb7874..df7a2ec801 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h
@@ -162,6 +162,40 @@ static inline void mavlink_msg_data32_send(mavlink_channel_t chan, uint8_t type,
 #endif
 }
 
+#if MAVLINK_MSG_ID_DATA32_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_data32_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t type, uint8_t len, const uint8_t *data)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint8_t(buf, 0, type);
+	_mav_put_uint8_t(buf, 1, len);
+	_mav_put_uint8_t_array(buf, 2, data, 32);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, buf, MAVLINK_MSG_ID_DATA32_LEN, MAVLINK_MSG_ID_DATA32_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, buf, MAVLINK_MSG_ID_DATA32_LEN);
+#endif
+#else
+	mavlink_data32_t *packet = (mavlink_data32_t *)msgbuf;
+	packet->type = type;
+	packet->len = len;
+	mav_array_memcpy(packet->data, data, sizeof(uint8_t)*32);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, (const char *)packet, MAVLINK_MSG_ID_DATA32_LEN, MAVLINK_MSG_ID_DATA32_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, (const char *)packet, MAVLINK_MSG_ID_DATA32_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE DATA32 UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h
index 6931ada167..c354f8908a 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h
@@ -162,6 +162,40 @@ static inline void mavlink_msg_data64_send(mavlink_channel_t chan, uint8_t type,
 #endif
 }
 
+#if MAVLINK_MSG_ID_DATA64_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_data64_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t type, uint8_t len, const uint8_t *data)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint8_t(buf, 0, type);
+	_mav_put_uint8_t(buf, 1, len);
+	_mav_put_uint8_t_array(buf, 2, data, 64);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, buf, MAVLINK_MSG_ID_DATA64_LEN, MAVLINK_MSG_ID_DATA64_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, buf, MAVLINK_MSG_ID_DATA64_LEN);
+#endif
+#else
+	mavlink_data64_t *packet = (mavlink_data64_t *)msgbuf;
+	packet->type = type;
+	packet->len = len;
+	mav_array_memcpy(packet->data, data, sizeof(uint8_t)*64);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, (const char *)packet, MAVLINK_MSG_ID_DATA64_LEN, MAVLINK_MSG_ID_DATA64_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, (const char *)packet, MAVLINK_MSG_ID_DATA64_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE DATA64 UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h
index cffc7d7e7f..634e3f81d5 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h
@@ -162,6 +162,40 @@ static inline void mavlink_msg_data96_send(mavlink_channel_t chan, uint8_t type,
 #endif
 }
 
+#if MAVLINK_MSG_ID_DATA96_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_data96_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t type, uint8_t len, const uint8_t *data)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint8_t(buf, 0, type);
+	_mav_put_uint8_t(buf, 1, len);
+	_mav_put_uint8_t_array(buf, 2, data, 96);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, buf, MAVLINK_MSG_ID_DATA96_LEN, MAVLINK_MSG_ID_DATA96_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, buf, MAVLINK_MSG_ID_DATA96_LEN);
+#endif
+#else
+	mavlink_data96_t *packet = (mavlink_data96_t *)msgbuf;
+	packet->type = type;
+	packet->len = len;
+	mav_array_memcpy(packet->data, data, sizeof(uint8_t)*96);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, (const char *)packet, MAVLINK_MSG_ID_DATA96_LEN, MAVLINK_MSG_ID_DATA96_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, (const char *)packet, MAVLINK_MSG_ID_DATA96_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE DATA96 UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_configure.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_configure.h
index c6518c4199..d5f9e7132c 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_configure.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_configure.h
@@ -256,6 +256,58 @@ static inline void mavlink_msg_digicam_configure_send(mavlink_channel_t chan, ui
 #endif
 }
 
+#if MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_digicam_configure_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component, uint8_t mode, uint16_t shutter_speed, uint8_t aperture, uint8_t iso, uint8_t exposure_type, uint8_t command_id, uint8_t engine_cut_off, uint8_t extra_param, float extra_value)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, extra_value);
+	_mav_put_uint16_t(buf, 4, shutter_speed);
+	_mav_put_uint8_t(buf, 6, target_system);
+	_mav_put_uint8_t(buf, 7, target_component);
+	_mav_put_uint8_t(buf, 8, mode);
+	_mav_put_uint8_t(buf, 9, aperture);
+	_mav_put_uint8_t(buf, 10, iso);
+	_mav_put_uint8_t(buf, 11, exposure_type);
+	_mav_put_uint8_t(buf, 12, command_id);
+	_mav_put_uint8_t(buf, 13, engine_cut_off);
+	_mav_put_uint8_t(buf, 14, extra_param);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, buf, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, buf, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN);
+#endif
+#else
+	mavlink_digicam_configure_t *packet = (mavlink_digicam_configure_t *)msgbuf;
+	packet->extra_value = extra_value;
+	packet->shutter_speed = shutter_speed;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+	packet->mode = mode;
+	packet->aperture = aperture;
+	packet->iso = iso;
+	packet->exposure_type = exposure_type;
+	packet->command_id = command_id;
+	packet->engine_cut_off = engine_cut_off;
+	packet->extra_param = extra_param;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, (const char *)packet, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, (const char *)packet, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE DIGICAM_CONFIGURE UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_control.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_control.h
index bfa5414a39..68484f7b84 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_control.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_control.h
@@ -245,6 +245,56 @@ static inline void mavlink_msg_digicam_control_send(mavlink_channel_t chan, uint
 #endif
 }
 
+#if MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_digicam_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component, uint8_t session, uint8_t zoom_pos, int8_t zoom_step, uint8_t focus_lock, uint8_t shot, uint8_t command_id, uint8_t extra_param, float extra_value)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, extra_value);
+	_mav_put_uint8_t(buf, 4, target_system);
+	_mav_put_uint8_t(buf, 5, target_component);
+	_mav_put_uint8_t(buf, 6, session);
+	_mav_put_uint8_t(buf, 7, zoom_pos);
+	_mav_put_int8_t(buf, 8, zoom_step);
+	_mav_put_uint8_t(buf, 9, focus_lock);
+	_mav_put_uint8_t(buf, 10, shot);
+	_mav_put_uint8_t(buf, 11, command_id);
+	_mav_put_uint8_t(buf, 12, extra_param);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, buf, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN, MAVLINK_MSG_ID_DIGICAM_CONTROL_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, buf, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN);
+#endif
+#else
+	mavlink_digicam_control_t *packet = (mavlink_digicam_control_t *)msgbuf;
+	packet->extra_value = extra_value;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+	packet->session = session;
+	packet->zoom_pos = zoom_pos;
+	packet->zoom_step = zoom_step;
+	packet->focus_lock = focus_lock;
+	packet->shot = shot;
+	packet->command_id = command_id;
+	packet->extra_param = extra_param;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, (const char *)packet, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN, MAVLINK_MSG_ID_DIGICAM_CONTROL_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, (const char *)packet, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE DIGICAM_CONTROL UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_fetch_point.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_fetch_point.h
index fe3677d53d..4021c7a096 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_fetch_point.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_fetch_point.h
@@ -168,6 +168,42 @@ static inline void mavlink_msg_fence_fetch_point_send(mavlink_channel_t chan, ui
 #endif
 }
 
+#if MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_fence_fetch_point_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component, uint8_t idx)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint8_t(buf, 0, target_system);
+	_mav_put_uint8_t(buf, 1, target_component);
+	_mav_put_uint8_t(buf, 2, idx);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, buf, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN, MAVLINK_MSG_ID_FENCE_FETCH_POINT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, buf, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN);
+#endif
+#else
+	mavlink_fence_fetch_point_t *packet = (mavlink_fence_fetch_point_t *)msgbuf;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+	packet->idx = idx;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, (const char *)packet, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN, MAVLINK_MSG_ID_FENCE_FETCH_POINT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, (const char *)packet, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE FENCE_FETCH_POINT UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_point.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_point.h
index febda6cdc5..6fc2c83fc4 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_point.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_point.h
@@ -201,6 +201,48 @@ static inline void mavlink_msg_fence_point_send(mavlink_channel_t chan, uint8_t
 #endif
 }
 
+#if MAVLINK_MSG_ID_FENCE_POINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_fence_point_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component, uint8_t idx, uint8_t count, float lat, float lng)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, lat);
+	_mav_put_float(buf, 4, lng);
+	_mav_put_uint8_t(buf, 8, target_system);
+	_mav_put_uint8_t(buf, 9, target_component);
+	_mav_put_uint8_t(buf, 10, idx);
+	_mav_put_uint8_t(buf, 11, count);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, buf, MAVLINK_MSG_ID_FENCE_POINT_LEN, MAVLINK_MSG_ID_FENCE_POINT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, buf, MAVLINK_MSG_ID_FENCE_POINT_LEN);
+#endif
+#else
+	mavlink_fence_point_t *packet = (mavlink_fence_point_t *)msgbuf;
+	packet->lat = lat;
+	packet->lng = lng;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+	packet->idx = idx;
+	packet->count = count;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, (const char *)packet, MAVLINK_MSG_ID_FENCE_POINT_LEN, MAVLINK_MSG_ID_FENCE_POINT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, (const char *)packet, MAVLINK_MSG_ID_FENCE_POINT_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE FENCE_POINT UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_status.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_status.h
index 6120904061..c50d37724e 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_status.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_status.h
@@ -179,6 +179,44 @@ static inline void mavlink_msg_fence_status_send(mavlink_channel_t chan, uint8_t
 #endif
 }
 
+#if MAVLINK_MSG_ID_FENCE_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_fence_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t breach_status, uint16_t breach_count, uint8_t breach_type, uint32_t breach_time)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint32_t(buf, 0, breach_time);
+	_mav_put_uint16_t(buf, 4, breach_count);
+	_mav_put_uint8_t(buf, 6, breach_status);
+	_mav_put_uint8_t(buf, 7, breach_type);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, buf, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, buf, MAVLINK_MSG_ID_FENCE_STATUS_LEN);
+#endif
+#else
+	mavlink_fence_status_t *packet = (mavlink_fence_status_t *)msgbuf;
+	packet->breach_time = breach_time;
+	packet->breach_count = breach_count;
+	packet->breach_status = breach_status;
+	packet->breach_type = breach_type;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, (const char *)packet, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, (const char *)packet, MAVLINK_MSG_ID_FENCE_STATUS_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE FENCE_STATUS UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h
index 2f5dea513a..acf031f62d 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h
@@ -157,6 +157,40 @@ static inline void mavlink_msg_hwstatus_send(mavlink_channel_t chan, uint16_t Vc
 #endif
 }
 
+#if MAVLINK_MSG_ID_HWSTATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_hwstatus_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint16_t Vcc, uint8_t I2Cerr)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint16_t(buf, 0, Vcc);
+	_mav_put_uint8_t(buf, 2, I2Cerr);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, buf, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, buf, MAVLINK_MSG_ID_HWSTATUS_LEN);
+#endif
+#else
+	mavlink_hwstatus_t *packet = (mavlink_hwstatus_t *)msgbuf;
+	packet->Vcc = Vcc;
+	packet->I2Cerr = I2Cerr;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, (const char *)packet, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, (const char *)packet, MAVLINK_MSG_ID_HWSTATUS_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE HWSTATUS UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h
index 34743fd021..5fef937181 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h
@@ -234,6 +234,54 @@ static inline void mavlink_msg_limits_status_send(mavlink_channel_t chan, uint8_
 #endif
 }
 
+#if MAVLINK_MSG_ID_LIMITS_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_limits_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t limits_state, uint32_t last_trigger, uint32_t last_action, uint32_t last_recovery, uint32_t last_clear, uint16_t breach_count, uint8_t mods_enabled, uint8_t mods_required, uint8_t mods_triggered)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint32_t(buf, 0, last_trigger);
+	_mav_put_uint32_t(buf, 4, last_action);
+	_mav_put_uint32_t(buf, 8, last_recovery);
+	_mav_put_uint32_t(buf, 12, last_clear);
+	_mav_put_uint16_t(buf, 16, breach_count);
+	_mav_put_uint8_t(buf, 18, limits_state);
+	_mav_put_uint8_t(buf, 19, mods_enabled);
+	_mav_put_uint8_t(buf, 20, mods_required);
+	_mav_put_uint8_t(buf, 21, mods_triggered);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, buf, MAVLINK_MSG_ID_LIMITS_STATUS_LEN, MAVLINK_MSG_ID_LIMITS_STATUS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, buf, MAVLINK_MSG_ID_LIMITS_STATUS_LEN);
+#endif
+#else
+	mavlink_limits_status_t *packet = (mavlink_limits_status_t *)msgbuf;
+	packet->last_trigger = last_trigger;
+	packet->last_action = last_action;
+	packet->last_recovery = last_recovery;
+	packet->last_clear = last_clear;
+	packet->breach_count = breach_count;
+	packet->limits_state = limits_state;
+	packet->mods_enabled = mods_enabled;
+	packet->mods_required = mods_required;
+	packet->mods_triggered = mods_triggered;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, (const char *)packet, MAVLINK_MSG_ID_LIMITS_STATUS_LEN, MAVLINK_MSG_ID_LIMITS_STATUS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, (const char *)packet, MAVLINK_MSG_ID_LIMITS_STATUS_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE LIMITS_STATUS UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h
index 55f772bbc5..c64b2e9377 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h
@@ -157,6 +157,40 @@ static inline void mavlink_msg_meminfo_send(mavlink_channel_t chan, uint16_t brk
 #endif
 }
 
+#if MAVLINK_MSG_ID_MEMINFO_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_meminfo_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint16_t brkval, uint16_t freemem)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint16_t(buf, 0, brkval);
+	_mav_put_uint16_t(buf, 2, freemem);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, buf, MAVLINK_MSG_ID_MEMINFO_LEN, MAVLINK_MSG_ID_MEMINFO_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, buf, MAVLINK_MSG_ID_MEMINFO_LEN);
+#endif
+#else
+	mavlink_meminfo_t *packet = (mavlink_meminfo_t *)msgbuf;
+	packet->brkval = brkval;
+	packet->freemem = freemem;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, (const char *)packet, MAVLINK_MSG_ID_MEMINFO_LEN, MAVLINK_MSG_ID_MEMINFO_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, (const char *)packet, MAVLINK_MSG_ID_MEMINFO_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE MEMINFO UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_configure.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_configure.h
index de717dfa4d..350bb0b785 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_configure.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_configure.h
@@ -201,6 +201,48 @@ static inline void mavlink_msg_mount_configure_send(mavlink_channel_t chan, uint
 #endif
 }
 
+#if MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_mount_configure_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component, uint8_t mount_mode, uint8_t stab_roll, uint8_t stab_pitch, uint8_t stab_yaw)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint8_t(buf, 0, target_system);
+	_mav_put_uint8_t(buf, 1, target_component);
+	_mav_put_uint8_t(buf, 2, mount_mode);
+	_mav_put_uint8_t(buf, 3, stab_roll);
+	_mav_put_uint8_t(buf, 4, stab_pitch);
+	_mav_put_uint8_t(buf, 5, stab_yaw);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, buf, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN, MAVLINK_MSG_ID_MOUNT_CONFIGURE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, buf, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN);
+#endif
+#else
+	mavlink_mount_configure_t *packet = (mavlink_mount_configure_t *)msgbuf;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+	packet->mount_mode = mount_mode;
+	packet->stab_roll = stab_roll;
+	packet->stab_pitch = stab_pitch;
+	packet->stab_yaw = stab_yaw;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, (const char *)packet, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN, MAVLINK_MSG_ID_MOUNT_CONFIGURE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, (const char *)packet, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE MOUNT_CONFIGURE UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_control.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_control.h
index 44416353ed..72d6e24197 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_control.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_control.h
@@ -201,6 +201,48 @@ static inline void mavlink_msg_mount_control_send(mavlink_channel_t chan, uint8_
 #endif
 }
 
+#if MAVLINK_MSG_ID_MOUNT_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_mount_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component, int32_t input_a, int32_t input_b, int32_t input_c, uint8_t save_position)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_int32_t(buf, 0, input_a);
+	_mav_put_int32_t(buf, 4, input_b);
+	_mav_put_int32_t(buf, 8, input_c);
+	_mav_put_uint8_t(buf, 12, target_system);
+	_mav_put_uint8_t(buf, 13, target_component);
+	_mav_put_uint8_t(buf, 14, save_position);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, buf, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN, MAVLINK_MSG_ID_MOUNT_CONTROL_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, buf, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN);
+#endif
+#else
+	mavlink_mount_control_t *packet = (mavlink_mount_control_t *)msgbuf;
+	packet->input_a = input_a;
+	packet->input_b = input_b;
+	packet->input_c = input_c;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+	packet->save_position = save_position;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, (const char *)packet, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN, MAVLINK_MSG_ID_MOUNT_CONTROL_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, (const char *)packet, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE MOUNT_CONTROL UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_status.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_status.h
index 4905905dc5..c140dd2c7d 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_status.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_status.h
@@ -190,6 +190,46 @@ static inline void mavlink_msg_mount_status_send(mavlink_channel_t chan, uint8_t
 #endif
 }
 
+#if MAVLINK_MSG_ID_MOUNT_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_mount_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component, int32_t pointing_a, int32_t pointing_b, int32_t pointing_c)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_int32_t(buf, 0, pointing_a);
+	_mav_put_int32_t(buf, 4, pointing_b);
+	_mav_put_int32_t(buf, 8, pointing_c);
+	_mav_put_uint8_t(buf, 12, target_system);
+	_mav_put_uint8_t(buf, 13, target_component);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, buf, MAVLINK_MSG_ID_MOUNT_STATUS_LEN, MAVLINK_MSG_ID_MOUNT_STATUS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, buf, MAVLINK_MSG_ID_MOUNT_STATUS_LEN);
+#endif
+#else
+	mavlink_mount_status_t *packet = (mavlink_mount_status_t *)msgbuf;
+	packet->pointing_a = pointing_a;
+	packet->pointing_b = pointing_b;
+	packet->pointing_c = pointing_c;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, (const char *)packet, MAVLINK_MSG_ID_MOUNT_STATUS_LEN, MAVLINK_MSG_ID_MOUNT_STATUS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, (const char *)packet, MAVLINK_MSG_ID_MOUNT_STATUS_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE MOUNT_STATUS UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h
index 8e9740e828..830eb4639a 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h
@@ -212,6 +212,50 @@ static inline void mavlink_msg_radio_send(mavlink_channel_t chan, uint8_t rssi,
 #endif
 }
 
+#if MAVLINK_MSG_ID_RADIO_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_radio_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t rssi, uint8_t remrssi, uint8_t txbuf, uint8_t noise, uint8_t remnoise, uint16_t rxerrors, uint16_t fixed)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint16_t(buf, 0, rxerrors);
+	_mav_put_uint16_t(buf, 2, fixed);
+	_mav_put_uint8_t(buf, 4, rssi);
+	_mav_put_uint8_t(buf, 5, remrssi);
+	_mav_put_uint8_t(buf, 6, txbuf);
+	_mav_put_uint8_t(buf, 7, noise);
+	_mav_put_uint8_t(buf, 8, remnoise);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, buf, MAVLINK_MSG_ID_RADIO_LEN, MAVLINK_MSG_ID_RADIO_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, buf, MAVLINK_MSG_ID_RADIO_LEN);
+#endif
+#else
+	mavlink_radio_t *packet = (mavlink_radio_t *)msgbuf;
+	packet->rxerrors = rxerrors;
+	packet->fixed = fixed;
+	packet->rssi = rssi;
+	packet->remrssi = remrssi;
+	packet->txbuf = txbuf;
+	packet->noise = noise;
+	packet->remnoise = remnoise;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, (const char *)packet, MAVLINK_MSG_ID_RADIO_LEN, MAVLINK_MSG_ID_RADIO_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, (const char *)packet, MAVLINK_MSG_ID_RADIO_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE RADIO UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_fetch_point.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_fetch_point.h
index f057e3c335..4598251fe1 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_fetch_point.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_fetch_point.h
@@ -168,6 +168,42 @@ static inline void mavlink_msg_rally_fetch_point_send(mavlink_channel_t chan, ui
 #endif
 }
 
+#if MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_rally_fetch_point_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component, uint8_t idx)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint8_t(buf, 0, target_system);
+	_mav_put_uint8_t(buf, 1, target_component);
+	_mav_put_uint8_t(buf, 2, idx);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, buf, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN, MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, buf, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN);
+#endif
+#else
+	mavlink_rally_fetch_point_t *packet = (mavlink_rally_fetch_point_t *)msgbuf;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+	packet->idx = idx;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, (const char *)packet, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN, MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, (const char *)packet, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE RALLY_FETCH_POINT UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_point.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_point.h
index 2c21db4c0e..b44e764a94 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_point.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_point.h
@@ -245,6 +245,56 @@ static inline void mavlink_msg_rally_point_send(mavlink_channel_t chan, uint8_t
 #endif
 }
 
+#if MAVLINK_MSG_ID_RALLY_POINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_rally_point_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component, uint8_t idx, uint8_t count, int32_t lat, int32_t lng, int16_t alt, int16_t break_alt, uint16_t land_dir, uint8_t flags)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_int32_t(buf, 0, lat);
+	_mav_put_int32_t(buf, 4, lng);
+	_mav_put_int16_t(buf, 8, alt);
+	_mav_put_int16_t(buf, 10, break_alt);
+	_mav_put_uint16_t(buf, 12, land_dir);
+	_mav_put_uint8_t(buf, 14, target_system);
+	_mav_put_uint8_t(buf, 15, target_component);
+	_mav_put_uint8_t(buf, 16, idx);
+	_mav_put_uint8_t(buf, 17, count);
+	_mav_put_uint8_t(buf, 18, flags);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, buf, MAVLINK_MSG_ID_RALLY_POINT_LEN, MAVLINK_MSG_ID_RALLY_POINT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, buf, MAVLINK_MSG_ID_RALLY_POINT_LEN);
+#endif
+#else
+	mavlink_rally_point_t *packet = (mavlink_rally_point_t *)msgbuf;
+	packet->lat = lat;
+	packet->lng = lng;
+	packet->alt = alt;
+	packet->break_alt = break_alt;
+	packet->land_dir = land_dir;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+	packet->idx = idx;
+	packet->count = count;
+	packet->flags = flags;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, (const char *)packet, MAVLINK_MSG_ID_RALLY_POINT_LEN, MAVLINK_MSG_ID_RALLY_POINT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, (const char *)packet, MAVLINK_MSG_ID_RALLY_POINT_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE RALLY_POINT UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rangefinder.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rangefinder.h
index c476447a87..464ce8a47f 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rangefinder.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rangefinder.h
@@ -157,6 +157,40 @@ static inline void mavlink_msg_rangefinder_send(mavlink_channel_t chan, float di
 #endif
 }
 
+#if MAVLINK_MSG_ID_RANGEFINDER_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_rangefinder_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  float distance, float voltage)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, distance);
+	_mav_put_float(buf, 4, voltage);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, buf, MAVLINK_MSG_ID_RANGEFINDER_LEN, MAVLINK_MSG_ID_RANGEFINDER_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, buf, MAVLINK_MSG_ID_RANGEFINDER_LEN);
+#endif
+#else
+	mavlink_rangefinder_t *packet = (mavlink_rangefinder_t *)msgbuf;
+	packet->distance = distance;
+	packet->voltage = voltage;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, (const char *)packet, MAVLINK_MSG_ID_RANGEFINDER_LEN, MAVLINK_MSG_ID_RANGEFINDER_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, (const char *)packet, MAVLINK_MSG_ID_RANGEFINDER_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE RANGEFINDER UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_sensor_offsets.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_sensor_offsets.h
index 31b7d989de..d18f31c954 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_sensor_offsets.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_sensor_offsets.h
@@ -267,6 +267,60 @@ static inline void mavlink_msg_sensor_offsets_send(mavlink_channel_t chan, int16
 #endif
 }
 
+#if MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_sensor_offsets_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z, float mag_declination, int32_t raw_press, int32_t raw_temp, float gyro_cal_x, float gyro_cal_y, float gyro_cal_z, float accel_cal_x, float accel_cal_y, float accel_cal_z)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, mag_declination);
+	_mav_put_int32_t(buf, 4, raw_press);
+	_mav_put_int32_t(buf, 8, raw_temp);
+	_mav_put_float(buf, 12, gyro_cal_x);
+	_mav_put_float(buf, 16, gyro_cal_y);
+	_mav_put_float(buf, 20, gyro_cal_z);
+	_mav_put_float(buf, 24, accel_cal_x);
+	_mav_put_float(buf, 28, accel_cal_y);
+	_mav_put_float(buf, 32, accel_cal_z);
+	_mav_put_int16_t(buf, 36, mag_ofs_x);
+	_mav_put_int16_t(buf, 38, mag_ofs_y);
+	_mav_put_int16_t(buf, 40, mag_ofs_z);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, buf, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN, MAVLINK_MSG_ID_SENSOR_OFFSETS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, buf, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN);
+#endif
+#else
+	mavlink_sensor_offsets_t *packet = (mavlink_sensor_offsets_t *)msgbuf;
+	packet->mag_declination = mag_declination;
+	packet->raw_press = raw_press;
+	packet->raw_temp = raw_temp;
+	packet->gyro_cal_x = gyro_cal_x;
+	packet->gyro_cal_y = gyro_cal_y;
+	packet->gyro_cal_z = gyro_cal_z;
+	packet->accel_cal_x = accel_cal_x;
+	packet->accel_cal_y = accel_cal_y;
+	packet->accel_cal_z = accel_cal_z;
+	packet->mag_ofs_x = mag_ofs_x;
+	packet->mag_ofs_y = mag_ofs_y;
+	packet->mag_ofs_z = mag_ofs_z;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, (const char *)packet, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN, MAVLINK_MSG_ID_SENSOR_OFFSETS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, (const char *)packet, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SENSOR_OFFSETS UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_set_mag_offsets.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_set_mag_offsets.h
index b2c629c39a..fc6aa71e06 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_set_mag_offsets.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_set_mag_offsets.h
@@ -190,6 +190,46 @@ static inline void mavlink_msg_set_mag_offsets_send(mavlink_channel_t chan, uint
 #endif
 }
 
+#if MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_set_mag_offsets_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_int16_t(buf, 0, mag_ofs_x);
+	_mav_put_int16_t(buf, 2, mag_ofs_y);
+	_mav_put_int16_t(buf, 4, mag_ofs_z);
+	_mav_put_uint8_t(buf, 6, target_system);
+	_mav_put_uint8_t(buf, 7, target_component);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, buf, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN, MAVLINK_MSG_ID_SET_MAG_OFFSETS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, buf, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN);
+#endif
+#else
+	mavlink_set_mag_offsets_t *packet = (mavlink_set_mag_offsets_t *)msgbuf;
+	packet->mag_ofs_x = mag_ofs_x;
+	packet->mag_ofs_y = mag_ofs_y;
+	packet->mag_ofs_z = mag_ofs_z;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, (const char *)packet, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN, MAVLINK_MSG_ID_SET_MAG_OFFSETS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, (const char *)packet, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SET_MAG_OFFSETS UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h
index 8ef44f76d5..48cfb6ffd5 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h
@@ -256,6 +256,58 @@ static inline void mavlink_msg_simstate_send(mavlink_channel_t chan, float roll,
 #endif
 }
 
+#if MAVLINK_MSG_ID_SIMSTATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_simstate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, int32_t lat, int32_t lng)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, roll);
+	_mav_put_float(buf, 4, pitch);
+	_mav_put_float(buf, 8, yaw);
+	_mav_put_float(buf, 12, xacc);
+	_mav_put_float(buf, 16, yacc);
+	_mav_put_float(buf, 20, zacc);
+	_mav_put_float(buf, 24, xgyro);
+	_mav_put_float(buf, 28, ygyro);
+	_mav_put_float(buf, 32, zgyro);
+	_mav_put_int32_t(buf, 36, lat);
+	_mav_put_int32_t(buf, 40, lng);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, buf, MAVLINK_MSG_ID_SIMSTATE_LEN, MAVLINK_MSG_ID_SIMSTATE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, buf, MAVLINK_MSG_ID_SIMSTATE_LEN);
+#endif
+#else
+	mavlink_simstate_t *packet = (mavlink_simstate_t *)msgbuf;
+	packet->roll = roll;
+	packet->pitch = pitch;
+	packet->yaw = yaw;
+	packet->xacc = xacc;
+	packet->yacc = yacc;
+	packet->zacc = zacc;
+	packet->xgyro = xgyro;
+	packet->ygyro = ygyro;
+	packet->zgyro = zgyro;
+	packet->lat = lat;
+	packet->lng = lng;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, (const char *)packet, MAVLINK_MSG_ID_SIMSTATE_LEN, MAVLINK_MSG_ID_SIMSTATE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, (const char *)packet, MAVLINK_MSG_ID_SIMSTATE_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SIMSTATE UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h
index 7608a7bd10..5d5edc4cc3 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h
@@ -168,6 +168,42 @@ static inline void mavlink_msg_wind_send(mavlink_channel_t chan, float direction
 #endif
 }
 
+#if MAVLINK_MSG_ID_WIND_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_wind_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  float direction, float speed, float speed_z)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, direction);
+	_mav_put_float(buf, 4, speed);
+	_mav_put_float(buf, 8, speed_z);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, buf, MAVLINK_MSG_ID_WIND_LEN, MAVLINK_MSG_ID_WIND_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, buf, MAVLINK_MSG_ID_WIND_LEN);
+#endif
+#else
+	mavlink_wind_t *packet = (mavlink_wind_t *)msgbuf;
+	packet->direction = direction;
+	packet->speed = speed;
+	packet->speed_z = speed_z;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, (const char *)packet, MAVLINK_MSG_ID_WIND_LEN, MAVLINK_MSG_ID_WIND_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, (const char *)packet, MAVLINK_MSG_ID_WIND_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE WIND UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h b/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h
index b2496334f7..489553ea4d 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h
@@ -1398,6 +1398,59 @@ static void mavlink_test_rally_fetch_point(uint8_t system_id, uint8_t component_
         MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
 }
 
+static void mavlink_test_compassmot_status(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
+{
+	mavlink_message_t msg;
+        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
+        uint16_t i;
+	mavlink_compassmot_status_t packet_in = {
+		17.0,
+	}45.0,
+	}73.0,
+	}101.0,
+	}18067,
+	}18171,
+	};
+	mavlink_compassmot_status_t packet1, packet2;
+        memset(&packet1, 0, sizeof(packet1));
+        	packet1.current = packet_in.current;
+        	packet1.CompensationX = packet_in.CompensationX;
+        	packet1.CompensationY = packet_in.CompensationY;
+        	packet1.CompensationZ = packet_in.CompensationZ;
+        	packet1.throttle = packet_in.throttle;
+        	packet1.interference = packet_in.interference;
+        
+        
+
+        memset(&packet2, 0, sizeof(packet2));
+	mavlink_msg_compassmot_status_encode(system_id, component_id, &msg, &packet1);
+	mavlink_msg_compassmot_status_decode(&msg, &packet2);
+        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+        memset(&packet2, 0, sizeof(packet2));
+	mavlink_msg_compassmot_status_pack(system_id, component_id, &msg , packet1.throttle , packet1.current , packet1.interference , packet1.CompensationX , packet1.CompensationY , packet1.CompensationZ );
+	mavlink_msg_compassmot_status_decode(&msg, &packet2);
+        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+        memset(&packet2, 0, sizeof(packet2));
+	mavlink_msg_compassmot_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.throttle , packet1.current , packet1.interference , packet1.CompensationX , packet1.CompensationY , packet1.CompensationZ );
+	mavlink_msg_compassmot_status_decode(&msg, &packet2);
+        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+        memset(&packet2, 0, sizeof(packet2));
+        mavlink_msg_to_send_buffer(buffer, &msg);
+        for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
+        	comm_send_ch(MAVLINK_COMM_0, buffer[i]);
+        }
+	mavlink_msg_compassmot_status_decode(last_msg, &packet2);
+        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+        
+        memset(&packet2, 0, sizeof(packet2));
+	mavlink_msg_compassmot_status_send(MAVLINK_COMM_1 , packet1.throttle , packet1.current , packet1.interference , packet1.CompensationX , packet1.CompensationY , packet1.CompensationZ );
+	mavlink_msg_compassmot_status_decode(last_msg, &packet2);
+        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+}
+
 static void mavlink_test_ahrs2(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
 {
 	mavlink_message_t msg;
@@ -1479,6 +1532,7 @@ static void mavlink_test_ardupilotmega(uint8_t system_id, uint8_t component_id,
 	mavlink_test_airspeed_autocal(system_id, component_id, last_msg);
 	mavlink_test_rally_point(system_id, component_id, last_msg);
 	mavlink_test_rally_fetch_point(system_id, component_id, last_msg);
+	mavlink_test_compassmot_status(system_id, component_id, last_msg);
 	mavlink_test_ahrs2(system_id, component_id, last_msg);
 }
 
diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/version.h b/mavlink/include/mavlink/v1.0/ardupilotmega/version.h
index 983237eb5a..1892fffb31 100644
--- a/mavlink/include/mavlink/v1.0/ardupilotmega/version.h
+++ b/mavlink/include/mavlink/v1.0/ardupilotmega/version.h
@@ -5,7 +5,7 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Tue Feb  4 15:27:33 2014"
+#define MAVLINK_BUILD_DATE "Sun Apr 13 09:37:48 2014"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
 #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
  
diff --git a/mavlink/include/mavlink/v1.0/autoquad/autoquad.h b/mavlink/include/mavlink/v1.0/autoquad/autoquad.h
index b4dde669fd..93e868dc35 100644
--- a/mavlink/include/mavlink/v1.0/autoquad/autoquad.h
+++ b/mavlink/include/mavlink/v1.0/autoquad/autoquad.h
@@ -12,15 +12,15 @@ extern "C" {
 // MESSAGE LENGTHS AND CRCS
 
 #ifndef MAVLINK_MESSAGE_LENGTHS
-#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 0, 0, 0, 0, 0, 13, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 82, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0}
+#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 42, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 0, 0, 0, 13, 255, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 82, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0}
 #endif
 
 #ifndef MAVLINK_MESSAGE_CRCS
-#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 0, 0, 0, 0, 0, 29, 223, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 241, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0}
+#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 85, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 241, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0}
 #endif
 
 #ifndef MAVLINK_MESSAGE_INFO
-#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, MAVLINK_MESSAGE_INFO_AQ_TELEMETRY_F, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}}
+#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, MAVLINK_MESSAGE_INFO_AQ_TELEMETRY_F, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}}
 #endif
 
 #include "../protocol.h"
@@ -71,6 +71,7 @@ enum MAV_CMD
 	MAV_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude|  */
 	MAV_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|  */
 	MAV_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal|  */
+	MAV_CMD_NAV_SPLINE_WAYPOINT=82, /* Navigate to MISSION using a spline path. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Empty| Empty| Empty| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal|  */
 	MAV_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
 	MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty|  */
 	MAV_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate.  Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude|  */
@@ -88,8 +89,16 @@ enum MAV_CMD
 	MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty|  */
 	MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty|  */
 	MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|  */
+	MAV_CMD_DO_DIGICAM_CONFIGURE=202, /* Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)|  */
+	MAV_CMD_DO_DIGICAM_CONTROL=203, /* Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty|  */
+	MAV_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty|  */
+	MAV_CMD_DO_MOUNT_CONTROL=205, /* Mission command to control a camera or antenna mount |pitch or lat in degrees, depending on mount mode.| roll or lon in degrees depending on mount mode| yaw or alt (in meters) depending on mount mode| reserved| reserved| reserved| MAV_MOUNT_MODE enum value|  */
+	MAV_CMD_DO_SET_CAM_TRIGG_DIST=206, /* Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty|  */
+	MAV_CMD_DO_FENCE_ENABLE=207, /* Mission command to enable the geofence |enable? (0=disable, 1=enable)| Empty| Empty| Empty| Empty| Empty| Empty|  */
+	MAV_CMD_DO_PARACHUTE=208, /* Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty|  */
+	MAV_CMD_DO_MOUNT_CONTROL_QUAT=220, /* Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1| q2 - quaternion param #2| q3 - quaternion param #3| q4 - quaternion param #4| Empty| Empty| Empty|  */
 	MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty|  */
+	MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Compass/Motor interference calibration: 0: no, 1: yes| Empty|  */
 	MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units|  */
 	MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty|  */
 	MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty|  */
diff --git a/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h b/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h
index ed7c86bcb8..e78d5687aa 100644
--- a/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h
+++ b/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h
@@ -366,6 +366,78 @@ static inline void mavlink_msg_aq_telemetry_f_send(mavlink_channel_t chan, uint1
 #endif
 }
 
+#if MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_aq_telemetry_f_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint16_t Index, float value1, float value2, float value3, float value4, float value5, float value6, float value7, float value8, float value9, float value10, float value11, float value12, float value13, float value14, float value15, float value16, float value17, float value18, float value19, float value20)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, value1);
+	_mav_put_float(buf, 4, value2);
+	_mav_put_float(buf, 8, value3);
+	_mav_put_float(buf, 12, value4);
+	_mav_put_float(buf, 16, value5);
+	_mav_put_float(buf, 20, value6);
+	_mav_put_float(buf, 24, value7);
+	_mav_put_float(buf, 28, value8);
+	_mav_put_float(buf, 32, value9);
+	_mav_put_float(buf, 36, value10);
+	_mav_put_float(buf, 40, value11);
+	_mav_put_float(buf, 44, value12);
+	_mav_put_float(buf, 48, value13);
+	_mav_put_float(buf, 52, value14);
+	_mav_put_float(buf, 56, value15);
+	_mav_put_float(buf, 60, value16);
+	_mav_put_float(buf, 64, value17);
+	_mav_put_float(buf, 68, value18);
+	_mav_put_float(buf, 72, value19);
+	_mav_put_float(buf, 76, value20);
+	_mav_put_uint16_t(buf, 80, Index);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F, buf, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN, MAVLINK_MSG_ID_AQ_TELEMETRY_F_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F, buf, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN);
+#endif
+#else
+	mavlink_aq_telemetry_f_t *packet = (mavlink_aq_telemetry_f_t *)msgbuf;
+	packet->value1 = value1;
+	packet->value2 = value2;
+	packet->value3 = value3;
+	packet->value4 = value4;
+	packet->value5 = value5;
+	packet->value6 = value6;
+	packet->value7 = value7;
+	packet->value8 = value8;
+	packet->value9 = value9;
+	packet->value10 = value10;
+	packet->value11 = value11;
+	packet->value12 = value12;
+	packet->value13 = value13;
+	packet->value14 = value14;
+	packet->value15 = value15;
+	packet->value16 = value16;
+	packet->value17 = value17;
+	packet->value18 = value18;
+	packet->value19 = value19;
+	packet->value20 = value20;
+	packet->Index = Index;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F, (const char *)packet, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN, MAVLINK_MSG_ID_AQ_TELEMETRY_F_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F, (const char *)packet, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE AQ_TELEMETRY_F UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/autoquad/version.h b/mavlink/include/mavlink/v1.0/autoquad/version.h
index d125e92b1a..5d2f33b601 100644
--- a/mavlink/include/mavlink/v1.0/autoquad/version.h
+++ b/mavlink/include/mavlink/v1.0/autoquad/version.h
@@ -5,7 +5,7 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Tue Feb  4 15:27:43 2014"
+#define MAVLINK_BUILD_DATE "Sun Apr 13 09:38:19 2014"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
 #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
  
diff --git a/mavlink/include/mavlink/v1.0/common/common.h b/mavlink/include/mavlink/v1.0/common/common.h
index 7e1cf765bf..020211d406 100644
--- a/mavlink/include/mavlink/v1.0/common/common.h
+++ b/mavlink/include/mavlink/v1.0/common/common.h
@@ -12,15 +12,15 @@ extern "C" {
 // MESSAGE LENGTHS AND CRCS
 
 #ifndef MAVLINK_MESSAGE_LENGTHS
-#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 0, 0, 0, 0, 0, 13, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0}
+#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 42, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 0, 0, 0, 13, 255, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0}
 #endif
 
 #ifndef MAVLINK_MESSAGE_CRCS
-#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 0, 0, 0, 0, 0, 29, 223, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0}
+#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 85, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0}
 #endif
 
 #ifndef MAVLINK_MESSAGE_INFO
-#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}}
+#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}}
 #endif
 
 #include "../protocol.h"
@@ -79,7 +79,8 @@ enum MAV_TYPE
 	MAV_TYPE_TRICOPTER=15, /* Octorotor | */
 	MAV_TYPE_FLAPPING_WING=16, /* Flapping wing | */
 	MAV_TYPE_KITE=17, /* Flapping wing | */
-	MAV_TYPE_ENUM_END=18, /*  | */
+	MAV_TYPE_ONBOARD_CONTROLLER=18, /* Onboard companion controller | */
+	MAV_TYPE_ENUM_END=19, /*  | */
 };
 #endif
 
@@ -228,7 +229,8 @@ enum MAV_SYS_STATUS_SENSOR
 	MAV_SYS_STATUS_SENSOR_3D_GYRO2=131072, /* 0x20000 2nd 3D gyro | */
 	MAV_SYS_STATUS_SENSOR_3D_ACCEL2=262144, /* 0x40000 2nd 3D accelerometer | */
 	MAV_SYS_STATUS_SENSOR_3D_MAG2=524288, /* 0x80000 2nd 3D magnetometer | */
-	MAV_SYS_STATUS_SENSOR_ENUM_END=524289, /*  | */
+	MAV_SYS_STATUS_GEOFENCE=1048576, /* 0x100000 geofence | */
+	MAV_SYS_STATUS_SENSOR_ENUM_END=1048577, /*  | */
 };
 #endif
 
@@ -242,7 +244,9 @@ enum MAV_FRAME
 	MAV_FRAME_MISSION=2, /* NOT a coordinate frame, indicates a mission command. | */
 	MAV_FRAME_GLOBAL_RELATIVE_ALT=3, /* Global coordinate frame, WGS84 coordinate system, relative altitude over ground with respect to the home position. First value / x: latitude, second value / y: longitude, third value / z: positive altitude with 0 being at the altitude of the home location. | */
 	MAV_FRAME_LOCAL_ENU=4, /* Local coordinate frame, Z-down (x: east, y: north, z: up) | */
-	MAV_FRAME_ENUM_END=5, /*  | */
+	MAV_FRAME_GLOBAL_INT=5, /* Global coordinate frame with some fields as scaled integers, WGS84 coordinate system. First value / x: latitude, second value / y: longitude, third value / z: positive altitude over mean sea level (MSL). Lat / Lon are scaled * 1E7 to avoid floating point accuracy limitations. | */
+	MAV_FRAME_GLOBAL_RELATIVE_ALT_INT=6, /* Global coordinate frame with some fields as scaled integers, WGS84 coordinate system, relative altitude over ground with respect to the home position. First value / x: latitude, second value / y: longitude, third value / z: positive altitude with 0 being at the altitude of the home location. Lat / Lon are scaled * 1E7 to avoid floating point accuracy limitations. | */
+	MAV_FRAME_ENUM_END=7, /*  | */
 };
 #endif
 
@@ -261,6 +265,57 @@ enum MAVLINK_DATA_STREAM_TYPE
 };
 #endif
 
+/** @brief  */
+#ifndef HAVE_ENUM_FENCE_ACTION
+#define HAVE_ENUM_FENCE_ACTION
+enum FENCE_ACTION
+{
+	FENCE_ACTION_NONE=0, /* Disable fenced mode | */
+	FENCE_ACTION_GUIDED=1, /* Switched to guided mode to return point (fence point 0) | */
+	FENCE_ACTION_REPORT=2, /* Report fence breach, but don't take action | */
+	FENCE_ACTION_GUIDED_THR_PASS=3, /* Switched to guided mode to return point (fence point 0) with manual throttle control | */
+	FENCE_ACTION_ENUM_END=4, /*  | */
+};
+#endif
+
+/** @brief  */
+#ifndef HAVE_ENUM_FENCE_BREACH
+#define HAVE_ENUM_FENCE_BREACH
+enum FENCE_BREACH
+{
+	FENCE_BREACH_NONE=0, /* No last fence breach | */
+	FENCE_BREACH_MINALT=1, /* Breached minimum altitude | */
+	FENCE_BREACH_MAXALT=2, /* Breached maximum altitude | */
+	FENCE_BREACH_BOUNDARY=3, /* Breached fence boundary | */
+	FENCE_BREACH_ENUM_END=4, /*  | */
+};
+#endif
+
+/** @brief Enumeration of possible mount operation modes */
+#ifndef HAVE_ENUM_MAV_MOUNT_MODE
+#define HAVE_ENUM_MAV_MOUNT_MODE
+enum MAV_MOUNT_MODE
+{
+	MAV_MOUNT_MODE_RETRACT=0, /* Load and keep safe position (Roll,Pitch,Yaw) from permant memory and stop stabilization | */
+	MAV_MOUNT_MODE_NEUTRAL=1, /* Load and keep neutral position (Roll,Pitch,Yaw) from permanent memory. | */
+	MAV_MOUNT_MODE_MAVLINK_TARGETING=2, /* Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization | */
+	MAV_MOUNT_MODE_RC_TARGETING=3, /* Load neutral position and start RC Roll,Pitch,Yaw control with stabilization | */
+	MAV_MOUNT_MODE_GPS_POINT=4, /* Load neutral position and start to point to Lat,Lon,Alt | */
+	MAV_MOUNT_MODE_ENUM_END=5, /*  | */
+};
+#endif
+
+/** @brief Enumeration of distance sensor types */
+#ifndef HAVE_ENUM_MAV_DISTANCE_SENSOR
+#define HAVE_ENUM_MAV_DISTANCE_SENSOR
+enum MAV_DISTANCE_SENSOR
+{
+	MAV_DISTANCE_SENSOR_LASER=0, /* Laser altimeter, e.g. LightWare SF02/F or PulsedLight units | */
+	MAV_DISTANCE_SENSOR_ULTRASOUND=1, /* Laser altimeter, e.g. MaxBotix units | */
+	MAV_DISTANCE_SENSOR_ENUM_END=2, /*  | */
+};
+#endif
+
 /** @brief Commands to be executed by the MAV. They can be executed on user request, or as part of a mission script. If the action is used in a mission, the parameter mapping to the waypoint/mission message is as follows: Param 1, Param 2, Param 3, Param 4, X: Param 5, Y:Param 6, Z:Param 7. This command list is similar what ARINC 424 is for commercial aircraft: A data format how to interpret waypoint/mission data. */
 #ifndef HAVE_ENUM_MAV_CMD
 #define HAVE_ENUM_MAV_CMD
@@ -275,6 +330,7 @@ enum MAV_CMD
 	MAV_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude|  */
 	MAV_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|  */
 	MAV_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal|  */
+	MAV_CMD_NAV_SPLINE_WAYPOINT=82, /* Navigate to MISSION using a spline path. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Empty| Empty| Empty| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal|  */
 	MAV_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
 	MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty|  */
 	MAV_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate.  Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude|  */
@@ -292,8 +348,16 @@ enum MAV_CMD
 	MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty|  */
 	MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty|  */
 	MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|  */
+	MAV_CMD_DO_DIGICAM_CONFIGURE=202, /* Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)|  */
+	MAV_CMD_DO_DIGICAM_CONTROL=203, /* Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty|  */
+	MAV_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty|  */
+	MAV_CMD_DO_MOUNT_CONTROL=205, /* Mission command to control a camera or antenna mount |pitch or lat in degrees, depending on mount mode.| roll or lon in degrees depending on mount mode| yaw or alt (in meters) depending on mount mode| reserved| reserved| reserved| MAV_MOUNT_MODE enum value|  */
+	MAV_CMD_DO_SET_CAM_TRIGG_DIST=206, /* Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty|  */
+	MAV_CMD_DO_FENCE_ENABLE=207, /* Mission command to enable the geofence |enable? (0=disable, 1=enable)| Empty| Empty| Empty| Empty| Empty| Empty|  */
+	MAV_CMD_DO_PARACHUTE=208, /* Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty|  */
+	MAV_CMD_DO_MOUNT_CONTROL_QUAT=220, /* Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1| q2 - quaternion param #2| q3 - quaternion param #3| q4 - quaternion param #4| Empty| Empty| Empty|  */
 	MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty|  */
+	MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Compass/Motor interference calibration: 0: no, 1: yes| Empty|  */
 	MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units|  */
 	MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty|  */
 	MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty|  */
@@ -433,6 +497,48 @@ enum MAV_SEVERITY
 };
 #endif
 
+/** @brief Power supply status flags (bitmask) */
+#ifndef HAVE_ENUM_MAV_POWER_STATUS
+#define HAVE_ENUM_MAV_POWER_STATUS
+enum MAV_POWER_STATUS
+{
+	MAV_POWER_STATUS_BRICK_VALID=1, /* main brick power supply valid | */
+	MAV_POWER_STATUS_SERVO_VALID=2, /* main servo power supply valid for FMU | */
+	MAV_POWER_STATUS_USB_CONNECTED=4, /* USB power is connected | */
+	MAV_POWER_STATUS_PERIPH_OVERCURRENT=8, /* peripheral supply is in over-current state | */
+	MAV_POWER_STATUS_PERIPH_HIPOWER_OVERCURRENT=16, /* hi-power peripheral supply is in over-current state | */
+	MAV_POWER_STATUS_CHANGED=32, /* Power status has changed since boot | */
+	MAV_POWER_STATUS_ENUM_END=33, /*  | */
+};
+#endif
+
+/** @brief SERIAL_CONTROL device types */
+#ifndef HAVE_ENUM_SERIAL_CONTROL_DEV
+#define HAVE_ENUM_SERIAL_CONTROL_DEV
+enum SERIAL_CONTROL_DEV
+{
+	SERIAL_CONTROL_DEV_TELEM1=0, /* First telemetry port | */
+	SERIAL_CONTROL_DEV_TELEM2=1, /* Second telemetry port | */
+	SERIAL_CONTROL_DEV_GPS1=2, /* First GPS port | */
+	SERIAL_CONTROL_DEV_GPS2=3, /* Second GPS port | */
+	SERIAL_CONTROL_DEV_ENUM_END=4, /*  | */
+};
+#endif
+
+/** @brief SERIAL_CONTROL flags (bitmask) */
+#ifndef HAVE_ENUM_SERIAL_CONTROL_FLAG
+#define HAVE_ENUM_SERIAL_CONTROL_FLAG
+enum SERIAL_CONTROL_FLAG
+{
+	SERIAL_CONTROL_FLAG_REPLY=1, /* Set if this is a reply | */
+	SERIAL_CONTROL_FLAG_RESPOND=2, /* Set if the sender wants the receiver to send a response as another SERIAL_CONTROL message | */
+	SERIAL_CONTROL_FLAG_EXCLUSIVE=4, /* Set if access to the serial port should be removed from whatever driver is currently using it, giving exclusive access to the SERIAL_CONTROL protocol. The port can be handed back by sending a request without this flag set | */
+	SERIAL_CONTROL_FLAG_BLOCKING=8, /* Block on writes to the serial port | */
+	SERIAL_CONTROL_FLAG_MULTI=16, /* Send multiple replies until port is drained | */
+	SERIAL_CONTROL_FLAG_ENUM_END=17, /*  | */
+};
+#endif
+
 
 
 // MAVLINK VERSION
@@ -500,6 +606,7 @@ enum MAV_SEVERITY
 #include "./mavlink_msg_nav_controller_output.h"
 #include "./mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h"
 #include "./mavlink_msg_state_correction.h"
+#include "./mavlink_msg_rc_channels.h"
 #include "./mavlink_msg_request_data_stream.h"
 #include "./mavlink_msg_data_stream.h"
 #include "./mavlink_msg_manual_control.h"
@@ -538,8 +645,11 @@ enum MAV_SEVERITY
 #include "./mavlink_msg_log_request_end.h"
 #include "./mavlink_msg_gps_inject_data.h"
 #include "./mavlink_msg_gps2_raw.h"
+#include "./mavlink_msg_power_status.h"
+#include "./mavlink_msg_serial_control.h"
 #include "./mavlink_msg_data_transmission_handshake.h"
 #include "./mavlink_msg_encapsulated_data.h"
+#include "./mavlink_msg_distance_sensor.h"
 #include "./mavlink_msg_battery_status.h"
 #include "./mavlink_msg_setpoint_8dof.h"
 #include "./mavlink_msg_setpoint_6dof.h"
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h
index 8ddf5bf099..ff2f104d40 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h
@@ -212,6 +212,50 @@ static inline void mavlink_msg_attitude_send(mavlink_channel_t chan, uint32_t ti
 #endif
 }
 
+#if MAVLINK_MSG_ID_ATTITUDE_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_attitude_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint32_t time_boot_ms, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint32_t(buf, 0, time_boot_ms);
+	_mav_put_float(buf, 4, roll);
+	_mav_put_float(buf, 8, pitch);
+	_mav_put_float(buf, 12, yaw);
+	_mav_put_float(buf, 16, rollspeed);
+	_mav_put_float(buf, 20, pitchspeed);
+	_mav_put_float(buf, 24, yawspeed);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, buf, MAVLINK_MSG_ID_ATTITUDE_LEN, MAVLINK_MSG_ID_ATTITUDE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, buf, MAVLINK_MSG_ID_ATTITUDE_LEN);
+#endif
+#else
+	mavlink_attitude_t *packet = (mavlink_attitude_t *)msgbuf;
+	packet->time_boot_ms = time_boot_ms;
+	packet->roll = roll;
+	packet->pitch = pitch;
+	packet->yaw = yaw;
+	packet->rollspeed = rollspeed;
+	packet->pitchspeed = pitchspeed;
+	packet->yawspeed = yawspeed;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_LEN, MAVLINK_MSG_ID_ATTITUDE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE ATTITUDE UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h
index 9f8d587598..3b97b40d0d 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h
@@ -223,6 +223,52 @@ static inline void mavlink_msg_attitude_quaternion_send(mavlink_channel_t chan,
 #endif
 }
 
+#if MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_attitude_quaternion_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint32_t time_boot_ms, float q1, float q2, float q3, float q4, float rollspeed, float pitchspeed, float yawspeed)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint32_t(buf, 0, time_boot_ms);
+	_mav_put_float(buf, 4, q1);
+	_mav_put_float(buf, 8, q2);
+	_mav_put_float(buf, 12, q3);
+	_mav_put_float(buf, 16, q4);
+	_mav_put_float(buf, 20, rollspeed);
+	_mav_put_float(buf, 24, pitchspeed);
+	_mav_put_float(buf, 28, yawspeed);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN);
+#endif
+#else
+	mavlink_attitude_quaternion_t *packet = (mavlink_attitude_quaternion_t *)msgbuf;
+	packet->time_boot_ms = time_boot_ms;
+	packet->q1 = q1;
+	packet->q2 = q2;
+	packet->q3 = q3;
+	packet->q4 = q4;
+	packet->rollspeed = rollspeed;
+	packet->pitchspeed = pitchspeed;
+	packet->yawspeed = yawspeed;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE ATTITUDE_QUATERNION UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_auth_key.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_auth_key.h
index 5703a59871..f31b6bbf48 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_auth_key.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_auth_key.h
@@ -146,6 +146,38 @@ static inline void mavlink_msg_auth_key_send(mavlink_channel_t chan, const char
 #endif
 }
 
+#if MAVLINK_MSG_ID_AUTH_KEY_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_auth_key_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  const char *key)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+
+	_mav_put_char_array(buf, 0, key, 32);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, buf, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, buf, MAVLINK_MSG_ID_AUTH_KEY_LEN);
+#endif
+#else
+	mavlink_auth_key_t *packet = (mavlink_auth_key_t *)msgbuf;
+
+	mav_array_memcpy(packet->key, key, sizeof(char)*32);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, (const char *)packet, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, (const char *)packet, MAVLINK_MSG_ID_AUTH_KEY_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE AUTH_KEY UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h
index 03e4d569e6..faaabf9f70 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h
@@ -256,6 +256,58 @@ static inline void mavlink_msg_battery_status_send(mavlink_channel_t chan, uint8
 #endif
 }
 
+#if MAVLINK_MSG_ID_BATTERY_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_battery_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t accu_id, uint16_t voltage_cell_1, uint16_t voltage_cell_2, uint16_t voltage_cell_3, uint16_t voltage_cell_4, uint16_t voltage_cell_5, uint16_t voltage_cell_6, int16_t current_battery, int32_t current_consumed, int32_t energy_consumed, int8_t battery_remaining)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_int32_t(buf, 0, current_consumed);
+	_mav_put_int32_t(buf, 4, energy_consumed);
+	_mav_put_uint16_t(buf, 8, voltage_cell_1);
+	_mav_put_uint16_t(buf, 10, voltage_cell_2);
+	_mav_put_uint16_t(buf, 12, voltage_cell_3);
+	_mav_put_uint16_t(buf, 14, voltage_cell_4);
+	_mav_put_uint16_t(buf, 16, voltage_cell_5);
+	_mav_put_uint16_t(buf, 18, voltage_cell_6);
+	_mav_put_int16_t(buf, 20, current_battery);
+	_mav_put_uint8_t(buf, 22, accu_id);
+	_mav_put_int8_t(buf, 23, battery_remaining);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, buf, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, buf, MAVLINK_MSG_ID_BATTERY_STATUS_LEN);
+#endif
+#else
+	mavlink_battery_status_t *packet = (mavlink_battery_status_t *)msgbuf;
+	packet->current_consumed = current_consumed;
+	packet->energy_consumed = energy_consumed;
+	packet->voltage_cell_1 = voltage_cell_1;
+	packet->voltage_cell_2 = voltage_cell_2;
+	packet->voltage_cell_3 = voltage_cell_3;
+	packet->voltage_cell_4 = voltage_cell_4;
+	packet->voltage_cell_5 = voltage_cell_5;
+	packet->voltage_cell_6 = voltage_cell_6;
+	packet->current_battery = current_battery;
+	packet->accu_id = accu_id;
+	packet->battery_remaining = battery_remaining;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, (const char *)packet, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, (const char *)packet, MAVLINK_MSG_ID_BATTERY_STATUS_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE BATTERY_STATUS UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control.h
index 0b6de930d2..3f0987f10a 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control.h
@@ -173,6 +173,42 @@ static inline void mavlink_msg_change_operator_control_send(mavlink_channel_t ch
 #endif
 }
 
+#if MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_change_operator_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t control_request, uint8_t version, const char *passkey)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint8_t(buf, 0, target_system);
+	_mav_put_uint8_t(buf, 1, control_request);
+	_mav_put_uint8_t(buf, 2, version);
+	_mav_put_char_array(buf, 3, passkey, 25);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN);
+#endif
+#else
+	mavlink_change_operator_control_t *packet = (mavlink_change_operator_control_t *)msgbuf;
+	packet->target_system = target_system;
+	packet->control_request = control_request;
+	packet->version = version;
+	mav_array_memcpy(packet->passkey, passkey, sizeof(char)*25);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, (const char *)packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, (const char *)packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE CHANGE_OPERATOR_CONTROL UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control_ack.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control_ack.h
index c6f6a28e4b..768e7ed5ca 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control_ack.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control_ack.h
@@ -168,6 +168,42 @@ static inline void mavlink_msg_change_operator_control_ack_send(mavlink_channel_
 #endif
 }
 
+#if MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_change_operator_control_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t gcs_system_id, uint8_t control_request, uint8_t ack)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint8_t(buf, 0, gcs_system_id);
+	_mav_put_uint8_t(buf, 1, control_request);
+	_mav_put_uint8_t(buf, 2, ack);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN);
+#endif
+#else
+	mavlink_change_operator_control_ack_t *packet = (mavlink_change_operator_control_ack_t *)msgbuf;
+	packet->gcs_system_id = gcs_system_id;
+	packet->control_request = control_request;
+	packet->ack = ack;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, (const char *)packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, (const char *)packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE CHANGE_OPERATOR_CONTROL_ACK UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_ack.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_ack.h
index dca2fe6819..d3d1630419 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_ack.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_ack.h
@@ -157,6 +157,40 @@ static inline void mavlink_msg_command_ack_send(mavlink_channel_t chan, uint16_t
 #endif
 }
 
+#if MAVLINK_MSG_ID_COMMAND_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_command_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint16_t command, uint8_t result)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint16_t(buf, 0, command);
+	_mav_put_uint8_t(buf, 2, result);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, buf, MAVLINK_MSG_ID_COMMAND_ACK_LEN, MAVLINK_MSG_ID_COMMAND_ACK_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, buf, MAVLINK_MSG_ID_COMMAND_ACK_LEN);
+#endif
+#else
+	mavlink_command_ack_t *packet = (mavlink_command_ack_t *)msgbuf;
+	packet->command = command;
+	packet->result = result;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, (const char *)packet, MAVLINK_MSG_ID_COMMAND_ACK_LEN, MAVLINK_MSG_ID_COMMAND_ACK_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, (const char *)packet, MAVLINK_MSG_ID_COMMAND_ACK_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE COMMAND_ACK UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_long.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_long.h
index 8f705c0dd2..161896b973 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_long.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_long.h
@@ -256,6 +256,58 @@ static inline void mavlink_msg_command_long_send(mavlink_channel_t chan, uint8_t
 #endif
 }
 
+#if MAVLINK_MSG_ID_COMMAND_LONG_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_command_long_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component, uint16_t command, uint8_t confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, param1);
+	_mav_put_float(buf, 4, param2);
+	_mav_put_float(buf, 8, param3);
+	_mav_put_float(buf, 12, param4);
+	_mav_put_float(buf, 16, param5);
+	_mav_put_float(buf, 20, param6);
+	_mav_put_float(buf, 24, param7);
+	_mav_put_uint16_t(buf, 28, command);
+	_mav_put_uint8_t(buf, 30, target_system);
+	_mav_put_uint8_t(buf, 31, target_component);
+	_mav_put_uint8_t(buf, 32, confirmation);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, buf, MAVLINK_MSG_ID_COMMAND_LONG_LEN, MAVLINK_MSG_ID_COMMAND_LONG_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, buf, MAVLINK_MSG_ID_COMMAND_LONG_LEN);
+#endif
+#else
+	mavlink_command_long_t *packet = (mavlink_command_long_t *)msgbuf;
+	packet->param1 = param1;
+	packet->param2 = param2;
+	packet->param3 = param3;
+	packet->param4 = param4;
+	packet->param5 = param5;
+	packet->param6 = param6;
+	packet->param7 = param7;
+	packet->command = command;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+	packet->confirmation = confirmation;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, (const char *)packet, MAVLINK_MSG_ID_COMMAND_LONG_LEN, MAVLINK_MSG_ID_COMMAND_LONG_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, (const char *)packet, MAVLINK_MSG_ID_COMMAND_LONG_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE COMMAND_LONG UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_stream.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_stream.h
index dc0768e128..640ffebf44 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_stream.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_stream.h
@@ -168,6 +168,42 @@ static inline void mavlink_msg_data_stream_send(mavlink_channel_t chan, uint8_t
 #endif
 }
 
+#if MAVLINK_MSG_ID_DATA_STREAM_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_data_stream_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t stream_id, uint16_t message_rate, uint8_t on_off)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint16_t(buf, 0, message_rate);
+	_mav_put_uint8_t(buf, 2, stream_id);
+	_mav_put_uint8_t(buf, 3, on_off);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_STREAM, buf, MAVLINK_MSG_ID_DATA_STREAM_LEN, MAVLINK_MSG_ID_DATA_STREAM_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_STREAM, buf, MAVLINK_MSG_ID_DATA_STREAM_LEN);
+#endif
+#else
+	mavlink_data_stream_t *packet = (mavlink_data_stream_t *)msgbuf;
+	packet->message_rate = message_rate;
+	packet->stream_id = stream_id;
+	packet->on_off = on_off;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_STREAM, (const char *)packet, MAVLINK_MSG_ID_DATA_STREAM_LEN, MAVLINK_MSG_ID_DATA_STREAM_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_STREAM, (const char *)packet, MAVLINK_MSG_ID_DATA_STREAM_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE DATA_STREAM UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_transmission_handshake.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_transmission_handshake.h
index fdd8fddd87..d84a73709d 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_transmission_handshake.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_transmission_handshake.h
@@ -212,6 +212,50 @@ static inline void mavlink_msg_data_transmission_handshake_send(mavlink_channel_
 #endif
 }
 
+#if MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_data_transmission_handshake_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t type, uint32_t size, uint16_t width, uint16_t height, uint16_t packets, uint8_t payload, uint8_t jpg_quality)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint32_t(buf, 0, size);
+	_mav_put_uint16_t(buf, 4, width);
+	_mav_put_uint16_t(buf, 6, height);
+	_mav_put_uint16_t(buf, 8, packets);
+	_mav_put_uint8_t(buf, 10, type);
+	_mav_put_uint8_t(buf, 11, payload);
+	_mav_put_uint8_t(buf, 12, jpg_quality);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, buf, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, buf, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN);
+#endif
+#else
+	mavlink_data_transmission_handshake_t *packet = (mavlink_data_transmission_handshake_t *)msgbuf;
+	packet->size = size;
+	packet->width = width;
+	packet->height = height;
+	packet->packets = packets;
+	packet->type = type;
+	packet->payload = payload;
+	packet->jpg_quality = jpg_quality;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, (const char *)packet, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, (const char *)packet, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE DATA_TRANSMISSION_HANDSHAKE UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h
index 9a6ed87eeb..2102af8623 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h
@@ -168,6 +168,42 @@ static inline void mavlink_msg_debug_send(mavlink_channel_t chan, uint32_t time_
 #endif
 }
 
+#if MAVLINK_MSG_ID_DEBUG_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_debug_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint32_t time_boot_ms, uint8_t ind, float value)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint32_t(buf, 0, time_boot_ms);
+	_mav_put_float(buf, 4, value);
+	_mav_put_uint8_t(buf, 8, ind);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, buf, MAVLINK_MSG_ID_DEBUG_LEN, MAVLINK_MSG_ID_DEBUG_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, buf, MAVLINK_MSG_ID_DEBUG_LEN);
+#endif
+#else
+	mavlink_debug_t *packet = (mavlink_debug_t *)msgbuf;
+	packet->time_boot_ms = time_boot_ms;
+	packet->value = value;
+	packet->ind = ind;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, (const char *)packet, MAVLINK_MSG_ID_DEBUG_LEN, MAVLINK_MSG_ID_DEBUG_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, (const char *)packet, MAVLINK_MSG_ID_DEBUG_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE DEBUG UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug_vect.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug_vect.h
index 6cfc75212e..67c339e898 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug_vect.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug_vect.h
@@ -184,6 +184,44 @@ static inline void mavlink_msg_debug_vect_send(mavlink_channel_t chan, const cha
 #endif
 }
 
+#if MAVLINK_MSG_ID_DEBUG_VECT_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_debug_vect_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  const char *name, uint64_t time_usec, float x, float y, float z)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint64_t(buf, 0, time_usec);
+	_mav_put_float(buf, 8, x);
+	_mav_put_float(buf, 12, y);
+	_mav_put_float(buf, 16, z);
+	_mav_put_char_array(buf, 20, name, 10);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, buf, MAVLINK_MSG_ID_DEBUG_VECT_LEN, MAVLINK_MSG_ID_DEBUG_VECT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, buf, MAVLINK_MSG_ID_DEBUG_VECT_LEN);
+#endif
+#else
+	mavlink_debug_vect_t *packet = (mavlink_debug_vect_t *)msgbuf;
+	packet->time_usec = time_usec;
+	packet->x = x;
+	packet->y = y;
+	packet->z = z;
+	mav_array_memcpy(packet->name, name, sizeof(char)*10);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, (const char *)packet, MAVLINK_MSG_ID_DEBUG_VECT_LEN, MAVLINK_MSG_ID_DEBUG_VECT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, (const char *)packet, MAVLINK_MSG_ID_DEBUG_VECT_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE DEBUG_VECT UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_distance_sensor.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_distance_sensor.h
new file mode 100644
index 0000000000..39b3843966
--- /dev/null
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_distance_sensor.h
@@ -0,0 +1,377 @@
+// MESSAGE DISTANCE_SENSOR PACKING
+
+#define MAVLINK_MSG_ID_DISTANCE_SENSOR 132
+
+typedef struct __mavlink_distance_sensor_t
+{
+ uint32_t time_boot_ms; ///< Time since system boot
+ uint16_t min_distance; ///< Minimum distance the sensor can measure in centimeters
+ uint16_t max_distance; ///< Maximum distance the sensor can measure in centimeters
+ uint16_t current_distance; ///< Current distance reading
+ uint8_t type; ///< Type from MAV_DISTANCE_SENSOR enum.
+ uint8_t id; ///< Onboard ID of the sensor
+ uint8_t orientation; ///< Direction the sensor faces from FIXME enum.
+ uint8_t covariance; ///< Measurement covariance in centimeters, 0 for unknown / invalid readings
+} mavlink_distance_sensor_t;
+
+#define MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN 14
+#define MAVLINK_MSG_ID_132_LEN 14
+
+#define MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC 85
+#define MAVLINK_MSG_ID_132_CRC 85
+
+
+
+#define MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR { \
+	"DISTANCE_SENSOR", \
+	8, \
+	{  { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_distance_sensor_t, time_boot_ms) }, \
+         { "min_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_distance_sensor_t, min_distance) }, \
+         { "max_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_distance_sensor_t, max_distance) }, \
+         { "current_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_distance_sensor_t, current_distance) }, \
+         { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_distance_sensor_t, type) }, \
+         { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_distance_sensor_t, id) }, \
+         { "orientation", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_distance_sensor_t, orientation) }, \
+         { "covariance", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_distance_sensor_t, covariance) }, \
+         } \
+}
+
+
+/**
+ * @brief Pack a distance_sensor message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ *
+ * @param time_boot_ms Time since system boot
+ * @param type Type from MAV_DISTANCE_SENSOR enum.
+ * @param id Onboard ID of the sensor
+ * @param orientation Direction the sensor faces from FIXME enum.
+ * @param min_distance Minimum distance the sensor can measure in centimeters
+ * @param max_distance Maximum distance the sensor can measure in centimeters
+ * @param current_distance Current distance reading
+ * @param covariance Measurement covariance in centimeters, 0 for unknown / invalid readings
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_distance_sensor_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
+						       uint32_t time_boot_ms, uint8_t type, uint8_t id, uint8_t orientation, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t covariance)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char buf[MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN];
+	_mav_put_uint32_t(buf, 0, time_boot_ms);
+	_mav_put_uint16_t(buf, 4, min_distance);
+	_mav_put_uint16_t(buf, 6, max_distance);
+	_mav_put_uint16_t(buf, 8, current_distance);
+	_mav_put_uint8_t(buf, 10, type);
+	_mav_put_uint8_t(buf, 11, id);
+	_mav_put_uint8_t(buf, 12, orientation);
+	_mav_put_uint8_t(buf, 13, covariance);
+
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN);
+#else
+	mavlink_distance_sensor_t packet;
+	packet.time_boot_ms = time_boot_ms;
+	packet.min_distance = min_distance;
+	packet.max_distance = max_distance;
+	packet.current_distance = current_distance;
+	packet.type = type;
+	packet.id = id;
+	packet.orientation = orientation;
+	packet.covariance = covariance;
+
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN);
+#endif
+
+	msg->msgid = MAVLINK_MSG_ID_DISTANCE_SENSOR;
+#if MAVLINK_CRC_EXTRA
+    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC);
+#else
+    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN);
+#endif
+}
+
+/**
+ * @brief Pack a distance_sensor message on a channel
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message will be sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param time_boot_ms Time since system boot
+ * @param type Type from MAV_DISTANCE_SENSOR enum.
+ * @param id Onboard ID of the sensor
+ * @param orientation Direction the sensor faces from FIXME enum.
+ * @param min_distance Minimum distance the sensor can measure in centimeters
+ * @param max_distance Maximum distance the sensor can measure in centimeters
+ * @param current_distance Current distance reading
+ * @param covariance Measurement covariance in centimeters, 0 for unknown / invalid readings
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_distance_sensor_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
+							   mavlink_message_t* msg,
+						           uint32_t time_boot_ms,uint8_t type,uint8_t id,uint8_t orientation,uint16_t min_distance,uint16_t max_distance,uint16_t current_distance,uint8_t covariance)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char buf[MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN];
+	_mav_put_uint32_t(buf, 0, time_boot_ms);
+	_mav_put_uint16_t(buf, 4, min_distance);
+	_mav_put_uint16_t(buf, 6, max_distance);
+	_mav_put_uint16_t(buf, 8, current_distance);
+	_mav_put_uint8_t(buf, 10, type);
+	_mav_put_uint8_t(buf, 11, id);
+	_mav_put_uint8_t(buf, 12, orientation);
+	_mav_put_uint8_t(buf, 13, covariance);
+
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN);
+#else
+	mavlink_distance_sensor_t packet;
+	packet.time_boot_ms = time_boot_ms;
+	packet.min_distance = min_distance;
+	packet.max_distance = max_distance;
+	packet.current_distance = current_distance;
+	packet.type = type;
+	packet.id = id;
+	packet.orientation = orientation;
+	packet.covariance = covariance;
+
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN);
+#endif
+
+	msg->msgid = MAVLINK_MSG_ID_DISTANCE_SENSOR;
+#if MAVLINK_CRC_EXTRA
+    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC);
+#else
+    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN);
+#endif
+}
+
+/**
+ * @brief Encode a distance_sensor struct
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ * @param distance_sensor C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_distance_sensor_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_distance_sensor_t* distance_sensor)
+{
+	return mavlink_msg_distance_sensor_pack(system_id, component_id, msg, distance_sensor->time_boot_ms, distance_sensor->type, distance_sensor->id, distance_sensor->orientation, distance_sensor->min_distance, distance_sensor->max_distance, distance_sensor->current_distance, distance_sensor->covariance);
+}
+
+/**
+ * @brief Encode a distance_sensor struct on a channel
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message will be sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param distance_sensor C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_distance_sensor_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_distance_sensor_t* distance_sensor)
+{
+	return mavlink_msg_distance_sensor_pack_chan(system_id, component_id, chan, msg, distance_sensor->time_boot_ms, distance_sensor->type, distance_sensor->id, distance_sensor->orientation, distance_sensor->min_distance, distance_sensor->max_distance, distance_sensor->current_distance, distance_sensor->covariance);
+}
+
+/**
+ * @brief Send a distance_sensor message
+ * @param chan MAVLink channel to send the message
+ *
+ * @param time_boot_ms Time since system boot
+ * @param type Type from MAV_DISTANCE_SENSOR enum.
+ * @param id Onboard ID of the sensor
+ * @param orientation Direction the sensor faces from FIXME enum.
+ * @param min_distance Minimum distance the sensor can measure in centimeters
+ * @param max_distance Maximum distance the sensor can measure in centimeters
+ * @param current_distance Current distance reading
+ * @param covariance Measurement covariance in centimeters, 0 for unknown / invalid readings
+ */
+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
+
+static inline void mavlink_msg_distance_sensor_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t type, uint8_t id, uint8_t orientation, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t covariance)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char buf[MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN];
+	_mav_put_uint32_t(buf, 0, time_boot_ms);
+	_mav_put_uint16_t(buf, 4, min_distance);
+	_mav_put_uint16_t(buf, 6, max_distance);
+	_mav_put_uint16_t(buf, 8, current_distance);
+	_mav_put_uint8_t(buf, 10, type);
+	_mav_put_uint8_t(buf, 11, id);
+	_mav_put_uint8_t(buf, 12, orientation);
+	_mav_put_uint8_t(buf, 13, covariance);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN);
+#endif
+#else
+	mavlink_distance_sensor_t packet;
+	packet.time_boot_ms = time_boot_ms;
+	packet.min_distance = min_distance;
+	packet.max_distance = max_distance;
+	packet.current_distance = current_distance;
+	packet.type = type;
+	packet.id = id;
+	packet.orientation = orientation;
+	packet.covariance = covariance;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, (const char *)&packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, (const char *)&packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN);
+#endif
+#endif
+}
+
+#if MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_distance_sensor_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint32_t time_boot_ms, uint8_t type, uint8_t id, uint8_t orientation, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t covariance)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint32_t(buf, 0, time_boot_ms);
+	_mav_put_uint16_t(buf, 4, min_distance);
+	_mav_put_uint16_t(buf, 6, max_distance);
+	_mav_put_uint16_t(buf, 8, current_distance);
+	_mav_put_uint8_t(buf, 10, type);
+	_mav_put_uint8_t(buf, 11, id);
+	_mav_put_uint8_t(buf, 12, orientation);
+	_mav_put_uint8_t(buf, 13, covariance);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN);
+#endif
+#else
+	mavlink_distance_sensor_t *packet = (mavlink_distance_sensor_t *)msgbuf;
+	packet->time_boot_ms = time_boot_ms;
+	packet->min_distance = min_distance;
+	packet->max_distance = max_distance;
+	packet->current_distance = current_distance;
+	packet->type = type;
+	packet->id = id;
+	packet->orientation = orientation;
+	packet->covariance = covariance;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, (const char *)packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, (const char *)packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN);
+#endif
+#endif
+}
+#endif
+
+#endif
+
+// MESSAGE DISTANCE_SENSOR UNPACKING
+
+
+/**
+ * @brief Get field time_boot_ms from distance_sensor message
+ *
+ * @return Time since system boot
+ */
+static inline uint32_t mavlink_msg_distance_sensor_get_time_boot_ms(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint32_t(msg,  0);
+}
+
+/**
+ * @brief Get field type from distance_sensor message
+ *
+ * @return Type from MAV_DISTANCE_SENSOR enum.
+ */
+static inline uint8_t mavlink_msg_distance_sensor_get_type(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint8_t(msg,  10);
+}
+
+/**
+ * @brief Get field id from distance_sensor message
+ *
+ * @return Onboard ID of the sensor
+ */
+static inline uint8_t mavlink_msg_distance_sensor_get_id(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint8_t(msg,  11);
+}
+
+/**
+ * @brief Get field orientation from distance_sensor message
+ *
+ * @return Direction the sensor faces from FIXME enum.
+ */
+static inline uint8_t mavlink_msg_distance_sensor_get_orientation(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint8_t(msg,  12);
+}
+
+/**
+ * @brief Get field min_distance from distance_sensor message
+ *
+ * @return Minimum distance the sensor can measure in centimeters
+ */
+static inline uint16_t mavlink_msg_distance_sensor_get_min_distance(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint16_t(msg,  4);
+}
+
+/**
+ * @brief Get field max_distance from distance_sensor message
+ *
+ * @return Maximum distance the sensor can measure in centimeters
+ */
+static inline uint16_t mavlink_msg_distance_sensor_get_max_distance(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint16_t(msg,  6);
+}
+
+/**
+ * @brief Get field current_distance from distance_sensor message
+ *
+ * @return Current distance reading
+ */
+static inline uint16_t mavlink_msg_distance_sensor_get_current_distance(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint16_t(msg,  8);
+}
+
+/**
+ * @brief Get field covariance from distance_sensor message
+ *
+ * @return Measurement covariance in centimeters, 0 for unknown / invalid readings
+ */
+static inline uint8_t mavlink_msg_distance_sensor_get_covariance(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint8_t(msg,  13);
+}
+
+/**
+ * @brief Decode a distance_sensor message into a struct
+ *
+ * @param msg The message to decode
+ * @param distance_sensor C-struct to decode the message contents into
+ */
+static inline void mavlink_msg_distance_sensor_decode(const mavlink_message_t* msg, mavlink_distance_sensor_t* distance_sensor)
+{
+#if MAVLINK_NEED_BYTE_SWAP
+	distance_sensor->time_boot_ms = mavlink_msg_distance_sensor_get_time_boot_ms(msg);
+	distance_sensor->min_distance = mavlink_msg_distance_sensor_get_min_distance(msg);
+	distance_sensor->max_distance = mavlink_msg_distance_sensor_get_max_distance(msg);
+	distance_sensor->current_distance = mavlink_msg_distance_sensor_get_current_distance(msg);
+	distance_sensor->type = mavlink_msg_distance_sensor_get_type(msg);
+	distance_sensor->id = mavlink_msg_distance_sensor_get_id(msg);
+	distance_sensor->orientation = mavlink_msg_distance_sensor_get_orientation(msg);
+	distance_sensor->covariance = mavlink_msg_distance_sensor_get_covariance(msg);
+#else
+	memcpy(distance_sensor, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN);
+#endif
+}
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_encapsulated_data.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_encapsulated_data.h
index 5900ea8388..dacd7c9f40 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_encapsulated_data.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_encapsulated_data.h
@@ -151,6 +151,38 @@ static inline void mavlink_msg_encapsulated_data_send(mavlink_channel_t chan, ui
 #endif
 }
 
+#if MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_encapsulated_data_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint16_t seqnr, const uint8_t *data)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint16_t(buf, 0, seqnr);
+	_mav_put_uint8_t_array(buf, 2, data, 253);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, buf, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN, MAVLINK_MSG_ID_ENCAPSULATED_DATA_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, buf, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN);
+#endif
+#else
+	mavlink_encapsulated_data_t *packet = (mavlink_encapsulated_data_t *)msgbuf;
+	packet->seqnr = seqnr;
+	mav_array_memcpy(packet->data, data, sizeof(uint8_t)*253);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, (const char *)packet, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN, MAVLINK_MSG_ID_ENCAPSULATED_DATA_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, (const char *)packet, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE ENCAPSULATED_DATA UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_dir_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_dir_list.h
index 4f31698d50..81c7031ced 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_dir_list.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_dir_list.h
@@ -162,6 +162,40 @@ static inline void mavlink_msg_file_transfer_dir_list_send(mavlink_channel_t cha
 #endif
 }
 
+#if MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_file_transfer_dir_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint64_t transfer_uid, const char *dir_path, uint8_t flags)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint64_t(buf, 0, transfer_uid);
+	_mav_put_uint8_t(buf, 248, flags);
+	_mav_put_char_array(buf, 8, dir_path, 240);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST, buf, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST, buf, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN);
+#endif
+#else
+	mavlink_file_transfer_dir_list_t *packet = (mavlink_file_transfer_dir_list_t *)msgbuf;
+	packet->transfer_uid = transfer_uid;
+	packet->flags = flags;
+	mav_array_memcpy(packet->dir_path, dir_path, sizeof(char)*240);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST, (const char *)packet, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST, (const char *)packet, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE FILE_TRANSFER_DIR_LIST UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_res.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_res.h
index fc6247faca..04981fa85a 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_res.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_res.h
@@ -157,6 +157,40 @@ static inline void mavlink_msg_file_transfer_res_send(mavlink_channel_t chan, ui
 #endif
 }
 
+#if MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_file_transfer_res_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint64_t transfer_uid, uint8_t result)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint64_t(buf, 0, transfer_uid);
+	_mav_put_uint8_t(buf, 8, result);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_RES, buf, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_RES_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_RES, buf, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN);
+#endif
+#else
+	mavlink_file_transfer_res_t *packet = (mavlink_file_transfer_res_t *)msgbuf;
+	packet->transfer_uid = transfer_uid;
+	packet->result = result;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_RES, (const char *)packet, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_RES_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_RES, (const char *)packet, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE FILE_TRANSFER_RES UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_start.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_start.h
index 05be77339b..7b1fba519a 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_start.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_start.h
@@ -184,6 +184,44 @@ static inline void mavlink_msg_file_transfer_start_send(mavlink_channel_t chan,
 #endif
 }
 
+#if MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_file_transfer_start_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint64_t transfer_uid, const char *dest_path, uint8_t direction, uint32_t file_size, uint8_t flags)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint64_t(buf, 0, transfer_uid);
+	_mav_put_uint32_t(buf, 8, file_size);
+	_mav_put_uint8_t(buf, 252, direction);
+	_mav_put_uint8_t(buf, 253, flags);
+	_mav_put_char_array(buf, 12, dest_path, 240);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_START, buf, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_START_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_START, buf, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN);
+#endif
+#else
+	mavlink_file_transfer_start_t *packet = (mavlink_file_transfer_start_t *)msgbuf;
+	packet->transfer_uid = transfer_uid;
+	packet->file_size = file_size;
+	packet->direction = direction;
+	packet->flags = flags;
+	mav_array_memcpy(packet->dest_path, dest_path, sizeof(char)*240);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_START, (const char *)packet, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_START_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_START, (const char *)packet, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE FILE_TRANSFER_START UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int.h
index 7ed3d2a636..ba15bf3654 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int.h
@@ -234,6 +234,54 @@ static inline void mavlink_msg_global_position_int_send(mavlink_channel_t chan,
 #endif
 }
 
+#if MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_global_position_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint32_t time_boot_ms, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, int16_t vx, int16_t vy, int16_t vz, uint16_t hdg)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint32_t(buf, 0, time_boot_ms);
+	_mav_put_int32_t(buf, 4, lat);
+	_mav_put_int32_t(buf, 8, lon);
+	_mav_put_int32_t(buf, 12, alt);
+	_mav_put_int32_t(buf, 16, relative_alt);
+	_mav_put_int16_t(buf, 20, vx);
+	_mav_put_int16_t(buf, 22, vy);
+	_mav_put_int16_t(buf, 24, vz);
+	_mav_put_uint16_t(buf, 26, hdg);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN);
+#endif
+#else
+	mavlink_global_position_int_t *packet = (mavlink_global_position_int_t *)msgbuf;
+	packet->time_boot_ms = time_boot_ms;
+	packet->lat = lat;
+	packet->lon = lon;
+	packet->alt = alt;
+	packet->relative_alt = relative_alt;
+	packet->vx = vx;
+	packet->vy = vy;
+	packet->vz = vz;
+	packet->hdg = hdg;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE GLOBAL_POSITION_INT UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_int.h
index 1a1c97199e..77d735cf4f 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_int.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_int.h
@@ -190,6 +190,46 @@ static inline void mavlink_msg_global_position_setpoint_int_send(mavlink_channel
 #endif
 }
 
+#if MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_global_position_setpoint_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t coordinate_frame, int32_t latitude, int32_t longitude, int32_t altitude, int16_t yaw)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_int32_t(buf, 0, latitude);
+	_mav_put_int32_t(buf, 4, longitude);
+	_mav_put_int32_t(buf, 8, altitude);
+	_mav_put_int16_t(buf, 12, yaw);
+	_mav_put_uint8_t(buf, 14, coordinate_frame);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN);
+#endif
+#else
+	mavlink_global_position_setpoint_int_t *packet = (mavlink_global_position_setpoint_int_t *)msgbuf;
+	packet->latitude = latitude;
+	packet->longitude = longitude;
+	packet->altitude = altitude;
+	packet->yaw = yaw;
+	packet->coordinate_frame = coordinate_frame;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE GLOBAL_POSITION_SETPOINT_INT UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_vision_position_estimate.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_vision_position_estimate.h
index f7be74c917..b2b75d7ef5 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_vision_position_estimate.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_vision_position_estimate.h
@@ -212,6 +212,50 @@ static inline void mavlink_msg_global_vision_position_estimate_send(mavlink_chan
 #endif
 }
 
+#if MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_global_vision_position_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint64_t(buf, 0, usec);
+	_mav_put_float(buf, 8, x);
+	_mav_put_float(buf, 12, y);
+	_mav_put_float(buf, 16, z);
+	_mav_put_float(buf, 20, roll);
+	_mav_put_float(buf, 24, pitch);
+	_mav_put_float(buf, 28, yaw);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN);
+#endif
+#else
+	mavlink_global_vision_position_estimate_t *packet = (mavlink_global_vision_position_estimate_t *)msgbuf;
+	packet->usec = usec;
+	packet->x = x;
+	packet->y = y;
+	packet->z = z;
+	packet->roll = roll;
+	packet->pitch = pitch;
+	packet->yaw = yaw;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE GLOBAL_VISION_POSITION_ESTIMATE UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_raw.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_raw.h
index 17e5bd0026..b184e7c9cc 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_raw.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_raw.h
@@ -267,6 +267,60 @@ static inline void mavlink_msg_gps2_raw_send(mavlink_channel_t chan, uint64_t ti
 #endif
 }
 
+#if MAVLINK_MSG_ID_GPS2_RAW_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_gps2_raw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint64_t(buf, 0, time_usec);
+	_mav_put_int32_t(buf, 8, lat);
+	_mav_put_int32_t(buf, 12, lon);
+	_mav_put_int32_t(buf, 16, alt);
+	_mav_put_uint32_t(buf, 20, dgps_age);
+	_mav_put_uint16_t(buf, 24, eph);
+	_mav_put_uint16_t(buf, 26, epv);
+	_mav_put_uint16_t(buf, 28, vel);
+	_mav_put_uint16_t(buf, 30, cog);
+	_mav_put_uint8_t(buf, 32, fix_type);
+	_mav_put_uint8_t(buf, 33, satellites_visible);
+	_mav_put_uint8_t(buf, 34, dgps_numch);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, buf, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, buf, MAVLINK_MSG_ID_GPS2_RAW_LEN);
+#endif
+#else
+	mavlink_gps2_raw_t *packet = (mavlink_gps2_raw_t *)msgbuf;
+	packet->time_usec = time_usec;
+	packet->lat = lat;
+	packet->lon = lon;
+	packet->alt = alt;
+	packet->dgps_age = dgps_age;
+	packet->eph = eph;
+	packet->epv = epv;
+	packet->vel = vel;
+	packet->cog = cog;
+	packet->fix_type = fix_type;
+	packet->satellites_visible = satellites_visible;
+	packet->dgps_numch = dgps_numch;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, (const char *)packet, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, (const char *)packet, MAVLINK_MSG_ID_GPS2_RAW_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE GPS2_RAW UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_global_origin.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_global_origin.h
index 016e9cb0e6..1589c8ca59 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_global_origin.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_global_origin.h
@@ -168,6 +168,42 @@ static inline void mavlink_msg_gps_global_origin_send(mavlink_channel_t chan, in
 #endif
 }
 
+#if MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_gps_global_origin_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  int32_t latitude, int32_t longitude, int32_t altitude)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_int32_t(buf, 0, latitude);
+	_mav_put_int32_t(buf, 4, longitude);
+	_mav_put_int32_t(buf, 8, altitude);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN);
+#endif
+#else
+	mavlink_gps_global_origin_t *packet = (mavlink_gps_global_origin_t *)msgbuf;
+	packet->latitude = latitude;
+	packet->longitude = longitude;
+	packet->altitude = altitude;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, (const char *)packet, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, (const char *)packet, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE GPS_GLOBAL_ORIGIN UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_inject_data.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_inject_data.h
index 485d8a4afc..362e2d7b9a 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_inject_data.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_inject_data.h
@@ -173,6 +173,42 @@ static inline void mavlink_msg_gps_inject_data_send(mavlink_channel_t chan, uint
 #endif
 }
 
+#if MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_gps_inject_data_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component, uint8_t len, const uint8_t *data)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint8_t(buf, 0, target_system);
+	_mav_put_uint8_t(buf, 1, target_component);
+	_mav_put_uint8_t(buf, 2, len);
+	_mav_put_uint8_t_array(buf, 3, data, 110);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN);
+#endif
+#else
+	mavlink_gps_inject_data_t *packet = (mavlink_gps_inject_data_t *)msgbuf;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+	packet->len = len;
+	mav_array_memcpy(packet->data, data, sizeof(uint8_t)*110);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, (const char *)packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, (const char *)packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE GPS_INJECT_DATA UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h
index a105f8cdad..b539962069 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h
@@ -245,6 +245,56 @@ static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t
 #endif
 }
 
+#if MAVLINK_MSG_ID_GPS_RAW_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_gps_raw_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint64_t(buf, 0, time_usec);
+	_mav_put_int32_t(buf, 8, lat);
+	_mav_put_int32_t(buf, 12, lon);
+	_mav_put_int32_t(buf, 16, alt);
+	_mav_put_uint16_t(buf, 20, eph);
+	_mav_put_uint16_t(buf, 22, epv);
+	_mav_put_uint16_t(buf, 24, vel);
+	_mav_put_uint16_t(buf, 26, cog);
+	_mav_put_uint8_t(buf, 28, fix_type);
+	_mav_put_uint8_t(buf, 29, satellites_visible);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, buf, MAVLINK_MSG_ID_GPS_RAW_INT_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, buf, MAVLINK_MSG_ID_GPS_RAW_INT_LEN);
+#endif
+#else
+	mavlink_gps_raw_int_t *packet = (mavlink_gps_raw_int_t *)msgbuf;
+	packet->time_usec = time_usec;
+	packet->lat = lat;
+	packet->lon = lon;
+	packet->alt = alt;
+	packet->eph = eph;
+	packet->epv = epv;
+	packet->vel = vel;
+	packet->cog = cog;
+	packet->fix_type = fix_type;
+	packet->satellites_visible = satellites_visible;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, (const char *)packet, MAVLINK_MSG_ID_GPS_RAW_INT_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, (const char *)packet, MAVLINK_MSG_ID_GPS_RAW_INT_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE GPS_RAW_INT UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_status.h
index 28d6b57d19..10659d0dd8 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_status.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_status.h
@@ -199,6 +199,46 @@ static inline void mavlink_msg_gps_status_send(mavlink_channel_t chan, uint8_t s
 #endif
 }
 
+#if MAVLINK_MSG_ID_GPS_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_gps_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t satellites_visible, const uint8_t *satellite_prn, const uint8_t *satellite_used, const uint8_t *satellite_elevation, const uint8_t *satellite_azimuth, const uint8_t *satellite_snr)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint8_t(buf, 0, satellites_visible);
+	_mav_put_uint8_t_array(buf, 1, satellite_prn, 20);
+	_mav_put_uint8_t_array(buf, 21, satellite_used, 20);
+	_mav_put_uint8_t_array(buf, 41, satellite_elevation, 20);
+	_mav_put_uint8_t_array(buf, 61, satellite_azimuth, 20);
+	_mav_put_uint8_t_array(buf, 81, satellite_snr, 20);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, buf, MAVLINK_MSG_ID_GPS_STATUS_LEN, MAVLINK_MSG_ID_GPS_STATUS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, buf, MAVLINK_MSG_ID_GPS_STATUS_LEN);
+#endif
+#else
+	mavlink_gps_status_t *packet = (mavlink_gps_status_t *)msgbuf;
+	packet->satellites_visible = satellites_visible;
+	mav_array_memcpy(packet->satellite_prn, satellite_prn, sizeof(uint8_t)*20);
+	mav_array_memcpy(packet->satellite_used, satellite_used, sizeof(uint8_t)*20);
+	mav_array_memcpy(packet->satellite_elevation, satellite_elevation, sizeof(uint8_t)*20);
+	mav_array_memcpy(packet->satellite_azimuth, satellite_azimuth, sizeof(uint8_t)*20);
+	mav_array_memcpy(packet->satellite_snr, satellite_snr, sizeof(uint8_t)*20);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, (const char *)packet, MAVLINK_MSG_ID_GPS_STATUS_LEN, MAVLINK_MSG_ID_GPS_STATUS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, (const char *)packet, MAVLINK_MSG_ID_GPS_STATUS_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE GPS_STATUS UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h
index 826138fad1..902e381ca8 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h
@@ -198,6 +198,48 @@ static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t ty
 #endif
 }
 
+#if MAVLINK_MSG_ID_HEARTBEAT_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_heartbeat_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t type, uint8_t autopilot, uint8_t base_mode, uint32_t custom_mode, uint8_t system_status)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint32_t(buf, 0, custom_mode);
+	_mav_put_uint8_t(buf, 4, type);
+	_mav_put_uint8_t(buf, 5, autopilot);
+	_mav_put_uint8_t(buf, 6, base_mode);
+	_mav_put_uint8_t(buf, 7, system_status);
+	_mav_put_uint8_t(buf, 8, 3);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, MAVLINK_MSG_ID_HEARTBEAT_LEN);
+#endif
+#else
+	mavlink_heartbeat_t *packet = (mavlink_heartbeat_t *)msgbuf;
+	packet->custom_mode = custom_mode;
+	packet->type = type;
+	packet->autopilot = autopilot;
+	packet->base_mode = base_mode;
+	packet->system_status = system_status;
+	packet->mavlink_version = 3;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)packet, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)packet, MAVLINK_MSG_ID_HEARTBEAT_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE HEARTBEAT UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_highres_imu.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_highres_imu.h
index 0dcd95ed32..2749cb097f 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_highres_imu.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_highres_imu.h
@@ -300,6 +300,66 @@ static inline void mavlink_msg_highres_imu_send(mavlink_channel_t chan, uint64_t
 #endif
 }
 
+#if MAVLINK_MSG_ID_HIGHRES_IMU_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_highres_imu_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint16_t fields_updated)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint64_t(buf, 0, time_usec);
+	_mav_put_float(buf, 8, xacc);
+	_mav_put_float(buf, 12, yacc);
+	_mav_put_float(buf, 16, zacc);
+	_mav_put_float(buf, 20, xgyro);
+	_mav_put_float(buf, 24, ygyro);
+	_mav_put_float(buf, 28, zgyro);
+	_mav_put_float(buf, 32, xmag);
+	_mav_put_float(buf, 36, ymag);
+	_mav_put_float(buf, 40, zmag);
+	_mav_put_float(buf, 44, abs_pressure);
+	_mav_put_float(buf, 48, diff_pressure);
+	_mav_put_float(buf, 52, pressure_alt);
+	_mav_put_float(buf, 56, temperature);
+	_mav_put_uint16_t(buf, 60, fields_updated);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, buf, MAVLINK_MSG_ID_HIGHRES_IMU_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, buf, MAVLINK_MSG_ID_HIGHRES_IMU_LEN);
+#endif
+#else
+	mavlink_highres_imu_t *packet = (mavlink_highres_imu_t *)msgbuf;
+	packet->time_usec = time_usec;
+	packet->xacc = xacc;
+	packet->yacc = yacc;
+	packet->zacc = zacc;
+	packet->xgyro = xgyro;
+	packet->ygyro = ygyro;
+	packet->zgyro = zgyro;
+	packet->xmag = xmag;
+	packet->ymag = ymag;
+	packet->zmag = zmag;
+	packet->abs_pressure = abs_pressure;
+	packet->diff_pressure = diff_pressure;
+	packet->pressure_alt = pressure_alt;
+	packet->temperature = temperature;
+	packet->fields_updated = fields_updated;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, (const char *)packet, MAVLINK_MSG_ID_HIGHRES_IMU_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, (const char *)packet, MAVLINK_MSG_ID_HIGHRES_IMU_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE HIGHRES_IMU UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_controls.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_controls.h
index aed5108d05..f7507b1cf7 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_controls.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_controls.h
@@ -256,6 +256,58 @@ static inline void mavlink_msg_hil_controls_send(mavlink_channel_t chan, uint64_
 #endif
 }
 
+#if MAVLINK_MSG_ID_HIL_CONTROLS_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_hil_controls_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint64_t time_usec, float roll_ailerons, float pitch_elevator, float yaw_rudder, float throttle, float aux1, float aux2, float aux3, float aux4, uint8_t mode, uint8_t nav_mode)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint64_t(buf, 0, time_usec);
+	_mav_put_float(buf, 8, roll_ailerons);
+	_mav_put_float(buf, 12, pitch_elevator);
+	_mav_put_float(buf, 16, yaw_rudder);
+	_mav_put_float(buf, 20, throttle);
+	_mav_put_float(buf, 24, aux1);
+	_mav_put_float(buf, 28, aux2);
+	_mav_put_float(buf, 32, aux3);
+	_mav_put_float(buf, 36, aux4);
+	_mav_put_uint8_t(buf, 40, mode);
+	_mav_put_uint8_t(buf, 41, nav_mode);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, buf, MAVLINK_MSG_ID_HIL_CONTROLS_LEN, MAVLINK_MSG_ID_HIL_CONTROLS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, buf, MAVLINK_MSG_ID_HIL_CONTROLS_LEN);
+#endif
+#else
+	mavlink_hil_controls_t *packet = (mavlink_hil_controls_t *)msgbuf;
+	packet->time_usec = time_usec;
+	packet->roll_ailerons = roll_ailerons;
+	packet->pitch_elevator = pitch_elevator;
+	packet->yaw_rudder = yaw_rudder;
+	packet->throttle = throttle;
+	packet->aux1 = aux1;
+	packet->aux2 = aux2;
+	packet->aux3 = aux3;
+	packet->aux4 = aux4;
+	packet->mode = mode;
+	packet->nav_mode = nav_mode;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, (const char *)packet, MAVLINK_MSG_ID_HIL_CONTROLS_LEN, MAVLINK_MSG_ID_HIL_CONTROLS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, (const char *)packet, MAVLINK_MSG_ID_HIL_CONTROLS_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE HIL_CONTROLS UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h
index 91aec5b08c..a22c0472f0 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h
@@ -278,6 +278,62 @@ static inline void mavlink_msg_hil_gps_send(mavlink_channel_t chan, uint64_t tim
 #endif
 }
 
+#if MAVLINK_MSG_ID_HIL_GPS_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_hil_gps_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, int16_t vn, int16_t ve, int16_t vd, uint16_t cog, uint8_t satellites_visible)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint64_t(buf, 0, time_usec);
+	_mav_put_int32_t(buf, 8, lat);
+	_mav_put_int32_t(buf, 12, lon);
+	_mav_put_int32_t(buf, 16, alt);
+	_mav_put_uint16_t(buf, 20, eph);
+	_mav_put_uint16_t(buf, 22, epv);
+	_mav_put_uint16_t(buf, 24, vel);
+	_mav_put_int16_t(buf, 26, vn);
+	_mav_put_int16_t(buf, 28, ve);
+	_mav_put_int16_t(buf, 30, vd);
+	_mav_put_uint16_t(buf, 32, cog);
+	_mav_put_uint8_t(buf, 34, fix_type);
+	_mav_put_uint8_t(buf, 35, satellites_visible);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, buf, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, buf, MAVLINK_MSG_ID_HIL_GPS_LEN);
+#endif
+#else
+	mavlink_hil_gps_t *packet = (mavlink_hil_gps_t *)msgbuf;
+	packet->time_usec = time_usec;
+	packet->lat = lat;
+	packet->lon = lon;
+	packet->alt = alt;
+	packet->eph = eph;
+	packet->epv = epv;
+	packet->vel = vel;
+	packet->vn = vn;
+	packet->ve = ve;
+	packet->vd = vd;
+	packet->cog = cog;
+	packet->fix_type = fix_type;
+	packet->satellites_visible = satellites_visible;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, (const char *)packet, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, (const char *)packet, MAVLINK_MSG_ID_HIL_GPS_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE HIL_GPS UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h
index acb1392e14..eaadf24359 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h
@@ -223,6 +223,52 @@ static inline void mavlink_msg_hil_optical_flow_send(mavlink_channel_t chan, uin
 #endif
 }
 
+#if MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_hil_optical_flow_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint64_t(buf, 0, time_usec);
+	_mav_put_float(buf, 8, flow_comp_m_x);
+	_mav_put_float(buf, 12, flow_comp_m_y);
+	_mav_put_float(buf, 16, ground_distance);
+	_mav_put_int16_t(buf, 20, flow_x);
+	_mav_put_int16_t(buf, 22, flow_y);
+	_mav_put_uint8_t(buf, 24, sensor_id);
+	_mav_put_uint8_t(buf, 25, quality);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN);
+#endif
+#else
+	mavlink_hil_optical_flow_t *packet = (mavlink_hil_optical_flow_t *)msgbuf;
+	packet->time_usec = time_usec;
+	packet->flow_comp_m_x = flow_comp_m_x;
+	packet->flow_comp_m_y = flow_comp_m_y;
+	packet->ground_distance = ground_distance;
+	packet->flow_x = flow_x;
+	packet->flow_y = flow_y;
+	packet->sensor_id = sensor_id;
+	packet->quality = quality;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, (const char *)packet, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, (const char *)packet, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE HIL_OPTICAL_FLOW UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_rc_inputs_raw.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_rc_inputs_raw.h
index a42bde50b2..227cd9d94e 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_rc_inputs_raw.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_rc_inputs_raw.h
@@ -289,6 +289,64 @@ static inline void mavlink_msg_hil_rc_inputs_raw_send(mavlink_channel_t chan, ui
 #endif
 }
 
+#if MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_hil_rc_inputs_raw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint64_t time_usec, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint8_t rssi)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint64_t(buf, 0, time_usec);
+	_mav_put_uint16_t(buf, 8, chan1_raw);
+	_mav_put_uint16_t(buf, 10, chan2_raw);
+	_mav_put_uint16_t(buf, 12, chan3_raw);
+	_mav_put_uint16_t(buf, 14, chan4_raw);
+	_mav_put_uint16_t(buf, 16, chan5_raw);
+	_mav_put_uint16_t(buf, 18, chan6_raw);
+	_mav_put_uint16_t(buf, 20, chan7_raw);
+	_mav_put_uint16_t(buf, 22, chan8_raw);
+	_mav_put_uint16_t(buf, 24, chan9_raw);
+	_mav_put_uint16_t(buf, 26, chan10_raw);
+	_mav_put_uint16_t(buf, 28, chan11_raw);
+	_mav_put_uint16_t(buf, 30, chan12_raw);
+	_mav_put_uint8_t(buf, 32, rssi);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, buf, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, buf, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN);
+#endif
+#else
+	mavlink_hil_rc_inputs_raw_t *packet = (mavlink_hil_rc_inputs_raw_t *)msgbuf;
+	packet->time_usec = time_usec;
+	packet->chan1_raw = chan1_raw;
+	packet->chan2_raw = chan2_raw;
+	packet->chan3_raw = chan3_raw;
+	packet->chan4_raw = chan4_raw;
+	packet->chan5_raw = chan5_raw;
+	packet->chan6_raw = chan6_raw;
+	packet->chan7_raw = chan7_raw;
+	packet->chan8_raw = chan8_raw;
+	packet->chan9_raw = chan9_raw;
+	packet->chan10_raw = chan10_raw;
+	packet->chan11_raw = chan11_raw;
+	packet->chan12_raw = chan12_raw;
+	packet->rssi = rssi;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, (const char *)packet, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, (const char *)packet, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE HIL_RC_INPUTS_RAW UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_sensor.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_sensor.h
index 6c2667473d..8672f8b8a3 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_sensor.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_sensor.h
@@ -300,6 +300,66 @@ static inline void mavlink_msg_hil_sensor_send(mavlink_channel_t chan, uint64_t
 #endif
 }
 
+#if MAVLINK_MSG_ID_HIL_SENSOR_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_hil_sensor_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint32_t fields_updated)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint64_t(buf, 0, time_usec);
+	_mav_put_float(buf, 8, xacc);
+	_mav_put_float(buf, 12, yacc);
+	_mav_put_float(buf, 16, zacc);
+	_mav_put_float(buf, 20, xgyro);
+	_mav_put_float(buf, 24, ygyro);
+	_mav_put_float(buf, 28, zgyro);
+	_mav_put_float(buf, 32, xmag);
+	_mav_put_float(buf, 36, ymag);
+	_mav_put_float(buf, 40, zmag);
+	_mav_put_float(buf, 44, abs_pressure);
+	_mav_put_float(buf, 48, diff_pressure);
+	_mav_put_float(buf, 52, pressure_alt);
+	_mav_put_float(buf, 56, temperature);
+	_mav_put_uint32_t(buf, 60, fields_updated);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, buf, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, buf, MAVLINK_MSG_ID_HIL_SENSOR_LEN);
+#endif
+#else
+	mavlink_hil_sensor_t *packet = (mavlink_hil_sensor_t *)msgbuf;
+	packet->time_usec = time_usec;
+	packet->xacc = xacc;
+	packet->yacc = yacc;
+	packet->zacc = zacc;
+	packet->xgyro = xgyro;
+	packet->ygyro = ygyro;
+	packet->zgyro = zgyro;
+	packet->xmag = xmag;
+	packet->ymag = ymag;
+	packet->zmag = zmag;
+	packet->abs_pressure = abs_pressure;
+	packet->diff_pressure = diff_pressure;
+	packet->pressure_alt = pressure_alt;
+	packet->temperature = temperature;
+	packet->fields_updated = fields_updated;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, (const char *)packet, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, (const char *)packet, MAVLINK_MSG_ID_HIL_SENSOR_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE HIL_SENSOR UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state.h
index bcc8577670..923ed60b95 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state.h
@@ -311,6 +311,68 @@ static inline void mavlink_msg_hil_state_send(mavlink_channel_t chan, uint64_t t
 #endif
 }
 
+#if MAVLINK_MSG_ID_HIL_STATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_hil_state_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint64_t time_usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint64_t(buf, 0, time_usec);
+	_mav_put_float(buf, 8, roll);
+	_mav_put_float(buf, 12, pitch);
+	_mav_put_float(buf, 16, yaw);
+	_mav_put_float(buf, 20, rollspeed);
+	_mav_put_float(buf, 24, pitchspeed);
+	_mav_put_float(buf, 28, yawspeed);
+	_mav_put_int32_t(buf, 32, lat);
+	_mav_put_int32_t(buf, 36, lon);
+	_mav_put_int32_t(buf, 40, alt);
+	_mav_put_int16_t(buf, 44, vx);
+	_mav_put_int16_t(buf, 46, vy);
+	_mav_put_int16_t(buf, 48, vz);
+	_mav_put_int16_t(buf, 50, xacc);
+	_mav_put_int16_t(buf, 52, yacc);
+	_mav_put_int16_t(buf, 54, zacc);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, buf, MAVLINK_MSG_ID_HIL_STATE_LEN, MAVLINK_MSG_ID_HIL_STATE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, buf, MAVLINK_MSG_ID_HIL_STATE_LEN);
+#endif
+#else
+	mavlink_hil_state_t *packet = (mavlink_hil_state_t *)msgbuf;
+	packet->time_usec = time_usec;
+	packet->roll = roll;
+	packet->pitch = pitch;
+	packet->yaw = yaw;
+	packet->rollspeed = rollspeed;
+	packet->pitchspeed = pitchspeed;
+	packet->yawspeed = yawspeed;
+	packet->lat = lat;
+	packet->lon = lon;
+	packet->alt = alt;
+	packet->vx = vx;
+	packet->vy = vy;
+	packet->vz = vz;
+	packet->xacc = xacc;
+	packet->yacc = yacc;
+	packet->zacc = zacc;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, (const char *)packet, MAVLINK_MSG_ID_HIL_STATE_LEN, MAVLINK_MSG_ID_HIL_STATE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, (const char *)packet, MAVLINK_MSG_ID_HIL_STATE_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE HIL_STATE UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h
index 732176193e..1a028bdf99 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h
@@ -305,6 +305,66 @@ static inline void mavlink_msg_hil_state_quaternion_send(mavlink_channel_t chan,
 #endif
 }
 
+#if MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_hil_state_quaternion_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint64_t time_usec, const float *attitude_quaternion, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, uint16_t ind_airspeed, uint16_t true_airspeed, int16_t xacc, int16_t yacc, int16_t zacc)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint64_t(buf, 0, time_usec);
+	_mav_put_float(buf, 24, rollspeed);
+	_mav_put_float(buf, 28, pitchspeed);
+	_mav_put_float(buf, 32, yawspeed);
+	_mav_put_int32_t(buf, 36, lat);
+	_mav_put_int32_t(buf, 40, lon);
+	_mav_put_int32_t(buf, 44, alt);
+	_mav_put_int16_t(buf, 48, vx);
+	_mav_put_int16_t(buf, 50, vy);
+	_mav_put_int16_t(buf, 52, vz);
+	_mav_put_uint16_t(buf, 54, ind_airspeed);
+	_mav_put_uint16_t(buf, 56, true_airspeed);
+	_mav_put_int16_t(buf, 58, xacc);
+	_mav_put_int16_t(buf, 60, yacc);
+	_mav_put_int16_t(buf, 62, zacc);
+	_mav_put_float_array(buf, 8, attitude_quaternion, 4);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, buf, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, buf, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN);
+#endif
+#else
+	mavlink_hil_state_quaternion_t *packet = (mavlink_hil_state_quaternion_t *)msgbuf;
+	packet->time_usec = time_usec;
+	packet->rollspeed = rollspeed;
+	packet->pitchspeed = pitchspeed;
+	packet->yawspeed = yawspeed;
+	packet->lat = lat;
+	packet->lon = lon;
+	packet->alt = alt;
+	packet->vx = vx;
+	packet->vy = vy;
+	packet->vz = vz;
+	packet->ind_airspeed = ind_airspeed;
+	packet->true_airspeed = true_airspeed;
+	packet->xacc = xacc;
+	packet->yacc = yacc;
+	packet->zacc = zacc;
+	mav_array_memcpy(packet->attitude_quaternion, attitude_quaternion, sizeof(float)*4);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, (const char *)packet, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, (const char *)packet, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE HIL_STATE_QUATERNION UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned.h
index a0b72c0e1e..e18a948695 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned.h
@@ -212,6 +212,50 @@ static inline void mavlink_msg_local_position_ned_send(mavlink_channel_t chan, u
 #endif
 }
 
+#if MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_local_position_ned_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint32_t time_boot_ms, float x, float y, float z, float vx, float vy, float vz)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint32_t(buf, 0, time_boot_ms);
+	_mav_put_float(buf, 4, x);
+	_mav_put_float(buf, 8, y);
+	_mav_put_float(buf, 12, z);
+	_mav_put_float(buf, 16, vx);
+	_mav_put_float(buf, 20, vy);
+	_mav_put_float(buf, 24, vz);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN);
+#endif
+#else
+	mavlink_local_position_ned_t *packet = (mavlink_local_position_ned_t *)msgbuf;
+	packet->time_boot_ms = time_boot_ms;
+	packet->x = x;
+	packet->y = y;
+	packet->z = z;
+	packet->vx = vx;
+	packet->vy = vy;
+	packet->vz = vz;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE LOCAL_POSITION_NED UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_system_global_offset.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_system_global_offset.h
index 8c46862027..af7d195b01 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_system_global_offset.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_system_global_offset.h
@@ -212,6 +212,50 @@ static inline void mavlink_msg_local_position_ned_system_global_offset_send(mavl
 #endif
 }
 
+#if MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_local_position_ned_system_global_offset_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint32_t time_boot_ms, float x, float y, float z, float roll, float pitch, float yaw)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint32_t(buf, 0, time_boot_ms);
+	_mav_put_float(buf, 4, x);
+	_mav_put_float(buf, 8, y);
+	_mav_put_float(buf, 12, z);
+	_mav_put_float(buf, 16, roll);
+	_mav_put_float(buf, 20, pitch);
+	_mav_put_float(buf, 24, yaw);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN);
+#endif
+#else
+	mavlink_local_position_ned_system_global_offset_t *packet = (mavlink_local_position_ned_system_global_offset_t *)msgbuf;
+	packet->time_boot_ms = time_boot_ms;
+	packet->x = x;
+	packet->y = y;
+	packet->z = z;
+	packet->roll = roll;
+	packet->pitch = pitch;
+	packet->yaw = yaw;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_setpoint.h
index 1794815f8e..8dae721beb 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_setpoint.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_setpoint.h
@@ -190,6 +190,46 @@ static inline void mavlink_msg_local_position_setpoint_send(mavlink_channel_t ch
 #endif
 }
 
+#if MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_local_position_setpoint_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t coordinate_frame, float x, float y, float z, float yaw)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, x);
+	_mav_put_float(buf, 4, y);
+	_mav_put_float(buf, 8, z);
+	_mav_put_float(buf, 12, yaw);
+	_mav_put_uint8_t(buf, 16, coordinate_frame);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, buf, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, buf, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN);
+#endif
+#else
+	mavlink_local_position_setpoint_t *packet = (mavlink_local_position_setpoint_t *)msgbuf;
+	packet->x = x;
+	packet->y = y;
+	packet->z = z;
+	packet->yaw = yaw;
+	packet->coordinate_frame = coordinate_frame;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE LOCAL_POSITION_SETPOINT UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_data.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_data.h
index 1cf5d15e48..48641f3eb0 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_data.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_data.h
@@ -173,6 +173,42 @@ static inline void mavlink_msg_log_data_send(mavlink_channel_t chan, uint16_t id
 #endif
 }
 
+#if MAVLINK_MSG_ID_LOG_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_log_data_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint16_t id, uint32_t ofs, uint8_t count, const uint8_t *data)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint32_t(buf, 0, ofs);
+	_mav_put_uint16_t(buf, 4, id);
+	_mav_put_uint8_t(buf, 6, count);
+	_mav_put_uint8_t_array(buf, 7, data, 90);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, buf, MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, buf, MAVLINK_MSG_ID_LOG_DATA_LEN);
+#endif
+#else
+	mavlink_log_data_t *packet = (mavlink_log_data_t *)msgbuf;
+	packet->ofs = ofs;
+	packet->id = id;
+	packet->count = count;
+	mav_array_memcpy(packet->data, data, sizeof(uint8_t)*90);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, (const char *)packet, MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, (const char *)packet, MAVLINK_MSG_ID_LOG_DATA_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE LOG_DATA UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_entry.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_entry.h
index 681d8f07cd..8ecaec3ae2 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_entry.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_entry.h
@@ -190,6 +190,46 @@ static inline void mavlink_msg_log_entry_send(mavlink_channel_t chan, uint16_t i
 #endif
 }
 
+#if MAVLINK_MSG_ID_LOG_ENTRY_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_log_entry_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint16_t id, uint16_t num_logs, uint16_t last_log_num, uint32_t time_utc, uint32_t size)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint32_t(buf, 0, time_utc);
+	_mav_put_uint32_t(buf, 4, size);
+	_mav_put_uint16_t(buf, 8, id);
+	_mav_put_uint16_t(buf, 10, num_logs);
+	_mav_put_uint16_t(buf, 12, last_log_num);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, buf, MAVLINK_MSG_ID_LOG_ENTRY_LEN, MAVLINK_MSG_ID_LOG_ENTRY_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, buf, MAVLINK_MSG_ID_LOG_ENTRY_LEN);
+#endif
+#else
+	mavlink_log_entry_t *packet = (mavlink_log_entry_t *)msgbuf;
+	packet->time_utc = time_utc;
+	packet->size = size;
+	packet->id = id;
+	packet->num_logs = num_logs;
+	packet->last_log_num = last_log_num;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, (const char *)packet, MAVLINK_MSG_ID_LOG_ENTRY_LEN, MAVLINK_MSG_ID_LOG_ENTRY_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, (const char *)packet, MAVLINK_MSG_ID_LOG_ENTRY_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE LOG_ENTRY UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_erase.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_erase.h
index feafeaf164..957c4d4cc0 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_erase.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_erase.h
@@ -157,6 +157,40 @@ static inline void mavlink_msg_log_erase_send(mavlink_channel_t chan, uint8_t ta
 #endif
 }
 
+#if MAVLINK_MSG_ID_LOG_ERASE_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_log_erase_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint8_t(buf, 0, target_system);
+	_mav_put_uint8_t(buf, 1, target_component);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, buf, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, buf, MAVLINK_MSG_ID_LOG_ERASE_LEN);
+#endif
+#else
+	mavlink_log_erase_t *packet = (mavlink_log_erase_t *)msgbuf;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, (const char *)packet, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, (const char *)packet, MAVLINK_MSG_ID_LOG_ERASE_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE LOG_ERASE UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_data.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_data.h
index 5be9eea471..ef5cbb67cb 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_data.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_data.h
@@ -190,6 +190,46 @@ static inline void mavlink_msg_log_request_data_send(mavlink_channel_t chan, uin
 #endif
 }
 
+#if MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_log_request_data_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component, uint16_t id, uint32_t ofs, uint32_t count)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint32_t(buf, 0, ofs);
+	_mav_put_uint32_t(buf, 4, count);
+	_mav_put_uint16_t(buf, 8, id);
+	_mav_put_uint8_t(buf, 10, target_system);
+	_mav_put_uint8_t(buf, 11, target_component);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, buf, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN, MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, buf, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN);
+#endif
+#else
+	mavlink_log_request_data_t *packet = (mavlink_log_request_data_t *)msgbuf;
+	packet->ofs = ofs;
+	packet->count = count;
+	packet->id = id;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, (const char *)packet, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN, MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, (const char *)packet, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE LOG_REQUEST_DATA UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_end.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_end.h
index 48a5a03b4c..23fcca29f7 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_end.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_end.h
@@ -157,6 +157,40 @@ static inline void mavlink_msg_log_request_end_send(mavlink_channel_t chan, uint
 #endif
 }
 
+#if MAVLINK_MSG_ID_LOG_REQUEST_END_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_log_request_end_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint8_t(buf, 0, target_system);
+	_mav_put_uint8_t(buf, 1, target_component);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, buf, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN, MAVLINK_MSG_ID_LOG_REQUEST_END_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, buf, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN);
+#endif
+#else
+	mavlink_log_request_end_t *packet = (mavlink_log_request_end_t *)msgbuf;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, (const char *)packet, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN, MAVLINK_MSG_ID_LOG_REQUEST_END_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, (const char *)packet, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE LOG_REQUEST_END UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_list.h
index 7a5b25c171..e511b53125 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_list.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_list.h
@@ -179,6 +179,44 @@ static inline void mavlink_msg_log_request_list_send(mavlink_channel_t chan, uin
 #endif
 }
 
+#if MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_log_request_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component, uint16_t start, uint16_t end)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint16_t(buf, 0, start);
+	_mav_put_uint16_t(buf, 2, end);
+	_mav_put_uint8_t(buf, 4, target_system);
+	_mav_put_uint8_t(buf, 5, target_component);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, buf, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN, MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, buf, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN);
+#endif
+#else
+	mavlink_log_request_list_t *packet = (mavlink_log_request_list_t *)msgbuf;
+	packet->start = start;
+	packet->end = end;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN, MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE LOG_REQUEST_LIST UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_control.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_control.h
index 6b6e9e148a..e93b759d0a 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_control.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_control.h
@@ -201,6 +201,48 @@ static inline void mavlink_msg_manual_control_send(mavlink_channel_t chan, uint8
 #endif
 }
 
+#if MAVLINK_MSG_ID_MANUAL_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_manual_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target, int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_int16_t(buf, 0, x);
+	_mav_put_int16_t(buf, 2, y);
+	_mav_put_int16_t(buf, 4, z);
+	_mav_put_int16_t(buf, 6, r);
+	_mav_put_uint16_t(buf, 8, buttons);
+	_mav_put_uint8_t(buf, 10, target);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, buf, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, buf, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN);
+#endif
+#else
+	mavlink_manual_control_t *packet = (mavlink_manual_control_t *)msgbuf;
+	packet->x = x;
+	packet->y = y;
+	packet->z = z;
+	packet->r = r;
+	packet->buttons = buttons;
+	packet->target = target;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, (const char *)packet, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, (const char *)packet, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE MANUAL_CONTROL UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_setpoint.h
index a694947c12..b276267269 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_setpoint.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_setpoint.h
@@ -212,6 +212,50 @@ static inline void mavlink_msg_manual_setpoint_send(mavlink_channel_t chan, uint
 #endif
 }
 
+#if MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_manual_setpoint_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint32_t time_boot_ms, float roll, float pitch, float yaw, float thrust, uint8_t mode_switch, uint8_t manual_override_switch)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint32_t(buf, 0, time_boot_ms);
+	_mav_put_float(buf, 4, roll);
+	_mav_put_float(buf, 8, pitch);
+	_mav_put_float(buf, 12, yaw);
+	_mav_put_float(buf, 16, thrust);
+	_mav_put_uint8_t(buf, 20, mode_switch);
+	_mav_put_uint8_t(buf, 21, manual_override_switch);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_SETPOINT, buf, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN, MAVLINK_MSG_ID_MANUAL_SETPOINT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_SETPOINT, buf, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN);
+#endif
+#else
+	mavlink_manual_setpoint_t *packet = (mavlink_manual_setpoint_t *)msgbuf;
+	packet->time_boot_ms = time_boot_ms;
+	packet->roll = roll;
+	packet->pitch = pitch;
+	packet->yaw = yaw;
+	packet->thrust = thrust;
+	packet->mode_switch = mode_switch;
+	packet->manual_override_switch = manual_override_switch;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN, MAVLINK_MSG_ID_MANUAL_SETPOINT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE MANUAL_SETPOINT UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_memory_vect.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_memory_vect.h
index 5f79329c25..2eb60cc0e6 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_memory_vect.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_memory_vect.h
@@ -173,6 +173,42 @@ static inline void mavlink_msg_memory_vect_send(mavlink_channel_t chan, uint16_t
 #endif
 }
 
+#if MAVLINK_MSG_ID_MEMORY_VECT_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_memory_vect_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint16_t address, uint8_t ver, uint8_t type, const int8_t *value)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint16_t(buf, 0, address);
+	_mav_put_uint8_t(buf, 2, ver);
+	_mav_put_uint8_t(buf, 3, type);
+	_mav_put_int8_t_array(buf, 4, value, 32);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, buf, MAVLINK_MSG_ID_MEMORY_VECT_LEN, MAVLINK_MSG_ID_MEMORY_VECT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, buf, MAVLINK_MSG_ID_MEMORY_VECT_LEN);
+#endif
+#else
+	mavlink_memory_vect_t *packet = (mavlink_memory_vect_t *)msgbuf;
+	packet->address = address;
+	packet->ver = ver;
+	packet->type = type;
+	mav_array_memcpy(packet->value, value, sizeof(int8_t)*32);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, (const char *)packet, MAVLINK_MSG_ID_MEMORY_VECT_LEN, MAVLINK_MSG_ID_MEMORY_VECT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, (const char *)packet, MAVLINK_MSG_ID_MEMORY_VECT_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE MEMORY_VECT UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_ack.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_ack.h
index 7421d8394a..43afd00f7e 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_ack.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_ack.h
@@ -168,6 +168,42 @@ static inline void mavlink_msg_mission_ack_send(mavlink_channel_t chan, uint8_t
 #endif
 }
 
+#if MAVLINK_MSG_ID_MISSION_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_mission_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component, uint8_t type)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint8_t(buf, 0, target_system);
+	_mav_put_uint8_t(buf, 1, target_component);
+	_mav_put_uint8_t(buf, 2, type);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, buf, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, buf, MAVLINK_MSG_ID_MISSION_ACK_LEN);
+#endif
+#else
+	mavlink_mission_ack_t *packet = (mavlink_mission_ack_t *)msgbuf;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+	packet->type = type;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, (const char *)packet, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, (const char *)packet, MAVLINK_MSG_ID_MISSION_ACK_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE MISSION_ACK UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_clear_all.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_clear_all.h
index 8f441c8e5c..120986873f 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_clear_all.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_clear_all.h
@@ -157,6 +157,40 @@ static inline void mavlink_msg_mission_clear_all_send(mavlink_channel_t chan, ui
 #endif
 }
 
+#if MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_mission_clear_all_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint8_t(buf, 0, target_system);
+	_mav_put_uint8_t(buf, 1, target_component);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, buf, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, buf, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN);
+#endif
+#else
+	mavlink_mission_clear_all_t *packet = (mavlink_mission_clear_all_t *)msgbuf;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, (const char *)packet, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, (const char *)packet, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE MISSION_CLEAR_ALL UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_count.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_count.h
index eac7793067..7e4748eaef 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_count.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_count.h
@@ -168,6 +168,42 @@ static inline void mavlink_msg_mission_count_send(mavlink_channel_t chan, uint8_
 #endif
 }
 
+#if MAVLINK_MSG_ID_MISSION_COUNT_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_mission_count_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component, uint16_t count)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint16_t(buf, 0, count);
+	_mav_put_uint8_t(buf, 2, target_system);
+	_mav_put_uint8_t(buf, 3, target_component);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, buf, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, buf, MAVLINK_MSG_ID_MISSION_COUNT_LEN);
+#endif
+#else
+	mavlink_mission_count_t *packet = (mavlink_mission_count_t *)msgbuf;
+	packet->count = count;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, (const char *)packet, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, (const char *)packet, MAVLINK_MSG_ID_MISSION_COUNT_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE MISSION_COUNT UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_current.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_current.h
index dbcdbd3f9c..201b7a6fb4 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_current.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_current.h
@@ -146,6 +146,38 @@ static inline void mavlink_msg_mission_current_send(mavlink_channel_t chan, uint
 #endif
 }
 
+#if MAVLINK_MSG_ID_MISSION_CURRENT_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_mission_current_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint16_t seq)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint16_t(buf, 0, seq);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, buf, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, buf, MAVLINK_MSG_ID_MISSION_CURRENT_LEN);
+#endif
+#else
+	mavlink_mission_current_t *packet = (mavlink_mission_current_t *)msgbuf;
+	packet->seq = seq;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, (const char *)packet, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, (const char *)packet, MAVLINK_MSG_ID_MISSION_CURRENT_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE MISSION_CURRENT UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h
index a8d60eca7e..ef9394f00c 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h
@@ -289,6 +289,64 @@ static inline void mavlink_msg_mission_item_send(mavlink_channel_t chan, uint8_t
 #endif
 }
 
+#if MAVLINK_MSG_ID_MISSION_ITEM_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_mission_item_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, param1);
+	_mav_put_float(buf, 4, param2);
+	_mav_put_float(buf, 8, param3);
+	_mav_put_float(buf, 12, param4);
+	_mav_put_float(buf, 16, x);
+	_mav_put_float(buf, 20, y);
+	_mav_put_float(buf, 24, z);
+	_mav_put_uint16_t(buf, 28, seq);
+	_mav_put_uint16_t(buf, 30, command);
+	_mav_put_uint8_t(buf, 32, target_system);
+	_mav_put_uint8_t(buf, 33, target_component);
+	_mav_put_uint8_t(buf, 34, frame);
+	_mav_put_uint8_t(buf, 35, current);
+	_mav_put_uint8_t(buf, 36, autocontinue);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, buf, MAVLINK_MSG_ID_MISSION_ITEM_LEN, MAVLINK_MSG_ID_MISSION_ITEM_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, buf, MAVLINK_MSG_ID_MISSION_ITEM_LEN);
+#endif
+#else
+	mavlink_mission_item_t *packet = (mavlink_mission_item_t *)msgbuf;
+	packet->param1 = param1;
+	packet->param2 = param2;
+	packet->param3 = param3;
+	packet->param4 = param4;
+	packet->x = x;
+	packet->y = y;
+	packet->z = z;
+	packet->seq = seq;
+	packet->command = command;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+	packet->frame = frame;
+	packet->current = current;
+	packet->autocontinue = autocontinue;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, (const char *)packet, MAVLINK_MSG_ID_MISSION_ITEM_LEN, MAVLINK_MSG_ID_MISSION_ITEM_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, (const char *)packet, MAVLINK_MSG_ID_MISSION_ITEM_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE MISSION_ITEM UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_reached.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_reached.h
index f3744fde6c..9dfa280433 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_reached.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_reached.h
@@ -146,6 +146,38 @@ static inline void mavlink_msg_mission_item_reached_send(mavlink_channel_t chan,
 #endif
 }
 
+#if MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_mission_item_reached_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint16_t seq)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint16_t(buf, 0, seq);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, buf, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, buf, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN);
+#endif
+#else
+	mavlink_mission_item_reached_t *packet = (mavlink_mission_item_reached_t *)msgbuf;
+	packet->seq = seq;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, (const char *)packet, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, (const char *)packet, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE MISSION_ITEM_REACHED UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request.h
index ac84e35540..29b0ef6ef5 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request.h
@@ -168,6 +168,42 @@ static inline void mavlink_msg_mission_request_send(mavlink_channel_t chan, uint
 #endif
 }
 
+#if MAVLINK_MSG_ID_MISSION_REQUEST_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_mission_request_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component, uint16_t seq)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint16_t(buf, 0, seq);
+	_mav_put_uint8_t(buf, 2, target_system);
+	_mav_put_uint8_t(buf, 3, target_component);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LEN);
+#endif
+#else
+	mavlink_mission_request_t *packet = (mavlink_mission_request_t *)msgbuf;
+	packet->seq = seq;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE MISSION_REQUEST UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_list.h
index d999babdbd..a275348acc 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_list.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_list.h
@@ -157,6 +157,40 @@ static inline void mavlink_msg_mission_request_list_send(mavlink_channel_t chan,
 #endif
 }
 
+#if MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_mission_request_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint8_t(buf, 0, target_system);
+	_mav_put_uint8_t(buf, 1, target_component);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN);
+#endif
+#else
+	mavlink_mission_request_list_t *packet = (mavlink_mission_request_list_t *)msgbuf;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE MISSION_REQUEST_LIST UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_partial_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_partial_list.h
index 35c7e12856..79a88dc084 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_partial_list.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_partial_list.h
@@ -179,6 +179,44 @@ static inline void mavlink_msg_mission_request_partial_list_send(mavlink_channel
 #endif
 }
 
+#if MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_mission_request_partial_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_int16_t(buf, 0, start_index);
+	_mav_put_int16_t(buf, 2, end_index);
+	_mav_put_uint8_t(buf, 4, target_system);
+	_mav_put_uint8_t(buf, 5, target_component);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN);
+#endif
+#else
+	mavlink_mission_request_partial_list_t *packet = (mavlink_mission_request_partial_list_t *)msgbuf;
+	packet->start_index = start_index;
+	packet->end_index = end_index;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE MISSION_REQUEST_PARTIAL_LIST UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_set_current.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_set_current.h
index 63b37cf8c8..0c2a3ada4e 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_set_current.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_set_current.h
@@ -168,6 +168,42 @@ static inline void mavlink_msg_mission_set_current_send(mavlink_channel_t chan,
 #endif
 }
 
+#if MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_mission_set_current_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component, uint16_t seq)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint16_t(buf, 0, seq);
+	_mav_put_uint8_t(buf, 2, target_system);
+	_mav_put_uint8_t(buf, 3, target_component);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT, buf, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_SET_CURRENT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT, buf, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN);
+#endif
+#else
+	mavlink_mission_set_current_t *packet = (mavlink_mission_set_current_t *)msgbuf;
+	packet->seq = seq;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT, (const char *)packet, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_SET_CURRENT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT, (const char *)packet, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE MISSION_SET_CURRENT UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_write_partial_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_write_partial_list.h
index 7de38bd7b5..17a5a6bdd8 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_write_partial_list.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_write_partial_list.h
@@ -179,6 +179,44 @@ static inline void mavlink_msg_mission_write_partial_list_send(mavlink_channel_t
 #endif
 }
 
+#if MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_mission_write_partial_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_int16_t(buf, 0, start_index);
+	_mav_put_int16_t(buf, 2, end_index);
+	_mav_put_uint8_t(buf, 4, target_system);
+	_mav_put_uint8_t(buf, 5, target_component);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN);
+#endif
+#else
+	mavlink_mission_write_partial_list_t *packet = (mavlink_mission_write_partial_list_t *)msgbuf;
+	packet->start_index = start_index;
+	packet->end_index = end_index;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE MISSION_WRITE_PARTIAL_LIST UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_float.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_float.h
index fbf4f75c95..df82704a4b 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_float.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_float.h
@@ -162,6 +162,40 @@ static inline void mavlink_msg_named_value_float_send(mavlink_channel_t chan, ui
 #endif
 }
 
+#if MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_named_value_float_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint32_t time_boot_ms, const char *name, float value)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint32_t(buf, 0, time_boot_ms);
+	_mav_put_float(buf, 4, value);
+	_mav_put_char_array(buf, 8, name, 10);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, buf, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, buf, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN);
+#endif
+#else
+	mavlink_named_value_float_t *packet = (mavlink_named_value_float_t *)msgbuf;
+	packet->time_boot_ms = time_boot_ms;
+	packet->value = value;
+	mav_array_memcpy(packet->name, name, sizeof(char)*10);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, (const char *)packet, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, (const char *)packet, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE NAMED_VALUE_FLOAT UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_int.h
index 052f247935..cfdd73d61e 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_int.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_int.h
@@ -162,6 +162,40 @@ static inline void mavlink_msg_named_value_int_send(mavlink_channel_t chan, uint
 #endif
 }
 
+#if MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_named_value_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint32_t time_boot_ms, const char *name, int32_t value)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint32_t(buf, 0, time_boot_ms);
+	_mav_put_int32_t(buf, 4, value);
+	_mav_put_char_array(buf, 8, name, 10);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, buf, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_INT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, buf, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN);
+#endif
+#else
+	mavlink_named_value_int_t *packet = (mavlink_named_value_int_t *)msgbuf;
+	packet->time_boot_ms = time_boot_ms;
+	packet->value = value;
+	mav_array_memcpy(packet->name, name, sizeof(char)*10);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, (const char *)packet, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_INT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, (const char *)packet, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE NAMED_VALUE_INT UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_nav_controller_output.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_nav_controller_output.h
index e95c144de7..b9a748b222 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_nav_controller_output.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_nav_controller_output.h
@@ -223,6 +223,52 @@ static inline void mavlink_msg_nav_controller_output_send(mavlink_channel_t chan
 #endif
 }
 
+#if MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_nav_controller_output_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  float nav_roll, float nav_pitch, int16_t nav_bearing, int16_t target_bearing, uint16_t wp_dist, float alt_error, float aspd_error, float xtrack_error)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, nav_roll);
+	_mav_put_float(buf, 4, nav_pitch);
+	_mav_put_float(buf, 8, alt_error);
+	_mav_put_float(buf, 12, aspd_error);
+	_mav_put_float(buf, 16, xtrack_error);
+	_mav_put_int16_t(buf, 20, nav_bearing);
+	_mav_put_int16_t(buf, 22, target_bearing);
+	_mav_put_uint16_t(buf, 24, wp_dist);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, buf, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, buf, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN);
+#endif
+#else
+	mavlink_nav_controller_output_t *packet = (mavlink_nav_controller_output_t *)msgbuf;
+	packet->nav_roll = nav_roll;
+	packet->nav_pitch = nav_pitch;
+	packet->alt_error = alt_error;
+	packet->aspd_error = aspd_error;
+	packet->xtrack_error = xtrack_error;
+	packet->nav_bearing = nav_bearing;
+	packet->target_bearing = target_bearing;
+	packet->wp_dist = wp_dist;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, (const char *)packet, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, (const char *)packet, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE NAV_CONTROLLER_OUTPUT UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_omnidirectional_flow.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_omnidirectional_flow.h
index 4debb6e663..4ee9c452fb 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_omnidirectional_flow.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_omnidirectional_flow.h
@@ -196,6 +196,46 @@ static inline void mavlink_msg_omnidirectional_flow_send(mavlink_channel_t chan,
 #endif
 }
 
+#if MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_omnidirectional_flow_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint64_t time_usec, uint8_t sensor_id, const int16_t *left, const int16_t *right, uint8_t quality, float front_distance_m)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint64_t(buf, 0, time_usec);
+	_mav_put_float(buf, 8, front_distance_m);
+	_mav_put_uint8_t(buf, 52, sensor_id);
+	_mav_put_uint8_t(buf, 53, quality);
+	_mav_put_int16_t_array(buf, 12, left, 10);
+	_mav_put_int16_t_array(buf, 32, right, 10);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW, buf, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW, buf, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN);
+#endif
+#else
+	mavlink_omnidirectional_flow_t *packet = (mavlink_omnidirectional_flow_t *)msgbuf;
+	packet->time_usec = time_usec;
+	packet->front_distance_m = front_distance_m;
+	packet->sensor_id = sensor_id;
+	packet->quality = quality;
+	mav_array_memcpy(packet->left, left, sizeof(int16_t)*10);
+	mav_array_memcpy(packet->right, right, sizeof(int16_t)*10);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW, (const char *)packet, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW, (const char *)packet, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE OMNIDIRECTIONAL_FLOW UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow.h
index cf6db9147e..6a2efc664f 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow.h
@@ -223,6 +223,52 @@ static inline void mavlink_msg_optical_flow_send(mavlink_channel_t chan, uint64_
 #endif
 }
 
+#if MAVLINK_MSG_ID_OPTICAL_FLOW_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_optical_flow_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint64_t(buf, 0, time_usec);
+	_mav_put_float(buf, 8, flow_comp_m_x);
+	_mav_put_float(buf, 12, flow_comp_m_y);
+	_mav_put_float(buf, 16, ground_distance);
+	_mav_put_int16_t(buf, 20, flow_x);
+	_mav_put_int16_t(buf, 22, flow_y);
+	_mav_put_uint8_t(buf, 24, sensor_id);
+	_mav_put_uint8_t(buf, 25, quality);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN);
+#endif
+#else
+	mavlink_optical_flow_t *packet = (mavlink_optical_flow_t *)msgbuf;
+	packet->time_usec = time_usec;
+	packet->flow_comp_m_x = flow_comp_m_x;
+	packet->flow_comp_m_y = flow_comp_m_y;
+	packet->ground_distance = ground_distance;
+	packet->flow_x = flow_x;
+	packet->flow_y = flow_y;
+	packet->sensor_id = sensor_id;
+	packet->quality = quality;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, (const char *)packet, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, (const char *)packet, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE OPTICAL_FLOW UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_list.h
index 39e0072741..f9466b002e 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_list.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_list.h
@@ -157,6 +157,40 @@ static inline void mavlink_msg_param_request_list_send(mavlink_channel_t chan, u
 #endif
 }
 
+#if MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_param_request_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint8_t(buf, 0, target_system);
+	_mav_put_uint8_t(buf, 1, target_component);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, buf, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, buf, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN);
+#endif
+#else
+	mavlink_param_request_list_t *packet = (mavlink_param_request_list_t *)msgbuf;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE PARAM_REQUEST_LIST UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_read.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_read.h
index 5d9113114b..730cff0669 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_read.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_read.h
@@ -173,6 +173,42 @@ static inline void mavlink_msg_param_request_read_send(mavlink_channel_t chan, u
 #endif
 }
 
+#if MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_param_request_read_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component, const char *param_id, int16_t param_index)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_int16_t(buf, 0, param_index);
+	_mav_put_uint8_t(buf, 2, target_system);
+	_mav_put_uint8_t(buf, 3, target_component);
+	_mav_put_char_array(buf, 4, param_id, 16);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, buf, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, buf, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN);
+#endif
+#else
+	mavlink_param_request_read_t *packet = (mavlink_param_request_read_t *)msgbuf;
+	packet->param_index = param_index;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+	mav_array_memcpy(packet->param_id, param_id, sizeof(char)*16);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, (const char *)packet, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, (const char *)packet, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE PARAM_REQUEST_READ UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_set.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_set.h
index 1bd1f00596..f669af1a25 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_set.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_set.h
@@ -184,6 +184,44 @@ static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t ta
 #endif
 }
 
+#if MAVLINK_MSG_ID_PARAM_SET_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_param_set_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component, const char *param_id, float param_value, uint8_t param_type)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, param_value);
+	_mav_put_uint8_t(buf, 4, target_system);
+	_mav_put_uint8_t(buf, 5, target_component);
+	_mav_put_uint8_t(buf, 22, param_type);
+	_mav_put_char_array(buf, 6, param_id, 16);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, buf, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, buf, MAVLINK_MSG_ID_PARAM_SET_LEN);
+#endif
+#else
+	mavlink_param_set_t *packet = (mavlink_param_set_t *)msgbuf;
+	packet->param_value = param_value;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+	packet->param_type = param_type;
+	mav_array_memcpy(packet->param_id, param_id, sizeof(char)*16);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, (const char *)packet, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, (const char *)packet, MAVLINK_MSG_ID_PARAM_SET_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE PARAM_SET UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_value.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_value.h
index 17c759811f..c27957913f 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_value.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_value.h
@@ -184,6 +184,44 @@ static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const ch
 #endif
 }
 
+#if MAVLINK_MSG_ID_PARAM_VALUE_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_param_value_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  const char *param_id, float param_value, uint8_t param_type, uint16_t param_count, uint16_t param_index)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, param_value);
+	_mav_put_uint16_t(buf, 4, param_count);
+	_mav_put_uint16_t(buf, 6, param_index);
+	_mav_put_uint8_t(buf, 24, param_type);
+	_mav_put_char_array(buf, 8, param_id, 16);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, buf, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, buf, MAVLINK_MSG_ID_PARAM_VALUE_LEN);
+#endif
+#else
+	mavlink_param_value_t *packet = (mavlink_param_value_t *)msgbuf;
+	packet->param_value = param_value;
+	packet->param_count = param_count;
+	packet->param_index = param_index;
+	packet->param_type = param_type;
+	mav_array_memcpy(packet->param_id, param_id, sizeof(char)*16);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, (const char *)packet, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, (const char *)packet, MAVLINK_MSG_ID_PARAM_VALUE_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE PARAM_VALUE UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h
index 0c68ca1765..b1501ba1f8 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h
@@ -179,6 +179,44 @@ static inline void mavlink_msg_ping_send(mavlink_channel_t chan, uint64_t time_u
 #endif
 }
 
+#if MAVLINK_MSG_ID_PING_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_ping_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint64_t time_usec, uint32_t seq, uint8_t target_system, uint8_t target_component)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint64_t(buf, 0, time_usec);
+	_mav_put_uint32_t(buf, 8, seq);
+	_mav_put_uint8_t(buf, 12, target_system);
+	_mav_put_uint8_t(buf, 13, target_component);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, buf, MAVLINK_MSG_ID_PING_LEN, MAVLINK_MSG_ID_PING_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, buf, MAVLINK_MSG_ID_PING_LEN);
+#endif
+#else
+	mavlink_ping_t *packet = (mavlink_ping_t *)msgbuf;
+	packet->time_usec = time_usec;
+	packet->seq = seq;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, (const char *)packet, MAVLINK_MSG_ID_PING_LEN, MAVLINK_MSG_ID_PING_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, (const char *)packet, MAVLINK_MSG_ID_PING_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE PING UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_power_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_power_status.h
new file mode 100644
index 0000000000..1a6259aa3d
--- /dev/null
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_power_status.h
@@ -0,0 +1,257 @@
+// MESSAGE POWER_STATUS PACKING
+
+#define MAVLINK_MSG_ID_POWER_STATUS 125
+
+typedef struct __mavlink_power_status_t
+{
+ uint16_t Vcc; ///< 5V rail voltage in millivolts
+ uint16_t Vservo; ///< servo rail voltage in millivolts
+ uint16_t flags; ///< power supply status flags (see MAV_POWER_STATUS enum)
+} mavlink_power_status_t;
+
+#define MAVLINK_MSG_ID_POWER_STATUS_LEN 6
+#define MAVLINK_MSG_ID_125_LEN 6
+
+#define MAVLINK_MSG_ID_POWER_STATUS_CRC 203
+#define MAVLINK_MSG_ID_125_CRC 203
+
+
+
+#define MAVLINK_MESSAGE_INFO_POWER_STATUS { \
+	"POWER_STATUS", \
+	3, \
+	{  { "Vcc", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_power_status_t, Vcc) }, \
+         { "Vservo", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_power_status_t, Vservo) }, \
+         { "flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_power_status_t, flags) }, \
+         } \
+}
+
+
+/**
+ * @brief Pack a power_status message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ *
+ * @param Vcc 5V rail voltage in millivolts
+ * @param Vservo servo rail voltage in millivolts
+ * @param flags power supply status flags (see MAV_POWER_STATUS enum)
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_power_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
+						       uint16_t Vcc, uint16_t Vservo, uint16_t flags)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char buf[MAVLINK_MSG_ID_POWER_STATUS_LEN];
+	_mav_put_uint16_t(buf, 0, Vcc);
+	_mav_put_uint16_t(buf, 2, Vservo);
+	_mav_put_uint16_t(buf, 4, flags);
+
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POWER_STATUS_LEN);
+#else
+	mavlink_power_status_t packet;
+	packet.Vcc = Vcc;
+	packet.Vservo = Vservo;
+	packet.flags = flags;
+
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POWER_STATUS_LEN);
+#endif
+
+	msg->msgid = MAVLINK_MSG_ID_POWER_STATUS;
+#if MAVLINK_CRC_EXTRA
+    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC);
+#else
+    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POWER_STATUS_LEN);
+#endif
+}
+
+/**
+ * @brief Pack a power_status message on a channel
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message will be sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param Vcc 5V rail voltage in millivolts
+ * @param Vservo servo rail voltage in millivolts
+ * @param flags power supply status flags (see MAV_POWER_STATUS enum)
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_power_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
+							   mavlink_message_t* msg,
+						           uint16_t Vcc,uint16_t Vservo,uint16_t flags)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char buf[MAVLINK_MSG_ID_POWER_STATUS_LEN];
+	_mav_put_uint16_t(buf, 0, Vcc);
+	_mav_put_uint16_t(buf, 2, Vservo);
+	_mav_put_uint16_t(buf, 4, flags);
+
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POWER_STATUS_LEN);
+#else
+	mavlink_power_status_t packet;
+	packet.Vcc = Vcc;
+	packet.Vservo = Vservo;
+	packet.flags = flags;
+
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POWER_STATUS_LEN);
+#endif
+
+	msg->msgid = MAVLINK_MSG_ID_POWER_STATUS;
+#if MAVLINK_CRC_EXTRA
+    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC);
+#else
+    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POWER_STATUS_LEN);
+#endif
+}
+
+/**
+ * @brief Encode a power_status struct
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ * @param power_status C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_power_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_power_status_t* power_status)
+{
+	return mavlink_msg_power_status_pack(system_id, component_id, msg, power_status->Vcc, power_status->Vservo, power_status->flags);
+}
+
+/**
+ * @brief Encode a power_status struct on a channel
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message will be sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param power_status C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_power_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_power_status_t* power_status)
+{
+	return mavlink_msg_power_status_pack_chan(system_id, component_id, chan, msg, power_status->Vcc, power_status->Vservo, power_status->flags);
+}
+
+/**
+ * @brief Send a power_status message
+ * @param chan MAVLink channel to send the message
+ *
+ * @param Vcc 5V rail voltage in millivolts
+ * @param Vservo servo rail voltage in millivolts
+ * @param flags power supply status flags (see MAV_POWER_STATUS enum)
+ */
+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
+
+static inline void mavlink_msg_power_status_send(mavlink_channel_t chan, uint16_t Vcc, uint16_t Vservo, uint16_t flags)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char buf[MAVLINK_MSG_ID_POWER_STATUS_LEN];
+	_mav_put_uint16_t(buf, 0, Vcc);
+	_mav_put_uint16_t(buf, 2, Vservo);
+	_mav_put_uint16_t(buf, 4, flags);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, buf, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, buf, MAVLINK_MSG_ID_POWER_STATUS_LEN);
+#endif
+#else
+	mavlink_power_status_t packet;
+	packet.Vcc = Vcc;
+	packet.Vservo = Vservo;
+	packet.flags = flags;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, (const char *)&packet, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, (const char *)&packet, MAVLINK_MSG_ID_POWER_STATUS_LEN);
+#endif
+#endif
+}
+
+#if MAVLINK_MSG_ID_POWER_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_power_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint16_t Vcc, uint16_t Vservo, uint16_t flags)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint16_t(buf, 0, Vcc);
+	_mav_put_uint16_t(buf, 2, Vservo);
+	_mav_put_uint16_t(buf, 4, flags);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, buf, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, buf, MAVLINK_MSG_ID_POWER_STATUS_LEN);
+#endif
+#else
+	mavlink_power_status_t *packet = (mavlink_power_status_t *)msgbuf;
+	packet->Vcc = Vcc;
+	packet->Vservo = Vservo;
+	packet->flags = flags;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, (const char *)packet, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, (const char *)packet, MAVLINK_MSG_ID_POWER_STATUS_LEN);
+#endif
+#endif
+}
+#endif
+
+#endif
+
+// MESSAGE POWER_STATUS UNPACKING
+
+
+/**
+ * @brief Get field Vcc from power_status message
+ *
+ * @return 5V rail voltage in millivolts
+ */
+static inline uint16_t mavlink_msg_power_status_get_Vcc(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint16_t(msg,  0);
+}
+
+/**
+ * @brief Get field Vservo from power_status message
+ *
+ * @return servo rail voltage in millivolts
+ */
+static inline uint16_t mavlink_msg_power_status_get_Vservo(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint16_t(msg,  2);
+}
+
+/**
+ * @brief Get field flags from power_status message
+ *
+ * @return power supply status flags (see MAV_POWER_STATUS enum)
+ */
+static inline uint16_t mavlink_msg_power_status_get_flags(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint16_t(msg,  4);
+}
+
+/**
+ * @brief Decode a power_status message into a struct
+ *
+ * @param msg The message to decode
+ * @param power_status C-struct to decode the message contents into
+ */
+static inline void mavlink_msg_power_status_decode(const mavlink_message_t* msg, mavlink_power_status_t* power_status)
+{
+#if MAVLINK_NEED_BYTE_SWAP
+	power_status->Vcc = mavlink_msg_power_status_get_Vcc(msg);
+	power_status->Vservo = mavlink_msg_power_status_get_Vservo(msg);
+	power_status->flags = mavlink_msg_power_status_get_flags(msg);
+#else
+	memcpy(power_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_POWER_STATUS_LEN);
+#endif
+}
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_radio_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_radio_status.h
index fceb7d1688..5763008fe2 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_radio_status.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_radio_status.h
@@ -212,6 +212,50 @@ static inline void mavlink_msg_radio_status_send(mavlink_channel_t chan, uint8_t
 #endif
 }
 
+#if MAVLINK_MSG_ID_RADIO_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_radio_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t rssi, uint8_t remrssi, uint8_t txbuf, uint8_t noise, uint8_t remnoise, uint16_t rxerrors, uint16_t fixed)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint16_t(buf, 0, rxerrors);
+	_mav_put_uint16_t(buf, 2, fixed);
+	_mav_put_uint8_t(buf, 4, rssi);
+	_mav_put_uint8_t(buf, 5, remrssi);
+	_mav_put_uint8_t(buf, 6, txbuf);
+	_mav_put_uint8_t(buf, 7, noise);
+	_mav_put_uint8_t(buf, 8, remnoise);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_STATUS, buf, MAVLINK_MSG_ID_RADIO_STATUS_LEN, MAVLINK_MSG_ID_RADIO_STATUS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_STATUS, buf, MAVLINK_MSG_ID_RADIO_STATUS_LEN);
+#endif
+#else
+	mavlink_radio_status_t *packet = (mavlink_radio_status_t *)msgbuf;
+	packet->rxerrors = rxerrors;
+	packet->fixed = fixed;
+	packet->rssi = rssi;
+	packet->remrssi = remrssi;
+	packet->txbuf = txbuf;
+	packet->noise = noise;
+	packet->remnoise = remnoise;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_STATUS, (const char *)packet, MAVLINK_MSG_ID_RADIO_STATUS_LEN, MAVLINK_MSG_ID_RADIO_STATUS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_STATUS, (const char *)packet, MAVLINK_MSG_ID_RADIO_STATUS_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE RADIO_STATUS UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_imu.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_imu.h
index 62a9b6eeae..a98e8ce721 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_imu.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_imu.h
@@ -245,6 +245,56 @@ static inline void mavlink_msg_raw_imu_send(mavlink_channel_t chan, uint64_t tim
 #endif
 }
 
+#if MAVLINK_MSG_ID_RAW_IMU_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_raw_imu_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint64_t time_usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint64_t(buf, 0, time_usec);
+	_mav_put_int16_t(buf, 8, xacc);
+	_mav_put_int16_t(buf, 10, yacc);
+	_mav_put_int16_t(buf, 12, zacc);
+	_mav_put_int16_t(buf, 14, xgyro);
+	_mav_put_int16_t(buf, 16, ygyro);
+	_mav_put_int16_t(buf, 18, zgyro);
+	_mav_put_int16_t(buf, 20, xmag);
+	_mav_put_int16_t(buf, 22, ymag);
+	_mav_put_int16_t(buf, 24, zmag);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, buf, MAVLINK_MSG_ID_RAW_IMU_LEN, MAVLINK_MSG_ID_RAW_IMU_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, buf, MAVLINK_MSG_ID_RAW_IMU_LEN);
+#endif
+#else
+	mavlink_raw_imu_t *packet = (mavlink_raw_imu_t *)msgbuf;
+	packet->time_usec = time_usec;
+	packet->xacc = xacc;
+	packet->yacc = yacc;
+	packet->zacc = zacc;
+	packet->xgyro = xgyro;
+	packet->ygyro = ygyro;
+	packet->zgyro = zgyro;
+	packet->xmag = xmag;
+	packet->ymag = ymag;
+	packet->zmag = zmag;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, (const char *)packet, MAVLINK_MSG_ID_RAW_IMU_LEN, MAVLINK_MSG_ID_RAW_IMU_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, (const char *)packet, MAVLINK_MSG_ID_RAW_IMU_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE RAW_IMU UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_pressure.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_pressure.h
index 82c5fad4ab..6bd37fcaee 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_pressure.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_pressure.h
@@ -190,6 +190,46 @@ static inline void mavlink_msg_raw_pressure_send(mavlink_channel_t chan, uint64_
 #endif
 }
 
+#if MAVLINK_MSG_ID_RAW_PRESSURE_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_raw_pressure_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint64_t time_usec, int16_t press_abs, int16_t press_diff1, int16_t press_diff2, int16_t temperature)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint64_t(buf, 0, time_usec);
+	_mav_put_int16_t(buf, 8, press_abs);
+	_mav_put_int16_t(buf, 10, press_diff1);
+	_mav_put_int16_t(buf, 12, press_diff2);
+	_mav_put_int16_t(buf, 14, temperature);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, buf, MAVLINK_MSG_ID_RAW_PRESSURE_LEN, MAVLINK_MSG_ID_RAW_PRESSURE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, buf, MAVLINK_MSG_ID_RAW_PRESSURE_LEN);
+#endif
+#else
+	mavlink_raw_pressure_t *packet = (mavlink_raw_pressure_t *)msgbuf;
+	packet->time_usec = time_usec;
+	packet->press_abs = press_abs;
+	packet->press_diff1 = press_diff1;
+	packet->press_diff2 = press_diff2;
+	packet->temperature = temperature;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, (const char *)packet, MAVLINK_MSG_ID_RAW_PRESSURE_LEN, MAVLINK_MSG_ID_RAW_PRESSURE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, (const char *)packet, MAVLINK_MSG_ID_RAW_PRESSURE_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE RAW_PRESSURE UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels.h
new file mode 100644
index 0000000000..479af122ff
--- /dev/null
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels.h
@@ -0,0 +1,689 @@
+// MESSAGE RC_CHANNELS PACKING
+
+#define MAVLINK_MSG_ID_RC_CHANNELS 65
+
+typedef struct __mavlink_rc_channels_t
+{
+ uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot)
+ uint16_t chan1_raw; ///< RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ uint16_t chan2_raw; ///< RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ uint16_t chan3_raw; ///< RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ uint16_t chan4_raw; ///< RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ uint16_t chan5_raw; ///< RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ uint16_t chan6_raw; ///< RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ uint16_t chan7_raw; ///< RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ uint16_t chan8_raw; ///< RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ uint16_t chan9_raw; ///< RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ uint16_t chan10_raw; ///< RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ uint16_t chan11_raw; ///< RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ uint16_t chan12_raw; ///< RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ uint16_t chan13_raw; ///< RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ uint16_t chan14_raw; ///< RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ uint16_t chan15_raw; ///< RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ uint16_t chan16_raw; ///< RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ uint16_t chan17_raw; ///< RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ uint16_t chan18_raw; ///< RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ uint8_t chancount; ///< Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available.
+ uint8_t rssi; ///< Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown.
+} mavlink_rc_channels_t;
+
+#define MAVLINK_MSG_ID_RC_CHANNELS_LEN 42
+#define MAVLINK_MSG_ID_65_LEN 42
+
+#define MAVLINK_MSG_ID_RC_CHANNELS_CRC 118
+#define MAVLINK_MSG_ID_65_CRC 118
+
+
+
+#define MAVLINK_MESSAGE_INFO_RC_CHANNELS { \
+	"RC_CHANNELS", \
+	21, \
+	{  { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_rc_channels_t, time_boot_ms) }, \
+         { "chan1_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_rc_channels_t, chan1_raw) }, \
+         { "chan2_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_rc_channels_t, chan2_raw) }, \
+         { "chan3_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_rc_channels_t, chan3_raw) }, \
+         { "chan4_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_rc_channels_t, chan4_raw) }, \
+         { "chan5_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_rc_channels_t, chan5_raw) }, \
+         { "chan6_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_rc_channels_t, chan6_raw) }, \
+         { "chan7_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_rc_channels_t, chan7_raw) }, \
+         { "chan8_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_rc_channels_t, chan8_raw) }, \
+         { "chan9_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_rc_channels_t, chan9_raw) }, \
+         { "chan10_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 22, offsetof(mavlink_rc_channels_t, chan10_raw) }, \
+         { "chan11_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_rc_channels_t, chan11_raw) }, \
+         { "chan12_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_rc_channels_t, chan12_raw) }, \
+         { "chan13_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_rc_channels_t, chan13_raw) }, \
+         { "chan14_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 30, offsetof(mavlink_rc_channels_t, chan14_raw) }, \
+         { "chan15_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 32, offsetof(mavlink_rc_channels_t, chan15_raw) }, \
+         { "chan16_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 34, offsetof(mavlink_rc_channels_t, chan16_raw) }, \
+         { "chan17_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 36, offsetof(mavlink_rc_channels_t, chan17_raw) }, \
+         { "chan18_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 38, offsetof(mavlink_rc_channels_t, chan18_raw) }, \
+         { "chancount", NULL, MAVLINK_TYPE_UINT8_T, 0, 40, offsetof(mavlink_rc_channels_t, chancount) }, \
+         { "rssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 41, offsetof(mavlink_rc_channels_t, rssi) }, \
+         } \
+}
+
+
+/**
+ * @brief Pack a rc_channels message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ *
+ * @param time_boot_ms Timestamp (milliseconds since system boot)
+ * @param chancount Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available.
+ * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan9_raw RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan10_raw RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan11_raw RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan12_raw RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan13_raw RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan14_raw RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan15_raw RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan16_raw RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan17_raw RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan18_raw RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown.
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_rc_channels_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
+						       uint32_t time_boot_ms, uint8_t chancount, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint16_t chan13_raw, uint16_t chan14_raw, uint16_t chan15_raw, uint16_t chan16_raw, uint16_t chan17_raw, uint16_t chan18_raw, uint8_t rssi)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char buf[MAVLINK_MSG_ID_RC_CHANNELS_LEN];
+	_mav_put_uint32_t(buf, 0, time_boot_ms);
+	_mav_put_uint16_t(buf, 4, chan1_raw);
+	_mav_put_uint16_t(buf, 6, chan2_raw);
+	_mav_put_uint16_t(buf, 8, chan3_raw);
+	_mav_put_uint16_t(buf, 10, chan4_raw);
+	_mav_put_uint16_t(buf, 12, chan5_raw);
+	_mav_put_uint16_t(buf, 14, chan6_raw);
+	_mav_put_uint16_t(buf, 16, chan7_raw);
+	_mav_put_uint16_t(buf, 18, chan8_raw);
+	_mav_put_uint16_t(buf, 20, chan9_raw);
+	_mav_put_uint16_t(buf, 22, chan10_raw);
+	_mav_put_uint16_t(buf, 24, chan11_raw);
+	_mav_put_uint16_t(buf, 26, chan12_raw);
+	_mav_put_uint16_t(buf, 28, chan13_raw);
+	_mav_put_uint16_t(buf, 30, chan14_raw);
+	_mav_put_uint16_t(buf, 32, chan15_raw);
+	_mav_put_uint16_t(buf, 34, chan16_raw);
+	_mav_put_uint16_t(buf, 36, chan17_raw);
+	_mav_put_uint16_t(buf, 38, chan18_raw);
+	_mav_put_uint8_t(buf, 40, chancount);
+	_mav_put_uint8_t(buf, 41, rssi);
+
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN);
+#else
+	mavlink_rc_channels_t packet;
+	packet.time_boot_ms = time_boot_ms;
+	packet.chan1_raw = chan1_raw;
+	packet.chan2_raw = chan2_raw;
+	packet.chan3_raw = chan3_raw;
+	packet.chan4_raw = chan4_raw;
+	packet.chan5_raw = chan5_raw;
+	packet.chan6_raw = chan6_raw;
+	packet.chan7_raw = chan7_raw;
+	packet.chan8_raw = chan8_raw;
+	packet.chan9_raw = chan9_raw;
+	packet.chan10_raw = chan10_raw;
+	packet.chan11_raw = chan11_raw;
+	packet.chan12_raw = chan12_raw;
+	packet.chan13_raw = chan13_raw;
+	packet.chan14_raw = chan14_raw;
+	packet.chan15_raw = chan15_raw;
+	packet.chan16_raw = chan16_raw;
+	packet.chan17_raw = chan17_raw;
+	packet.chan18_raw = chan18_raw;
+	packet.chancount = chancount;
+	packet.rssi = rssi;
+
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN);
+#endif
+
+	msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS;
+#if MAVLINK_CRC_EXTRA
+    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC);
+#else
+    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_LEN);
+#endif
+}
+
+/**
+ * @brief Pack a rc_channels message on a channel
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message will be sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param time_boot_ms Timestamp (milliseconds since system boot)
+ * @param chancount Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available.
+ * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan9_raw RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan10_raw RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan11_raw RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan12_raw RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan13_raw RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan14_raw RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan15_raw RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan16_raw RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan17_raw RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan18_raw RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown.
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_rc_channels_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
+							   mavlink_message_t* msg,
+						           uint32_t time_boot_ms,uint8_t chancount,uint16_t chan1_raw,uint16_t chan2_raw,uint16_t chan3_raw,uint16_t chan4_raw,uint16_t chan5_raw,uint16_t chan6_raw,uint16_t chan7_raw,uint16_t chan8_raw,uint16_t chan9_raw,uint16_t chan10_raw,uint16_t chan11_raw,uint16_t chan12_raw,uint16_t chan13_raw,uint16_t chan14_raw,uint16_t chan15_raw,uint16_t chan16_raw,uint16_t chan17_raw,uint16_t chan18_raw,uint8_t rssi)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char buf[MAVLINK_MSG_ID_RC_CHANNELS_LEN];
+	_mav_put_uint32_t(buf, 0, time_boot_ms);
+	_mav_put_uint16_t(buf, 4, chan1_raw);
+	_mav_put_uint16_t(buf, 6, chan2_raw);
+	_mav_put_uint16_t(buf, 8, chan3_raw);
+	_mav_put_uint16_t(buf, 10, chan4_raw);
+	_mav_put_uint16_t(buf, 12, chan5_raw);
+	_mav_put_uint16_t(buf, 14, chan6_raw);
+	_mav_put_uint16_t(buf, 16, chan7_raw);
+	_mav_put_uint16_t(buf, 18, chan8_raw);
+	_mav_put_uint16_t(buf, 20, chan9_raw);
+	_mav_put_uint16_t(buf, 22, chan10_raw);
+	_mav_put_uint16_t(buf, 24, chan11_raw);
+	_mav_put_uint16_t(buf, 26, chan12_raw);
+	_mav_put_uint16_t(buf, 28, chan13_raw);
+	_mav_put_uint16_t(buf, 30, chan14_raw);
+	_mav_put_uint16_t(buf, 32, chan15_raw);
+	_mav_put_uint16_t(buf, 34, chan16_raw);
+	_mav_put_uint16_t(buf, 36, chan17_raw);
+	_mav_put_uint16_t(buf, 38, chan18_raw);
+	_mav_put_uint8_t(buf, 40, chancount);
+	_mav_put_uint8_t(buf, 41, rssi);
+
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN);
+#else
+	mavlink_rc_channels_t packet;
+	packet.time_boot_ms = time_boot_ms;
+	packet.chan1_raw = chan1_raw;
+	packet.chan2_raw = chan2_raw;
+	packet.chan3_raw = chan3_raw;
+	packet.chan4_raw = chan4_raw;
+	packet.chan5_raw = chan5_raw;
+	packet.chan6_raw = chan6_raw;
+	packet.chan7_raw = chan7_raw;
+	packet.chan8_raw = chan8_raw;
+	packet.chan9_raw = chan9_raw;
+	packet.chan10_raw = chan10_raw;
+	packet.chan11_raw = chan11_raw;
+	packet.chan12_raw = chan12_raw;
+	packet.chan13_raw = chan13_raw;
+	packet.chan14_raw = chan14_raw;
+	packet.chan15_raw = chan15_raw;
+	packet.chan16_raw = chan16_raw;
+	packet.chan17_raw = chan17_raw;
+	packet.chan18_raw = chan18_raw;
+	packet.chancount = chancount;
+	packet.rssi = rssi;
+
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN);
+#endif
+
+	msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS;
+#if MAVLINK_CRC_EXTRA
+    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC);
+#else
+    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_LEN);
+#endif
+}
+
+/**
+ * @brief Encode a rc_channels struct
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ * @param rc_channels C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_rc_channels_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rc_channels_t* rc_channels)
+{
+	return mavlink_msg_rc_channels_pack(system_id, component_id, msg, rc_channels->time_boot_ms, rc_channels->chancount, rc_channels->chan1_raw, rc_channels->chan2_raw, rc_channels->chan3_raw, rc_channels->chan4_raw, rc_channels->chan5_raw, rc_channels->chan6_raw, rc_channels->chan7_raw, rc_channels->chan8_raw, rc_channels->chan9_raw, rc_channels->chan10_raw, rc_channels->chan11_raw, rc_channels->chan12_raw, rc_channels->chan13_raw, rc_channels->chan14_raw, rc_channels->chan15_raw, rc_channels->chan16_raw, rc_channels->chan17_raw, rc_channels->chan18_raw, rc_channels->rssi);
+}
+
+/**
+ * @brief Encode a rc_channels struct on a channel
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message will be sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param rc_channels C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_rc_channels_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rc_channels_t* rc_channels)
+{
+	return mavlink_msg_rc_channels_pack_chan(system_id, component_id, chan, msg, rc_channels->time_boot_ms, rc_channels->chancount, rc_channels->chan1_raw, rc_channels->chan2_raw, rc_channels->chan3_raw, rc_channels->chan4_raw, rc_channels->chan5_raw, rc_channels->chan6_raw, rc_channels->chan7_raw, rc_channels->chan8_raw, rc_channels->chan9_raw, rc_channels->chan10_raw, rc_channels->chan11_raw, rc_channels->chan12_raw, rc_channels->chan13_raw, rc_channels->chan14_raw, rc_channels->chan15_raw, rc_channels->chan16_raw, rc_channels->chan17_raw, rc_channels->chan18_raw, rc_channels->rssi);
+}
+
+/**
+ * @brief Send a rc_channels message
+ * @param chan MAVLink channel to send the message
+ *
+ * @param time_boot_ms Timestamp (milliseconds since system boot)
+ * @param chancount Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available.
+ * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan9_raw RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan10_raw RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan11_raw RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan12_raw RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan13_raw RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan14_raw RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan15_raw RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan16_raw RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan17_raw RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param chan18_raw RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown.
+ */
+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
+
+static inline void mavlink_msg_rc_channels_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t chancount, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint16_t chan13_raw, uint16_t chan14_raw, uint16_t chan15_raw, uint16_t chan16_raw, uint16_t chan17_raw, uint16_t chan18_raw, uint8_t rssi)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char buf[MAVLINK_MSG_ID_RC_CHANNELS_LEN];
+	_mav_put_uint32_t(buf, 0, time_boot_ms);
+	_mav_put_uint16_t(buf, 4, chan1_raw);
+	_mav_put_uint16_t(buf, 6, chan2_raw);
+	_mav_put_uint16_t(buf, 8, chan3_raw);
+	_mav_put_uint16_t(buf, 10, chan4_raw);
+	_mav_put_uint16_t(buf, 12, chan5_raw);
+	_mav_put_uint16_t(buf, 14, chan6_raw);
+	_mav_put_uint16_t(buf, 16, chan7_raw);
+	_mav_put_uint16_t(buf, 18, chan8_raw);
+	_mav_put_uint16_t(buf, 20, chan9_raw);
+	_mav_put_uint16_t(buf, 22, chan10_raw);
+	_mav_put_uint16_t(buf, 24, chan11_raw);
+	_mav_put_uint16_t(buf, 26, chan12_raw);
+	_mav_put_uint16_t(buf, 28, chan13_raw);
+	_mav_put_uint16_t(buf, 30, chan14_raw);
+	_mav_put_uint16_t(buf, 32, chan15_raw);
+	_mav_put_uint16_t(buf, 34, chan16_raw);
+	_mav_put_uint16_t(buf, 36, chan17_raw);
+	_mav_put_uint16_t(buf, 38, chan18_raw);
+	_mav_put_uint8_t(buf, 40, chancount);
+	_mav_put_uint8_t(buf, 41, rssi);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN);
+#endif
+#else
+	mavlink_rc_channels_t packet;
+	packet.time_boot_ms = time_boot_ms;
+	packet.chan1_raw = chan1_raw;
+	packet.chan2_raw = chan2_raw;
+	packet.chan3_raw = chan3_raw;
+	packet.chan4_raw = chan4_raw;
+	packet.chan5_raw = chan5_raw;
+	packet.chan6_raw = chan6_raw;
+	packet.chan7_raw = chan7_raw;
+	packet.chan8_raw = chan8_raw;
+	packet.chan9_raw = chan9_raw;
+	packet.chan10_raw = chan10_raw;
+	packet.chan11_raw = chan11_raw;
+	packet.chan12_raw = chan12_raw;
+	packet.chan13_raw = chan13_raw;
+	packet.chan14_raw = chan14_raw;
+	packet.chan15_raw = chan15_raw;
+	packet.chan16_raw = chan16_raw;
+	packet.chan17_raw = chan17_raw;
+	packet.chan18_raw = chan18_raw;
+	packet.chancount = chancount;
+	packet.rssi = rssi;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, (const char *)&packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, (const char *)&packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN);
+#endif
+#endif
+}
+
+#if MAVLINK_MSG_ID_RC_CHANNELS_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_rc_channels_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint32_t time_boot_ms, uint8_t chancount, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint16_t chan13_raw, uint16_t chan14_raw, uint16_t chan15_raw, uint16_t chan16_raw, uint16_t chan17_raw, uint16_t chan18_raw, uint8_t rssi)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint32_t(buf, 0, time_boot_ms);
+	_mav_put_uint16_t(buf, 4, chan1_raw);
+	_mav_put_uint16_t(buf, 6, chan2_raw);
+	_mav_put_uint16_t(buf, 8, chan3_raw);
+	_mav_put_uint16_t(buf, 10, chan4_raw);
+	_mav_put_uint16_t(buf, 12, chan5_raw);
+	_mav_put_uint16_t(buf, 14, chan6_raw);
+	_mav_put_uint16_t(buf, 16, chan7_raw);
+	_mav_put_uint16_t(buf, 18, chan8_raw);
+	_mav_put_uint16_t(buf, 20, chan9_raw);
+	_mav_put_uint16_t(buf, 22, chan10_raw);
+	_mav_put_uint16_t(buf, 24, chan11_raw);
+	_mav_put_uint16_t(buf, 26, chan12_raw);
+	_mav_put_uint16_t(buf, 28, chan13_raw);
+	_mav_put_uint16_t(buf, 30, chan14_raw);
+	_mav_put_uint16_t(buf, 32, chan15_raw);
+	_mav_put_uint16_t(buf, 34, chan16_raw);
+	_mav_put_uint16_t(buf, 36, chan17_raw);
+	_mav_put_uint16_t(buf, 38, chan18_raw);
+	_mav_put_uint8_t(buf, 40, chancount);
+	_mav_put_uint8_t(buf, 41, rssi);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN);
+#endif
+#else
+	mavlink_rc_channels_t *packet = (mavlink_rc_channels_t *)msgbuf;
+	packet->time_boot_ms = time_boot_ms;
+	packet->chan1_raw = chan1_raw;
+	packet->chan2_raw = chan2_raw;
+	packet->chan3_raw = chan3_raw;
+	packet->chan4_raw = chan4_raw;
+	packet->chan5_raw = chan5_raw;
+	packet->chan6_raw = chan6_raw;
+	packet->chan7_raw = chan7_raw;
+	packet->chan8_raw = chan8_raw;
+	packet->chan9_raw = chan9_raw;
+	packet->chan10_raw = chan10_raw;
+	packet->chan11_raw = chan11_raw;
+	packet->chan12_raw = chan12_raw;
+	packet->chan13_raw = chan13_raw;
+	packet->chan14_raw = chan14_raw;
+	packet->chan15_raw = chan15_raw;
+	packet->chan16_raw = chan16_raw;
+	packet->chan17_raw = chan17_raw;
+	packet->chan18_raw = chan18_raw;
+	packet->chancount = chancount;
+	packet->rssi = rssi;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN);
+#endif
+#endif
+}
+#endif
+
+#endif
+
+// MESSAGE RC_CHANNELS UNPACKING
+
+
+/**
+ * @brief Get field time_boot_ms from rc_channels message
+ *
+ * @return Timestamp (milliseconds since system boot)
+ */
+static inline uint32_t mavlink_msg_rc_channels_get_time_boot_ms(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint32_t(msg,  0);
+}
+
+/**
+ * @brief Get field chancount from rc_channels message
+ *
+ * @return Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available.
+ */
+static inline uint8_t mavlink_msg_rc_channels_get_chancount(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint8_t(msg,  40);
+}
+
+/**
+ * @brief Get field chan1_raw from rc_channels message
+ *
+ * @return RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ */
+static inline uint16_t mavlink_msg_rc_channels_get_chan1_raw(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint16_t(msg,  4);
+}
+
+/**
+ * @brief Get field chan2_raw from rc_channels message
+ *
+ * @return RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ */
+static inline uint16_t mavlink_msg_rc_channels_get_chan2_raw(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint16_t(msg,  6);
+}
+
+/**
+ * @brief Get field chan3_raw from rc_channels message
+ *
+ * @return RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ */
+static inline uint16_t mavlink_msg_rc_channels_get_chan3_raw(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint16_t(msg,  8);
+}
+
+/**
+ * @brief Get field chan4_raw from rc_channels message
+ *
+ * @return RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ */
+static inline uint16_t mavlink_msg_rc_channels_get_chan4_raw(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint16_t(msg,  10);
+}
+
+/**
+ * @brief Get field chan5_raw from rc_channels message
+ *
+ * @return RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ */
+static inline uint16_t mavlink_msg_rc_channels_get_chan5_raw(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint16_t(msg,  12);
+}
+
+/**
+ * @brief Get field chan6_raw from rc_channels message
+ *
+ * @return RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ */
+static inline uint16_t mavlink_msg_rc_channels_get_chan6_raw(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint16_t(msg,  14);
+}
+
+/**
+ * @brief Get field chan7_raw from rc_channels message
+ *
+ * @return RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ */
+static inline uint16_t mavlink_msg_rc_channels_get_chan7_raw(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint16_t(msg,  16);
+}
+
+/**
+ * @brief Get field chan8_raw from rc_channels message
+ *
+ * @return RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ */
+static inline uint16_t mavlink_msg_rc_channels_get_chan8_raw(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint16_t(msg,  18);
+}
+
+/**
+ * @brief Get field chan9_raw from rc_channels message
+ *
+ * @return RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ */
+static inline uint16_t mavlink_msg_rc_channels_get_chan9_raw(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint16_t(msg,  20);
+}
+
+/**
+ * @brief Get field chan10_raw from rc_channels message
+ *
+ * @return RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ */
+static inline uint16_t mavlink_msg_rc_channels_get_chan10_raw(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint16_t(msg,  22);
+}
+
+/**
+ * @brief Get field chan11_raw from rc_channels message
+ *
+ * @return RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ */
+static inline uint16_t mavlink_msg_rc_channels_get_chan11_raw(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint16_t(msg,  24);
+}
+
+/**
+ * @brief Get field chan12_raw from rc_channels message
+ *
+ * @return RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ */
+static inline uint16_t mavlink_msg_rc_channels_get_chan12_raw(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint16_t(msg,  26);
+}
+
+/**
+ * @brief Get field chan13_raw from rc_channels message
+ *
+ * @return RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ */
+static inline uint16_t mavlink_msg_rc_channels_get_chan13_raw(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint16_t(msg,  28);
+}
+
+/**
+ * @brief Get field chan14_raw from rc_channels message
+ *
+ * @return RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ */
+static inline uint16_t mavlink_msg_rc_channels_get_chan14_raw(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint16_t(msg,  30);
+}
+
+/**
+ * @brief Get field chan15_raw from rc_channels message
+ *
+ * @return RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ */
+static inline uint16_t mavlink_msg_rc_channels_get_chan15_raw(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint16_t(msg,  32);
+}
+
+/**
+ * @brief Get field chan16_raw from rc_channels message
+ *
+ * @return RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ */
+static inline uint16_t mavlink_msg_rc_channels_get_chan16_raw(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint16_t(msg,  34);
+}
+
+/**
+ * @brief Get field chan17_raw from rc_channels message
+ *
+ * @return RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ */
+static inline uint16_t mavlink_msg_rc_channels_get_chan17_raw(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint16_t(msg,  36);
+}
+
+/**
+ * @brief Get field chan18_raw from rc_channels message
+ *
+ * @return RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel is unused.
+ */
+static inline uint16_t mavlink_msg_rc_channels_get_chan18_raw(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint16_t(msg,  38);
+}
+
+/**
+ * @brief Get field rssi from rc_channels message
+ *
+ * @return Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown.
+ */
+static inline uint8_t mavlink_msg_rc_channels_get_rssi(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint8_t(msg,  41);
+}
+
+/**
+ * @brief Decode a rc_channels message into a struct
+ *
+ * @param msg The message to decode
+ * @param rc_channels C-struct to decode the message contents into
+ */
+static inline void mavlink_msg_rc_channels_decode(const mavlink_message_t* msg, mavlink_rc_channels_t* rc_channels)
+{
+#if MAVLINK_NEED_BYTE_SWAP
+	rc_channels->time_boot_ms = mavlink_msg_rc_channels_get_time_boot_ms(msg);
+	rc_channels->chan1_raw = mavlink_msg_rc_channels_get_chan1_raw(msg);
+	rc_channels->chan2_raw = mavlink_msg_rc_channels_get_chan2_raw(msg);
+	rc_channels->chan3_raw = mavlink_msg_rc_channels_get_chan3_raw(msg);
+	rc_channels->chan4_raw = mavlink_msg_rc_channels_get_chan4_raw(msg);
+	rc_channels->chan5_raw = mavlink_msg_rc_channels_get_chan5_raw(msg);
+	rc_channels->chan6_raw = mavlink_msg_rc_channels_get_chan6_raw(msg);
+	rc_channels->chan7_raw = mavlink_msg_rc_channels_get_chan7_raw(msg);
+	rc_channels->chan8_raw = mavlink_msg_rc_channels_get_chan8_raw(msg);
+	rc_channels->chan9_raw = mavlink_msg_rc_channels_get_chan9_raw(msg);
+	rc_channels->chan10_raw = mavlink_msg_rc_channels_get_chan10_raw(msg);
+	rc_channels->chan11_raw = mavlink_msg_rc_channels_get_chan11_raw(msg);
+	rc_channels->chan12_raw = mavlink_msg_rc_channels_get_chan12_raw(msg);
+	rc_channels->chan13_raw = mavlink_msg_rc_channels_get_chan13_raw(msg);
+	rc_channels->chan14_raw = mavlink_msg_rc_channels_get_chan14_raw(msg);
+	rc_channels->chan15_raw = mavlink_msg_rc_channels_get_chan15_raw(msg);
+	rc_channels->chan16_raw = mavlink_msg_rc_channels_get_chan16_raw(msg);
+	rc_channels->chan17_raw = mavlink_msg_rc_channels_get_chan17_raw(msg);
+	rc_channels->chan18_raw = mavlink_msg_rc_channels_get_chan18_raw(msg);
+	rc_channels->chancount = mavlink_msg_rc_channels_get_chancount(msg);
+	rc_channels->rssi = mavlink_msg_rc_channels_get_rssi(msg);
+#else
+	memcpy(rc_channels, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RC_CHANNELS_LEN);
+#endif
+}
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_override.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_override.h
index 0d8a1514b1..15d4c6f958 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_override.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_override.h
@@ -245,6 +245,56 @@ static inline void mavlink_msg_rc_channels_override_send(mavlink_channel_t chan,
 #endif
 }
 
+#if MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_rc_channels_override_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint16_t(buf, 0, chan1_raw);
+	_mav_put_uint16_t(buf, 2, chan2_raw);
+	_mav_put_uint16_t(buf, 4, chan3_raw);
+	_mav_put_uint16_t(buf, 6, chan4_raw);
+	_mav_put_uint16_t(buf, 8, chan5_raw);
+	_mav_put_uint16_t(buf, 10, chan6_raw);
+	_mav_put_uint16_t(buf, 12, chan7_raw);
+	_mav_put_uint16_t(buf, 14, chan8_raw);
+	_mav_put_uint8_t(buf, 16, target_system);
+	_mav_put_uint8_t(buf, 17, target_component);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, buf, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, buf, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN);
+#endif
+#else
+	mavlink_rc_channels_override_t *packet = (mavlink_rc_channels_override_t *)msgbuf;
+	packet->chan1_raw = chan1_raw;
+	packet->chan2_raw = chan2_raw;
+	packet->chan3_raw = chan3_raw;
+	packet->chan4_raw = chan4_raw;
+	packet->chan5_raw = chan5_raw;
+	packet->chan6_raw = chan6_raw;
+	packet->chan7_raw = chan7_raw;
+	packet->chan8_raw = chan8_raw;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE RC_CHANNELS_OVERRIDE UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_raw.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_raw.h
index 3c0aed0202..d75a5934aa 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_raw.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_raw.h
@@ -256,6 +256,58 @@ static inline void mavlink_msg_rc_channels_raw_send(mavlink_channel_t chan, uint
 #endif
 }
 
+#if MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_rc_channels_raw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint32_t time_boot_ms, uint8_t port, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint8_t rssi)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint32_t(buf, 0, time_boot_ms);
+	_mav_put_uint16_t(buf, 4, chan1_raw);
+	_mav_put_uint16_t(buf, 6, chan2_raw);
+	_mav_put_uint16_t(buf, 8, chan3_raw);
+	_mav_put_uint16_t(buf, 10, chan4_raw);
+	_mav_put_uint16_t(buf, 12, chan5_raw);
+	_mav_put_uint16_t(buf, 14, chan6_raw);
+	_mav_put_uint16_t(buf, 16, chan7_raw);
+	_mav_put_uint16_t(buf, 18, chan8_raw);
+	_mav_put_uint8_t(buf, 20, port);
+	_mav_put_uint8_t(buf, 21, rssi);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, buf, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN, MAVLINK_MSG_ID_RC_CHANNELS_RAW_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, buf, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN);
+#endif
+#else
+	mavlink_rc_channels_raw_t *packet = (mavlink_rc_channels_raw_t *)msgbuf;
+	packet->time_boot_ms = time_boot_ms;
+	packet->chan1_raw = chan1_raw;
+	packet->chan2_raw = chan2_raw;
+	packet->chan3_raw = chan3_raw;
+	packet->chan4_raw = chan4_raw;
+	packet->chan5_raw = chan5_raw;
+	packet->chan6_raw = chan6_raw;
+	packet->chan7_raw = chan7_raw;
+	packet->chan8_raw = chan8_raw;
+	packet->port = port;
+	packet->rssi = rssi;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN, MAVLINK_MSG_ID_RC_CHANNELS_RAW_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE RC_CHANNELS_RAW UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_scaled.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_scaled.h
index 2153ab3928..f400f987dd 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_scaled.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_scaled.h
@@ -256,6 +256,58 @@ static inline void mavlink_msg_rc_channels_scaled_send(mavlink_channel_t chan, u
 #endif
 }
 
+#if MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_rc_channels_scaled_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint32_t time_boot_ms, uint8_t port, int16_t chan1_scaled, int16_t chan2_scaled, int16_t chan3_scaled, int16_t chan4_scaled, int16_t chan5_scaled, int16_t chan6_scaled, int16_t chan7_scaled, int16_t chan8_scaled, uint8_t rssi)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint32_t(buf, 0, time_boot_ms);
+	_mav_put_int16_t(buf, 4, chan1_scaled);
+	_mav_put_int16_t(buf, 6, chan2_scaled);
+	_mav_put_int16_t(buf, 8, chan3_scaled);
+	_mav_put_int16_t(buf, 10, chan4_scaled);
+	_mav_put_int16_t(buf, 12, chan5_scaled);
+	_mav_put_int16_t(buf, 14, chan6_scaled);
+	_mav_put_int16_t(buf, 16, chan7_scaled);
+	_mav_put_int16_t(buf, 18, chan8_scaled);
+	_mav_put_uint8_t(buf, 20, port);
+	_mav_put_uint8_t(buf, 21, rssi);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, buf, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, buf, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN);
+#endif
+#else
+	mavlink_rc_channels_scaled_t *packet = (mavlink_rc_channels_scaled_t *)msgbuf;
+	packet->time_boot_ms = time_boot_ms;
+	packet->chan1_scaled = chan1_scaled;
+	packet->chan2_scaled = chan2_scaled;
+	packet->chan3_scaled = chan3_scaled;
+	packet->chan4_scaled = chan4_scaled;
+	packet->chan5_scaled = chan5_scaled;
+	packet->chan6_scaled = chan6_scaled;
+	packet->chan7_scaled = chan7_scaled;
+	packet->chan8_scaled = chan8_scaled;
+	packet->port = port;
+	packet->rssi = rssi;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE RC_CHANNELS_SCALED UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_request_data_stream.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_request_data_stream.h
index c754ad8761..2ebcc54da5 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_request_data_stream.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_request_data_stream.h
@@ -190,6 +190,46 @@ static inline void mavlink_msg_request_data_stream_send(mavlink_channel_t chan,
 #endif
 }
 
+#if MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_request_data_stream_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component, uint8_t req_stream_id, uint16_t req_message_rate, uint8_t start_stop)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint16_t(buf, 0, req_message_rate);
+	_mav_put_uint8_t(buf, 2, target_system);
+	_mav_put_uint8_t(buf, 3, target_component);
+	_mav_put_uint8_t(buf, 4, req_stream_id);
+	_mav_put_uint8_t(buf, 5, start_stop);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, buf, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, buf, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN);
+#endif
+#else
+	mavlink_request_data_stream_t *packet = (mavlink_request_data_stream_t *)msgbuf;
+	packet->req_message_rate = req_message_rate;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+	packet->req_stream_id = req_stream_id;
+	packet->start_stop = start_stop;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, (const char *)packet, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, (const char *)packet, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE REQUEST_DATA_STREAM UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h
index ac3ef4fa9f..131b756b60 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h
@@ -190,6 +190,46 @@ static inline void mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(mavlink
 #endif
 }
 
+#if MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint32_t time_boot_ms, float roll_rate, float pitch_rate, float yaw_rate, float thrust)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint32_t(buf, 0, time_boot_ms);
+	_mav_put_float(buf, 4, roll_rate);
+	_mav_put_float(buf, 8, pitch_rate);
+	_mav_put_float(buf, 12, yaw_rate);
+	_mav_put_float(buf, 16, thrust);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN);
+#endif
+#else
+	mavlink_roll_pitch_yaw_rates_thrust_setpoint_t *packet = (mavlink_roll_pitch_yaw_rates_thrust_setpoint_t *)msgbuf;
+	packet->time_boot_ms = time_boot_ms;
+	packet->roll_rate = roll_rate;
+	packet->pitch_rate = pitch_rate;
+	packet->yaw_rate = yaw_rate;
+	packet->thrust = thrust;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE ROLL_PITCH_YAW_RATES_THRUST_SETPOINT UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h
index 626477322b..757e6d138b 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h
@@ -190,6 +190,46 @@ static inline void mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_send(mavlink
 #endif
 }
 
+#if MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint32_t time_boot_ms, float roll_speed, float pitch_speed, float yaw_speed, float thrust)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint32_t(buf, 0, time_boot_ms);
+	_mav_put_float(buf, 4, roll_speed);
+	_mav_put_float(buf, 8, pitch_speed);
+	_mav_put_float(buf, 12, yaw_speed);
+	_mav_put_float(buf, 16, thrust);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN);
+#endif
+#else
+	mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *packet = (mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *)msgbuf;
+	packet->time_boot_ms = time_boot_ms;
+	packet->roll_speed = roll_speed;
+	packet->pitch_speed = pitch_speed;
+	packet->yaw_speed = yaw_speed;
+	packet->thrust = thrust;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h
index ffcdc547b4..f04eedcb36 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h
@@ -190,6 +190,46 @@ static inline void mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(mavlink_chann
 #endif
 }
 
+#if MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_roll_pitch_yaw_thrust_setpoint_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint32_t time_boot_ms, float roll, float pitch, float yaw, float thrust)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint32_t(buf, 0, time_boot_ms);
+	_mav_put_float(buf, 4, roll);
+	_mav_put_float(buf, 8, pitch);
+	_mav_put_float(buf, 12, yaw);
+	_mav_put_float(buf, 16, thrust);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN);
+#endif
+#else
+	mavlink_roll_pitch_yaw_thrust_setpoint_t *packet = (mavlink_roll_pitch_yaw_thrust_setpoint_t *)msgbuf;
+	packet->time_boot_ms = time_boot_ms;
+	packet->roll = roll;
+	packet->pitch = pitch;
+	packet->yaw = yaw;
+	packet->thrust = thrust;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE ROLL_PITCH_YAW_THRUST_SETPOINT UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_allowed_area.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_allowed_area.h
index fcd54cbb78..0176dfcc8d 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_allowed_area.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_allowed_area.h
@@ -212,6 +212,50 @@ static inline void mavlink_msg_safety_allowed_area_send(mavlink_channel_t chan,
 #endif
 }
 
+#if MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_safety_allowed_area_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, p1x);
+	_mav_put_float(buf, 4, p1y);
+	_mav_put_float(buf, 8, p1z);
+	_mav_put_float(buf, 12, p2x);
+	_mav_put_float(buf, 16, p2y);
+	_mav_put_float(buf, 20, p2z);
+	_mav_put_uint8_t(buf, 24, frame);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, buf, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, buf, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN);
+#endif
+#else
+	mavlink_safety_allowed_area_t *packet = (mavlink_safety_allowed_area_t *)msgbuf;
+	packet->p1x = p1x;
+	packet->p1y = p1y;
+	packet->p1z = p1z;
+	packet->p2x = p2x;
+	packet->p2y = p2y;
+	packet->p2z = p2z;
+	packet->frame = frame;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, (const char *)packet, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, (const char *)packet, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SAFETY_ALLOWED_AREA UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_set_allowed_area.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_set_allowed_area.h
index 61f6af1554..dafbc260b4 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_set_allowed_area.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_set_allowed_area.h
@@ -234,6 +234,54 @@ static inline void mavlink_msg_safety_set_allowed_area_send(mavlink_channel_t ch
 #endif
 }
 
+#if MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_safety_set_allowed_area_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, p1x);
+	_mav_put_float(buf, 4, p1y);
+	_mav_put_float(buf, 8, p1z);
+	_mav_put_float(buf, 12, p2x);
+	_mav_put_float(buf, 16, p2y);
+	_mav_put_float(buf, 20, p2z);
+	_mav_put_uint8_t(buf, 24, target_system);
+	_mav_put_uint8_t(buf, 25, target_component);
+	_mav_put_uint8_t(buf, 26, frame);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, buf, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, buf, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN);
+#endif
+#else
+	mavlink_safety_set_allowed_area_t *packet = (mavlink_safety_set_allowed_area_t *)msgbuf;
+	packet->p1x = p1x;
+	packet->p1y = p1y;
+	packet->p1z = p1z;
+	packet->p2x = p2x;
+	packet->p2y = p2y;
+	packet->p2z = p2z;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+	packet->frame = frame;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, (const char *)packet, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, (const char *)packet, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SAFETY_SET_ALLOWED_AREA UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu.h
index 3010d051af..1648a56f89 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu.h
@@ -245,6 +245,56 @@ static inline void mavlink_msg_scaled_imu_send(mavlink_channel_t chan, uint32_t
 #endif
 }
 
+#if MAVLINK_MSG_ID_SCALED_IMU_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_scaled_imu_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint32_t(buf, 0, time_boot_ms);
+	_mav_put_int16_t(buf, 4, xacc);
+	_mav_put_int16_t(buf, 6, yacc);
+	_mav_put_int16_t(buf, 8, zacc);
+	_mav_put_int16_t(buf, 10, xgyro);
+	_mav_put_int16_t(buf, 12, ygyro);
+	_mav_put_int16_t(buf, 14, zgyro);
+	_mav_put_int16_t(buf, 16, xmag);
+	_mav_put_int16_t(buf, 18, ymag);
+	_mav_put_int16_t(buf, 20, zmag);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, buf, MAVLINK_MSG_ID_SCALED_IMU_LEN, MAVLINK_MSG_ID_SCALED_IMU_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, buf, MAVLINK_MSG_ID_SCALED_IMU_LEN);
+#endif
+#else
+	mavlink_scaled_imu_t *packet = (mavlink_scaled_imu_t *)msgbuf;
+	packet->time_boot_ms = time_boot_ms;
+	packet->xacc = xacc;
+	packet->yacc = yacc;
+	packet->zacc = zacc;
+	packet->xgyro = xgyro;
+	packet->ygyro = ygyro;
+	packet->zgyro = zgyro;
+	packet->xmag = xmag;
+	packet->ymag = ymag;
+	packet->zmag = zmag;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, (const char *)packet, MAVLINK_MSG_ID_SCALED_IMU_LEN, MAVLINK_MSG_ID_SCALED_IMU_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, (const char *)packet, MAVLINK_MSG_ID_SCALED_IMU_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SCALED_IMU UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu2.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu2.h
index ea4dbbf81d..1dea121dde 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu2.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu2.h
@@ -245,6 +245,56 @@ static inline void mavlink_msg_scaled_imu2_send(mavlink_channel_t chan, uint32_t
 #endif
 }
 
+#if MAVLINK_MSG_ID_SCALED_IMU2_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_scaled_imu2_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint32_t(buf, 0, time_boot_ms);
+	_mav_put_int16_t(buf, 4, xacc);
+	_mav_put_int16_t(buf, 6, yacc);
+	_mav_put_int16_t(buf, 8, zacc);
+	_mav_put_int16_t(buf, 10, xgyro);
+	_mav_put_int16_t(buf, 12, ygyro);
+	_mav_put_int16_t(buf, 14, zgyro);
+	_mav_put_int16_t(buf, 16, xmag);
+	_mav_put_int16_t(buf, 18, ymag);
+	_mav_put_int16_t(buf, 20, zmag);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, buf, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, buf, MAVLINK_MSG_ID_SCALED_IMU2_LEN);
+#endif
+#else
+	mavlink_scaled_imu2_t *packet = (mavlink_scaled_imu2_t *)msgbuf;
+	packet->time_boot_ms = time_boot_ms;
+	packet->xacc = xacc;
+	packet->yacc = yacc;
+	packet->zacc = zacc;
+	packet->xgyro = xgyro;
+	packet->ygyro = ygyro;
+	packet->zgyro = zgyro;
+	packet->xmag = xmag;
+	packet->ymag = ymag;
+	packet->zmag = zmag;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, (const char *)packet, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, (const char *)packet, MAVLINK_MSG_ID_SCALED_IMU2_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SCALED_IMU2 UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_pressure.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_pressure.h
index 10324bc941..e0d6d87662 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_pressure.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_pressure.h
@@ -179,6 +179,44 @@ static inline void mavlink_msg_scaled_pressure_send(mavlink_channel_t chan, uint
 #endif
 }
 
+#if MAVLINK_MSG_ID_SCALED_PRESSURE_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_scaled_pressure_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint32_t(buf, 0, time_boot_ms);
+	_mav_put_float(buf, 4, press_abs);
+	_mav_put_float(buf, 8, press_diff);
+	_mav_put_int16_t(buf, 12, temperature);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, buf, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, buf, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN);
+#endif
+#else
+	mavlink_scaled_pressure_t *packet = (mavlink_scaled_pressure_t *)msgbuf;
+	packet->time_boot_ms = time_boot_ms;
+	packet->press_abs = press_abs;
+	packet->press_diff = press_diff;
+	packet->temperature = temperature;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, (const char *)packet, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, (const char *)packet, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SCALED_PRESSURE UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_serial_control.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_serial_control.h
new file mode 100644
index 0000000000..cbf72bae31
--- /dev/null
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_serial_control.h
@@ -0,0 +1,321 @@
+// MESSAGE SERIAL_CONTROL PACKING
+
+#define MAVLINK_MSG_ID_SERIAL_CONTROL 126
+
+typedef struct __mavlink_serial_control_t
+{
+ uint32_t baudrate; ///< Baudrate of transfer. Zero means no change.
+ uint16_t timeout; ///< Timeout for reply data in milliseconds
+ uint8_t device; ///< See SERIAL_CONTROL_DEV enum
+ uint8_t flags; ///< See SERIAL_CONTROL_FLAG enum
+ uint8_t count; ///< how many bytes in this transfer
+ uint8_t data[70]; ///< serial data
+} mavlink_serial_control_t;
+
+#define MAVLINK_MSG_ID_SERIAL_CONTROL_LEN 79
+#define MAVLINK_MSG_ID_126_LEN 79
+
+#define MAVLINK_MSG_ID_SERIAL_CONTROL_CRC 220
+#define MAVLINK_MSG_ID_126_CRC 220
+
+#define MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN 70
+
+#define MAVLINK_MESSAGE_INFO_SERIAL_CONTROL { \
+	"SERIAL_CONTROL", \
+	6, \
+	{  { "baudrate", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_serial_control_t, baudrate) }, \
+         { "timeout", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_serial_control_t, timeout) }, \
+         { "device", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_serial_control_t, device) }, \
+         { "flags", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_serial_control_t, flags) }, \
+         { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_serial_control_t, count) }, \
+         { "data", NULL, MAVLINK_TYPE_UINT8_T, 70, 9, offsetof(mavlink_serial_control_t, data) }, \
+         } \
+}
+
+
+/**
+ * @brief Pack a serial_control message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ *
+ * @param device See SERIAL_CONTROL_DEV enum
+ * @param flags See SERIAL_CONTROL_FLAG enum
+ * @param timeout Timeout for reply data in milliseconds
+ * @param baudrate Baudrate of transfer. Zero means no change.
+ * @param count how many bytes in this transfer
+ * @param data serial data
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_serial_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
+						       uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, uint8_t count, const uint8_t *data)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char buf[MAVLINK_MSG_ID_SERIAL_CONTROL_LEN];
+	_mav_put_uint32_t(buf, 0, baudrate);
+	_mav_put_uint16_t(buf, 4, timeout);
+	_mav_put_uint8_t(buf, 6, device);
+	_mav_put_uint8_t(buf, 7, flags);
+	_mav_put_uint8_t(buf, 8, count);
+	_mav_put_uint8_t_array(buf, 9, data, 70);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN);
+#else
+	mavlink_serial_control_t packet;
+	packet.baudrate = baudrate;
+	packet.timeout = timeout;
+	packet.device = device;
+	packet.flags = flags;
+	packet.count = count;
+	mav_array_memcpy(packet.data, data, sizeof(uint8_t)*70);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN);
+#endif
+
+	msg->msgid = MAVLINK_MSG_ID_SERIAL_CONTROL;
+#if MAVLINK_CRC_EXTRA
+    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC);
+#else
+    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN);
+#endif
+}
+
+/**
+ * @brief Pack a serial_control message on a channel
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message will be sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param device See SERIAL_CONTROL_DEV enum
+ * @param flags See SERIAL_CONTROL_FLAG enum
+ * @param timeout Timeout for reply data in milliseconds
+ * @param baudrate Baudrate of transfer. Zero means no change.
+ * @param count how many bytes in this transfer
+ * @param data serial data
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_serial_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
+							   mavlink_message_t* msg,
+						           uint8_t device,uint8_t flags,uint16_t timeout,uint32_t baudrate,uint8_t count,const uint8_t *data)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char buf[MAVLINK_MSG_ID_SERIAL_CONTROL_LEN];
+	_mav_put_uint32_t(buf, 0, baudrate);
+	_mav_put_uint16_t(buf, 4, timeout);
+	_mav_put_uint8_t(buf, 6, device);
+	_mav_put_uint8_t(buf, 7, flags);
+	_mav_put_uint8_t(buf, 8, count);
+	_mav_put_uint8_t_array(buf, 9, data, 70);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN);
+#else
+	mavlink_serial_control_t packet;
+	packet.baudrate = baudrate;
+	packet.timeout = timeout;
+	packet.device = device;
+	packet.flags = flags;
+	packet.count = count;
+	mav_array_memcpy(packet.data, data, sizeof(uint8_t)*70);
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN);
+#endif
+
+	msg->msgid = MAVLINK_MSG_ID_SERIAL_CONTROL;
+#if MAVLINK_CRC_EXTRA
+    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC);
+#else
+    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN);
+#endif
+}
+
+/**
+ * @brief Encode a serial_control struct
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ * @param serial_control C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_serial_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_control_t* serial_control)
+{
+	return mavlink_msg_serial_control_pack(system_id, component_id, msg, serial_control->device, serial_control->flags, serial_control->timeout, serial_control->baudrate, serial_control->count, serial_control->data);
+}
+
+/**
+ * @brief Encode a serial_control struct on a channel
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message will be sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param serial_control C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_serial_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_control_t* serial_control)
+{
+	return mavlink_msg_serial_control_pack_chan(system_id, component_id, chan, msg, serial_control->device, serial_control->flags, serial_control->timeout, serial_control->baudrate, serial_control->count, serial_control->data);
+}
+
+/**
+ * @brief Send a serial_control message
+ * @param chan MAVLink channel to send the message
+ *
+ * @param device See SERIAL_CONTROL_DEV enum
+ * @param flags See SERIAL_CONTROL_FLAG enum
+ * @param timeout Timeout for reply data in milliseconds
+ * @param baudrate Baudrate of transfer. Zero means no change.
+ * @param count how many bytes in this transfer
+ * @param data serial data
+ */
+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
+
+static inline void mavlink_msg_serial_control_send(mavlink_channel_t chan, uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, uint8_t count, const uint8_t *data)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char buf[MAVLINK_MSG_ID_SERIAL_CONTROL_LEN];
+	_mav_put_uint32_t(buf, 0, baudrate);
+	_mav_put_uint16_t(buf, 4, timeout);
+	_mav_put_uint8_t(buf, 6, device);
+	_mav_put_uint8_t(buf, 7, flags);
+	_mav_put_uint8_t(buf, 8, count);
+	_mav_put_uint8_t_array(buf, 9, data, 70);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN);
+#endif
+#else
+	mavlink_serial_control_t packet;
+	packet.baudrate = baudrate;
+	packet.timeout = timeout;
+	packet.device = device;
+	packet.flags = flags;
+	packet.count = count;
+	mav_array_memcpy(packet.data, data, sizeof(uint8_t)*70);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN);
+#endif
+#endif
+}
+
+#if MAVLINK_MSG_ID_SERIAL_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_serial_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, uint8_t count, const uint8_t *data)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint32_t(buf, 0, baudrate);
+	_mav_put_uint16_t(buf, 4, timeout);
+	_mav_put_uint8_t(buf, 6, device);
+	_mav_put_uint8_t(buf, 7, flags);
+	_mav_put_uint8_t(buf, 8, count);
+	_mav_put_uint8_t_array(buf, 9, data, 70);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN);
+#endif
+#else
+	mavlink_serial_control_t *packet = (mavlink_serial_control_t *)msgbuf;
+	packet->baudrate = baudrate;
+	packet->timeout = timeout;
+	packet->device = device;
+	packet->flags = flags;
+	packet->count = count;
+	mav_array_memcpy(packet->data, data, sizeof(uint8_t)*70);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, (const char *)packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, (const char *)packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN);
+#endif
+#endif
+}
+#endif
+
+#endif
+
+// MESSAGE SERIAL_CONTROL UNPACKING
+
+
+/**
+ * @brief Get field device from serial_control message
+ *
+ * @return See SERIAL_CONTROL_DEV enum
+ */
+static inline uint8_t mavlink_msg_serial_control_get_device(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint8_t(msg,  6);
+}
+
+/**
+ * @brief Get field flags from serial_control message
+ *
+ * @return See SERIAL_CONTROL_FLAG enum
+ */
+static inline uint8_t mavlink_msg_serial_control_get_flags(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint8_t(msg,  7);
+}
+
+/**
+ * @brief Get field timeout from serial_control message
+ *
+ * @return Timeout for reply data in milliseconds
+ */
+static inline uint16_t mavlink_msg_serial_control_get_timeout(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint16_t(msg,  4);
+}
+
+/**
+ * @brief Get field baudrate from serial_control message
+ *
+ * @return Baudrate of transfer. Zero means no change.
+ */
+static inline uint32_t mavlink_msg_serial_control_get_baudrate(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint32_t(msg,  0);
+}
+
+/**
+ * @brief Get field count from serial_control message
+ *
+ * @return how many bytes in this transfer
+ */
+static inline uint8_t mavlink_msg_serial_control_get_count(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_uint8_t(msg,  8);
+}
+
+/**
+ * @brief Get field data from serial_control message
+ *
+ * @return serial data
+ */
+static inline uint16_t mavlink_msg_serial_control_get_data(const mavlink_message_t* msg, uint8_t *data)
+{
+	return _MAV_RETURN_uint8_t_array(msg, data, 70,  9);
+}
+
+/**
+ * @brief Decode a serial_control message into a struct
+ *
+ * @param msg The message to decode
+ * @param serial_control C-struct to decode the message contents into
+ */
+static inline void mavlink_msg_serial_control_decode(const mavlink_message_t* msg, mavlink_serial_control_t* serial_control)
+{
+#if MAVLINK_NEED_BYTE_SWAP
+	serial_control->baudrate = mavlink_msg_serial_control_get_baudrate(msg);
+	serial_control->timeout = mavlink_msg_serial_control_get_timeout(msg);
+	serial_control->device = mavlink_msg_serial_control_get_device(msg);
+	serial_control->flags = mavlink_msg_serial_control_get_flags(msg);
+	serial_control->count = mavlink_msg_serial_control_get_count(msg);
+	mavlink_msg_serial_control_get_data(msg, serial_control->data);
+#else
+	memcpy(serial_control, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_CONTROL_LEN);
+#endif
+}
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_servo_output_raw.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_servo_output_raw.h
index 6a14e93ed3..e8408862ff 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_servo_output_raw.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_servo_output_raw.h
@@ -245,6 +245,56 @@ static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uin
 #endif
 }
 
+#if MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_servo_output_raw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint32_t time_usec, uint8_t port, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint32_t(buf, 0, time_usec);
+	_mav_put_uint16_t(buf, 4, servo1_raw);
+	_mav_put_uint16_t(buf, 6, servo2_raw);
+	_mav_put_uint16_t(buf, 8, servo3_raw);
+	_mav_put_uint16_t(buf, 10, servo4_raw);
+	_mav_put_uint16_t(buf, 12, servo5_raw);
+	_mav_put_uint16_t(buf, 14, servo6_raw);
+	_mav_put_uint16_t(buf, 16, servo7_raw);
+	_mav_put_uint16_t(buf, 18, servo8_raw);
+	_mav_put_uint8_t(buf, 20, port);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, buf, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, buf, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN);
+#endif
+#else
+	mavlink_servo_output_raw_t *packet = (mavlink_servo_output_raw_t *)msgbuf;
+	packet->time_usec = time_usec;
+	packet->servo1_raw = servo1_raw;
+	packet->servo2_raw = servo2_raw;
+	packet->servo3_raw = servo3_raw;
+	packet->servo4_raw = servo4_raw;
+	packet->servo5_raw = servo5_raw;
+	packet->servo6_raw = servo6_raw;
+	packet->servo7_raw = servo7_raw;
+	packet->servo8_raw = servo8_raw;
+	packet->port = port;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, (const char *)packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, (const char *)packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SERVO_OUTPUT_RAW UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_global_position_setpoint_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_global_position_setpoint_int.h
index 6f0d7a69da..94a7b432bf 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_global_position_setpoint_int.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_global_position_setpoint_int.h
@@ -190,6 +190,46 @@ static inline void mavlink_msg_set_global_position_setpoint_int_send(mavlink_cha
 #endif
 }
 
+#if MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_set_global_position_setpoint_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t coordinate_frame, int32_t latitude, int32_t longitude, int32_t altitude, int16_t yaw)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_int32_t(buf, 0, latitude);
+	_mav_put_int32_t(buf, 4, longitude);
+	_mav_put_int32_t(buf, 8, altitude);
+	_mav_put_int16_t(buf, 12, yaw);
+	_mav_put_uint8_t(buf, 14, coordinate_frame);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT, buf, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT, buf, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN);
+#endif
+#else
+	mavlink_set_global_position_setpoint_int_t *packet = (mavlink_set_global_position_setpoint_int_t *)msgbuf;
+	packet->latitude = latitude;
+	packet->longitude = longitude;
+	packet->altitude = altitude;
+	packet->yaw = yaw;
+	packet->coordinate_frame = coordinate_frame;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT, (const char *)packet, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT, (const char *)packet, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SET_GLOBAL_POSITION_SETPOINT_INT UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_gps_global_origin.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_gps_global_origin.h
index c444d8d52c..1c018cbee3 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_gps_global_origin.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_gps_global_origin.h
@@ -179,6 +179,44 @@ static inline void mavlink_msg_set_gps_global_origin_send(mavlink_channel_t chan
 #endif
 }
 
+#if MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_set_gps_global_origin_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_int32_t(buf, 0, latitude);
+	_mav_put_int32_t(buf, 4, longitude);
+	_mav_put_int32_t(buf, 8, altitude);
+	_mav_put_uint8_t(buf, 12, target_system);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN);
+#endif
+#else
+	mavlink_set_gps_global_origin_t *packet = (mavlink_set_gps_global_origin_t *)msgbuf;
+	packet->latitude = latitude;
+	packet->longitude = longitude;
+	packet->altitude = altitude;
+	packet->target_system = target_system;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, (const char *)packet, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, (const char *)packet, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SET_GPS_GLOBAL_ORIGIN UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_local_position_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_local_position_setpoint.h
index 6f2835e031..04dbe6e9a2 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_local_position_setpoint.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_local_position_setpoint.h
@@ -212,6 +212,50 @@ static inline void mavlink_msg_set_local_position_setpoint_send(mavlink_channel_
 #endif
 }
 
+#if MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_set_local_position_setpoint_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component, uint8_t coordinate_frame, float x, float y, float z, float yaw)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, x);
+	_mav_put_float(buf, 4, y);
+	_mav_put_float(buf, 8, z);
+	_mav_put_float(buf, 12, yaw);
+	_mav_put_uint8_t(buf, 16, target_system);
+	_mav_put_uint8_t(buf, 17, target_component);
+	_mav_put_uint8_t(buf, 18, coordinate_frame);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT, buf, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT, buf, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN);
+#endif
+#else
+	mavlink_set_local_position_setpoint_t *packet = (mavlink_set_local_position_setpoint_t *)msgbuf;
+	packet->x = x;
+	packet->y = y;
+	packet->z = z;
+	packet->yaw = yaw;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+	packet->coordinate_frame = coordinate_frame;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SET_LOCAL_POSITION_SETPOINT UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_mode.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_mode.h
index 1aff42cce3..4b60a4120b 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_mode.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_mode.h
@@ -168,6 +168,42 @@ static inline void mavlink_msg_set_mode_send(mavlink_channel_t chan, uint8_t tar
 #endif
 }
 
+#if MAVLINK_MSG_ID_SET_MODE_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_set_mode_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t base_mode, uint32_t custom_mode)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint32_t(buf, 0, custom_mode);
+	_mav_put_uint8_t(buf, 4, target_system);
+	_mav_put_uint8_t(buf, 5, base_mode);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, buf, MAVLINK_MSG_ID_SET_MODE_LEN, MAVLINK_MSG_ID_SET_MODE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, buf, MAVLINK_MSG_ID_SET_MODE_LEN);
+#endif
+#else
+	mavlink_set_mode_t *packet = (mavlink_set_mode_t *)msgbuf;
+	packet->custom_mode = custom_mode;
+	packet->target_system = target_system;
+	packet->base_mode = base_mode;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, (const char *)packet, MAVLINK_MSG_ID_SET_MODE_LEN, MAVLINK_MSG_ID_SET_MODE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, (const char *)packet, MAVLINK_MSG_ID_SET_MODE_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SET_MODE UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_motors_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_motors_setpoint.h
index 8ceb8888fc..8c0e2a014a 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_motors_setpoint.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_motors_setpoint.h
@@ -190,6 +190,46 @@ static inline void mavlink_msg_set_quad_motors_setpoint_send(mavlink_channel_t c
 #endif
 }
 
+#if MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_set_quad_motors_setpoint_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint16_t motor_front_nw, uint16_t motor_right_ne, uint16_t motor_back_se, uint16_t motor_left_sw)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint16_t(buf, 0, motor_front_nw);
+	_mav_put_uint16_t(buf, 2, motor_right_ne);
+	_mav_put_uint16_t(buf, 4, motor_back_se);
+	_mav_put_uint16_t(buf, 6, motor_left_sw);
+	_mav_put_uint8_t(buf, 8, target_system);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT, buf, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT, buf, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN);
+#endif
+#else
+	mavlink_set_quad_motors_setpoint_t *packet = (mavlink_set_quad_motors_setpoint_t *)msgbuf;
+	packet->motor_front_nw = motor_front_nw;
+	packet->motor_right_ne = motor_right_ne;
+	packet->motor_back_se = motor_back_se;
+	packet->motor_left_sw = motor_left_sw;
+	packet->target_system = target_system;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SET_QUAD_MOTORS_SETPOINT UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h
index 9ef294cc97..c9a084773c 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h
@@ -234,6 +234,52 @@ static inline void mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_send(mav
 #endif
 }
 
+#if MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t group, uint8_t mode, const uint8_t *led_red, const uint8_t *led_blue, const uint8_t *led_green, const int16_t *roll, const int16_t *pitch, const int16_t *yaw, const uint16_t *thrust)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint8_t(buf, 32, group);
+	_mav_put_uint8_t(buf, 33, mode);
+	_mav_put_int16_t_array(buf, 0, roll, 4);
+	_mav_put_int16_t_array(buf, 8, pitch, 4);
+	_mav_put_int16_t_array(buf, 16, yaw, 4);
+	_mav_put_uint16_t_array(buf, 24, thrust, 4);
+	_mav_put_uint8_t_array(buf, 34, led_red, 4);
+	_mav_put_uint8_t_array(buf, 38, led_blue, 4);
+	_mav_put_uint8_t_array(buf, 42, led_green, 4);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, buf, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, buf, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN);
+#endif
+#else
+	mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t *packet = (mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t *)msgbuf;
+	packet->group = group;
+	packet->mode = mode;
+	mav_array_memcpy(packet->roll, roll, sizeof(int16_t)*4);
+	mav_array_memcpy(packet->pitch, pitch, sizeof(int16_t)*4);
+	mav_array_memcpy(packet->yaw, yaw, sizeof(int16_t)*4);
+	mav_array_memcpy(packet->thrust, thrust, sizeof(uint16_t)*4);
+	mav_array_memcpy(packet->led_red, led_red, sizeof(uint8_t)*4);
+	mav_array_memcpy(packet->led_blue, led_blue, sizeof(uint8_t)*4);
+	mav_array_memcpy(packet->led_green, led_green, sizeof(uint8_t)*4);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, (const char *)packet, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, (const char *)packet, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h
index 7d8d526f8b..338bf0e438 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h
@@ -198,6 +198,46 @@ static inline void mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_send(mavlink
 #endif
 }
 
+#if MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t group, uint8_t mode, const int16_t *roll, const int16_t *pitch, const int16_t *yaw, const uint16_t *thrust)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint8_t(buf, 32, group);
+	_mav_put_uint8_t(buf, 33, mode);
+	_mav_put_int16_t_array(buf, 0, roll, 4);
+	_mav_put_int16_t_array(buf, 8, pitch, 4);
+	_mav_put_int16_t_array(buf, 16, yaw, 4);
+	_mav_put_uint16_t_array(buf, 24, thrust, 4);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, buf, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, buf, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN);
+#endif
+#else
+	mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t *packet = (mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t *)msgbuf;
+	packet->group = group;
+	packet->mode = mode;
+	mav_array_memcpy(packet->roll, roll, sizeof(int16_t)*4);
+	mav_array_memcpy(packet->pitch, pitch, sizeof(int16_t)*4);
+	mav_array_memcpy(packet->yaw, yaw, sizeof(int16_t)*4);
+	mav_array_memcpy(packet->thrust, thrust, sizeof(uint16_t)*4);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, (const char *)packet, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, (const char *)packet, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h
index 5846ba41f7..4d0e5931fa 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h
@@ -201,6 +201,48 @@ static inline void mavlink_msg_set_roll_pitch_yaw_speed_thrust_send(mavlink_chan
 #endif
 }
 
+#if MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_set_roll_pitch_yaw_speed_thrust_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed, float thrust)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, roll_speed);
+	_mav_put_float(buf, 4, pitch_speed);
+	_mav_put_float(buf, 8, yaw_speed);
+	_mav_put_float(buf, 12, thrust);
+	_mav_put_uint8_t(buf, 16, target_system);
+	_mav_put_uint8_t(buf, 17, target_component);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST, buf, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST, buf, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN);
+#endif
+#else
+	mavlink_set_roll_pitch_yaw_speed_thrust_t *packet = (mavlink_set_roll_pitch_yaw_speed_thrust_t *)msgbuf;
+	packet->roll_speed = roll_speed;
+	packet->pitch_speed = pitch_speed;
+	packet->yaw_speed = yaw_speed;
+	packet->thrust = thrust;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST, (const char *)packet, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST, (const char *)packet, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SET_ROLL_PITCH_YAW_SPEED_THRUST UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_thrust.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_thrust.h
index 334fd39e36..21e0a43f24 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_thrust.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_thrust.h
@@ -201,6 +201,48 @@ static inline void mavlink_msg_set_roll_pitch_yaw_thrust_send(mavlink_channel_t
 #endif
 }
 
+#if MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_set_roll_pitch_yaw_thrust_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw, float thrust)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, roll);
+	_mav_put_float(buf, 4, pitch);
+	_mav_put_float(buf, 8, yaw);
+	_mav_put_float(buf, 12, thrust);
+	_mav_put_uint8_t(buf, 16, target_system);
+	_mav_put_uint8_t(buf, 17, target_component);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST, buf, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST, buf, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN);
+#endif
+#else
+	mavlink_set_roll_pitch_yaw_thrust_t *packet = (mavlink_set_roll_pitch_yaw_thrust_t *)msgbuf;
+	packet->roll = roll;
+	packet->pitch = pitch;
+	packet->yaw = yaw;
+	packet->thrust = thrust;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST, (const char *)packet, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST, (const char *)packet, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SET_ROLL_PITCH_YAW_THRUST UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h
index 93ce345b65..90b63427b9 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h
@@ -212,6 +212,50 @@ static inline void mavlink_msg_setpoint_6dof_send(mavlink_channel_t chan, uint8_
 #endif
 }
 
+#if MAVLINK_MSG_ID_SETPOINT_6DOF_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_setpoint_6dof_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, float trans_x, float trans_y, float trans_z, float rot_x, float rot_y, float rot_z)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, trans_x);
+	_mav_put_float(buf, 4, trans_y);
+	_mav_put_float(buf, 8, trans_z);
+	_mav_put_float(buf, 12, rot_x);
+	_mav_put_float(buf, 16, rot_y);
+	_mav_put_float(buf, 20, rot_z);
+	_mav_put_uint8_t(buf, 24, target_system);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_6DOF, buf, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN, MAVLINK_MSG_ID_SETPOINT_6DOF_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_6DOF, buf, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN);
+#endif
+#else
+	mavlink_setpoint_6dof_t *packet = (mavlink_setpoint_6dof_t *)msgbuf;
+	packet->trans_x = trans_x;
+	packet->trans_y = trans_y;
+	packet->trans_z = trans_z;
+	packet->rot_x = rot_x;
+	packet->rot_y = rot_y;
+	packet->rot_z = rot_z;
+	packet->target_system = target_system;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_6DOF, (const char *)packet, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN, MAVLINK_MSG_ID_SETPOINT_6DOF_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_6DOF, (const char *)packet, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SETPOINT_6DOF UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h
index de80e46459..308a211cde 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h
@@ -234,6 +234,54 @@ static inline void mavlink_msg_setpoint_8dof_send(mavlink_channel_t chan, uint8_
 #endif
 }
 
+#if MAVLINK_MSG_ID_SETPOINT_8DOF_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_setpoint_8dof_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, float val1, float val2, float val3, float val4, float val5, float val6, float val7, float val8)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, val1);
+	_mav_put_float(buf, 4, val2);
+	_mav_put_float(buf, 8, val3);
+	_mav_put_float(buf, 12, val4);
+	_mav_put_float(buf, 16, val5);
+	_mav_put_float(buf, 20, val6);
+	_mav_put_float(buf, 24, val7);
+	_mav_put_float(buf, 28, val8);
+	_mav_put_uint8_t(buf, 32, target_system);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_8DOF, buf, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN, MAVLINK_MSG_ID_SETPOINT_8DOF_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_8DOF, buf, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN);
+#endif
+#else
+	mavlink_setpoint_8dof_t *packet = (mavlink_setpoint_8dof_t *)msgbuf;
+	packet->val1 = val1;
+	packet->val2 = val2;
+	packet->val3 = val3;
+	packet->val4 = val4;
+	packet->val5 = val5;
+	packet->val6 = val6;
+	packet->val7 = val7;
+	packet->val8 = val8;
+	packet->target_system = target_system;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_8DOF, (const char *)packet, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN, MAVLINK_MSG_ID_SETPOINT_8DOF_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_8DOF, (const char *)packet, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SETPOINT_8DOF UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h
index ebd657cf38..db3a926424 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h
@@ -366,6 +366,78 @@ static inline void mavlink_msg_sim_state_send(mavlink_channel_t chan, float q1,
 #endif
 }
 
+#if MAVLINK_MSG_ID_SIM_STATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_sim_state_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  float q1, float q2, float q3, float q4, float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float lat, float lon, float alt, float std_dev_horz, float std_dev_vert, float vn, float ve, float vd)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, q1);
+	_mav_put_float(buf, 4, q2);
+	_mav_put_float(buf, 8, q3);
+	_mav_put_float(buf, 12, q4);
+	_mav_put_float(buf, 16, roll);
+	_mav_put_float(buf, 20, pitch);
+	_mav_put_float(buf, 24, yaw);
+	_mav_put_float(buf, 28, xacc);
+	_mav_put_float(buf, 32, yacc);
+	_mav_put_float(buf, 36, zacc);
+	_mav_put_float(buf, 40, xgyro);
+	_mav_put_float(buf, 44, ygyro);
+	_mav_put_float(buf, 48, zgyro);
+	_mav_put_float(buf, 52, lat);
+	_mav_put_float(buf, 56, lon);
+	_mav_put_float(buf, 60, alt);
+	_mav_put_float(buf, 64, std_dev_horz);
+	_mav_put_float(buf, 68, std_dev_vert);
+	_mav_put_float(buf, 72, vn);
+	_mav_put_float(buf, 76, ve);
+	_mav_put_float(buf, 80, vd);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, buf, MAVLINK_MSG_ID_SIM_STATE_LEN, MAVLINK_MSG_ID_SIM_STATE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, buf, MAVLINK_MSG_ID_SIM_STATE_LEN);
+#endif
+#else
+	mavlink_sim_state_t *packet = (mavlink_sim_state_t *)msgbuf;
+	packet->q1 = q1;
+	packet->q2 = q2;
+	packet->q3 = q3;
+	packet->q4 = q4;
+	packet->roll = roll;
+	packet->pitch = pitch;
+	packet->yaw = yaw;
+	packet->xacc = xacc;
+	packet->yacc = yacc;
+	packet->zacc = zacc;
+	packet->xgyro = xgyro;
+	packet->ygyro = ygyro;
+	packet->zgyro = zgyro;
+	packet->lat = lat;
+	packet->lon = lon;
+	packet->alt = alt;
+	packet->std_dev_horz = std_dev_horz;
+	packet->std_dev_vert = std_dev_vert;
+	packet->vn = vn;
+	packet->ve = ve;
+	packet->vd = vd;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, (const char *)packet, MAVLINK_MSG_ID_SIM_STATE_LEN, MAVLINK_MSG_ID_SIM_STATE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, (const char *)packet, MAVLINK_MSG_ID_SIM_STATE_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SIM_STATE UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_state_correction.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_state_correction.h
index 2db627b96a..be2629dd8a 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_state_correction.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_state_correction.h
@@ -234,6 +234,54 @@ static inline void mavlink_msg_state_correction_send(mavlink_channel_t chan, flo
 #endif
 }
 
+#if MAVLINK_MSG_ID_STATE_CORRECTION_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_state_correction_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  float xErr, float yErr, float zErr, float rollErr, float pitchErr, float yawErr, float vxErr, float vyErr, float vzErr)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, xErr);
+	_mav_put_float(buf, 4, yErr);
+	_mav_put_float(buf, 8, zErr);
+	_mav_put_float(buf, 12, rollErr);
+	_mav_put_float(buf, 16, pitchErr);
+	_mav_put_float(buf, 20, yawErr);
+	_mav_put_float(buf, 24, vxErr);
+	_mav_put_float(buf, 28, vyErr);
+	_mav_put_float(buf, 32, vzErr);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATE_CORRECTION, buf, MAVLINK_MSG_ID_STATE_CORRECTION_LEN, MAVLINK_MSG_ID_STATE_CORRECTION_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATE_CORRECTION, buf, MAVLINK_MSG_ID_STATE_CORRECTION_LEN);
+#endif
+#else
+	mavlink_state_correction_t *packet = (mavlink_state_correction_t *)msgbuf;
+	packet->xErr = xErr;
+	packet->yErr = yErr;
+	packet->zErr = zErr;
+	packet->rollErr = rollErr;
+	packet->pitchErr = pitchErr;
+	packet->yawErr = yawErr;
+	packet->vxErr = vxErr;
+	packet->vyErr = vyErr;
+	packet->vzErr = vzErr;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATE_CORRECTION, (const char *)packet, MAVLINK_MSG_ID_STATE_CORRECTION_LEN, MAVLINK_MSG_ID_STATE_CORRECTION_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATE_CORRECTION, (const char *)packet, MAVLINK_MSG_ID_STATE_CORRECTION_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE STATE_CORRECTION UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h
index 536ca06349..c8c2547b31 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h
@@ -151,6 +151,38 @@ static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t s
 #endif
 }
 
+#if MAVLINK_MSG_ID_STATUSTEXT_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_statustext_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t severity, const char *text)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint8_t(buf, 0, severity);
+	_mav_put_char_array(buf, 1, text, 50);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, buf, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, buf, MAVLINK_MSG_ID_STATUSTEXT_LEN);
+#endif
+#else
+	mavlink_statustext_t *packet = (mavlink_statustext_t *)msgbuf;
+	packet->severity = severity;
+	mav_array_memcpy(packet->text, text, sizeof(char)*50);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, (const char *)packet, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, (const char *)packet, MAVLINK_MSG_ID_STATUSTEXT_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE STATUSTEXT UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h
index 101b366783..1b1730d5f5 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h
@@ -278,6 +278,62 @@ static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint32_t
 #endif
 }
 
+#if MAVLINK_MSG_ID_SYS_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_sys_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint32_t onboard_control_sensors_present, uint32_t onboard_control_sensors_enabled, uint32_t onboard_control_sensors_health, uint16_t load, uint16_t voltage_battery, int16_t current_battery, int8_t battery_remaining, uint16_t drop_rate_comm, uint16_t errors_comm, uint16_t errors_count1, uint16_t errors_count2, uint16_t errors_count3, uint16_t errors_count4)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint32_t(buf, 0, onboard_control_sensors_present);
+	_mav_put_uint32_t(buf, 4, onboard_control_sensors_enabled);
+	_mav_put_uint32_t(buf, 8, onboard_control_sensors_health);
+	_mav_put_uint16_t(buf, 12, load);
+	_mav_put_uint16_t(buf, 14, voltage_battery);
+	_mav_put_int16_t(buf, 16, current_battery);
+	_mav_put_uint16_t(buf, 18, drop_rate_comm);
+	_mav_put_uint16_t(buf, 20, errors_comm);
+	_mav_put_uint16_t(buf, 22, errors_count1);
+	_mav_put_uint16_t(buf, 24, errors_count2);
+	_mav_put_uint16_t(buf, 26, errors_count3);
+	_mav_put_uint16_t(buf, 28, errors_count4);
+	_mav_put_int8_t(buf, 30, battery_remaining);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, buf, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, buf, MAVLINK_MSG_ID_SYS_STATUS_LEN);
+#endif
+#else
+	mavlink_sys_status_t *packet = (mavlink_sys_status_t *)msgbuf;
+	packet->onboard_control_sensors_present = onboard_control_sensors_present;
+	packet->onboard_control_sensors_enabled = onboard_control_sensors_enabled;
+	packet->onboard_control_sensors_health = onboard_control_sensors_health;
+	packet->load = load;
+	packet->voltage_battery = voltage_battery;
+	packet->current_battery = current_battery;
+	packet->drop_rate_comm = drop_rate_comm;
+	packet->errors_comm = errors_comm;
+	packet->errors_count1 = errors_count1;
+	packet->errors_count2 = errors_count2;
+	packet->errors_count3 = errors_count3;
+	packet->errors_count4 = errors_count4;
+	packet->battery_remaining = battery_remaining;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, (const char *)packet, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, (const char *)packet, MAVLINK_MSG_ID_SYS_STATUS_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SYS_STATUS UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_system_time.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_system_time.h
index 1807567aea..988c669c36 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_system_time.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_system_time.h
@@ -157,6 +157,40 @@ static inline void mavlink_msg_system_time_send(mavlink_channel_t chan, uint64_t
 #endif
 }
 
+#if MAVLINK_MSG_ID_SYSTEM_TIME_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_system_time_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint64_t time_unix_usec, uint32_t time_boot_ms)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint64_t(buf, 0, time_unix_usec);
+	_mav_put_uint32_t(buf, 8, time_boot_ms);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, buf, MAVLINK_MSG_ID_SYSTEM_TIME_LEN, MAVLINK_MSG_ID_SYSTEM_TIME_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, buf, MAVLINK_MSG_ID_SYSTEM_TIME_LEN);
+#endif
+#else
+	mavlink_system_time_t *packet = (mavlink_system_time_t *)msgbuf;
+	packet->time_unix_usec = time_unix_usec;
+	packet->time_boot_ms = time_boot_ms;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, (const char *)packet, MAVLINK_MSG_ID_SYSTEM_TIME_LEN, MAVLINK_MSG_ID_SYSTEM_TIME_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, (const char *)packet, MAVLINK_MSG_ID_SYSTEM_TIME_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SYSTEM_TIME UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vfr_hud.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vfr_hud.h
index 5b1093a3d7..b130ee50b5 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vfr_hud.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vfr_hud.h
@@ -201,6 +201,48 @@ static inline void mavlink_msg_vfr_hud_send(mavlink_channel_t chan, float airspe
 #endif
 }
 
+#if MAVLINK_MSG_ID_VFR_HUD_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_vfr_hud_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  float airspeed, float groundspeed, int16_t heading, uint16_t throttle, float alt, float climb)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, airspeed);
+	_mav_put_float(buf, 4, groundspeed);
+	_mav_put_float(buf, 8, alt);
+	_mav_put_float(buf, 12, climb);
+	_mav_put_int16_t(buf, 16, heading);
+	_mav_put_uint16_t(buf, 18, throttle);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, buf, MAVLINK_MSG_ID_VFR_HUD_LEN, MAVLINK_MSG_ID_VFR_HUD_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, buf, MAVLINK_MSG_ID_VFR_HUD_LEN);
+#endif
+#else
+	mavlink_vfr_hud_t *packet = (mavlink_vfr_hud_t *)msgbuf;
+	packet->airspeed = airspeed;
+	packet->groundspeed = groundspeed;
+	packet->alt = alt;
+	packet->climb = climb;
+	packet->heading = heading;
+	packet->throttle = throttle;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, (const char *)packet, MAVLINK_MSG_ID_VFR_HUD_LEN, MAVLINK_MSG_ID_VFR_HUD_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, (const char *)packet, MAVLINK_MSG_ID_VFR_HUD_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE VFR_HUD UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vicon_position_estimate.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vicon_position_estimate.h
index a254202e4e..b3fa7bc1c1 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vicon_position_estimate.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vicon_position_estimate.h
@@ -212,6 +212,50 @@ static inline void mavlink_msg_vicon_position_estimate_send(mavlink_channel_t ch
 #endif
 }
 
+#if MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_vicon_position_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint64_t(buf, 0, usec);
+	_mav_put_float(buf, 8, x);
+	_mav_put_float(buf, 12, y);
+	_mav_put_float(buf, 16, z);
+	_mav_put_float(buf, 20, roll);
+	_mav_put_float(buf, 24, pitch);
+	_mav_put_float(buf, 28, yaw);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN);
+#endif
+#else
+	mavlink_vicon_position_estimate_t *packet = (mavlink_vicon_position_estimate_t *)msgbuf;
+	packet->usec = usec;
+	packet->x = x;
+	packet->y = y;
+	packet->z = z;
+	packet->roll = roll;
+	packet->pitch = pitch;
+	packet->yaw = yaw;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE VICON_POSITION_ESTIMATE UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_position_estimate.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_position_estimate.h
index f7a741b09e..8f82fb6824 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_position_estimate.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_position_estimate.h
@@ -212,6 +212,50 @@ static inline void mavlink_msg_vision_position_estimate_send(mavlink_channel_t c
 #endif
 }
 
+#if MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_vision_position_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint64_t(buf, 0, usec);
+	_mav_put_float(buf, 8, x);
+	_mav_put_float(buf, 12, y);
+	_mav_put_float(buf, 16, z);
+	_mav_put_float(buf, 20, roll);
+	_mav_put_float(buf, 24, pitch);
+	_mav_put_float(buf, 28, yaw);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN);
+#endif
+#else
+	mavlink_vision_position_estimate_t *packet = (mavlink_vision_position_estimate_t *)msgbuf;
+	packet->usec = usec;
+	packet->x = x;
+	packet->y = y;
+	packet->z = z;
+	packet->roll = roll;
+	packet->pitch = pitch;
+	packet->yaw = yaw;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE VISION_POSITION_ESTIMATE UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_speed_estimate.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_speed_estimate.h
index 6602251283..7528014c76 100644
--- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_speed_estimate.h
+++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_speed_estimate.h
@@ -179,6 +179,44 @@ static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan
 #endif
 }
 
+#if MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_vision_speed_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint64_t usec, float x, float y, float z)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint64_t(buf, 0, usec);
+	_mav_put_float(buf, 8, x);
+	_mav_put_float(buf, 12, y);
+	_mav_put_float(buf, 16, z);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN);
+#endif
+#else
+	mavlink_vision_speed_estimate_t *packet = (mavlink_vision_speed_estimate_t *)msgbuf;
+	packet->usec = usec;
+	packet->x = x;
+	packet->y = y;
+	packet->z = z;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE VISION_SPEED_ESTIMATE UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/common/testsuite.h b/mavlink/include/mavlink/v1.0/common/testsuite.h
index c5aa9ddf3a..de23fcb2a2 100644
--- a/mavlink/include/mavlink/v1.0/common/testsuite.h
+++ b/mavlink/include/mavlink/v1.0/common/testsuite.h
@@ -2805,6 +2805,89 @@ static void mavlink_test_state_correction(uint8_t system_id, uint8_t component_i
         MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
 }
 
+static void mavlink_test_rc_channels(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
+{
+	mavlink_message_t msg;
+        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
+        uint16_t i;
+	mavlink_rc_channels_t packet_in = {
+		963497464,
+	}17443,
+	}17547,
+	}17651,
+	}17755,
+	}17859,
+	}17963,
+	}18067,
+	}18171,
+	}18275,
+	}18379,
+	}18483,
+	}18587,
+	}18691,
+	}18795,
+	}18899,
+	}19003,
+	}19107,
+	}19211,
+	}125,
+	}192,
+	};
+	mavlink_rc_channels_t packet1, packet2;
+        memset(&packet1, 0, sizeof(packet1));
+        	packet1.time_boot_ms = packet_in.time_boot_ms;
+        	packet1.chan1_raw = packet_in.chan1_raw;
+        	packet1.chan2_raw = packet_in.chan2_raw;
+        	packet1.chan3_raw = packet_in.chan3_raw;
+        	packet1.chan4_raw = packet_in.chan4_raw;
+        	packet1.chan5_raw = packet_in.chan5_raw;
+        	packet1.chan6_raw = packet_in.chan6_raw;
+        	packet1.chan7_raw = packet_in.chan7_raw;
+        	packet1.chan8_raw = packet_in.chan8_raw;
+        	packet1.chan9_raw = packet_in.chan9_raw;
+        	packet1.chan10_raw = packet_in.chan10_raw;
+        	packet1.chan11_raw = packet_in.chan11_raw;
+        	packet1.chan12_raw = packet_in.chan12_raw;
+        	packet1.chan13_raw = packet_in.chan13_raw;
+        	packet1.chan14_raw = packet_in.chan14_raw;
+        	packet1.chan15_raw = packet_in.chan15_raw;
+        	packet1.chan16_raw = packet_in.chan16_raw;
+        	packet1.chan17_raw = packet_in.chan17_raw;
+        	packet1.chan18_raw = packet_in.chan18_raw;
+        	packet1.chancount = packet_in.chancount;
+        	packet1.rssi = packet_in.rssi;
+        
+        
+
+        memset(&packet2, 0, sizeof(packet2));
+	mavlink_msg_rc_channels_encode(system_id, component_id, &msg, &packet1);
+	mavlink_msg_rc_channels_decode(&msg, &packet2);
+        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+        memset(&packet2, 0, sizeof(packet2));
+	mavlink_msg_rc_channels_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.chancount , packet1.chan1_raw , packet1.chan2_raw , packet1.chan3_raw , packet1.chan4_raw , packet1.chan5_raw , packet1.chan6_raw , packet1.chan7_raw , packet1.chan8_raw , packet1.chan9_raw , packet1.chan10_raw , packet1.chan11_raw , packet1.chan12_raw , packet1.chan13_raw , packet1.chan14_raw , packet1.chan15_raw , packet1.chan16_raw , packet1.chan17_raw , packet1.chan18_raw , packet1.rssi );
+	mavlink_msg_rc_channels_decode(&msg, &packet2);
+        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+        memset(&packet2, 0, sizeof(packet2));
+	mavlink_msg_rc_channels_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.chancount , packet1.chan1_raw , packet1.chan2_raw , packet1.chan3_raw , packet1.chan4_raw , packet1.chan5_raw , packet1.chan6_raw , packet1.chan7_raw , packet1.chan8_raw , packet1.chan9_raw , packet1.chan10_raw , packet1.chan11_raw , packet1.chan12_raw , packet1.chan13_raw , packet1.chan14_raw , packet1.chan15_raw , packet1.chan16_raw , packet1.chan17_raw , packet1.chan18_raw , packet1.rssi );
+	mavlink_msg_rc_channels_decode(&msg, &packet2);
+        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+        memset(&packet2, 0, sizeof(packet2));
+        mavlink_msg_to_send_buffer(buffer, &msg);
+        for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
+        	comm_send_ch(MAVLINK_COMM_0, buffer[i]);
+        }
+	mavlink_msg_rc_channels_decode(last_msg, &packet2);
+        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+        
+        memset(&packet2, 0, sizeof(packet2));
+	mavlink_msg_rc_channels_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.chancount , packet1.chan1_raw , packet1.chan2_raw , packet1.chan3_raw , packet1.chan4_raw , packet1.chan5_raw , packet1.chan6_raw , packet1.chan7_raw , packet1.chan8_raw , packet1.chan9_raw , packet1.chan10_raw , packet1.chan11_raw , packet1.chan12_raw , packet1.chan13_raw , packet1.chan14_raw , packet1.chan15_raw , packet1.chan16_raw , packet1.chan17_raw , packet1.chan18_raw , packet1.rssi );
+	mavlink_msg_rc_channels_decode(last_msg, &packet2);
+        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+}
+
 static void mavlink_test_request_data_stream(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
 {
 	mavlink_message_t msg;
@@ -4953,6 +5036,106 @@ static void mavlink_test_gps2_raw(uint8_t system_id, uint8_t component_id, mavli
         MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
 }
 
+static void mavlink_test_power_status(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
+{
+	mavlink_message_t msg;
+        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
+        uint16_t i;
+	mavlink_power_status_t packet_in = {
+		17235,
+	}17339,
+	}17443,
+	};
+	mavlink_power_status_t packet1, packet2;
+        memset(&packet1, 0, sizeof(packet1));
+        	packet1.Vcc = packet_in.Vcc;
+        	packet1.Vservo = packet_in.Vservo;
+        	packet1.flags = packet_in.flags;
+        
+        
+
+        memset(&packet2, 0, sizeof(packet2));
+	mavlink_msg_power_status_encode(system_id, component_id, &msg, &packet1);
+	mavlink_msg_power_status_decode(&msg, &packet2);
+        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+        memset(&packet2, 0, sizeof(packet2));
+	mavlink_msg_power_status_pack(system_id, component_id, &msg , packet1.Vcc , packet1.Vservo , packet1.flags );
+	mavlink_msg_power_status_decode(&msg, &packet2);
+        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+        memset(&packet2, 0, sizeof(packet2));
+	mavlink_msg_power_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.Vcc , packet1.Vservo , packet1.flags );
+	mavlink_msg_power_status_decode(&msg, &packet2);
+        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+        memset(&packet2, 0, sizeof(packet2));
+        mavlink_msg_to_send_buffer(buffer, &msg);
+        for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
+        	comm_send_ch(MAVLINK_COMM_0, buffer[i]);
+        }
+	mavlink_msg_power_status_decode(last_msg, &packet2);
+        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+        
+        memset(&packet2, 0, sizeof(packet2));
+	mavlink_msg_power_status_send(MAVLINK_COMM_1 , packet1.Vcc , packet1.Vservo , packet1.flags );
+	mavlink_msg_power_status_decode(last_msg, &packet2);
+        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+}
+
+static void mavlink_test_serial_control(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
+{
+	mavlink_message_t msg;
+        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
+        uint16_t i;
+	mavlink_serial_control_t packet_in = {
+		963497464,
+	}17443,
+	}151,
+	}218,
+	}29,
+	}{ 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165 },
+	};
+	mavlink_serial_control_t packet1, packet2;
+        memset(&packet1, 0, sizeof(packet1));
+        	packet1.baudrate = packet_in.baudrate;
+        	packet1.timeout = packet_in.timeout;
+        	packet1.device = packet_in.device;
+        	packet1.flags = packet_in.flags;
+        	packet1.count = packet_in.count;
+        
+        	mav_array_memcpy(packet1.data, packet_in.data, sizeof(uint8_t)*70);
+        
+
+        memset(&packet2, 0, sizeof(packet2));
+	mavlink_msg_serial_control_encode(system_id, component_id, &msg, &packet1);
+	mavlink_msg_serial_control_decode(&msg, &packet2);
+        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+        memset(&packet2, 0, sizeof(packet2));
+	mavlink_msg_serial_control_pack(system_id, component_id, &msg , packet1.device , packet1.flags , packet1.timeout , packet1.baudrate , packet1.count , packet1.data );
+	mavlink_msg_serial_control_decode(&msg, &packet2);
+        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+        memset(&packet2, 0, sizeof(packet2));
+	mavlink_msg_serial_control_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.device , packet1.flags , packet1.timeout , packet1.baudrate , packet1.count , packet1.data );
+	mavlink_msg_serial_control_decode(&msg, &packet2);
+        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+        memset(&packet2, 0, sizeof(packet2));
+        mavlink_msg_to_send_buffer(buffer, &msg);
+        for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
+        	comm_send_ch(MAVLINK_COMM_0, buffer[i]);
+        }
+	mavlink_msg_serial_control_decode(last_msg, &packet2);
+        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+        
+        memset(&packet2, 0, sizeof(packet2));
+	mavlink_msg_serial_control_send(MAVLINK_COMM_1 , packet1.device , packet1.flags , packet1.timeout , packet1.baudrate , packet1.count , packet1.data );
+	mavlink_msg_serial_control_decode(last_msg, &packet2);
+        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+}
+
 static void mavlink_test_data_transmission_handshake(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
 {
 	mavlink_message_t msg;
@@ -5053,6 +5236,63 @@ static void mavlink_test_encapsulated_data(uint8_t system_id, uint8_t component_
         MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
 }
 
+static void mavlink_test_distance_sensor(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
+{
+	mavlink_message_t msg;
+        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
+        uint16_t i;
+	mavlink_distance_sensor_t packet_in = {
+		963497464,
+	}17443,
+	}17547,
+	}17651,
+	}163,
+	}230,
+	}41,
+	}108,
+	};
+	mavlink_distance_sensor_t packet1, packet2;
+        memset(&packet1, 0, sizeof(packet1));
+        	packet1.time_boot_ms = packet_in.time_boot_ms;
+        	packet1.min_distance = packet_in.min_distance;
+        	packet1.max_distance = packet_in.max_distance;
+        	packet1.current_distance = packet_in.current_distance;
+        	packet1.type = packet_in.type;
+        	packet1.id = packet_in.id;
+        	packet1.orientation = packet_in.orientation;
+        	packet1.covariance = packet_in.covariance;
+        
+        
+
+        memset(&packet2, 0, sizeof(packet2));
+	mavlink_msg_distance_sensor_encode(system_id, component_id, &msg, &packet1);
+	mavlink_msg_distance_sensor_decode(&msg, &packet2);
+        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+        memset(&packet2, 0, sizeof(packet2));
+	mavlink_msg_distance_sensor_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.type , packet1.id , packet1.orientation , packet1.min_distance , packet1.max_distance , packet1.current_distance , packet1.covariance );
+	mavlink_msg_distance_sensor_decode(&msg, &packet2);
+        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+        memset(&packet2, 0, sizeof(packet2));
+	mavlink_msg_distance_sensor_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.type , packet1.id , packet1.orientation , packet1.min_distance , packet1.max_distance , packet1.current_distance , packet1.covariance );
+	mavlink_msg_distance_sensor_decode(&msg, &packet2);
+        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+        memset(&packet2, 0, sizeof(packet2));
+        mavlink_msg_to_send_buffer(buffer, &msg);
+        for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
+        	comm_send_ch(MAVLINK_COMM_0, buffer[i]);
+        }
+	mavlink_msg_distance_sensor_decode(last_msg, &packet2);
+        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+        
+        memset(&packet2, 0, sizeof(packet2));
+	mavlink_msg_distance_sensor_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.type , packet1.id , packet1.orientation , packet1.min_distance , packet1.max_distance , packet1.current_distance , packet1.covariance );
+	mavlink_msg_distance_sensor_decode(last_msg, &packet2);
+        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+}
+
 static void mavlink_test_battery_status(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
 {
 	mavlink_message_t msg;
@@ -5571,6 +5811,7 @@ static void mavlink_test_common(uint8_t system_id, uint8_t component_id, mavlink
 	mavlink_test_nav_controller_output(system_id, component_id, last_msg);
 	mavlink_test_set_quad_swarm_led_roll_pitch_yaw_thrust(system_id, component_id, last_msg);
 	mavlink_test_state_correction(system_id, component_id, last_msg);
+	mavlink_test_rc_channels(system_id, component_id, last_msg);
 	mavlink_test_request_data_stream(system_id, component_id, last_msg);
 	mavlink_test_data_stream(system_id, component_id, last_msg);
 	mavlink_test_manual_control(system_id, component_id, last_msg);
@@ -5609,8 +5850,11 @@ static void mavlink_test_common(uint8_t system_id, uint8_t component_id, mavlink
 	mavlink_test_log_request_end(system_id, component_id, last_msg);
 	mavlink_test_gps_inject_data(system_id, component_id, last_msg);
 	mavlink_test_gps2_raw(system_id, component_id, last_msg);
+	mavlink_test_power_status(system_id, component_id, last_msg);
+	mavlink_test_serial_control(system_id, component_id, last_msg);
 	mavlink_test_data_transmission_handshake(system_id, component_id, last_msg);
 	mavlink_test_encapsulated_data(system_id, component_id, last_msg);
+	mavlink_test_distance_sensor(system_id, component_id, last_msg);
 	mavlink_test_battery_status(system_id, component_id, last_msg);
 	mavlink_test_setpoint_8dof(system_id, component_id, last_msg);
 	mavlink_test_setpoint_6dof(system_id, component_id, last_msg);
diff --git a/mavlink/include/mavlink/v1.0/common/version.h b/mavlink/include/mavlink/v1.0/common/version.h
index 2f3736f454..8ad3783550 100644
--- a/mavlink/include/mavlink/v1.0/common/version.h
+++ b/mavlink/include/mavlink/v1.0/common/version.h
@@ -5,7 +5,7 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Tue Feb  4 15:28:29 2014"
+#define MAVLINK_BUILD_DATE "Sun Apr 13 09:39:04 2014"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
 #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
  
diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/matrixpilot.h b/mavlink/include/mavlink/v1.0/matrixpilot/matrixpilot.h
index 2f15d31c4d..cc7545cfef 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/matrixpilot.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/matrixpilot.h
@@ -12,15 +12,15 @@ extern "C" {
 // MESSAGE LENGTHS AND CRCS
 
 #ifndef MAVLINK_MESSAGE_LENGTHS
-#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 0, 0, 0, 0, 0, 13, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 2, 6, 58, 6, 0, 53, 7, 3, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 70, 10, 24, 20, 24, 28, 14, 17, 60, 110, 28, 16, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0}
+#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 42, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 0, 0, 0, 13, 255, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 2, 6, 58, 6, 0, 53, 7, 3, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 70, 10, 24, 20, 24, 28, 14, 17, 60, 110, 28, 16, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0}
 #endif
 
 #ifndef MAVLINK_MESSAGE_CRCS
-#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 0, 0, 0, 0, 0, 29, 223, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 181, 26, 101, 109, 0, 12, 218, 133, 208, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 150, 169, 191, 121, 54, 171, 142, 249, 123, 7, 222, 55, 154, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0}
+#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 85, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 181, 26, 101, 109, 0, 12, 218, 133, 208, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 150, 169, 191, 121, 54, 171, 142, 249, 123, 7, 222, 55, 154, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0}
 #endif
 
 #ifndef MAVLINK_MESSAGE_INFO
-#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_SET, MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_READ_REQ, MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_BUFFER_FUNCTION, MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_BUFFER_FUNCTION_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_DIRECTORY, MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_DIRECTORY_ACK, MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_COMMAND, MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F2_A, MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F2_B, MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F4, MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F5, MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F6, MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F7, MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F8, MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F13, MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F14, MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F15, MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F16, MAVLINK_MESSAGE_INFO_ALTITUDES, MAVLINK_MESSAGE_INFO_AIRSPEEDS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}}
+#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_SET, MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_READ_REQ, MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_BUFFER_FUNCTION, MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_BUFFER_FUNCTION_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_DIRECTORY, MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_DIRECTORY_ACK, MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_COMMAND, MAVLINK_MESSAGE_INFO_FLEXIFUNCTION_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F2_A, MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F2_B, MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F4, MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F5, MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F6, MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F7, MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F8, MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F13, MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F14, MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F15, MAVLINK_MESSAGE_INFO_SERIAL_UDB_EXTRA_F16, MAVLINK_MESSAGE_INFO_ALTITUDES, MAVLINK_MESSAGE_INFO_AIRSPEEDS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}}
 #endif
 
 #include "../protocol.h"
@@ -61,6 +61,7 @@ enum MAV_CMD
 	MAV_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude|  */
 	MAV_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|  */
 	MAV_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal|  */
+	MAV_CMD_NAV_SPLINE_WAYPOINT=82, /* Navigate to MISSION using a spline path. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Empty| Empty| Empty| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal|  */
 	MAV_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
 	MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty|  */
 	MAV_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate.  Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude|  */
@@ -78,8 +79,16 @@ enum MAV_CMD
 	MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty|  */
 	MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty|  */
 	MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|  */
+	MAV_CMD_DO_DIGICAM_CONFIGURE=202, /* Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)|  */
+	MAV_CMD_DO_DIGICAM_CONTROL=203, /* Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty|  */
+	MAV_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty|  */
+	MAV_CMD_DO_MOUNT_CONTROL=205, /* Mission command to control a camera or antenna mount |pitch or lat in degrees, depending on mount mode.| roll or lon in degrees depending on mount mode| yaw or alt (in meters) depending on mount mode| reserved| reserved| reserved| MAV_MOUNT_MODE enum value|  */
+	MAV_CMD_DO_SET_CAM_TRIGG_DIST=206, /* Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty|  */
+	MAV_CMD_DO_FENCE_ENABLE=207, /* Mission command to enable the geofence |enable? (0=disable, 1=enable)| Empty| Empty| Empty| Empty| Empty| Empty|  */
+	MAV_CMD_DO_PARACHUTE=208, /* Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty|  */
+	MAV_CMD_DO_MOUNT_CONTROL_QUAT=220, /* Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1| q2 - quaternion param #2| q3 - quaternion param #3| q4 - quaternion param #4| Empty| Empty| Empty|  */
 	MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty|  */
+	MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Compass/Motor interference calibration: 0: no, 1: yes| Empty|  */
 	MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units|  */
 	MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty|  */
 	MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty|  */
diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h
index bc47a547ad..c834a3df09 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h
@@ -212,6 +212,50 @@ static inline void mavlink_msg_airspeeds_send(mavlink_channel_t chan, uint32_t t
 #endif
 }
 
+#if MAVLINK_MSG_ID_AIRSPEEDS_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_airspeeds_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint32_t time_boot_ms, int16_t airspeed_imu, int16_t airspeed_pitot, int16_t airspeed_hot_wire, int16_t airspeed_ultrasonic, int16_t aoa, int16_t aoy)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint32_t(buf, 0, time_boot_ms);
+	_mav_put_int16_t(buf, 4, airspeed_imu);
+	_mav_put_int16_t(buf, 6, airspeed_pitot);
+	_mav_put_int16_t(buf, 8, airspeed_hot_wire);
+	_mav_put_int16_t(buf, 10, airspeed_ultrasonic);
+	_mav_put_int16_t(buf, 12, aoa);
+	_mav_put_int16_t(buf, 14, aoy);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEEDS, buf, MAVLINK_MSG_ID_AIRSPEEDS_LEN, MAVLINK_MSG_ID_AIRSPEEDS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEEDS, buf, MAVLINK_MSG_ID_AIRSPEEDS_LEN);
+#endif
+#else
+	mavlink_airspeeds_t *packet = (mavlink_airspeeds_t *)msgbuf;
+	packet->time_boot_ms = time_boot_ms;
+	packet->airspeed_imu = airspeed_imu;
+	packet->airspeed_pitot = airspeed_pitot;
+	packet->airspeed_hot_wire = airspeed_hot_wire;
+	packet->airspeed_ultrasonic = airspeed_ultrasonic;
+	packet->aoa = aoa;
+	packet->aoy = aoy;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEEDS, (const char *)packet, MAVLINK_MSG_ID_AIRSPEEDS_LEN, MAVLINK_MSG_ID_AIRSPEEDS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEEDS, (const char *)packet, MAVLINK_MSG_ID_AIRSPEEDS_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE AIRSPEEDS UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h
index e64787cc73..b009068ffc 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h
@@ -212,6 +212,50 @@ static inline void mavlink_msg_altitudes_send(mavlink_channel_t chan, uint32_t t
 #endif
 }
 
+#if MAVLINK_MSG_ID_ALTITUDES_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_altitudes_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint32_t time_boot_ms, int32_t alt_gps, int32_t alt_imu, int32_t alt_barometric, int32_t alt_optical_flow, int32_t alt_range_finder, int32_t alt_extra)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint32_t(buf, 0, time_boot_ms);
+	_mav_put_int32_t(buf, 4, alt_gps);
+	_mav_put_int32_t(buf, 8, alt_imu);
+	_mav_put_int32_t(buf, 12, alt_barometric);
+	_mav_put_int32_t(buf, 16, alt_optical_flow);
+	_mav_put_int32_t(buf, 20, alt_range_finder);
+	_mav_put_int32_t(buf, 24, alt_extra);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ALTITUDES, buf, MAVLINK_MSG_ID_ALTITUDES_LEN, MAVLINK_MSG_ID_ALTITUDES_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ALTITUDES, buf, MAVLINK_MSG_ID_ALTITUDES_LEN);
+#endif
+#else
+	mavlink_altitudes_t *packet = (mavlink_altitudes_t *)msgbuf;
+	packet->time_boot_ms = time_boot_ms;
+	packet->alt_gps = alt_gps;
+	packet->alt_imu = alt_imu;
+	packet->alt_barometric = alt_barometric;
+	packet->alt_optical_flow = alt_optical_flow;
+	packet->alt_range_finder = alt_range_finder;
+	packet->alt_extra = alt_extra;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ALTITUDES, (const char *)packet, MAVLINK_MSG_ID_ALTITUDES_LEN, MAVLINK_MSG_ID_ALTITUDES_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ALTITUDES, (const char *)packet, MAVLINK_MSG_ID_ALTITUDES_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE ALTITUDES UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function.h
index 581bd35dc5..e7b803a14b 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function.h
@@ -206,6 +206,48 @@ static inline void mavlink_msg_flexifunction_buffer_function_send(mavlink_channe
 #endif
 }
 
+#if MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_flexifunction_buffer_function_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component, uint16_t func_index, uint16_t func_count, uint16_t data_address, uint16_t data_size, const int8_t *data)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint16_t(buf, 0, func_index);
+	_mav_put_uint16_t(buf, 2, func_count);
+	_mav_put_uint16_t(buf, 4, data_address);
+	_mav_put_uint16_t(buf, 6, data_size);
+	_mav_put_uint8_t(buf, 8, target_system);
+	_mav_put_uint8_t(buf, 9, target_component);
+	_mav_put_int8_t_array(buf, 10, data, 48);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN);
+#endif
+#else
+	mavlink_flexifunction_buffer_function_t *packet = (mavlink_flexifunction_buffer_function_t *)msgbuf;
+	packet->func_index = func_index;
+	packet->func_count = func_count;
+	packet->data_address = data_address;
+	packet->data_size = data_size;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+	mav_array_memcpy(packet->data, data, sizeof(int8_t)*48);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE FLEXIFUNCTION_BUFFER_FUNCTION UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function_ack.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function_ack.h
index 790afd52b3..06631b40ed 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function_ack.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function_ack.h
@@ -179,6 +179,44 @@ static inline void mavlink_msg_flexifunction_buffer_function_ack_send(mavlink_ch
 #endif
 }
 
+#if MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_flexifunction_buffer_function_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component, uint16_t func_index, uint16_t result)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint16_t(buf, 0, func_index);
+	_mav_put_uint16_t(buf, 2, result);
+	_mav_put_uint8_t(buf, 4, target_system);
+	_mav_put_uint8_t(buf, 5, target_component);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN);
+#endif
+#else
+	mavlink_flexifunction_buffer_function_ack_t *packet = (mavlink_flexifunction_buffer_function_ack_t *)msgbuf;
+	packet->func_index = func_index;
+	packet->result = result;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE FLEXIFUNCTION_BUFFER_FUNCTION_ACK UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command.h
index ce722c8a4d..ac8366eefe 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command.h
@@ -168,6 +168,42 @@ static inline void mavlink_msg_flexifunction_command_send(mavlink_channel_t chan
 #endif
 }
 
+#if MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_flexifunction_command_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component, uint8_t command_type)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint8_t(buf, 0, target_system);
+	_mav_put_uint8_t(buf, 1, target_component);
+	_mav_put_uint8_t(buf, 2, command_type);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN);
+#endif
+#else
+	mavlink_flexifunction_command_t *packet = (mavlink_flexifunction_command_t *)msgbuf;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+	packet->command_type = command_type;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE FLEXIFUNCTION_COMMAND UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command_ack.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command_ack.h
index 070dc4bf8d..c88e2dc39f 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command_ack.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command_ack.h
@@ -157,6 +157,40 @@ static inline void mavlink_msg_flexifunction_command_ack_send(mavlink_channel_t
 #endif
 }
 
+#if MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_flexifunction_command_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint16_t command_type, uint16_t result)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint16_t(buf, 0, command_type);
+	_mav_put_uint16_t(buf, 2, result);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN);
+#endif
+#else
+	mavlink_flexifunction_command_ack_t *packet = (mavlink_flexifunction_command_ack_t *)msgbuf;
+	packet->command_type = command_type;
+	packet->result = result;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE FLEXIFUNCTION_COMMAND_ACK UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory.h
index ef262a6b1d..619f810c55 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory.h
@@ -195,6 +195,46 @@ static inline void mavlink_msg_flexifunction_directory_send(mavlink_channel_t ch
 #endif
 }
 
+#if MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_flexifunction_directory_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component, uint8_t directory_type, uint8_t start_index, uint8_t count, const int8_t *directory_data)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint8_t(buf, 0, target_system);
+	_mav_put_uint8_t(buf, 1, target_component);
+	_mav_put_uint8_t(buf, 2, directory_type);
+	_mav_put_uint8_t(buf, 3, start_index);
+	_mav_put_uint8_t(buf, 4, count);
+	_mav_put_int8_t_array(buf, 5, directory_data, 48);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN);
+#endif
+#else
+	mavlink_flexifunction_directory_t *packet = (mavlink_flexifunction_directory_t *)msgbuf;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+	packet->directory_type = directory_type;
+	packet->start_index = start_index;
+	packet->count = count;
+	mav_array_memcpy(packet->directory_data, directory_data, sizeof(int8_t)*48);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE FLEXIFUNCTION_DIRECTORY UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory_ack.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory_ack.h
index d3a386ce54..0287183d85 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory_ack.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory_ack.h
@@ -201,6 +201,48 @@ static inline void mavlink_msg_flexifunction_directory_ack_send(mavlink_channel_
 #endif
 }
 
+#if MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_flexifunction_directory_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component, uint8_t directory_type, uint8_t start_index, uint8_t count, uint16_t result)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint16_t(buf, 0, result);
+	_mav_put_uint8_t(buf, 2, target_system);
+	_mav_put_uint8_t(buf, 3, target_component);
+	_mav_put_uint8_t(buf, 4, directory_type);
+	_mav_put_uint8_t(buf, 5, start_index);
+	_mav_put_uint8_t(buf, 6, count);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN);
+#endif
+#else
+	mavlink_flexifunction_directory_ack_t *packet = (mavlink_flexifunction_directory_ack_t *)msgbuf;
+	packet->result = result;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+	packet->directory_type = directory_type;
+	packet->start_index = start_index;
+	packet->count = count;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE FLEXIFUNCTION_DIRECTORY_ACK UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_read_req.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_read_req.h
index e50f77b080..570782e792 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_read_req.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_read_req.h
@@ -179,6 +179,44 @@ static inline void mavlink_msg_flexifunction_read_req_send(mavlink_channel_t cha
 #endif
 }
 
+#if MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_flexifunction_read_req_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component, int16_t read_req_type, int16_t data_index)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_int16_t(buf, 0, read_req_type);
+	_mav_put_int16_t(buf, 2, data_index);
+	_mav_put_uint8_t(buf, 4, target_system);
+	_mav_put_uint8_t(buf, 5, target_component);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN);
+#endif
+#else
+	mavlink_flexifunction_read_req_t *packet = (mavlink_flexifunction_read_req_t *)msgbuf;
+	packet->read_req_type = read_req_type;
+	packet->data_index = data_index;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE FLEXIFUNCTION_READ_REQ UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_set.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_set.h
index 41afb3811d..c10000640d 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_set.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_set.h
@@ -157,6 +157,40 @@ static inline void mavlink_msg_flexifunction_set_send(mavlink_channel_t chan, ui
 #endif
 }
 
+#if MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_flexifunction_set_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint8_t(buf, 0, target_system);
+	_mav_put_uint8_t(buf, 1, target_component);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_SET, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_SET, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN);
+#endif
+#else
+	mavlink_flexifunction_set_t *packet = (mavlink_flexifunction_set_t *)msgbuf;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_SET, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_SET, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE FLEXIFUNCTION_SET UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f13.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f13.h
index d21ab48168..bbb753a06b 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f13.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f13.h
@@ -179,6 +179,44 @@ static inline void mavlink_msg_serial_udb_extra_f13_send(mavlink_channel_t chan,
 #endif
 }
 
+#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_serial_udb_extra_f13_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  int16_t sue_week_no, int32_t sue_lat_origin, int32_t sue_lon_origin, int32_t sue_alt_origin)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_int32_t(buf, 0, sue_lat_origin);
+	_mav_put_int32_t(buf, 4, sue_lon_origin);
+	_mav_put_int32_t(buf, 8, sue_alt_origin);
+	_mav_put_int16_t(buf, 12, sue_week_no);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN);
+#endif
+#else
+	mavlink_serial_udb_extra_f13_t *packet = (mavlink_serial_udb_extra_f13_t *)msgbuf;
+	packet->sue_lat_origin = sue_lat_origin;
+	packet->sue_lon_origin = sue_lon_origin;
+	packet->sue_alt_origin = sue_alt_origin;
+	packet->sue_week_no = sue_week_no;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SERIAL_UDB_EXTRA_F13 UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f14.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f14.h
index 6ac12f28a6..112564d006 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f14.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f14.h
@@ -256,6 +256,58 @@ static inline void mavlink_msg_serial_udb_extra_f14_send(mavlink_channel_t chan,
 #endif
 }
 
+#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_serial_udb_extra_f14_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t sue_WIND_ESTIMATION, uint8_t sue_GPS_TYPE, uint8_t sue_DR, uint8_t sue_BOARD_TYPE, uint8_t sue_AIRFRAME, int16_t sue_RCON, int16_t sue_TRAP_FLAGS, uint32_t sue_TRAP_SOURCE, int16_t sue_osc_fail_count, uint8_t sue_CLOCK_CONFIG, uint8_t sue_FLIGHT_PLAN_TYPE)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint32_t(buf, 0, sue_TRAP_SOURCE);
+	_mav_put_int16_t(buf, 4, sue_RCON);
+	_mav_put_int16_t(buf, 6, sue_TRAP_FLAGS);
+	_mav_put_int16_t(buf, 8, sue_osc_fail_count);
+	_mav_put_uint8_t(buf, 10, sue_WIND_ESTIMATION);
+	_mav_put_uint8_t(buf, 11, sue_GPS_TYPE);
+	_mav_put_uint8_t(buf, 12, sue_DR);
+	_mav_put_uint8_t(buf, 13, sue_BOARD_TYPE);
+	_mav_put_uint8_t(buf, 14, sue_AIRFRAME);
+	_mav_put_uint8_t(buf, 15, sue_CLOCK_CONFIG);
+	_mav_put_uint8_t(buf, 16, sue_FLIGHT_PLAN_TYPE);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN);
+#endif
+#else
+	mavlink_serial_udb_extra_f14_t *packet = (mavlink_serial_udb_extra_f14_t *)msgbuf;
+	packet->sue_TRAP_SOURCE = sue_TRAP_SOURCE;
+	packet->sue_RCON = sue_RCON;
+	packet->sue_TRAP_FLAGS = sue_TRAP_FLAGS;
+	packet->sue_osc_fail_count = sue_osc_fail_count;
+	packet->sue_WIND_ESTIMATION = sue_WIND_ESTIMATION;
+	packet->sue_GPS_TYPE = sue_GPS_TYPE;
+	packet->sue_DR = sue_DR;
+	packet->sue_BOARD_TYPE = sue_BOARD_TYPE;
+	packet->sue_AIRFRAME = sue_AIRFRAME;
+	packet->sue_CLOCK_CONFIG = sue_CLOCK_CONFIG;
+	packet->sue_FLIGHT_PLAN_TYPE = sue_FLIGHT_PLAN_TYPE;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SERIAL_UDB_EXTRA_F14 UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f15.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f15.h
index 10c3f4ca4f..77666407de 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f15.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f15.h
@@ -158,6 +158,40 @@ static inline void mavlink_msg_serial_udb_extra_f15_send(mavlink_channel_t chan,
 #endif
 }
 
+#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_serial_udb_extra_f15_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  const uint8_t *sue_ID_VEHICLE_MODEL_NAME, const uint8_t *sue_ID_VEHICLE_REGISTRATION)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+
+	_mav_put_uint8_t_array(buf, 0, sue_ID_VEHICLE_MODEL_NAME, 40);
+	_mav_put_uint8_t_array(buf, 40, sue_ID_VEHICLE_REGISTRATION, 20);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN);
+#endif
+#else
+	mavlink_serial_udb_extra_f15_t *packet = (mavlink_serial_udb_extra_f15_t *)msgbuf;
+
+	mav_array_memcpy(packet->sue_ID_VEHICLE_MODEL_NAME, sue_ID_VEHICLE_MODEL_NAME, sizeof(uint8_t)*40);
+	mav_array_memcpy(packet->sue_ID_VEHICLE_REGISTRATION, sue_ID_VEHICLE_REGISTRATION, sizeof(uint8_t)*20);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SERIAL_UDB_EXTRA_F15 UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f16.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f16.h
index 659e6b7c52..ca9f0556f7 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f16.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f16.h
@@ -158,6 +158,40 @@ static inline void mavlink_msg_serial_udb_extra_f16_send(mavlink_channel_t chan,
 #endif
 }
 
+#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_serial_udb_extra_f16_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  const uint8_t *sue_ID_LEAD_PILOT, const uint8_t *sue_ID_DIY_DRONES_URL)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+
+	_mav_put_uint8_t_array(buf, 0, sue_ID_LEAD_PILOT, 40);
+	_mav_put_uint8_t_array(buf, 40, sue_ID_DIY_DRONES_URL, 70);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN);
+#endif
+#else
+	mavlink_serial_udb_extra_f16_t *packet = (mavlink_serial_udb_extra_f16_t *)msgbuf;
+
+	mav_array_memcpy(packet->sue_ID_LEAD_PILOT, sue_ID_LEAD_PILOT, sizeof(uint8_t)*40);
+	mav_array_memcpy(packet->sue_ID_DIY_DRONES_URL, sue_ID_DIY_DRONES_URL, sizeof(uint8_t)*70);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SERIAL_UDB_EXTRA_F16 UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_a.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_a.h
index 15ba68a346..41b9f09c00 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_a.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_a.h
@@ -443,6 +443,92 @@ static inline void mavlink_msg_serial_udb_extra_f2_a_send(mavlink_channel_t chan
 #endif
 }
 
+#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_serial_udb_extra_f2_a_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint32_t sue_time, uint8_t sue_status, int32_t sue_latitude, int32_t sue_longitude, int32_t sue_altitude, uint16_t sue_waypoint_index, int16_t sue_rmat0, int16_t sue_rmat1, int16_t sue_rmat2, int16_t sue_rmat3, int16_t sue_rmat4, int16_t sue_rmat5, int16_t sue_rmat6, int16_t sue_rmat7, int16_t sue_rmat8, uint16_t sue_cog, int16_t sue_sog, uint16_t sue_cpu_load, int16_t sue_voltage_milis, uint16_t sue_air_speed_3DIMU, int16_t sue_estimated_wind_0, int16_t sue_estimated_wind_1, int16_t sue_estimated_wind_2, int16_t sue_magFieldEarth0, int16_t sue_magFieldEarth1, int16_t sue_magFieldEarth2, int16_t sue_svs, int16_t sue_hdop)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint32_t(buf, 0, sue_time);
+	_mav_put_int32_t(buf, 4, sue_latitude);
+	_mav_put_int32_t(buf, 8, sue_longitude);
+	_mav_put_int32_t(buf, 12, sue_altitude);
+	_mav_put_uint16_t(buf, 16, sue_waypoint_index);
+	_mav_put_int16_t(buf, 18, sue_rmat0);
+	_mav_put_int16_t(buf, 20, sue_rmat1);
+	_mav_put_int16_t(buf, 22, sue_rmat2);
+	_mav_put_int16_t(buf, 24, sue_rmat3);
+	_mav_put_int16_t(buf, 26, sue_rmat4);
+	_mav_put_int16_t(buf, 28, sue_rmat5);
+	_mav_put_int16_t(buf, 30, sue_rmat6);
+	_mav_put_int16_t(buf, 32, sue_rmat7);
+	_mav_put_int16_t(buf, 34, sue_rmat8);
+	_mav_put_uint16_t(buf, 36, sue_cog);
+	_mav_put_int16_t(buf, 38, sue_sog);
+	_mav_put_uint16_t(buf, 40, sue_cpu_load);
+	_mav_put_int16_t(buf, 42, sue_voltage_milis);
+	_mav_put_uint16_t(buf, 44, sue_air_speed_3DIMU);
+	_mav_put_int16_t(buf, 46, sue_estimated_wind_0);
+	_mav_put_int16_t(buf, 48, sue_estimated_wind_1);
+	_mav_put_int16_t(buf, 50, sue_estimated_wind_2);
+	_mav_put_int16_t(buf, 52, sue_magFieldEarth0);
+	_mav_put_int16_t(buf, 54, sue_magFieldEarth1);
+	_mav_put_int16_t(buf, 56, sue_magFieldEarth2);
+	_mav_put_int16_t(buf, 58, sue_svs);
+	_mav_put_int16_t(buf, 60, sue_hdop);
+	_mav_put_uint8_t(buf, 62, sue_status);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN);
+#endif
+#else
+	mavlink_serial_udb_extra_f2_a_t *packet = (mavlink_serial_udb_extra_f2_a_t *)msgbuf;
+	packet->sue_time = sue_time;
+	packet->sue_latitude = sue_latitude;
+	packet->sue_longitude = sue_longitude;
+	packet->sue_altitude = sue_altitude;
+	packet->sue_waypoint_index = sue_waypoint_index;
+	packet->sue_rmat0 = sue_rmat0;
+	packet->sue_rmat1 = sue_rmat1;
+	packet->sue_rmat2 = sue_rmat2;
+	packet->sue_rmat3 = sue_rmat3;
+	packet->sue_rmat4 = sue_rmat4;
+	packet->sue_rmat5 = sue_rmat5;
+	packet->sue_rmat6 = sue_rmat6;
+	packet->sue_rmat7 = sue_rmat7;
+	packet->sue_rmat8 = sue_rmat8;
+	packet->sue_cog = sue_cog;
+	packet->sue_sog = sue_sog;
+	packet->sue_cpu_load = sue_cpu_load;
+	packet->sue_voltage_milis = sue_voltage_milis;
+	packet->sue_air_speed_3DIMU = sue_air_speed_3DIMU;
+	packet->sue_estimated_wind_0 = sue_estimated_wind_0;
+	packet->sue_estimated_wind_1 = sue_estimated_wind_1;
+	packet->sue_estimated_wind_2 = sue_estimated_wind_2;
+	packet->sue_magFieldEarth0 = sue_magFieldEarth0;
+	packet->sue_magFieldEarth1 = sue_magFieldEarth1;
+	packet->sue_magFieldEarth2 = sue_magFieldEarth2;
+	packet->sue_svs = sue_svs;
+	packet->sue_hdop = sue_hdop;
+	packet->sue_status = sue_status;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SERIAL_UDB_EXTRA_F2_A UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_b.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_b.h
index 7cb8c87da5..3e1757f743 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_b.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_b.h
@@ -498,6 +498,102 @@ static inline void mavlink_msg_serial_udb_extra_f2_b_send(mavlink_channel_t chan
 #endif
 }
 
+#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_serial_udb_extra_f2_b_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint32_t sue_time, int16_t sue_pwm_input_1, int16_t sue_pwm_input_2, int16_t sue_pwm_input_3, int16_t sue_pwm_input_4, int16_t sue_pwm_input_5, int16_t sue_pwm_input_6, int16_t sue_pwm_input_7, int16_t sue_pwm_input_8, int16_t sue_pwm_input_9, int16_t sue_pwm_input_10, int16_t sue_pwm_output_1, int16_t sue_pwm_output_2, int16_t sue_pwm_output_3, int16_t sue_pwm_output_4, int16_t sue_pwm_output_5, int16_t sue_pwm_output_6, int16_t sue_pwm_output_7, int16_t sue_pwm_output_8, int16_t sue_pwm_output_9, int16_t sue_pwm_output_10, int16_t sue_imu_location_x, int16_t sue_imu_location_y, int16_t sue_imu_location_z, uint32_t sue_flags, int16_t sue_osc_fails, int16_t sue_imu_velocity_x, int16_t sue_imu_velocity_y, int16_t sue_imu_velocity_z, int16_t sue_waypoint_goal_x, int16_t sue_waypoint_goal_y, int16_t sue_waypoint_goal_z, int16_t sue_memory_stack_free)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint32_t(buf, 0, sue_time);
+	_mav_put_uint32_t(buf, 4, sue_flags);
+	_mav_put_int16_t(buf, 8, sue_pwm_input_1);
+	_mav_put_int16_t(buf, 10, sue_pwm_input_2);
+	_mav_put_int16_t(buf, 12, sue_pwm_input_3);
+	_mav_put_int16_t(buf, 14, sue_pwm_input_4);
+	_mav_put_int16_t(buf, 16, sue_pwm_input_5);
+	_mav_put_int16_t(buf, 18, sue_pwm_input_6);
+	_mav_put_int16_t(buf, 20, sue_pwm_input_7);
+	_mav_put_int16_t(buf, 22, sue_pwm_input_8);
+	_mav_put_int16_t(buf, 24, sue_pwm_input_9);
+	_mav_put_int16_t(buf, 26, sue_pwm_input_10);
+	_mav_put_int16_t(buf, 28, sue_pwm_output_1);
+	_mav_put_int16_t(buf, 30, sue_pwm_output_2);
+	_mav_put_int16_t(buf, 32, sue_pwm_output_3);
+	_mav_put_int16_t(buf, 34, sue_pwm_output_4);
+	_mav_put_int16_t(buf, 36, sue_pwm_output_5);
+	_mav_put_int16_t(buf, 38, sue_pwm_output_6);
+	_mav_put_int16_t(buf, 40, sue_pwm_output_7);
+	_mav_put_int16_t(buf, 42, sue_pwm_output_8);
+	_mav_put_int16_t(buf, 44, sue_pwm_output_9);
+	_mav_put_int16_t(buf, 46, sue_pwm_output_10);
+	_mav_put_int16_t(buf, 48, sue_imu_location_x);
+	_mav_put_int16_t(buf, 50, sue_imu_location_y);
+	_mav_put_int16_t(buf, 52, sue_imu_location_z);
+	_mav_put_int16_t(buf, 54, sue_osc_fails);
+	_mav_put_int16_t(buf, 56, sue_imu_velocity_x);
+	_mav_put_int16_t(buf, 58, sue_imu_velocity_y);
+	_mav_put_int16_t(buf, 60, sue_imu_velocity_z);
+	_mav_put_int16_t(buf, 62, sue_waypoint_goal_x);
+	_mav_put_int16_t(buf, 64, sue_waypoint_goal_y);
+	_mav_put_int16_t(buf, 66, sue_waypoint_goal_z);
+	_mav_put_int16_t(buf, 68, sue_memory_stack_free);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN);
+#endif
+#else
+	mavlink_serial_udb_extra_f2_b_t *packet = (mavlink_serial_udb_extra_f2_b_t *)msgbuf;
+	packet->sue_time = sue_time;
+	packet->sue_flags = sue_flags;
+	packet->sue_pwm_input_1 = sue_pwm_input_1;
+	packet->sue_pwm_input_2 = sue_pwm_input_2;
+	packet->sue_pwm_input_3 = sue_pwm_input_3;
+	packet->sue_pwm_input_4 = sue_pwm_input_4;
+	packet->sue_pwm_input_5 = sue_pwm_input_5;
+	packet->sue_pwm_input_6 = sue_pwm_input_6;
+	packet->sue_pwm_input_7 = sue_pwm_input_7;
+	packet->sue_pwm_input_8 = sue_pwm_input_8;
+	packet->sue_pwm_input_9 = sue_pwm_input_9;
+	packet->sue_pwm_input_10 = sue_pwm_input_10;
+	packet->sue_pwm_output_1 = sue_pwm_output_1;
+	packet->sue_pwm_output_2 = sue_pwm_output_2;
+	packet->sue_pwm_output_3 = sue_pwm_output_3;
+	packet->sue_pwm_output_4 = sue_pwm_output_4;
+	packet->sue_pwm_output_5 = sue_pwm_output_5;
+	packet->sue_pwm_output_6 = sue_pwm_output_6;
+	packet->sue_pwm_output_7 = sue_pwm_output_7;
+	packet->sue_pwm_output_8 = sue_pwm_output_8;
+	packet->sue_pwm_output_9 = sue_pwm_output_9;
+	packet->sue_pwm_output_10 = sue_pwm_output_10;
+	packet->sue_imu_location_x = sue_imu_location_x;
+	packet->sue_imu_location_y = sue_imu_location_y;
+	packet->sue_imu_location_z = sue_imu_location_z;
+	packet->sue_osc_fails = sue_osc_fails;
+	packet->sue_imu_velocity_x = sue_imu_velocity_x;
+	packet->sue_imu_velocity_y = sue_imu_velocity_y;
+	packet->sue_imu_velocity_z = sue_imu_velocity_z;
+	packet->sue_waypoint_goal_x = sue_waypoint_goal_x;
+	packet->sue_waypoint_goal_y = sue_waypoint_goal_y;
+	packet->sue_waypoint_goal_z = sue_waypoint_goal_z;
+	packet->sue_memory_stack_free = sue_memory_stack_free;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SERIAL_UDB_EXTRA_F2_B UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f4.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f4.h
index 77c616cc2e..edd1e924c8 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f4.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f4.h
@@ -245,6 +245,56 @@ static inline void mavlink_msg_serial_udb_extra_f4_send(mavlink_channel_t chan,
 #endif
 }
 
+#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_serial_udb_extra_f4_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t sue_ROLL_STABILIZATION_AILERONS, uint8_t sue_ROLL_STABILIZATION_RUDDER, uint8_t sue_PITCH_STABILIZATION, uint8_t sue_YAW_STABILIZATION_RUDDER, uint8_t sue_YAW_STABILIZATION_AILERON, uint8_t sue_AILERON_NAVIGATION, uint8_t sue_RUDDER_NAVIGATION, uint8_t sue_ALTITUDEHOLD_STABILIZED, uint8_t sue_ALTITUDEHOLD_WAYPOINT, uint8_t sue_RACING_MODE)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint8_t(buf, 0, sue_ROLL_STABILIZATION_AILERONS);
+	_mav_put_uint8_t(buf, 1, sue_ROLL_STABILIZATION_RUDDER);
+	_mav_put_uint8_t(buf, 2, sue_PITCH_STABILIZATION);
+	_mav_put_uint8_t(buf, 3, sue_YAW_STABILIZATION_RUDDER);
+	_mav_put_uint8_t(buf, 4, sue_YAW_STABILIZATION_AILERON);
+	_mav_put_uint8_t(buf, 5, sue_AILERON_NAVIGATION);
+	_mav_put_uint8_t(buf, 6, sue_RUDDER_NAVIGATION);
+	_mav_put_uint8_t(buf, 7, sue_ALTITUDEHOLD_STABILIZED);
+	_mav_put_uint8_t(buf, 8, sue_ALTITUDEHOLD_WAYPOINT);
+	_mav_put_uint8_t(buf, 9, sue_RACING_MODE);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN);
+#endif
+#else
+	mavlink_serial_udb_extra_f4_t *packet = (mavlink_serial_udb_extra_f4_t *)msgbuf;
+	packet->sue_ROLL_STABILIZATION_AILERONS = sue_ROLL_STABILIZATION_AILERONS;
+	packet->sue_ROLL_STABILIZATION_RUDDER = sue_ROLL_STABILIZATION_RUDDER;
+	packet->sue_PITCH_STABILIZATION = sue_PITCH_STABILIZATION;
+	packet->sue_YAW_STABILIZATION_RUDDER = sue_YAW_STABILIZATION_RUDDER;
+	packet->sue_YAW_STABILIZATION_AILERON = sue_YAW_STABILIZATION_AILERON;
+	packet->sue_AILERON_NAVIGATION = sue_AILERON_NAVIGATION;
+	packet->sue_RUDDER_NAVIGATION = sue_RUDDER_NAVIGATION;
+	packet->sue_ALTITUDEHOLD_STABILIZED = sue_ALTITUDEHOLD_STABILIZED;
+	packet->sue_ALTITUDEHOLD_WAYPOINT = sue_ALTITUDEHOLD_WAYPOINT;
+	packet->sue_RACING_MODE = sue_RACING_MODE;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SERIAL_UDB_EXTRA_F4 UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f5.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f5.h
index e115a9b445..a821f65bb7 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f5.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f5.h
@@ -201,6 +201,48 @@ static inline void mavlink_msg_serial_udb_extra_f5_send(mavlink_channel_t chan,
 #endif
 }
 
+#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_serial_udb_extra_f5_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  float sue_YAWKP_AILERON, float sue_YAWKD_AILERON, float sue_ROLLKP, float sue_ROLLKD, float sue_YAW_STABILIZATION_AILERON, float sue_AILERON_BOOST)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, sue_YAWKP_AILERON);
+	_mav_put_float(buf, 4, sue_YAWKD_AILERON);
+	_mav_put_float(buf, 8, sue_ROLLKP);
+	_mav_put_float(buf, 12, sue_ROLLKD);
+	_mav_put_float(buf, 16, sue_YAW_STABILIZATION_AILERON);
+	_mav_put_float(buf, 20, sue_AILERON_BOOST);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN);
+#endif
+#else
+	mavlink_serial_udb_extra_f5_t *packet = (mavlink_serial_udb_extra_f5_t *)msgbuf;
+	packet->sue_YAWKP_AILERON = sue_YAWKP_AILERON;
+	packet->sue_YAWKD_AILERON = sue_YAWKD_AILERON;
+	packet->sue_ROLLKP = sue_ROLLKP;
+	packet->sue_ROLLKD = sue_ROLLKD;
+	packet->sue_YAW_STABILIZATION_AILERON = sue_YAW_STABILIZATION_AILERON;
+	packet->sue_AILERON_BOOST = sue_AILERON_BOOST;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SERIAL_UDB_EXTRA_F5 UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f6.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f6.h
index 656103d090..0eb24c9866 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f6.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f6.h
@@ -190,6 +190,46 @@ static inline void mavlink_msg_serial_udb_extra_f6_send(mavlink_channel_t chan,
 #endif
 }
 
+#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_serial_udb_extra_f6_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  float sue_PITCHGAIN, float sue_PITCHKD, float sue_RUDDER_ELEV_MIX, float sue_ROLL_ELEV_MIX, float sue_ELEVATOR_BOOST)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, sue_PITCHGAIN);
+	_mav_put_float(buf, 4, sue_PITCHKD);
+	_mav_put_float(buf, 8, sue_RUDDER_ELEV_MIX);
+	_mav_put_float(buf, 12, sue_ROLL_ELEV_MIX);
+	_mav_put_float(buf, 16, sue_ELEVATOR_BOOST);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN);
+#endif
+#else
+	mavlink_serial_udb_extra_f6_t *packet = (mavlink_serial_udb_extra_f6_t *)msgbuf;
+	packet->sue_PITCHGAIN = sue_PITCHGAIN;
+	packet->sue_PITCHKD = sue_PITCHKD;
+	packet->sue_RUDDER_ELEV_MIX = sue_RUDDER_ELEV_MIX;
+	packet->sue_ROLL_ELEV_MIX = sue_ROLL_ELEV_MIX;
+	packet->sue_ELEVATOR_BOOST = sue_ELEVATOR_BOOST;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SERIAL_UDB_EXTRA_F6 UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f7.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f7.h
index 51c98e6b7a..58de7d01c8 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f7.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f7.h
@@ -201,6 +201,48 @@ static inline void mavlink_msg_serial_udb_extra_f7_send(mavlink_channel_t chan,
 #endif
 }
 
+#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_serial_udb_extra_f7_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  float sue_YAWKP_RUDDER, float sue_YAWKD_RUDDER, float sue_ROLLKP_RUDDER, float sue_ROLLKD_RUDDER, float sue_RUDDER_BOOST, float sue_RTL_PITCH_DOWN)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, sue_YAWKP_RUDDER);
+	_mav_put_float(buf, 4, sue_YAWKD_RUDDER);
+	_mav_put_float(buf, 8, sue_ROLLKP_RUDDER);
+	_mav_put_float(buf, 12, sue_ROLLKD_RUDDER);
+	_mav_put_float(buf, 16, sue_RUDDER_BOOST);
+	_mav_put_float(buf, 20, sue_RTL_PITCH_DOWN);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN);
+#endif
+#else
+	mavlink_serial_udb_extra_f7_t *packet = (mavlink_serial_udb_extra_f7_t *)msgbuf;
+	packet->sue_YAWKP_RUDDER = sue_YAWKP_RUDDER;
+	packet->sue_YAWKD_RUDDER = sue_YAWKD_RUDDER;
+	packet->sue_ROLLKP_RUDDER = sue_ROLLKP_RUDDER;
+	packet->sue_ROLLKD_RUDDER = sue_ROLLKD_RUDDER;
+	packet->sue_RUDDER_BOOST = sue_RUDDER_BOOST;
+	packet->sue_RTL_PITCH_DOWN = sue_RTL_PITCH_DOWN;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SERIAL_UDB_EXTRA_F7 UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f8.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f8.h
index 8557a95e27..5cab816555 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f8.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f8.h
@@ -212,6 +212,50 @@ static inline void mavlink_msg_serial_udb_extra_f8_send(mavlink_channel_t chan,
 #endif
 }
 
+#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_serial_udb_extra_f8_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  float sue_HEIGHT_TARGET_MAX, float sue_HEIGHT_TARGET_MIN, float sue_ALT_HOLD_THROTTLE_MIN, float sue_ALT_HOLD_THROTTLE_MAX, float sue_ALT_HOLD_PITCH_MIN, float sue_ALT_HOLD_PITCH_MAX, float sue_ALT_HOLD_PITCH_HIGH)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, sue_HEIGHT_TARGET_MAX);
+	_mav_put_float(buf, 4, sue_HEIGHT_TARGET_MIN);
+	_mav_put_float(buf, 8, sue_ALT_HOLD_THROTTLE_MIN);
+	_mav_put_float(buf, 12, sue_ALT_HOLD_THROTTLE_MAX);
+	_mav_put_float(buf, 16, sue_ALT_HOLD_PITCH_MIN);
+	_mav_put_float(buf, 20, sue_ALT_HOLD_PITCH_MAX);
+	_mav_put_float(buf, 24, sue_ALT_HOLD_PITCH_HIGH);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN);
+#endif
+#else
+	mavlink_serial_udb_extra_f8_t *packet = (mavlink_serial_udb_extra_f8_t *)msgbuf;
+	packet->sue_HEIGHT_TARGET_MAX = sue_HEIGHT_TARGET_MAX;
+	packet->sue_HEIGHT_TARGET_MIN = sue_HEIGHT_TARGET_MIN;
+	packet->sue_ALT_HOLD_THROTTLE_MIN = sue_ALT_HOLD_THROTTLE_MIN;
+	packet->sue_ALT_HOLD_THROTTLE_MAX = sue_ALT_HOLD_THROTTLE_MAX;
+	packet->sue_ALT_HOLD_PITCH_MIN = sue_ALT_HOLD_PITCH_MIN;
+	packet->sue_ALT_HOLD_PITCH_MAX = sue_ALT_HOLD_PITCH_MAX;
+	packet->sue_ALT_HOLD_PITCH_HIGH = sue_ALT_HOLD_PITCH_HIGH;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SERIAL_UDB_EXTRA_F8 UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/version.h b/mavlink/include/mavlink/v1.0/matrixpilot/version.h
index a8be16c424..50cbda6c78 100644
--- a/mavlink/include/mavlink/v1.0/matrixpilot/version.h
+++ b/mavlink/include/mavlink/v1.0/matrixpilot/version.h
@@ -5,7 +5,7 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Tue Feb  4 15:28:04 2014"
+#define MAVLINK_BUILD_DATE "Sun Apr 13 09:38:32 2014"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
 #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
  
diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_attitude_control.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_attitude_control.h
index ef5354d5e1..f1cac9f2cd 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_attitude_control.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_attitude_control.h
@@ -234,6 +234,54 @@ static inline void mavlink_msg_attitude_control_send(mavlink_channel_t chan, uin
 #endif
 }
 
+#if MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_attitude_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target, float roll, float pitch, float yaw, float thrust, uint8_t roll_manual, uint8_t pitch_manual, uint8_t yaw_manual, uint8_t thrust_manual)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, roll);
+	_mav_put_float(buf, 4, pitch);
+	_mav_put_float(buf, 8, yaw);
+	_mav_put_float(buf, 12, thrust);
+	_mav_put_uint8_t(buf, 16, target);
+	_mav_put_uint8_t(buf, 17, roll_manual);
+	_mav_put_uint8_t(buf, 18, pitch_manual);
+	_mav_put_uint8_t(buf, 19, yaw_manual);
+	_mav_put_uint8_t(buf, 20, thrust_manual);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL, buf, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN, MAVLINK_MSG_ID_ATTITUDE_CONTROL_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL, buf, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN);
+#endif
+#else
+	mavlink_attitude_control_t *packet = (mavlink_attitude_control_t *)msgbuf;
+	packet->roll = roll;
+	packet->pitch = pitch;
+	packet->yaw = yaw;
+	packet->thrust = thrust;
+	packet->target = target;
+	packet->roll_manual = roll_manual;
+	packet->pitch_manual = pitch_manual;
+	packet->yaw_manual = yaw_manual;
+	packet->thrust_manual = thrust_manual;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN, MAVLINK_MSG_ID_ATTITUDE_CONTROL_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE ATTITUDE_CONTROL UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_brief_feature.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_brief_feature.h
index d41093792e..7bc27982a8 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_brief_feature.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_brief_feature.h
@@ -217,6 +217,50 @@ static inline void mavlink_msg_brief_feature_send(mavlink_channel_t chan, float
 #endif
 }
 
+#if MAVLINK_MSG_ID_BRIEF_FEATURE_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_brief_feature_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  float x, float y, float z, uint8_t orientation_assignment, uint16_t size, uint16_t orientation, const uint8_t *descriptor, float response)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, x);
+	_mav_put_float(buf, 4, y);
+	_mav_put_float(buf, 8, z);
+	_mav_put_float(buf, 12, response);
+	_mav_put_uint16_t(buf, 16, size);
+	_mav_put_uint16_t(buf, 18, orientation);
+	_mav_put_uint8_t(buf, 20, orientation_assignment);
+	_mav_put_uint8_t_array(buf, 21, descriptor, 32);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BRIEF_FEATURE, buf, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN, MAVLINK_MSG_ID_BRIEF_FEATURE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BRIEF_FEATURE, buf, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN);
+#endif
+#else
+	mavlink_brief_feature_t *packet = (mavlink_brief_feature_t *)msgbuf;
+	packet->x = x;
+	packet->y = y;
+	packet->z = z;
+	packet->response = response;
+	packet->size = size;
+	packet->orientation = orientation;
+	packet->orientation_assignment = orientation_assignment;
+	mav_array_memcpy(packet->descriptor, descriptor, sizeof(uint8_t)*32);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BRIEF_FEATURE, (const char *)packet, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN, MAVLINK_MSG_ID_BRIEF_FEATURE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BRIEF_FEATURE, (const char *)packet, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE BRIEF_FEATURE UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_available.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_available.h
index ae4db825df..37270d49c0 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_available.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_available.h
@@ -388,6 +388,82 @@ static inline void mavlink_msg_image_available_send(mavlink_channel_t chan, uint
 #endif
 }
 
+#if MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_image_available_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint64_t cam_id, uint8_t cam_no, uint64_t timestamp, uint64_t valid_until, uint32_t img_seq, uint32_t img_buf_index, uint16_t width, uint16_t height, uint16_t depth, uint8_t channels, uint32_t key, uint32_t exposure, float gain, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint64_t(buf, 0, cam_id);
+	_mav_put_uint64_t(buf, 8, timestamp);
+	_mav_put_uint64_t(buf, 16, valid_until);
+	_mav_put_uint32_t(buf, 24, img_seq);
+	_mav_put_uint32_t(buf, 28, img_buf_index);
+	_mav_put_uint32_t(buf, 32, key);
+	_mav_put_uint32_t(buf, 36, exposure);
+	_mav_put_float(buf, 40, gain);
+	_mav_put_float(buf, 44, roll);
+	_mav_put_float(buf, 48, pitch);
+	_mav_put_float(buf, 52, yaw);
+	_mav_put_float(buf, 56, local_z);
+	_mav_put_float(buf, 60, lat);
+	_mav_put_float(buf, 64, lon);
+	_mav_put_float(buf, 68, alt);
+	_mav_put_float(buf, 72, ground_x);
+	_mav_put_float(buf, 76, ground_y);
+	_mav_put_float(buf, 80, ground_z);
+	_mav_put_uint16_t(buf, 84, width);
+	_mav_put_uint16_t(buf, 86, height);
+	_mav_put_uint16_t(buf, 88, depth);
+	_mav_put_uint8_t(buf, 90, cam_no);
+	_mav_put_uint8_t(buf, 91, channels);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE, buf, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN, MAVLINK_MSG_ID_IMAGE_AVAILABLE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE, buf, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN);
+#endif
+#else
+	mavlink_image_available_t *packet = (mavlink_image_available_t *)msgbuf;
+	packet->cam_id = cam_id;
+	packet->timestamp = timestamp;
+	packet->valid_until = valid_until;
+	packet->img_seq = img_seq;
+	packet->img_buf_index = img_buf_index;
+	packet->key = key;
+	packet->exposure = exposure;
+	packet->gain = gain;
+	packet->roll = roll;
+	packet->pitch = pitch;
+	packet->yaw = yaw;
+	packet->local_z = local_z;
+	packet->lat = lat;
+	packet->lon = lon;
+	packet->alt = alt;
+	packet->ground_x = ground_x;
+	packet->ground_y = ground_y;
+	packet->ground_z = ground_z;
+	packet->width = width;
+	packet->height = height;
+	packet->depth = depth;
+	packet->cam_no = cam_no;
+	packet->channels = channels;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE, (const char *)packet, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN, MAVLINK_MSG_ID_IMAGE_AVAILABLE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE, (const char *)packet, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE IMAGE_AVAILABLE UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_trigger_control.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_trigger_control.h
index 664574be94..85f8eff556 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_trigger_control.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_trigger_control.h
@@ -146,6 +146,38 @@ static inline void mavlink_msg_image_trigger_control_send(mavlink_channel_t chan
 #endif
 }
 
+#if MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_image_trigger_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t enable)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint8_t(buf, 0, enable);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL, buf, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL, buf, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN);
+#endif
+#else
+	mavlink_image_trigger_control_t *packet = (mavlink_image_trigger_control_t *)msgbuf;
+	packet->enable = enable;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL, (const char *)packet, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL, (const char *)packet, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE IMAGE_TRIGGER_CONTROL UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_triggered.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_triggered.h
index f3a2243afc..b438c74ea5 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_triggered.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_triggered.h
@@ -267,6 +267,60 @@ static inline void mavlink_msg_image_triggered_send(mavlink_channel_t chan, uint
 #endif
 }
 
+#if MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_image_triggered_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint64_t timestamp, uint32_t seq, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint64_t(buf, 0, timestamp);
+	_mav_put_uint32_t(buf, 8, seq);
+	_mav_put_float(buf, 12, roll);
+	_mav_put_float(buf, 16, pitch);
+	_mav_put_float(buf, 20, yaw);
+	_mav_put_float(buf, 24, local_z);
+	_mav_put_float(buf, 28, lat);
+	_mav_put_float(buf, 32, lon);
+	_mav_put_float(buf, 36, alt);
+	_mav_put_float(buf, 40, ground_x);
+	_mav_put_float(buf, 44, ground_y);
+	_mav_put_float(buf, 48, ground_z);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED, buf, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN, MAVLINK_MSG_ID_IMAGE_TRIGGERED_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED, buf, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN);
+#endif
+#else
+	mavlink_image_triggered_t *packet = (mavlink_image_triggered_t *)msgbuf;
+	packet->timestamp = timestamp;
+	packet->seq = seq;
+	packet->roll = roll;
+	packet->pitch = pitch;
+	packet->yaw = yaw;
+	packet->local_z = local_z;
+	packet->lat = lat;
+	packet->lon = lon;
+	packet->alt = alt;
+	packet->ground_x = ground_x;
+	packet->ground_y = ground_y;
+	packet->ground_z = ground_z;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED, (const char *)packet, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN, MAVLINK_MSG_ID_IMAGE_TRIGGERED_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED, (const char *)packet, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE IMAGE_TRIGGERED UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h
index 6bdcceaf9a..05452637ea 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h
@@ -212,6 +212,50 @@ static inline void mavlink_msg_marker_send(mavlink_channel_t chan, uint16_t id,
 #endif
 }
 
+#if MAVLINK_MSG_ID_MARKER_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_marker_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint16_t id, float x, float y, float z, float roll, float pitch, float yaw)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, x);
+	_mav_put_float(buf, 4, y);
+	_mav_put_float(buf, 8, z);
+	_mav_put_float(buf, 12, roll);
+	_mav_put_float(buf, 16, pitch);
+	_mav_put_float(buf, 20, yaw);
+	_mav_put_uint16_t(buf, 24, id);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MARKER, buf, MAVLINK_MSG_ID_MARKER_LEN, MAVLINK_MSG_ID_MARKER_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MARKER, buf, MAVLINK_MSG_ID_MARKER_LEN);
+#endif
+#else
+	mavlink_marker_t *packet = (mavlink_marker_t *)msgbuf;
+	packet->x = x;
+	packet->y = y;
+	packet->z = z;
+	packet->roll = roll;
+	packet->pitch = pitch;
+	packet->yaw = yaw;
+	packet->id = id;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MARKER, (const char *)packet, MAVLINK_MSG_ID_MARKER_LEN, MAVLINK_MSG_ID_MARKER_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MARKER, (const char *)packet, MAVLINK_MSG_ID_MARKER_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE MARKER UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_pattern_detected.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_pattern_detected.h
index aa7b827a36..bde400da9a 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_pattern_detected.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_pattern_detected.h
@@ -173,6 +173,42 @@ static inline void mavlink_msg_pattern_detected_send(mavlink_channel_t chan, uin
 #endif
 }
 
+#if MAVLINK_MSG_ID_PATTERN_DETECTED_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_pattern_detected_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t type, float confidence, const char *file, uint8_t detected)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, confidence);
+	_mav_put_uint8_t(buf, 4, type);
+	_mav_put_uint8_t(buf, 105, detected);
+	_mav_put_char_array(buf, 5, file, 100);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PATTERN_DETECTED, buf, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN, MAVLINK_MSG_ID_PATTERN_DETECTED_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PATTERN_DETECTED, buf, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN);
+#endif
+#else
+	mavlink_pattern_detected_t *packet = (mavlink_pattern_detected_t *)msgbuf;
+	packet->confidence = confidence;
+	packet->type = type;
+	packet->detected = detected;
+	mav_array_memcpy(packet->file, file, sizeof(char)*100);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PATTERN_DETECTED, (const char *)packet, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN, MAVLINK_MSG_ID_PATTERN_DETECTED_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PATTERN_DETECTED, (const char *)packet, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE PATTERN_DETECTED UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest.h
index 913a52897e..86e9345194 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest.h
@@ -217,6 +217,50 @@ static inline void mavlink_msg_point_of_interest_send(mavlink_channel_t chan, ui
 #endif
 }
 
+#if MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_point_of_interest_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const char *name)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, x);
+	_mav_put_float(buf, 4, y);
+	_mav_put_float(buf, 8, z);
+	_mav_put_uint16_t(buf, 12, timeout);
+	_mav_put_uint8_t(buf, 14, type);
+	_mav_put_uint8_t(buf, 15, color);
+	_mav_put_uint8_t(buf, 16, coordinate_system);
+	_mav_put_char_array(buf, 17, name, 26);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST, buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN, MAVLINK_MSG_ID_POINT_OF_INTEREST_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST, buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN);
+#endif
+#else
+	mavlink_point_of_interest_t *packet = (mavlink_point_of_interest_t *)msgbuf;
+	packet->x = x;
+	packet->y = y;
+	packet->z = z;
+	packet->timeout = timeout;
+	packet->type = type;
+	packet->color = color;
+	packet->coordinate_system = coordinate_system;
+	mav_array_memcpy(packet->name, name, sizeof(char)*26);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST, (const char *)packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN, MAVLINK_MSG_ID_POINT_OF_INTEREST_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST, (const char *)packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE POINT_OF_INTEREST UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h
index e244364318..e5c04a851c 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h
@@ -250,6 +250,56 @@ static inline void mavlink_msg_point_of_interest_connection_send(mavlink_channel
 #endif
 }
 
+#if MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_point_of_interest_connection_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const char *name)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, xp1);
+	_mav_put_float(buf, 4, yp1);
+	_mav_put_float(buf, 8, zp1);
+	_mav_put_float(buf, 12, xp2);
+	_mav_put_float(buf, 16, yp2);
+	_mav_put_float(buf, 20, zp2);
+	_mav_put_uint16_t(buf, 24, timeout);
+	_mav_put_uint8_t(buf, 26, type);
+	_mav_put_uint8_t(buf, 27, color);
+	_mav_put_uint8_t(buf, 28, coordinate_system);
+	_mav_put_char_array(buf, 29, name, 26);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION, buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION, buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN);
+#endif
+#else
+	mavlink_point_of_interest_connection_t *packet = (mavlink_point_of_interest_connection_t *)msgbuf;
+	packet->xp1 = xp1;
+	packet->yp1 = yp1;
+	packet->zp1 = zp1;
+	packet->xp2 = xp2;
+	packet->yp2 = yp2;
+	packet->zp2 = zp2;
+	packet->timeout = timeout;
+	packet->type = type;
+	packet->color = color;
+	packet->coordinate_system = coordinate_system;
+	mav_array_memcpy(packet->name, name, sizeof(char)*26);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION, (const char *)packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION, (const char *)packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE POINT_OF_INTEREST_CONNECTION UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_position_control_setpoint.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_position_control_setpoint.h
index 6f4ca510ae..480004cefe 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_position_control_setpoint.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_position_control_setpoint.h
@@ -190,6 +190,46 @@ static inline void mavlink_msg_position_control_setpoint_send(mavlink_channel_t
 #endif
 }
 
+#if MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_position_control_setpoint_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint16_t id, float x, float y, float z, float yaw)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, x);
+	_mav_put_float(buf, 4, y);
+	_mav_put_float(buf, 8, z);
+	_mav_put_float(buf, 12, yaw);
+	_mav_put_uint16_t(buf, 16, id);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT, buf, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT, buf, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN);
+#endif
+#else
+	mavlink_position_control_setpoint_t *packet = (mavlink_position_control_setpoint_t *)msgbuf;
+	packet->x = x;
+	packet->y = y;
+	packet->z = z;
+	packet->yaw = yaw;
+	packet->id = id;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE POSITION_CONTROL_SETPOINT UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h
index 08cedbb4b2..41a5126482 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h
@@ -212,6 +212,50 @@ static inline void mavlink_msg_raw_aux_send(mavlink_channel_t chan, uint16_t adc
 #endif
 }
 
+#if MAVLINK_MSG_ID_RAW_AUX_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_raw_aux_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t vbat, int16_t temp, int32_t baro)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_int32_t(buf, 0, baro);
+	_mav_put_uint16_t(buf, 4, adc1);
+	_mav_put_uint16_t(buf, 6, adc2);
+	_mav_put_uint16_t(buf, 8, adc3);
+	_mav_put_uint16_t(buf, 10, adc4);
+	_mav_put_uint16_t(buf, 12, vbat);
+	_mav_put_int16_t(buf, 14, temp);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_AUX, buf, MAVLINK_MSG_ID_RAW_AUX_LEN, MAVLINK_MSG_ID_RAW_AUX_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_AUX, buf, MAVLINK_MSG_ID_RAW_AUX_LEN);
+#endif
+#else
+	mavlink_raw_aux_t *packet = (mavlink_raw_aux_t *)msgbuf;
+	packet->baro = baro;
+	packet->adc1 = adc1;
+	packet->adc2 = adc2;
+	packet->adc3 = adc3;
+	packet->adc4 = adc4;
+	packet->vbat = vbat;
+	packet->temp = temp;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_AUX, (const char *)packet, MAVLINK_MSG_ID_RAW_AUX_LEN, MAVLINK_MSG_ID_RAW_AUX_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_AUX, (const char *)packet, MAVLINK_MSG_ID_RAW_AUX_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE RAW_AUX UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_cam_shutter.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_cam_shutter.h
index b26d748c25..8fe14dd546 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_cam_shutter.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_cam_shutter.h
@@ -201,6 +201,48 @@ static inline void mavlink_msg_set_cam_shutter_send(mavlink_channel_t chan, uint
 #endif
 }
 
+#if MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_set_cam_shutter_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t cam_no, uint8_t cam_mode, uint8_t trigger_pin, uint16_t interval, uint16_t exposure, float gain)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, gain);
+	_mav_put_uint16_t(buf, 4, interval);
+	_mav_put_uint16_t(buf, 6, exposure);
+	_mav_put_uint8_t(buf, 8, cam_no);
+	_mav_put_uint8_t(buf, 9, cam_mode);
+	_mav_put_uint8_t(buf, 10, trigger_pin);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER, buf, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN, MAVLINK_MSG_ID_SET_CAM_SHUTTER_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER, buf, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN);
+#endif
+#else
+	mavlink_set_cam_shutter_t *packet = (mavlink_set_cam_shutter_t *)msgbuf;
+	packet->gain = gain;
+	packet->interval = interval;
+	packet->exposure = exposure;
+	packet->cam_no = cam_no;
+	packet->cam_mode = cam_mode;
+	packet->trigger_pin = trigger_pin;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER, (const char *)packet, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN, MAVLINK_MSG_ID_SET_CAM_SHUTTER_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER, (const char *)packet, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SET_CAM_SHUTTER UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_position_control_offset.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_position_control_offset.h
index f1f9e698f8..c4e4fc19a2 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_position_control_offset.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_position_control_offset.h
@@ -201,6 +201,48 @@ static inline void mavlink_msg_set_position_control_offset_send(mavlink_channel_
 #endif
 }
 
+#if MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_set_position_control_offset_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system, uint8_t target_component, float x, float y, float z, float yaw)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, x);
+	_mav_put_float(buf, 4, y);
+	_mav_put_float(buf, 8, z);
+	_mav_put_float(buf, 12, yaw);
+	_mav_put_uint8_t(buf, 16, target_system);
+	_mav_put_uint8_t(buf, 17, target_component);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET, buf, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET, buf, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN);
+#endif
+#else
+	mavlink_set_position_control_offset_t *packet = (mavlink_set_position_control_offset_t *)msgbuf;
+	packet->x = x;
+	packet->y = y;
+	packet->z = z;
+	packet->yaw = yaw;
+	packet->target_system = target_system;
+	packet->target_component = target_component;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET, (const char *)packet, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET, (const char *)packet, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SET_POSITION_CONTROL_OFFSET UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_command.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_command.h
index 9458087da9..d71a2ed2e3 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_command.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_command.h
@@ -179,6 +179,44 @@ static inline void mavlink_msg_watchdog_command_send(mavlink_channel_t chan, uin
 #endif
 }
 
+#if MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_watchdog_command_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target_system_id, uint16_t watchdog_id, uint16_t process_id, uint8_t command_id)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint16_t(buf, 0, watchdog_id);
+	_mav_put_uint16_t(buf, 2, process_id);
+	_mav_put_uint8_t(buf, 4, target_system_id);
+	_mav_put_uint8_t(buf, 5, command_id);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND, buf, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN, MAVLINK_MSG_ID_WATCHDOG_COMMAND_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND, buf, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN);
+#endif
+#else
+	mavlink_watchdog_command_t *packet = (mavlink_watchdog_command_t *)msgbuf;
+	packet->watchdog_id = watchdog_id;
+	packet->process_id = process_id;
+	packet->target_system_id = target_system_id;
+	packet->command_id = command_id;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND, (const char *)packet, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN, MAVLINK_MSG_ID_WATCHDOG_COMMAND_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND, (const char *)packet, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE WATCHDOG_COMMAND UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h
index 3f4295ee5d..a8e49b6426 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h
@@ -157,6 +157,40 @@ static inline void mavlink_msg_watchdog_heartbeat_send(mavlink_channel_t chan, u
 #endif
 }
 
+#if MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_watchdog_heartbeat_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint16_t watchdog_id, uint16_t process_count)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint16_t(buf, 0, watchdog_id);
+	_mav_put_uint16_t(buf, 2, process_count);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT, buf, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT, buf, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN);
+#endif
+#else
+	mavlink_watchdog_heartbeat_t *packet = (mavlink_watchdog_heartbeat_t *)msgbuf;
+	packet->watchdog_id = watchdog_id;
+	packet->process_count = process_count;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT, (const char *)packet, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT, (const char *)packet, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE WATCHDOG_HEARTBEAT UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_info.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_info.h
index 55853cdde4..a5e0cc67e4 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_info.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_info.h
@@ -185,6 +185,44 @@ static inline void mavlink_msg_watchdog_process_info_send(mavlink_channel_t chan
 #endif
 }
 
+#if MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_watchdog_process_info_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint16_t watchdog_id, uint16_t process_id, const char *name, const char *arguments, int32_t timeout)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_int32_t(buf, 0, timeout);
+	_mav_put_uint16_t(buf, 4, watchdog_id);
+	_mav_put_uint16_t(buf, 6, process_id);
+	_mav_put_char_array(buf, 8, name, 100);
+	_mav_put_char_array(buf, 108, arguments, 147);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO, buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO, buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN);
+#endif
+#else
+	mavlink_watchdog_process_info_t *packet = (mavlink_watchdog_process_info_t *)msgbuf;
+	packet->timeout = timeout;
+	packet->watchdog_id = watchdog_id;
+	packet->process_id = process_id;
+	mav_array_memcpy(packet->name, name, sizeof(char)*100);
+	mav_array_memcpy(packet->arguments, arguments, sizeof(char)*147);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO, (const char *)packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO, (const char *)packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE WATCHDOG_PROCESS_INFO UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_status.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_status.h
index a0410d803e..15c9598e40 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_status.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_status.h
@@ -201,6 +201,48 @@ static inline void mavlink_msg_watchdog_process_status_send(mavlink_channel_t ch
 #endif
 }
 
+#if MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_watchdog_process_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint16_t watchdog_id, uint16_t process_id, uint8_t state, uint8_t muted, int32_t pid, uint16_t crashes)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_int32_t(buf, 0, pid);
+	_mav_put_uint16_t(buf, 4, watchdog_id);
+	_mav_put_uint16_t(buf, 6, process_id);
+	_mav_put_uint16_t(buf, 8, crashes);
+	_mav_put_uint8_t(buf, 10, state);
+	_mav_put_uint8_t(buf, 11, muted);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS, buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS, buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN);
+#endif
+#else
+	mavlink_watchdog_process_status_t *packet = (mavlink_watchdog_process_status_t *)msgbuf;
+	packet->pid = pid;
+	packet->watchdog_id = watchdog_id;
+	packet->process_id = process_id;
+	packet->crashes = crashes;
+	packet->state = state;
+	packet->muted = muted;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS, (const char *)packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS, (const char *)packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE WATCHDOG_PROCESS_STATUS UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h b/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h
index 428619eedb..a624a25797 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h
@@ -12,15 +12,15 @@ extern "C" {
 // MESSAGE LENGTHS AND CRCS
 
 #ifndef MAVLINK_MESSAGE_LENGTHS
-#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 0, 0, 0, 0, 0, 13, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 0, 11, 52, 1, 92, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 26, 16, 0, 0, 0, 0, 0, 0, 0, 4, 255, 12, 6, 0, 0, 0, 0, 0, 0, 106, 43, 55, 0, 0, 53, 0, 0, 0, 0, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0}
+#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 42, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 0, 0, 0, 13, 255, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 0, 11, 52, 1, 92, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 26, 16, 0, 0, 0, 0, 0, 0, 0, 4, 255, 12, 6, 0, 0, 0, 0, 0, 0, 106, 43, 55, 0, 0, 53, 0, 0, 0, 0, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0}
 #endif
 
 #ifndef MAVLINK_MESSAGE_CRCS
-#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 0, 0, 0, 0, 0, 29, 223, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 0, 108, 86, 95, 224, 0, 0, 0, 0, 0, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 28, 249, 182, 0, 0, 0, 0, 0, 0, 0, 153, 16, 29, 162, 0, 0, 0, 0, 0, 0, 90, 95, 36, 0, 0, 88, 0, 0, 0, 0, 254, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0}
+#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 85, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 0, 108, 86, 95, 224, 0, 0, 0, 0, 0, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 28, 249, 182, 0, 0, 0, 0, 0, 0, 0, 153, 16, 29, 162, 0, 0, 0, 0, 0, 0, 90, 95, 36, 0, 0, 88, 0, 0, 0, 0, 254, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0}
 #endif
 
 #ifndef MAVLINK_MESSAGE_INFO
-#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_CAM_SHUTTER, MAVLINK_MESSAGE_INFO_IMAGE_TRIGGERED, MAVLINK_MESSAGE_INFO_IMAGE_TRIGGER_CONTROL, MAVLINK_MESSAGE_INFO_IMAGE_AVAILABLE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_POSITION_CONTROL_OFFSET, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_POSITION_CONTROL_SETPOINT, MAVLINK_MESSAGE_INFO_MARKER, MAVLINK_MESSAGE_INFO_RAW_AUX, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_WATCHDOG_HEARTBEAT, MAVLINK_MESSAGE_INFO_WATCHDOG_PROCESS_INFO, MAVLINK_MESSAGE_INFO_WATCHDOG_PROCESS_STATUS, MAVLINK_MESSAGE_INFO_WATCHDOG_COMMAND, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PATTERN_DETECTED, MAVLINK_MESSAGE_INFO_POINT_OF_INTEREST, MAVLINK_MESSAGE_INFO_POINT_OF_INTEREST_CONNECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BRIEF_FEATURE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ATTITUDE_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}}
+#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_CAM_SHUTTER, MAVLINK_MESSAGE_INFO_IMAGE_TRIGGERED, MAVLINK_MESSAGE_INFO_IMAGE_TRIGGER_CONTROL, MAVLINK_MESSAGE_INFO_IMAGE_AVAILABLE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_POSITION_CONTROL_OFFSET, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_POSITION_CONTROL_SETPOINT, MAVLINK_MESSAGE_INFO_MARKER, MAVLINK_MESSAGE_INFO_RAW_AUX, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_WATCHDOG_HEARTBEAT, MAVLINK_MESSAGE_INFO_WATCHDOG_PROCESS_INFO, MAVLINK_MESSAGE_INFO_WATCHDOG_PROCESS_STATUS, MAVLINK_MESSAGE_INFO_WATCHDOG_COMMAND, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PATTERN_DETECTED, MAVLINK_MESSAGE_INFO_POINT_OF_INTEREST, MAVLINK_MESSAGE_INFO_POINT_OF_INTEREST_CONNECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BRIEF_FEATURE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ATTITUDE_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}}
 #endif
 
 #include "../protocol.h"
@@ -56,6 +56,7 @@ enum MAV_CMD
 	MAV_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude|  */
 	MAV_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|  */
 	MAV_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal|  */
+	MAV_CMD_NAV_SPLINE_WAYPOINT=82, /* Navigate to MISSION using a spline path. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Empty| Empty| Empty| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal|  */
 	MAV_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
 	MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty|  */
 	MAV_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate.  Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude|  */
@@ -73,8 +74,16 @@ enum MAV_CMD
 	MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty|  */
 	MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty|  */
 	MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|  */
+	MAV_CMD_DO_DIGICAM_CONFIGURE=202, /* Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)|  */
+	MAV_CMD_DO_DIGICAM_CONTROL=203, /* Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty|  */
+	MAV_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty|  */
+	MAV_CMD_DO_MOUNT_CONTROL=205, /* Mission command to control a camera or antenna mount |pitch or lat in degrees, depending on mount mode.| roll or lon in degrees depending on mount mode| yaw or alt (in meters) depending on mount mode| reserved| reserved| reserved| MAV_MOUNT_MODE enum value|  */
+	MAV_CMD_DO_SET_CAM_TRIGG_DIST=206, /* Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty|  */
+	MAV_CMD_DO_FENCE_ENABLE=207, /* Mission command to enable the geofence |enable? (0=disable, 1=enable)| Empty| Empty| Empty| Empty| Empty| Empty|  */
+	MAV_CMD_DO_PARACHUTE=208, /* Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty|  */
+	MAV_CMD_DO_MOUNT_CONTROL_QUAT=220, /* Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1| q2 - quaternion param #2| q3 - quaternion param #3| q4 - quaternion param #4| Empty| Empty| Empty|  */
 	MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty|  */
+	MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Compass/Motor interference calibration: 0: no, 1: yes| Empty|  */
 	MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units|  */
 	MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty|  */
 	MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty|  */
diff --git a/mavlink/include/mavlink/v1.0/pixhawk/version.h b/mavlink/include/mavlink/v1.0/pixhawk/version.h
index aa79f6ffb5..6610a894c9 100644
--- a/mavlink/include/mavlink/v1.0/pixhawk/version.h
+++ b/mavlink/include/mavlink/v1.0/pixhawk/version.h
@@ -5,7 +5,7 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Tue Feb  4 15:28:16 2014"
+#define MAVLINK_BUILD_DATE "Sun Apr 13 09:38:46 2014"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
 #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
  
diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_ack.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_ack.h
index 73e9d75dbc..a87ce87d37 100644
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_ack.h
+++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_ack.h
@@ -173,6 +173,40 @@ static inline void mavlink_msg_cmd_airspeed_ack_send(mavlink_channel_t chan, flo
 #endif
 }
 
+#if MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_cmd_airspeed_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  float spCmd, uint8_t ack)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, spCmd);
+	_mav_put_uint8_t(buf, 4, ack);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK, buf, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK, buf, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN);
+#endif
+#else
+	mavlink_cmd_airspeed_ack_t *packet = (mavlink_cmd_airspeed_ack_t *)msgbuf;
+	packet->spCmd = spCmd;
+	packet->ack = ack;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK, (const char *)packet, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK, (const char *)packet, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE CMD_AIRSPEED_ACK UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h
index b6b567f993..71d61e1e08 100644
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h
+++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h
@@ -173,6 +173,40 @@ static inline void mavlink_msg_cmd_airspeed_chng_send(mavlink_channel_t chan, ui
 #endif
 }
 
+#if MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_cmd_airspeed_chng_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t target, float spCmd)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, spCmd);
+	_mav_put_uint8_t(buf, 4, target);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG, buf, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG, buf, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN);
+#endif
+#else
+	mavlink_cmd_airspeed_chng_t *packet = (mavlink_cmd_airspeed_chng_t *)msgbuf;
+	packet->spCmd = spCmd;
+	packet->target = target;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG, (const char *)packet, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG, (const char *)packet, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE CMD_AIRSPEED_CHNG UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h
index 642f7aab93..40a3db25a0 100644
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h
+++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h
@@ -154,6 +154,38 @@ static inline void mavlink_msg_filt_rot_vel_send(mavlink_channel_t chan, const f
 #endif
 }
 
+#if MAVLINK_MSG_ID_FILT_ROT_VEL_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_filt_rot_vel_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  const float *rotVel)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+
+	_mav_put_float_array(buf, 0, rotVel, 3);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILT_ROT_VEL, buf, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN, MAVLINK_MSG_ID_FILT_ROT_VEL_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILT_ROT_VEL, buf, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN);
+#endif
+#else
+	mavlink_filt_rot_vel_t *packet = (mavlink_filt_rot_vel_t *)msgbuf;
+
+	mav_array_memcpy(packet->rotVel, rotVel, sizeof(float)*3);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILT_ROT_VEL, (const char *)packet, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN, MAVLINK_MSG_ID_FILT_ROT_VEL_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILT_ROT_VEL, (const char *)packet, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE FILT_ROT_VEL UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h
index 0f8369881a..bceed1013c 100644
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h
+++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h
@@ -174,6 +174,40 @@ static inline void mavlink_msg_llc_out_send(mavlink_channel_t chan, const int16_
 #endif
 }
 
+#if MAVLINK_MSG_ID_LLC_OUT_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_llc_out_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  const int16_t *servoOut, const int16_t *MotorOut)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+
+	_mav_put_int16_t_array(buf, 0, servoOut, 4);
+	_mav_put_int16_t_array(buf, 8, MotorOut, 2);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LLC_OUT, buf, MAVLINK_MSG_ID_LLC_OUT_LEN, MAVLINK_MSG_ID_LLC_OUT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LLC_OUT, buf, MAVLINK_MSG_ID_LLC_OUT_LEN);
+#endif
+#else
+	mavlink_llc_out_t *packet = (mavlink_llc_out_t *)msgbuf;
+
+	mav_array_memcpy(packet->servoOut, servoOut, sizeof(int16_t)*4);
+	mav_array_memcpy(packet->MotorOut, MotorOut, sizeof(int16_t)*2);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LLC_OUT, (const char *)packet, MAVLINK_MSG_ID_LLC_OUT_LEN, MAVLINK_MSG_ID_LLC_OUT_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LLC_OUT, (const char *)packet, MAVLINK_MSG_ID_LLC_OUT_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE LLC_OUT UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h
index 5d93823262..f5a43e4fa4 100644
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h
+++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h
@@ -154,6 +154,38 @@ static inline void mavlink_msg_obs_air_temp_send(mavlink_channel_t chan, float a
 #endif
 }
 
+#if MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_obs_air_temp_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  float airT)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, airT);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_TEMP, buf, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN, MAVLINK_MSG_ID_OBS_AIR_TEMP_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_TEMP, buf, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN);
+#endif
+#else
+	mavlink_obs_air_temp_t *packet = (mavlink_obs_air_temp_t *)msgbuf;
+	packet->airT = airT;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_TEMP, (const char *)packet, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN, MAVLINK_MSG_ID_OBS_AIR_TEMP_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_TEMP, (const char *)packet, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE OBS_AIR_TEMP UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h
index 35d813ca15..67d5ab2a22 100644
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h
+++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h
@@ -192,6 +192,42 @@ static inline void mavlink_msg_obs_air_velocity_send(mavlink_channel_t chan, flo
 #endif
 }
 
+#if MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_obs_air_velocity_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  float magnitude, float aoa, float slip)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, magnitude);
+	_mav_put_float(buf, 4, aoa);
+	_mav_put_float(buf, 8, slip);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_VELOCITY, buf, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_VELOCITY, buf, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN);
+#endif
+#else
+	mavlink_obs_air_velocity_t *packet = (mavlink_obs_air_velocity_t *)msgbuf;
+	packet->magnitude = magnitude;
+	packet->aoa = aoa;
+	packet->slip = slip;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_VELOCITY, (const char *)packet, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_VELOCITY, (const char *)packet, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE OBS_AIR_VELOCITY UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h
index 9c80cd66eb..e1178e25fa 100644
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h
+++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h
@@ -154,6 +154,38 @@ static inline void mavlink_msg_obs_attitude_send(mavlink_channel_t chan, const d
 #endif
 }
 
+#if MAVLINK_MSG_ID_OBS_ATTITUDE_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_obs_attitude_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  const double *quat)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+
+	_mav_put_double_array(buf, 0, quat, 4);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_ATTITUDE, buf, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN, MAVLINK_MSG_ID_OBS_ATTITUDE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_ATTITUDE, buf, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN);
+#endif
+#else
+	mavlink_obs_attitude_t *packet = (mavlink_obs_attitude_t *)msgbuf;
+
+	mav_array_memcpy(packet->quat, quat, sizeof(double)*4);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_ATTITUDE, (const char *)packet, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN, MAVLINK_MSG_ID_OBS_ATTITUDE_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_ATTITUDE, (const char *)packet, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE OBS_ATTITUDE UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h
index 24dd43b579..b164687ff8 100644
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h
+++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h
@@ -174,6 +174,40 @@ static inline void mavlink_msg_obs_bias_send(mavlink_channel_t chan, const float
 #endif
 }
 
+#if MAVLINK_MSG_ID_OBS_BIAS_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_obs_bias_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  const float *accBias, const float *gyroBias)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+
+	_mav_put_float_array(buf, 0, accBias, 3);
+	_mav_put_float_array(buf, 12, gyroBias, 3);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_BIAS, buf, MAVLINK_MSG_ID_OBS_BIAS_LEN, MAVLINK_MSG_ID_OBS_BIAS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_BIAS, buf, MAVLINK_MSG_ID_OBS_BIAS_LEN);
+#endif
+#else
+	mavlink_obs_bias_t *packet = (mavlink_obs_bias_t *)msgbuf;
+
+	mav_array_memcpy(packet->accBias, accBias, sizeof(float)*3);
+	mav_array_memcpy(packet->gyroBias, gyroBias, sizeof(float)*3);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_BIAS, (const char *)packet, MAVLINK_MSG_ID_OBS_BIAS_LEN, MAVLINK_MSG_ID_OBS_BIAS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_BIAS, (const char *)packet, MAVLINK_MSG_ID_OBS_BIAS_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE OBS_BIAS UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h
index cfc2fe7e10..f0ecef0e7e 100644
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h
+++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h
@@ -192,6 +192,42 @@ static inline void mavlink_msg_obs_position_send(mavlink_channel_t chan, int32_t
 #endif
 }
 
+#if MAVLINK_MSG_ID_OBS_POSITION_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_obs_position_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  int32_t lon, int32_t lat, int32_t alt)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_int32_t(buf, 0, lon);
+	_mav_put_int32_t(buf, 4, lat);
+	_mav_put_int32_t(buf, 8, alt);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_POSITION, buf, MAVLINK_MSG_ID_OBS_POSITION_LEN, MAVLINK_MSG_ID_OBS_POSITION_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_POSITION, buf, MAVLINK_MSG_ID_OBS_POSITION_LEN);
+#endif
+#else
+	mavlink_obs_position_t *packet = (mavlink_obs_position_t *)msgbuf;
+	packet->lon = lon;
+	packet->lat = lat;
+	packet->alt = alt;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_POSITION, (const char *)packet, MAVLINK_MSG_ID_OBS_POSITION_LEN, MAVLINK_MSG_ID_OBS_POSITION_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_POSITION, (const char *)packet, MAVLINK_MSG_ID_OBS_POSITION_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE OBS_POSITION UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h
index 24e272bf7a..31617c8839 100644
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h
+++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h
@@ -154,6 +154,38 @@ static inline void mavlink_msg_obs_qff_send(mavlink_channel_t chan, float qff)
 #endif
 }
 
+#if MAVLINK_MSG_ID_OBS_QFF_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_obs_qff_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  float qff)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, qff);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_QFF, buf, MAVLINK_MSG_ID_OBS_QFF_LEN, MAVLINK_MSG_ID_OBS_QFF_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_QFF, buf, MAVLINK_MSG_ID_OBS_QFF_LEN);
+#endif
+#else
+	mavlink_obs_qff_t *packet = (mavlink_obs_qff_t *)msgbuf;
+	packet->qff = qff;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_QFF, (const char *)packet, MAVLINK_MSG_ID_OBS_QFF_LEN, MAVLINK_MSG_ID_OBS_QFF_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_QFF, (const char *)packet, MAVLINK_MSG_ID_OBS_QFF_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE OBS_QFF UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h
index 6e3776632b..ec815d6929 100644
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h
+++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h
@@ -154,6 +154,38 @@ static inline void mavlink_msg_obs_velocity_send(mavlink_channel_t chan, const f
 #endif
 }
 
+#if MAVLINK_MSG_ID_OBS_VELOCITY_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_obs_velocity_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  const float *vel)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+
+	_mav_put_float_array(buf, 0, vel, 3);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_VELOCITY, buf, MAVLINK_MSG_ID_OBS_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_VELOCITY_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_VELOCITY, buf, MAVLINK_MSG_ID_OBS_VELOCITY_LEN);
+#endif
+#else
+	mavlink_obs_velocity_t *packet = (mavlink_obs_velocity_t *)msgbuf;
+
+	mav_array_memcpy(packet->vel, vel, sizeof(float)*3);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_VELOCITY, (const char *)packet, MAVLINK_MSG_ID_OBS_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_VELOCITY_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_VELOCITY, (const char *)packet, MAVLINK_MSG_ID_OBS_VELOCITY_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE OBS_VELOCITY UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h
index 55f7cb2ae1..c40391d441 100644
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h
+++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h
@@ -154,6 +154,38 @@ static inline void mavlink_msg_obs_wind_send(mavlink_channel_t chan, const float
 #endif
 }
 
+#if MAVLINK_MSG_ID_OBS_WIND_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_obs_wind_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  const float *wind)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+
+	_mav_put_float_array(buf, 0, wind, 3);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_WIND, buf, MAVLINK_MSG_ID_OBS_WIND_LEN, MAVLINK_MSG_ID_OBS_WIND_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_WIND, buf, MAVLINK_MSG_ID_OBS_WIND_LEN);
+#endif
+#else
+	mavlink_obs_wind_t *packet = (mavlink_obs_wind_t *)msgbuf;
+
+	mav_array_memcpy(packet->wind, wind, sizeof(float)*3);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_WIND, (const char *)packet, MAVLINK_MSG_ID_OBS_WIND_LEN, MAVLINK_MSG_ID_OBS_WIND_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_WIND, (const char *)packet, MAVLINK_MSG_ID_OBS_WIND_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE OBS_WIND UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h
index e0963ece79..70524f464c 100644
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h
+++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h
@@ -186,6 +186,40 @@ static inline void mavlink_msg_pm_elec_send(mavlink_channel_t chan, float PwCons
 #endif
 }
 
+#if MAVLINK_MSG_ID_PM_ELEC_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_pm_elec_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  float PwCons, float BatStat, const float *PwGen)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, PwCons);
+	_mav_put_float(buf, 4, BatStat);
+	_mav_put_float_array(buf, 8, PwGen, 3);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PM_ELEC, buf, MAVLINK_MSG_ID_PM_ELEC_LEN, MAVLINK_MSG_ID_PM_ELEC_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PM_ELEC, buf, MAVLINK_MSG_ID_PM_ELEC_LEN);
+#endif
+#else
+	mavlink_pm_elec_t *packet = (mavlink_pm_elec_t *)msgbuf;
+	packet->PwCons = PwCons;
+	packet->BatStat = BatStat;
+	mav_array_memcpy(packet->PwGen, PwGen, sizeof(float)*3);
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PM_ELEC, (const char *)packet, MAVLINK_MSG_ID_PM_ELEC_LEN, MAVLINK_MSG_ID_PM_ELEC_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PM_ELEC, (const char *)packet, MAVLINK_MSG_ID_PM_ELEC_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE PM_ELEC UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h
index 94f3e58bb9..72f8a8193b 100644
--- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h
+++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h
@@ -211,6 +211,44 @@ static inline void mavlink_msg_sys_stat_send(mavlink_channel_t chan, uint8_t gps
 #endif
 }
 
+#if MAVLINK_MSG_ID_SYS_Stat_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_sys_stat_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t gps, uint8_t act, uint8_t mod, uint8_t commRssi)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_uint8_t(buf, 0, gps);
+	_mav_put_uint8_t(buf, 1, act);
+	_mav_put_uint8_t(buf, 2, mod);
+	_mav_put_uint8_t(buf, 3, commRssi);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_Stat, buf, MAVLINK_MSG_ID_SYS_Stat_LEN, MAVLINK_MSG_ID_SYS_Stat_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_Stat, buf, MAVLINK_MSG_ID_SYS_Stat_LEN);
+#endif
+#else
+	mavlink_sys_stat_t *packet = (mavlink_sys_stat_t *)msgbuf;
+	packet->gps = gps;
+	packet->act = act;
+	packet->mod = mod;
+	packet->commRssi = commRssi;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_Stat, (const char *)packet, MAVLINK_MSG_ID_SYS_Stat_LEN, MAVLINK_MSG_ID_SYS_Stat_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_Stat, (const char *)packet, MAVLINK_MSG_ID_SYS_Stat_LEN);
+#endif
+#endif
+}
+#endif
+
 #endif
 
 // MESSAGE SYS_Stat UNPACKING
diff --git a/mavlink/include/mavlink/v1.0/sensesoar/sensesoar.h b/mavlink/include/mavlink/v1.0/sensesoar/sensesoar.h
index 26666b0cc7..2eab863546 100644
--- a/mavlink/include/mavlink/v1.0/sensesoar/sensesoar.h
+++ b/mavlink/include/mavlink/v1.0/sensesoar/sensesoar.h
@@ -12,15 +12,15 @@ extern "C" {
 // MESSAGE LENGTHS AND CRCS
 
 #ifndef MAVLINK_MESSAGE_LENGTHS
-#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 0, 0, 0, 0, 0, 13, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 12, 0, 12, 0, 32, 0, 12, 0, 12, 0, 24, 0, 4, 4, 12, 0, 12, 0, 20, 0, 4, 0, 5, 0, 5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0}
+#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 42, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 0, 0, 0, 13, 255, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 12, 0, 12, 0, 32, 0, 12, 0, 12, 0, 24, 0, 4, 4, 12, 0, 12, 0, 20, 0, 4, 0, 5, 0, 5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0}
 #endif
 
 #ifndef MAVLINK_MESSAGE_CRCS
-#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 0, 0, 0, 0, 0, 29, 223, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 15, 0, 108, 0, 146, 0, 16, 0, 32, 0, 159, 0, 24, 248, 79, 0, 5, 0, 170, 0, 157, 0, 209, 0, 243, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0}
+#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 85, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 15, 0, 108, 0, 146, 0, 16, 0, 32, 0, 159, 0, 24, 248, 79, 0, 5, 0, 170, 0, 157, 0, 209, 0, 243, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0}
 #endif
 
 #ifndef MAVLINK_MESSAGE_INFO
-#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_POSITION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_VELOCITY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_ATTITUDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_WIND, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_AIR_VELOCITY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_BIAS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_QFF, MAVLINK_MESSAGE_INFO_OBS_AIR_TEMP, MAVLINK_MESSAGE_INFO_FILT_ROT_VEL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LLC_OUT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PM_ELEC, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SYS_Stat, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_CMD_AIRSPEED_CHNG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_CMD_AIRSPEED_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}}
+#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_POSITION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_VELOCITY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_ATTITUDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_WIND, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_AIR_VELOCITY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_BIAS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_QFF, MAVLINK_MESSAGE_INFO_OBS_AIR_TEMP, MAVLINK_MESSAGE_INFO_FILT_ROT_VEL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LLC_OUT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PM_ELEC, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SYS_Stat, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_CMD_AIRSPEED_CHNG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_CMD_AIRSPEED_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}}
 #endif
 
 #include "../protocol.h"
diff --git a/mavlink/include/mavlink/v1.0/sensesoar/version.h b/mavlink/include/mavlink/v1.0/sensesoar/version.h
index cdd683949e..78d5628039 100644
--- a/mavlink/include/mavlink/v1.0/sensesoar/version.h
+++ b/mavlink/include/mavlink/v1.0/sensesoar/version.h
@@ -5,7 +5,7 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Tue Feb  4 15:28:27 2014"
+#define MAVLINK_BUILD_DATE "Sun Apr 13 09:39:00 2014"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
 #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
  

From e7594a75fb9de2f75c96fa836aef1d956370070d Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 14 Apr 2014 21:44:51 +0200
Subject: [PATCH 37/97] startup: cleanup of cox mixer files (Thanks Rune)

---
 ROMFS/px4fmu_common/init.d/11001_hexa_cox                     | 2 +-
 ROMFS/px4fmu_common/init.d/12001_octo_cox                     | 2 +-
 ROMFS/px4fmu_common/init.d/rcS                                | 2 +-
 ROMFS/px4fmu_common/mixers/{hexa_cox.mix => FMU_hexa_cox.mix} | 0
 4 files changed, 3 insertions(+), 3 deletions(-)
 rename ROMFS/px4fmu_common/mixers/{hexa_cox.mix => FMU_hexa_cox.mix} (100%)

diff --git a/ROMFS/px4fmu_common/init.d/11001_hexa_cox b/ROMFS/px4fmu_common/init.d/11001_hexa_cox
index 38e65435b6..df2e609bc8 100644
--- a/ROMFS/px4fmu_common/init.d/11001_hexa_cox
+++ b/ROMFS/px4fmu_common/init.d/11001_hexa_cox
@@ -9,7 +9,7 @@
 
 sh /etc/init.d/rc.mc_defaults
 
-set MIXER hexa_cox
+set MIXER FMU_hexa_cox
 
 # We only can run one channel group with one rate, so set all 8 channels
 set PWM_OUTPUTS 12345678
diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox b/ROMFS/px4fmu_common/init.d/12001_octo_cox
index 99f902a6dd..8703f5f2fe 100644
--- a/ROMFS/px4fmu_common/init.d/12001_octo_cox
+++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox
@@ -7,6 +7,6 @@
 
 sh /etc/init.d/rc.mc_defaults
 
-set MIXER octo_cox
+set MIXER FMU_octo_cox
 
 set PWM_OUTPUTS 12345678
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 0cb6d281a2..2ab1e2e96f 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -514,7 +514,7 @@ then
 			then
 				set MAV_TYPE 13
 			fi
-			if [ $MIXER == hexa_cox ]
+			if [ $MIXER == FMU_hexa_cox ]
 			then
 				set MAV_TYPE 13
 			fi
diff --git a/ROMFS/px4fmu_common/mixers/hexa_cox.mix b/ROMFS/px4fmu_common/mixers/FMU_hexa_cox.mix
similarity index 100%
rename from ROMFS/px4fmu_common/mixers/hexa_cox.mix
rename to ROMFS/px4fmu_common/mixers/FMU_hexa_cox.mix

From 654ab4635ad0fb70d6c0cb601d56e3d97691bdac Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 18 Apr 2014 11:15:40 +0200
Subject: [PATCH 38/97] navigator: wrong mission topic was copied, clearer
 naming of offboard mission now

---
 src/modules/mavlink/mavlink_main.cpp     | 4 ++--
 src/modules/navigator/navigator_main.cpp | 6 +++---
 src/modules/uORB/objects_common.cpp      | 2 +-
 src/modules/uORB/topics/mission.h        | 2 +-
 4 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 072f16aaef..36d47b7ee4 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -832,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);
 	}
 }
 
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 1bb5d33a1f..494266dd39 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -510,7 +510,7 @@ Navigator::offboard_mission_update(bool isrotaryWing)
 {
 	struct mission_s offboard_mission;
 
-	if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) {
+	if (orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &offboard_mission) == OK) {
 
 		/* Check mission feasibility, for now do not handle the return value,
 		 * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
@@ -543,7 +543,7 @@ Navigator::onboard_mission_update()
 {
 	struct mission_s onboard_mission;
 
-	if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) {
+	if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) {
 
 		_mission.set_onboard_mission_count(onboard_mission.count);
 		_mission.set_current_onboard_mission_index(onboard_mission.current_index);
@@ -611,7 +611,7 @@ Navigator::task_main()
 	 * do subscriptions
 	 */
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	_offboard_mission_sub = orb_subscribe(ORB_ID(mission));
+	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
 	_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
 	_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
 	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index c8a589bb7c..90675bb2e9 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -127,7 +127,7 @@ ORB_DEFINE(position_setpoint_triplet, struct position_setpoint_triplet_s);
 ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s);
 
 #include "topics/mission.h"
-ORB_DEFINE(mission, struct mission_s);
+ORB_DEFINE(offboard_mission, struct mission_s);
 ORB_DEFINE(onboard_mission, struct mission_s);
 
 #include "topics/mission_result.h"
diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h
index 1ccdb7181e..dfc461ae4e 100644
--- a/src/modules/uORB/topics/mission.h
+++ b/src/modules/uORB/topics/mission.h
@@ -107,7 +107,7 @@ struct mission_s
  */
 
 /* register this as object request broker structure */
-ORB_DECLARE(mission);
+ORB_DECLARE(offboard_mission);
 ORB_DECLARE(onboard_mission);
 
 #endif

From 488785250f4f1fa3c2f6d1e3283fd8eabb6b3144 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 21 Apr 2014 17:35:42 +0200
Subject: [PATCH 39/97] fw_att_pos_estimator: added simple in-air/on-ground
 detector

---
 .../fw_att_pos_estimator_main.cpp             | 22 ++++++++++++++++++-
 1 file changed, 21 insertions(+), 1 deletion(-)

diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
index 840cd585ec..a61fff9427 100644
--- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
+++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
@@ -211,6 +211,9 @@ private:
 
 	AttPosEKF					*_ekf;
 
+	float						_velocity_xy_filtered;
+	float						_velocity_z_filtered;
+
 	/**
 	 * Update our local parameter cache.
 	 */
@@ -292,7 +295,9 @@ FixedwingEstimator::FixedwingEstimator() :
 	_initialized(false),
 	_gps_initialized(false),
 	_mavlink_fd(-1),
-	_ekf(nullptr)
+	_ekf(nullptr),
+	_velocity_xy_filtered(0.0f),
+	_velocity_z_filtered(0.0f)
 {
 
 	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
@@ -1030,6 +1035,21 @@ FixedwingEstimator::task_main()
 				_local_pos.z_global = false;
 				_local_pos.yaw = _att.yaw;
 
+				// _velocity_xy_filtered = 0.9f*_velocity_xy_filtered + 0.1f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy);
+				// _velocity_z_filtered = 0.9f*_velocity_z_filtered + 0.1f*fabsf(_local_pos.vz);
+
+
+				/* crude land detector for fixedwing only,
+				 * TODO: adapt so that it works for both, maybe move to another location
+				 */
+				if (_velocity_xy_filtered < 2
+					&& _velocity_z_filtered < 2
+					&& _airspeed.true_airspeed_m_s < 10) {
+					_local_pos.landed = true;
+				} else {
+					_local_pos.landed = false;
+				}
+
 				/* lazily publish the local position only once available */
 				if (_local_pos_pub > 0) {
 					/* publish the attitude setpoint */

From c3c0328e8bb9211580dbe5a52ecb23e0452cb402 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 21 Apr 2014 17:36:59 +0200
Subject: [PATCH 40/97] navigator: lot's of cleanup (WIP)

---
 src/modules/commander/commander.cpp           |  53 +-
 .../commander/state_machine_helper.cpp        |   6 +-
 .../fw_pos_control_l1_main.cpp                |   8 +-
 src/modules/mavlink/mavlink_main.cpp          |   7 +-
 src/modules/mavlink/mavlink_messages.cpp      |   5 +-
 .../mc_pos_control/mc_pos_control_main.cpp    |   2 +-
 src/modules/navigator/geofence_params.c       |   1 -
 .../{navigator_mission.cpp => mission.cpp}    | 259 ++---
 .../{navigator_mission.h => mission.h}        |  52 +-
 src/modules/navigator/module.mk               |   6 +-
 src/modules/navigator/navigator_main.cpp      | 978 ++++++------------
 src/modules/navigator/navigator_params.c      |  41 -
 src/modules/navigator/navigator_state.h       |  21 -
 src/modules/navigator/rtl.cpp                 | 253 +++++
 src/modules/navigator/rtl.h                   |  92 ++
 src/modules/navigator/rtl_params.c            |  87 ++
 src/modules/systemlib/state_table.h           |  23 +-
 src/modules/uORB/topics/mission.h             |   2 +
 .../uORB/topics/position_setpoint_triplet.h   |  12 +-
 .../uORB/topics/vehicle_global_position.h     |  12 +-
 src/modules/uORB/topics/vehicle_status.h      |  13 +-
 21 files changed, 980 insertions(+), 953 deletions(-)
 rename src/modules/navigator/{navigator_mission.cpp => mission.cpp} (61%)
 rename src/modules/navigator/{navigator_mission.h => mission.h} (63%)
 delete mode 100644 src/modules/navigator/navigator_state.h
 create mode 100644 src/modules/navigator/rtl.cpp
 create mode 100644 src/modules/navigator/rtl.h
 create mode 100644 src/modules/navigator/rtl_params.c

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index e7cf2b3fac..c2d24a8834 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -547,15 +547,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			unsigned int mav_goto = cmd->param1;
 
 			if (mav_goto == 0) {	// MAV_GOTO_DO_HOLD
-				status->set_nav_state = NAV_STATE_LOITER;
-				status->set_nav_state_timestamp = hrt_absolute_time();
+				status->set_nav_state = NAVIGATION_STATE_LOITER;
 				mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd");
 				result = VEHICLE_CMD_RESULT_ACCEPTED;
 				ret = true;
 
 			} else if (mav_goto == 1) {	// MAV_GOTO_DO_CONTINUE
-				status->set_nav_state = NAV_STATE_MISSION;
-				status->set_nav_state_timestamp = hrt_absolute_time();
+				status->set_nav_state = NAVIGATION_STATE_MISSION;
 				mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd");
 				result = VEHICLE_CMD_RESULT_ACCEPTED;
 				ret = true;
@@ -668,8 +666,7 @@ int commander_thread_main(int argc, char *argv[])
 	// We want to accept RC inputs as default
 	status.rc_input_blocked = false;
 	status.main_state = MAIN_STATE_MANUAL;
-	status.set_nav_state = NAV_STATE_NONE;
-	status.set_nav_state_timestamp = 0;
+	status.set_nav_state = NAVIGATION_STATE_NONE;
 	status.arming_state = ARMING_STATE_INIT;
 	status.hil_state = HIL_STATE_OFF;
 	status.failsafe_state = FAILSAFE_STATE_NORMAL;
@@ -944,7 +941,7 @@ int commander_thread_main(int argc, char *argv[])
 		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
 
 		static bool published_condition_landed_fw = false;
-		if (status.is_rotary_wing && status.condition_local_altitude_valid) {
+		if (status.condition_local_altitude_valid) {
 			if (status.condition_landed != local_position.landed) {
 				status.condition_landed = local_position.landed;
 				status_changed = true;
@@ -1222,30 +1219,30 @@ int commander_thread_main(int argc, char *argv[])
 				mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied");
 			}
 
-			/* set navigation state */
-			/* RETURN switch, overrides MISSION switch */
-			if (sp_man.return_switch == SWITCH_POS_ON) {
-				/* switch to RTL if not already landed after RTL and home position set */
-				status.set_nav_state = NAV_STATE_RTL;
-				status.set_nav_state_timestamp = hrt_absolute_time();
+			if (sp_man.mode_switch == SWITCH_POS_ON) {
 
-			} else {
-				/* MISSION switch */
-				if (sp_man.mission_switch == SWITCH_POS_ON) {
-					/* stick is in LOITER position */
-					status.set_nav_state = NAV_STATE_LOITER;
-					status.set_nav_state_timestamp = hrt_absolute_time();
+				/* set navigation state */
+				/* RETURN switch, overrides MISSION switch */
+				if (sp_man.return_switch == SWITCH_POS_ON) {
+					/* switch to RTL if not already landed after RTL and home position set */
+					status.set_nav_state = NAVIGATION_STATE_RTL;
 
-				} else if (sp_man.mission_switch != SWITCH_POS_NONE) {
-					/* stick is in MISSION position */
-					status.set_nav_state = NAV_STATE_MISSION;
-					status.set_nav_state_timestamp = hrt_absolute_time();
+				} else {
+					/* MISSION switch */
+					if (sp_man.mission_switch == SWITCH_POS_ON) {
+						/* stick is in LOITER position */
+						status.set_nav_state = NAVIGATION_STATE_LOITER;
 
-				} else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) &&
-						pos_sp_triplet.nav_state == NAV_STATE_RTL) {
-					/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
-					status.set_nav_state = NAV_STATE_MISSION;
-					status.set_nav_state_timestamp = hrt_absolute_time();
+					} else if (sp_man.mission_switch != SWITCH_POS_NONE) {
+						/* stick is in MISSION position */
+						status.set_nav_state = NAVIGATION_STATE_MISSION;
+					}
+					/* XXX: I don't understand this */					
+					//else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) &&
+					// 		pos_sp_triplet.nav_state == NAVIGATION_STATE_RTL) {
+					// 	/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
+					// 	status.set_nav_state = NAV_STATE_MISSION;
+					// }
 				}
 			}
 
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index e6f245cf93..54f0f13f4b 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -450,8 +450,7 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f
 
 			/* global position and home position required for RTL */
 			if (status->condition_global_position_valid && status->condition_home_position_valid) {
-				status->set_nav_state = NAV_STATE_RTL;
-				status->set_nav_state_timestamp = hrt_absolute_time();
+				status->set_nav_state = NAVIGATION_STATE_RTL;
 				ret = TRANSITION_CHANGED;
 			}
 
@@ -461,8 +460,7 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f
 
 			/* at least relative altitude estimate required for landing */
 			if (status->condition_local_altitude_valid || status->condition_global_position_valid) {
-				status->set_nav_state = NAV_STATE_LAND;
-				status->set_nav_state_timestamp = hrt_absolute_time();
+				status->set_nav_state = NAVIGATION_STATE_LAND;
 				ret = TRANSITION_CHANGED;
 			}
 
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 7f13df7854..a00a388a86 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -150,8 +150,6 @@ private:
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
-	bool		_setpoint_valid;		/**< flag if the position control setpoint is valid */
-
 	/** manual control states */
 	float		_seatbelt_hold_heading;		/**< heading the system should hold in seatbelt mode */
 	double		_loiter_hold_lat;
@@ -393,7 +391,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
 
 /* states */
-	_setpoint_valid(false),
 	_loiter_hold(false),
 	land_noreturn_horizontal(false),
 	land_noreturn_vertical(false),
@@ -663,7 +660,6 @@ FixedwingPositionControl::vehicle_setpoint_poll()
 
 	if (pos_sp_triplet_updated) {
 		orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
-		_setpoint_valid = true;
 	}
 }
 
@@ -708,7 +704,7 @@ void
 FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet)
 {
 
-	if (_global_pos_valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) {
+	if (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) {
 
 		/* rotate ground speed vector with current attitude */
 		math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
@@ -781,7 +777,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 	// XXX this should only execute if auto AND safety off (actuators active),
 	// else integrators should be constantly reset.
-	if (_control_mode.flag_control_position_enabled) {
+	if (pos_sp_triplet.current.valid) {
 
 		/* get circle mode */
 		bool was_circle_mode = _l1_control.circle_mode();
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 36d47b7ee4..c60c854958 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -872,7 +872,7 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item
 		break;
 	case MAV_CMD_DO_JUMP:
 		mission_item->do_jump_mission_index = mavlink_mission_item->param1;
-		/* TODO: also save param2 (repeat count) */
+		mission_item->do_jump_repeat_count = mavlink_mission_item->param2;
 		break;
 	default:
 		mission_item->acceptance_radius = mavlink_mission_item->param2;
@@ -889,6 +889,9 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item
 	// mission_item->index = mavlink_mission_item->seq;
 	mission_item->origin = ORIGIN_MAVLINK;
 
+	/* reset DO_JUMP count */
+	mission_item->do_jump_current_count = 0;
+
 	return OK;
 }
 
@@ -908,7 +911,7 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_
 
 	case NAV_CMD_DO_JUMP:
 		mavlink_mission_item->param1 = mission_item->do_jump_mission_index;
-		/* TODO: also save param2 (repeat count) */
+		mavlink_mission_item->param2 = mission_item->do_jump_repeat_count;
 		break;
 
 	default:
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 648228e3be..3cf69bf7ce 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -117,7 +117,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 	union px4_custom_mode custom_mode;
 	custom_mode.data = 0;
 
-	if (pos_sp_triplet->nav_state == NAV_STATE_NONE) {
+	if (pos_sp_triplet->nav_state == NAV_STATE_NONE_ON_GROUND
+		|| pos_sp_triplet->nav_state == NAV_STATE_NONE_IN_AIR) {
 		/* 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);
@@ -142,7 +143,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 		*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) {
+		if (pos_sp_triplet->nav_state == NAV_STATE_AUTO_ON_GROUND) {
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
 
 		} else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) {
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 2bd2d356a0..7d3d39d185 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -645,7 +645,7 @@ MulticopterPositionControl::task_main()
 				_pos_sp_triplet.current.valid = true;
 				_pos_sp_triplet.next.valid = true;
 
-				_pos_sp_triplet.nav_state = NAV_STATE_NONE;
+				// _pos_sp_triplet.nav_state = NAV_STATE_NONE;
 				_pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL;
 				_pos_sp_triplet.current.lat = _lat_sp;
 				_pos_sp_triplet.current.lon = _lon_sp;
diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c
index 5831a0ca94..653b1ad849 100644
--- a/src/modules/navigator/geofence_params.c
+++ b/src/modules/navigator/geofence_params.c
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/mission.cpp
similarity index 61%
rename from src/modules/navigator/navigator_mission.cpp
rename to src/modules/navigator/mission.cpp
index ac7e604efe..e95bcfd436 100644
--- a/src/modules/navigator/navigator_mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: @author Julian Oes <joes@student.ethz.ch>
+ *   Copyright (c) 2013-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
@@ -33,22 +32,22 @@
  ****************************************************************************/
 /**
  * @file navigator_mission.cpp
+ *
  * Helper class to access missions
+ *
+ * @author Julian Oes <julian@oes.ch>
  */
 
 #include <string.h>
 #include <stdlib.h>
+
 #include <dataman/dataman.h>
 #include <systemlib/err.h>
+
 #include <uORB/uORB.h>
 #include <uORB/topics/mission_result.h>
-#include "navigator_mission.h"
 
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-static const int ERROR = -1;
+#include "mission.h"
 
 
 Mission::Mission() :
@@ -60,27 +59,43 @@ Mission::Mission() :
 	_onboard_mission_item_count(0),
 	_onboard_mission_allowed(false),
 	_current_mission_type(MISSION_TYPE_NONE),
-	_mission_result_pub(-1)
+	_mission_result_pub(-1),
+	_mission_result({})
 {
-	memset(&_mission_result, 0, sizeof(struct mission_result_s));
 }
 
 Mission::~Mission()
 {
-
 }
 
 void
-Mission::set_offboard_dataman_id(int new_id)
+Mission::set_offboard_dataman_id(const int new_id)
 {
 	_offboard_dataman_id = new_id;
 }
 
 void
-Mission::set_current_offboard_mission_index(int new_index)
+Mission::set_offboard_mission_count(int new_count)
 {
-	if (new_index != -1) {
-		warnx("specifically set to %d", new_index);
+	_offboard_mission_item_count = new_count;
+}
+
+void
+Mission::set_onboard_mission_count(int new_count)
+{
+	_onboard_mission_item_count = new_count;
+}
+
+void
+Mission::set_onboard_mission_allowed(bool allowed)
+{
+	_onboard_mission_allowed = allowed;
+}
+
+bool
+Mission::command_current_offboard_mission_index(const int new_index)
+{
+	if (new_index >= 0) {
 		_current_offboard_mission_index = (unsigned)new_index;
 	} else {
 
@@ -92,8 +107,8 @@ Mission::set_current_offboard_mission_index(int new_index)
 	report_current_offboard_mission_item();
 }
 
-void
-Mission::set_current_onboard_mission_index(int new_index)
+bool
+Mission::command_current_onboard_mission_index(int new_index)
 {
 	if (new_index != -1) {
 		_current_onboard_mission_index = (unsigned)new_index;
@@ -108,153 +123,130 @@ Mission::set_current_onboard_mission_index(int new_index)
 	// report_current_mission_item();
 }
 
-void
-Mission::set_offboard_mission_count(unsigned new_count)
-{
-	_offboard_mission_item_count = new_count;
-}
-
-void
-Mission::set_onboard_mission_count(unsigned new_count)
-{
-	_onboard_mission_item_count = new_count;
-}
-
-void
-Mission::set_onboard_mission_allowed(bool allowed)
-{
-	_onboard_mission_allowed = allowed;
-}
-
 bool
-Mission::current_mission_available()
+Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, int *index)
 {
-	return (current_onboard_mission_available() || current_offboard_mission_available());
-
-}
-
-bool
-Mission::next_mission_available()
-{
-	return (next_onboard_mission_available() || next_offboard_mission_available());
-}
-
-int
-Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, unsigned *index)
-{
-	int ret = ERROR;
+	*onboard = false;
+	*index = -1;
 
 	/* try onboard mission first */
-	if (current_onboard_mission_available()) {
-
-		ret = get_mission_item(DM_KEY_WAYPOINTS_ONBOARD, &_current_onboard_mission_index, new_mission_item);
-		if (ret == OK) {
+	if (_current_onboard_mission_index < _onboard_mission_item_count && _onboard_mission_allowed) {
+		if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index, new_mission_item)) {
 			_current_mission_type = MISSION_TYPE_ONBOARD;
 			*onboard = true;
 			*index = _current_onboard_mission_index;
-		}
 
+			return true;
+		}
+	}
+	
 	/* otherwise fallback to offboard */
-	} else if (current_offboard_mission_available()) {
+	if (_current_offboard_mission_index < _offboard_mission_item_count) {
 
 		dm_item_t dm_current;
-
 		if (_offboard_dataman_id == 0) {
 			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
-
 		} else {
 			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
 		}
+		if (read_mission_item(dm_current, true, &_current_offboard_mission_index, new_mission_item)) {
 
-		ret = get_mission_item(dm_current, &_current_offboard_mission_index, new_mission_item);
-		if (ret == OK) {
 			_current_mission_type = MISSION_TYPE_OFFBOARD;
 			*onboard = false;
 			*index = _current_offboard_mission_index;
-		}
 
-	} else {
-		/* happens when no more mission items can be added as a next item */
-		_current_mission_type = MISSION_TYPE_NONE;
-		ret == ERROR;
+			return true;
+		}
 	}
 
-	return ret;
+	/* happens when no more mission items can be added as a next item */
+	_current_mission_type = MISSION_TYPE_NONE;
+
+	return false;
 }
 
-int
+bool
 Mission::get_next_mission_item(struct mission_item_s *new_mission_item)
 {
-	int ret = ERROR;
+	int next_temp_mission_index = _current_onboard_mission_index + 1;
 
 	/* try onboard mission first */
-	if (next_onboard_mission_available()) {
+	if (next_temp_mission_index < _onboard_mission_item_count && _onboard_mission_allowed) {
+		if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, new_mission_item)) {
+			return true;
+		}
+	}
 
-		unsigned next_onboard_mission_index = _current_onboard_mission_index + 1;
-		ret = get_mission_item(DM_KEY_WAYPOINTS_ONBOARD, &next_onboard_mission_index, new_mission_item);
+	/* then try offboard mission */
+	dm_item_t dm_current;
+	if (_offboard_dataman_id == 0) {
+		dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+	} else {
+		dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
+	}
+	next_temp_mission_index = _current_offboard_mission_index + 1;
+	if (next_temp_mission_index < _offboard_mission_item_count) {
+		if (read_mission_item(dm_current, false, &next_temp_mission_index, new_mission_item)) {
+			return true;
+		}
+	}
 
-	/* otherwise fallback to offboard */
-	} else if (next_offboard_mission_available()) {
+	/* both failed, bail out */
+	return false;
+}
 
-		dm_item_t dm_current;
+bool
+Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, struct mission_item_s *new_mission_item)
+{
+	/* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */
+	for (int i=0; i<10; i++) {
+		const ssize_t len = sizeof(struct mission_item_s);
 
-		if (_offboard_dataman_id == 0) {
-			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
-
-		} else {
-			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
+		/* read mission item from datamanager */
+		if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) {
+			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+			return false;
 		}
 
-		unsigned next_offboard_mission_index = _current_offboard_mission_index + 1;
-		ret = get_mission_item(dm_current, &next_offboard_mission_index, new_mission_item);
+		/* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */
+		if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) {
 
-	} else {
-		/* happens when no more mission items can be added as a next item */
-		ret = ERROR;
-	}
-	return ret;
-}
+			if (new_mission_item->do_jump_current_count < new_mission_item->do_jump_repeat_count) {
 
+				/* only raise the repeat count if this is for the current mission item
+				 * but not for the next mission item */
+				if (is_current) {
+					(new_mission_item->do_jump_current_count)++;
 
-bool
-Mission::current_onboard_mission_available()
-{
-	return _onboard_mission_item_count > _current_onboard_mission_index && _onboard_mission_allowed;
-}
+					/* save repeat count */
+					if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET, new_mission_item, len) != len) {
+						/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+						return false;
+					}
+				}
+				/* set new mission item index and repeat
+				 * we don't have to validate here, if it's invalid, we should realize this later .*/
+				*mission_index = new_mission_item->do_jump_mission_index;
+			} else {
+				return false;
+			}
 
-bool
-Mission::current_offboard_mission_available()
-{
-	return _offboard_mission_item_count > _current_offboard_mission_index;
-}
-
-bool
-Mission::next_onboard_mission_available()
-{
-	unsigned next = 0;
-
-	if (_current_mission_type != MISSION_TYPE_ONBOARD) {
-		next = 1;
+		} else {
+			/* if it's not a DO_JUMP, then we were successful */
+			return true;
+		}
 	}
 
-	return _onboard_mission_item_count > (_current_onboard_mission_index + next) && _onboard_mission_allowed;
-}
-
-bool
-Mission::next_offboard_mission_available()
-{
-	unsigned next = 0;
-
-	if (_current_mission_type != MISSION_TYPE_OFFBOARD) {
-		next = 1;
-	}
-
-	return _offboard_mission_item_count > (_current_offboard_mission_index + next);
+	/* we have given up, we don't want to cycle forever */
+	warnx("ERROR: cycling through mission items without success");
+	return false;
 }
 
 void
 Mission::move_to_next()
 {
+	report_mission_item_reached();
+	
 	switch (_current_mission_type) {
 	case MISSION_TYPE_ONBOARD:
 		_current_onboard_mission_index++;
@@ -277,12 +269,14 @@ Mission::report_mission_item_reached()
 		_mission_result.mission_reached = true;
 		_mission_result.mission_index_reached = _current_offboard_mission_index;
 	}
+	publish_mission_result();
 }
 
 void
 Mission::report_current_offboard_mission_item()
 {
 	_mission_result.index_current_mission = _current_offboard_mission_index;
+	publish_mission_result();
 }
 
 void
@@ -301,32 +295,3 @@ Mission::publish_mission_result()
 	_mission_result.mission_reached = false;
 }
 
-int
-Mission::get_mission_item(const dm_item_t dm_item, unsigned *mission_index, struct mission_item_s *new_mission_item)
-{
-	/* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */
-	for (int i=0; i<10; i++) {
-
-		const ssize_t len = sizeof(struct mission_item_s);
-
-		/* read mission item from datamanager */
-		if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) {
-			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
-			return ERROR;
-		}
-
-		/* check for DO_JUMP item */
-		if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) {
-			/* set new mission item index and repeat
-			 * we don't have to validate here, if it's invalid, we should realize this later .*/
-			*mission_index = new_mission_item->do_jump_mission_index;
-		} else {
-			/* if it's not a DO_JUMP, then we were successful */
-			return OK;
-		}
-	}
-
-	/* we have given up, we don't want to cycle forever */
-	warnx("ERROR: cycling through mission items without success");
-	return ERROR;
-}
diff --git a/src/modules/navigator/navigator_mission.h b/src/modules/navigator/mission.h
similarity index 63%
rename from src/modules/navigator/navigator_mission.h
rename to src/modules/navigator/mission.h
index fef1454102..0fa2ff3fa3 100644
--- a/src/modules/navigator/navigator_mission.h
+++ b/src/modules/navigator/mission.h
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: @author Julian Oes <joes@student.ethz.ch>
+ *   Copyright (c) 2013-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
@@ -34,6 +33,7 @@
 /**
  * @file navigator_mission.h
  * Helper class to access missions
+ * @author Julian Oes <julian@oes.ch>
  */
 
 #ifndef NAVIGATOR_MISSION_H
@@ -41,6 +41,7 @@
 
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
+#include <dataman/dataman.h>
 
 
 class __EXPORT Mission
@@ -52,44 +53,36 @@ public:
 	Mission();
 
 	/**
-	 * Destructor, also kills the sensors task.
+	 * Destructor
 	 */
 	~Mission();
 
-	void		set_offboard_dataman_id(int new_id);
-	void		set_current_offboard_mission_index(int new_index);
-	void		set_current_onboard_mission_index(int new_index);
-	void		set_offboard_mission_count(unsigned new_count);
-	void		set_onboard_mission_count(unsigned new_count);
+	void		set_offboard_dataman_id(const int new_id);
+	void		set_offboard_mission_count(const int new_count);
+	void		set_onboard_mission_count(const int new_count);
+	void		set_onboard_mission_allowed(const bool allowed);
 
-	void		set_onboard_mission_allowed(bool allowed);
+	bool		command_current_offboard_mission_index(const int new_index);
+	bool		command_current_onboard_mission_index(const int new_index);
 
-	bool		current_mission_available();
-	bool		next_mission_available();
-
-	int		get_current_mission_item(struct mission_item_s *mission_item, bool *onboard, unsigned *index);
-	int		get_next_mission_item(struct mission_item_s *mission_item);
+	bool		get_current_mission_item(struct mission_item_s *mission_item, bool *onboard, int *index);
+	bool		get_next_mission_item(struct mission_item_s *mission_item);
 
 	void		move_to_next();
 
+private:
+	bool		read_mission_item(const dm_item_t dm_item, const bool is_current, int *mission_index, struct mission_item_s *new_mission_item);
+
 	void		report_mission_item_reached();
 	void		report_current_offboard_mission_item();
+
 	void		publish_mission_result();
 
-private:
-	int		get_mission_item(const dm_item_t dm_item, unsigned *mission_index, struct mission_item_s *new_mission_item);
-
-	bool		current_onboard_mission_available();
-	bool		current_offboard_mission_available();
-	bool		next_onboard_mission_available();
-	bool		next_offboard_mission_available();
-
 	int 		_offboard_dataman_id;
-	unsigned	_current_offboard_mission_index;
-	unsigned	_current_onboard_mission_index;
-	unsigned	_offboard_mission_item_count;		/** number of offboard mission items available */
-	unsigned	_onboard_mission_item_count;		/** number of onboard mission items available */
-
+	int		_current_offboard_mission_index;
+	int		_current_onboard_mission_index;
+	int		_offboard_mission_item_count;		/** number of offboard mission items available */
+	int		_onboard_mission_item_count;		/** number of onboard mission items available */
 	bool		_onboard_mission_allowed;
 
 	enum {
@@ -98,9 +91,8 @@ private:
 		MISSION_TYPE_OFFBOARD,
 	} 		_current_mission_type;
 
-	int		_mission_result_pub;
-
-	struct mission_result_s _mission_result;
+	orb_advert_t	 _mission_result_pub;			/**< publish mission result topic */
+	mission_result_s _mission_result;			/**< mission result for commander/mavlink */
 };
 
 #endif
diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk
index 55f8a4caac..5f113f6865 100644
--- a/src/modules/navigator/module.mk
+++ b/src/modules/navigator/module.mk
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2013-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
@@ -39,7 +39,9 @@ MODULE_COMMAND	= navigator
 
 SRCS		= navigator_main.cpp \
 		  navigator_params.c \
-		  navigator_mission.cpp \
+		  mission.cpp \
+		  rtl.cpp \
+		  rtl_params.c \
 		  mission_feasibility_checker.cpp \
 		  geofence.cpp \
 		  geofence_params.c
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 494266dd39..34a28aec31 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -1,10 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *           @author Jean Cyr <jean.m.cyr@gmail.com>
- *           @author Julian Oes <joes@student.ethz.ch>
- *           @author Anton Babushkin <anton.babushkin@me.com>
+ *   Copyright (c) 2013-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
@@ -35,19 +31,20 @@
  *
  ****************************************************************************/
 /**
- * @file navigator_main.c
+ * @file navigator_main.cpp
  * Implementation of the main navigation state machine.
  *
- * Handles missions, geo fencing and failsafe navigation behavior.
- * Published the mission item triplet for the position controller.
+ * Handles mission items, geo fencing and failsafe navigation behavior.
+ * Published the position setpoint triplet for the position controller.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
  * @author Jean Cyr <jean.m.cyr@gmail.com>
- * @author Julian Oes <joes@student.ethz.ch>
+ * @author Julian Oes <julian@oes.ch>
  * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #include <nuttx/config.h>
+
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
@@ -58,9 +55,13 @@
 #include <poll.h>
 #include <time.h>
 #include <sys/ioctl.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+
 #include <drivers/device/device.h>
 #include <drivers/drv_hrt.h>
 #include <arch/board/board.h>
+
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/home_position.h>
@@ -71,20 +72,19 @@
 #include <uORB/topics/mission.h>
 #include <uORB/topics/fence.h>
 #include <uORB/topics/navigation_capabilities.h>
+
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/state_table.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
 #include <geo/geo.h>
-#include <mathlib/mathlib.h>
 #include <dataman/dataman.h>
+#include <mathlib/mathlib.h>
 #include <mavlink/mavlink_log.h>
-#include <sys/types.h>
-#include <sys/stat.h>
 
-#include "navigator_state.h"
-#include "navigator_mission.h"
+#include "mission.h"
+#include "rtl.h"
 #include "mission_feasibility_checker.h"
 #include "geofence.h"
 
@@ -130,12 +130,12 @@ public:
 	/**
 	 * Add point to geofence
 	 */
-	void add_fence_point(int argc, char *argv[]);
+	void		add_fence_point(int argc, char *argv[]);
 
 	/**
 	 * Load fence from file
 	 */
-	void load_fence_from_file(const char *filename);
+	void		load_fence_from_file(const char *filename);
 
 private:
 
@@ -154,15 +154,15 @@ private:
 	int		_control_mode_sub;		/**< vehicle control mode subscription */
 
 	orb_advert_t	_pos_sp_triplet_pub;		/**< publish position setpoint triplet */
-	orb_advert_t	_mission_result_pub;		/**< publish mission result topic */
 
-	struct vehicle_status_s				_vstatus;		/**< vehicle status */
-	struct vehicle_control_mode_s			_control_mode;		/**< vehicle control mode */
-	struct vehicle_global_position_s		_global_pos;		/**< global vehicle position */
-	struct home_position_s				_home_pos;		/**< home position for RTL */
-	struct position_setpoint_triplet_s		_pos_sp_triplet;	/**< triplet of position setpoints */
-	struct mission_result_s				_mission_result;	/**< mission result for commander/mavlink */
-	struct mission_item_s				_mission_item;		/**< current mission item */
+	vehicle_status_s				_vstatus;		/**< vehicle status */
+	vehicle_control_mode_s				_control_mode;		/**< vehicle control mode */
+	vehicle_global_position_s			_global_pos;		/**< global vehicle position */
+	home_position_s					_home_pos;		/**< home position for RTL */
+	position_setpoint_triplet_s			_pos_sp_triplet;	/**< triplet of position setpoints */
+	mission_item_s 					_mission_item;		/**< current mission item */
+
+	bool 						_mission_item_valid;
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
@@ -174,56 +174,41 @@ private:
 
 	struct navigation_capabilities_s		_nav_caps;
 
-	class 		Mission				_mission;
+	Mission						_mission;
+
+	RTL 						_rtl;
 
-	bool		_mission_item_valid;		/**< current mission item valid */
-	bool		_global_pos_valid;		/**< track changes of global_position.global_valid flag */
-	bool		_reset_loiter_pos;		/**< if true then loiter position should be set to current position */
 	bool		_waypoint_position_reached;
 	bool		_waypoint_yaw_reached;
 	uint64_t	_time_first_inside_orbit;
-	bool		_need_takeoff;			/**< if need to perform vertical takeoff before going to waypoint (only for MISSION mode and VTOL vehicles) */
-	bool		_do_takeoff;			/**< vertical takeoff state, current mission item is generated by navigator (only for MISSION mode and VTOL vehicles) */
-
 	MissionFeasibilityChecker missionFeasiblityChecker;
 
-	uint64_t	_set_nav_state_timestamp;	/**< timestamp of last handled navigation state request */
-
-	bool		_pos_sp_triplet_updated;
-
-	const char *nav_states_str[NAV_STATE_MAX];
+	bool		_update_triplet;
 
 	struct {
-		float min_altitude;
 		float acceptance_radius;
 		float loiter_radius;
 		int onboard_mission_enabled;
 		float takeoff_alt;
-		float land_alt;
-		float rtl_alt;
-		float rtl_land_delay;
 	}		_parameters;			/**< local copies of parameters */
 
 	struct {
-		param_t min_altitude;
 		param_t acceptance_radius;
 		param_t loiter_radius;
 		param_t onboard_mission_enabled;
 		param_t takeoff_alt;
-		param_t land_alt;
-		param_t rtl_alt;
-		param_t rtl_land_delay;
 	}		_parameter_handles;		/**< handles for parameters */
 
 	enum Event {
 		EVENT_NONE_REQUESTED,
-		EVENT_READY_REQUESTED,
 		EVENT_LOITER_REQUESTED,
 		EVENT_MISSION_REQUESTED,
 		EVENT_RTL_REQUESTED,
-		EVENT_LAND_REQUESTED,
+		EVENT_TAKEN_OFF,
+		EVENT_LANDED,
 		EVENT_MISSION_CHANGED,
 		EVENT_HOME_POSITION_CHANGED,
+		EVENT_MISSION_ITEM_REACHED,
 		MAX_EVENT
 	};
 
@@ -232,15 +217,6 @@ private:
 	 */
 	static StateTable::Tran const myTable[NAV_STATE_MAX][MAX_EVENT];
 
-	enum RTLState {
-		RTL_STATE_NONE = 0,
-		RTL_STATE_CLIMB,
-		RTL_STATE_RETURN,
-		RTL_STATE_DESCEND
-	};
-
-	enum RTLState _rtl_state;
-
 	/**
 	 * Update our local parameter cache.
 	 */
@@ -264,7 +240,7 @@ private:
 	/**
 	 * Retrieve offboard mission.
 	 */
-	void		offboard_mission_update(bool isrotaryWing);
+	void		offboard_mission_update();
 
 	/**
 	 * Retrieve onboard mission.
@@ -296,19 +272,20 @@ private:
 	/**
 	 * Functions that are triggered when a new state is entered.
 	 */
-	void		start_none();
-	void		start_ready();
-	void		start_loiter();
-	void		start_mission();
-	void		start_rtl();
-	void		start_land();
-	void		start_land_home();
+	bool		start_none_on_ground();
+	bool		start_none_in_air();
+	bool		start_auto_on_ground();
+	bool		start_loiter();
+	bool		start_mission();
+	bool		advance_mission();
+	bool		start_rtl();
+	bool		advance_rtl();
+	bool		start_land();
 
 	/**
 	 * Fork for state transitions
 	 */
-	void		request_loiter_or_ready();
-	void		request_mission_if_available();
+	// void		request_loiter_or_ready();
 
 	/**
 	 * Guards offboard mission
@@ -333,12 +310,12 @@ private:
 	/**
 	 * Perform actions when current mission item reached.
 	 */
-	void		on_mission_item_reached();
+	// void		on_mission_item_reached();
 
 	/**
 	 * Move to next waypoint
 	 */
-	void		set_mission_item();
+	bool		set_mission_items();
 
 	/**
 	 * Switch to next RTL state
@@ -348,7 +325,7 @@ private:
 	/**
 	 * Set position setpoint for mission item
 	 */
-	void		position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item);
+	void		mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp);
 
 	/**
 	 * Helper function to get a takeoff item
@@ -377,12 +354,9 @@ Navigator::Navigator() :
 
 /* state machine transition table */
 	StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT),
-
 	_task_should_exit(false),
 	_navigator_task(-1),
 	_mavlink_fd(-1),
-
-/* subscriptions */
 	_global_pos_sub(-1),
 	_home_pos_sub(-1),
 	_vstatus_sub(-1),
@@ -391,53 +365,29 @@ Navigator::Navigator() :
 	_onboard_mission_sub(-1),
 	_capabilities_sub(-1),
 	_control_mode_sub(-1),
-
-/* publications */
 	_pos_sp_triplet_pub(-1),
-
-/* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
-
 	_geofence_violation_warning_sent(false),
 	_fence_valid(false),
 	_inside_fence(true),
 	_mission(),
+	_pos_sp_triplet({}),
+	_mission_item({}),
 	_mission_item_valid(false),
-	_global_pos_valid(false),
-	_reset_loiter_pos(true),
 	_waypoint_position_reached(false),
 	_waypoint_yaw_reached(false),
 	_time_first_inside_orbit(0),
-	_need_takeoff(true),
-	_do_takeoff(false),
-	_set_nav_state_timestamp(0),
-	_pos_sp_triplet_updated(false),
-/* states */
-	_rtl_state(RTL_STATE_NONE)
+	_update_triplet(false)
 {
-	_parameter_handles.min_altitude = param_find("NAV_MIN_ALT");
 	_parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD");
 	_parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD");
 	_parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN");
 	_parameter_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT");
-	_parameter_handles.land_alt = param_find("NAV_LAND_ALT");
-	_parameter_handles.rtl_alt = param_find("NAV_RTL_ALT");
-	_parameter_handles.rtl_land_delay = param_find("NAV_RTL_LAND_T");
-
-	memset(&_pos_sp_triplet, 0, sizeof(struct position_setpoint_triplet_s));
-	memset(&_mission_item, 0, sizeof(struct mission_item_s));
-
-	memset(&nav_states_str, 0, sizeof(nav_states_str));
-	nav_states_str[0] = "NONE";
-	nav_states_str[1] = "READY";
-	nav_states_str[2] = "LOITER";
-	nav_states_str[3] = "MISSION";
-	nav_states_str[4] = "RTL";
-	nav_states_str[5] = "LAND";
 
 	/* Initialize state machine */
-	myState = NAV_STATE_NONE;
-	start_none();
+	myState = NAV_STATE_NONE_ON_GROUND;
+
+	start_none_on_ground();
 }
 
 Navigator::~Navigator()
@@ -472,16 +422,12 @@ Navigator::parameters_update()
 	struct parameter_update_s update;
 	orb_copy(ORB_ID(parameter_update), _params_sub, &update);
 
-	param_get(_parameter_handles.min_altitude, &(_parameters.min_altitude));
 	param_get(_parameter_handles.acceptance_radius, &(_parameters.acceptance_radius));
 	param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius));
 	param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled));
-	param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt));
-	param_get(_parameter_handles.land_alt, &(_parameters.land_alt));
-	param_get(_parameter_handles.rtl_alt, &(_parameters.rtl_alt));
-	param_get(_parameter_handles.rtl_land_delay, &(_parameters.rtl_land_delay));
+	param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt));	
 
-	_mission.set_onboard_mission_allowed((bool)_parameter_handles.onboard_mission_enabled);
+	_mission.set_onboard_mission_allowed((bool)_parameters.onboard_mission_enabled);
 
 	_geofence.updateParams();
 }
@@ -496,6 +442,8 @@ void
 Navigator::home_position_update()
 {
 	orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos);
+
+	_rtl.set_home_position(&_home_pos);
 }
 
 void
@@ -506,7 +454,7 @@ Navigator::navigation_capabilities_update()
 
 
 void
-Navigator::offboard_mission_update(bool isrotaryWing)
+Navigator::offboard_mission_update()
 {
 	struct mission_s offboard_mission;
 
@@ -523,19 +471,17 @@ Navigator::offboard_mission_update(bool isrotaryWing)
 			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
 		}
 
-		missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence);
+		missionFeasiblityChecker.checkMissionFeasible(_vstatus.is_rotary_wing, dm_current, (size_t)offboard_mission.count, _geofence);
 
 		_mission.set_offboard_dataman_id(offboard_mission.dataman_id);
 
 		_mission.set_offboard_mission_count(offboard_mission.count);
-		_mission.set_current_offboard_mission_index(offboard_mission.current_index);
+		_mission.command_current_offboard_mission_index(offboard_mission.current_index);
 
 	} else {
 		_mission.set_offboard_mission_count(0);
-		_mission.set_current_offboard_mission_index(0);
+		_mission.command_current_offboard_mission_index(0);
 	}
-
-	_mission.publish_mission_result();
 }
 
 void
@@ -546,11 +492,11 @@ Navigator::onboard_mission_update()
 	if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) {
 
 		_mission.set_onboard_mission_count(onboard_mission.count);
-		_mission.set_current_onboard_mission_index(onboard_mission.current_index);
+		_mission.command_current_onboard_mission_index(onboard_mission.current_index);
 
 	} else {
 		_mission.set_onboard_mission_count(0);
-		_mission.set_current_onboard_mission_index(0);
+		_mission.command_current_onboard_mission_index(0);
 	}
 }
 
@@ -626,13 +572,13 @@ Navigator::task_main()
 	global_position_update();
 	home_position_update();
 	navigation_capabilities_update();
-	offboard_mission_update(_vstatus.is_rotary_wing);
+	offboard_mission_update();
 	onboard_mission_update();
 
 	/* rate limit position updates to 50 Hz */
 	orb_set_interval(_global_pos_sub, 20);
 
-	unsigned prevState = NAV_STATE_NONE;
+	unsigned prevState = NAV_STATE_NONE_ON_GROUND;
 	hrt_abstime mavlink_open_time = 0;
 	const hrt_abstime mavlink_open_interval = 500000;
 
@@ -690,55 +636,45 @@ Navigator::task_main()
 		if (fds[6].revents & POLLIN) {
 			vehicle_status_update();
 
-			/* evaluate state requested by commander */
-			if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) {
-				if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) {
-					/* commander requested new navigation mode, try to set it */
-					switch (_vstatus.set_nav_state) {
-					case NAV_STATE_NONE:
-						/* nothing to do */
-						break;
-
-					case NAV_STATE_LOITER:
-						request_loiter_or_ready();
-						break;
-
-					case NAV_STATE_MISSION:
-						request_mission_if_available();
-						break;
-
-					case NAV_STATE_RTL:
-						if (!(_rtl_state == RTL_STATE_DESCEND &&
-							  (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
-							_vstatus.condition_home_position_valid) {
-							dispatch(EVENT_RTL_REQUESTED);
-						}
-
-						break;
-
-					case NAV_STATE_LAND:
-						dispatch(EVENT_LAND_REQUESTED);
-
-						break;
-
-					default:
-						warnx("ERROR: Requested navigation state not supported");
-						break;
-					}
-
-				} else {
-					/* on first switch to AUTO try mission by default, if none is available fallback to loiter */
-					if (myState == NAV_STATE_NONE) {
-						request_mission_if_available();
-					}
-				}
-
-			} else {
-				/* navigator shouldn't act */
+			/* commander requested new navigation mode, forward it to state machine */
+			switch (_vstatus.set_nav_state) {
+			case NAVIGATION_STATE_NONE:
 				dispatch(EVENT_NONE_REQUESTED);
+				break;
+
+			case NAVIGATION_STATE_LOITER:
+				dispatch(EVENT_LOITER_REQUESTED);
+				break;
+
+			case NAVIGATION_STATE_MISSION:
+				dispatch(EVENT_MISSION_REQUESTED);
+				break;
+
+			case NAVIGATION_STATE_RTL:
+				/* TODO: what is this for? */
+				// if (!(_rtl_state == RTL_STATE_DESCEND &&
+				// 	  (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
+				// 	_vstatus.condition_home_position_valid) {
+				dispatch(EVENT_RTL_REQUESTED);
+				// }
+				break;
+
+			case NAVIGATION_STATE_LAND:
+				/* TODO: add this */
+				// dispatch(EVENT_LAND_REQUESTED);
+				break;
+
+			default:
+				warnx("ERROR: Requested navigation state not supported");
+				break;
 			}
 
-			_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
+			/* commander sets in-air/on-ground flag as well */
+			if (_vstatus.condition_landed) {
+				dispatch(EVENT_LANDED);
+			} else {
+				dispatch(EVENT_TAKEN_OFF);
+			}
 		}
 
 		/* parameters updated */
@@ -754,74 +690,49 @@ Navigator::task_main()
 
 		/* offboard mission updated */
 		if (fds[4].revents & POLLIN) {
-			offboard_mission_update(_vstatus.is_rotary_wing);
-			// XXX check if mission really changed
+			offboard_mission_update();
+			/* TODO: check if mission really changed */
 			dispatch(EVENT_MISSION_CHANGED);
 		}
 
 		/* onboard mission updated */
 		if (fds[5].revents & POLLIN) {
 			onboard_mission_update();
-			// XXX check if mission really changed
+			/* TODO: check if mission really changed */
 			dispatch(EVENT_MISSION_CHANGED);
 		}
 
 		/* home position updated */
 		if (fds[2].revents & POLLIN) {
 			home_position_update();
-			// XXX check if home position really changed
+			/* TODO check if home position really changed */
 			dispatch(EVENT_HOME_POSITION_CHANGED);
 		}
 
 		/* global position updated */
 		if (fds[1].revents & POLLIN) {
 			global_position_update();
-
-			/* publish position setpoint triplet on each position update if navigator active */
-			if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) {
-				_pos_sp_triplet_updated = true;
-
-				if (myState == NAV_STATE_LAND && _global_pos.global_valid && !_global_pos_valid) {
-					/* got global position when landing, update setpoint */
-					start_land();
-				}
-
-				_global_pos_valid = _global_pos.global_valid;
-
-				/* check if waypoint has been reached in MISSION, RTL and LAND modes */
-				if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) {
-					if (check_mission_item_reached()) {
-						on_mission_item_reached();
-					}
-				}
+			if (check_mission_item_reached()) {
+				dispatch(EVENT_MISSION_ITEM_REACHED);
 			}
 
 			/* Check geofence violation */
 			if (!_geofence.inside(&_global_pos)) {
-				//xxx: publish geofence violation here (or change local flag depending on which app handles the flight termination)
 
 				/* Issue a warning about the geofence violation once */
 				if (!_geofence_violation_warning_sent) {
 					mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation");
 					_geofence_violation_warning_sent = true;
 				}
-
 			} else {
 				/* Reset the _geofence_violation_warning_sent field */
 				_geofence_violation_warning_sent = false;
 			}
 		}
 
-		/* publish position setpoint triplet if updated */
-		if (_pos_sp_triplet_updated) {
-			_pos_sp_triplet_updated = false;
+		if (_update_triplet ) {
 			publish_position_setpoint_triplet();
-		}
-
-		/* notify user about state changes */
-		if (myState != prevState) {
-			mavlink_log_info(_mavlink_fd, "#audio: navigation state: %s", nav_states_str[myState]);
-			prevState = myState;
+			_update_triplet = false;
 		}
 
 		perf_end(_loop_perf);
@@ -879,8 +790,12 @@ Navigator::status()
 	}
 
 	switch (myState) {
-	case NAV_STATE_NONE:
-		warnx("State: None");
+	case NAV_STATE_NONE_ON_GROUND:
+		warnx("State: None on ground");
+		break;
+
+	case NAV_STATE_NONE_IN_AIR:
+		warnx("State: None in air");
 		break;
 
 	case NAV_STATE_LOITER:
@@ -895,6 +810,10 @@ Navigator::status()
 		warnx("State: RTL");
 		break;
 
+	case NAV_STATE_LAND:
+		warnx("State: LAND");
+		break;
+
 	default:
 		warnx("State: Unknown");
 		break;
@@ -903,92 +822,119 @@ Navigator::status()
 
 StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
 	{
-		/* NAV_STATE_NONE */
-		/* EVENT_NONE_REQUESTED */		{NO_ACTION, NAV_STATE_NONE},
-		/* EVENT_READY_REQUESTED */		{ACTION(&Navigator::start_ready), NAV_STATE_READY},
+		/* NAV_STATE_NONE_ON_GROUND */
+		/* EVENT_NONE_REQUESTED */		{NO_ACTION, NAV_STATE_NONE_ON_GROUND},
+		/* EVENT_LOITER_REQUESTED */		{NO_ACTION, NAV_STATE_NONE_ON_GROUND},
+		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_auto_on_ground), NAV_STATE_AUTO_ON_GROUND},
+		/* EVENT_RTL_REQUESTED */		{NO_ACTION, NAV_STATE_NONE_ON_GROUND},
+		/* EVENT_TAKEN_OFF */			{ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR},
+		/* EVENT_LANDED */			{NO_ACTION, NAV_STATE_NONE_ON_GROUND},
+		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_NONE_ON_GROUND},
+		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_NONE_ON_GROUND},
+		/* EVENT_MISSION_ITEM_REACHED */	{NO_ACTION, NAV_STATE_NONE_ON_GROUND},
+	},
+	{
+		/* NAV_STATE_NONE_IN_AIR */
+		/* EVENT_NONE_REQUESTED */		{NO_ACTION, NAV_STATE_NONE_IN_AIR},
 		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
 		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
 		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
-		/* EVENT_LAND_REQUESTED */		{ACTION(&Navigator::start_land), NAV_STATE_LAND},
-		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_NONE},
-		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_NONE},
+		/* EVENT_TAKEN_OFF */			{NO_ACTION, NAV_STATE_NONE_IN_AIR},
+		/* EVENT_LANDED */			{ACTION(&Navigator::start_none_on_ground), NAV_STATE_NONE_ON_GROUND},
+		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_NONE_IN_AIR},
+		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_NONE_IN_AIR},
+		/* EVENT_MISSION_ITEM_REACHED */	{NO_ACTION, NAV_STATE_NONE_IN_AIR},
 	},
 	{
-		/* NAV_STATE_READY */
-		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), NAV_STATE_NONE},
-		/* EVENT_READY_REQUESTED */		{NO_ACTION, NAV_STATE_READY},
-		/* EVENT_LOITER_REQUESTED */		{NO_ACTION, NAV_STATE_READY},
-		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
-		/* EVENT_RTL_REQUESTED */		{NO_ACTION, NAV_STATE_READY},
-		/* EVENT_LAND_REQUESTED */		{NO_ACTION, NAV_STATE_READY},
-		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_READY},
-		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_READY},
+		/* NAV_STATE_AUTO_ON_GROUND */
+		/* EVENT_NONE_REQUESTED */		{NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
+		/* EVENT_LOITER_REQUESTED */		{NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
+		/* EVENT_MISSION_REQUESTED */		{NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
+		/* EVENT_RTL_REQUESTED */		{NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
+		/* EVENT_TAKEN_OFF */			{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
+		/* EVENT_LANDED */			{NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
+		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
+		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
+		/* EVENT_MISSION_ITEM_REACHED */	{NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
 	},
 	{
 		/* NAV_STATE_LOITER */
-		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), NAV_STATE_NONE},
-		/* EVENT_READY_REQUESTED */		{NO_ACTION, NAV_STATE_LOITER},
+		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR},
 		/* EVENT_LOITER_REQUESTED */		{NO_ACTION, NAV_STATE_LOITER},
 		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
 		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
-		/* EVENT_LAND_REQUESTED */		{ACTION(&Navigator::start_land), NAV_STATE_LAND},
+		/* EVENT_TAKEN_OFF */			{NO_ACTION, NAV_STATE_LOITER},
+		/* EVENT_LANDED */			{NO_ACTION, NAV_STATE_LOITER},
 		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_LOITER},
 		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_LOITER},
+		/* EVENT_MISSION_ITEM_REACHED */	{NO_ACTION, NAV_STATE_LOITER},
 	},
 	{
 		/* NAV_STATE_MISSION */
-		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), NAV_STATE_NONE},
-		/* EVENT_READY_REQUESTED */		{ACTION(&Navigator::start_ready), NAV_STATE_READY},
+		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR},
 		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
 		/* EVENT_MISSION_REQUESTED */		{NO_ACTION, NAV_STATE_MISSION},
 		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
-		/* EVENT_LAND_REQUESTED */		{ACTION(&Navigator::start_land), NAV_STATE_LAND},
+		/* EVENT_TAKEN_OFF */			{NO_ACTION, NAV_STATE_MISSION},
+		/* EVENT_LANDED */			{NO_ACTION, NAV_STATE_MISSION},
 		/* EVENT_MISSION_CHANGED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
 		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_MISSION},
+		/* EVENT_MISSION_ITEM_REACHED */	{ACTION(&Navigator::advance_mission), NAV_STATE_MISSION},
 	},
 	{
 		/* NAV_STATE_RTL */
-		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), NAV_STATE_NONE},
-		/* EVENT_READY_REQUESTED */		{ACTION(&Navigator::start_ready), NAV_STATE_READY},
+		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR},
 		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
 		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
 		/* EVENT_RTL_REQUESTED */		{NO_ACTION, NAV_STATE_RTL},
-		/* EVENT_LAND_REQUESTED */		{ACTION(&Navigator::start_land_home), NAV_STATE_LAND},
+		/* EVENT_TAKEN_OFF */			{NO_ACTION, NAV_STATE_RTL},
+		/* EVENT_LANDED */			{NO_ACTION, NAV_STATE_RTL},
 		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_RTL},
-		/* EVENT_HOME_POSITION_CHANGED */	{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},	// TODO need to reset rtl_state
+		/* EVENT_HOME_POSITION_CHANGED */	{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
+		/* EVENT_MISSION_ITEM_REACHED */	{ACTION(&Navigator::advance_rtl), NAV_STATE_RTL},
 	},
 	{
 		/* NAV_STATE_LAND */
-		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), NAV_STATE_NONE},
-		/* EVENT_READY_REQUESTED */		{ACTION(&Navigator::start_ready), NAV_STATE_READY},
+		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR},
 		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
 		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
 		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
-		/* EVENT_LAND_REQUESTED */		{NO_ACTION, NAV_STATE_LAND},
+		/* EVENT_TAKEN_OFF */			{NO_ACTION, NAV_STATE_LAND},
+		/* EVENT_LANDED */			{ACTION(&Navigator::start_auto_on_ground), NAV_STATE_AUTO_ON_GROUND},
 		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_LAND},
 		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_LAND},
+		/* EVENT_MISSION_ITEM_REACHED */	{NO_ACTION, NAV_STATE_LAND},
 	},
 };
 
-void
-Navigator::start_none()
+bool
+Navigator::start_none_on_ground()
 {
 	reset_reached();
 
 	_pos_sp_triplet.previous.valid = false;
 	_pos_sp_triplet.current.valid = false;
 	_pos_sp_triplet.next.valid = false;
-	_mission_item_valid = false;
 
-	_reset_loiter_pos = true;
-	_do_takeoff = false;
-	_rtl_state = RTL_STATE_NONE;
-
-	_pos_sp_triplet_updated = true;
+	_update_triplet = true;
+	return true;
 }
 
-void
-Navigator::start_ready()
+bool
+Navigator::start_none_in_air()
+{
+	reset_reached();
+
+	_pos_sp_triplet.previous.valid = false;
+	_pos_sp_triplet.current.valid = false;
+	_pos_sp_triplet.next.valid = false;
+
+	_update_triplet = true;
+	return true;
+}
+
+bool
+Navigator::start_auto_on_ground()
 {
 	reset_reached();
 
@@ -998,46 +944,26 @@ Navigator::start_ready()
 
 	_pos_sp_triplet.current.type = SETPOINT_TYPE_IDLE;
 
-	_mission_item_valid = false;
 
-	_reset_loiter_pos = true;
-	_do_takeoff = false;
+	// if (_rtl_state != RTL_STATE_DESCEND) {
+	// 	 reset RTL state if landed not at home 
+	// 	_rtl_state = RTL_STATE_NONE;
+	// }
 
-	if (_rtl_state != RTL_STATE_DESCEND) {
-		/* reset RTL state if landed not at home */
-		_rtl_state = RTL_STATE_NONE;
-	}
-
-	_pos_sp_triplet_updated = true;
+	_update_triplet = true;
+	return true;
 }
 
-void
+bool
 Navigator::start_loiter()
 {
-	reset_reached();
-
-	_do_takeoff = false;
-
-	/* set loiter position if needed */
-	if (_reset_loiter_pos || !_pos_sp_triplet.current.valid) {
-		_reset_loiter_pos = false;
+	/* if no existing item available, use current position */
+	if (!(_pos_sp_triplet.current.valid && _waypoint_position_reached)) {
 
 		_pos_sp_triplet.current.lat = _global_pos.lat;
 		_pos_sp_triplet.current.lon = _global_pos.lon;
 		_pos_sp_triplet.current.yaw = NAN;	// NAN means to use current yaw
-
-		float min_alt_amsl = _parameters.min_altitude + _home_pos.alt;
-
-		/* use current altitude if above min altitude set by parameter */
-		if (_global_pos.alt < min_alt_amsl && !_vstatus.is_rotary_wing) {
-			_pos_sp_triplet.current.alt = min_alt_amsl;
-			mavlink_log_info(_mavlink_fd, "#audio: loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt));
-
-		} else {
-			_pos_sp_triplet.current.alt = _global_pos.alt;
-			mavlink_log_info(_mavlink_fd, "#audio: loiter at current altitude");
-		}
-
+		_pos_sp_triplet.current.alt = _global_pos.alt;
 	}
 	_pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER;
 	_pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius;
@@ -1045,167 +971,146 @@ Navigator::start_loiter()
 	_pos_sp_triplet.previous.valid = false;
 	_pos_sp_triplet.current.valid = true;
 	_pos_sp_triplet.next.valid = false;
-	_mission_item_valid = false;
 
-	_pos_sp_triplet_updated = true;
+	mavlink_log_info(_mavlink_fd, "#audio: loiter at current altitude");
+
+	_update_triplet = true;
+	return true;	
 }
 
-void
+bool
 Navigator::start_mission()
 {
-	_need_takeoff = true;
+	/* start fresh */
+	_pos_sp_triplet.previous.valid = false;
+	_pos_sp_triplet.current.valid = false;
+	_pos_sp_triplet.next.valid = false;
 
-	set_mission_item();
+	return set_mission_items();
 }
 
-void
-Navigator::set_mission_item()
+bool
+Navigator::advance_mission()
 {
+	/* tell mission to move by one */
+	_mission.move_to_next();
+
+	/* now try to set the new mission items, if it fails, it will dispatch loiter */
+	return set_mission_items();
+}
+
+bool
+Navigator::set_mission_items()
+{
+	if (_pos_sp_triplet.current.valid) {
+		memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
+		_pos_sp_triplet.previous.valid = true;
+	}
+
+	bool onboard;
+	int index;
+
+	/* if we fail to set the current mission, continue to loiter */
+	if (!_mission.get_current_mission_item(&_mission_item, &onboard, &index)) {
+
+		dispatch(EVENT_LOITER_REQUESTED);
+		return false;
+	}
+
+	/* if we got an RTL mission item, switch to RTL mode and give up */
+	if (_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
+		dispatch(EVENT_RTL_REQUESTED);
+		return false;
+	}
+
+	_mission_item_valid = true;
+
+	/* convert the current mission item and set it valid */
+	mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
+	_pos_sp_triplet.current.valid = true;
+	
+
+	mission_item_s next_mission_item;
+
+	bool last_wp = false;
+	/* now try to set the next mission item as well, if there is no more next
+	 * this means we're heading to the last waypoint */
+	if (_mission.get_next_mission_item(&next_mission_item)) {
+		/* convert the next mission item and set it valid */
+		mission_item_to_position_setpoint(&next_mission_item, &_pos_sp_triplet.next);
+		_pos_sp_triplet.next.valid = true;
+	} else {
+		last_wp = true;
+	}
+
+	/* notify user about what happened */
+	mavlink_log_info(_mavlink_fd, "#audio: heading to %s%swaypoint %d",
+		(last_wp ? "last " : "" ), (onboard ? "onboard " : ""), index);
+
+	_update_triplet = true;
+
 	reset_reached();
 
-	/* copy current mission to previous item */
-	memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
-
-	_reset_loiter_pos = true;
-	_do_takeoff = false;
-
-	int ret;
-	bool onboard;
-	unsigned index;
-
-	ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index);
-
-	if (ret == OK) {
-		_mission.report_current_offboard_mission_item();
-
-		_mission_item_valid = true;
-		position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
-
-		if (_mission_item.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH &&
-		    _mission_item.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT &&
-		    _mission_item.nav_cmd != NAV_CMD_LOITER_TURN_COUNT &&
-		    _mission_item.nav_cmd != NAV_CMD_LOITER_UNLIMITED) {
-			/* don't reset RTL state on RTL or LOITER items */
-			_rtl_state = RTL_STATE_NONE;
-		}
-
-		if (_vstatus.is_rotary_wing) {
-			if (_need_takeoff && (
-				    _mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
-				    _mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
-				    _mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH ||
-				    _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
-				    _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
-				    _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED
-			    )) {
-				/* do special TAKEOFF handling for VTOL */
-				_need_takeoff = false;
-
-				/* calculate desired takeoff altitude AMSL */
-				float takeoff_alt_amsl = _pos_sp_triplet.current.alt;
-
-				if (_vstatus.condition_landed) {
-					/* takeoff to at least NAV_TAKEOFF_ALT from ground if landed */
-					takeoff_alt_amsl = fmaxf(takeoff_alt_amsl, _global_pos.alt + _parameters.takeoff_alt);
-				}
-
-				/* check if we really need takeoff */
-				if (_vstatus.condition_landed || _global_pos.alt < takeoff_alt_amsl - _mission_item.acceptance_radius) {
-					/* force TAKEOFF if landed or waypoint altitude is more than current */
-					_do_takeoff = true;
-
-					/* move current position setpoint to next */
-					memcpy(&_pos_sp_triplet.next, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
-
-					/* set current setpoint to takeoff */
-
-					_pos_sp_triplet.current.lat = _global_pos.lat;
-					_pos_sp_triplet.current.lon = _global_pos.lon;
-					_pos_sp_triplet.current.alt = takeoff_alt_amsl;
-					_pos_sp_triplet.current.yaw = NAN;
-					_pos_sp_triplet.current.type = SETPOINT_TYPE_TAKEOFF;
-				}
-
-			} else if (_mission_item.nav_cmd == NAV_CMD_LAND) {
-				/* will need takeoff after landing */
-				_need_takeoff = true;
-			}
-		}
-
-		if (_do_takeoff) {
-			mavlink_log_info(_mavlink_fd, "#audio: takeoff to %.1fm above home", (double)(_pos_sp_triplet.current.alt - _home_pos.alt));
-
-		} else {
-			if (onboard) {
-				mavlink_log_info(_mavlink_fd, "#audio: heading to onboard WP %d", index);
-
-			} else {
-				mavlink_log_info(_mavlink_fd, "#audio: heading to offboard WP %d", index);
-			}
-		}
-
-	} else {
-		/* since a mission is not advanced without WPs available, this is not supposed to happen */
-		_mission_item_valid = false;
-		_pos_sp_triplet.current.valid = false;
-		warnx("ERROR: current WP can't be set");
-	}
-
-	if (!_do_takeoff) {
-		mission_item_s item_next;
-		ret = _mission.get_next_mission_item(&item_next);
-
-		if (ret == OK) {
-			position_setpoint_from_mission_item(&_pos_sp_triplet.next, &item_next);
-
-		} else {
-			/* this will fail for the last WP */
-			_pos_sp_triplet.next.valid = false;
-		}
-	}
-
-	_pos_sp_triplet_updated = true;
+	return true;
 }
 
-void
+bool
 Navigator::start_rtl()
 {
-	_do_takeoff = false;
+	/* TODO check if RTL was successfull, if not we have a problem and need to dispatch something */
+	if (_rtl.get_current_rtl_item(&_global_pos, &_mission_item)) {
 
-	/* decide if we need climb */
-	if (_rtl_state == RTL_STATE_NONE) {
-		if (_global_pos.alt < _home_pos.alt + _parameters.rtl_alt) {
-			_rtl_state = RTL_STATE_CLIMB;
+		_mission_item_valid = true;
 
-		} else {
-			_rtl_state = RTL_STATE_RETURN;
-		}
+		mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
+		_pos_sp_triplet.current.valid = true;
+
+		reset_reached();
+
+		_update_triplet = true;
+		return true;
 	}
+	dispatch(EVENT_LOITER_REQUESTED);
 
-	/* if switching directly to return state, reset altitude setpoint */
-	if (_rtl_state == RTL_STATE_RETURN) {
-		_mission_item.altitude_is_relative = false;
-		_mission_item.altitude = _global_pos.alt;
-	}
-
-	_reset_loiter_pos = true;
-	set_rtl_item();
+	return false;
 }
 
-void
+bool
+Navigator::advance_rtl()
+{
+	/* tell mission to move by one */
+	_rtl.move_to_next();
+
+	/* now try to set the new mission items, if it fails, it will dispatch loiter */
+	if (_rtl.get_current_rtl_item(&_global_pos, &_mission_item)) {
+
+		_mission_item_valid = true;
+
+		mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
+		_pos_sp_triplet.current.valid = true;
+
+		reset_reached();
+
+		_update_triplet = true;
+		return true;
+	}
+
+	dispatch(EVENT_LOITER_REQUESTED);
+	return false;
+}
+
+bool
 Navigator::start_land()
 {
+	/* TODO: verify/test */
+
 	reset_reached();
 
 	/* this state can be requested by commander even if no global position available,
 	 * in his case controller must perform landing without position control */
-	_do_takeoff = false;
-	_reset_loiter_pos = true;
 
 	memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
 
-	_mission_item_valid = true;
-
 	_mission_item.lat = _global_pos.lat;
 	_mission_item.lon = _global_pos.lon;
 	_mission_item.altitude_is_relative = false;
@@ -1220,185 +1125,22 @@ Navigator::start_land()
 	_mission_item.autocontinue = true;
 	_mission_item.origin = ORIGIN_ONBOARD;
 
-	position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
-
-	_pos_sp_triplet.next.valid = false;
-}
-
-void
-Navigator::start_land_home()
-{
-	reset_reached();
-
-	/* land to home position, should be called when hovering above home, from RTL state */
-	_do_takeoff = false;
-	_reset_loiter_pos = true;
-
-	memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
-
 	_mission_item_valid = true;
 
-	_mission_item.lat = _home_pos.lat;
-	_mission_item.lon = _home_pos.lon;
-	_mission_item.altitude_is_relative = false;
-	_mission_item.altitude = _home_pos.alt;
-	_mission_item.yaw = NAN;
-	_mission_item.loiter_radius = _parameters.loiter_radius;
-	_mission_item.loiter_direction = 1;
-	_mission_item.nav_cmd = NAV_CMD_LAND;
-	_mission_item.acceptance_radius = _parameters.acceptance_radius;
-	_mission_item.time_inside = 0.0f;
-	_mission_item.pitch_min = 0.0f;
-	_mission_item.autocontinue = true;
-	_mission_item.origin = ORIGIN_ONBOARD;
-
-	position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
+	mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
 
 	_pos_sp_triplet.next.valid = false;
+
+	_update_triplet = true;
+	return true;
 }
 
 void
-Navigator::set_rtl_item()
-{
-	reset_reached();
-
-	switch (_rtl_state) {
-	case RTL_STATE_CLIMB: {
-			memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
-
-			float climb_alt = _home_pos.alt + _parameters.rtl_alt;
-
-			if (_vstatus.condition_landed) {
-				climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt);
-			}
-
-			_mission_item_valid = true;
-
-			_mission_item.lat = _global_pos.lat;
-			_mission_item.lon = _global_pos.lon;
-			_mission_item.altitude_is_relative = false;
-			_mission_item.altitude = climb_alt;
-			_mission_item.yaw = NAN;
-			_mission_item.loiter_radius = _parameters.loiter_radius;
-			_mission_item.loiter_direction = 1;
-			_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
-			_mission_item.acceptance_radius = _parameters.acceptance_radius;
-			_mission_item.time_inside = 0.0f;
-			_mission_item.pitch_min = 0.0f;
-			_mission_item.autocontinue = true;
-			_mission_item.origin = ORIGIN_ONBOARD;
-
-			position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
-
-			_pos_sp_triplet.next.valid = false;
-
-			mavlink_log_info(_mavlink_fd, "#audio: RTL: climb to %.1fm above home", (double)(climb_alt - _home_pos.alt));
-			break;
-		}
-
-	case RTL_STATE_RETURN: {
-			memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
-
-			_mission_item_valid = true;
-
-			_mission_item.lat = _home_pos.lat;
-			_mission_item.lon = _home_pos.lon;
-			// don't change altitude
-			if (_pos_sp_triplet.previous.valid) {
-				/* if previous setpoint is valid then use it to calculate heading to home */
-				_mission_item.yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, _mission_item.lat, _mission_item.lon);
-
-			} else {
-				/* else use current position */
-				_mission_item.yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, _mission_item.lat, _mission_item.lon);
-			}
-			_mission_item.loiter_radius = _parameters.loiter_radius;
-			_mission_item.loiter_direction = 1;
-			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-			_mission_item.acceptance_radius = _parameters.acceptance_radius;
-			_mission_item.time_inside = 0.0f;
-			_mission_item.pitch_min = 0.0f;
-			_mission_item.autocontinue = true;
-			_mission_item.origin = ORIGIN_ONBOARD;
-
-			position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
-
-			_pos_sp_triplet.next.valid = false;
-
-			mavlink_log_info(_mavlink_fd, "#audio: RTL: return at %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt));
-			break;
-		}
-
-	case RTL_STATE_DESCEND: {
-			memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
-
-			_mission_item_valid = true;
-
-			_mission_item.lat = _home_pos.lat;
-			_mission_item.lon = _home_pos.lon;
-			_mission_item.altitude_is_relative = false;
-			_mission_item.altitude = _home_pos.alt + _parameters.land_alt;
-			_mission_item.yaw = NAN;
-			_mission_item.loiter_radius = _parameters.loiter_radius;
-			_mission_item.loiter_direction = 1;
-			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-			_mission_item.acceptance_radius = _parameters.acceptance_radius;
-			_mission_item.time_inside = _parameters.rtl_land_delay < 0.0f ? 0.0f : _parameters.rtl_land_delay;
-			_mission_item.pitch_min = 0.0f;
-			_mission_item.autocontinue = _parameters.rtl_land_delay > -0.001f;
-			_mission_item.origin = ORIGIN_ONBOARD;
-
-			position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
-
-			_pos_sp_triplet.next.valid = false;
-
-			mavlink_log_info(_mavlink_fd, "#audio: RTL: descend to %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt));
-			break;
-		}
-
-	default: {
-			mavlink_log_critical(_mavlink_fd, "#audio: [navigator] error: unknown RTL state: %d", _rtl_state);
-			start_loiter();
-			break;
-		}
-	}
-
-	_pos_sp_triplet_updated = true;
-}
-
-void
-Navigator::request_loiter_or_ready()
-{
-	/* XXX workaround: no landing detector for fixedwing yet */
-	if (_vstatus.condition_landed && _vstatus.is_rotary_wing) {
-		dispatch(EVENT_READY_REQUESTED);
-
-	} else {
-		dispatch(EVENT_LOITER_REQUESTED);
-	}
-}
-
-void
-Navigator::request_mission_if_available()
-{
-	if (_mission.current_mission_available()) {
-		dispatch(EVENT_MISSION_REQUESTED);
-
-	} else {
-		request_loiter_or_ready();
-	}
-}
-
-void
-Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item)
+Navigator::mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp)
 {
 	sp->valid = true;
 
 	if (item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
-		/* set home position for RTL item */
-		sp->lat = _home_pos.lat;
-		sp->lon = _home_pos.lon;
-		sp->alt = _home_pos.alt + _parameters.rtl_alt;
 
 		if (_pos_sp_triplet.previous.valid) {
 			/* if previous setpoint is valid then use it to calculate heading to home */
@@ -1408,9 +1150,6 @@ Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_
 			/* else use current position */
 			sp->yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, sp->lat, sp->lon);
 		}
-		sp->loiter_radius = _parameters.loiter_radius;
-		sp->loiter_direction = 1;
-		sp->pitch_min = 0.0f;
 
 	} else {
 		sp->lat = item->lat;
@@ -1450,14 +1189,14 @@ Navigator::check_mission_item_reached()
 		return _vstatus.condition_landed;
 	}
 
-	/* XXX TODO count turns */
-	if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
-	     _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
-	     _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
-	    _mission_item.loiter_radius > 0.01f) {
+	/* TODO count turns */
+	// if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
+	//      _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+	//      _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
+	//     _mission_item.loiter_radius > 0.01f) {
 
-		return false;
-	}
+	// 	return false;
+	// }
 
 	uint64_t now = hrt_absolute_time();
 
@@ -1475,22 +1214,19 @@ Navigator::check_mission_item_reached()
 		float dist_xy = -1.0f;
 		float dist_z = -1.0f;
 
-		/* calculate AMSL altitude for this waypoint */
-		float wp_alt_amsl = _mission_item.altitude;
+		float altitude_amsl = _mission_item.altitude_is_relative
+					? _mission_item.altitude + _home_pos.alt : _mission_item.altitude;
 
-		if (_mission_item.altitude_is_relative)
-			wp_alt_amsl += _home_pos.alt;
-
-		dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl,
-				(double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt,
+		dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl,
+				_global_pos.lat, _global_pos.lon, _global_pos.alt,
 				&dist_xy, &dist_z);
 
-		if (_do_takeoff) {
-			if (_global_pos.alt > wp_alt_amsl - acceptance_radius) {
-				/* require only altitude for takeoff */
+		if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
+
+			/* require only altitude for takeoff */
+			if (_global_pos.alt > altitude_amsl - acceptance_radius) {
 				_waypoint_position_reached = true;
 			}
-
 		} else {
 			if (dist >= 0.0f && dist <= acceptance_radius) {
 				_waypoint_position_reached = true;
@@ -1499,7 +1235,10 @@ Navigator::check_mission_item_reached()
 	}
 
 	if (_waypoint_position_reached && !_waypoint_yaw_reached) {
-		if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item.yaw)) {
+		
+		/* TODO: removed takeoff, why? */
+		if (_vstatus.is_rotary_wing && isfinite(_mission_item.yaw)) {
+
 			/* check yaw if defined only for rotary wing except takeoff */
 			float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw);
 
@@ -1514,24 +1253,23 @@ Navigator::check_mission_item_reached()
 
 	/* check if the current waypoint was reached */
 	if (_waypoint_position_reached && _waypoint_yaw_reached) {
+
 		if (_time_first_inside_orbit == 0) {
 			_time_first_inside_orbit = now;
 
 			if (_mission_item.time_inside > 0.01f) {
-				mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs", (double)_mission_item.time_inside);
+				mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs",
+					(double)_mission_item.time_inside);
 			}
 		}
 
 		/* check if the MAV was long enough inside the waypoint orbit */
 		if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item.time_inside * 1e6)
 		    || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
-			reset_reached();
 			return true;
 		}
 	}
-
 	return false;
-
 }
 
 void
@@ -1543,62 +1281,6 @@ Navigator::reset_reached()
 
 }
 
-void
-Navigator::on_mission_item_reached()
-{
-	if (myState == NAV_STATE_MISSION) {
-
-		_mission.report_mission_item_reached();
-
-		if (_do_takeoff) {
-			/* takeoff completed */
-			_do_takeoff = false;
-			mavlink_log_info(_mavlink_fd, "#audio: takeoff completed");
-
-		} else {
-			/* advance by one mission item */
-			_mission.move_to_next();
-		}
-
-		if (_mission.current_mission_available()) {
-			set_mission_item();
-
-		} else {
-			/* if no more mission items available then finish mission */
-			/* loiter at last waypoint */
-			_reset_loiter_pos = false;
-			mavlink_log_info(_mavlink_fd, "[navigator] mission completed");
-			request_loiter_or_ready();
-		}
-
-	} else if (myState == NAV_STATE_RTL) {
-		/* RTL completed */
-		if (_rtl_state == RTL_STATE_DESCEND) {
-			/* hovering above home position, land if needed or loiter */
-			mavlink_log_info(_mavlink_fd, "[navigator] RTL completed");
-
-			if (_mission_item.autocontinue) {
-				dispatch(EVENT_LAND_REQUESTED);
-
-			} else {
-				_reset_loiter_pos = false;
-				dispatch(EVENT_LOITER_REQUESTED);
-			}
-
-		} else {
-			/* next RTL step */
-			_rtl_state = (RTLState)(_rtl_state + 1);
-			set_rtl_item();
-		}
-
-	} else if (myState == NAV_STATE_LAND) {
-		/* landing completed */
-		mavlink_log_info(_mavlink_fd, "[navigator] landing completed");
-		dispatch(EVENT_READY_REQUESTED);
-	}
-	_mission.publish_mission_result();
-}
-
 void
 Navigator::publish_position_setpoint_triplet()
 {
@@ -1607,11 +1289,9 @@ Navigator::publish_position_setpoint_triplet()
 
 	/* lazily publish the position setpoint triplet only once available */
 	if (_pos_sp_triplet_pub > 0) {
-		/* publish the position setpoint triplet */
 		orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet);
 
 	} else {
-		/* advertise and publish */
 		_pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet);
 	}
 }
diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
index 9ef359c6dc..49969a3827 100644
--- a/src/modules/navigator/navigator_params.c
+++ b/src/modules/navigator/navigator_params.c
@@ -52,16 +52,6 @@
  * Navigator parameters, accessible via MAVLink
  */
 
-/**
- * Minimum altitude (fixed wing only)
- *
- * Minimum altitude above home for LOITER.
- *
- * @unit meters
- * @group Navigation
- */
-PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f);
-
 /**
  * Waypoint acceptance radius
  *
@@ -101,37 +91,6 @@ PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0);
  */
 PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f);
 
-/**
- * Landing altitude
- *
- * Stay at this altitude above home position after RTL descending. Land (i.e. slowly descend) from this altitude if autolanding allowed.
- *
- * @unit meters
- * @group Navigation
- */
-PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f);
-
-/**
- * Return-To-Launch altitude
- *
- * Minimum altitude above home position for going home in RTL mode.
- *
- * @unit meters
- * @group Navigation
- */
-PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f);
-
-/**
- * Return-To-Launch delay
- *
- * Delay after descend before landing in RTL mode.
- * If set to -1 the system will not land but loiter at NAV_LAND_ALT.
- *
- * @unit seconds
- * @group Navigation
- */
-PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f);
-
 /**
  * Enable parachute deployment
  *
diff --git a/src/modules/navigator/navigator_state.h b/src/modules/navigator/navigator_state.h
deleted file mode 100644
index 6a1475c9b0..0000000000
--- a/src/modules/navigator/navigator_state.h
+++ /dev/null
@@ -1,21 +0,0 @@
-/*
- * navigator_state.h
- *
- *  Created on: 27.01.2014
- *      Author: ton
- */
-
-#ifndef NAVIGATOR_STATE_H_
-#define NAVIGATOR_STATE_H_
-
-typedef enum {
-	NAV_STATE_NONE = 0,
-	NAV_STATE_READY,
-	NAV_STATE_LOITER,
-	NAV_STATE_MISSION,
-	NAV_STATE_RTL,
-	NAV_STATE_LAND,
-	NAV_STATE_MAX
-} nav_state_t;
-
-#endif /* NAVIGATOR_STATE_H_ */
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
new file mode 100644
index 0000000000..6e621e39dd
--- /dev/null
+++ b/src/modules/navigator/rtl.cpp
@@ -0,0 +1,253 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-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 navigator_rtl.cpp
+ * Helper class to access RTL
+ * @author Julian Oes <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <string.h>
+#include <stdlib.h>
+#include <math.h>
+#include <fcntl.h>
+
+#include <mavlink/mavlink_log.h>
+#include <systemlib/err.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/home_position.h>
+
+#include "rtl.h"
+
+
+RTL::RTL() :
+	SuperBlock(NULL, "RTL"),
+	_mavlink_fd(-1),
+	_rtl_state(RTL_STATE_NONE),
+	_home_position({}),
+	_param_return_alt(this, "RETURN_ALT"),
+	_param_descend_alt(this, "DESCEND_ALT"),
+	_param_land_delay(this, "LAND_DELAY"),
+	_loiter_radius(50),
+	_acceptance_radius(50)
+{
+	/* load initial params */
+	updateParams();
+}
+
+RTL::~RTL()
+{
+}
+
+void
+RTL::set_home_position(const home_position_s *new_home_position)
+{
+	memcpy(&_home_position, new_home_position, sizeof(home_position_s));
+}
+
+bool
+RTL::get_current_rtl_item(const vehicle_global_position_s *global_position, mission_item_s *new_mission_item)
+{
+	/* Open mavlink fd */
+	if (_mavlink_fd < 0) {
+		/* try to open the mavlink log device every once in a while */
+		_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+	}
+
+	/* decide if we need climb */
+	if (_rtl_state == RTL_STATE_NONE) {
+		if (global_position->alt < _home_position.alt + _param_return_alt.get()) {
+			_rtl_state = RTL_STATE_CLIMB;
+
+		} else {
+			_rtl_state = RTL_STATE_RETURN;
+		}
+	}
+
+	/* if switching directly to return state, set altitude setpoint to current altitude */
+	if (_rtl_state == RTL_STATE_RETURN) {
+		new_mission_item->altitude_is_relative = false;
+		new_mission_item->altitude = global_position->alt;
+	}
+
+	switch (_rtl_state) {
+	case RTL_STATE_CLIMB: {
+
+		float climb_alt = _home_position.alt + _param_return_alt.get();
+
+		/* TODO understand and fix this */
+		// if (_vstatus.condition_landed) {
+		// 	climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt);
+		// }
+
+		new_mission_item->lat = global_position->lat;
+		new_mission_item->lon = global_position->lon;
+		new_mission_item->altitude_is_relative = false;
+		new_mission_item->altitude = climb_alt;
+		new_mission_item->yaw = NAN;
+		new_mission_item->loiter_radius = _loiter_radius;
+		new_mission_item->loiter_direction = 1;
+		new_mission_item->nav_cmd = NAV_CMD_TAKEOFF;
+		new_mission_item->acceptance_radius = _acceptance_radius;
+		new_mission_item->time_inside = 0.0f;
+		new_mission_item->pitch_min = 0.0f;
+		new_mission_item->autocontinue = true;
+		new_mission_item->origin = ORIGIN_ONBOARD;
+
+		mavlink_log_info(_mavlink_fd, "#audio: RTL: climb to %d meters above home",
+			(int)(climb_alt - _home_position.alt));
+		break;
+	}
+	case RTL_STATE_RETURN: {
+
+		new_mission_item->lat = _home_position.lat;
+		new_mission_item->lon = _home_position.lon;
+
+		/* TODO: add this again */
+		// don't change altitude
+		// if (_pos_sp_triplet.previous.valid) {
+		// 	/* if previous setpoint is valid then use it to calculate heading to home */
+		// 	new_mission_item->yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, new_mission_item->lat, new_mission_item->lon);
+
+		// } else {
+		// 	/* else use current position */
+		// 	new_mission_item->yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, new_mission_item->lat, new_mission_item->lon);
+		// }
+		new_mission_item->loiter_radius = _loiter_radius;
+		new_mission_item->loiter_direction = 1;
+		new_mission_item->nav_cmd = NAV_CMD_WAYPOINT;
+		new_mission_item->acceptance_radius = _acceptance_radius;
+		new_mission_item->time_inside = 0.0f;
+		new_mission_item->pitch_min = 0.0f;
+		new_mission_item->autocontinue = true;
+		new_mission_item->origin = ORIGIN_ONBOARD;
+
+		mavlink_log_info(_mavlink_fd, "#audio: RTL: return at %d meters above home",
+			(int)(new_mission_item->altitude - _home_position.alt));
+		break;
+	}
+
+	case RTL_STATE_DESCEND: {
+
+		new_mission_item->lat = _home_position.lat;
+		new_mission_item->lon = _home_position.lon;
+		new_mission_item->altitude_is_relative = false;
+		new_mission_item->altitude = _home_position.alt + _param_descend_alt.get();
+		new_mission_item->yaw = NAN;
+		new_mission_item->loiter_radius = _loiter_radius;
+		new_mission_item->loiter_direction = 1;
+		new_mission_item->nav_cmd = NAV_CMD_WAYPOINT;
+		new_mission_item->acceptance_radius = _acceptance_radius;
+		new_mission_item->time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
+		new_mission_item->pitch_min = 0.0f;
+		new_mission_item->autocontinue = _param_land_delay.get() > -0.001f;
+		new_mission_item->origin = ORIGIN_ONBOARD;
+
+		mavlink_log_info(_mavlink_fd, "#audio: RTL: descend to %d meters above home",
+			(int)(new_mission_item->altitude - _home_position.alt));
+		break;
+	}
+
+	case RTL_STATE_LAND: {
+
+		new_mission_item->lat = _home_position.lat;
+		new_mission_item->lon = _home_position.lon;
+		new_mission_item->altitude_is_relative = false;
+		new_mission_item->altitude = _home_position.alt;
+		new_mission_item->yaw = NAN;
+		new_mission_item->loiter_radius = _loiter_radius;
+		new_mission_item->loiter_direction = 1;
+		new_mission_item->nav_cmd = NAV_CMD_LAND;
+		new_mission_item->acceptance_radius = _acceptance_radius;
+		new_mission_item->time_inside = 0.0f;
+		new_mission_item->pitch_min = 0.0f;
+		new_mission_item->autocontinue = true;
+		new_mission_item->origin = ORIGIN_ONBOARD;
+
+		mavlink_log_info(_mavlink_fd, "#audio: RTL: land at home");
+		break;
+	}
+
+	case RTL_STATE_FINISHED: {
+		/* nothing to do, report fail */
+		return false;
+	}
+
+	default:
+		return false;	
+	}
+
+	return true;
+}
+
+bool
+RTL::get_next_rtl_item(mission_item_s *new_mission_item)
+{
+	/* TODO implement */
+	return false;
+}
+
+void
+RTL::move_to_next()
+{
+	switch (_rtl_state) {
+	case RTL_STATE_CLIMB:
+		_rtl_state = RTL_STATE_RETURN;
+		break;
+	
+	case RTL_STATE_RETURN:
+		_rtl_state = RTL_STATE_DESCEND;
+		break;
+
+	case RTL_STATE_DESCEND:
+		/* only go to land if autoland is enabled */
+		if (_param_land_delay.get() < 0) {
+			_rtl_state = RTL_STATE_FINISHED;
+		} else {
+			_rtl_state = RTL_STATE_LAND;
+		}
+		break;
+
+	case RTL_STATE_LAND:
+		_rtl_state = RTL_STATE_FINISHED;
+		break;
+
+	case RTL_STATE_FINISHED:
+		break;
+
+	default:
+		break;
+	}
+}
\ No newline at end of file
diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h
new file mode 100644
index 0000000000..c761837fcf
--- /dev/null
+++ b/src/modules/navigator/rtl.h
@@ -0,0 +1,92 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-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 navigator_mission.h
+ * Helper class to access missions
+ * @author Julian Oes <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#ifndef NAVIGATOR_RTL_H
+#define NAVIGATOR_RTL_H
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+#include <uORB/topics/mission.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/vehicle_global_position.h>
+
+class RTL : public control::SuperBlock
+{
+public:
+	/**
+	 * Constructor
+	 */
+	RTL();
+
+	/**
+	 * Destructor
+	 */
+	~RTL();
+
+	void		set_home_position(const home_position_s *home_position);
+
+	bool		get_current_rtl_item(const vehicle_global_position_s *global_position, mission_item_s *new_mission_item);
+	bool		get_next_rtl_item(mission_item_s *mission_item);
+
+	void		move_to_next();
+
+private:
+	int 		_mavlink_fd;
+
+	enum RTLState {
+		RTL_STATE_NONE = 0,
+		RTL_STATE_CLIMB,
+		RTL_STATE_RETURN,
+		RTL_STATE_DESCEND,
+		RTL_STATE_LAND,
+		RTL_STATE_FINISHED,
+	}		_rtl_state;
+
+	home_position_s _home_position;
+	float		_loiter_radius;
+	float		_acceptance_radius;
+
+
+	control::BlockParamFloat _param_return_alt;
+	control::BlockParamFloat _param_descend_alt;
+	control::BlockParamFloat _param_land_delay;
+};
+
+#endif
diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c
new file mode 100644
index 0000000000..7a8b1d745c
--- /dev/null
+++ b/src/modules/navigator/rtl_params.c
@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ *   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 rtl_params.c
+ *
+ * Parameters for RTL
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include <nuttx/config.h>
+
+#include <systemlib/param/param.h>
+
+/*
+ * RTL parameters, accessible via MAVLink
+ */
+
+/**
+ * RTL altitude
+ *
+ * Altitude to fly back in RTL in meters
+ *
+ * @unit meters
+ * @min 0
+ * @max 1
+ * @group RTL
+ */
+PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 100);
+
+
+/**
+ * RTL loiter altitude
+ *
+ * Stay at this altitude above home position after RTL descending.
+ * Land (i.e. slowly descend) from this altitude if autolanding allowed.
+ *
+ * @unit meters
+ * @min 0
+ * @max 100
+ * @group RTL
+ */
+PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20);
+
+/**
+ * RTL delay
+ *
+ * Delay after descend before landing in RTL mode.
+ * If set to -1 the system will not land but loiter at NAV_LAND_ALT.
+ *
+ * @unit seconds
+ * @min -1
+ * @max
+ * @group RTL
+ */
+PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f);
diff --git a/src/modules/systemlib/state_table.h b/src/modules/systemlib/state_table.h
index f2709d29ff..38378651b8 100644
--- a/src/modules/systemlib/state_table.h
+++ b/src/modules/systemlib/state_table.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2013-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
@@ -35,6 +35,7 @@
  * @file state_table.h
  * 
  * Finite-State-Machine helper class for state table
+ * @author: Julian Oes <julian@oes.ch>
  */
 
 #ifndef __SYSTEMLIB_STATE_TABLE_H
@@ -43,7 +44,7 @@
 class StateTable
 {
 public:
-	typedef void (StateTable::*Action)();
+	typedef bool (StateTable::*Action)();
 	struct Tran {
 		Action action;
 		unsigned nextState;
@@ -53,17 +54,23 @@ public:
 	: myTable(table), myNsignals(nSignals), myNstates(nStates) {}
 	
 	#define NO_ACTION &StateTable::doNothing
-	#define ACTION(_target) static_cast<StateTable::Action>(_target)
+	#define ACTION(_target) StateTable::Action(_target)
 
 	virtual ~StateTable() {}
 	
 	void dispatch(unsigned const sig) {
-		register Tran const *t = myTable + myState*myNsignals + sig;
-		(this->*(t->action))();
-
-		myState = t->nextState;
+		/* get transition using state table */
+		Tran const *t = myTable + myState*myNsignals + sig;
+		/* first up change state, this allows to do further dispatchs in the state functions */
+		
+		/* now execute state function, if it runs with success, accept new state */
+		if ((this->*(t->action))()) {
+			myState = t->nextState;
+		}
+	}
+	bool doNothing() {
+		return true;
 	}
-	void doNothing() {}
 protected:
 	unsigned myState;
 private:
diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h
index dfc461ae4e..4532b329de 100644
--- a/src/modules/uORB/topics/mission.h
+++ b/src/modules/uORB/topics/mission.h
@@ -93,6 +93,8 @@ struct mission_item_s {
 	bool autocontinue;		/**< true if next waypoint should follow after this one */
 	enum ORIGIN origin;		/**< where the waypoint has been generated		*/
 	int do_jump_mission_index;	/**< the mission index that we want to jump to		*/
+	int do_jump_repeat_count;	/**< how many times the jump should be repeated		*/
+	int do_jump_current_count;	/**< how many times the jump has already been repeated	*/
 };
 
 struct mission_s
diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h
index 34aaa30ddc..85e8ef8a54 100644
--- a/src/modules/uORB/topics/position_setpoint_triplet.h
+++ b/src/modules/uORB/topics/position_setpoint_triplet.h
@@ -45,7 +45,6 @@
 #include <stdint.h>
 #include <stdbool.h>
 #include "../uORB.h"
-#include <navigator/navigator_state.h>
 
 /**
  * @addtogroup topics
@@ -74,6 +73,17 @@ struct position_setpoint_s
 	float pitch_min;		/**< minimal pitch angle for fixed wing takeoff waypoints */
 };
 
+typedef enum {
+	NAV_STATE_NONE_ON_GROUND = 0,
+	NAV_STATE_NONE_IN_AIR,
+	NAV_STATE_AUTO_ON_GROUND,
+	NAV_STATE_LOITER,
+	NAV_STATE_MISSION,
+	NAV_STATE_RTL,
+	NAV_STATE_LAND,
+	NAV_STATE_MAX
+} nav_state_t;
+
 /**
  * Global position setpoint triplet in WGS84 coordinates.
  *
diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h
index adcd028f06..cfab695f85 100644
--- a/src/modules/uORB/topics/vehicle_global_position.h
+++ b/src/modules/uORB/topics/vehicle_global_position.h
@@ -36,7 +36,7 @@
  * Definition of the global fused WGS84 position uORB topic.
  *
  * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
+ * @author Julian Oes <julian@oes.ch>
  * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
@@ -66,16 +66,16 @@ struct vehicle_global_position_s {
 	bool global_valid;		/**< true if position satisfies validity criteria of estimator */
 	bool baro_valid;		/**< true if baro_alt is valid (vel_d is also valid in this case) */
 
-	uint64_t time_gps_usec; /**< GPS timestamp in microseconds					   */
+	uint64_t time_gps_usec; 	/**< GPS timestamp in microseconds					   */
 	double lat;			/**< Latitude in degrees							 	   */
 	double lon;			/**< Longitude in degrees							 	   */
 	float alt;			/**< Altitude AMSL in meters						 	   */
-	float vel_n; 		/**< Ground north velocity, m/s				 			   */
-	float vel_e;		/**< Ground east velocity, m/s							   */
-	float vel_d;		/**< Ground downside velocity, m/s						   */
+	float vel_n; 			/**< Ground north velocity, m/s				 			   */
+	float vel_e;			/**< Ground east velocity, m/s							   */
+	float vel_d;			/**< Ground downside velocity, m/s						   */
 	float yaw; 			/**< Yaw in radians -PI..+PI.							   */
 
-	float baro_alt;		/**< Barometric altitude (not raw baro but fused with accelerometer) */
+	float baro_alt;			/**< Barometric altitude (not raw baro but fused with accelerometer) */
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 48af4c9e2d..c1bd828d8b 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -54,8 +54,6 @@
 #include <stdbool.h>
 #include "../uORB.h"
 
-#include <navigator/navigator_state.h>
-
 /**
  * @addtogroup topics @{
  */
@@ -93,6 +91,14 @@ typedef enum {
 	FAILSAFE_STATE_MAX
 } failsafe_state_t;
 
+typedef enum {
+	NAVIGATION_STATE_NONE = 0,
+	NAVIGATION_STATE_MISSION,
+	NAVIGATION_STATE_LOITER,
+	NAVIGATION_STATE_RTL,
+	NAVIGATION_STATE_LAND
+} navigation_state_t;
+
 enum VEHICLE_MODE_FLAG {
 	VEHICLE_MODE_FLAG_SAFETY_ARMED = 128,
 	VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64,
@@ -152,8 +158,7 @@ struct vehicle_status_s {
 	uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
 
 	main_state_t main_state;				/**< main state machine */
-	unsigned int set_nav_state;	/**< set navigation state machine to specified value */
-	uint64_t set_nav_state_timestamp;	/**< timestamp of latest change of set_nav_state */
+	navigation_state_t set_nav_state;		/**< set navigation state machine to specified value */
 	arming_state_t arming_state;			/**< current arming state */
 	hil_state_t hil_state;					/**< current hil state */
 	failsafe_state_t failsafe_state;		/**< current failsafe state */

From d41a01483a9a1e61c12492501bf975021595b3a6 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 21 Apr 2014 22:48:12 +0200
Subject: [PATCH 41/97] fw_att_pos_estimator: lines were commented out by
 mistake

---
 .../fw_att_pos_estimator/fw_att_pos_estimator_main.cpp        | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
index a61fff9427..32a6029fe0 100644
--- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
+++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
@@ -1035,8 +1035,8 @@ FixedwingEstimator::task_main()
 				_local_pos.z_global = false;
 				_local_pos.yaw = _att.yaw;
 
-				// _velocity_xy_filtered = 0.9f*_velocity_xy_filtered + 0.1f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy);
-				// _velocity_z_filtered = 0.9f*_velocity_z_filtered + 0.1f*fabsf(_local_pos.vz);
+				 _velocity_xy_filtered = 0.9f*_velocity_xy_filtered + 0.1f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy);
+				 _velocity_z_filtered = 0.9f*_velocity_z_filtered + 0.1f*fabsf(_local_pos.vz);
 
 
 				/* crude land detector for fixedwing only,

From c0d4ecf2b629a90222cf26d904c7b1260b1147c4 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sat, 26 Apr 2014 11:42:46 +0200
Subject: [PATCH 42/97] navigator: some warnings and cleanup

---
 src/modules/navigator/navigator_main.cpp | 36 ++++++++++++------------
 1 file changed, 18 insertions(+), 18 deletions(-)

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 34a28aec31..f39dc88e31 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -159,8 +159,9 @@ private:
 	vehicle_control_mode_s				_control_mode;		/**< vehicle control mode */
 	vehicle_global_position_s			_global_pos;		/**< global vehicle position */
 	home_position_s					_home_pos;		/**< home position for RTL */
-	position_setpoint_triplet_s			_pos_sp_triplet;	/**< triplet of position setpoints */
 	mission_item_s 					_mission_item;		/**< current mission item */
+	navigation_capabilities_s			_nav_caps;
+	position_setpoint_triplet_s			_pos_sp_triplet;	/**< triplet of position setpoints */
 
 	bool 						_mission_item_valid;
 
@@ -172,7 +173,6 @@ private:
 	bool						_fence_valid;		/**< flag if fence is valid */
 	bool						_inside_fence;		/**< vehicle is inside fence */
 
-	struct navigation_capabilities_s		_nav_caps;
 
 	Mission						_mission;
 
@@ -307,11 +307,6 @@ private:
 	 */
 	bool		check_mission_item_reached();
 
-	/**
-	 * Perform actions when current mission item reached.
-	 */
-	// void		on_mission_item_reached();
-
 	/**
 	 * Move to next waypoint
 	 */
@@ -352,7 +347,7 @@ Navigator	*g_navigator;
 
 Navigator::Navigator() :
 
-/* state machine transition table */
+	/* state machine transition table */
 	StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT),
 	_task_should_exit(false),
 	_navigator_task(-1),
@@ -366,18 +361,27 @@ Navigator::Navigator() :
 	_capabilities_sub(-1),
 	_control_mode_sub(-1),
 	_pos_sp_triplet_pub(-1),
+	_vstatus({}),
+	_control_mode({}),
+	_global_pos({}),
+	_home_pos({}),
+	_mission_item({}),
+	_nav_caps({}),
+	_pos_sp_triplet({}),
+	_mission_item_valid(false),
 	_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
+	_geofence({}),
 	_geofence_violation_warning_sent(false),
 	_fence_valid(false),
 	_inside_fence(true),
-	_mission(),
-	_pos_sp_triplet({}),
-	_mission_item({}),
-	_mission_item_valid(false),
+	_mission({}),
+	_rtl({}),
 	_waypoint_position_reached(false),
 	_waypoint_yaw_reached(false),
 	_time_first_inside_orbit(0),
-	_update_triplet(false)
+	_update_triplet(false),
+	_parameters({}),
+	_parameter_handles({})
 {
 	_parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD");
 	_parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD");
@@ -530,12 +534,9 @@ Navigator::task_main()
 {
 	/* inform about start */
 	warnx("Initializing..");
-	fflush(stdout);
 
 	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 
-	mavlink_log_info(_mavlink_fd, "[navigator] started");
-
 	/* Try to load the geofence:
 	 * if /fs/microsd/etc/geofence.txt load from this file
 	 * else clear geofence data in datamanager
@@ -578,7 +579,6 @@ Navigator::task_main()
 	/* rate limit position updates to 50 Hz */
 	orb_set_interval(_global_pos_sub, 20);
 
-	unsigned prevState = NAV_STATE_NONE_ON_GROUND;
 	hrt_abstime mavlink_open_time = 0;
 	const hrt_abstime mavlink_open_interval = 500000;
 
@@ -811,7 +811,7 @@ Navigator::status()
 		break;
 
 	case NAV_STATE_LAND:
-		warnx("State: LAND");
+		warnx("State: Land");
 		break;
 
 	default:

From 9f857f86e44a6385daa383d9b02173ad3c36fa01 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sat, 26 Apr 2014 12:24:52 +0200
Subject: [PATCH 43/97] navigator: corrected the RTL waypoint types

---
 src/modules/navigator/rtl.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index 6e621e39dd..b8ea062758 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -119,7 +119,7 @@ RTL::get_current_rtl_item(const vehicle_global_position_s *global_position, miss
 		new_mission_item->yaw = NAN;
 		new_mission_item->loiter_radius = _loiter_radius;
 		new_mission_item->loiter_direction = 1;
-		new_mission_item->nav_cmd = NAV_CMD_TAKEOFF;
+		new_mission_item->nav_cmd = NAV_CMD_WAYPOINT;
 		new_mission_item->acceptance_radius = _acceptance_radius;
 		new_mission_item->time_inside = 0.0f;
 		new_mission_item->pitch_min = 0.0f;
@@ -168,7 +168,7 @@ RTL::get_current_rtl_item(const vehicle_global_position_s *global_position, miss
 		new_mission_item->yaw = NAN;
 		new_mission_item->loiter_radius = _loiter_radius;
 		new_mission_item->loiter_direction = 1;
-		new_mission_item->nav_cmd = NAV_CMD_WAYPOINT;
+		new_mission_item->nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
 		new_mission_item->acceptance_radius = _acceptance_radius;
 		new_mission_item->time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
 		new_mission_item->pitch_min = 0.0f;
@@ -250,4 +250,4 @@ RTL::move_to_next()
 	default:
 		break;
 	}
-}
\ No newline at end of file
+}

From ea0142810a734bd8cade70668eefa562a495ec4a Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sat, 26 Apr 2014 12:55:27 +0200
Subject: [PATCH 44/97] navigator: comments and whitespace

---
 src/modules/navigator/navigator_main.cpp | 94 ++++++++----------------
 1 file changed, 32 insertions(+), 62 deletions(-)

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index f39dc88e31..c3fc4e939b 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -142,7 +142,7 @@ private:
 	bool		_task_should_exit;		/**< if true, sensor task should exit */
 	int		_navigator_task;		/**< task handle for sensor task */
 
-	int		_mavlink_fd;
+	int		_mavlink_fd;			/**< the file descriptor to send messages over mavlink */
 
 	int		_global_pos_sub;		/**< global position subscription */
 	int		_home_pos_sub;			/**< home position subscription */
@@ -160,30 +160,31 @@ private:
 	vehicle_global_position_s			_global_pos;		/**< global vehicle position */
 	home_position_s					_home_pos;		/**< home position for RTL */
 	mission_item_s 					_mission_item;		/**< current mission item */
-	navigation_capabilities_s			_nav_caps;
+	navigation_capabilities_s			_nav_caps;		/**< navigation capabilities */
 	position_setpoint_triplet_s			_pos_sp_triplet;	/**< triplet of position setpoints */
 
-	bool 						_mission_item_valid;
+	bool 		_mission_item_valid;		/**< flags if the current mission item is valid */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
-	Geofence					_geofence;
-	bool						_geofence_violation_warning_sent;
+	Geofence	_geofence;			/**< class that handles the geofence */
+	bool		_geofence_violation_warning_sent; /**< prevents spaming to mavlink */
 
-	bool						_fence_valid;		/**< flag if fence is valid */
-	bool						_inside_fence;		/**< vehicle is inside fence */
+	bool		_fence_valid;			/**< flag if fence is valid */
+	bool		_inside_fence;			/**< vehicle is inside fence */
 
 
-	Mission						_mission;
+	Mission		_mission;			/**< class that handles the missions */
 
-	RTL 						_rtl;
+	RTL 		_rtl;				/**< class that handles RTL */
 
-	bool		_waypoint_position_reached;
-	bool		_waypoint_yaw_reached;
-	uint64_t	_time_first_inside_orbit;
-	MissionFeasibilityChecker missionFeasiblityChecker;
+	bool		_waypoint_position_reached;	/**< flags if the position of the mission item has been reached */
+	bool		_waypoint_yaw_reached;		/**< flags if the yaw position of the mission item has been reached */
+	uint64_t	_time_first_inside_orbit;	/**< the time when we were first in the acceptance radius of the mission item */
 
-	bool		_update_triplet;
+	MissionFeasibilityChecker			missionFeasiblityChecker; /**< class that checks if a mission is feasible */
+
+	bool		_update_triplet;		/**< flags if position setpoint triplet needs to be published */
 
 	struct {
 		float acceptance_radius;
@@ -210,7 +211,7 @@ private:
 		EVENT_HOME_POSITION_CHANGED,
 		EVENT_MISSION_ITEM_REACHED,
 		MAX_EVENT
-	};
+	};						/**< possible events that can be thrown at state machine */
 
 	/**
 	 * State machine transition table
@@ -267,8 +268,6 @@ private:
 	 */
 	void		task_main();
 
-	void		publish_safepoints(unsigned points);
-
 	/**
 	 * Functions that are triggered when a new state is entered.
 	 */
@@ -283,22 +282,7 @@ private:
 	bool		start_land();
 
 	/**
-	 * Fork for state transitions
-	 */
-	// void		request_loiter_or_ready();
-
-	/**
-	 * Guards offboard mission
-	 */
-	bool		offboard_mission_available(unsigned relative_index);
-
-	/**
-	 * Guards onboard mission
-	 */
-	bool		onboard_mission_available(unsigned relative_index);
-
-	/**
-	 * Reset all "reached" flags.
+	 * Reset all "mission item reached" flags.
 	 */
 	void		reset_reached();
 
@@ -308,27 +292,24 @@ private:
 	bool		check_mission_item_reached();
 
 	/**
-	 * Move to next waypoint
+	 * Set mission items by accessing the mission class.
+	 * If failing, tell the state machine to loiter.
 	 */
 	bool		set_mission_items();
 
 	/**
-	 * Switch to next RTL state
+	 * Set a RTL item by accessing the RTL class.
+	 * If failing, tell the state machine to loiter.
 	 */
 	void		set_rtl_item();
 
 	/**
-	 * Set position setpoint for mission item
+	 * Translate mission item to a position setpoint.
 	 */
 	void		mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp);
 
 	/**
-	 * Helper function to get a takeoff item
-	 */
-	void		get_takeoff_setpoint(position_setpoint_s *pos_sp);
-
-	/**
-	 * Publish a new mission item triplet for position controller
+	 * Publish a new position setpoint triplet for position controllers
 	 */
 	void		publish_position_setpoint_triplet();
 };
@@ -608,13 +589,12 @@ Navigator::task_main()
 		/* wait for up to 100ms for data */
 		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
 
-		/* timed out - periodic check for _task_should_exit, etc. */
 		if (pret == 0) {
+			/* timed out - periodic check for _task_should_exit, etc. */
 			continue;
-		}
 
-		/* this is undesirable but not much we can do - might want to flag unhappy status */
-		if (pret < 0) {
+		} else if (pret < 0) {
+			/* this is undesirable but not much we can do - might want to flag unhappy status */
 			warn("poll error %d, %d", pret, errno);
 			continue;
 		}
@@ -651,16 +631,11 @@ Navigator::task_main()
 				break;
 
 			case NAVIGATION_STATE_RTL:
-				/* TODO: what is this for? */
-				// if (!(_rtl_state == RTL_STATE_DESCEND &&
-				// 	  (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
-				// 	_vstatus.condition_home_position_valid) {
 				dispatch(EVENT_RTL_REQUESTED);
-				// }
 				break;
 
 			case NAVIGATION_STATE_LAND:
-				/* TODO: add this */
+				/* TODO: add and test this */
 				// dispatch(EVENT_LAND_REQUESTED);
 				break;
 
@@ -781,6 +756,7 @@ Navigator::status()
 
 	if (_fence_valid) {
 		warnx("Geofence is valid");
+		/* TODO: needed? */
 //		warnx("Vertex longitude latitude");
 //		for (unsigned i = 0; i < _fence.count; i++)
 //		warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat);
@@ -944,12 +920,6 @@ Navigator::start_auto_on_ground()
 
 	_pos_sp_triplet.current.type = SETPOINT_TYPE_IDLE;
 
-
-	// if (_rtl_state != RTL_STATE_DESCEND) {
-	// 	 reset RTL state if landed not at home 
-	// 	_rtl_state = RTL_STATE_NONE;
-	// }
-
 	_update_triplet = true;
 	return true;
 }
@@ -1057,7 +1027,6 @@ Navigator::set_mission_items()
 bool
 Navigator::start_rtl()
 {
-	/* TODO check if RTL was successfull, if not we have a problem and need to dispatch something */
 	if (_rtl.get_current_rtl_item(&_global_pos, &_mission_item)) {
 
 		_mission_item_valid = true;
@@ -1070,8 +1039,9 @@ Navigator::start_rtl()
 		_update_triplet = true;
 		return true;
 	}
-	dispatch(EVENT_LOITER_REQUESTED);
 
+	/* if RTL doesn't work, fallback to loiter */
+	dispatch(EVENT_LOITER_REQUESTED);
 	return false;
 }
 
@@ -1189,7 +1159,7 @@ Navigator::check_mission_item_reached()
 		return _vstatus.condition_landed;
 	}
 
-	/* TODO count turns */
+	/* TODO: count turns */
 	// if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
 	//      _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
 	//      _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
@@ -1242,7 +1212,7 @@ Navigator::check_mission_item_reached()
 			/* check yaw if defined only for rotary wing except takeoff */
 			float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw);
 
-			if (fabsf(yaw_err) < 0.2f) { /* XXX get rid of magic number */
+			if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */
 				_waypoint_yaw_reached = true;
 			}
 

From 13dfe0447ccfa4f75b551d02b5c979a6ade4c81a Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 26 Apr 2014 16:07:29 +0200
Subject: [PATCH 45/97] Send current local position estimate as well

---
 src/modules/commander/commander.cpp     | 24 ++++++++++++++++--------
 src/modules/uORB/topics/home_position.h |  5 ++++-
 2 files changed, 20 insertions(+), 9 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 3527117341..dd2614c1d6 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -927,6 +927,14 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
 		}
 
+		/* update local position estimate */
+		orb_check(local_position_sub, &updated);
+
+		if (updated) {
+			/* position changed */
+			orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
+		}
+
 		/* update condition_global_position_valid */
 		/* hysteresis for EPH/EPV */
 		bool eph_epv_good;
@@ -952,6 +960,10 @@ int commander_thread_main(int argc, char *argv[])
 			home.lon = global_position.lon;
 			home.alt = global_position.alt;
 
+			home.x = local_position.x;
+			home.y = local_position.y;
+			home.z = local_position.z;
+
 			warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
 			mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
 
@@ -968,14 +980,6 @@ int commander_thread_main(int argc, char *argv[])
 			tune_positive(true);
 		}
 
-		/* update local position estimate */
-		orb_check(local_position_sub, &updated);
-
-		if (updated) {
-			/* position changed */
-			orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
-		}
-
 		/* update condition_local_position_valid and condition_local_altitude_valid */
 		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && eph_epv_good, &(status.condition_local_position_valid), &status_changed);
 		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
@@ -1338,6 +1342,10 @@ int commander_thread_main(int argc, char *argv[])
 				home.lon = global_position.lon;
 				home.alt = global_position.alt;
 
+				home.x = local_position.x;
+				home.y = local_position.y;
+				home.z = local_position.z;
+
 				warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
 				mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
 
diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h
index 08d11abaea..70071130d6 100644
--- a/src/modules/uORB/topics/home_position.h
+++ b/src/modules/uORB/topics/home_position.h
@@ -59,10 +59,13 @@ struct home_position_s
 {
 	uint64_t timestamp;			/**< Timestamp (microseconds since system boot)	*/
 
-	//bool altitude_is_relative;	// TODO what means home relative altitude? we need clear definition of reference altitude then
 	double lat;				/**< Latitude in degrees 			*/
 	double lon;				/**< Longitude in degrees			*/
 	float alt;				/**< Altitude in meters				*/
+
+	float x;				/**< X coordinate in meters			*/
+	float y;				/**< Y coordinate in meters			*/
+	float z;				/**< Z coordinate in meters			*/
 };
 
 /**

From e882824ee15e0c5fff58c7f223ec7be181c7af8f Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sat, 26 Apr 2014 23:31:15 +0200
Subject: [PATCH 46/97] eph and epv renaming, make this compile again

---
 src/drivers/gps/gps.cpp                       |  6 +++---
 src/drivers/gps/mtk.cpp                       |  4 ++--
 src/drivers/gps/ubx.cpp                       |  4 ++--
 .../attitude_estimator_ekf_main.cpp           |  4 ++--
 .../fw_att_pos_estimator_main.cpp             |  5 ++---
 src/modules/mavlink/mavlink_messages.cpp      |  4 ++--
 src/modules/mavlink/mavlink_receiver.cpp      |  6 +++---
 src/modules/navigator/navigator_main.cpp      | 19 ++++++++++---------
 .../position_estimator_inav_main.c            | 12 ++++++------
 src/modules/sdlog2/sdlog2.c                   |  4 ++--
 .../uORB/topics/vehicle_gps_position.h        |  4 ++--
 11 files changed, 36 insertions(+), 36 deletions(-)

diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index a902bdf2f5..f05f4a4092 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -282,8 +282,8 @@ GPS::task_main()
 			_report.p_variance_m = 10.0f;
 			_report.c_variance_rad = 0.1f;
 			_report.fix_type = 3;
-			_report.eph_m = 3.0f;
-			_report.epv_m = 7.0f;
+			_report.eph = 3.0f;
+			_report.epv = 7.0f;
 			_report.timestamp_velocity = hrt_absolute_time();
 			_report.vel_n_m_s = 0.0f;
 			_report.vel_e_m_s = 0.0f;
@@ -446,7 +446,7 @@ GPS::print_info()
 		warnx("position lock: %dD, satellites: %d, last update: %fms ago", (int)_report.fix_type,
 				_report.satellites_visible, (hrt_absolute_time() - _report.timestamp_position) / 1000.0f);
 		warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
-		warnx("eph: %.2fm, epv: %.2fm", _report.eph_m, _report.epv_m);
+		warnx("eph: %.2fm, epv: %.2fm", _report.eph, _report.epv);
 		warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate());
 		warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate());
 		warnx("rate publication:\t%6.2f Hz", (double)_rate);
diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp
index c90ecbe281..99f88fb8aa 100644
--- a/src/drivers/gps/mtk.cpp
+++ b/src/drivers/gps/mtk.cpp
@@ -253,8 +253,8 @@ MTK::handle_message(gps_mtk_packet_t &packet)
 
 	_gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm
 	_gps_position->fix_type = packet.fix_type;
-	_gps_position->eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit
-	_gps_position->epv_m = 0.0; //unknown in mtk custom mode
+	_gps_position->eph = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit
+	_gps_position->epv = 0.0; //unknown in mtk custom mode
 	_gps_position->vel_m_s = ((float)packet.ground_speed) * 1e-2f; // from cm/s to m/s
 	_gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
 	_gps_position->satellites_visible = packet.satellites;
diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp
index 8a2afecb7e..95965b60d6 100644
--- a/src/drivers/gps/ubx.cpp
+++ b/src/drivers/gps/ubx.cpp
@@ -425,8 +425,8 @@ UBX::handle_message()
 					_gps_position->lat = packet->lat;
 					_gps_position->lon = packet->lon;
 					_gps_position->alt = packet->height_msl;
-					_gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m
-					_gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m
+					_gps_position->eph = (float)packet->hAcc * 1e-3f; // from mm to m
+					_gps_position->epv = (float)packet->vAcc * 1e-3f; // from mm to m
 					_gps_position->timestamp_position = hrt_absolute_time();
 
 					_rate_count_lat_lon++;
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index c61b6ff3fe..ec679f1ae2 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -398,7 +398,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
 					hrt_abstime vel_t = 0;
 					bool vel_valid = false;
-					if (ekf_params.acc_comp == 1 && gps.fix_type >= 3 && gps.eph_m < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) {
+					if (ekf_params.acc_comp == 1 && gps.fix_type >= 3 && gps.eph < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) {
 						vel_valid = true;
 						if (gps_updated) {
 							vel_t = gps.timestamp_velocity;
@@ -407,7 +407,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 							vel(2) = gps.vel_d_m_s;
 						}
 
-					} else if (ekf_params.acc_comp == 2 && gps.eph_m < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) {
+					} else if (ekf_params.acc_comp == 2 && gps.eph < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) {
 						vel_valid = true;
 						if (global_pos_updated) {
 							vel_t = global_pos.timestamp;
diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
index e5435e843f..f47531b26e 100644
--- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
@@ -1247,7 +1247,6 @@ FixedwingEstimator::task_main()
 
 				/* local pos alt is negative, change sign and add alt offset */
 				_global_pos.alt = _local_pos.ref_alt + (-_local_pos.z);
-				_global_pos.rel_alt = (-_local_pos.z);
 
 				if (_local_pos.v_z_valid) {
 					_global_pos.vel_d = _local_pos.vz;
@@ -1255,8 +1254,8 @@ FixedwingEstimator::task_main()
 
 				_global_pos.yaw = _local_pos.yaw;
 
-				_global_pos.eph = _gps.eph_m;
-				_global_pos.epv = _gps.epv_m;
+				_global_pos.eph = _gps.eph;
+				_global_pos.epv = _gps.epv;
 
 				_global_pos.timestamp = _local_pos.timestamp;
 
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 27b1af046d..67ded1230c 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -541,8 +541,8 @@ protected:
 						     gps->lat,
 						     gps->lon,
 						     gps->alt,
-						     cm_uint16_from_m_float(gps->eph_m),
-						     cm_uint16_from_m_float(gps->epv_m),
+						     cm_uint16_from_m_float(gps->eph),
+						     cm_uint16_from_m_float(gps->epv),
 						     gps->vel_m_s * 100.0f,
 						     _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f,
 						     gps->satellites_visible);
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 33a4fef126..95314d56f8 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -661,12 +661,12 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
 	hil_gps.lat = gps.lat;
 	hil_gps.lon = gps.lon;
 	hil_gps.alt = gps.alt;
-	hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m
-	hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m
+	hil_gps.eph = (float)gps.eph * 1e-2f; // from cm to m
+	hil_gps.epv = (float)gps.epv * 1e-2f; // from cm to m
 
 	hil_gps.timestamp_variance = timestamp;
 	hil_gps.s_variance_m_s = 5.0f;
-	hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m;
+	hil_gps.p_variance_m = hil_gps.eph * hil_gps.eph;
 
 	hil_gps.timestamp_velocity = timestamp;
 	hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index c3fc4e939b..d45446e5fa 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -743,16 +743,17 @@ Navigator::start()
 void
 Navigator::status()
 {
-	warnx("Global position is %svalid", _global_pos.global_valid ? "" : "in");
+	/* TODO: add this again */
+	// warnx("Global position is %svalid", _global_pos_valid ? "" : "in");
 
-	if (_global_pos.global_valid) {
-		warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat);
-		warnx("Altitude %5.5f meters, altitude above home %5.5f meters",
-		      (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt));
-		warnx("Ground velocity in m/s, N %5.5f, E %5.5f, D %5.5f",
-		      (double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d);
-		warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F));
-	}
+	// if (_global_pos.global_valid) {
+	// 	warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat);
+	// 	warnx("Altitude %5.5f meters, altitude above home %5.5f meters",
+	// 	      (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt));
+	// 	warnx("Ground velocity in m/s, N %5.5f, E %5.5f, D %5.5f",
+	// 	      (double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d);
+	// 	warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F));
+	// }
 
 	if (_fence_valid) {
 		warnx("Geofence is valid");
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 368424853c..54c8a7d176 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -592,13 +592,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 				/* hysteresis for GPS quality */
 				if (gps_valid) {
-					if (gps.eph_m > max_eph_epv * 1.5f || gps.epv_m > max_eph_epv * 1.5f || gps.fix_type < 3) {
+					if (gps.eph > max_eph_epv * 1.5f || gps.epv > max_eph_epv * 1.5f || gps.fix_type < 3) {
 						gps_valid = false;
 						mavlink_log_info(mavlink_fd, "[inav] GPS signal lost");
 					}
 
 				} else {
-					if (gps.eph_m < max_eph_epv && gps.epv_m < max_eph_epv && gps.fix_type >= 3) {
+					if (gps.eph < max_eph_epv && gps.epv < max_eph_epv && gps.fix_type >= 3) {
 						gps_valid = true;
 						reset_est = true;
 						mavlink_log_info(mavlink_fd, "[inav] GPS signal found");
@@ -673,8 +673,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 							corr_gps[2][1] = 0.0f;
 						}
 
-						w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph_m);
-						w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv_m);
+						w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph);
+						w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv);
 					}
 
 				} else {
@@ -951,8 +951,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				global_pos.yaw = local_pos.yaw;
 
 				// TODO implement dead-reckoning
-				global_pos.eph = gps.eph_m;
-				global_pos.epv = gps.epv_m;
+				global_pos.eph = gps.eph;
+				global_pos.epv = gps.epv;
 
 				if (vehicle_global_position_pub < 0) {
 					vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index b74d4183bd..5d49cc4c99 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -972,8 +972,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.msg_type = LOG_GPS_MSG;
 			log_msg.body.log_GPS.gps_time = buf_gps_pos.time_gps_usec;
 			log_msg.body.log_GPS.fix_type = buf_gps_pos.fix_type;
-			log_msg.body.log_GPS.eph = buf_gps_pos.eph_m;
-			log_msg.body.log_GPS.epv = buf_gps_pos.epv_m;
+			log_msg.body.log_GPS.eph = buf_gps_pos.eph;
+			log_msg.body.log_GPS.epv = buf_gps_pos.epv;
 			log_msg.body.log_GPS.lat = buf_gps_pos.lat;
 			log_msg.body.log_GPS.lon = buf_gps_pos.lon;
 			log_msg.body.log_GPS.alt = buf_gps_pos.alt * 0.001f;
diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h
index 794c3f8bc7..a758102783 100644
--- a/src/modules/uORB/topics/vehicle_gps_position.h
+++ b/src/modules/uORB/topics/vehicle_gps_position.h
@@ -65,8 +65,8 @@ struct vehicle_gps_position_s {
 	float c_variance_rad;				/**< course accuracy estimate rad */
 	uint8_t fix_type; 				/**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.   */
 
-	float eph_m;					/**< GPS HDOP horizontal dilution of position in m */
-	float epv_m;					/**< GPS VDOP horizontal dilution of position in m */
+	float eph;					/**< GPS HDOP horizontal dilution of position in m */
+	float epv;					/**< GPS VDOP horizontal dilution of position in m */
 
 	uint64_t timestamp_velocity;			/**< Timestamp for velocity informations */
 	float vel_m_s;					/**< GPS ground speed (m/s) */

From a6f71f10d0664754e0b3c8ad2161dc81befb2ca9 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sun, 27 Apr 2014 02:24:33 +0200
Subject: [PATCH 47/97] fw_att_pos_estimator: some tuning for the land/in-air
 detector

---
 .../fw_att_pos_estimator_main.cpp                 | 15 +++++++++------
 1 file changed, 9 insertions(+), 6 deletions(-)

diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
index f47531b26e..fcbd90405a 100644
--- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
@@ -240,6 +240,7 @@ private:
 
 	float						_velocity_xy_filtered;
 	float						_velocity_z_filtered;
+	float						_airspeed_filtered; 						
 
 	/**
 	 * Update our local parameter cache.
@@ -343,7 +344,8 @@ FixedwingEstimator::FixedwingEstimator() :
 	_mavlink_fd(-1),
 	_ekf(nullptr),
 	_velocity_xy_filtered(0.0f),
-	_velocity_z_filtered(0.0f)
+	_velocity_z_filtered(0.0f),
+	_airspeed_filtered(0.0f)
 {
 
 	last_run = hrt_absolute_time();
@@ -1200,16 +1202,17 @@ FixedwingEstimator::task_main()
 				_local_pos.z_global = false;
 				_local_pos.yaw = _att.yaw;
 
-				 _velocity_xy_filtered = 0.9f*_velocity_xy_filtered + 0.1f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy);
-				 _velocity_z_filtered = 0.9f*_velocity_z_filtered + 0.1f*fabsf(_local_pos.vz);
+				 _velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy);
+				 _velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_local_pos.vz);
+				 _airspeed_filtered = 0.95*_airspeed_filtered + + 0.05*_airspeed.true_airspeed_m_s;
 
 
 				/* crude land detector for fixedwing only,
 				 * TODO: adapt so that it works for both, maybe move to another location
 				 */
-				if (_velocity_xy_filtered < 2
-					&& _velocity_z_filtered < 2
-					&& _airspeed.true_airspeed_m_s < 10) {
+				if (_velocity_xy_filtered < 5
+					&& _velocity_z_filtered < 10
+					&& _airspeed_filtered < 10) {
 					_local_pos.landed = true;
 				} else {
 					_local_pos.landed = false;

From 721c90291c12405b4f4a6cf5ddc9ca8cec9f0077 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sun, 27 Apr 2014 02:26:09 +0200
Subject: [PATCH 48/97] commander: some HIL commands and land detector cleanup

---
 src/modules/commander/commander.cpp | 142 ++++++++++++----------------
 1 file changed, 59 insertions(+), 83 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 6f86e0c2f0..13336736d7 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2013-2014 PX4 Development Team. All rights reserved.
  *   Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
  *           Lorenz Meier <lm@inf.ethz.ch>
  *           Thomas Gubler <thomasgubler@student.ethz.ch>
@@ -383,7 +383,6 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 {
 	/* result of the command */
 	enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED;
-	bool ret = false;
 
 	/* only handle commands that are meant to be handled by this system and component */
 	if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components
@@ -395,89 +394,73 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 	/* request to set different system mode */
 	switch (cmd->command) {
 	case VEHICLE_CMD_DO_SET_MODE: {
-			uint8_t base_mode = (uint8_t) cmd->param1;
-			uint8_t custom_main_mode = (uint8_t) cmd->param2;
-			transition_result_t arming_res = TRANSITION_NOT_CHANGED;
+		uint8_t base_mode = (uint8_t) cmd->param1;
+		uint8_t custom_main_mode = (uint8_t) cmd->param2;
 
-			/* set HIL state */
-			hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
-			int hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd);
+		/* set HIL state */
+		hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
+		int hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd);
 
-			/* if HIL got enabled, reset battery status state */
-			if (hil_ret == OK && status->hil_state == HIL_STATE_ON) {
-				/* reset the arming mode to disarmed */
-				arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed);
+		/* if HIL got enabled, reset battery status state */
+		if (hil_ret == OK && status->hil_state == HIL_STATE_ON) {
+			/* reset the arming mode to disarmed */
+			arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed);
+		}
 
-				if (arming_res != TRANSITION_DENIED) {
-					mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby");
+		// Transition the arming state
+		transition_result_t arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
 
-				} else {
-					mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state");
-				}
+		/* set main state */
+		transition_result_t main_res = TRANSITION_DENIED;
+
+		if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
+			/* use autopilot-specific mode */
+			if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
+				/* MANUAL */
+				main_res = main_state_transition(status, MAIN_STATE_MANUAL);
+
+			} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) {
+				/* SEATBELT */
+				main_res = main_state_transition(status, MAIN_STATE_SEATBELT);
+
+			} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) {
+				/* EASY */
+				main_res = main_state_transition(status, MAIN_STATE_EASY);
+
+			} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
+				/* AUTO */
+				main_res = main_state_transition(status, MAIN_STATE_AUTO);
 			}
 
-			if (hil_ret == OK)
-				ret = true;
+		} else {
+			/* use base mode */
+			if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
+				/* AUTO */
+				main_res = main_state_transition(status, MAIN_STATE_AUTO);
 
-            // Transition the arming state
-            arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
-
-			if (arming_res == TRANSITION_CHANGED)
-				ret = true;
-
-			/* set main state */
-			transition_result_t main_res = TRANSITION_DENIED;
-
-			if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
-				/* use autopilot-specific mode */
-				if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
-					/* MANUAL */
-					main_res = main_state_transition(status, MAIN_STATE_MANUAL);
-
-				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) {
-					/* SEATBELT */
-					main_res = main_state_transition(status, MAIN_STATE_SEATBELT);
-
-				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) {
+			} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
+				if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
 					/* EASY */
 					main_res = main_state_transition(status, MAIN_STATE_EASY);
 
-				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
-					/* AUTO */
-					main_res = main_state_transition(status, MAIN_STATE_AUTO);
-				}
-
-			} else {
-				/* use base mode */
-				if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
-					/* AUTO */
-					main_res = main_state_transition(status, MAIN_STATE_AUTO);
-
-				} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
-					if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
-						/* EASY */
-						main_res = main_state_transition(status, MAIN_STATE_EASY);
-
-					} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
-						/* MANUAL */
-						main_res = main_state_transition(status, MAIN_STATE_MANUAL);
-					}
+				} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
+					/* MANUAL */
+					main_res = main_state_transition(status, MAIN_STATE_MANUAL);
 				}
 			}
-
-			if (main_res == TRANSITION_CHANGED)
-				ret = true;
-
-			if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) {
-				result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-			} else {
-				result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
-			}
-
-			break;
 		}
 
+
+		if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) {
+			result = VEHICLE_CMD_RESULT_ACCEPTED;
+
+		} else {
+			result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+		}
+
+		break;
+	}
+
 	case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
             // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm.
             // We use an float epsilon delta to test float equality.
@@ -503,13 +486,11 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 				status->set_nav_state = NAVIGATION_STATE_LOITER;
 				mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd");
 				result = VEHICLE_CMD_RESULT_ACCEPTED;
-				ret = true;
 
 			} else if (mav_goto == 1) {	// MAV_GOTO_DO_CONTINUE
 				status->set_nav_state = NAVIGATION_STATE_MISSION;
 				mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd");
 				result = VEHICLE_CMD_RESULT_ACCEPTED;
-				ret = true;
 
 			} else {
 				mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", cmd->param1, cmd->param2, cmd->param3, cmd->param4, cmd->param5, cmd->param6, cmd->param7);
@@ -525,7 +506,6 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) {
 				transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
 				result = VEHICLE_CMD_RESULT_ACCEPTED;
-				ret = true;
 
 			} else {
 				/* reject parachute depoyment not armed */
@@ -981,12 +961,10 @@ int commander_thread_main(int argc, char *argv[])
 		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && eph_epv_good, &(status.condition_local_position_valid), &status_changed);
 		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
 
-		static bool published_condition_landed_fw = false;
 		if (status.condition_local_altitude_valid) {
 			if (status.condition_landed != local_position.landed) {
 				status.condition_landed = local_position.landed;
 				status_changed = true;
-				published_condition_landed_fw = false; //make sure condition_landed is published again if the system type changes
 
 				if (status.condition_landed) {
 					mavlink_log_critical(mavlink_fd, "#audio: LANDED");
@@ -995,12 +973,6 @@ int commander_thread_main(int argc, char *argv[])
 					mavlink_log_critical(mavlink_fd, "#audio: IN AIR");
 				}
 			}
-		} else {
-			if (!published_condition_landed_fw) {
-				status.condition_landed = false; // Fixedwing does not have a landing detector currently
-				published_condition_landed_fw = true;
-				status_changed = true;
-			}
 		}
 
 		/* update battery status */
@@ -1265,6 +1237,8 @@ int commander_thread_main(int argc, char *argv[])
 							/* LAND not allowed, set TERMINATION state */
 							transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
 						}
+					} else {
+						status.set_nav_state = NAVIGATION_STATE_MISSION;
 					}
 
 				} else {
@@ -1284,7 +1258,9 @@ int commander_thread_main(int argc, char *argv[])
 							/* LAND not allowed, set TERMINATION state */
 							res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
 						}
-					}
+					} else {
+						status.set_nav_state = NAVIGATION_STATE_RTL;
+					}	
 				}
 
 			} else {

From 470513d9bb67bc19bd0ac70d209c681dc321ddfb Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sun, 27 Apr 2014 11:48:03 +0200
Subject: [PATCH 49/97] make it compile again after merge

---
 .../ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp   | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
index a8b6eae1cb..7b9a558b56 100644
--- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
@@ -979,7 +979,7 @@ FixedwingEstimator::task_main()
 				// struct home_position_s home;
 				// orb_copy(ORB_ID(home_position), _home_sub, &home);
 
-				if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph_m < _parameters.pos_stddev_threshold && _gps.epv_m < _parameters.pos_stddev_threshold) {
+				if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) {
 					_ekf->velNED[0] = _gps.vel_n_m_s;
 					_ekf->velNED[1] = _gps.vel_e_m_s;
 					_ekf->velNED[2] = _gps.vel_d_m_s;
@@ -1017,7 +1017,7 @@ FixedwingEstimator::task_main()
 					mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt);
 					warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt,
 						(double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]);
-					warnx("GPS: eph: %8.4f, epv: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m);
+					warnx("GPS: eph: %8.4f, epv: %8.4f", (double)_gps.eph, (double)_gps.epv);
 
 					_gps_initialized = true;
 
@@ -1259,8 +1259,8 @@ FixedwingEstimator::task_main()
 					_global_pos.lat = est_lat;
 					_global_pos.lon = est_lon;
 					_global_pos.time_gps_usec = _gps.time_gps_usec;
-					_global_pos.eph = _gps.eph_m;
-					_global_pos.epv = _gps.epv_m;
+					_global_pos.eph = _gps.eph;
+					_global_pos.epv = _gps.epv;
 				}
 
 				if (_local_pos.v_xy_valid) {

From 68352cb923d366b66bb68c8d946c4960b6f7ff1a Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 7 May 2014 23:53:27 +0200
Subject: [PATCH 50/97] merge fixes

---
 src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
index 35ba96f590..dadcc1dcea 100644
--- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
@@ -1029,7 +1029,7 @@ FixedwingEstimator::task_main()
 
 				float initVelNED[3];
 
-				if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph_m < _parameters.pos_stddev_threshold && _gps.epv_m < _parameters.pos_stddev_threshold) {
+				if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) {
 
 					initVelNED[0] = _gps.vel_n_m_s;
 					initVelNED[1] = _gps.vel_e_m_s;

From ed6c2a5168ca891f20594687acfd3c6bbf7e1cf9 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 27 May 2014 21:56:32 +0200
Subject: [PATCH 51/97] commander and navigator: lot's of changes, failsafe
 handling in commander, navigator only for execution (WIP)

---
 src/drivers/blinkm/blinkm.cpp                 |   5 +-
 src/drivers/gps/mtk.cpp                       |   4 +-
 src/modules/commander/commander.cpp           | 488 ++++++++----------
 .../commander/state_machine_helper.cpp        | 422 ++++-----------
 src/modules/commander/state_machine_helper.h  |  16 +-
 src/modules/mavlink/mavlink_messages.cpp      |  86 +--
 src/modules/navigator/navigator_main.cpp      | 240 +--------
 src/modules/systemlib/state_table.h           |  28 +-
 src/modules/uORB/topics/mission_result.h      |   1 +
 .../uORB/topics/position_setpoint_triplet.h   |  15 +-
 src/modules/uORB/topics/vehicle_status.h      |  35 +-
 11 files changed, 423 insertions(+), 917 deletions(-)

diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp
index 5c502f6822..98c491ce67 100644
--- a/src/drivers/blinkm/blinkm.cpp
+++ b/src/drivers/blinkm/blinkm.cpp
@@ -655,13 +655,14 @@ BlinkM::led()
 							/* indicate main control state */
 							if (vehicle_status_raw.main_state == MAIN_STATE_POSCTL)
 								led_color_4 = LED_GREEN;
-							else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO)
+							/* TODO: add other Auto modes */
+							else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO_MISSION)
 								led_color_4 = LED_BLUE;
 							else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTL)
 								led_color_4 = LED_YELLOW;
 							else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL)
 								led_color_4 = LED_WHITE;
-							else 
+							else
 								led_color_4 = LED_OFF;
 							led_color_5 = led_color_4;
 						}
diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp
index ea4e95fb23..41716cd97e 100644
--- a/src/drivers/gps/mtk.cpp
+++ b/src/drivers/gps/mtk.cpp
@@ -251,8 +251,8 @@ MTK::handle_message(gps_mtk_packet_t &packet)
 		_gps_position->lon = 0;
 
 		// Indicate this data is not usable and bail out
-		_gps_position->eph_m = 1000.0f;
-		_gps_position->epv_m = 1000.0f;
+		_gps_position->eph = 1000.0f;
+		_gps_position->epv = 1000.0f;
 		_gps_position->fix_type = 0;
 		return;
 	}
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 77d810a811..908384bb65 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1,11 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (C) 2013-2014 PX4 Development Team. All rights reserved.
- *   Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
- *           Lorenz Meier <lm@inf.ethz.ch>
- *           Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *           Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -38,8 +33,13 @@
 
 /**
  * @file commander.cpp
- * Main system state machine implementation.
+ * Main fail-safe handling.
  *
+ * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #include <nuttx/config.h>
@@ -76,6 +76,7 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/safety.h>
+#include <uORB/topics/mission_result.h>
 
 #include <drivers/drv_led.h>
 #include <drivers/drv_hrt.h>
@@ -87,6 +88,7 @@
 #include <systemlib/err.h>
 #include <systemlib/cpuload.h>
 #include <systemlib/rc_check.h>
+#include <systemlib/state_table.h>
 
 #include "px4_custom_mode.h"
 #include "commander_helper.h"
@@ -387,103 +389,80 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armed
 
 bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub)
 {
-	/* result of the command */
-	enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED;
-
 	/* only handle commands that are meant to be handled by this system and component */
 	if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components
 		return false;
 	}
 
-	/* only handle high-priority commands here */
+	/* result of the command */
+	enum VEHICLE_CMD_RESULT cmd_result = VEHICLE_CMD_RESULT_UNSUPPORTED;
 
 	/* request to set different system mode */
 	switch (cmd->command) {
 	case VEHICLE_CMD_DO_SET_MODE: {
-		uint8_t base_mode = (uint8_t) cmd->param1;
-		uint8_t custom_main_mode = (uint8_t) cmd->param2;
+			uint8_t base_mode = (uint8_t)cmd->param1;
+			uint8_t custom_main_mode = (uint8_t)cmd->param2;
 
-		/* set HIL state */
-		hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
-		int hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd);
+			transition_result_t arming_ret = TRANSITION_NOT_CHANGED;
 
-		/* if HIL got enabled, reset battery status state */
-		if (hil_ret == OK && status->hil_state == HIL_STATE_ON) {
-			/* reset the arming mode to disarmed */
-			arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed);
-		}
+			transition_result_t main_ret = TRANSITION_NOT_CHANGED;
 
-		// Transition the arming state
-		transition_result_t arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
+			/* set HIL state */
+			hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
+			transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd);
 
-		/* set main state */
-		transition_result_t main_res = TRANSITION_DENIED;
+			// Transition the arming state
+			arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
 
-		if (hil_ret == OK) {
-			ret = true;
-		}
-
-		// Transition the arming state
-		arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
-
-		if (arming_res == TRANSITION_CHANGED) {
-			ret = true;
-		}
-
-			} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
-				/* AUTO */
-				main_res = main_state_transition(status, MAIN_STATE_AUTO);
-			}
+			if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
+				/* use autopilot-specific mode */
+				if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
+					/* MANUAL */
+					main_ret = main_state_transition(status, MAIN_STATE_MANUAL);
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
 					/* ALTCTL */
-					main_res = main_state_transition(status, MAIN_STATE_ALTCTL);
+					main_ret = main_state_transition(status, MAIN_STATE_ALTCTL);
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
 					/* POSCTL */
-					main_res = main_state_transition(status, MAIN_STATE_POSCTL);
+					main_ret = main_state_transition(status, MAIN_STATE_POSCTL);
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
 					/* AUTO */
-					main_res = main_state_transition(status, MAIN_STATE_AUTO);
+					main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION);
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
 					/* ACRO */
-					main_res = main_state_transition(status, MAIN_STATE_ACRO);
+					main_ret = main_state_transition(status, MAIN_STATE_ACRO);
 				}
 
-				} else {
+			} else {
 				/* use base mode */
 				if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
 					/* AUTO */
-					main_res = main_state_transition(status, MAIN_STATE_AUTO);
+					main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION);
 
 				} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
 					if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
 						/* POSCTL */
-						main_res = main_state_transition(status, MAIN_STATE_POSCTL);
+						main_ret = main_state_transition(status, MAIN_STATE_POSCTL);
 
 					} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
 						/* MANUAL */
-						main_res = main_state_transition(status, MAIN_STATE_MANUAL);
+						main_ret = main_state_transition(status, MAIN_STATE_MANUAL);
 					}
 				}
 			}
+
+			if (hil_ret != TRANSITION_DENIED && arming_ret != TRANSITION_DENIED && main_ret != TRANSITION_DENIED) {
+				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
+
+			} else {
+				cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+			}
 		}
-
-		if (main_res == TRANSITION_CHANGED) {
-			ret = true;
-		}
-
-		if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) {
-			result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-		} else {
-			result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
-		}
-
 		break;
-	}
 
 	case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
 			// Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm.
@@ -502,10 +481,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 
 				if (arming_res == TRANSITION_DENIED) {
 					mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
-					result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+					cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 
 				} else {
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
+					cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 				}
 			}
 		}
@@ -516,14 +495,14 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			unsigned int mav_goto = cmd->param1;
 
 			if (mav_goto == 0) {	// MAV_GOTO_DO_HOLD
-				status->set_nav_state = NAVIGATION_STATE_LOITER;
+				status->set_nav_state = NAVIGATION_STATE_AUTO_LOITER;
 				mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd");
-				result = VEHICLE_CMD_RESULT_ACCEPTED;
+				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else if (mav_goto == 1) {	// MAV_GOTO_DO_CONTINUE
-				status->set_nav_state = NAVIGATION_STATE_MISSION;
+				status->set_nav_state = NAVIGATION_STATE_AUTO_MISSION;
 				mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd");
-				result = VEHICLE_CMD_RESULT_ACCEPTED;
+				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else {
 				mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", cmd->param1, cmd->param2, cmd->param3, cmd->param4, cmd->param5, cmd->param6, cmd->param7);
@@ -531,6 +510,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		}
 		break;
 
+#if 0
 	/* Flight termination */
 	case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
 
@@ -538,15 +518,16 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			//xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
 			if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) {
 				transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
-				result = VEHICLE_CMD_RESULT_ACCEPTED;
+				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else {
 				/* reject parachute depoyment not armed */
-				result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 			}
 
 		}
 		break;
+#endif
 
 	case VEHICLE_CMD_DO_SET_HOME: {
 			bool use_current = cmd->param1 > 0.5f;
@@ -560,10 +541,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 
 					home->timestamp = hrt_absolute_time();
 
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
+					cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 
 				} else {
-					result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+					cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 				}
 
 			} else {
@@ -574,10 +555,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 
 				home->timestamp = hrt_absolute_time();
 
-				result = VEHICLE_CMD_RESULT_ACCEPTED;
+				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 			}
 
-			if (result == VEHICLE_CMD_RESULT_ACCEPTED) {
+			if (cmd_result == VEHICLE_CMD_RESULT_ACCEPTED) {
 				warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home->lat, home->lon, (double)home->alt);
 				mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home->lat, home->lon, (double)home->alt);
 
@@ -609,13 +590,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		break;
 	}
 
-	if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
+	if (cmd_result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
 		/* already warned about unsupported commands in "default" case */
-		answer_command(*cmd, result);
+		answer_command(*cmd, cmd_result);
 	}
 
 	/* send any requested ACKs */
-	if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
+	if (cmd->confirmation > 0 && cmd_result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
 		/* send acknowledge command */
 		// XXX TODO
 	}
@@ -682,7 +663,7 @@ int commander_thread_main(int argc, char *argv[])
 	// We want to accept RC inputs as default
 	status.rc_input_blocked = false;
 	status.main_state = MAIN_STATE_MANUAL;
-	status.set_nav_state = NAVIGATION_STATE_NONE;
+	status.set_nav_state = NAVIGATION_STATE_MANUAL;
 	status.arming_state = ARMING_STATE_INIT;
 	status.hil_state = HIL_STATE_OFF;
 	status.failsafe_state = FAILSAFE_STATE_NORMAL;
@@ -772,6 +753,11 @@ int commander_thread_main(int argc, char *argv[])
 	safety.safety_switch_available = false;
 	safety.safety_off = false;
 
+	/* Subscribe to mission result topic */
+	int mission_result_sub = orb_subscribe(ORB_ID(mission_result));
+	struct mission_result_s mission_result;
+	memset(&mission_result, 0, sizeof(mission_result));
+
 	/* Subscribe to manual control data */
 	int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	struct manual_control_setpoint_s sp_man;
@@ -849,6 +835,13 @@ int commander_thread_main(int argc, char *argv[])
 
 	start_time = hrt_absolute_time();
 
+	transition_result_t arming_ret;
+
+	/* check which state machines for changes, clear "changed" flag */
+	bool arming_state_changed = false;
+	bool main_state_changed = false;
+	bool failsafe_state_changed = false;
+
 	while (!thread_should_exit) {
 
 		if (mavlink_fd < 0 && counter % (1000000 / MAVLINK_OPEN_INTERVAL) == 0) {
@@ -856,6 +849,9 @@ int commander_thread_main(int argc, char *argv[])
 			mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 		}
 
+		arming_ret = TRANSITION_NOT_CHANGED;
+
+
 		/* update parameters */
 		orb_check(param_changed_sub, &updated);
 
@@ -935,6 +931,7 @@ int commander_thread_main(int argc, char *argv[])
 
 				if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) {
 					mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch");
+					arming_state_changed = true;
 				}
 			}
 		}
@@ -1131,12 +1128,21 @@ int commander_thread_main(int argc, char *argv[])
 			status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
 
 			if (armed.armed) {
-				arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
+				arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
+
+				if (arming_ret == TRANSITION_CHANGED) {
+					warnx("changed 1");
+					arming_state_changed = true;
+				}
 
 			} else {
-				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
-			}
+				arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
 
+				if (arming_ret == TRANSITION_CHANGED) {
+					warnx("changed 2");
+					arming_state_changed = true;
+				}
+			}
 			status_changed = true;
 		}
 
@@ -1144,11 +1150,18 @@ int commander_thread_main(int argc, char *argv[])
 
 		/* If in INIT state, try to proceed to STANDBY state */
 		if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
-			// XXX check for sensors
-			arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
+			/* TODO: check for sensors */
+			warnx("arming status1: %d", status.arming_state);
+			arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
+			warnx("arming status2: %d", status.arming_state);
+
+			if (arming_ret == TRANSITION_CHANGED) {
+				warnx("changed");
+				arming_state_changed = true;
+			}
 
 		} else {
-			// XXX: Add emergency stuff if sensors are lost
+			/* TODO: Add emergency stuff if sensors are lost */
 		}
 
 
@@ -1167,6 +1180,12 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
 		}
 
+		orb_check(mission_result_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
+		}
+
 		/* start RC input check */
 		if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
 			/* handle the case where RC signal was regained */
@@ -1184,11 +1203,6 @@ int commander_thread_main(int argc, char *argv[])
 
 			status.rc_signal_lost = false;
 
-			transition_result_t arming_res;	// store all transitions results here
-
-			/* arm/disarm by RC */
-			arming_res = TRANSITION_NOT_CHANGED;
-
 			/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST mode and landed) -> disarm
 			 * do it only for rotary wings */
 			if (status.is_rotary_wing &&
@@ -1199,7 +1213,11 @@ int commander_thread_main(int argc, char *argv[])
 				if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
 					/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
 					arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
-					arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed);
+					arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed);
+					if (arming_ret == TRANSITION_CHANGED) {
+						warnx("stick 1");
+						arming_state_changed = true;
+					}
 					stick_off_counter = 0;
 
 				} else {
@@ -1221,7 +1239,11 @@ int commander_thread_main(int argc, char *argv[])
 						print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first.");
 
 					} else {
-						arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
+						arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
+						if (arming_ret == TRANSITION_CHANGED) {
+							warnx("stick 2");
+							arming_state_changed = true;
+						}
 					}
 
 					stick_on_counter = 0;
@@ -1234,23 +1256,25 @@ int commander_thread_main(int argc, char *argv[])
 				stick_on_counter = 0;
 			}
 
-			if (arming_res == TRANSITION_CHANGED) {
+			if (arming_ret == TRANSITION_CHANGED) {
 				if (status.arming_state == ARMING_STATE_ARMED) {
 					mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC");
 
 				} else {
 					mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC");
 				}
+				arming_state_changed = true;
 
-			} else if (arming_res == TRANSITION_DENIED) {
+			} else if (arming_ret == TRANSITION_DENIED) {
 				/* DENIED here indicates bug in the commander */
 				mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied");
 			}
 
-			if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
-				/* recover from failsafe */
-				(void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
-			}
+
+			// if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
+			//	/* recover from failsafe */
+			//	(void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
+			// }
 
 			/* evaluate the main state machine according to mode switches */
 			transition_result_t main_res = set_main_state_rc(&status, &sp_man);
@@ -1258,102 +1282,41 @@ int commander_thread_main(int argc, char *argv[])
 			/* play tune on mode change only if armed, blink LED always */
 			if (main_res == TRANSITION_CHANGED) {
 				tune_positive(armed.armed);
+				main_state_changed = true;
 
 			} else if (main_res == TRANSITION_DENIED) {
 				/* DENIED here indicates bug in the commander */
 				mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied");
 			}
 
-			/* set navigation state */
-			/* RETURN switch, overrides MISSION switch */
-			if (sp_man.return_switch == SWITCH_POS_ON) {
-				/* switch to RTL if not already landed after RTL and home position set */
-				status.set_nav_state = NAVIGATION_STATE_RTL;
-
-
-			} else {
-
-				/* LOITER switch */
-				if (sp_man.loiter_switch == SWITCH_POS_ON) {
-					/* stick is in LOITER position */
-					status.set_nav_state = NAVIGATION_STATE_LOITER;
-
-				} else if (sp_man.loiter_switch != SWITCH_POS_NONE) {
-					/* stick is in MISSION position */
-					status.set_nav_state = NAVIGATION_STATE_MISSION;
-
-				} else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) &&
-					   pos_sp_triplet.nav_state == NAV_STATE_RTL) {
-					/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
-					status.set_nav_state = NAVIGATION_STATE_MISSION;
-				}
-			}
-
 		} else {
 			if (!status.rc_signal_lost) {
 				mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST");
 				status.rc_signal_lost = true;
 				status_changed = true;
 			}
-
-			if (armed.armed) {
-				if (status.main_state == MAIN_STATE_AUTO) {
-					/* check if AUTO mode still allowed */
-					transition_result_t auto_res = main_state_transition(&status, MAIN_STATE_AUTO);
-
-					if (auto_res == TRANSITION_NOT_CHANGED) {
-						last_auto_state_valid = hrt_absolute_time();
-					}
-
-					/* still invalid state after the timeout interval, execute failsafe */
-					if ((hrt_elapsed_time(&last_auto_state_valid) > FAILSAFE_DEFAULT_TIMEOUT) && (auto_res == TRANSITION_DENIED)) {
-						/* AUTO mode denied, don't try RTL, switch to failsafe state LAND */
-						auto_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
-
-						if (auto_res == TRANSITION_DENIED) {
-							/* LAND not allowed, set TERMINATION state */
-							(void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
-						}
-					} else {
-						status.set_nav_state = NAVIGATION_STATE_MISSION;
-					}
-
-				} else {
-					/* failsafe for manual modes */
-					transition_result_t manual_res = TRANSITION_DENIED;
-
-					if (!status.condition_landed) {
-						/* vehicle is not landed, try to perform RTL */
-						manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
-					}
-
-					if (manual_res == TRANSITION_DENIED) {
-						/* RTL not allowed (no global position estimate) or not wanted, try LAND */
-						manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
-
-						if (manual_res == TRANSITION_DENIED) {
-							/* LAND not allowed, set TERMINATION state */
-							(void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
-						}
-					} else {
-						status.set_nav_state = NAVIGATION_STATE_RTL;
-					}
-				}
-
-			} else {
-				if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
-					/* reset failsafe when disarmed */
-					(void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
-				}
-			}
 		}
 
-		// TODO remove this hack
-		/* flight termination in manual mode if assist switch is on POSCTL position */
-		if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctl_switch == SWITCH_POS_ON) {
-			if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
-				tune_positive(armed.armed);
+		if (armed.armed) {
+			/* if RC is lost, switch to RTL_RC or Termination unless a mission is running
+			 * and not yet finished) */
+			if (status.rc_signal_lost
+		            && !(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.mission_finished)) {
+
+				/* if we have a global position, we can switch to RTL, if not, we can try to land */
+				if (status.condition_global_position_valid) {
+					status.failsafe_state = FAILSAFE_STATE_RTL_RC;
+				} else {
+					status.failsafe_state = FAILSAFE_STATE_LAND;
+				}
 			}
+
+			/* if the data link is lost, switch to RTL_DL or Termination */
+			/* TODO: add this */
+
+		} else {
+			/* reset failsafe when disarmed */
+			status.failsafe_state = FAILSAFE_STATE_NORMAL;
 		}
 
 		/* handle commands last, as the system needs to be updated to handle them */
@@ -1369,11 +1332,6 @@ int commander_thread_main(int argc, char *argv[])
 			}
 		}
 
-		/* check which state machines for changes, clear "changed" flag */
-		bool arming_state_changed = check_arming_state_changed();
-		bool main_state_changed = check_main_state_changed();
-		bool failsafe_state_changed = check_failsafe_state_changed();
-
 		hrt_abstime t1 = hrt_absolute_time();
 
 		/* print new state */
@@ -1408,18 +1366,24 @@ int commander_thread_main(int argc, char *argv[])
 				/* mark home position as set */
 				status.condition_home_position_valid = true;
 			}
+			arming_state_changed = false;
 		}
 
 		was_armed = armed.armed;
 
+		/* now set nav state according to failsafe and main state */
+		set_nav_state(&status);
+
 		if (main_state_changed) {
 			status_changed = true;
 			mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]);
+			main_state_changed = false;
 		}
 
 		if (failsafe_state_changed) {
 			status_changed = true;
 			mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %s", failsafe_states_str[status.failsafe_state]);
+			failsafe_state_changed = false;
 		}
 
 		/* publish states (armed, control mode, vehicle status) at least with 5 Hz */
@@ -1608,6 +1572,7 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
 	switch (sp_man->mode_switch) {
 	case SWITCH_POS_NONE:
 		res = TRANSITION_NOT_CHANGED;
+		warnx("NONE");
 		break;
 
 	case SWITCH_POS_OFF:		// MANUAL
@@ -1648,14 +1613,13 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
 		break;
 
 	case SWITCH_POS_ON:			// AUTO
-		res = main_state_transition(status, MAIN_STATE_AUTO);
+		res = main_state_transition(status, MAIN_STATE_AUTO_MISSION);
 
 		if (res != TRANSITION_DENIED) {
 			break;	// changed successfully or already in this state
 		}
 
 		// else fallback to ALTCTL (POSCTL likely will not work too)
-		print_reject_mode(status, "AUTO");
 		res = main_state_transition(status, MAIN_STATE_ALTCTL);
 
 		if (res != TRANSITION_DENIED) {
@@ -1675,85 +1639,93 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
 }
 
 void
-
 set_control_mode()
 {
-	/* set vehicle_control_mode according to main state and failsafe state */
+	/* set vehicle_control_mode according to set_navigation_state */
 	control_mode.flag_armed = armed.armed;
+	/* TODO: check this */
 	control_mode.flag_external_manual_override_ok = !status.is_rotary_wing;
 	control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON;
 
-	control_mode.flag_control_termination_enabled = false;
-
-	/* set this flag when navigator should act */
-	bool navigator_enabled = false;
-
-	switch (status.failsafe_state) {
-	case FAILSAFE_STATE_NORMAL:
-		switch (status.main_state) {
-		case MAIN_STATE_MANUAL:
-			control_mode.flag_control_manual_enabled = true;
-			control_mode.flag_control_auto_enabled = false;
-			control_mode.flag_control_rates_enabled = status.is_rotary_wing;
-			control_mode.flag_control_attitude_enabled = status.is_rotary_wing;
-			control_mode.flag_control_altitude_enabled = false;
-			control_mode.flag_control_climb_rate_enabled = false;
-			control_mode.flag_control_position_enabled = false;
-			control_mode.flag_control_velocity_enabled = false;
-			break;
-
-		case MAIN_STATE_ALTCTL:
-			control_mode.flag_control_manual_enabled = true;
-			control_mode.flag_control_auto_enabled = false;
-			control_mode.flag_control_rates_enabled = true;
-			control_mode.flag_control_attitude_enabled = true;
-			control_mode.flag_control_altitude_enabled = true;
-			control_mode.flag_control_climb_rate_enabled = true;
-			control_mode.flag_control_position_enabled = false;
-			control_mode.flag_control_velocity_enabled = false;
-			break;
-
-		case MAIN_STATE_POSCTL:
-			control_mode.flag_control_manual_enabled = true;
-			control_mode.flag_control_auto_enabled = false;
-			control_mode.flag_control_rates_enabled = true;
-			control_mode.flag_control_attitude_enabled = true;
-			control_mode.flag_control_altitude_enabled = true;
-			control_mode.flag_control_climb_rate_enabled = true;
-			control_mode.flag_control_position_enabled = true;
-			control_mode.flag_control_velocity_enabled = true;
-			break;
-
-		case MAIN_STATE_AUTO:
-			navigator_enabled = true;
-			break;
-
-		case MAIN_STATE_ACRO:
-			control_mode.flag_control_manual_enabled = true;
-			control_mode.flag_control_auto_enabled = false;
-			control_mode.flag_control_rates_enabled = true;
-			control_mode.flag_control_attitude_enabled = false;
-			control_mode.flag_control_altitude_enabled = false;
-			control_mode.flag_control_climb_rate_enabled = false;
-			control_mode.flag_control_position_enabled = false;
-			control_mode.flag_control_velocity_enabled = false;
-			break;
-
-		default:
-			break;
-		}
-
+	switch (status.set_nav_state) {
+	case NAVIGATION_STATE_MANUAL:
+		control_mode.flag_control_manual_enabled = true;
+		control_mode.flag_control_auto_enabled = false;
+		control_mode.flag_control_rates_enabled = status.is_rotary_wing;
+		control_mode.flag_control_attitude_enabled = status.is_rotary_wing;
+		control_mode.flag_control_altitude_enabled = false;
+		control_mode.flag_control_climb_rate_enabled = false;
+		control_mode.flag_control_position_enabled = false;
+		control_mode.flag_control_velocity_enabled = false;
+		control_mode.flag_control_termination_enabled = false;
 		break;
 
-	case FAILSAFE_STATE_RTL:
-		navigator_enabled = true;
+	case NAVIGATION_STATE_ACRO:
+		control_mode.flag_control_manual_enabled = true;
+		control_mode.flag_control_auto_enabled = false;
+		control_mode.flag_control_rates_enabled = true;
+		control_mode.flag_control_attitude_enabled = false;
+		control_mode.flag_control_altitude_enabled = false;
+		control_mode.flag_control_climb_rate_enabled = false;
+		control_mode.flag_control_position_enabled = false;
+		control_mode.flag_control_velocity_enabled = false;
+		control_mode.flag_control_termination_enabled = false;
 		break;
 
-	case FAILSAFE_STATE_LAND:
-		navigator_enabled = true;
+	case NAVIGATION_STATE_ALTCTL:
+		control_mode.flag_control_manual_enabled = true;
+		control_mode.flag_control_auto_enabled = false;
+		control_mode.flag_control_rates_enabled = true;
+		control_mode.flag_control_attitude_enabled = true;
+		control_mode.flag_control_altitude_enabled = true;
+		control_mode.flag_control_climb_rate_enabled = true;
+		control_mode.flag_control_position_enabled = false;
+		control_mode.flag_control_velocity_enabled = false;
+		control_mode.flag_control_termination_enabled = false;
 		break;
 
-	case FAILSAFE_STATE_TERMINATION:
+	case NAVIGATION_STATE_POSCTL:
+		control_mode.flag_control_manual_enabled = true;
+		control_mode.flag_control_auto_enabled = false;
+		control_mode.flag_control_rates_enabled = true;
+		control_mode.flag_control_attitude_enabled = true;
+		control_mode.flag_control_altitude_enabled = true;
+		control_mode.flag_control_climb_rate_enabled = true;
+		control_mode.flag_control_position_enabled = true;
+		control_mode.flag_control_velocity_enabled = true;
+		control_mode.flag_control_termination_enabled = false;
+		break;
+
+	case NAVIGATION_STATE_AUTO_MISSION:
+	case NAVIGATION_STATE_AUTO_LOITER:
+	case NAVIGATION_STATE_AUTO_RTL:
+	case NAVIGATION_STATE_AUTO_RTL_RC:
+	case NAVIGATION_STATE_AUTO_RTL_DL:
+		control_mode.flag_control_manual_enabled = false;
+		control_mode.flag_control_auto_enabled = true;
+		control_mode.flag_control_rates_enabled = true;
+		control_mode.flag_control_attitude_enabled = true;
+		control_mode.flag_control_altitude_enabled = true;
+		control_mode.flag_control_climb_rate_enabled = true;
+		control_mode.flag_control_position_enabled = true;
+		control_mode.flag_control_velocity_enabled = true;
+		control_mode.flag_control_termination_enabled = false;
+		break;
+
+	case NAVIGATION_STATE_LAND:
+		control_mode.flag_control_manual_enabled = false;
+		control_mode.flag_control_auto_enabled = true;
+		control_mode.flag_control_rates_enabled = true;
+		control_mode.flag_control_attitude_enabled = true;
+		/* in failsafe LAND mode position may be not available */
+		control_mode.flag_control_position_enabled = status.condition_local_position_valid;
+		control_mode.flag_control_velocity_enabled = status.condition_local_position_valid;
+		control_mode.flag_control_altitude_enabled = true;
+		control_mode.flag_control_climb_rate_enabled = true;
+		control_mode.flag_control_termination_enabled = false;
+		break;
+
+	case NAVIGATION_STATE_TERMINATION:
 		/* disable all controllers on termination */
 		control_mode.flag_control_manual_enabled = false;
 		control_mode.flag_control_auto_enabled = false;
@@ -1769,21 +1741,6 @@ set_control_mode()
 	default:
 		break;
 	}
-
-	/* navigator has control, set control mode flags according to nav state*/
-	if (navigator_enabled) {
-		control_mode.flag_control_manual_enabled = false;
-		control_mode.flag_control_auto_enabled = true;
-		control_mode.flag_control_rates_enabled = true;
-		control_mode.flag_control_attitude_enabled = true;
-
-		/* in failsafe LAND mode position may be not available */
-		control_mode.flag_control_position_enabled = status.condition_local_position_valid;
-		control_mode.flag_control_velocity_enabled = status.condition_local_position_valid;
-
-		control_mode.flag_control_altitude_enabled = true;
-		control_mode.flag_control_climb_rate_enabled = true;
-	}
 }
 
 void
@@ -1927,7 +1884,6 @@ void *commander_low_prio_loop(void *arg)
 
 				/* try to go to INIT/PREFLIGHT arming state */
 
-				// XXX disable interrupts in arming_state_transition
 				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) {
 					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
 					break;
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 6f0e9794aa..2144800790 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -1,8 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -36,6 +34,9 @@
 /**
  * @file state_machine_helper.cpp
  * State machine helper functions implementations
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <julian@oes.ch>
  */
 
 #include <stdio.h>
@@ -59,30 +60,20 @@
 #include "state_machine_helper.h"
 #include "commander_helper.h"
 
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-static const int ERROR = -1;
-
-static bool arming_state_changed = true;
-static bool main_state_changed = true;
-static bool failsafe_state_changed = true;
-
 // This array defines the arming state transitions. The rows are the new state, and the columns
 // are the current state. Using new state and current  state you can index into the array which
 // will be true for a valid transition or false for a invalid transition. In some cases even
 // though the transition is marked as true additional checks must be made. See arming_state_transition
 // code for those checks.
 static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = {
-	//                                  INIT,   STANDBY,    ARMED,  ARMED_ERROR,    STANDBY_ERROR,  REBOOT,     IN_AIR_RESTORE
-	{ /* ARMING_STATE_INIT */           true,   true,       false,  false,          false,          false,      false },
-	{ /* ARMING_STATE_STANDBY */        true,   true,       true,   true,           false,          false,      false },
-	{ /* ARMING_STATE_ARMED */          false,  true,       true,   false,          false,          false,      true },
-	{ /* ARMING_STATE_ARMED_ERROR */    false,  false,      true,   true,           false,          false,      false },
-	{ /* ARMING_STATE_STANDBY_ERROR */  true,   true,       false,  true,           true,           false,      false },
-	{ /* ARMING_STATE_REBOOT */         true,   true,       false,  false,          true,           true,       true },
-	{ /* ARMING_STATE_IN_AIR_RESTORE */ false,  false,      false,  false,          false,          false,      false }, // NYI
+	//                                  INIT,  STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE
+	{ /* ARMING_STATE_INIT */           true,  true,    false, false,       false,         false,  false },
+	{ /* ARMING_STATE_STANDBY */        true,  true,    true,  true,        false,         false,  false },
+	{ /* ARMING_STATE_ARMED */          false, true,    true,  false,       false,         false,  true },
+	{ /* ARMING_STATE_ARMED_ERROR */    false, false,   true,  true,        false,         false,  false },
+	{ /* ARMING_STATE_STANDBY_ERROR */  true,  true,    false, true,        true,          false,  false },
+	{ /* ARMING_STATE_REBOOT */         true,  true,    false, false,       true,          true,   true },
+	{ /* ARMING_STATE_IN_AIR_RESTORE */ false, false,   false, false,       false,         false,  false }, // NYI
 };
 
 // You can index into the array with an arming_state_t in order to get it's textual representation
@@ -165,7 +156,6 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 			armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY;
 			ret = TRANSITION_CHANGED;
 			status->arming_state = new_arming_state;
-			arming_state_changed = true;
 		}
 	}
 
@@ -199,69 +189,58 @@ bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safet
 	}
 }
 
-bool
-check_arming_state_changed()
-{
-	if (arming_state_changed) {
-		arming_state_changed = false;
-		return true;
-
-	} else {
-		return false;
-	}
-}
-
 transition_result_t
 main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state)
 {
 	transition_result_t ret = TRANSITION_DENIED;
 
-	/* transition may be denied even if requested the same state because conditions may be changed */
+	/* transition may be denied even if the same state is requested because conditions may have changed */
 	switch (new_main_state) {
 	case MAIN_STATE_MANUAL:
-		ret = TRANSITION_CHANGED;
-		break;
-
 	case MAIN_STATE_ACRO:
 		ret = TRANSITION_CHANGED;
 		break;
 
 	case MAIN_STATE_ALTCTL:
-
 		/* need at minimum altitude estimate */
+		/* TODO: add this for fixedwing as well */
 		if (!status->is_rotary_wing ||
 		    (status->condition_local_altitude_valid ||
 		     status->condition_global_position_valid)) {
 			ret = TRANSITION_CHANGED;
 		}
-
 		break;
 
 	case MAIN_STATE_POSCTL:
-
 		/* need at minimum local position estimate */
 		if (status->condition_local_position_valid ||
 		    status->condition_global_position_valid) {
 			ret = TRANSITION_CHANGED;
 		}
-
 		break;
 
-	case MAIN_STATE_AUTO:
-
+	case MAIN_STATE_AUTO_MISSION:
+	case MAIN_STATE_AUTO_LOITER:
 		/* need global position estimate */
 		if (status->condition_global_position_valid) {
 			ret = TRANSITION_CHANGED;
 		}
+		break;
 
+	case MAIN_STATE_AUTO_RTL:
+		/* need global position and home position */
+		if (status->condition_global_position_valid && status->condition_home_position_valid) {
+			ret = TRANSITION_CHANGED;
+		}
+		break;
+
+	case MAIN_STATE_MAX:
+	default:
 		break;
 	}
-
 	if (ret == TRANSITION_CHANGED) {
 		if (status->main_state != new_main_state) {
 			status->main_state = new_main_state;
-			main_state_changed = true;
-
 		} else {
 			ret = TRANSITION_NOT_CHANGED;
 		}
@@ -270,70 +249,35 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 	return ret;
 }
 
-bool
-check_main_state_changed()
-{
-	if (main_state_changed) {
-		main_state_changed = false;
-		return true;
-
-	} else {
-		return false;
-	}
-}
-
-bool
-check_failsafe_state_changed()
-{
-	if (failsafe_state_changed) {
-		failsafe_state_changed = false;
-		return true;
-
-	} else {
-		return false;
-	}
-}
-
 /**
-* Transition from one hil state to another
-*/
-int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
+ * Transition from one hil state to another
+ */
+transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
 {
-	bool valid_transition = false;
-	int ret = ERROR;
+	transition_result_t ret = TRANSITION_DENIED;
 
 	if (current_status->hil_state == new_state) {
-		valid_transition = true;
+		ret = TRANSITION_NOT_CHANGED;
 
 	} else {
-
 		switch (new_state) {
-
 		case HIL_STATE_OFF:
-
 			/* we're in HIL and unexpected things can happen if we disable HIL now */
 			mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)");
-			valid_transition = false;
-
+			ret = TRANSITION_DENIED;
 			break;
 
 		case HIL_STATE_ON:
-
 			if (current_status->arming_state == ARMING_STATE_INIT
 			    || current_status->arming_state == ARMING_STATE_STANDBY
 			    || current_status->arming_state == ARMING_STATE_STANDBY_ERROR) {
 
-				mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
-				valid_transition = true;
-
-				// Disable publication of all attached sensors
-
+				/* Disable publication of all attached sensors */
 				/* list directory */
 				DIR		*d;
 				d = opendir("/dev");
 
 				if (d) {
-
 					struct dirent	*direntry;
 					char devname[24];
 
@@ -388,288 +332,98 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 
 						printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
 					}
-
 					closedir(d);
+					ret = TRANSITION_CHANGED;
+					mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
+
 
 				} else {
 					/* failed opening dir */
-					warnx("FAILED LISTING DEVICE ROOT DIRECTORY");
-					return 1;
+					mavlink_log_info(mavlink_fd, "FAILED LISTING DEVICE ROOT DIRECTORY");
+					ret = TRANSITION_DENIED;
 				}
+			} else {
+				mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed");
+				ret = TRANSITION_DENIED;
 			}
-
 			break;
 
 		default:
-			warnx("Unknown hil state");
+			warnx("Unknown HIL state");
 			break;
 		}
 	}
 
-	if (valid_transition) {
+	if (ret = TRANSITION_CHANGED) {
 		current_status->hil_state = new_state;
-
 		current_status->timestamp = hrt_absolute_time();
-		orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-
 		// XXX also set lockdown here
-
-		ret = OK;
-
-	} else {
-		mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition");
+		orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
 	}
-
 	return ret;
 }
 
 
 /**
-* Transition from one failsafe state to another
-*/
-transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state)
+ * Check failsafe and main status and set navigation status for navigator accordingly
+ */
+void set_nav_state(struct vehicle_status_s *status)
 {
-	transition_result_t ret = TRANSITION_DENIED;
-
-	/* transition may be denied even if requested the same state because conditions may be changed */
-	if (status->failsafe_state == FAILSAFE_STATE_TERMINATION) {
-		/* transitions from TERMINATION to other states not allowed */
-		if (new_failsafe_state == FAILSAFE_STATE_TERMINATION) {
-			ret = TRANSITION_NOT_CHANGED;
-		}
-
-	} else {
-		switch (new_failsafe_state) {
-		case FAILSAFE_STATE_NORMAL:
-			/* always allowed (except from TERMINATION state) */
-			ret = TRANSITION_CHANGED;
+	switch (status->failsafe_state) {
+	case FAILSAFE_STATE_NORMAL:
+		/* evaluate main state to decide in normal (non-failsafe) mode */
+		switch (status->main_state) {
+		case MAIN_STATE_MANUAL:
+			status->set_nav_state = NAVIGATION_STATE_MANUAL;
 			break;
 
-		case FAILSAFE_STATE_RTL:
-
-			/* global position and home position required for RTL */
-			if (status->condition_global_position_valid && status->condition_home_position_valid) {
-				status->set_nav_state = NAVIGATION_STATE_RTL;
-				ret = TRANSITION_CHANGED;
-			}
-
+		case MAIN_STATE_ALTCTL:
+			status->set_nav_state = NAVIGATION_STATE_ALTCTL;
 			break;
 
-		case FAILSAFE_STATE_LAND:
-
-			/* at least relative altitude estimate required for landing */
-			if (status->condition_local_altitude_valid || status->condition_global_position_valid) {
-				status->set_nav_state = NAVIGATION_STATE_LAND;
-				ret = TRANSITION_CHANGED;
-			}
-
+		case MAIN_STATE_POSCTL:
+			status->set_nav_state = NAVIGATION_STATE_POSCTL;
 			break;
 
-		case FAILSAFE_STATE_TERMINATION:
-			/* always allowed */
-			ret = TRANSITION_CHANGED;
+		case MAIN_STATE_AUTO_MISSION:
+			status->set_nav_state = NAVIGATION_STATE_AUTO_MISSION;
+			break;
+
+		case MAIN_STATE_AUTO_LOITER:
+			status->set_nav_state = NAVIGATION_STATE_AUTO_LOITER;
+			break;
+
+		case MAIN_STATE_AUTO_RTL:
+			status->set_nav_state = NAVIGATION_STATE_AUTO_RTL;
+			break;
+
+		case MAIN_STATE_ACRO:
+			status->set_nav_state = NAVIGATION_STATE_ACRO;
 			break;
 
 		default:
 			break;
 		}
+		break;
 
-		if (ret == TRANSITION_CHANGED) {
-			if (status->failsafe_state != new_failsafe_state) {
-				status->failsafe_state = new_failsafe_state;
-				failsafe_state_changed = true;
+	case FAILSAFE_STATE_RTL_RC:
+		status->set_nav_state = NAVIGATION_STATE_AUTO_RTL_RC;
+		break;
 
-			} else {
-				ret = TRANSITION_NOT_CHANGED;
-			}
-		}
+	case FAILSAFE_STATE_RTL_DL:
+		status->set_nav_state = NAVIGATION_STATE_AUTO_RTL_DL;
+		break;
+
+	case FAILSAFE_STATE_LAND:
+		status->set_nav_state = NAVIGATION_STATE_LAND;
+		break;
+
+	case FAILSAFE_STATE_TERMINATION:
+		status->set_nav_state = NAVIGATION_STATE_TERMINATION;
+		break;
+
+	default:
+		break;
 	}
-
-	return ret;
 }
 
-
-
-// /*
-//  * Wrapper functions (to be used in the commander), all functions assume lock on current_status
-//  */
-
-// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR
-//  *
-//  * START SUBSYSTEM/EMERGENCY FUNCTIONS
-//  * */
-
-// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_present |= 1 << *subsystem_type;
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-// }
-
-// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type);
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-
-// 	/* if a subsystem was removed something went completely wrong */
-
-// 	switch (*subsystem_type) {
-// 	case SUBSYSTEM_TYPE_GYRO:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_ACC:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_MAG:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_GPS:
-// 		{
-// 			uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV];
-
-// 			if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
-// 				//global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY);
-// 				state_machine_emergency(status_pub, current_status);
-// 			}
-// 		}
-// 		break;
-
-// 	default:
-// 		break;
-// 	}
-
-// }
-
-// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type;
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-// }
-
-// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type);
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-
-// 	/* if a subsystem was disabled something went completely wrong */
-
-// 	switch (*subsystem_type) {
-// 	case SUBSYSTEM_TYPE_GYRO:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_ACC:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_MAG:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_GPS:
-// 		{
-// 			uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]);
-
-// 			if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
-// 				//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY);
-// 				state_machine_emergency(status_pub, current_status);
-// 			}
-// 		}
-// 		break;
-
-// 	default:
-// 		break;
-// 	}
-
-// }
-
-
-///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/
-//
-//int update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
-//{
-//	int ret = 1;
-//
-////	/* Switch on HIL if in standby and not already in HIL mode */
-////	if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
-////	    && !current_status->flag_hil_enabled) {
-////		if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) {
-////			/* Enable HIL on request */
-////			current_status->flag_hil_enabled = true;
-////			ret = OK;
-////			state_machine_publish(status_pub, current_status, mavlink_fd);
-////			publish_armed_status(current_status);
-////			printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
-////
-////		} else if (current_status->state_machine != SYSTEM_STATE_STANDBY &&
-////			   current_status->flag_fmu_armed) {
-////
-////			mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!")
-////
-////		} else {
-////
-////			mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.")
-////		}
-////	}
-//
-//	/* switch manual / auto */
-//	if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
-//		update_state_machine_mode_auto(status_pub, current_status, mavlink_fd);
-//
-//	} else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) {
-//		update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd);
-//
-//	} else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
-//		update_state_machine_mode_guided(status_pub, current_status, mavlink_fd);
-//
-//	} else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
-//		update_state_machine_mode_manual(status_pub, current_status, mavlink_fd);
-//	}
-//
-//	/* vehicle is disarmed, mode requests arming */
-//	if (!(current_status->flag_fmu_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
-//		/* only arm in standby state */
-//		// XXX REMOVE
-//		if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
-//			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
-//			ret = OK;
-//			printf("[cmd] arming due to command request\n");
-//		}
-//	}
-//
-//	/* vehicle is armed, mode requests disarming */
-//	if (current_status->flag_fmu_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
-//		/* only disarm in ground ready */
-//		if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
-//			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
-//			ret = OK;
-//			printf("[cmd] disarming due to command request\n");
-//		}
-//	}
-//
-//	/* NEVER actually switch off HIL without reboot */
-//	if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
-//		warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n");
-//		mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL");
-//		ret = ERROR;
-//	}
-//
-//	return ret;
-//}
-
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index 0ddd4f05a2..594bf23a1d 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -56,25 +56,17 @@ typedef enum {
 
 } transition_result_t;
 
+bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
+
 transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
 		arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd = 0);
 
-bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
-
-bool check_arming_state_changed();
-
 transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state);
 
-bool check_main_state_changed();
-
 transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state);
 
-bool check_navigation_state_changed();
+transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd);
 
-bool check_failsafe_state_changed();
-
-void set_navigation_state_changed();
-
-int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd);
+void set_nav_state(struct vehicle_status_s *status);
 
 #endif /* STATE_MACHINE_HELPER_H_ */
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 70087cf3e7..31c0c8f79f 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -118,51 +118,71 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 	union px4_custom_mode custom_mode;
 	custom_mode.data = 0;
 
-	if (pos_sp_triplet->nav_state == NAV_STATE_NONE_ON_GROUND
-		|| pos_sp_triplet->nav_state == NAV_STATE_NONE_IN_AIR) {
-		/* 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);
+	switch (status->set_nav_state) {
+
+		case NAVIGATION_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;
+			break;
 
-		} else if (status->main_state == MAIN_STATE_ALTCTL) {
-			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
-			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL;
-
-		} else if (status->main_state == MAIN_STATE_POSCTL) {
-			*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_POSCTL;
-
-		} 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 if (status->main_state == MAIN_STATE_ACRO) {
+		case NAVIGATION_STATE_ACRO:
 			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO;
-		}
+			break;
 
-	} 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;
+		case NAVIGATION_STATE_ALTCTL:
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
+			                      | MAV_MODE_FLAG_STABILIZE_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL;
+			break;
 
-		if (pos_sp_triplet->nav_state == NAV_STATE_AUTO_ON_GROUND) {
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
+		case NAVIGATION_STATE_POSCTL:
+			*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_POSCTL;
+			break;
 
-		} 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) {
+		case NAVIGATION_STATE_AUTO_MISSION:
+			*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_MISSION;
+			break;
 
-		} else if (pos_sp_triplet->nav_state == NAV_STATE_RTL) {
+		case NAVIGATION_STATE_AUTO_LOITER:
+			*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_LOITER;
+			break;
+
+		case NAVIGATION_STATE_AUTO_RTL:
+		case NAVIGATION_STATE_AUTO_RTL_RC:
+		case NAVIGATION_STATE_AUTO_RTL_DL:
+			*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_RTL;
+			break;
 
-		} else if (pos_sp_triplet->nav_state == NAV_STATE_LAND) {
+		case NAVIGATION_STATE_LAND:
+			*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_LAND;
-		}
+			break;
+
+		case NAVIGATION_STATE_TERMINATION:
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
+			break;
+
 	}
 
 	*mavlink_custom_mode = custom_mode.data;
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 74694447ac..f30cd9a946 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -32,7 +32,6 @@
  ****************************************************************************/
 /**
  * @file navigator_main.cpp
- * Implementation of the main navigation state machine.
  *
  * Handles mission items, geo fencing and failsafe navigation behavior.
  * Published the position setpoint triplet for the position controller.
@@ -75,7 +74,6 @@
 
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
-#include <systemlib/state_table.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
 #include <geo/geo.h>
@@ -102,7 +100,7 @@ static const int ERROR = -1;
  */
 extern "C" __EXPORT int navigator_main(int argc, char *argv[]);
 
-class Navigator : public StateTable
+class Navigator
 {
 public:
 	/**
@@ -116,7 +114,7 @@ public:
 	~Navigator();
 
 	/**
-	* Start the navigator task.
+	 * Start the navigator task.
 	 *
 	 * @return		OK on success.
 	 */
@@ -200,24 +198,6 @@ private:
 		param_t takeoff_alt;
 	}		_parameter_handles;		/**< handles for parameters */
 
-	enum Event {
-		EVENT_NONE_REQUESTED,
-		EVENT_LOITER_REQUESTED,
-		EVENT_MISSION_REQUESTED,
-		EVENT_RTL_REQUESTED,
-		EVENT_TAKEN_OFF,
-		EVENT_LANDED,
-		EVENT_MISSION_CHANGED,
-		EVENT_HOME_POSITION_CHANGED,
-		EVENT_MISSION_ITEM_REACHED,
-		MAX_EVENT
-	};						/**< possible events that can be thrown at state machine */
-
-	/**
-	 * State machine transition table
-	 */
-	static StateTable::Tran const myTable[NAV_STATE_MAX][MAX_EVENT];
-
 	/**
 	 * Update our local parameter cache.
 	 */
@@ -268,41 +248,6 @@ private:
 	 */
 	void		task_main();
 
-	/**
-	 * Functions that are triggered when a new state is entered.
-	 */
-	bool		start_none_on_ground();
-	bool		start_none_in_air();
-	bool		start_auto_on_ground();
-	bool		start_loiter();
-	bool		start_mission();
-	bool		advance_mission();
-	bool		start_rtl();
-	bool		advance_rtl();
-	bool		start_land();
-
-	/**
-	 * Reset all "mission item reached" flags.
-	 */
-	void		reset_reached();
-
-	/**
-	 * Check if current mission item has been reached.
-	 */
-	bool		check_mission_item_reached();
-
-	/**
-	 * Set mission items by accessing the mission class.
-	 * If failing, tell the state machine to loiter.
-	 */
-	bool		set_mission_items();
-
-	/**
-	 * Set a RTL item by accessing the RTL class.
-	 * If failing, tell the state machine to loiter.
-	 */
-	void		set_rtl_item();
-
 	/**
 	 * Translate mission item to a position setpoint.
 	 */
@@ -329,7 +274,6 @@ Navigator	*g_navigator;
 Navigator::Navigator() :
 
 	/* state machine transition table */
-	StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT),
 	_task_should_exit(false),
 	_navigator_task(-1),
 	_mavlink_fd(-1),
@@ -368,11 +312,6 @@ Navigator::Navigator() :
 	_parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD");
 	_parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN");
 	_parameter_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT");
-
-	/* Initialize state machine */
-	myState = NAV_STATE_NONE_ON_GROUND;
-
-	start_none_on_ground();
 }
 
 Navigator::~Navigator()
@@ -615,41 +554,6 @@ Navigator::task_main()
 		/* vehicle status updated */
 		if (fds[6].revents & POLLIN) {
 			vehicle_status_update();
-
-			/* commander requested new navigation mode, forward it to state machine */
-			switch (_vstatus.set_nav_state) {
-			case NAVIGATION_STATE_NONE:
-				dispatch(EVENT_NONE_REQUESTED);
-				break;
-
-			case NAVIGATION_STATE_LOITER:
-				dispatch(EVENT_LOITER_REQUESTED);
-				break;
-
-			case NAVIGATION_STATE_MISSION:
-				dispatch(EVENT_MISSION_REQUESTED);
-				break;
-
-			case NAVIGATION_STATE_RTL:
-				dispatch(EVENT_RTL_REQUESTED);
-				break;
-
-			case NAVIGATION_STATE_LAND:
-				/* TODO: add and test this */
-				// dispatch(EVENT_LAND_REQUESTED);
-				break;
-
-			default:
-				warnx("ERROR: Requested navigation state not supported");
-				break;
-			}
-
-			/* commander sets in-air/on-ground flag as well */
-			if (_vstatus.condition_landed) {
-				dispatch(EVENT_LANDED);
-			} else {
-				dispatch(EVENT_TAKEN_OFF);
-			}
 		}
 
 		/* parameters updated */
@@ -666,30 +570,21 @@ Navigator::task_main()
 		/* offboard mission updated */
 		if (fds[4].revents & POLLIN) {
 			offboard_mission_update();
-			/* TODO: check if mission really changed */
-			dispatch(EVENT_MISSION_CHANGED);
 		}
 
 		/* onboard mission updated */
 		if (fds[5].revents & POLLIN) {
 			onboard_mission_update();
-			/* TODO: check if mission really changed */
-			dispatch(EVENT_MISSION_CHANGED);
 		}
 
 		/* home position updated */
 		if (fds[2].revents & POLLIN) {
 			home_position_update();
-			/* TODO check if home position really changed */
-			dispatch(EVENT_HOME_POSITION_CHANGED);
 		}
 
 		/* global position updated */
 		if (fds[1].revents & POLLIN) {
 			global_position_update();
-			if (check_mission_item_reached()) {
-				dispatch(EVENT_MISSION_ITEM_REACHED);
-			}
 
 			/* Check geofence violation */
 			if (!_geofence.inside(&_global_pos)) {
@@ -765,125 +660,8 @@ Navigator::status()
 	} else {
 		warnx("Geofence not set");
 	}
-
-	switch (myState) {
-	case NAV_STATE_NONE_ON_GROUND:
-		warnx("State: None on ground");
-		break;
-
-	case NAV_STATE_NONE_IN_AIR:
-		warnx("State: None in air");
-		break;
-
-	case NAV_STATE_LOITER:
-		warnx("State: Loiter");
-		break;
-
-	case NAV_STATE_MISSION:
-		warnx("State: Mission");
-		break;
-
-	case NAV_STATE_RTL:
-		warnx("State: RTL");
-		break;
-
-	case NAV_STATE_LAND:
-		warnx("State: Land");
-		break;
-
-	default:
-		warnx("State: Unknown");
-		break;
-	}
 }
-
-StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
-	{
-		/* NAV_STATE_NONE_ON_GROUND */
-		/* EVENT_NONE_REQUESTED */		{NO_ACTION, NAV_STATE_NONE_ON_GROUND},
-		/* EVENT_LOITER_REQUESTED */		{NO_ACTION, NAV_STATE_NONE_ON_GROUND},
-		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_auto_on_ground), NAV_STATE_AUTO_ON_GROUND},
-		/* EVENT_RTL_REQUESTED */		{NO_ACTION, NAV_STATE_NONE_ON_GROUND},
-		/* EVENT_TAKEN_OFF */			{ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR},
-		/* EVENT_LANDED */			{NO_ACTION, NAV_STATE_NONE_ON_GROUND},
-		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_NONE_ON_GROUND},
-		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_NONE_ON_GROUND},
-		/* EVENT_MISSION_ITEM_REACHED */	{NO_ACTION, NAV_STATE_NONE_ON_GROUND},
-	},
-	{
-		/* NAV_STATE_NONE_IN_AIR */
-		/* EVENT_NONE_REQUESTED */		{NO_ACTION, NAV_STATE_NONE_IN_AIR},
-		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
-		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
-		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
-		/* EVENT_TAKEN_OFF */			{NO_ACTION, NAV_STATE_NONE_IN_AIR},
-		/* EVENT_LANDED */			{ACTION(&Navigator::start_none_on_ground), NAV_STATE_NONE_ON_GROUND},
-		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_NONE_IN_AIR},
-		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_NONE_IN_AIR},
-		/* EVENT_MISSION_ITEM_REACHED */	{NO_ACTION, NAV_STATE_NONE_IN_AIR},
-	},
-	{
-		/* NAV_STATE_AUTO_ON_GROUND */
-		/* EVENT_NONE_REQUESTED */		{NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
-		/* EVENT_LOITER_REQUESTED */		{NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
-		/* EVENT_MISSION_REQUESTED */		{NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
-		/* EVENT_RTL_REQUESTED */		{NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
-		/* EVENT_TAKEN_OFF */			{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
-		/* EVENT_LANDED */			{NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
-		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
-		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
-		/* EVENT_MISSION_ITEM_REACHED */	{NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
-	},
-	{
-		/* NAV_STATE_LOITER */
-		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR},
-		/* EVENT_LOITER_REQUESTED */		{NO_ACTION, NAV_STATE_LOITER},
-		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
-		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
-		/* EVENT_TAKEN_OFF */			{NO_ACTION, NAV_STATE_LOITER},
-		/* EVENT_LANDED */			{NO_ACTION, NAV_STATE_LOITER},
-		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_LOITER},
-		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_LOITER},
-		/* EVENT_MISSION_ITEM_REACHED */	{NO_ACTION, NAV_STATE_LOITER},
-	},
-	{
-		/* NAV_STATE_MISSION */
-		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR},
-		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
-		/* EVENT_MISSION_REQUESTED */		{NO_ACTION, NAV_STATE_MISSION},
-		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
-		/* EVENT_TAKEN_OFF */			{NO_ACTION, NAV_STATE_MISSION},
-		/* EVENT_LANDED */			{NO_ACTION, NAV_STATE_MISSION},
-		/* EVENT_MISSION_CHANGED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
-		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_MISSION},
-		/* EVENT_MISSION_ITEM_REACHED */	{ACTION(&Navigator::advance_mission), NAV_STATE_MISSION},
-	},
-	{
-		/* NAV_STATE_RTL */
-		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR},
-		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
-		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
-		/* EVENT_RTL_REQUESTED */		{NO_ACTION, NAV_STATE_RTL},
-		/* EVENT_TAKEN_OFF */			{NO_ACTION, NAV_STATE_RTL},
-		/* EVENT_LANDED */			{NO_ACTION, NAV_STATE_RTL},
-		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_RTL},
-		/* EVENT_HOME_POSITION_CHANGED */	{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
-		/* EVENT_MISSION_ITEM_REACHED */	{ACTION(&Navigator::advance_rtl), NAV_STATE_RTL},
-	},
-	{
-		/* NAV_STATE_LAND */
-		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR},
-		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
-		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
-		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
-		/* EVENT_TAKEN_OFF */			{NO_ACTION, NAV_STATE_LAND},
-		/* EVENT_LANDED */			{ACTION(&Navigator::start_auto_on_ground), NAV_STATE_AUTO_ON_GROUND},
-		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_LAND},
-		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_LAND},
-		/* EVENT_MISSION_ITEM_REACHED */	{NO_ACTION, NAV_STATE_LAND},
-	},
-};
-
+#if 0
 bool
 Navigator::start_none_on_ground()
 {
@@ -984,13 +762,11 @@ Navigator::set_mission_items()
 	/* if we fail to set the current mission, continue to loiter */
 	if (!_mission.get_current_mission_item(&_mission_item, &onboard, &index)) {
 
-		dispatch(EVENT_LOITER_REQUESTED);
 		return false;
 	}
 
 	/* if we got an RTL mission item, switch to RTL mode and give up */
 	if (_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
-		dispatch(EVENT_RTL_REQUESTED);
 		return false;
 	}
 
@@ -1042,7 +818,6 @@ Navigator::start_rtl()
 	}
 
 	/* if RTL doesn't work, fallback to loiter */
-	dispatch(EVENT_LOITER_REQUESTED);
 	return false;
 }
 
@@ -1066,7 +841,6 @@ Navigator::advance_rtl()
 		return true;
 	}
 
-	dispatch(EVENT_LOITER_REQUESTED);
 	return false;
 }
 
@@ -1105,7 +879,7 @@ Navigator::start_land()
 	_update_triplet = true;
 	return true;
 }
-
+#endif
 void
 Navigator::mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp)
 {
@@ -1147,7 +921,7 @@ Navigator::mission_item_to_position_setpoint(const mission_item_s *item, positio
 		sp->type = SETPOINT_TYPE_NORMAL;
 	}
 }
-
+#if 0
 bool
 Navigator::check_mission_item_reached()
 {
@@ -1251,12 +1025,12 @@ Navigator::reset_reached()
 	_waypoint_yaw_reached = false;
 
 }
-
+#endif
 void
 Navigator::publish_position_setpoint_triplet()
 {
 	/* update navigation state */
-	_pos_sp_triplet.nav_state = static_cast<nav_state_t>(myState);
+	/* TODO: set nav_state */
 
 	/* lazily publish the position setpoint triplet only once available */
 	if (_pos_sp_triplet_pub > 0) {
diff --git a/src/modules/systemlib/state_table.h b/src/modules/systemlib/state_table.h
index 38378651b8..e6011fdef2 100644
--- a/src/modules/systemlib/state_table.h
+++ b/src/modules/systemlib/state_table.h
@@ -33,7 +33,7 @@
 
 /**
  * @file state_table.h
- * 
+ *
  * Finite-State-Machine helper class for state table
  * @author: Julian Oes <julian@oes.ch>
  */
@@ -44,32 +44,32 @@
 class StateTable
 {
 public:
-	typedef bool (StateTable::*Action)();
+	typedef void (StateTable::*Action)();
 	struct Tran {
 		Action action;
 		unsigned nextState;
 	};
-	
+
 	StateTable(Tran const *table, unsigned nStates, unsigned nSignals)
 	: myTable(table), myNsignals(nSignals), myNstates(nStates) {}
-	
+
 	#define NO_ACTION &StateTable::doNothing
 	#define ACTION(_target) StateTable::Action(_target)
 
 	virtual ~StateTable() {}
-	
+
 	void dispatch(unsigned const sig) {
 		/* get transition using state table */
 		Tran const *t = myTable + myState*myNsignals + sig;
-		/* first up change state, this allows to do further dispatchs in the state functions */
-		
-		/* now execute state function, if it runs with success, accept new state */
-		if ((this->*(t->action))()) {
-			myState = t->nextState;
-		}
+
+		/* accept new state */
+		myState = t->nextState;
+
+		/*  */
+		(this->*(t->action))();
 	}
-	bool doNothing() {
-		return true;
+	void doNothing() {
+		return;
 	}
 protected:
 	unsigned myState;
@@ -79,4 +79,4 @@ private:
 	unsigned myNstates;
 };
 
-#endif
\ No newline at end of file
+#endif
diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h
index 7c3921198a..ad654a9ff3 100644
--- a/src/modules/uORB/topics/mission_result.h
+++ b/src/modules/uORB/topics/mission_result.h
@@ -56,6 +56,7 @@ struct mission_result_s
 	bool mission_reached;		/**< true if mission has been reached			*/
 	unsigned mission_index_reached;	/**< index of the mission which has been reached	*/
 	unsigned index_current_mission; /**< index of the current mission			*/
+	bool mission_finished;		/**< true if mission has been completed			*/
 };
 
 /**
diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h
index 85e8ef8a54..c2741a05bd 100644
--- a/src/modules/uORB/topics/position_setpoint_triplet.h
+++ b/src/modules/uORB/topics/position_setpoint_triplet.h
@@ -74,14 +74,13 @@ struct position_setpoint_s
 };
 
 typedef enum {
-	NAV_STATE_NONE_ON_GROUND = 0,
-	NAV_STATE_NONE_IN_AIR,
-	NAV_STATE_AUTO_ON_GROUND,
-	NAV_STATE_LOITER,
-	NAV_STATE_MISSION,
-	NAV_STATE_RTL,
-	NAV_STATE_LAND,
-	NAV_STATE_MAX
+	NAV_STATE_MANUAL = 0,
+	NAV_STATE_POSCTL,
+	NAV_STATE_AUTO,
+	NAV_STATE_RC_LOSS,
+	NAV_STATE_DL_LOSS,
+	NAV_STATE_TERMINATION,
+	MAX_NAV_STATE
 } nav_state_t;
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index d902dc49e4..259d7329e8 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -63,9 +63,11 @@ typedef enum {
 	MAIN_STATE_MANUAL = 0,
 	MAIN_STATE_ALTCTL,
 	MAIN_STATE_POSCTL,
-	MAIN_STATE_AUTO,
+	MAIN_STATE_AUTO_MISSION,
+	MAIN_STATE_AUTO_LOITER,
+	MAIN_STATE_AUTO_RTL,
 	MAIN_STATE_ACRO,
-	MAIN_STATE_MAX
+	MAIN_STATE_MAX,
 } main_state_t;
 
 // If you change the order, add or remove arming_state_t states make sure to update the arrays
@@ -78,7 +80,7 @@ typedef enum {
 	ARMING_STATE_STANDBY_ERROR,
 	ARMING_STATE_REBOOT,
 	ARMING_STATE_IN_AIR_RESTORE,
-	ARMING_STATE_MAX
+	ARMING_STATE_MAX,
 } arming_state_t;
 
 typedef enum {
@@ -88,18 +90,25 @@ typedef enum {
 
 typedef enum {
 	FAILSAFE_STATE_NORMAL = 0,		/**< Normal operation */
-	FAILSAFE_STATE_RTL,				/**< Return To Launch */
-	FAILSAFE_STATE_LAND,			/**< Land without position control */
+	FAILSAFE_STATE_RTL_RC,			/**< Return To Launch on remote control loss */
+	FAILSAFE_STATE_RTL_DL,			/**< Return To Launch on datalink loss */
+	FAILSAFE_STATE_LAND,			/**< Land as safe as possible */
 	FAILSAFE_STATE_TERMINATION,		/**< Disable motors and use parachute, can't be recovered */
-	FAILSAFE_STATE_MAX
+	FAILSAFE_STATE_MAX,
 } failsafe_state_t;
 
 typedef enum {
-	NAVIGATION_STATE_NONE = 0,
-	NAVIGATION_STATE_MISSION,
-	NAVIGATION_STATE_LOITER,
-	NAVIGATION_STATE_RTL,
-	NAVIGATION_STATE_LAND
+	NAVIGATION_STATE_MANUAL = 0,
+	NAVIGATION_STATE_ACRO,
+	NAVIGATION_STATE_ALTCTL,
+	NAVIGATION_STATE_POSCTL,
+	NAVIGATION_STATE_AUTO_MISSION,
+	NAVIGATION_STATE_AUTO_LOITER,
+	NAVIGATION_STATE_AUTO_RTL,
+	NAVIGATION_STATE_AUTO_RTL_RC,
+	NAVIGATION_STATE_AUTO_RTL_DL,
+	NAVIGATION_STATE_LAND,
+	NAVIGATION_STATE_TERMINATION,
 } navigation_state_t;
 
 enum VEHICLE_MODE_FLAG {
@@ -160,10 +169,10 @@ struct vehicle_status_s {
 	uint16_t counter;   /**< incremented by the writing thread everytime new data is stored */
 	uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
 
-	main_state_t main_state;				/**< main state machine */
+	main_state_t main_state;		    	/**< main state machine */
 	navigation_state_t set_nav_state;		/**< set navigation state machine to specified value */
 	arming_state_t arming_state;			/**< current arming state */
-	hil_state_t hil_state;					/**< current hil state */
+	hil_state_t hil_state;				/**< current hil state */
 	failsafe_state_t failsafe_state;		/**< current failsafe state */
 
 	int32_t system_type;				/**< system type, inspired by MAVLink's VEHICLE_TYPE enum */

From c8903b167230bc2efdb908e78135a0fa0abc745c Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 27 May 2014 23:24:01 +0200
Subject: [PATCH 52/97] commander: modes and RC loss working now

---
 src/modules/commander/commander.cpp | 69 ++++++++++++++++-------------
 1 file changed, 37 insertions(+), 32 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 908384bb65..5638bc09fc 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -625,8 +625,10 @@ int commander_thread_main(int argc, char *argv[])
 	main_states_str[0] = "MANUAL";
 	main_states_str[1] = "ALTCTL";
 	main_states_str[2] = "POSCTL";
-	main_states_str[3] = "AUTO";
-	main_states_str[4] = "ACRO";
+	main_states_str[3] = "AUTO_MISSION";
+	main_states_str[4] = "AUTO_LOITER";
+	main_states_str[5] = "AUTO_RTL";
+	main_states_str[6] = "ACRO";
 
 	char *arming_states_str[ARMING_STATE_MAX];
 	arming_states_str[0] = "INIT";
@@ -639,9 +641,10 @@ int commander_thread_main(int argc, char *argv[])
 
 	char *failsafe_states_str[FAILSAFE_STATE_MAX];
 	failsafe_states_str[0] = "NORMAL";
-	failsafe_states_str[1] = "RTL";
-	failsafe_states_str[2] = "LAND";
-	failsafe_states_str[3] = "TERMINATION";
+	failsafe_states_str[1] = "RTL_RC";
+	failsafe_states_str[2] = "RTL_DL";
+	failsafe_states_str[3] = "LAND";
+	failsafe_states_str[4] = "TERMINATION";
 
 	/* pthread for slow low prio thread */
 	pthread_t commander_low_prio_thread;
@@ -1151,12 +1154,9 @@ int commander_thread_main(int argc, char *argv[])
 		/* If in INIT state, try to proceed to STANDBY state */
 		if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
 			/* TODO: check for sensors */
-			warnx("arming status1: %d", status.arming_state);
 			arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
-			warnx("arming status2: %d", status.arming_state);
 
 			if (arming_ret == TRANSITION_CHANGED) {
-				warnx("changed");
 				arming_state_changed = true;
 			}
 
@@ -1198,6 +1198,9 @@ int commander_thread_main(int argc, char *argv[])
 				if (status.rc_signal_lost) {
 					mavlink_log_critical(mavlink_fd, "#audio: RC signal regained");
 					status_changed = true;
+
+					status.failsafe_state = FAILSAFE_STATE_NORMAL;
+					failsafe_state_changed = true;
 				}
 			}
 
@@ -1215,7 +1218,6 @@ int commander_thread_main(int argc, char *argv[])
 					arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
 					arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed);
 					if (arming_ret == TRANSITION_CHANGED) {
-						warnx("stick 1");
 						arming_state_changed = true;
 					}
 					stick_off_counter = 0;
@@ -1241,7 +1243,6 @@ int commander_thread_main(int argc, char *argv[])
 					} else {
 						arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
 						if (arming_ret == TRANSITION_CHANGED) {
-							warnx("stick 2");
 							arming_state_changed = true;
 						}
 					}
@@ -1294,29 +1295,19 @@ int commander_thread_main(int argc, char *argv[])
 				mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST");
 				status.rc_signal_lost = true;
 				status_changed = true;
-			}
-		}
 
-		if (armed.armed) {
-			/* if RC is lost, switch to RTL_RC or Termination unless a mission is running
-			 * and not yet finished) */
-			if (status.rc_signal_lost
-		            && !(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.mission_finished)) {
 
-				/* if we have a global position, we can switch to RTL, if not, we can try to land */
-				if (status.condition_global_position_valid) {
-					status.failsafe_state = FAILSAFE_STATE_RTL_RC;
-				} else {
-					status.failsafe_state = FAILSAFE_STATE_LAND;
+				if (!(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.mission_finished)) {
+
+					/* if we have a global position, we can switch to RTL, if not, we can try to land */
+					if (status.condition_global_position_valid) {
+						status.failsafe_state = FAILSAFE_STATE_RTL_RC;
+					} else {
+						status.failsafe_state = FAILSAFE_STATE_LAND;
+					}
+					failsafe_state_changed = true;
 				}
 			}
-
-			/* if the data link is lost, switch to RTL_DL or Termination */
-			/* TODO: add this */
-
-		} else {
-			/* reset failsafe when disarmed */
-			status.failsafe_state = FAILSAFE_STATE_NORMAL;
 		}
 
 		/* handle commands last, as the system needs to be updated to handle them */
@@ -1613,10 +1604,24 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
 		break;
 
 	case SWITCH_POS_ON:			// AUTO
-		res = main_state_transition(status, MAIN_STATE_AUTO_MISSION);
+		if (sp_man->return_switch == SWITCH_POS_ON) {
+			res = main_state_transition(status, MAIN_STATE_AUTO_RTL);
 
-		if (res != TRANSITION_DENIED) {
-			break;	// changed successfully or already in this state
+			if (res != TRANSITION_DENIED) {
+				break;	// changed successfully or already in this state
+			}
+		} else if (sp_man->loiter_switch == SWITCH_POS_ON) {
+			res = main_state_transition(status, MAIN_STATE_AUTO_LOITER);
+
+			if (res != TRANSITION_DENIED) {
+				break;	// changed successfully or already in this state
+			}
+		} else {
+			res = main_state_transition(status, MAIN_STATE_AUTO_MISSION);
+
+			if (res != TRANSITION_DENIED) {
+				break;	// changed successfully or already in this state
+			}
 		}
 
 		// else fallback to ALTCTL (POSCTL likely will not work too)

From 854bb7fe089daf8bf3d4e9f2cac1cb2b99a67ac7 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 3 Jun 2014 16:01:28 +0200
Subject: [PATCH 53/97] navigator: mission class added (WIP)

---
 .../fw_pos_control_l1_main.cpp                |   2 +-
 src/modules/mavlink/mavlink_main.cpp          |   2 +-
 src/modules/navigator/mission.cpp             | 480 ++++++++++++------
 src/modules/navigator/mission.h               | 164 ++++--
 src/modules/navigator/module.mk               |   1 -
 src/modules/navigator/navigator.h             | 205 ++++++++
 src/modules/navigator/navigator_main.cpp      | 338 ++----------
 src/modules/navigator/rtl.cpp                 |  31 +-
 src/modules/navigator/rtl.h                   |  21 +-
 src/modules/sdlog2/sdlog2.c                   |   2 +-
 src/modules/uORB/topics/mission.h             |  12 +-
 .../uORB/topics/position_setpoint_triplet.h   |  30 +-
 12 files changed, 770 insertions(+), 518 deletions(-)
 create mode 100644 src/modules/navigator/navigator.h

diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index a53118dec3..b8d94fe184 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -861,7 +861,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 		}
 
-		if (pos_sp_triplet.current.type == SETPOINT_TYPE_NORMAL) {
+		if (pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION) {
 			/* waypoint is a plain navigation waypoint */
 			_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed);
 			_att_sp.roll_body = _l1_control.nav_roll();
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 6b45736e93..48f91481d2 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -415,7 +415,7 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self)
 void
 Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self)
 {
-	
+
 	Mavlink *inst;
 	LL_FOREACH(_mavlink_instances, inst) {
 		if (inst != self) {
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index adbb0cfa23..f3a86666f0 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -38,30 +38,42 @@
  * @author Julian Oes <julian@oes.ch>
  */
 
+#include <sys/types.h>
 #include <string.h>
 #include <stdlib.h>
+#include <unistd.h>
+
+#include <drivers/drv_hrt.h>
 
 #include <dataman/dataman.h>
 #include <systemlib/err.h>
+#include <geo/geo.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/mission_result.h>
 
+#include "navigator.h"
 #include "mission.h"
 
 
-Mission::Mission() :
-
-	_offboard_dataman_id(-1),
-	_current_offboard_mission_index(0),
-	_current_onboard_mission_index(0),
-	_offboard_mission_item_count(0),
-	_onboard_mission_item_count(0),
-	_onboard_mission_allowed(false),
-	_current_mission_type(MISSION_TYPE_NONE),
+Mission::Mission(Navigator *navigator) :
+	SuperBlock(NULL, "MIS"),
+	_navigator(navigator),
+	_first_run(true),
+	_param_onboard_enabled(this, "ONBOARD_EN"),
+	_onboard_mission_sub(-1),
+	_offboard_mission_sub(-1),
+	_onboard_mission({0}),
+	_offboard_mission({0}),
+	_mission_item({0}),
 	_mission_result_pub(-1),
-	_mission_result({})
+	_mission_result({0}),
+	_mission_type(MISSION_TYPE_NONE)
 {
+	/* load initial params */
+	updateParams();
+	/* set initial mission items */
+	reset();
 }
 
 Mission::~Mission()
@@ -69,134 +81,335 @@ Mission::~Mission()
 }
 
 void
-Mission::set_offboard_dataman_id(const int new_id)
+Mission::reset()
 {
-	_offboard_dataman_id = new_id;
-}
-
-void
-Mission::set_offboard_mission_count(int new_count)
-{
-	_offboard_mission_item_count = new_count;
-}
-
-void
-Mission::set_onboard_mission_count(int new_count)
-{
-	_onboard_mission_item_count = new_count;
-}
-
-void
-Mission::set_onboard_mission_allowed(bool allowed)
-{
-	_onboard_mission_allowed = allowed;
+	_first_run = true;
 }
 
 bool
-Mission::command_current_offboard_mission_index(const int new_index)
+Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet)
 {
-	if (new_index >= 0) {
-		_current_offboard_mission_index = (unsigned)new_index;
-	} else {
+	bool updated = false;
+	/* check if anything has changed, and reset mission items if needed */
+	if (is_onboard_mission_updated() || is_offboard_mission_updated() || _first_run) {
+		set_mission_items(pos_sp_triplet);
+		updated = true;
+		_first_run = false;
+	}
 
-		/* if less WPs available, reset to first WP */
-		if (_current_offboard_mission_index >= _offboard_mission_item_count) {
-			_current_offboard_mission_index = 0;
+	if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
+		advance_mission();
+		set_mission_items(pos_sp_triplet);
+		updated = true;
+	}
+	return updated;
+}
+
+bool
+Mission::is_mission_item_reached()
+{
+	/* TODO: count turns */
+#if 0
+	if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
+	     _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+	     _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
+	    _mission_item.loiter_radius > 0.01f) {
+
+		return false;
+	}
+#endif
+
+	hrt_abstime now = hrt_absolute_time();
+
+	if (!_waypoint_position_reached) {
+
+		float dist = -1.0f;
+		float dist_xy = -1.0f;
+		float dist_z = -1.0f;
+
+		float altitude_amsl = _mission_item.altitude_is_relative
+				      ? _mission_item.altitude + _navigator->get_home_position()->alt
+			              : _mission_item.altitude;
+
+		dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl,
+				                          _navigator->get_global_position()->lat,
+							  _navigator->get_global_position()->lon,
+							  _navigator->get_global_position()->alt,
+				&dist_xy, &dist_z);
+
+		if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
+
+			/* require only altitude for takeoff */
+			if (_navigator->get_global_position()->alt > altitude_amsl - _mission_item.acceptance_radius) {
+				_waypoint_position_reached = true;
+			}
+		} else {
+			if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) {
+				_waypoint_position_reached = true;
+			}
 		}
 	}
-	report_current_offboard_mission_item();
-}
 
-bool
-Mission::command_current_onboard_mission_index(int new_index)
-{
-	if (new_index != -1) {
-		_current_onboard_mission_index = (unsigned)new_index;
-	} else {
+	if (_waypoint_position_reached && !_waypoint_yaw_reached) {
 
-		/* if less WPs available, reset to first WP */
-		if (_current_onboard_mission_index >= _onboard_mission_item_count) {
-			_current_onboard_mission_index = 0;
+		/* TODO: removed takeoff, why? */
+		if (_navigator->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) {
+
+			/* check yaw if defined only for rotary wing except takeoff */
+			float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw);
+
+			if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */
+				_waypoint_yaw_reached = true;
+			}
+
+		} else {
+			_waypoint_yaw_reached = true;
 		}
 	}
-	// TODO: implement this for onboard missions as well
-	// report_current_mission_item();
-}
 
-bool
-Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, int *index)
-{
-	*onboard = false;
-	*index = -1;
+	/* check if the current waypoint was reached */
+	if (_waypoint_position_reached && _waypoint_yaw_reached) {
 
-	/* try onboard mission first */
-	if (_current_onboard_mission_index < _onboard_mission_item_count && _onboard_mission_allowed) {
-		if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index, new_mission_item)) {
-			_current_mission_type = MISSION_TYPE_ONBOARD;
-			*onboard = true;
-			*index = _current_onboard_mission_index;
+		if (_time_first_inside_orbit == 0) {
+			_time_first_inside_orbit = now;
 
+			// if (_mission_item.time_inside > 0.01f) {
+			// 	mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs",
+			// 		(double)_mission_item.time_inside);
+			// }
+		}
+
+		/* check if the MAV was long enough inside the waypoint orbit */
+		if (now - _time_first_inside_orbit >= (hrt_abstime)_mission_item.time_inside * 1e6f) {
 			return true;
 		}
 	}
+	return false;
+}
 
-	/* otherwise fallback to offboard */
-	if (_current_offboard_mission_index < _offboard_mission_item_count) {
+void
+Mission::reset_mission_item_reached() {
+	_waypoint_position_reached = false;
+	_waypoint_yaw_reached = false;
+	_time_first_inside_orbit = 0;
+}
 
+void
+Mission::mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp)
+{
+	sp->valid = true;
+
+	sp->lat = item->lat;
+	sp->lon = item->lon;
+	sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude;
+	sp->yaw = item->yaw;
+	sp->loiter_radius = item->loiter_radius;
+	sp->loiter_direction = item->loiter_direction;
+	sp->pitch_min = item->pitch_min;
+
+	if (item->nav_cmd == NAV_CMD_TAKEOFF) {
+		sp->type = SETPOINT_TYPE_TAKEOFF;
+
+	} else if (item->nav_cmd == NAV_CMD_LAND) {
+		sp->type = SETPOINT_TYPE_LAND;
+
+	} else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+		   item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
+		   item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
+		sp->type = SETPOINT_TYPE_LOITER;
+
+	} else {
+		sp->type = SETPOINT_TYPE_POSITION;
+	}
+}
+
+bool
+Mission::is_onboard_mission_updated()
+{
+	bool updated;
+	orb_check(_onboard_mission_sub, &updated);
+
+	if (!updated) {
+		return false;
+	}
+
+	if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &_onboard_mission) != OK) {
+		_onboard_mission.count = 0;
+		_onboard_mission.current_index = 0;
+	}
+	return true;
+}
+
+bool
+Mission::is_offboard_mission_updated()
+{
+	bool updated;
+	orb_check(_offboard_mission_sub, &updated);
+	if (!updated) {
+		return false;
+	}
+
+	if (orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &_offboard_mission) == OK) {
+
+		/* Check mission feasibility, for now do not handle the return value,
+		 * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
 		dm_item_t dm_current;
-		if (_offboard_dataman_id == 0) {
+
+		if (_offboard_mission.dataman_id == 0) {
 			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
 		} else {
 			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
 		}
-		if (read_mission_item(dm_current, true, &_current_offboard_mission_index, new_mission_item)) {
 
-			_current_mission_type = MISSION_TYPE_OFFBOARD;
-			*onboard = false;
-			*index = _current_offboard_mission_index;
-
-			return true;
-		}
-	}
-
-	/* happens when no more mission items can be added as a next item */
-	_current_mission_type = MISSION_TYPE_NONE;
-
-	return false;
-}
-
-bool
-Mission::get_next_mission_item(struct mission_item_s *new_mission_item)
-{
-	int next_temp_mission_index = _current_onboard_mission_index + 1;
-
-	/* try onboard mission first */
-	if (next_temp_mission_index < _onboard_mission_item_count && _onboard_mission_allowed) {
-		if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, new_mission_item)) {
-			return true;
-		}
-	}
-
-	/* then try offboard mission */
-	dm_item_t dm_current;
-	if (_offboard_dataman_id == 0) {
-		dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+		missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current,
+			                                      (size_t)_offboard_mission.count,
+							      _navigator->get_geofence());
 	} else {
-		dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
+		_offboard_mission.count = 0;
+		_offboard_mission.current_index = 0;
 	}
-	next_temp_mission_index = _current_offboard_mission_index + 1;
-	if (next_temp_mission_index < _offboard_mission_item_count) {
-		if (read_mission_item(dm_current, false, &next_temp_mission_index, new_mission_item)) {
+	return true;
+}
+
+
+void
+Mission::advance_mission()
+{
+	switch (_mission_type) {
+	case MISSION_TYPE_ONBOARD:
+		_onboard_mission.current_index++;
+		break;
+
+	case MISSION_TYPE_OFFBOARD:
+		_offboard_mission.current_index++;
+		break;
+
+	case MISSION_TYPE_NONE:
+	default:
+		break;
+	}
+}
+
+void
+Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
+{
+	warnx("set mission items");
+
+	set_previous_pos_setpoint(&pos_sp_triplet->current, &pos_sp_triplet->previous);
+
+	if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) {
+		/* try setting onboard mission item */
+		_mission_type = MISSION_TYPE_ONBOARD;
+
+	} else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) {
+		/* try setting offboard mission item */
+		_mission_type = MISSION_TYPE_OFFBOARD;
+	} else {
+		_mission_type = MISSION_TYPE_NONE;
+	}
+}
+
+void
+Mission::set_previous_pos_setpoint(const struct position_setpoint_s *current_pos_sp,
+		                   struct position_setpoint_s *previous_pos_sp)
+{
+	/* reuse current setpoint as previous setpoint */
+	if (current_pos_sp->valid) {
+		memcpy(previous_pos_sp, current_pos_sp, sizeof(struct position_setpoint_s));
+	}
+}
+
+bool
+Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
+{
+	if (_param_onboard_enabled.get() > 0
+	    && _onboard_mission.current_index < (int)_onboard_mission.count) {
+		struct mission_item_s new_mission_item;
+		if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_onboard_mission.current_index,
+					&new_mission_item)) {
+			/* convert the current mission item and set it valid */
+			mission_item_to_position_setpoint(&_mission_item, current_pos_sp);
+			current_pos_sp->valid = true;
+
+			/* TODO: report this somehow */
 			return true;
 		}
 	}
-
-	/* both failed, bail out */
 	return false;
 }
 
 bool
-Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, struct mission_item_s *new_mission_item)
+Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
+{
+	if (_offboard_mission.current_index < (int)_offboard_mission.count) {
+		dm_item_t dm_current;
+		if (_offboard_mission.dataman_id == 0) {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+		} else {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
+		}
+		struct mission_item_s new_mission_item;
+		if (read_mission_item(dm_current, true, &_offboard_mission.current_index, &new_mission_item)) {
+			/* convert the current mission item and set it valid */
+			mission_item_to_position_setpoint(&_mission_item, current_pos_sp);
+			current_pos_sp->valid = true;
+
+			report_current_offboard_mission_item();
+			return true;
+		}
+	}
+	return false;
+}
+
+void
+Mission::get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp)
+{
+	int next_temp_mission_index = _onboard_mission.current_index + 1;
+
+	/* try if there is a next onboard mission */
+	if (next_temp_mission_index < (int)_onboard_mission.count) {
+		struct mission_item_s new_mission_item;
+		if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, &new_mission_item)) {
+			/* convert next mission item to position setpoint */
+			mission_item_to_position_setpoint(&new_mission_item, next_pos_sp);
+			next_pos_sp->valid = true;
+			return;
+		}
+	}
+
+	/* give up */
+	next_pos_sp->valid = false;
+	return;
+}
+
+void
+Mission::get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp)
+{
+	/* try if there is a next offboard mission */
+	int next_temp_mission_index = _offboard_mission.current_index + 1;
+	if (next_temp_mission_index < (int)_offboard_mission.count) {
+		dm_item_t dm_current;
+		if (_offboard_mission.dataman_id == 0) {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+		} else {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
+		}
+		struct mission_item_s new_mission_item;
+		if (read_mission_item(dm_current, false, &next_temp_mission_index, &new_mission_item)) {
+			/* convert next mission item to position setpoint */
+			mission_item_to_position_setpoint(&new_mission_item, next_pos_sp);
+			next_pos_sp->valid = true;
+			return;
+		}
+	}
+	/* give up */
+	next_pos_sp->valid = false;
+	return;
+}
+
+bool
+Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index,
+		           struct mission_item_s *new_mission_item)
 {
 	/* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */
 	for (int i=0; i<10; i++) {
@@ -211,26 +424,26 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio
 		/* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */
 		if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) {
 
-			if (new_mission_item->do_jump_current_count < new_mission_item->do_jump_repeat_count) {
-
-				/* only raise the repeat count if this is for the current mission item
-				 * but not for the next mission item */
-				if (is_current) {
-					(new_mission_item->do_jump_current_count)++;
-
-					/* save repeat count */
-					if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET, new_mission_item, len) != len) {
-						/* not supposed to happen unless the datamanager can't access the SD card, etc. */
-						return false;
-					}
-				}
-				/* set new mission item index and repeat
-				 * we don't have to validate here, if it's invalid, we should realize this later .*/
-				*mission_index = new_mission_item->do_jump_mission_index;
-			} else {
+			if (new_mission_item->do_jump_current_count >= new_mission_item->do_jump_repeat_count) {
 				return false;
 			}
 
+			/* only raise the repeat count if this is for the current mission item
+			 * but not for the next mission item */
+			if (is_current) {
+				(new_mission_item->do_jump_current_count)++;
+
+				/* save repeat count */
+				if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET,
+					     new_mission_item, len) != len) {
+					/* not supposed to happen unless the datamanager can't access the dataman */
+					return false;
+				}
+			}
+			/* set new mission item index and repeat
+			 * we don't have to validate here, if it's invalid, we should realize this later .*/
+			*mission_index = new_mission_item->do_jump_mission_index;
+
 		} else {
 			/* if it's not a DO_JUMP, then we were successful */
 			return true;
@@ -242,32 +455,12 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio
 	return false;
 }
 
-void
-Mission::move_to_next()
-{
-	report_mission_item_reached();
-
-	switch (_current_mission_type) {
-	case MISSION_TYPE_ONBOARD:
-		_current_onboard_mission_index++;
-		break;
-
-	case MISSION_TYPE_OFFBOARD:
-		_current_offboard_mission_index++;
-		break;
-
-	case MISSION_TYPE_NONE:
-	default:
-		break;
-	}
-}
-
 void
 Mission::report_mission_item_reached()
 {
-	if (_current_mission_type == MISSION_TYPE_OFFBOARD) {
+	if (_mission_type == MISSION_TYPE_OFFBOARD) {
 		_mission_result.mission_reached = true;
-		_mission_result.mission_index_reached = _current_offboard_mission_index;
+		_mission_result.mission_index_reached = _offboard_mission.current_index;
 	}
 	publish_mission_result();
 }
@@ -275,7 +468,7 @@ Mission::report_mission_item_reached()
 void
 Mission::report_current_offboard_mission_item()
 {
-	_mission_result.index_current_mission = _current_offboard_mission_index;
+	_mission_result.index_current_mission = _offboard_mission.current_index;
 	publish_mission_result();
 }
 
@@ -294,4 +487,3 @@ Mission::publish_mission_result()
 	/* reset reached bool */
 	_mission_result.mission_reached = false;
 }
-
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index 5e0e9702dc..e86dd25bb4 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -31,70 +31,172 @@
  *
  ****************************************************************************/
 /**
- * @file navigator_mission.h
+ * @file mission.h
  * Helper class to access missions
- * @author Julian Oes <julian@oes.ch>
  *
- * @author Julian Oes <joes@student.ethz.ch>
+ * @author Julian Oes <julian@oes.ch>
  */
 
 #ifndef NAVIGATOR_MISSION_H
 #define NAVIGATOR_MISSION_H
 
-#include <uORB/topics/mission.h>
-#include <uORB/topics/mission_result.h>
+#include <drivers/drv_hrt.h>
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
 #include <dataman/dataman.h>
 
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/mission_result.h>
 
-class __EXPORT Mission
+#include "mission_feasibility_checker.h"
+
+class Navigator;
+
+class Mission : public control::SuperBlock
 {
 public:
 	/**
 	 * Constructor
 	 */
-	Mission();
+	Mission(Navigator *navigator);
 
 	/**
 	 * Destructor
 	 */
-	~Mission();
+	virtual ~Mission();
 
-	void		set_offboard_dataman_id(const int new_id);
-	void		set_offboard_mission_count(const int new_count);
-	void		set_onboard_mission_count(const int new_count);
-	void		set_onboard_mission_allowed(const bool allowed);
+	/**
+	 * This function is called while the mode is inactive
+	 */
+	virtual void reset();
 
-	bool		command_current_offboard_mission_index(const int new_index);
-	bool		command_current_onboard_mission_index(const int new_index);
+	/**
+	 * This function is called while the mode is active
+	 */
+	virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet);
 
-	bool		get_current_mission_item(struct mission_item_s *mission_item, bool *onboard, int *index);
-	bool		get_next_mission_item(struct mission_item_s *mission_item);
+protected:
+	/**
+	 * Check if mission item has been reached
+	 * @return true if successfully reached
+	 */
+	bool is_mission_item_reached();
+	/**
+	 * Reset all reached flags
+	 */
+	void reset_mission_item_reached();
 
-	void		move_to_next();
+	/**
+	 * Convert a mission item to a position setpoint
+	 */
+	void mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp);
+
+	class Navigator *_navigator;
+
+	bool _waypoint_position_reached;
+	bool _waypoint_yaw_reached;
+	hrt_abstime _time_first_inside_orbit;
+
+	bool _first_run;
 
 private:
-	bool		read_mission_item(const dm_item_t dm_item, const bool is_current, int *mission_index, struct mission_item_s *new_mission_item);
+	/**
+	 * Update onboard mission topic
+	 * @return true if onboard mission has been updated
+	 */
+	bool is_onboard_mission_updated();
 
-	void		report_mission_item_reached();
-	void		report_current_offboard_mission_item();
+	/**
+	 * Update offboard mission topic
+	 * @return true if offboard mission has been updated
+	 */
+	bool is_offboard_mission_updated();
 
-	void		publish_mission_result();
+	/**
+	 * Move on to next mission item or switch to loiter
+	 */
+	void advance_mission();
 
-	int 		_offboard_dataman_id;
-	int		_current_offboard_mission_index;
-	int		_current_onboard_mission_index;
-	int		_offboard_mission_item_count;		/** number of offboard mission items available */
-	int		_onboard_mission_item_count;		/** number of onboard mission items available */
-	bool		_onboard_mission_allowed;
+	/**
+	 * Set new mission items
+	 */
+	void set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet);
+
+	/**
+	 * Set previous position setpoint
+	 */
+	void set_previous_pos_setpoint(const struct position_setpoint_s *current_pos_sp,
+			               struct position_setpoint_s *previous_pos_sp);
+
+	/**
+	 * Try to set the current position setpoint from an onboard mission item
+	 * @return true if mission item successfully set
+	 */
+	bool is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp);
+
+	/**
+	 * Try to set the current position setpoint from an offboard mission item
+	 * @return true if mission item successfully set
+	 */
+	bool is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp);
+
+	/**
+	 * Try to set the next position setpoint from an onboard mission item
+	 */
+	void get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp);
+
+	/**
+	 * Try to set the next position setpoint from an offboard mission item
+	 */
+	void get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp);
+
+	/**
+	 * Read a mission item from the dataman and watch out for DO_JUMPS
+	 * @return true if successful
+	 */
+	bool read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index,
+			       struct mission_item_s *new_mission_item);
+
+	/**
+	 * Report that a mission item has been reached
+	 */
+	void report_mission_item_reached();
+
+	/**
+	 * Rport the current mission item
+	 */
+	void report_current_offboard_mission_item();
+
+	/**
+	 * Publish the mission result so commander and mavlink know what is going on
+	 */
+	void publish_mission_result();
+
+	control::BlockParamFloat _param_onboard_enabled;
+
+	int _onboard_mission_sub;
+	int _offboard_mission_sub;
+
+	struct mission_s _onboard_mission;
+	struct mission_s _offboard_mission;
+
+	struct mission_item_s _mission_item;
+
+	orb_advert_t _mission_result_pub;
+	struct mission_result_s _mission_result;
 
 	enum {
 		MISSION_TYPE_NONE,
 		MISSION_TYPE_ONBOARD,
-		MISSION_TYPE_OFFBOARD,
-	} 		_current_mission_type;
+		MISSION_TYPE_OFFBOARD
+	} _mission_type;
+
+	MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */
 
-	orb_advert_t	 _mission_result_pub;			/**< publish mission result topic */
-	mission_result_s _mission_result;			/**< mission result for commander/mavlink */
 };
 
 #endif
diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk
index 8913d4d1cf..2d07bc49c9 100644
--- a/src/modules/navigator/module.mk
+++ b/src/modules/navigator/module.mk
@@ -41,7 +41,6 @@ SRCS		= navigator_main.cpp \
 		  navigator_params.c \
 		  mission.cpp \
 		  rtl.cpp \
-		  rtl_params.c \
 		  mission_feasibility_checker.cpp \
 		  geofence.cpp \
 		  geofence_params.c
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
new file mode 100644
index 0000000000..1838fe32b0
--- /dev/null
+++ b/src/modules/navigator/navigator.h
@@ -0,0 +1,205 @@
+/***************************************************************************
+ *
+ *   Copyright (c) 2013-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 navigator.h
+ * Helper class to access missions
+ * @author Julian Oes <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#ifndef NAVIGATOR_H
+#define NAVIGATOR_H
+
+#include <systemlib/perf_counter.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/vehicle_global_position.h>
+
+#include "mission.h"
+#include "rtl.h"
+#include "geofence.h"
+
+class Navigator
+{
+public:
+	/**
+	 * Constructor
+	 */
+	Navigator();
+
+	/**
+	 * Destructor, also kills the navigators task.
+	 */
+	~Navigator();
+
+	/**
+	 * Start the navigator task.
+	 *
+	 * @return		OK on success.
+	 */
+	int		start();
+
+	/**
+	 * Display the navigator status.
+	 */
+	void		status();
+
+	/**
+	 * Add point to geofence
+	 */
+	void		add_fence_point(int argc, char *argv[]);
+
+	/**
+	 * Load fence from file
+	 */
+	void		load_fence_from_file(const char *filename);
+
+	struct vehicle_status_s* get_vstatus() { return &_vstatus; }
+	struct vehicle_global_position_s* get_global_position() { return &_global_pos; }
+	struct home_position_s* get_home_position() { return &_home_pos; }
+	Geofence& get_geofence() { return _geofence; }
+
+private:
+
+	bool		_task_should_exit;		/**< if true, sensor task should exit */
+	int		_navigator_task;		/**< task handle for sensor task */
+
+	int		_mavlink_fd;			/**< the file descriptor to send messages over mavlink */
+
+	int		_global_pos_sub;		/**< global position subscription */
+	int		_home_pos_sub;			/**< home position subscription */
+	int		_vstatus_sub;			/**< vehicle status subscription */
+	int		_params_sub;			/**< notification of parameter updates */
+	int		_offboard_mission_sub;		/**< notification of offboard mission updates */
+	int 		_onboard_mission_sub;		/**< notification of onboard mission updates */
+	int		_capabilities_sub;		/**< notification of vehicle capabilities updates */
+	int		_control_mode_sub;		/**< vehicle control mode subscription */
+
+	orb_advert_t	_pos_sp_triplet_pub;		/**< publish position setpoint triplet */
+
+	vehicle_status_s				_vstatus;		/**< vehicle status */
+	vehicle_control_mode_s				_control_mode;		/**< vehicle control mode */
+	vehicle_global_position_s			_global_pos;		/**< global vehicle position */
+	home_position_s					_home_pos;		/**< home position for RTL */
+	mission_item_s 					_mission_item;		/**< current mission item */
+	navigation_capabilities_s			_nav_caps;		/**< navigation capabilities */
+	position_setpoint_triplet_s			_pos_sp_triplet;	/**< triplet of position setpoints */
+
+	bool 		_mission_item_valid;		/**< flags if the current mission item is valid */
+
+	perf_counter_t	_loop_perf;			/**< loop performance counter */
+
+	Geofence	_geofence;			/**< class that handles the geofence */
+	bool		_geofence_violation_warning_sent; /**< prevents spaming to mavlink */
+
+	bool		_fence_valid;			/**< flag if fence is valid */
+	bool		_inside_fence;			/**< vehicle is inside fence */
+
+	Mission		_mission;			/**< class that handles the missions */
+	//Loiter		_loiter;			/**< class that handles loiter */
+	RTL 		_rtl;				/**< class that handles RTL */
+
+	bool		_waypoint_position_reached;	/**< flags if the position of the mission item has been reached */
+	bool		_waypoint_yaw_reached;		/**< flags if the yaw position of the mission item has been reached */
+	uint64_t	_time_first_inside_orbit;	/**< the time when we were first in the acceptance radius of the mission item */
+
+	bool		_update_triplet;		/**< flags if position setpoint triplet needs to be published */
+
+	struct {
+		float acceptance_radius;
+		float loiter_radius;
+		int onboard_mission_enabled;
+		float takeoff_alt;
+	}		_parameters;			/**< local copies of parameters */
+
+	struct {
+		param_t acceptance_radius;
+		param_t loiter_radius;
+		param_t onboard_mission_enabled;
+		param_t takeoff_alt;
+	}		_parameter_handles;		/**< handles for parameters */
+
+	/**
+	 * Update our local parameter cache.
+	 */
+	void		parameters_update();
+
+	/**
+	 * Retrieve global position
+	 */
+	void		global_position_update();
+
+	/**
+	 * Retrieve home position
+	 */
+	void		home_position_update();
+
+	/**
+	 * Retreive navigation capabilities
+	 */
+	void		navigation_capabilities_update();
+
+	/**
+	 * Retrieve vehicle status
+	 */
+	void		vehicle_status_update();
+
+	/**
+	 * Retrieve vehicle control mode
+	 */
+	void		vehicle_control_mode_update();
+
+	/**
+	 * Shim for calling task_main from task_create.
+	 */
+	static void	task_main_trampoline(int argc, char *argv[]);
+
+	/**
+	 * Main task.
+	 */
+	void		task_main();
+
+	/**
+	 * Translate mission item to a position setpoint.
+	 */
+	void		mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp);
+
+	/**
+	 * Publish a new position setpoint triplet for position controllers
+	 */
+	void		publish_position_setpoint_triplet();
+};
+#endif
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index f30cd9a946..b991ffc8c5 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -62,11 +62,8 @@
 #include <arch/board/board.h>
 
 #include <uORB/uORB.h>
-#include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/home_position.h>
-#include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/fence.h>
@@ -74,24 +71,13 @@
 
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
-#include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
 #include <geo/geo.h>
 #include <dataman/dataman.h>
 #include <mathlib/mathlib.h>
 #include <mavlink/mavlink_log.h>
 
-#include "mission.h"
-#include "rtl.h"
-#include "mission_feasibility_checker.h"
-#include "geofence.h"
-
-
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-static const int ERROR = -1;
+#include "navigator.h"
 
 /**
  * navigator app start / stop handling function
@@ -100,174 +86,10 @@ static const int ERROR = -1;
  */
 extern "C" __EXPORT int navigator_main(int argc, char *argv[]);
 
-class Navigator
-{
-public:
-	/**
-	 * Constructor
-	 */
-	Navigator();
-
-	/**
-	 * Destructor, also kills the navigators task.
-	 */
-	~Navigator();
-
-	/**
-	 * Start the navigator task.
-	 *
-	 * @return		OK on success.
-	 */
-	int		start();
-
-	/**
-	 * Display the navigator status.
-	 */
-	void		status();
-
-	/**
-	 * Add point to geofence
-	 */
-	void		add_fence_point(int argc, char *argv[]);
-
-	/**
-	 * Load fence from file
-	 */
-	void		load_fence_from_file(const char *filename);
-
-private:
-
-	bool		_task_should_exit;		/**< if true, sensor task should exit */
-	int		_navigator_task;		/**< task handle for sensor task */
-
-	int		_mavlink_fd;			/**< the file descriptor to send messages over mavlink */
-
-	int		_global_pos_sub;		/**< global position subscription */
-	int		_home_pos_sub;			/**< home position subscription */
-	int		_vstatus_sub;			/**< vehicle status subscription */
-	int		_params_sub;			/**< notification of parameter updates */
-	int		_offboard_mission_sub;		/**< notification of offboard mission updates */
-	int 		_onboard_mission_sub;		/**< notification of onboard mission updates */
-	int		_capabilities_sub;		/**< notification of vehicle capabilities updates */
-	int		_control_mode_sub;		/**< vehicle control mode subscription */
-
-	orb_advert_t	_pos_sp_triplet_pub;		/**< publish position setpoint triplet */
-
-	vehicle_status_s				_vstatus;		/**< vehicle status */
-	vehicle_control_mode_s				_control_mode;		/**< vehicle control mode */
-	vehicle_global_position_s			_global_pos;		/**< global vehicle position */
-	home_position_s					_home_pos;		/**< home position for RTL */
-	mission_item_s 					_mission_item;		/**< current mission item */
-	navigation_capabilities_s			_nav_caps;		/**< navigation capabilities */
-	position_setpoint_triplet_s			_pos_sp_triplet;	/**< triplet of position setpoints */
-
-	bool 		_mission_item_valid;		/**< flags if the current mission item is valid */
-
-	perf_counter_t	_loop_perf;			/**< loop performance counter */
-
-	Geofence	_geofence;			/**< class that handles the geofence */
-	bool		_geofence_violation_warning_sent; /**< prevents spaming to mavlink */
-
-	bool		_fence_valid;			/**< flag if fence is valid */
-	bool		_inside_fence;			/**< vehicle is inside fence */
-
-
-	Mission		_mission;			/**< class that handles the missions */
-
-	RTL 		_rtl;				/**< class that handles RTL */
-
-	bool		_waypoint_position_reached;	/**< flags if the position of the mission item has been reached */
-	bool		_waypoint_yaw_reached;		/**< flags if the yaw position of the mission item has been reached */
-	uint64_t	_time_first_inside_orbit;	/**< the time when we were first in the acceptance radius of the mission item */
-
-	MissionFeasibilityChecker			missionFeasiblityChecker; /**< class that checks if a mission is feasible */
-
-	bool		_update_triplet;		/**< flags if position setpoint triplet needs to be published */
-
-	struct {
-		float acceptance_radius;
-		float loiter_radius;
-		int onboard_mission_enabled;
-		float takeoff_alt;
-	}		_parameters;			/**< local copies of parameters */
-
-	struct {
-		param_t acceptance_radius;
-		param_t loiter_radius;
-		param_t onboard_mission_enabled;
-		param_t takeoff_alt;
-	}		_parameter_handles;		/**< handles for parameters */
-
-	/**
-	 * Update our local parameter cache.
-	 */
-	void		parameters_update();
-
-	/**
-	 * Retrieve global position
-	 */
-	void		global_position_update();
-
-	/**
-	 * Retrieve home position
-	 */
-	void		home_position_update();
-
-	/**
-	 * Retreive navigation capabilities
-	 */
-	void		navigation_capabilities_update();
-
-	/**
-	 * Retrieve offboard mission.
-	 */
-	void		offboard_mission_update();
-
-	/**
-	 * Retrieve onboard mission.
-	 */
-	void		onboard_mission_update();
-
-	/**
-	 * Retrieve vehicle status
-	 */
-	void		vehicle_status_update();
-
-	/**
-	 * Retrieve vehicle control mode
-	 */
-	void		vehicle_control_mode_update();
-
-	/**
-	 * Shim for calling task_main from task_create.
-	 */
-	static void	task_main_trampoline(int argc, char *argv[]);
-
-	/**
-	 * Main task.
-	 */
-	void		task_main();
-
-	/**
-	 * Translate mission item to a position setpoint.
-	 */
-	void		mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp);
-
-	/**
-	 * Publish a new position setpoint triplet for position controllers
-	 */
-	void		publish_position_setpoint_triplet();
-};
 
 namespace navigator
 {
 
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-static const int ERROR = -1;
-
 Navigator	*g_navigator;
 }
 
@@ -299,8 +121,9 @@ Navigator::Navigator() :
 	_geofence_violation_warning_sent(false),
 	_fence_valid(false),
 	_inside_fence(true),
-	_mission({}),
-	_rtl({}),
+	_mission(this),
+	//_loiter(&_global_pos, &_home_pos, &_vstatus),
+	_rtl(this),
 	_waypoint_position_reached(false),
 	_waypoint_yaw_reached(false),
 	_time_first_inside_orbit(0),
@@ -351,7 +174,7 @@ Navigator::parameters_update()
 	param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled));
 	param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt));
 
-	_mission.set_onboard_mission_allowed((bool)_parameters.onboard_mission_enabled);
+	//_mission.set_onboard_mission_allowed((bool)_parameters.onboard_mission_enabled);
 
 	_geofence.updateParams();
 }
@@ -366,8 +189,6 @@ void
 Navigator::home_position_update()
 {
 	orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos);
-
-	_rtl.set_home_position(&_home_pos);
 }
 
 void
@@ -376,54 +197,6 @@ Navigator::navigation_capabilities_update()
 	orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
 }
 
-
-void
-Navigator::offboard_mission_update()
-{
-	struct mission_s offboard_mission;
-
-	if (orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &offboard_mission) == OK) {
-
-		/* Check mission feasibility, for now do not handle the return value,
-		 * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
-		dm_item_t dm_current;
-
-		if (offboard_mission.dataman_id == 0) {
-			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
-
-		} else {
-			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
-		}
-
-		missionFeasiblityChecker.checkMissionFeasible(_vstatus.is_rotary_wing, dm_current, (size_t)offboard_mission.count, _geofence);
-
-		_mission.set_offboard_dataman_id(offboard_mission.dataman_id);
-
-		_mission.set_offboard_mission_count(offboard_mission.count);
-		_mission.command_current_offboard_mission_index(offboard_mission.current_index);
-
-	} else {
-		_mission.set_offboard_mission_count(0);
-		_mission.command_current_offboard_mission_index(0);
-	}
-}
-
-void
-Navigator::onboard_mission_update()
-{
-	struct mission_s onboard_mission;
-
-	if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) {
-
-		_mission.set_onboard_mission_count(onboard_mission.count);
-		_mission.command_current_onboard_mission_index(onboard_mission.current_index);
-
-	} else {
-		_mission.set_onboard_mission_count(0);
-		_mission.command_current_onboard_mission_index(0);
-	}
-}
-
 void
 Navigator::vehicle_status_update()
 {
@@ -459,8 +232,7 @@ Navigator::task_main()
 
 	/* Try to load the geofence:
 	 * if /fs/microsd/etc/geofence.txt load from this file
-	 * else clear geofence data in datamanager
-	 */
+	 * else clear geofence data in datamanager */
 	struct stat buffer;
 
 	if (stat(GEOFENCE_FILENAME, &buffer) == 0) {
@@ -474,9 +246,7 @@ Navigator::task_main()
 			warnx("Could not clear geofence");
 	}
 
-	/*
-	 * do subscriptions
-	 */
+	/* do subscriptions */
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
 	_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
@@ -493,8 +263,6 @@ Navigator::task_main()
 	global_position_update();
 	home_position_update();
 	navigation_capabilities_update();
-	offboard_mission_update();
-	onboard_mission_update();
 
 	/* rate limit position updates to 50 Hz */
 	orb_set_interval(_global_pos_sub, 20);
@@ -503,7 +271,7 @@ Navigator::task_main()
 	const hrt_abstime mavlink_open_interval = 500000;
 
 	/* wakeup source(s) */
-	struct pollfd fds[8];
+	struct pollfd fds[6];
 
 	/* Setup of loop */
 	fds[0].fd = _params_sub;
@@ -514,14 +282,10 @@ Navigator::task_main()
 	fds[2].events = POLLIN;
 	fds[3].fd = _capabilities_sub;
 	fds[3].events = POLLIN;
-	fds[4].fd = _offboard_mission_sub;
+	fds[4].fd = _vstatus_sub;
 	fds[4].events = POLLIN;
-	fds[5].fd = _onboard_mission_sub;
+	fds[5].fd = _control_mode_sub;
 	fds[5].events = POLLIN;
-	fds[6].fd = _vstatus_sub;
-	fds[6].events = POLLIN;
-	fds[7].fd = _control_mode_sub;
-	fds[7].events = POLLIN;
 
 	while (!_task_should_exit) {
 
@@ -547,12 +311,12 @@ Navigator::task_main()
 		}
 
 		/* vehicle control mode updated */
-		if (fds[7].revents & POLLIN) {
+		if (fds[5].revents & POLLIN) {
 			vehicle_control_mode_update();
 		}
 
 		/* vehicle status updated */
-		if (fds[6].revents & POLLIN) {
+		if (fds[4].revents & POLLIN) {
 			vehicle_status_update();
 		}
 
@@ -567,16 +331,6 @@ Navigator::task_main()
 			navigation_capabilities_update();
 		}
 
-		/* offboard mission updated */
-		if (fds[4].revents & POLLIN) {
-			offboard_mission_update();
-		}
-
-		/* onboard mission updated */
-		if (fds[5].revents & POLLIN) {
-			onboard_mission_update();
-		}
-
 		/* home position updated */
 		if (fds[2].revents & POLLIN) {
 			home_position_update();
@@ -600,6 +354,33 @@ Navigator::task_main()
 			}
 		}
 
+		/* Do stuff according to navigation state set by commander */
+		switch (_vstatus.set_nav_state) {
+			case NAVIGATION_STATE_MANUAL:
+			case NAVIGATION_STATE_ACRO:
+			case NAVIGATION_STATE_ALTCTL:
+			case NAVIGATION_STATE_POSCTL:
+				break;
+			case NAVIGATION_STATE_AUTO_MISSION:
+				_update_triplet = _mission.update(&_pos_sp_triplet);
+				_rtl.reset();
+				break;
+			case NAVIGATION_STATE_AUTO_LOITER:
+				//_loiter.update();
+				break;
+			case NAVIGATION_STATE_AUTO_RTL:
+			case NAVIGATION_STATE_AUTO_RTL_RC:
+			case NAVIGATION_STATE_AUTO_RTL_DL:
+				_mission.reset();
+				_update_triplet = _rtl.update(&_pos_sp_triplet);
+				break;
+			case NAVIGATION_STATE_LAND:
+			case NAVIGATION_STATE_TERMINATION:
+			default:
+				break;
+
+		}
+
 		if (_update_triplet ) {
 			publish_position_setpoint_triplet();
 			_update_triplet = false;
@@ -880,47 +661,6 @@ Navigator::start_land()
 	return true;
 }
 #endif
-void
-Navigator::mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp)
-{
-	sp->valid = true;
-
-	if (item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
-
-		if (_pos_sp_triplet.previous.valid) {
-			/* if previous setpoint is valid then use it to calculate heading to home */
-			sp->yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, sp->lat, sp->lon);
-
-		} else {
-			/* else use current position */
-			sp->yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, sp->lat, sp->lon);
-		}
-
-	} else {
-		sp->lat = item->lat;
-		sp->lon = item->lon;
-		sp->alt = item->altitude_is_relative ? item->altitude + _home_pos.alt : item->altitude;
-		sp->yaw = item->yaw;
-		sp->loiter_radius = item->loiter_radius;
-		sp->loiter_direction = item->loiter_direction;
-		sp->pitch_min = item->pitch_min;
-	}
-
-	if (item->nav_cmd == NAV_CMD_TAKEOFF) {
-		sp->type = SETPOINT_TYPE_TAKEOFF;
-
-	} else if (item->nav_cmd == NAV_CMD_LAND) {
-		sp->type = SETPOINT_TYPE_LAND;
-
-	} else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
-		   item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
-		   item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
-		sp->type = SETPOINT_TYPE_LOITER;
-
-	} else {
-		sp->type = SETPOINT_TYPE_NORMAL;
-	}
-}
 #if 0
 bool
 Navigator::check_mission_item_reached()
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index b8ea062758..9d7886aa61 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -51,26 +51,41 @@
 
 #include "rtl.h"
 
-
-RTL::RTL() :
-	SuperBlock(NULL, "RTL"),
+RTL::RTL(Navigator *navigator) :
+	Mission(navigator),
 	_mavlink_fd(-1),
 	_rtl_state(RTL_STATE_NONE),
 	_home_position({}),
+	_loiter_radius(50),
+	_acceptance_radius(50),
 	_param_return_alt(this, "RETURN_ALT"),
 	_param_descend_alt(this, "DESCEND_ALT"),
-	_param_land_delay(this, "LAND_DELAY"),
-	_loiter_radius(50),
-	_acceptance_radius(50)
+	_param_land_delay(this, "LAND_DELAY")
 {
 	/* load initial params */
 	updateParams();
+	/* initial reset */
+	reset();
 }
 
 RTL::~RTL()
 {
 }
 
+bool
+RTL::update(struct position_setpoint_triplet_s *pos_sp_triplet)
+{
+	bool updated = false;
+
+	return updated;
+}
+
+void
+RTL::reset()
+{
+
+}
+
 void
 RTL::set_home_position(const home_position_s *new_home_position)
 {
@@ -206,7 +221,7 @@ RTL::get_current_rtl_item(const vehicle_global_position_s *global_position, miss
 	}
 
 	default:
-		return false;	
+		return false;
 	}
 
 	return true;
@@ -226,7 +241,7 @@ RTL::move_to_next()
 	case RTL_STATE_CLIMB:
 		_rtl_state = RTL_STATE_RETURN;
 		break;
-	
+
 	case RTL_STATE_RETURN:
 		_rtl_state = RTL_STATE_DESCEND;
 		break;
diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h
index c761837fcf..8d1bfec590 100644
--- a/src/modules/navigator/rtl.h
+++ b/src/modules/navigator/rtl.h
@@ -1,4 +1,4 @@
-/****************************************************************************
+/***************************************************************************
  *
  *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
  *
@@ -43,23 +43,33 @@
 #include <controllib/blocks.hpp>
 #include <controllib/block/BlockParam.hpp>
 
+#include <uORB/topics/mission.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 
-class RTL : public control::SuperBlock
+#include "mission.h"
+
+class Navigator;
+
+class RTL : public Mission
 {
 public:
 	/**
 	 * Constructor
 	 */
-	RTL();
+	RTL(Navigator *navigator);
 
 	/**
 	 * Destructor
 	 */
-	~RTL();
+	virtual ~RTL();
 
+	virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet);
+
+	virtual void reset();
+
+private:
 	void		set_home_position(const home_position_s *home_position);
 
 	bool		get_current_rtl_item(const vehicle_global_position_s *global_position, mission_item_s *new_mission_item);
@@ -67,7 +77,6 @@ public:
 
 	void		move_to_next();
 
-private:
 	int 		_mavlink_fd;
 
 	enum RTLState {
@@ -77,7 +86,7 @@ private:
 		RTL_STATE_DESCEND,
 		RTL_STATE_LAND,
 		RTL_STATE_FINISHED,
-	}		_rtl_state;
+	} _rtl_state;
 
 	home_position_s _home_position;
 	float		_loiter_radius;
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 66a9e31c62..2b6caf1592 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1340,7 +1340,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		/* --- GLOBAL POSITION SETPOINT --- */
 		if (copy_if_updated(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet)) {
 			log_msg.msg_type = LOG_GPSP_MSG;
-			log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state;
+			log_msg.body.log_GPSP.nav_state = 0;  /* TODO: Fix this */
 			log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d);
 			log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d);
 			log_msg.body.log_GPSP.alt = buf.triplet.current.alt;
diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h
index 4532b329de..d25db3a772 100644
--- a/src/modules/uORB/topics/mission.h
+++ b/src/modules/uORB/topics/mission.h
@@ -1,9 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
- *           @author Julian Oes <joes@student.ethz.ch>
- *           @author Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -37,6 +34,9 @@
 /**
  * @file mission.h
  * Definition of a mission consisting of mission items.
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
 #ifndef TOPIC_MISSION_H_
@@ -92,9 +92,9 @@ struct mission_item_s {
 	float pitch_min;		/**< minimal pitch angle for fixed wing takeoff waypoints */
 	bool autocontinue;		/**< true if next waypoint should follow after this one */
 	enum ORIGIN origin;		/**< where the waypoint has been generated		*/
-	int do_jump_mission_index;	/**< the mission index that we want to jump to		*/
-	int do_jump_repeat_count;	/**< how many times the jump should be repeated		*/
-	int do_jump_current_count;	/**< how many times the jump has already been repeated	*/
+	int do_jump_mission_index;	/**< index where the do jump will go to                 */
+	unsigned do_jump_repeat_count;	/**< how many times do jump needs to be done            */
+	unsigned do_jump_current_count;	/**< count how many times the jump has been done	*/
 };
 
 struct mission_s
diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h
index c2741a05bd..ce42035ba8 100644
--- a/src/modules/uORB/topics/position_setpoint_triplet.h
+++ b/src/modules/uORB/topics/position_setpoint_triplet.h
@@ -1,9 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
- *           @author Julian Oes <joes@student.ethz.ch>
- *           @author Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -37,6 +34,10 @@
 /**
  * @file mission_item_triplet.h
  * Definition of the global WGS84 position setpoint uORB topic.
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
 #ifndef TOPIC_MISSION_ITEM_TRIPLET_H_
@@ -53,11 +54,12 @@
 
 enum SETPOINT_TYPE
 {
-	SETPOINT_TYPE_NORMAL = 0,		/**< normal setpoint */
-	SETPOINT_TYPE_LOITER,			/**< loiter setpoint */
-	SETPOINT_TYPE_TAKEOFF,			/**< takeoff setpoint */
-	SETPOINT_TYPE_LAND,			/**< land setpoint, altitude must be ignored, vehicle must descend until landing */
-	SETPOINT_TYPE_IDLE,			/**< do nothing, switch off motors or keep at idle speed (MC) */
+	SETPOINT_TYPE_POSITION = 0,	/**< position setpoint */
+	SETPOINT_TYPE_VELOCITY,		/**< velocity setpoint */
+	SETPOINT_TYPE_LOITER,		/**< loiter setpoint */
+	SETPOINT_TYPE_TAKEOFF,		/**< takeoff setpoint */
+	SETPOINT_TYPE_LAND,		/**< land setpoint, altitude must be ignored, descend until landing */
+	SETPOINT_TYPE_IDLE,		/**< do nothing, switch off motors or keep at idle speed (MC) */
 };
 
 struct position_setpoint_s
@@ -73,16 +75,6 @@ struct position_setpoint_s
 	float pitch_min;		/**< minimal pitch angle for fixed wing takeoff waypoints */
 };
 
-typedef enum {
-	NAV_STATE_MANUAL = 0,
-	NAV_STATE_POSCTL,
-	NAV_STATE_AUTO,
-	NAV_STATE_RC_LOSS,
-	NAV_STATE_DL_LOSS,
-	NAV_STATE_TERMINATION,
-	MAX_NAV_STATE
-} nav_state_t;
-
 /**
  * Global position setpoint triplet in WGS84 coordinates.
  *
@@ -93,8 +85,6 @@ struct position_setpoint_triplet_s
 	struct position_setpoint_s previous;
 	struct position_setpoint_s current;
 	struct position_setpoint_s next;
-
-	nav_state_t nav_state;			/**< navigation state */
 };
 
 /**

From 82b7b80f475d12fc31eb63c0d69a7cf8f75ac534 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 3 Jun 2014 21:20:44 +0200
Subject: [PATCH 54/97] navigator: bugfixing (WIP: mission topic not copying)

---
 src/modules/navigator/mission.cpp        | 37 ++++++++++++++++++------
 src/modules/navigator/mission.h          |  2 ++
 src/modules/navigator/navigator.h        |  2 --
 src/modules/navigator/navigator_main.cpp |  9 ++----
 src/modules/navigator/rtl.cpp            |  4 +--
 5 files changed, 34 insertions(+), 20 deletions(-)

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index f3a86666f0..7e02f8c15c 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -50,6 +50,7 @@
 #include <geo/geo.h>
 
 #include <uORB/uORB.h>
+#include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
 
 #include "navigator.h"
@@ -61,8 +62,6 @@ Mission::Mission(Navigator *navigator) :
 	_navigator(navigator),
 	_first_run(true),
 	_param_onboard_enabled(this, "ONBOARD_EN"),
-	_onboard_mission_sub(-1),
-	_offboard_mission_sub(-1),
 	_onboard_mission({0}),
 	_offboard_mission({0}),
 	_mission_item({0}),
@@ -74,6 +73,9 @@ Mission::Mission(Navigator *navigator) :
 	updateParams();
 	/* set initial mission items */
 	reset();
+
+	_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
+	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
 }
 
 Mission::~Mission()
@@ -89,9 +91,14 @@ Mission::reset()
 bool
 Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet)
 {
+	/* check if anything has changed */
+	bool onboard_updated = false; //is_onboard_mission_updated();
+	bool offboard_updated = is_offboard_mission_updated();
+
 	bool updated = false;
-	/* check if anything has changed, and reset mission items if needed */
-	if (is_onboard_mission_updated() || is_offboard_mission_updated() || _first_run) {
+
+	/* reset mission items if needed */
+	if (onboard_updated || offboard_updated) {
 		set_mission_items(pos_sp_triplet);
 		updated = true;
 		_first_run = false;
@@ -229,7 +236,7 @@ Mission::is_onboard_mission_updated()
 	bool updated;
 	orb_check(_onboard_mission_sub, &updated);
 
-	if (!updated) {
+	if (!updated && !_first_run) {
 		return false;
 	}
 
@@ -244,12 +251,17 @@ bool
 Mission::is_offboard_mission_updated()
 {
 	bool updated;
+	warnx("sub: %d", _offboard_mission_sub);
 	orb_check(_offboard_mission_sub, &updated);
-	if (!updated) {
+	if (!updated && !_first_run) {
+		warnx("not updated");
 		return false;
 	}
-
-	if (orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &_offboard_mission) == OK) {
+	struct mission_s offboard_mission;
+	int ret = orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &offboard_mission);
+	warnx("ret: %d", ret);
+	if (ret == OK) {
+		warnx("copy new offboard mission");
 
 		/* Check mission feasibility, for now do not handle the return value,
 		 * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
@@ -263,8 +275,10 @@ Mission::is_offboard_mission_updated()
 
 		missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current,
 			                                      (size_t)_offboard_mission.count,
-							      _navigator->get_geofence());
+							      _navigator->get_geofence(),
+							      _navigator->get_home_position()->alt);
 	} else {
+		warnx("no success with orb_copy");
 		_offboard_mission.count = 0;
 		_offboard_mission.current_index = 0;
 	}
@@ -341,7 +355,9 @@ Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current
 bool
 Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
 {
+	warnx("try offboard mission: %d, %d", _offboard_mission.current_index, _offboard_mission.count );
 	if (_offboard_mission.current_index < (int)_offboard_mission.count) {
+		warnx("theoretically possible");
 		dm_item_t dm_current;
 		if (_offboard_mission.dataman_id == 0) {
 			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
@@ -356,8 +372,11 @@ Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *curren
 
 			report_current_offboard_mission_item();
 			return true;
+		} else {
+			warnx("read fail");
 		}
 	}
+	warnx("failed with offboard mission");
 	return false;
 }
 
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index e86dd25bb4..65a0991b57 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -47,9 +47,11 @@
 
 #include <dataman/dataman.h>
 
+#include <uORB/uORB.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
 
 #include "mission_feasibility_checker.h"
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 1838fe32b0..927e773917 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -103,8 +103,6 @@ private:
 	int		_home_pos_sub;			/**< home position subscription */
 	int		_vstatus_sub;			/**< vehicle status subscription */
 	int		_params_sub;			/**< notification of parameter updates */
-	int		_offboard_mission_sub;		/**< notification of offboard mission updates */
-	int 		_onboard_mission_sub;		/**< notification of onboard mission updates */
 	int		_capabilities_sub;		/**< notification of vehicle capabilities updates */
 	int		_control_mode_sub;		/**< vehicle control mode subscription */
 
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index f910321967..9dd2531270 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -103,8 +103,6 @@ Navigator::Navigator() :
 	_home_pos_sub(-1),
 	_vstatus_sub(-1),
 	_params_sub(-1),
-	_offboard_mission_sub(-1),
-	_onboard_mission_sub(-1),
 	_capabilities_sub(-1),
 	_control_mode_sub(-1),
 	_pos_sp_triplet_pub(-1),
@@ -248,8 +246,6 @@ Navigator::task_main()
 
 	/* do subscriptions */
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
-	_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
 	_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
 	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
 	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
@@ -360,6 +356,8 @@ Navigator::task_main()
 			case NAVIGATION_STATE_ACRO:
 			case NAVIGATION_STATE_ALTCTL:
 			case NAVIGATION_STATE_POSCTL:
+				_mission.reset();
+				_rtl.reset();
 				break;
 			case NAVIGATION_STATE_AUTO_MISSION:
 				_update_triplet = _mission.update(&_pos_sp_triplet);
@@ -660,8 +658,6 @@ Navigator::start_land()
 	_update_triplet = true;
 	return true;
 }
-#endif
-#if 0
 bool
 Navigator::check_mission_item_reached()
 {
@@ -766,6 +762,7 @@ Navigator::reset_reached()
 	_waypoint_yaw_reached = false;
 
 }
+#endif
 void
 Navigator::publish_position_setpoint_triplet()
 {
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index 9d7886aa61..dc87b0521f 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -57,9 +57,7 @@ RTL::RTL(Navigator *navigator) :
 	_rtl_state(RTL_STATE_NONE),
 	_home_position({}),
 	_loiter_radius(50),
-	_acceptance_radius(50),
-	_param_return_alt(this, "RETURN_ALT"),
-	_param_descend_alt(this, "DESCEND_ALT"),
+	_acceptance_radius(50)
 	_param_land_delay(this, "LAND_DELAY")
 {
 	/* load initial params */

From 5ed3652c2fdc20ba70df1cde6c28d58fa6be0e04 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 3 Jun 2014 21:29:26 +0200
Subject: [PATCH 55/97] navigator: compile fix

---
 src/modules/navigator/rtl.cpp | 4 +++-
 src/modules/navigator/rtl.h   | 1 -
 2 files changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index dc87b0521f..9d7886aa61 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -57,7 +57,9 @@ RTL::RTL(Navigator *navigator) :
 	_rtl_state(RTL_STATE_NONE),
 	_home_position({}),
 	_loiter_radius(50),
-	_acceptance_radius(50)
+	_acceptance_radius(50),
+	_param_return_alt(this, "RETURN_ALT"),
+	_param_descend_alt(this, "DESCEND_ALT"),
 	_param_land_delay(this, "LAND_DELAY")
 {
 	/* load initial params */
diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h
index 8d1bfec590..60336d5d03 100644
--- a/src/modules/navigator/rtl.h
+++ b/src/modules/navigator/rtl.h
@@ -92,7 +92,6 @@ private:
 	float		_loiter_radius;
 	float		_acceptance_radius;
 
-
 	control::BlockParamFloat _param_return_alt;
 	control::BlockParamFloat _param_descend_alt;
 	control::BlockParamFloat _param_land_delay;

From 93295861c532595e080ae61c32d5e2cb625b4eac Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 4 Jun 2014 01:07:50 +0200
Subject: [PATCH 56/97] navigator: missions work again, loiter when finished or
 no mission available or sd card removed works as well

---
 src/modules/navigator/mission.cpp        | 133 +++++++++++++++++------
 src/modules/navigator/mission.h          |  16 ++-
 src/modules/navigator/mission_params.c   |  68 ++++++++++++
 src/modules/navigator/module.mk          |   1 +
 src/modules/navigator/navigator.h        |   7 ++
 src/modules/navigator/navigator_main.cpp |   4 +
 6 files changed, 191 insertions(+), 38 deletions(-)
 create mode 100644 src/modules/navigator/mission_params.c

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 7e02f8c15c..38badcff9f 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -62,20 +62,22 @@ Mission::Mission(Navigator *navigator) :
 	_navigator(navigator),
 	_first_run(true),
 	_param_onboard_enabled(this, "ONBOARD_EN"),
+	_param_loiter_radius(this, "LOITER_RAD"),
 	_onboard_mission({0}),
 	_offboard_mission({0}),
+	_current_onboard_mission_index(-1),
+	_current_offboard_mission_index(-1),
 	_mission_item({0}),
 	_mission_result_pub(-1),
 	_mission_result({0}),
-	_mission_type(MISSION_TYPE_NONE)
+	_mission_type(MISSION_TYPE_NONE),
+	_loiter_set(false)
 {
 	/* load initial params */
 	updateParams();
 	/* set initial mission items */
 	reset();
 
-	_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
-	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
 }
 
 Mission::~Mission()
@@ -86,13 +88,15 @@ void
 Mission::reset()
 {
 	_first_run = true;
+	_loiter_set = false;
 }
 
 bool
 Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet)
 {
+
 	/* check if anything has changed */
-	bool onboard_updated = false; //is_onboard_mission_updated();
+	bool onboard_updated = is_onboard_mission_updated();
 	bool offboard_updated = is_offboard_mission_updated();
 
 	bool updated = false;
@@ -104,11 +108,20 @@ Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet)
 		_first_run = false;
 	}
 
+	/* lets check if we reached the current mission item */
 	if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
 		advance_mission();
 		set_mission_items(pos_sp_triplet);
 		updated = true;
 	}
+
+	/* maybe we couldn't actually set a mission, therefore lets set a loiter setpoint */
+	if (_mission_type == MISSION_TYPE_NONE && !_loiter_set) {
+		bool use_current_pos_sp = (pos_sp_triplet->current.valid && _waypoint_position_reached);
+		set_loiter_item(use_current_pos_sp, pos_sp_triplet);
+		updated = true;
+		_loiter_set = true;
+	}
 	return updated;
 }
 
@@ -195,17 +208,17 @@ Mission::is_mission_item_reached()
 }
 
 void
-Mission::reset_mission_item_reached() {
+Mission::reset_mission_item_reached()
+{
 	_waypoint_position_reached = false;
 	_waypoint_yaw_reached = false;
 	_time_first_inside_orbit = 0;
 }
 
 void
-Mission::mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp)
+Mission::mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp)
 {
 	sp->valid = true;
-
 	sp->lat = item->lat;
 	sp->lon = item->lon;
 	sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude;
@@ -230,19 +243,56 @@ Mission::mission_item_to_position_setpoint(const mission_item_s *item, position_
 	}
 }
 
+void
+Mission::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet)
+{
+	if (reuse_current_pos_sp && pos_sp_triplet->current.valid) {
+		/* nothing to be done, just use the current item */
+	} else {
+		pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
+		pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
+		pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
+		pos_sp_triplet->current.yaw = NAN;	/* NAN means to use current yaw */
+	}
+	pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER;
+	pos_sp_triplet->current.loiter_radius = _param_loiter_radius.get();
+	pos_sp_triplet->current.loiter_direction = 1;
+
+	pos_sp_triplet->previous.valid = false;
+	pos_sp_triplet->current.valid = true;
+	pos_sp_triplet->next.valid = false;
+}
+
+
 bool
 Mission::is_onboard_mission_updated()
 {
 	bool updated;
-	orb_check(_onboard_mission_sub, &updated);
+	orb_check(_navigator->get_onboard_mission_sub(), &updated);
 
 	if (!updated && !_first_run) {
 		return false;
 	}
 
-	if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &_onboard_mission) != OK) {
+	if (orb_copy(ORB_ID(onboard_mission), _navigator->get_onboard_mission_sub(), &_onboard_mission) == OK) {
+		/* accept the current index set by the onboard mission if it is within bounds */
+		if (_onboard_mission.current_index >=0
+		&& _onboard_mission.current_index < (int)_onboard_mission.count) {
+			_current_onboard_mission_index = _onboard_mission.current_index;
+		} else {
+			/* if less WPs available, reset to first WP */
+			if (_current_onboard_mission_index >= (int)_onboard_mission.count) {
+				_current_onboard_mission_index = 0;
+			/* if not initialized, set it to 0 */
+			} else if (_current_onboard_mission_index < 0) {
+				_current_onboard_mission_index = 0;
+			}
+			/* otherwise, just leave it */
+		}
+	} else {
 		_onboard_mission.count = 0;
 		_onboard_mission.current_index = 0;
+		_current_onboard_mission_index = 0;
 	}
 	return true;
 }
@@ -251,17 +301,27 @@ bool
 Mission::is_offboard_mission_updated()
 {
 	bool updated;
-	warnx("sub: %d", _offboard_mission_sub);
-	orb_check(_offboard_mission_sub, &updated);
+	orb_check(_navigator->get_offboard_mission_sub(), &updated);
+
 	if (!updated && !_first_run) {
-		warnx("not updated");
 		return false;
 	}
-	struct mission_s offboard_mission;
-	int ret = orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &offboard_mission);
-	warnx("ret: %d", ret);
-	if (ret == OK) {
-		warnx("copy new offboard mission");
+	if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) {
+
+		/* determine current index */
+		if (_offboard_mission.current_index >= 0
+		    && _offboard_mission.current_index < (int)_offboard_mission.count) {
+			_current_offboard_mission_index = _offboard_mission.current_index;
+		} else {
+			/* if less WPs available, reset to first WP */
+			if (_current_offboard_mission_index >= (int)_offboard_mission.count) {
+				_current_offboard_mission_index = 0;
+			/* if not initialized, set it to 0 */
+			} else if (_current_offboard_mission_index < 0) {
+				_current_offboard_mission_index = 0;
+			}
+			/* otherwise, just leave it */
+		}
 
 		/* Check mission feasibility, for now do not handle the return value,
 		 * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
@@ -278,9 +338,9 @@ Mission::is_offboard_mission_updated()
 							      _navigator->get_geofence(),
 							      _navigator->get_home_position()->alt);
 	} else {
-		warnx("no success with orb_copy");
 		_offboard_mission.count = 0;
 		_offboard_mission.current_index = 0;
+		_current_offboard_mission_index = 0;
 	}
 	return true;
 }
@@ -291,11 +351,11 @@ Mission::advance_mission()
 {
 	switch (_mission_type) {
 	case MISSION_TYPE_ONBOARD:
-		_onboard_mission.current_index++;
+		_current_onboard_mission_index++;
 		break;
 
 	case MISSION_TYPE_OFFBOARD:
-		_offboard_mission.current_index++;
+		_current_offboard_mission_index++;
 		break;
 
 	case MISSION_TYPE_NONE:
@@ -307,17 +367,17 @@ Mission::advance_mission()
 void
 Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
 {
-	warnx("set mission items");
-
 	set_previous_pos_setpoint(&pos_sp_triplet->current, &pos_sp_triplet->previous);
 
 	if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) {
 		/* try setting onboard mission item */
 		_mission_type = MISSION_TYPE_ONBOARD;
+		_loiter_set = false;
 
 	} else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) {
 		/* try setting offboard mission item */
 		_mission_type = MISSION_TYPE_OFFBOARD;
+		_loiter_set = false;
 	} else {
 		_mission_type = MISSION_TYPE_NONE;
 	}
@@ -337,15 +397,18 @@ bool
 Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
 {
 	if (_param_onboard_enabled.get() > 0
-	    && _onboard_mission.current_index < (int)_onboard_mission.count) {
+	    && _current_onboard_mission_index < (int)_onboard_mission.count) {
 		struct mission_item_s new_mission_item;
-		if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_onboard_mission.current_index,
+		if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index,
 					&new_mission_item)) {
 			/* convert the current mission item and set it valid */
-			mission_item_to_position_setpoint(&_mission_item, current_pos_sp);
+			mission_item_to_position_setpoint(&new_mission_item, current_pos_sp);
 			current_pos_sp->valid = true;
 
+			reset_mission_item_reached();
+
 			/* TODO: report this somehow */
+			memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s));
 			return true;
 		}
 	}
@@ -355,9 +418,7 @@ Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current
 bool
 Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
 {
-	warnx("try offboard mission: %d, %d", _offboard_mission.current_index, _offboard_mission.count );
-	if (_offboard_mission.current_index < (int)_offboard_mission.count) {
-		warnx("theoretically possible");
+	if (_current_offboard_mission_index < (int)_offboard_mission.count) {
 		dm_item_t dm_current;
 		if (_offboard_mission.dataman_id == 0) {
 			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
@@ -365,18 +426,20 @@ Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *curren
 			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
 		}
 		struct mission_item_s new_mission_item;
-		if (read_mission_item(dm_current, true, &_offboard_mission.current_index, &new_mission_item)) {
+		if (read_mission_item(dm_current, true, &_current_offboard_mission_index, &new_mission_item)) {
 			/* convert the current mission item and set it valid */
-			mission_item_to_position_setpoint(&_mission_item, current_pos_sp);
+			mission_item_to_position_setpoint(&new_mission_item, current_pos_sp);
 			current_pos_sp->valid = true;
 
+			reset_mission_item_reached();
+
 			report_current_offboard_mission_item();
+			memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s));
 			return true;
 		} else {
-			warnx("read fail");
+			warnx("ERROR: WP read fail");
 		}
 	}
-	warnx("failed with offboard mission");
 	return false;
 }
 
@@ -443,6 +506,7 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio
 		/* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */
 		if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) {
 
+			/* TODO: do this check more gracefully since it is not a serious error */
 			if (new_mission_item->do_jump_current_count >= new_mission_item->do_jump_repeat_count) {
 				return false;
 			}
@@ -458,6 +522,7 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio
 					/* not supposed to happen unless the datamanager can't access the dataman */
 					return false;
 				}
+				/* TODO: report about DO JUMP count */
 			}
 			/* set new mission item index and repeat
 			 * we don't have to validate here, if it's invalid, we should realize this later .*/
@@ -479,7 +544,7 @@ Mission::report_mission_item_reached()
 {
 	if (_mission_type == MISSION_TYPE_OFFBOARD) {
 		_mission_result.mission_reached = true;
-		_mission_result.mission_index_reached = _offboard_mission.current_index;
+		_mission_result.mission_index_reached = _current_offboard_mission_index;
 	}
 	publish_mission_result();
 }
@@ -487,7 +552,7 @@ Mission::report_mission_item_reached()
 void
 Mission::report_current_offboard_mission_item()
 {
-	_mission_result.index_current_mission = _offboard_mission.current_index;
+	_mission_result.index_current_mission = _current_offboard_mission_index;
 	publish_mission_result();
 }
 
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index 65a0991b57..f75c8a8827 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -58,7 +58,7 @@
 
 class Navigator;
 
-class Mission : public control::SuperBlock
+class __EXPORT Mission : public control::SuperBlock
 {
 public:
 	/**
@@ -97,6 +97,11 @@ protected:
 	 */
 	void mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp);
 
+	/**
+	 * Set a loiter item, if possible reuse the position setpoint, otherwise take the current position
+	 */
+	void set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet);
+
 	class Navigator *_navigator;
 
 	bool _waypoint_position_reached;
@@ -179,13 +184,14 @@ private:
 	void publish_mission_result();
 
 	control::BlockParamFloat _param_onboard_enabled;
-
-	int _onboard_mission_sub;
-	int _offboard_mission_sub;
+	control::BlockParamFloat _param_loiter_radius;
 
 	struct mission_s _onboard_mission;
 	struct mission_s _offboard_mission;
 
+	int _current_onboard_mission_index;
+	int _current_offboard_mission_index;
+
 	struct mission_item_s _mission_item;
 
 	orb_advert_t _mission_result_pub;
@@ -197,6 +203,8 @@ private:
 		MISSION_TYPE_OFFBOARD
 	} _mission_type;
 
+	bool _loiter_set;
+
 	MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */
 
 };
diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c
new file mode 100644
index 0000000000..9696b3e53b
--- /dev/null
+++ b/src/modules/navigator/mission_params.c
@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ *   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 mission_params.c
+ *
+ * Parameters for mission.
+ *
+ * @author Julian Oes <joes@student.ethz.ch>
+ */
+
+#include <nuttx/config.h>
+
+#include <systemlib/param/param.h>
+
+/*
+ * Mission parameters, accessible via MAVLink
+ */
+
+/**
+ * Loiter radius (fixed wing only)
+ *
+ * Default value of loiter radius (if not specified in mission item).
+ *
+ * @unit meters
+ * @min 0.0
+ * @group Mission
+ */
+PARAM_DEFINE_FLOAT(MIS_LOITER_RAD, 50.0f);
+
+/**
+ * Enable onboard mission
+ *
+ * @min 0
+ * @max 1
+ * @group Mission
+ */
+PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 0);
diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk
index 2d07bc49c9..e2cc475daa 100644
--- a/src/modules/navigator/module.mk
+++ b/src/modules/navigator/module.mk
@@ -40,6 +40,7 @@ MODULE_COMMAND	= navigator
 SRCS		= navigator_main.cpp \
 		  navigator_params.c \
 		  mission.cpp \
+		  mission_params.c \
 		  rtl.cpp \
 		  mission_feasibility_checker.cpp \
 		  geofence.cpp \
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 927e773917..a6ab855195 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -87,9 +87,14 @@ public:
 	 */
 	void		load_fence_from_file(const char *filename);
 
+	/**
+	 * Getters
+	 */
 	struct vehicle_status_s* get_vstatus() { return &_vstatus; }
 	struct vehicle_global_position_s* get_global_position() { return &_global_pos; }
 	struct home_position_s* get_home_position() { return &_home_pos; }
+	int get_onboard_mission_sub() { return _onboard_mission_sub; }
+	int get_offboard_mission_sub() { return _offboard_mission_sub; }
 	Geofence& get_geofence() { return _geofence; }
 
 private:
@@ -105,6 +110,8 @@ private:
 	int		_params_sub;			/**< notification of parameter updates */
 	int		_capabilities_sub;		/**< notification of vehicle capabilities updates */
 	int		_control_mode_sub;		/**< vehicle control mode subscription */
+	int		_onboard_mission_sub;		/**< onboard mission subscription */
+	int		_offboard_mission_sub;		/**< offboard mission subscription */
 
 	orb_advert_t	_pos_sp_triplet_pub;		/**< publish position setpoint triplet */
 
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 9dd2531270..b7dfec0474 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -106,6 +106,8 @@ Navigator::Navigator() :
 	_capabilities_sub(-1),
 	_control_mode_sub(-1),
 	_pos_sp_triplet_pub(-1),
+	_onboard_mission_sub(-1),
+	_offboard_mission_sub(-1),
 	_vstatus({}),
 	_control_mode({}),
 	_global_pos({}),
@@ -251,6 +253,8 @@ Navigator::task_main()
 	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_home_pos_sub = orb_subscribe(ORB_ID(home_position));
+	_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
+	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
 
 	/* copy all topics first time */
 	vehicle_status_update();

From 09e4cc605ba898c8fc2d92c105ec511ffcb6781d Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 4 Jun 2014 15:16:20 +0200
Subject: [PATCH 57/97] navigator: use different param names for mission and
 RTL

---
 src/modules/navigator/mission.cpp        | 4 ++--
 src/modules/navigator/mission.h          | 2 +-
 src/modules/navigator/navigator_main.cpp | 4 ++--
 src/modules/navigator/rtl.cpp            | 4 ++--
 src/modules/navigator/rtl.h              | 2 +-
 5 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 38badcff9f..879e5b6181 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -57,8 +57,8 @@
 #include "mission.h"
 
 
-Mission::Mission(Navigator *navigator) :
-	SuperBlock(NULL, "MIS"),
+Mission::Mission(Navigator *navigator, const char *name) :
+	SuperBlock(NULL, name),
 	_navigator(navigator),
 	_first_run(true),
 	_param_onboard_enabled(this, "ONBOARD_EN"),
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index f75c8a8827..a9ef71606d 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -64,7 +64,7 @@ public:
 	/**
 	 * Constructor
 	 */
-	Mission(Navigator *navigator);
+	Mission(Navigator *navigator, const char *name);
 
 	/**
 	 * Destructor
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index b7dfec0474..e166058b9a 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -121,9 +121,9 @@ Navigator::Navigator() :
 	_geofence_violation_warning_sent(false),
 	_fence_valid(false),
 	_inside_fence(true),
-	_mission(this),
+	_mission(this, "MIS"),
 	//_loiter(&_global_pos, &_home_pos, &_vstatus),
-	_rtl(this),
+	_rtl(this, "RTL"),
 	_waypoint_position_reached(false),
 	_waypoint_yaw_reached(false),
 	_time_first_inside_orbit(0),
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index 9d7886aa61..10499085b4 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -51,8 +51,8 @@
 
 #include "rtl.h"
 
-RTL::RTL(Navigator *navigator) :
-	Mission(navigator),
+RTL::RTL(Navigator *navigator, const char *name) :
+	Mission(navigator, name),
 	_mavlink_fd(-1),
 	_rtl_state(RTL_STATE_NONE),
 	_home_position({}),
diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h
index 60336d5d03..a3a819403f 100644
--- a/src/modules/navigator/rtl.h
+++ b/src/modules/navigator/rtl.h
@@ -58,7 +58,7 @@ public:
 	/**
 	 * Constructor
 	 */
-	RTL(Navigator *navigator);
+	RTL(Navigator *navigator, const char *name);
 
 	/**
 	 * Destructor

From 425b454a87f0eb4dd0300154cdeffa5723c1b3b8 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 4 Jun 2014 15:33:09 +0200
Subject: [PATCH 58/97] navigator: parameter cleanup

---
 src/modules/navigator/mission_params.c   | 15 +++-
 src/modules/navigator/module.mk          |  2 +-
 src/modules/navigator/navigator.h        | 20 -----
 src/modules/navigator/navigator_main.cpp | 60 +++------------
 src/modules/navigator/navigator_params.c | 96 ------------------------
 src/modules/navigator/rtl.cpp            |  1 +
 src/modules/navigator/rtl.h              |  1 +
 src/modules/navigator/rtl_params.c       | 11 +++
 8 files changed, 39 insertions(+), 167 deletions(-)
 delete mode 100644 src/modules/navigator/navigator_params.c

diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c
index 9696b3e53b..f5067a70b7 100644
--- a/src/modules/navigator/mission_params.c
+++ b/src/modules/navigator/mission_params.c
@@ -48,9 +48,20 @@
  */
 
 /**
- * Loiter radius (fixed wing only)
+ * Take-off altitude
  *
- * Default value of loiter radius (if not specified in mission item).
+ * Even if first waypoint has altitude less then MIS_TAKEOFF_ALT above home position, system will climb to
+ * MIS_TAKEOFF_ALT on takeoff, then go to waypoint.
+ *
+ * @unit meters
+ * @group Mission
+ */
+PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f);
+
+/**
+ * Loiter radius after/during mission (FW only)
+ *
+ * Default value of loiter radius (fixedwing only).
  *
  * @unit meters
  * @min 0.0
diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk
index e2cc475daa..88d8aac5d4 100644
--- a/src/modules/navigator/module.mk
+++ b/src/modules/navigator/module.mk
@@ -38,10 +38,10 @@
 MODULE_COMMAND	= navigator
 
 SRCS		= navigator_main.cpp \
-		  navigator_params.c \
 		  mission.cpp \
 		  mission_params.c \
 		  rtl.cpp \
+		  rtl_params.c \
 		  mission_feasibility_checker.cpp \
 		  geofence.cpp \
 		  geofence_params.c
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index a6ab855195..9f877cd763 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -107,7 +107,6 @@ private:
 	int		_global_pos_sub;		/**< global position subscription */
 	int		_home_pos_sub;			/**< home position subscription */
 	int		_vstatus_sub;			/**< vehicle status subscription */
-	int		_params_sub;			/**< notification of parameter updates */
 	int		_capabilities_sub;		/**< notification of vehicle capabilities updates */
 	int		_control_mode_sub;		/**< vehicle control mode subscription */
 	int		_onboard_mission_sub;		/**< onboard mission subscription */
@@ -143,25 +142,6 @@ private:
 
 	bool		_update_triplet;		/**< flags if position setpoint triplet needs to be published */
 
-	struct {
-		float acceptance_radius;
-		float loiter_radius;
-		int onboard_mission_enabled;
-		float takeoff_alt;
-	}		_parameters;			/**< local copies of parameters */
-
-	struct {
-		param_t acceptance_radius;
-		param_t loiter_radius;
-		param_t onboard_mission_enabled;
-		param_t takeoff_alt;
-	}		_parameter_handles;		/**< handles for parameters */
-
-	/**
-	 * Update our local parameter cache.
-	 */
-	void		parameters_update();
-
 	/**
 	 * Retrieve global position
 	 */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index e166058b9a..3c8875a74e 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -64,12 +64,10 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/parameter_update.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/fence.h>
 #include <uORB/topics/navigation_capabilities.h>
 
-#include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/systemlib.h>
 #include <geo/geo.h>
@@ -102,7 +100,6 @@ Navigator::Navigator() :
 	_global_pos_sub(-1),
 	_home_pos_sub(-1),
 	_vstatus_sub(-1),
-	_params_sub(-1),
 	_capabilities_sub(-1),
 	_control_mode_sub(-1),
 	_pos_sp_triplet_pub(-1),
@@ -127,14 +124,8 @@ Navigator::Navigator() :
 	_waypoint_position_reached(false),
 	_waypoint_yaw_reached(false),
 	_time_first_inside_orbit(0),
-	_update_triplet(false),
-	_parameters({}),
-	_parameter_handles({})
+	_update_triplet(false)
 {
-	_parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD");
-	_parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD");
-	_parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN");
-	_parameter_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT");
 }
 
 Navigator::~Navigator()
@@ -162,23 +153,6 @@ Navigator::~Navigator()
 	navigator::g_navigator = nullptr;
 }
 
-void
-Navigator::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.acceptance_radius, &(_parameters.acceptance_radius));
-	param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius));
-	param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled));
-	param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt));
-
-	//_mission.set_onboard_mission_allowed((bool)_parameters.onboard_mission_enabled);
-
-	_geofence.updateParams();
-}
-
 void
 Navigator::global_position_update()
 {
@@ -251,7 +225,6 @@ Navigator::task_main()
 	_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
 	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
 	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
-	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_home_pos_sub = orb_subscribe(ORB_ID(home_position));
 	_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
 	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
@@ -259,7 +232,6 @@ Navigator::task_main()
 	/* copy all topics first time */
 	vehicle_status_update();
 	vehicle_control_mode_update();
-	parameters_update();
 	global_position_update();
 	home_position_update();
 	navigation_capabilities_update();
@@ -271,21 +243,19 @@ Navigator::task_main()
 	const hrt_abstime mavlink_open_interval = 500000;
 
 	/* wakeup source(s) */
-	struct pollfd fds[6];
+	struct pollfd fds[5];
 
 	/* Setup of loop */
-	fds[0].fd = _params_sub;
+	fds[0].fd = _global_pos_sub;
 	fds[0].events = POLLIN;
-	fds[1].fd = _global_pos_sub;
+	fds[1].fd = _home_pos_sub;
 	fds[1].events = POLLIN;
-	fds[2].fd = _home_pos_sub;
+	fds[2].fd = _capabilities_sub;
 	fds[2].events = POLLIN;
-	fds[3].fd = _capabilities_sub;
+	fds[3].fd = _vstatus_sub;
 	fds[3].events = POLLIN;
-	fds[4].fd = _vstatus_sub;
+	fds[4].fd = _control_mode_sub;
 	fds[4].events = POLLIN;
-	fds[5].fd = _control_mode_sub;
-	fds[5].events = POLLIN;
 
 	while (!_task_should_exit) {
 
@@ -311,33 +281,27 @@ Navigator::task_main()
 		}
 
 		/* vehicle control mode updated */
-		if (fds[5].revents & POLLIN) {
+		if (fds[4].revents & POLLIN) {
 			vehicle_control_mode_update();
 		}
 
 		/* vehicle status updated */
-		if (fds[4].revents & POLLIN) {
+		if (fds[3].revents & POLLIN) {
 			vehicle_status_update();
 		}
 
-		/* parameters updated */
-		if (fds[0].revents & POLLIN) {
-			parameters_update();
-			/* note that these new parameters won't be in effect until a mission triplet is published again */
-		}
-
 		/* navigation capabilities updated */
-		if (fds[3].revents & POLLIN) {
+		if (fds[2].revents & POLLIN) {
 			navigation_capabilities_update();
 		}
 
 		/* home position updated */
-		if (fds[2].revents & POLLIN) {
+		if (fds[1].revents & POLLIN) {
 			home_position_update();
 		}
 
 		/* global position updated */
-		if (fds[1].revents & POLLIN) {
+		if (fds[0].revents & POLLIN) {
 			global_position_update();
 
 			/* Check geofence violation */
diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
deleted file mode 100644
index ad079a250f..0000000000
--- a/src/modules/navigator/navigator_params.c
+++ /dev/null
@@ -1,96 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013 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 navigator_params.c
- *
- * Parameters defined by the navigator task.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
- * @author Anton Babushkin <anton.babushkin@me.com>
- */
-
-#include <nuttx/config.h>
-
-#include <systemlib/param/param.h>
-
-/*
- * Navigator parameters, accessible via MAVLink
- */
-
-/**
- * Waypoint acceptance radius
- *
- * Default value of acceptance radius (if not specified in mission item).
- *
- * @unit meters
- * @min 0.0
- * @group Navigation
- */
-PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f);
-
-/**
- * Loiter radius (fixed wing only)
- *
- * Default value of loiter radius (if not specified in mission item).
- *
- * @unit meters
- * @min 0.0
- * @group Navigation
- */
-PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
-
-/**
- * Enable onboard mission
- *
- * @group Navigation
- */
-PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0);
-
-/**
- * Take-off altitude
- *
- * Even if first waypoint has altitude less then NAV_TAKEOFF_ALT above home position, system will climb to NAV_TAKEOFF_ALT on takeoff, then go to waypoint.
- *
- * @unit meters
- * @group Navigation
- */
-PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f);
-
-/**
- * Enable parachute deployment
- *
- * @group Navigation
- */
-PARAM_DEFINE_INT32(NAV_PARACHUTE_EN, 0);
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index 10499085b4..d4e609584f 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -58,6 +58,7 @@ RTL::RTL(Navigator *navigator, const char *name) :
 	_home_position({}),
 	_loiter_radius(50),
 	_acceptance_radius(50),
+	_param_loiter_rad(this, "LOITER_RAD"),
 	_param_return_alt(this, "RETURN_ALT"),
 	_param_descend_alt(this, "DESCEND_ALT"),
 	_param_land_delay(this, "LAND_DELAY")
diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h
index a3a819403f..d84fd1a6f5 100644
--- a/src/modules/navigator/rtl.h
+++ b/src/modules/navigator/rtl.h
@@ -92,6 +92,7 @@ private:
 	float		_loiter_radius;
 	float		_acceptance_radius;
 
+	control::BlockParamFloat _param_loiter_rad;
 	control::BlockParamFloat _param_return_alt;
 	control::BlockParamFloat _param_descend_alt;
 	control::BlockParamFloat _param_land_delay;
diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c
index 7a8b1d745c..bfe6ce7e18 100644
--- a/src/modules/navigator/rtl_params.c
+++ b/src/modules/navigator/rtl_params.c
@@ -47,6 +47,17 @@
  * RTL parameters, accessible via MAVLink
  */
 
+/**
+ * Loiter radius after RTL (FW only)
+ *
+ * Default value of loiter radius after RTL (fixedwing only).
+ *
+ * @unit meters
+ * @min 0.0
+ * @group RTL
+ */
+PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f);
+
 /**
  * RTL altitude
  *

From 7af1103bf3d4936c259e4ee44454d7e34100a7d0 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 6 Jun 2014 00:55:18 +0200
Subject: [PATCH 59/97] navigator: mission and loiter working now

---
 src/modules/navigator/loiter.cpp         | 79 ++++++++++++++++++++++++
 src/modules/navigator/loiter.h           | 69 +++++++++++++++++++++
 src/modules/navigator/mission.cpp        | 33 +++++-----
 src/modules/navigator/mission.h          | 20 +++---
 src/modules/navigator/module.mk          |  1 +
 src/modules/navigator/navigator.h        | 28 +++++----
 src/modules/navigator/navigator_main.cpp | 19 +++---
 7 files changed, 202 insertions(+), 47 deletions(-)
 create mode 100644 src/modules/navigator/loiter.cpp
 create mode 100644 src/modules/navigator/loiter.h

diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp
new file mode 100644
index 0000000000..6da08f072e
--- /dev/null
+++ b/src/modules/navigator/loiter.cpp
@@ -0,0 +1,79 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-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 loiter.cpp
+ *
+ * Helper class to loiter
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include <string.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <math.h>
+#include <fcntl.h>
+
+#include <mavlink/mavlink_log.h>
+#include <systemlib/err.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+
+#include "loiter.h"
+
+Loiter::Loiter(Navigator *navigator, const char *name) :
+	Mission(navigator, name)
+{
+	/* load initial params */
+	updateParams();
+	/* initial reset */
+	reset();
+}
+
+Loiter::~Loiter()
+{
+}
+
+bool
+Loiter::update(struct position_setpoint_triplet_s *pos_sp_triplet)
+{
+	/* set loiter item, don't reuse an existing position setpoint */
+	return set_loiter_item(false, pos_sp_triplet);;
+}
+
+void
+Loiter::reset()
+{
+}
+
diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h
new file mode 100644
index 0000000000..4ae265e447
--- /dev/null
+++ b/src/modules/navigator/loiter.h
@@ -0,0 +1,69 @@
+/***************************************************************************
+ *
+ *   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 loiter.h
+ *
+ * Helper class to loiter
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#ifndef NAVIGATOR_LOITER_H
+#define NAVIGATOR_LOITER_H
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+#include "mission.h"
+
+class Navigator;
+
+class Loiter : public Mission
+{
+public:
+	/**
+	 * Constructor
+	 */
+	Loiter(Navigator *navigator, const char *name);
+
+	/**
+	 * Destructor
+	 */
+	virtual ~Loiter();
+
+	virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet);
+
+	virtual void reset();
+};
+
+#endif
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 879e5b6181..839c4c9606 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -70,8 +70,7 @@ Mission::Mission(Navigator *navigator, const char *name) :
 	_mission_item({0}),
 	_mission_result_pub(-1),
 	_mission_result({0}),
-	_mission_type(MISSION_TYPE_NONE),
-	_loiter_set(false)
+	_mission_type(MISSION_TYPE_NONE)
 {
 	/* load initial params */
 	updateParams();
@@ -88,13 +87,11 @@ void
 Mission::reset()
 {
 	_first_run = true;
-	_loiter_set = false;
 }
 
 bool
 Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet)
 {
-
 	/* check if anything has changed */
 	bool onboard_updated = is_onboard_mission_updated();
 	bool offboard_updated = is_offboard_mission_updated();
@@ -115,13 +112,6 @@ Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet)
 		updated = true;
 	}
 
-	/* maybe we couldn't actually set a mission, therefore lets set a loiter setpoint */
-	if (_mission_type == MISSION_TYPE_NONE && !_loiter_set) {
-		bool use_current_pos_sp = (pos_sp_triplet->current.valid && _waypoint_position_reached);
-		set_loiter_item(use_current_pos_sp, pos_sp_triplet);
-		updated = true;
-		_loiter_set = true;
-	}
 	return updated;
 }
 
@@ -243,12 +233,18 @@ Mission::mission_item_to_position_setpoint(const struct mission_item_s *item, st
 	}
 }
 
-void
+bool
 Mission::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet)
 {
+	if (_navigator->get_is_in_loiter()) {
+		/* already loitering, bail out */
+		return false;
+	}
+
 	if (reuse_current_pos_sp && pos_sp_triplet->current.valid) {
-		/* nothing to be done, just use the current item */
+		/* leave position setpoint as is */
 	} else {
+		/* use current position */
 		pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
 		pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
 		pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
@@ -261,6 +257,9 @@ Mission::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_tri
 	pos_sp_triplet->previous.valid = false;
 	pos_sp_triplet->current.valid = true;
 	pos_sp_triplet->next.valid = false;
+
+	_navigator->set_is_in_loiter(true);
+	return true;
 }
 
 
@@ -372,14 +371,18 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
 	if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) {
 		/* try setting onboard mission item */
 		_mission_type = MISSION_TYPE_ONBOARD;
-		_loiter_set = false;
+		_navigator->set_is_in_loiter(false);
 
 	} else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) {
 		/* try setting offboard mission item */
 		_mission_type = MISSION_TYPE_OFFBOARD;
-		_loiter_set = false;
+		_navigator->set_is_in_loiter(false);
 	} else {
 		_mission_type = MISSION_TYPE_NONE;
+
+		bool use_current_pos_sp = pos_sp_triplet->current.valid && _waypoint_position_reached;
+		reset_mission_item_reached();
+		set_loiter_item(use_current_pos_sp, pos_sp_triplet);
 	}
 }
 
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index a9ef71606d..4d0083d85e 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -32,6 +32,7 @@
  ****************************************************************************/
 /**
  * @file mission.h
+ *
  * Helper class to access missions
  *
  * @author Julian Oes <julian@oes.ch>
@@ -58,7 +59,7 @@
 
 class Navigator;
 
-class __EXPORT Mission : public control::SuperBlock
+class Mission : public control::SuperBlock
 {
 public:
 	/**
@@ -99,17 +100,12 @@ protected:
 
 	/**
 	 * Set a loiter item, if possible reuse the position setpoint, otherwise take the current position
+	 * @return true if setpoint has changed
 	 */
-	void set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet);
+	bool set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet);
 
 	class Navigator *_navigator;
 
-	bool _waypoint_position_reached;
-	bool _waypoint_yaw_reached;
-	hrt_abstime _time_first_inside_orbit;
-
-	bool _first_run;
-
 private:
 	/**
 	 * Update onboard mission topic
@@ -183,6 +179,12 @@ private:
 	 */
 	void publish_mission_result();
 
+	bool _waypoint_position_reached;
+	bool _waypoint_yaw_reached;
+	hrt_abstime _time_first_inside_orbit;
+
+	bool _first_run;
+
 	control::BlockParamFloat _param_onboard_enabled;
 	control::BlockParamFloat _param_loiter_radius;
 
@@ -203,8 +205,6 @@ private:
 		MISSION_TYPE_OFFBOARD
 	} _mission_type;
 
-	bool _loiter_set;
-
 	MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */
 
 };
diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk
index 88d8aac5d4..f4243c5b80 100644
--- a/src/modules/navigator/module.mk
+++ b/src/modules/navigator/module.mk
@@ -40,6 +40,7 @@ MODULE_COMMAND	= navigator
 SRCS		= navigator_main.cpp \
 		  mission.cpp \
 		  mission_params.c \
+		  loiter.cpp \
 		  rtl.cpp \
 		  rtl_params.c \
 		  mission_feasibility_checker.cpp \
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 9f877cd763..0c551bb41a 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -49,6 +49,7 @@
 #include <uORB/topics/vehicle_global_position.h>
 
 #include "mission.h"
+#include "loiter.h"
 #include "rtl.h"
 #include "geofence.h"
 
@@ -87,15 +88,21 @@ public:
 	 */
 	void		load_fence_from_file(const char *filename);
 
+	/**
+	 * Setters
+	 */
+	void		set_is_in_loiter(bool is_in_loiter) { _is_in_loiter = is_in_loiter; }
+
 	/**
 	 * Getters
 	 */
-	struct vehicle_status_s* get_vstatus() { return &_vstatus; }
-	struct vehicle_global_position_s* get_global_position() { return &_global_pos; }
-	struct home_position_s* get_home_position() { return &_home_pos; }
-	int get_onboard_mission_sub() { return _onboard_mission_sub; }
-	int get_offboard_mission_sub() { return _offboard_mission_sub; }
-	Geofence& get_geofence() { return _geofence; }
+	struct vehicle_status_s*	    get_vstatus() { return &_vstatus; }
+	struct vehicle_global_position_s*   get_global_position() { return &_global_pos; }
+	struct home_position_s*		    get_home_position() { return &_home_pos; }
+	int		get_onboard_mission_sub() { return _onboard_mission_sub; }
+	int		get_offboard_mission_sub() { return _offboard_mission_sub; }
+	Geofence&	get_geofence() { return _geofence; }
+	bool		get_is_in_loiter() { return _is_in_loiter; }
 
 private:
 
@@ -133,14 +140,11 @@ private:
 	bool		_inside_fence;			/**< vehicle is inside fence */
 
 	Mission		_mission;			/**< class that handles the missions */
-	//Loiter		_loiter;			/**< class that handles loiter */
+	Loiter		_loiter;			/**< class that handles loiter */
 	RTL 		_rtl;				/**< class that handles RTL */
 
-	bool		_waypoint_position_reached;	/**< flags if the position of the mission item has been reached */
-	bool		_waypoint_yaw_reached;		/**< flags if the yaw position of the mission item has been reached */
-	uint64_t	_time_first_inside_orbit;	/**< the time when we were first in the acceptance radius of the mission item */
-
-	bool		_update_triplet;		/**< flags if position setpoint triplet needs to be published */
+	bool		_is_in_loiter;			/**< flags if current position SP can be used to loiter */
+	bool		_update_triplet;		/**< flags if position SP triplet needs to be published */
 
 	/**
 	 * Retrieve global position
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 3c8875a74e..44c1075c15 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -102,9 +102,9 @@ Navigator::Navigator() :
 	_vstatus_sub(-1),
 	_capabilities_sub(-1),
 	_control_mode_sub(-1),
-	_pos_sp_triplet_pub(-1),
 	_onboard_mission_sub(-1),
 	_offboard_mission_sub(-1),
+	_pos_sp_triplet_pub(-1),
 	_vstatus({}),
 	_control_mode({}),
 	_global_pos({}),
@@ -119,11 +119,8 @@ Navigator::Navigator() :
 	_fence_valid(false),
 	_inside_fence(true),
 	_mission(this, "MIS"),
-	//_loiter(&_global_pos, &_home_pos, &_vstatus),
+	_loiter(this, "LOI"),
 	_rtl(this, "RTL"),
-	_waypoint_position_reached(false),
-	_waypoint_yaw_reached(false),
-	_time_first_inside_orbit(0),
 	_update_triplet(false)
 {
 }
@@ -325,26 +322,29 @@ Navigator::task_main()
 			case NAVIGATION_STATE_ALTCTL:
 			case NAVIGATION_STATE_POSCTL:
 				_mission.reset();
+				_loiter.reset();
 				_rtl.reset();
+				_is_in_loiter = false;
 				break;
 			case NAVIGATION_STATE_AUTO_MISSION:
 				_update_triplet = _mission.update(&_pos_sp_triplet);
-				_rtl.reset();
 				break;
 			case NAVIGATION_STATE_AUTO_LOITER:
-				//_loiter.update();
+				_update_triplet = _loiter.update(&_pos_sp_triplet);
 				break;
 			case NAVIGATION_STATE_AUTO_RTL:
 			case NAVIGATION_STATE_AUTO_RTL_RC:
 			case NAVIGATION_STATE_AUTO_RTL_DL:
-				_mission.reset();
 				_update_triplet = _rtl.update(&_pos_sp_triplet);
 				break;
 			case NAVIGATION_STATE_LAND:
 			case NAVIGATION_STATE_TERMINATION:
 			default:
+				_mission.reset();
+				_loiter.reset();
+				_rtl.reset();
+				_is_in_loiter = false;
 				break;
-
 		}
 
 		if (_update_triplet ) {
@@ -354,7 +354,6 @@ Navigator::task_main()
 
 		perf_end(_loop_perf);
 	}
-
 	warnx("exiting.");
 
 	_navigator_task = -1;

From 034856a24c4ffd41c991797e75517ed456f77a54 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 6 Jun 2014 17:16:49 +0200
Subject: [PATCH 60/97] mission_feasability_checker: fixed warnings

---
 src/modules/navigator/mission_feasibility_checker.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp
index e1a6854b2f..dd7f4c8013 100644
--- a/src/modules/navigator/mission_feasibility_checker.cpp
+++ b/src/modules/navigator/mission_feasibility_checker.cpp
@@ -215,11 +215,12 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
 
 
 //	float slope_alt = wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt;
+	return false;
 }
 
 void MissionFeasibilityChecker::updateNavigationCapabilities()
 {
-	int res = orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
+	(void)orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
 }
 
 void MissionFeasibilityChecker::init()

From 9bfae10b73406ca4f6600a0441c6edf5077f1446 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 6 Jun 2014 17:17:06 +0200
Subject: [PATCH 61/97] geofence: fixed warnings

---
 src/modules/navigator/geofence.cpp | 9 +++++----
 1 file changed, 5 insertions(+), 4 deletions(-)

diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp
index bc8dbca506..2662153084 100644
--- a/src/modules/navigator/geofence.cpp
+++ b/src/modules/navigator/geofence.cpp
@@ -78,7 +78,7 @@ bool Geofence::inside(const struct vehicle_global_position_s *vehicle)
 {
 	double lat = vehicle->lat / 1e7d;
 	double lon = vehicle->lon / 1e7d;
-	float	alt = vehicle->alt;
+	//float	alt = vehicle->alt;
 
 	return inside(lat, lon, vehicle->alt);
 }
@@ -116,9 +116,9 @@ bool Geofence::inside(double lat, double lon, float altitude)
 				}
 
 				// skip vertex 0 (return point)
-				if (((temp_vertex_i.lon) >= lon != (temp_vertex_j.lon >= lon)) &&
-							(lat <= (temp_vertex_j.lat - temp_vertex_i.lat) * (lon - temp_vertex_i.lon) /
-							 (temp_vertex_j.lon - temp_vertex_i.lon) + temp_vertex_i.lat)) {
+				if (((double)temp_vertex_i.lon >= lon) != ((double)temp_vertex_j.lon >= lon) &&
+							(lat <= (double)(temp_vertex_j.lat - temp_vertex_i.lat) * (lon - (double)temp_vertex_i.lon) /
+							 (double)(temp_vertex_j.lon - temp_vertex_i.lon) + (double)temp_vertex_i.lat)) {
 							c = !c;
 				}
 
@@ -294,4 +294,5 @@ Geofence::loadFromFile(const char *filename)
 int Geofence::clearDm()
 {
 	dm_clear(DM_KEY_FENCE_POINTS);
+	return OK;
 }

From d78c3a224267f4dbd1fac72e893c81b83b43df9b Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 6 Jun 2014 17:17:41 +0200
Subject: [PATCH 62/97] navigator: new class structure, loiter and mission
 working

---
 src/modules/navigator/loiter.cpp         |   3 +-
 src/modules/navigator/loiter.h           |  19 +-
 src/modules/navigator/mission.cpp        | 196 ++-------------------
 src/modules/navigator/mission.h          |  46 +----
 src/modules/navigator/mission_block.cpp  | 215 +++++++++++++++++++++++
 src/modules/navigator/mission_block.h    | 105 +++++++++++
 src/modules/navigator/module.mk          |   2 +
 src/modules/navigator/navigator.h        |   4 +
 src/modules/navigator/navigator_main.cpp |  34 +++-
 src/modules/navigator/navigator_mode.cpp |  70 ++++++++
 src/modules/navigator/navigator_mode.h   |  86 +++++++++
 src/modules/navigator/rtl.cpp            |   4 +-
 src/modules/navigator/rtl.h              |  24 ++-
 13 files changed, 565 insertions(+), 243 deletions(-)
 create mode 100644 src/modules/navigator/mission_block.cpp
 create mode 100644 src/modules/navigator/mission_block.h
 create mode 100644 src/modules/navigator/navigator_mode.cpp
 create mode 100644 src/modules/navigator/navigator_mode.h

diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp
index 6da08f072e..035d035e16 100644
--- a/src/modules/navigator/loiter.cpp
+++ b/src/modules/navigator/loiter.cpp
@@ -53,7 +53,8 @@
 #include "loiter.h"
 
 Loiter::Loiter(Navigator *navigator, const char *name) :
-	Mission(navigator, name)
+	NavigatorMode(navigator, name),
+	MissionBlock(navigator)
 {
 	/* load initial params */
 	updateParams();
diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h
index 4ae265e447..a83b53f438 100644
--- a/src/modules/navigator/loiter.h
+++ b/src/modules/navigator/loiter.h
@@ -44,11 +44,10 @@
 #include <controllib/blocks.hpp>
 #include <controllib/block/BlockParam.hpp>
 
-#include "mission.h"
+#include "navigator_mode.h"
+#include "mission_block.h"
 
-class Navigator;
-
-class Loiter : public Mission
+class Loiter : public NavigatorMode, MissionBlock
 {
 public:
 	/**
@@ -59,11 +58,17 @@ public:
 	/**
 	 * Destructor
 	 */
-	virtual ~Loiter();
+	~Loiter();
 
-	virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet);
+	/**
+	 * This function is called while the mode is inactive
+	 */
+	bool update(struct position_setpoint_triplet_s *pos_sp_triplet);
 
-	virtual void reset();
+	/**
+	 * This function is called while the mode is active
+	 */
+	void reset();
 };
 
 #endif
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 839c4c9606..167f24ca64 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -56,18 +56,15 @@
 #include "navigator.h"
 #include "mission.h"
 
-
 Mission::Mission(Navigator *navigator, const char *name) :
-	SuperBlock(NULL, name),
-	_navigator(navigator),
-	_first_run(true),
+	NavigatorMode(navigator, name),
+	MissionBlock(navigator),
 	_param_onboard_enabled(this, "ONBOARD_EN"),
 	_param_loiter_radius(this, "LOITER_RAD"),
 	_onboard_mission({0}),
 	_offboard_mission({0}),
 	_current_onboard_mission_index(-1),
 	_current_offboard_mission_index(-1),
-	_mission_item({0}),
 	_mission_result_pub(-1),
 	_mission_result({0}),
 	_mission_type(MISSION_TYPE_NONE)
@@ -92,14 +89,23 @@ Mission::reset()
 bool
 Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet)
 {
-	/* check if anything has changed */
-	bool onboard_updated = is_onboard_mission_updated();
-	bool offboard_updated = is_offboard_mission_updated();
+	bool updated;
 
-	bool updated = false;
+	/* check if anything has changed */
+	bool onboard_updated;
+	orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
+	if (onboard_updated) {
+		update_onboard_mission();
+	}
+
+	bool offboard_updated;
+	orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);
+	if (offboard_updated) {
+		update_offboard_mission();
+	}
 
 	/* reset mission items if needed */
-	if (onboard_updated || offboard_updated) {
+	if (onboard_updated || offboard_updated || _first_run) {
 		set_mission_items(pos_sp_triplet);
 		updated = true;
 		_first_run = false;
@@ -115,164 +121,9 @@ Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet)
 	return updated;
 }
 
-bool
-Mission::is_mission_item_reached()
-{
-	/* TODO: count turns */
-#if 0
-	if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
-	     _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
-	     _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
-	    _mission_item.loiter_radius > 0.01f) {
-
-		return false;
-	}
-#endif
-
-	hrt_abstime now = hrt_absolute_time();
-
-	if (!_waypoint_position_reached) {
-
-		float dist = -1.0f;
-		float dist_xy = -1.0f;
-		float dist_z = -1.0f;
-
-		float altitude_amsl = _mission_item.altitude_is_relative
-				      ? _mission_item.altitude + _navigator->get_home_position()->alt
-			              : _mission_item.altitude;
-
-		dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl,
-				                          _navigator->get_global_position()->lat,
-							  _navigator->get_global_position()->lon,
-							  _navigator->get_global_position()->alt,
-				&dist_xy, &dist_z);
-
-		if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
-
-			/* require only altitude for takeoff */
-			if (_navigator->get_global_position()->alt > altitude_amsl - _mission_item.acceptance_radius) {
-				_waypoint_position_reached = true;
-			}
-		} else {
-			if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) {
-				_waypoint_position_reached = true;
-			}
-		}
-	}
-
-	if (_waypoint_position_reached && !_waypoint_yaw_reached) {
-
-		/* TODO: removed takeoff, why? */
-		if (_navigator->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) {
-
-			/* check yaw if defined only for rotary wing except takeoff */
-			float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw);
-
-			if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */
-				_waypoint_yaw_reached = true;
-			}
-
-		} else {
-			_waypoint_yaw_reached = true;
-		}
-	}
-
-	/* check if the current waypoint was reached */
-	if (_waypoint_position_reached && _waypoint_yaw_reached) {
-
-		if (_time_first_inside_orbit == 0) {
-			_time_first_inside_orbit = now;
-
-			// if (_mission_item.time_inside > 0.01f) {
-			// 	mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs",
-			// 		(double)_mission_item.time_inside);
-			// }
-		}
-
-		/* check if the MAV was long enough inside the waypoint orbit */
-		if (now - _time_first_inside_orbit >= (hrt_abstime)_mission_item.time_inside * 1e6f) {
-			return true;
-		}
-	}
-	return false;
-}
-
 void
-Mission::reset_mission_item_reached()
+Mission::update_onboard_mission()
 {
-	_waypoint_position_reached = false;
-	_waypoint_yaw_reached = false;
-	_time_first_inside_orbit = 0;
-}
-
-void
-Mission::mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp)
-{
-	sp->valid = true;
-	sp->lat = item->lat;
-	sp->lon = item->lon;
-	sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude;
-	sp->yaw = item->yaw;
-	sp->loiter_radius = item->loiter_radius;
-	sp->loiter_direction = item->loiter_direction;
-	sp->pitch_min = item->pitch_min;
-
-	if (item->nav_cmd == NAV_CMD_TAKEOFF) {
-		sp->type = SETPOINT_TYPE_TAKEOFF;
-
-	} else if (item->nav_cmd == NAV_CMD_LAND) {
-		sp->type = SETPOINT_TYPE_LAND;
-
-	} else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
-		   item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
-		   item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
-		sp->type = SETPOINT_TYPE_LOITER;
-
-	} else {
-		sp->type = SETPOINT_TYPE_POSITION;
-	}
-}
-
-bool
-Mission::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet)
-{
-	if (_navigator->get_is_in_loiter()) {
-		/* already loitering, bail out */
-		return false;
-	}
-
-	if (reuse_current_pos_sp && pos_sp_triplet->current.valid) {
-		/* leave position setpoint as is */
-	} else {
-		/* use current position */
-		pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
-		pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
-		pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
-		pos_sp_triplet->current.yaw = NAN;	/* NAN means to use current yaw */
-	}
-	pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER;
-	pos_sp_triplet->current.loiter_radius = _param_loiter_radius.get();
-	pos_sp_triplet->current.loiter_direction = 1;
-
-	pos_sp_triplet->previous.valid = false;
-	pos_sp_triplet->current.valid = true;
-	pos_sp_triplet->next.valid = false;
-
-	_navigator->set_is_in_loiter(true);
-	return true;
-}
-
-
-bool
-Mission::is_onboard_mission_updated()
-{
-	bool updated;
-	orb_check(_navigator->get_onboard_mission_sub(), &updated);
-
-	if (!updated && !_first_run) {
-		return false;
-	}
-
 	if (orb_copy(ORB_ID(onboard_mission), _navigator->get_onboard_mission_sub(), &_onboard_mission) == OK) {
 		/* accept the current index set by the onboard mission if it is within bounds */
 		if (_onboard_mission.current_index >=0
@@ -293,18 +144,11 @@ Mission::is_onboard_mission_updated()
 		_onboard_mission.current_index = 0;
 		_current_onboard_mission_index = 0;
 	}
-	return true;
 }
 
-bool
-Mission::is_offboard_mission_updated()
+void
+Mission::update_offboard_mission()
 {
-	bool updated;
-	orb_check(_navigator->get_offboard_mission_sub(), &updated);
-
-	if (!updated && !_first_run) {
-		return false;
-	}
 	if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) {
 
 		/* determine current index */
@@ -341,7 +185,6 @@ Mission::is_offboard_mission_updated()
 		_offboard_mission.current_index = 0;
 		_current_offboard_mission_index = 0;
 	}
-	return true;
 }
 
 
@@ -381,7 +224,6 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_type = MISSION_TYPE_NONE;
 
 		bool use_current_pos_sp = pos_sp_triplet->current.valid && _waypoint_position_reached;
-		reset_mission_item_reached();
 		set_loiter_item(use_current_pos_sp, pos_sp_triplet);
 	}
 }
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index 4d0083d85e..ad8cb467cc 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -33,7 +33,7 @@
 /**
  * @file mission.h
  *
- * Helper class to access missions
+ * Navigator mode to access missions
  *
  * @author Julian Oes <julian@oes.ch>
  */
@@ -50,16 +50,19 @@
 
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
 
+#include "navigator_mode.h"
+#include "mission_block.h"
 #include "mission_feasibility_checker.h"
 
 class Navigator;
 
-class Mission : public control::SuperBlock
+class Mission : public NavigatorMode, MissionBlock
 {
 public:
 	/**
@@ -82,42 +85,16 @@ public:
 	 */
 	virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet);
 
-protected:
-	/**
-	 * Check if mission item has been reached
-	 * @return true if successfully reached
-	 */
-	bool is_mission_item_reached();
-	/**
-	 * Reset all reached flags
-	 */
-	void reset_mission_item_reached();
-
-	/**
-	 * Convert a mission item to a position setpoint
-	 */
-	void mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp);
-
-	/**
-	 * Set a loiter item, if possible reuse the position setpoint, otherwise take the current position
-	 * @return true if setpoint has changed
-	 */
-	bool set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet);
-
-	class Navigator *_navigator;
-
 private:
 	/**
 	 * Update onboard mission topic
-	 * @return true if onboard mission has been updated
 	 */
-	bool is_onboard_mission_updated();
+	void update_onboard_mission();
 
 	/**
 	 * Update offboard mission topic
-	 * @return true if offboard mission has been updated
 	 */
-	bool is_offboard_mission_updated();
+	void update_offboard_mission();
 
 	/**
 	 * Move on to next mission item or switch to loiter
@@ -179,12 +156,6 @@ private:
 	 */
 	void publish_mission_result();
 
-	bool _waypoint_position_reached;
-	bool _waypoint_yaw_reached;
-	hrt_abstime _time_first_inside_orbit;
-
-	bool _first_run;
-
 	control::BlockParamFloat _param_onboard_enabled;
 	control::BlockParamFloat _param_loiter_radius;
 
@@ -194,8 +165,6 @@ private:
 	int _current_onboard_mission_index;
 	int _current_offboard_mission_index;
 
-	struct mission_item_s _mission_item;
-
 	orb_advert_t _mission_result_pub;
 	struct mission_result_s _mission_result;
 
@@ -206,7 +175,6 @@ private:
 	} _mission_type;
 
 	MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */
-
 };
 
 #endif
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
new file mode 100644
index 0000000000..92090d9951
--- /dev/null
+++ b/src/modules/navigator/mission_block.cpp
@@ -0,0 +1,215 @@
+/****************************************************************************
+ *
+ *   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 mission_block.cpp
+ *
+ * Helper class to use mission items
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include <sys/types.h>
+#include <string.h>
+#include <stdlib.h>
+#include <unistd.h>
+
+#include <systemlib/err.h>
+#include <geo/geo.h>
+
+#include <uORB/uORB.h>
+
+#include "navigator.h"
+#include "mission_block.h"
+
+
+MissionBlock::MissionBlock(Navigator *navigator) :
+	_waypoint_position_reached(false),
+	_waypoint_yaw_reached(false),
+	_time_first_inside_orbit(0),
+	_mission_item({0}),
+	_mission_item_valid(false),
+	_navigator_priv(navigator)
+{
+}
+
+MissionBlock::~MissionBlock()
+{
+}
+
+bool
+MissionBlock::is_mission_item_reached()
+{
+	/* TODO: count turns */
+#if 0
+	if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
+	     _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+	     _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
+	    _mission_item.loiter_radius > 0.01f) {
+
+		return false;
+	}
+#endif
+
+	hrt_abstime now = hrt_absolute_time();
+
+	if (!_waypoint_position_reached) {
+
+		float dist = -1.0f;
+		float dist_xy = -1.0f;
+		float dist_z = -1.0f;
+
+		float altitude_amsl = _mission_item.altitude_is_relative
+				      ? _mission_item.altitude + _navigator_priv->get_home_position()->alt
+			              : _mission_item.altitude;
+
+		dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl,
+				                          _navigator_priv->get_global_position()->lat,
+							  _navigator_priv->get_global_position()->lon,
+							  _navigator_priv->get_global_position()->alt,
+				&dist_xy, &dist_z);
+
+		if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
+
+			/* require only altitude for takeoff */
+			if (_navigator_priv->get_global_position()->alt > altitude_amsl - _mission_item.acceptance_radius) {
+				_waypoint_position_reached = true;
+			}
+		} else {
+			if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) {
+				_waypoint_position_reached = true;
+			}
+		}
+	}
+
+	if (_waypoint_position_reached && !_waypoint_yaw_reached) {
+
+		/* TODO: removed takeoff, why? */
+		if (_navigator_priv->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) {
+
+			/* check yaw if defined only for rotary wing except takeoff */
+			float yaw_err = _wrap_pi(_mission_item.yaw - _navigator_priv->get_global_position()->yaw);
+
+			if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */
+				_waypoint_yaw_reached = true;
+			}
+
+		} else {
+			_waypoint_yaw_reached = true;
+		}
+	}
+
+	/* check if the current waypoint was reached */
+	if (_waypoint_position_reached && _waypoint_yaw_reached) {
+
+		if (_time_first_inside_orbit == 0) {
+			_time_first_inside_orbit = now;
+
+			// if (_mission_item.time_inside > 0.01f) {
+			// 	mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs",
+			// 		(double)_mission_item.time_inside);
+			// }
+		}
+
+		/* check if the MAV was long enough inside the waypoint orbit */
+		if (now - _time_first_inside_orbit >= (hrt_abstime)_mission_item.time_inside * 1e6f) {
+			return true;
+		}
+	}
+	return false;
+}
+
+void
+MissionBlock::reset_mission_item_reached()
+{
+	_waypoint_position_reached = false;
+	_waypoint_yaw_reached = false;
+	_time_first_inside_orbit = 0;
+}
+
+void
+MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp)
+{
+	sp->valid = true;
+	sp->lat = item->lat;
+	sp->lon = item->lon;
+	sp->alt = item->altitude_is_relative ? item->altitude + _navigator_priv->get_home_position()->alt : item->altitude;
+	sp->yaw = item->yaw;
+	sp->loiter_radius = item->loiter_radius;
+	sp->loiter_direction = item->loiter_direction;
+	sp->pitch_min = item->pitch_min;
+
+	if (item->nav_cmd == NAV_CMD_TAKEOFF) {
+		sp->type = SETPOINT_TYPE_TAKEOFF;
+
+	} else if (item->nav_cmd == NAV_CMD_LAND) {
+		sp->type = SETPOINT_TYPE_LAND;
+
+	} else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+		   item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
+		   item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
+		sp->type = SETPOINT_TYPE_LOITER;
+
+	} else {
+		sp->type = SETPOINT_TYPE_POSITION;
+	}
+}
+
+bool
+MissionBlock::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet)
+{
+	if (_navigator_priv->get_is_in_loiter()) {
+		/* already loitering, bail out */
+		return false;
+	}
+
+	if (reuse_current_pos_sp && pos_sp_triplet->current.valid) {
+		/* leave position setpoint as is */
+	} else {
+		/* use current position */
+		pos_sp_triplet->current.lat = _navigator_priv->get_global_position()->lat;
+		pos_sp_triplet->current.lon = _navigator_priv->get_global_position()->lon;
+		pos_sp_triplet->current.alt = _navigator_priv->get_global_position()->alt;
+		pos_sp_triplet->current.yaw = NAN;	/* NAN means to use current yaw */
+	}
+	pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER;
+	pos_sp_triplet->current.loiter_radius = _navigator_priv->get_loiter_radius();
+	pos_sp_triplet->current.loiter_direction = 1;
+
+	pos_sp_triplet->previous.valid = false;
+	pos_sp_triplet->current.valid = true;
+	pos_sp_triplet->next.valid = false;
+
+	_navigator_priv->set_is_in_loiter(true);
+	return true;
+}
+
diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h
new file mode 100644
index 0000000000..47e6fe81fe
--- /dev/null
+++ b/src/modules/navigator/mission_block.h
@@ -0,0 +1,105 @@
+/****************************************************************************
+ *
+ *   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 mission_block.h
+ *
+ * Helper class to use mission items
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#ifndef NAVIGATOR_MISSION_BLOCK_H
+#define NAVIGATOR_MISSION_BLOCK_H
+
+#include <drivers/drv_hrt.h>
+
+#include <uORB/topics/mission.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+
+class Navigator;
+
+class MissionBlock
+{
+public:
+	/**
+	 * Constructor
+	 *
+	 * @param pointer to parent class
+	 */
+	MissionBlock(Navigator *navigator);
+
+	/**
+	 * Destructor
+	 */
+	virtual ~MissionBlock();
+
+	/**
+	 * Check if mission item has been reached
+	 * @return true if successfully reached
+	 */
+	bool is_mission_item_reached();
+	/**
+	 * Reset all reached flags
+	 */
+	void reset_mission_item_reached();
+
+	/**
+	 * Convert a mission item to a position setpoint
+	 *
+	 * @param the mission item to convert
+	 * @param the position setpoint that needs to be set
+	 */
+	void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp);
+
+	/**
+	 * Set a loiter item, if possible reuse the position setpoint, otherwise take the current position
+	 *
+	 * @param true if the current position setpoint should be re-used
+	 * @param the position setpoint triplet to set
+	 * @return true if setpoint has changed
+	 */
+	bool set_loiter_item(const bool reuse_current_pos_sp, position_setpoint_triplet_s *pos_sp_triplet);
+
+	bool _waypoint_position_reached;
+	bool _waypoint_yaw_reached;
+	hrt_abstime _time_first_inside_orbit;
+
+	mission_item_s _mission_item;
+	bool _mission_item_valid;
+
+private:
+	Navigator *_navigator_priv;
+};
+
+#endif
diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk
index f4243c5b80..c7cba64cc2 100644
--- a/src/modules/navigator/module.mk
+++ b/src/modules/navigator/module.mk
@@ -38,6 +38,8 @@
 MODULE_COMMAND	= navigator
 
 SRCS		= navigator_main.cpp \
+		  navigator_mode.cpp \
+		  mission_block.cpp \
 		  mission.cpp \
 		  mission_params.c \
 		  loiter.cpp \
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 0c551bb41a..476e9a36c7 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -48,6 +48,7 @@
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_global_position.h>
 
+#include "navigator_mode.h"
 #include "mission.h"
 #include "loiter.h"
 #include "rtl.h"
@@ -104,6 +105,8 @@ public:
 	Geofence&	get_geofence() { return _geofence; }
 	bool		get_is_in_loiter() { return _is_in_loiter; }
 
+	float		get_loiter_radius() { return 50.0f; }; /* TODO: make param*/
+
 private:
 
 	bool		_task_should_exit;		/**< if true, sensor task should exit */
@@ -139,6 +142,7 @@ private:
 	bool		_fence_valid;			/**< flag if fence is valid */
 	bool		_inside_fence;			/**< vehicle is inside fence */
 
+	NavigatorMode	*_navigation_mode;		/**< abstract pointer to current navigation mode class */
 	Mission		_mission;			/**< class that handles the missions */
 	Loiter		_loiter;			/**< class that handles loiter */
 	RTL 		_rtl;				/**< class that handles RTL */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 44c1075c15..fe1f4893ef 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -118,6 +118,7 @@ Navigator::Navigator() :
 	_geofence_violation_warning_sent(false),
 	_fence_valid(false),
 	_inside_fence(true),
+	_navigation_mode(nullptr),
 	_mission(this, "MIS"),
 	_loiter(this, "LOI"),
 	_rtl(this, "RTL"),
@@ -321,32 +322,47 @@ Navigator::task_main()
 			case NAVIGATION_STATE_ACRO:
 			case NAVIGATION_STATE_ALTCTL:
 			case NAVIGATION_STATE_POSCTL:
-				_mission.reset();
-				_loiter.reset();
-				_rtl.reset();
+				_navigation_mode = nullptr;
 				_is_in_loiter = false;
 				break;
 			case NAVIGATION_STATE_AUTO_MISSION:
-				_update_triplet = _mission.update(&_pos_sp_triplet);
+				_navigation_mode = &_mission;
 				break;
 			case NAVIGATION_STATE_AUTO_LOITER:
-				_update_triplet = _loiter.update(&_pos_sp_triplet);
+				_navigation_mode = &_loiter;
 				break;
 			case NAVIGATION_STATE_AUTO_RTL:
 			case NAVIGATION_STATE_AUTO_RTL_RC:
 			case NAVIGATION_STATE_AUTO_RTL_DL:
-				_update_triplet = _rtl.update(&_pos_sp_triplet);
+				_navigation_mode = &_rtl;
 				break;
 			case NAVIGATION_STATE_LAND:
 			case NAVIGATION_STATE_TERMINATION:
 			default:
-				_mission.reset();
-				_loiter.reset();
-				_rtl.reset();
+				_navigation_mode = nullptr;
 				_is_in_loiter = false;
 				break;
 		}
 
+		/* TODO: make list of modes and loop through it */
+		if (_navigation_mode == &_mission) {
+			_update_triplet = _mission.update(&_pos_sp_triplet);
+		} else {
+			_mission.reset();
+		}
+
+		if (_navigation_mode == &_rtl) {
+			_update_triplet = _rtl.update(&_pos_sp_triplet);
+		} else {
+			_rtl.reset();
+		}
+
+		if (_navigation_mode == &_loiter) {
+			_update_triplet = _loiter.update(&_pos_sp_triplet);
+		} else {
+			_loiter.reset();
+		}
+
 		if (_update_triplet ) {
 			publish_position_setpoint_triplet();
 			_update_triplet = false;
diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp
new file mode 100644
index 0000000000..c968291901
--- /dev/null
+++ b/src/modules/navigator/navigator_mode.cpp
@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   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 navigator_mode.cpp
+ *
+ * Helper class for different modes in navigator
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include "navigator_mode.h"
+
+NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) :
+	SuperBlock(NULL, name),
+	_navigator(navigator),
+	_first_run(true)
+{
+	/* load initial params */
+	updateParams();
+	/* set initial mission items */
+	reset();
+}
+
+NavigatorMode::~NavigatorMode()
+{
+}
+
+void
+NavigatorMode::reset()
+{
+	_first_run = true;
+}
+
+bool
+NavigatorMode::update(struct position_setpoint_triplet_s *pos_sp_triplet)
+{
+	pos_sp_triplet->current.valid = false;
+	_first_run = false;
+	return false;
+}
diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h
new file mode 100644
index 0000000000..0844bb94d4
--- /dev/null
+++ b/src/modules/navigator/navigator_mode.h
@@ -0,0 +1,86 @@
+/****************************************************************************
+ *
+ *   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 navigator_mode.h
+ *
+ * Helper class for different modes in navigator
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#ifndef NAVIGATOR_MODE_H
+#define NAVIGATOR_MODE_H
+
+#include <drivers/drv_hrt.h>
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+#include <dataman/dataman.h>
+
+#include <uORB/topics/position_setpoint_triplet.h>
+
+class Navigator;
+
+class NavigatorMode : public control::SuperBlock
+{
+public:
+	/**
+	 * Constructor
+	 */
+	NavigatorMode(Navigator *navigator, const char *name);
+
+	/**
+	 * Destructor
+	 */
+	virtual ~NavigatorMode();
+
+	/**
+	 * This function is called while the mode is inactive
+	 */
+	virtual void reset();
+
+	/**
+	 * This function is called while the mode is active
+	 *
+	 * @param position setpoint triplet to set
+	 * @return true if position setpoint triplet has been changed
+	 */
+	virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet);
+
+protected:
+	Navigator *_navigator;
+	bool _first_run;
+};
+
+#endif
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index d4e609584f..774aa802d8 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -52,13 +52,13 @@
 #include "rtl.h"
 
 RTL::RTL(Navigator *navigator, const char *name) :
-	Mission(navigator, name),
+	NavigatorMode(navigator, name),
+	MissionBlock(navigator),
 	_mavlink_fd(-1),
 	_rtl_state(RTL_STATE_NONE),
 	_home_position({}),
 	_loiter_radius(50),
 	_acceptance_radius(50),
-	_param_loiter_rad(this, "LOITER_RAD"),
 	_param_return_alt(this, "RETURN_ALT"),
 	_param_descend_alt(this, "DESCEND_ALT"),
 	_param_land_delay(this, "LAND_DELAY")
diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h
index d84fd1a6f5..9b32fbb0cd 100644
--- a/src/modules/navigator/rtl.h
+++ b/src/modules/navigator/rtl.h
@@ -31,8 +31,9 @@
  *
  ****************************************************************************/
 /**
- * @file navigator_mission.h
- * Helper class to access missions
+ * @file navigator_rtl.h
+ * Helper class for RTL
+ *
  * @author Julian Oes <julian@oes.ch>
  * @author Anton Babushkin <anton.babushkin@me.com>
  */
@@ -48,11 +49,12 @@
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 
-#include "mission.h"
+#include "navigator_mode.h"
+#include "mission_block.h"
 
 class Navigator;
 
-class RTL : public Mission
+class RTL : public NavigatorMode, MissionBlock
 {
 public:
 	/**
@@ -63,11 +65,18 @@ public:
 	/**
 	 * Destructor
 	 */
-	virtual ~RTL();
+	~RTL();
 
-	virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet);
+	/**
+	 * This function is called while the mode is inactive
+	 */
+	void reset();
+
+	/**
+	 * This function is called while the mode is active
+	 */
+	bool update(struct position_setpoint_triplet_s *pos_sp_triplet);
 
-	virtual void reset();
 
 private:
 	void		set_home_position(const home_position_s *home_position);
@@ -92,7 +101,6 @@ private:
 	float		_loiter_radius;
 	float		_acceptance_radius;
 
-	control::BlockParamFloat _param_loiter_rad;
 	control::BlockParamFloat _param_return_alt;
 	control::BlockParamFloat _param_descend_alt;
 	control::BlockParamFloat _param_land_delay;

From 85d7fdc741a39184d251e2d35d914a6506d6ecd1 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 6 Jun 2014 18:13:45 +0200
Subject: [PATCH 63/97] navigator: param enhancements

---
 src/modules/navigator/mission.cpp        |  3 +-
 src/modules/navigator/mission.h          |  1 -
 src/modules/navigator/mission_params.c   | 10 -----
 src/modules/navigator/module.mk          |  1 +
 src/modules/navigator/navigator.h        | 15 ++++++-
 src/modules/navigator/navigator_main.cpp | 26 +++++++++--
 src/modules/navigator/navigator_params.c | 55 ++++++++++++++++++++++++
 7 files changed, 93 insertions(+), 18 deletions(-)
 create mode 100644 src/modules/navigator/navigator_params.c

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 167f24ca64..93007d9392 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -60,7 +60,6 @@ Mission::Mission(Navigator *navigator, const char *name) :
 	NavigatorMode(navigator, name),
 	MissionBlock(navigator),
 	_param_onboard_enabled(this, "ONBOARD_EN"),
-	_param_loiter_radius(this, "LOITER_RAD"),
 	_onboard_mission({0}),
 	_offboard_mission({0}),
 	_current_onboard_mission_index(-1),
@@ -241,6 +240,8 @@ Mission::set_previous_pos_setpoint(const struct position_setpoint_s *current_pos
 bool
 Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
 {
+	/* make sure param is up to date */
+	updateParams();
 	if (_param_onboard_enabled.get() > 0
 	    && _current_onboard_mission_index < (int)_onboard_mission.count) {
 		struct mission_item_s new_mission_item;
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index ad8cb467cc..cb3242c871 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -157,7 +157,6 @@ private:
 	void publish_mission_result();
 
 	control::BlockParamFloat _param_onboard_enabled;
-	control::BlockParamFloat _param_loiter_radius;
 
 	struct mission_s _onboard_mission;
 	struct mission_s _offboard_mission;
diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c
index f5067a70b7..8692328db1 100644
--- a/src/modules/navigator/mission_params.c
+++ b/src/modules/navigator/mission_params.c
@@ -58,16 +58,6 @@
  */
 PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f);
 
-/**
- * Loiter radius after/during mission (FW only)
- *
- * Default value of loiter radius (fixedwing only).
- *
- * @unit meters
- * @min 0.0
- * @group Mission
- */
-PARAM_DEFINE_FLOAT(MIS_LOITER_RAD, 50.0f);
 
 /**
  * Enable onboard mission
diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk
index c7cba64cc2..a1e109030e 100644
--- a/src/modules/navigator/module.mk
+++ b/src/modules/navigator/module.mk
@@ -38,6 +38,7 @@
 MODULE_COMMAND	= navigator
 
 SRCS		= navigator_main.cpp \
+		  navigator_params.c \
 		  navigator_mode.cpp \
 		  mission_block.cpp \
 		  mission.cpp \
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 476e9a36c7..b22da71177 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -42,11 +42,15 @@
 
 #include <systemlib/perf_counter.h>
 
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
 #include <uORB/uORB.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/parameter_update.h>
 
 #include "navigator_mode.h"
 #include "mission.h"
@@ -54,7 +58,7 @@
 #include "rtl.h"
 #include "geofence.h"
 
-class Navigator
+class Navigator : public control::SuperBlock
 {
 public:
 	/**
@@ -105,7 +109,7 @@ public:
 	Geofence&	get_geofence() { return _geofence; }
 	bool		get_is_in_loiter() { return _is_in_loiter; }
 
-	float		get_loiter_radius() { return 50.0f; }; /* TODO: make param*/
+	float		get_loiter_radius() { return _param_loiter_radius.get(); };
 
 private:
 
@@ -121,6 +125,7 @@ private:
 	int		_control_mode_sub;		/**< vehicle control mode subscription */
 	int		_onboard_mission_sub;		/**< onboard mission subscription */
 	int		_offboard_mission_sub;		/**< offboard mission subscription */
+	int		_param_update_sub;		/**< param update subscription */
 
 	orb_advert_t	_pos_sp_triplet_pub;		/**< publish position setpoint triplet */
 
@@ -150,6 +155,7 @@ private:
 	bool		_is_in_loiter;			/**< flags if current position SP can be used to loiter */
 	bool		_update_triplet;		/**< flags if position SP triplet needs to be published */
 
+	control::BlockParamFloat _param_loiter_radius;	/**< loiter radius for fixedwing */
 	/**
 	 * Retrieve global position
 	 */
@@ -175,6 +181,11 @@ private:
 	 */
 	void		vehicle_control_mode_update();
 
+	/**
+	 * Update parameters
+	 */
+	void		params_update();
+
 	/**
 	 * Shim for calling task_main from task_create.
 	 */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index fe1f4893ef..b1658a6b8d 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -92,8 +92,7 @@ Navigator	*g_navigator;
 }
 
 Navigator::Navigator() :
-
-	/* state machine transition table */
+	SuperBlock(NULL, "NAV"),
 	_task_should_exit(false),
 	_navigator_task(-1),
 	_mavlink_fd(-1),
@@ -122,8 +121,10 @@ Navigator::Navigator() :
 	_mission(this, "MIS"),
 	_loiter(this, "LOI"),
 	_rtl(this, "RTL"),
-	_update_triplet(false)
+	_update_triplet(false),
+	_param_loiter_radius(this, "LOITER_RAD")
 {
+	updateParams();
 }
 
 Navigator::~Navigator()
@@ -188,6 +189,13 @@ Navigator::vehicle_control_mode_update()
 	}
 }
 
+void
+Navigator::params_update()
+{
+	parameter_update_s param_update;
+	orb_copy(ORB_ID(parameter_update), _param_update_sub, &param_update);
+}
+
 void
 Navigator::task_main_trampoline(int argc, char *argv[])
 {
@@ -226,6 +234,7 @@ Navigator::task_main()
 	_home_pos_sub = orb_subscribe(ORB_ID(home_position));
 	_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
 	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
+	_param_update_sub - orb_subscribe(ORB_ID(parameter_update));
 
 	/* copy all topics first time */
 	vehicle_status_update();
@@ -233,6 +242,7 @@ Navigator::task_main()
 	global_position_update();
 	home_position_update();
 	navigation_capabilities_update();
+	params_update();
 
 	/* rate limit position updates to 50 Hz */
 	orb_set_interval(_global_pos_sub, 20);
@@ -241,7 +251,7 @@ Navigator::task_main()
 	const hrt_abstime mavlink_open_interval = 500000;
 
 	/* wakeup source(s) */
-	struct pollfd fds[5];
+	struct pollfd fds[6];
 
 	/* Setup of loop */
 	fds[0].fd = _global_pos_sub;
@@ -254,6 +264,8 @@ Navigator::task_main()
 	fds[3].events = POLLIN;
 	fds[4].fd = _control_mode_sub;
 	fds[4].events = POLLIN;
+	fds[5].fd = _param_update_sub;
+	fds[5].events = POLLIN;
 
 	while (!_task_should_exit) {
 
@@ -278,6 +290,12 @@ Navigator::task_main()
 			_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 		}
 
+		/* parameters updated */
+		if (fds[5].revents & POLLIN) {
+			params_update();
+			updateParams();
+		}
+
 		/* vehicle control mode updated */
 		if (fds[4].revents & POLLIN) {
 			vehicle_control_mode_update();
diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
new file mode 100644
index 0000000000..9235f3046b
--- /dev/null
+++ b/src/modules/navigator/navigator_params.c
@@ -0,0 +1,55 @@
+/****************************************************************************
+ *
+ *   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 navigator_params.c
+ *
+ * Parameters for navigator in general
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include <nuttx/config.h>
+
+#include <systemlib/param/param.h>
+
+/**
+ * Loiter radius (FW only)
+ *
+ * Default value of loiter radius for missions, loiter, RTL, etc. (fixedwing only).
+ *
+ * @unit meters
+ * @min 0.0
+ * @group Mission
+ */
+PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);

From b97b74491bfa7bddf8b7a80525f031e4c83fe5d4 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 6 Jun 2014 18:27:59 +0200
Subject: [PATCH 64/97] navigator: typo

---
 src/modules/navigator/navigator_main.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index b1658a6b8d..241d1d8a44 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -234,7 +234,7 @@ Navigator::task_main()
 	_home_pos_sub = orb_subscribe(ORB_ID(home_position));
 	_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
 	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
-	_param_update_sub - orb_subscribe(ORB_ID(parameter_update));
+	_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
 
 	/* copy all topics first time */
 	vehicle_status_update();

From 1bec9dfe2bab4200a2e2859d1efee0a1bb66e6d1 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 6 Jun 2014 20:02:38 +0200
Subject: [PATCH 65/97] navigator: RTL working again

---
 src/modules/navigator/mission.cpp  |   2 +-
 src/modules/navigator/navigator.h  |   4 +-
 src/modules/navigator/rtl.cpp      | 216 ++++++++++++++++-------------
 src/modules/navigator/rtl.h        |  25 ++--
 src/modules/navigator/rtl_params.c |  12 ++
 5 files changed, 145 insertions(+), 114 deletions(-)

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 93007d9392..cd694be9a0 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -88,7 +88,7 @@ Mission::reset()
 bool
 Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet)
 {
-	bool updated;
+	bool updated = false;
 
 	/* check if anything has changed */
 	bool onboard_updated;
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index b22da71177..296294a918 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -108,8 +108,8 @@ public:
 	int		get_offboard_mission_sub() { return _offboard_mission_sub; }
 	Geofence&	get_geofence() { return _geofence; }
 	bool		get_is_in_loiter() { return _is_in_loiter; }
-
-	float		get_loiter_radius() { return _param_loiter_radius.get(); };
+	float		get_loiter_radius() { return _param_loiter_radius.get(); }
+	int		get_mavlink_fd() { return _mavlink_fd; }
 
 private:
 
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index 774aa802d8..c4c5f2aabb 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -49,19 +49,17 @@
 #include <uORB/topics/mission.h>
 #include <uORB/topics/home_position.h>
 
+#include "navigator.h"
 #include "rtl.h"
 
 RTL::RTL(Navigator *navigator, const char *name) :
 	NavigatorMode(navigator, name),
 	MissionBlock(navigator),
-	_mavlink_fd(-1),
 	_rtl_state(RTL_STATE_NONE),
-	_home_position({}),
-	_loiter_radius(50),
-	_acceptance_radius(50),
 	_param_return_alt(this, "RETURN_ALT"),
 	_param_descend_alt(this, "DESCEND_ALT"),
-	_param_land_delay(this, "LAND_DELAY")
+	_param_land_delay(this, "LAND_DELAY"),
+	_param_acceptance_radius(this, "ACCEPT_RAD")
 {
 	/* load initial params */
 	updateParams();
@@ -73,40 +71,52 @@ RTL::~RTL()
 {
 }
 
+void
+RTL::reset()
+{
+	_first_run = true;
+	_rtl_state = RTL_STATE_NONE;
+}
+
 bool
 RTL::update(struct position_setpoint_triplet_s *pos_sp_triplet)
 {
 	bool updated = false;
 
+	if (_first_run) {
+		set_rtl_item(pos_sp_triplet);
+		updated = true;
+		_first_run = false;
+	}
+
+	if ((_rtl_state == RTL_STATE_CLIMB
+	     || _rtl_state == RTL_STATE_RETURN
+	     || _rtl_state == RTL_STATE_DESCEND)
+	    && is_mission_item_reached()) {
+		advance_rtl();
+		set_rtl_item(pos_sp_triplet);
+		updated = true;
+	}
+
 	return updated;
 }
 
 void
-RTL::reset()
+RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 {
+	/* make sure we have the latest params */
+	updateParams();
 
-}
-
-void
-RTL::set_home_position(const home_position_s *new_home_position)
-{
-	memcpy(&_home_position, new_home_position, sizeof(home_position_s));
-}
-
-bool
-RTL::get_current_rtl_item(const vehicle_global_position_s *global_position, mission_item_s *new_mission_item)
-{
-	/* Open mavlink fd */
-	if (_mavlink_fd < 0) {
-		/* try to open the mavlink log device every once in a while */
-		_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-	}
-
-	/* decide if we need climb */
+	/* decide where to enter the RTL procedure when we switch into it */
 	if (_rtl_state == RTL_STATE_NONE) {
-		if (global_position->alt < _home_position.alt + _param_return_alt.get()) {
+		/* for safety reasons don't go into RTL if landed */
+		if (_navigator->get_vstatus()->condition_landed) {
+			_rtl_state = RTL_STATE_FINISHED;
+		/* if lower than return altitude, climb up first */
+		} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
+			   + _param_return_alt.get()) {
 			_rtl_state = RTL_STATE_CLIMB;
-
+		/* otherwise go straight to return */
 		} else {
 			_rtl_state = RTL_STATE_RETURN;
 		}
@@ -114,129 +124,137 @@ RTL::get_current_rtl_item(const vehicle_global_position_s *global_position, miss
 
 	/* if switching directly to return state, set altitude setpoint to current altitude */
 	if (_rtl_state == RTL_STATE_RETURN) {
-		new_mission_item->altitude_is_relative = false;
-		new_mission_item->altitude = global_position->alt;
+		_mission_item.altitude_is_relative = false;
+		_mission_item.altitude = _navigator->get_global_position()->alt;
 	}
 
 	switch (_rtl_state) {
 	case RTL_STATE_CLIMB: {
 
-		float climb_alt = _home_position.alt + _param_return_alt.get();
+		float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get();
 
 		/* TODO understand and fix this */
 		// if (_vstatus.condition_landed) {
 		// 	climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt);
 		// }
 
-		new_mission_item->lat = global_position->lat;
-		new_mission_item->lon = global_position->lon;
-		new_mission_item->altitude_is_relative = false;
-		new_mission_item->altitude = climb_alt;
-		new_mission_item->yaw = NAN;
-		new_mission_item->loiter_radius = _loiter_radius;
-		new_mission_item->loiter_direction = 1;
-		new_mission_item->nav_cmd = NAV_CMD_WAYPOINT;
-		new_mission_item->acceptance_radius = _acceptance_radius;
-		new_mission_item->time_inside = 0.0f;
-		new_mission_item->pitch_min = 0.0f;
-		new_mission_item->autocontinue = true;
-		new_mission_item->origin = ORIGIN_ONBOARD;
+		_mission_item.lat = _navigator->get_global_position()->lat;
+		_mission_item.lon = _navigator->get_global_position()->lon;
+		_mission_item.altitude_is_relative = false;
+		_mission_item.altitude = climb_alt;
+		_mission_item.yaw = NAN;
+		_mission_item.loiter_radius = _navigator->get_loiter_radius();
+		_mission_item.loiter_direction = 1;
+		_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.time_inside = 0.0f;
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = true;
+		_mission_item.origin = ORIGIN_ONBOARD;
 
-		mavlink_log_info(_mavlink_fd, "#audio: RTL: climb to %d meters above home",
-			(int)(climb_alt - _home_position.alt));
+		_navigator->set_is_in_loiter(false);
+
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: climb to %d meters above home",
+			(int)(climb_alt - _navigator->get_home_position()->alt));
 		break;
 	}
 	case RTL_STATE_RETURN: {
 
-		new_mission_item->lat = _home_position.lat;
-		new_mission_item->lon = _home_position.lon;
+		_mission_item.lat = _navigator->get_home_position()->lat;
+		_mission_item.lon = _navigator->get_home_position()->lon;
 
 		/* TODO: add this again */
 		// don't change altitude
 		// if (_pos_sp_triplet.previous.valid) {
 		// 	/* if previous setpoint is valid then use it to calculate heading to home */
-		// 	new_mission_item->yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, new_mission_item->lat, new_mission_item->lon);
+		// 	_mission_item.yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, _mission_item.lat, _mission_item.lon);
 
 		// } else {
 		// 	/* else use current position */
-		// 	new_mission_item->yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, new_mission_item->lat, new_mission_item->lon);
+		// 	_mission_item.yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, _mission_item.lat, _mission_item.lon);
 		// }
-		new_mission_item->loiter_radius = _loiter_radius;
-		new_mission_item->loiter_direction = 1;
-		new_mission_item->nav_cmd = NAV_CMD_WAYPOINT;
-		new_mission_item->acceptance_radius = _acceptance_radius;
-		new_mission_item->time_inside = 0.0f;
-		new_mission_item->pitch_min = 0.0f;
-		new_mission_item->autocontinue = true;
-		new_mission_item->origin = ORIGIN_ONBOARD;
+		_mission_item.loiter_radius = _navigator->get_loiter_radius();
+		_mission_item.loiter_direction = 1;
+		_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.time_inside = 0.0f;
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = true;
+		_mission_item.origin = ORIGIN_ONBOARD;
 
-		mavlink_log_info(_mavlink_fd, "#audio: RTL: return at %d meters above home",
-			(int)(new_mission_item->altitude - _home_position.alt));
+		_navigator->set_is_in_loiter(false);
+
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: return at %d meters above home",
+			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
 		break;
 	}
-
 	case RTL_STATE_DESCEND: {
 
-		new_mission_item->lat = _home_position.lat;
-		new_mission_item->lon = _home_position.lon;
-		new_mission_item->altitude_is_relative = false;
-		new_mission_item->altitude = _home_position.alt + _param_descend_alt.get();
-		new_mission_item->yaw = NAN;
-		new_mission_item->loiter_radius = _loiter_radius;
-		new_mission_item->loiter_direction = 1;
-		new_mission_item->nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
-		new_mission_item->acceptance_radius = _acceptance_radius;
-		new_mission_item->time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
-		new_mission_item->pitch_min = 0.0f;
-		new_mission_item->autocontinue = _param_land_delay.get() > -0.001f;
-		new_mission_item->origin = ORIGIN_ONBOARD;
+		_mission_item.lat = _navigator->get_home_position()->lat;
+		_mission_item.lon = _navigator->get_home_position()->lon;
+		_mission_item.altitude_is_relative = false;
+		_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();
+		_mission_item.yaw = NAN;
+		_mission_item.loiter_radius = _navigator->get_loiter_radius();
+		_mission_item.loiter_direction = 1;
+		_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
+		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = _param_land_delay.get() > -0.001f;
+		_mission_item.origin = ORIGIN_ONBOARD;
 
-		mavlink_log_info(_mavlink_fd, "#audio: RTL: descend to %d meters above home",
-			(int)(new_mission_item->altitude - _home_position.alt));
+		_navigator->set_is_in_loiter(true);
+
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home",
+			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
 		break;
 	}
 
 	case RTL_STATE_LAND: {
 
-		new_mission_item->lat = _home_position.lat;
-		new_mission_item->lon = _home_position.lon;
-		new_mission_item->altitude_is_relative = false;
-		new_mission_item->altitude = _home_position.alt;
-		new_mission_item->yaw = NAN;
-		new_mission_item->loiter_radius = _loiter_radius;
-		new_mission_item->loiter_direction = 1;
-		new_mission_item->nav_cmd = NAV_CMD_LAND;
-		new_mission_item->acceptance_radius = _acceptance_radius;
-		new_mission_item->time_inside = 0.0f;
-		new_mission_item->pitch_min = 0.0f;
-		new_mission_item->autocontinue = true;
-		new_mission_item->origin = ORIGIN_ONBOARD;
+		_mission_item.lat = _navigator->get_home_position()->lat;
+		_mission_item.lon = _navigator->get_home_position()->lon;
+		_mission_item.altitude_is_relative = false;
+		_mission_item.altitude = _navigator->get_home_position()->alt;
+		_mission_item.yaw = NAN;
+		_mission_item.loiter_radius = _navigator->get_loiter_radius();
+		_mission_item.loiter_direction = 1;
+		_mission_item.nav_cmd = NAV_CMD_LAND;
+		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.time_inside = 0.0f;
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = true;
+		_mission_item.origin = ORIGIN_ONBOARD;
 
-		mavlink_log_info(_mavlink_fd, "#audio: RTL: land at home");
+		_navigator->set_is_in_loiter(false);
+
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: land at home");
 		break;
 	}
 
 	case RTL_STATE_FINISHED: {
 		/* nothing to do, report fail */
-		return false;
 	}
 
 	default:
-		return false;
+		break;
 	}
 
-	return true;
-}
-
-bool
-RTL::get_next_rtl_item(mission_item_s *new_mission_item)
-{
-	/* TODO implement */
-	return false;
+	if (_rtl_state == RTL_STATE_FINISHED) {
+		pos_sp_triplet->current.valid = false;
+		pos_sp_triplet->next.valid = false;
+	} else {
+		/* if not finished, convert mission item to current position setpoint and make it valid */
+		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+		reset_mission_item_reached();
+		pos_sp_triplet->current.valid = true;
+		pos_sp_triplet->next.valid = false;
+	}
 }
 
 void
-RTL::move_to_next()
+RTL::advance_rtl()
 {
 	switch (_rtl_state) {
 	case RTL_STATE_CLIMB:
diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h
index 9b32fbb0cd..9836f5064e 100644
--- a/src/modules/navigator/rtl.h
+++ b/src/modules/navigator/rtl.h
@@ -74,19 +74,23 @@ public:
 
 	/**
 	 * This function is called while the mode is active
+	 *
+	 * @param position setpoint triplet that needs to be set
+	 * @return true if updated
 	 */
-	bool update(struct position_setpoint_triplet_s *pos_sp_triplet);
+	bool update(position_setpoint_triplet_s *pos_sp_triplet);
 
 
 private:
-	void		set_home_position(const home_position_s *home_position);
+	/**
+	 * Set the RTL item
+	 */
+	void		set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet);
 
-	bool		get_current_rtl_item(const vehicle_global_position_s *global_position, mission_item_s *new_mission_item);
-	bool		get_next_rtl_item(mission_item_s *mission_item);
-
-	void		move_to_next();
-
-	int 		_mavlink_fd;
+	/**
+	 * Move to next RTL item
+	 */
+	void		advance_rtl();
 
 	enum RTLState {
 		RTL_STATE_NONE = 0,
@@ -97,13 +101,10 @@ private:
 		RTL_STATE_FINISHED,
 	} _rtl_state;
 
-	home_position_s _home_position;
-	float		_loiter_radius;
-	float		_acceptance_radius;
-
 	control::BlockParamFloat _param_return_alt;
 	control::BlockParamFloat _param_descend_alt;
 	control::BlockParamFloat _param_land_delay;
+	control::BlockParamFloat _param_acceptance_radius;
 };
 
 #endif
diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c
index bfe6ce7e18..63724f4612 100644
--- a/src/modules/navigator/rtl_params.c
+++ b/src/modules/navigator/rtl_params.c
@@ -96,3 +96,15 @@ PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20);
  * @group RTL
  */
 PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f);
+
+/**
+ * RTL acceptance radius
+ *
+ * Acceptance radius for waypoints set for RTL
+ *
+ * @unit meters
+ * @min 1
+ * @max
+ * @group RTL
+ */
+PARAM_DEFINE_FLOAT(RTL_ACCEPT_RAD, 25.0f);

From 94c4fc56aa229bcb909e5437804e7475adfbcdba Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 6 Jun 2014 23:08:11 +0200
Subject: [PATCH 66/97] navigator: audio messages about what is happening, RTL
 during mission not triggered but after mission

---
 src/modules/commander/commander.cpp | 17 +++++++-
 src/modules/navigator/mission.cpp   | 60 +++++++++++++++++++++++------
 src/modules/navigator/mission.h     |  5 +++
 src/modules/navigator/rtl.cpp       |  6 +--
 4 files changed, 70 insertions(+), 18 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index d7a95b0d6d..bb75b2af07 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1296,7 +1296,6 @@ int commander_thread_main(int argc, char *argv[])
 				status.rc_signal_lost = true;
 				status_changed = true;
 
-
 				if (!(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.mission_finished)) {
 
 					/* if we have a global position, we can switch to RTL, if not, we can try to land */
@@ -1306,10 +1305,26 @@ int commander_thread_main(int argc, char *argv[])
 						status.failsafe_state = FAILSAFE_STATE_LAND;
 					}
 					failsafe_state_changed = true;
+				} else {
+					mavlink_log_info(mavlink_fd, "#audio: no RTL during Mission");
 				}
 			}
 		}
 
+		/* hack to detect if we finished a mission after we lost RC, so that we can trigger RTL now */
+		if (status.rc_signal_lost && status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION &&
+		    mission_result.mission_finished && status.failsafe_state != FAILSAFE_STATE_RTL_RC) {
+			/* if we have a global position, we can switch to RTL, if not, we can try to land */
+			if (status.condition_global_position_valid) {
+				status.failsafe_state = FAILSAFE_STATE_RTL_RC;
+				mavlink_log_info(mavlink_fd, "#audio: RTL after Mission is finished");
+			} else {
+				/* this probably doesn't make sense since we are in mission and have global position */
+				status.failsafe_state = FAILSAFE_STATE_LAND;
+			}
+			failsafe_state_changed = true;
+		}
+
 		/* handle commands last, as the system needs to be updated to handle them */
 		orb_check(cmd_sub, &updated);
 
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index cd694be9a0..db606e6fac 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -46,6 +46,7 @@
 #include <drivers/drv_hrt.h>
 
 #include <dataman/dataman.h>
+#include <mavlink/mavlink_log.h>
 #include <systemlib/err.h>
 #include <geo/geo.h>
 
@@ -210,20 +211,39 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
 {
 	set_previous_pos_setpoint(&pos_sp_triplet->current, &pos_sp_triplet->previous);
 
+	/* try setting onboard mission item */
 	if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) {
-		/* try setting onboard mission item */
+		/* if mission type changed, notify */
+		if (_mission_type != MISSION_TYPE_ONBOARD) {
+			mavlink_log_info(_navigator->get_mavlink_fd(),
+					"#audio: onboard mission running");
+		}
 		_mission_type = MISSION_TYPE_ONBOARD;
 		_navigator->set_is_in_loiter(false);
 
+	/* try setting offboard mission item */
 	} else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) {
-		/* try setting offboard mission item */
+		/* if mission type changed, notify */
+		if (_mission_type != MISSION_TYPE_OFFBOARD) {
+			mavlink_log_info(_navigator->get_mavlink_fd(),
+					"#audio: offboard mission running");
+		}
 		_mission_type = MISSION_TYPE_OFFBOARD;
 		_navigator->set_is_in_loiter(false);
 	} else {
+		if (_mission_type != MISSION_TYPE_NONE) {
+			mavlink_log_info(_navigator->get_mavlink_fd(),
+					"#audio: mission finished");
+		} else {
+			mavlink_log_info(_navigator->get_mavlink_fd(),
+					"#audio: no mission available");
+		}
 		_mission_type = MISSION_TYPE_NONE;
 
 		bool use_current_pos_sp = pos_sp_triplet->current.valid && _waypoint_position_reached;
 		set_loiter_item(use_current_pos_sp, pos_sp_triplet);
+		reset_mission_item_reached();
+		report_mission_finished();
 	}
 }
 
@@ -242,8 +262,9 @@ Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current
 {
 	/* make sure param is up to date */
 	updateParams();
-	if (_param_onboard_enabled.get() > 0
-	    && _current_onboard_mission_index < (int)_onboard_mission.count) {
+	if (_param_onboard_enabled.get() > 0 &&
+	    _current_onboard_mission_index >= 0&&
+	    _current_onboard_mission_index < (int)_onboard_mission.count) {
 		struct mission_item_s new_mission_item;
 		if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index,
 					&new_mission_item)) {
@@ -264,7 +285,8 @@ Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current
 bool
 Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
 {
-	if (_current_offboard_mission_index < (int)_offboard_mission.count) {
+	if (_current_offboard_mission_index >= 0 &&
+	    _current_offboard_mission_index < (int)_offboard_mission.count) {
 		dm_item_t dm_current;
 		if (_offboard_mission.dataman_id == 0) {
 			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
@@ -282,8 +304,6 @@ Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *curren
 			report_current_offboard_mission_item();
 			memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s));
 			return true;
-		} else {
-			warnx("ERROR: WP read fail");
 		}
 	}
 	return false;
@@ -295,7 +315,8 @@ Mission::get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp)
 	int next_temp_mission_index = _onboard_mission.current_index + 1;
 
 	/* try if there is a next onboard mission */
-	if (next_temp_mission_index < (int)_onboard_mission.count) {
+	if (_onboard_mission.current_index >= 0 &&
+	    next_temp_mission_index < (int)_onboard_mission.count) {
 		struct mission_item_s new_mission_item;
 		if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, &new_mission_item)) {
 			/* convert next mission item to position setpoint */
@@ -315,7 +336,9 @@ Mission::get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp)
 {
 	/* try if there is a next offboard mission */
 	int next_temp_mission_index = _offboard_mission.current_index + 1;
-	if (next_temp_mission_index < (int)_offboard_mission.count) {
+	warnx("next index: %d, count; %d", next_temp_mission_index, _offboard_mission.count);
+	if (_offboard_mission.current_index >= 0 &&
+	    next_temp_mission_index < (int)_offboard_mission.count) {
 		dm_item_t dm_current;
 		if (_offboard_mission.dataman_id == 0) {
 			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
@@ -346,14 +369,17 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio
 		/* read mission item from datamanager */
 		if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) {
 			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+			mavlink_log_critical(_navigator->get_mavlink_fd(),
+			                     "#audio: ERROR waypoint could not be read");
 			return false;
 		}
 
 		/* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */
 		if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) {
 
-			/* TODO: do this check more gracefully since it is not a serious error */
 			if (new_mission_item->do_jump_current_count >= new_mission_item->do_jump_repeat_count) {
+				mavlink_log_info(_navigator->get_mavlink_fd(),
+						 "#audio: DO JUMP repetitions completed");
 				return false;
 			}
 
@@ -366,9 +392,10 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio
 				if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET,
 					     new_mission_item, len) != len) {
 					/* not supposed to happen unless the datamanager can't access the dataman */
+					mavlink_log_critical(_navigator->get_mavlink_fd(),
+							     "#audio: ERROR DO JUMP waypoint could not be written");
 					return false;
 				}
-				/* TODO: report about DO JUMP count */
 			}
 			/* set new mission item index and repeat
 			 * we don't have to validate here, if it's invalid, we should realize this later .*/
@@ -381,7 +408,8 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio
 	}
 
 	/* we have given up, we don't want to cycle forever */
-	warnx("ERROR: cycling through mission items without success");
+	mavlink_log_critical(_navigator->get_mavlink_fd(),
+			     "#audio: ERROR DO JUMP is cycling, giving up");
 	return false;
 }
 
@@ -402,6 +430,13 @@ Mission::report_current_offboard_mission_item()
 	publish_mission_result();
 }
 
+void
+Mission::report_mission_finished()
+{
+	_mission_result.mission_finished = true;
+	publish_mission_result();
+}
+
 void
 Mission::publish_mission_result()
 {
@@ -416,4 +451,5 @@ Mission::publish_mission_result()
 	}
 	/* reset reached bool */
 	_mission_result.mission_reached = false;
+	_mission_result.mission_finished = false;
 }
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index cb3242c871..a3dd09ecd8 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -151,6 +151,11 @@ private:
 	 */
 	void report_current_offboard_mission_item();
 
+	/**
+	 * Report that the mission is finished if one exists or that none exists
+	 */
+	void report_mission_finished();
+
 	/**
 	 * Publish the mission result so commander and mavlink know what is going on
 	 */
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index c4c5f2aabb..dde35d5b6d 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -112,6 +112,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		/* for safety reasons don't go into RTL if landed */
 		if (_navigator->get_vstatus()->condition_landed) {
 			_rtl_state = RTL_STATE_FINISHED;
+			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
 		/* if lower than return altitude, climb up first */
 		} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
 			   + _param_return_alt.get()) {
@@ -133,11 +134,6 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 
 		float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get();
 
-		/* TODO understand and fix this */
-		// if (_vstatus.condition_landed) {
-		// 	climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt);
-		// }
-
 		_mission_item.lat = _navigator->get_global_position()->lat;
 		_mission_item.lon = _navigator->get_global_position()->lon;
 		_mission_item.altitude_is_relative = false;

From 13b6dffb2e719219ffccccd6681d36e695165aa5 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sat, 7 Jun 2014 17:18:58 +0200
Subject: [PATCH 67/97] navigator: don't reset descend WP in RTL

---
 src/modules/navigator/rtl.cpp | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index dde35d5b6d..8888c6b625 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -90,8 +90,7 @@ RTL::update(struct position_setpoint_triplet_s *pos_sp_triplet)
 	}
 
 	if ((_rtl_state == RTL_STATE_CLIMB
-	     || _rtl_state == RTL_STATE_RETURN
-	     || _rtl_state == RTL_STATE_DESCEND)
+	     || _rtl_state == RTL_STATE_RETURN)
 	    && is_mission_item_reached()) {
 		advance_rtl();
 		set_rtl_item(pos_sp_triplet);

From d3f182d433551c72a2fd8105481938129b8332ed Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 9 Jun 2014 11:37:33 +0200
Subject: [PATCH 68/97] navigator: don't check reached for land waypoints

---
 src/modules/navigator/mission_block.cpp | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 92090d9951..861aed8139 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -69,6 +69,10 @@ MissionBlock::~MissionBlock()
 bool
 MissionBlock::is_mission_item_reached()
 {
+	/* don't check landed WPs */
+	if (_mission_item.nav_cmd == NAV_CMD_LAND) {
+		return false;
+	}
 	/* TODO: count turns */
 #if 0
 	if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||

From fab1b4e3669ce083c124b2ef04b38e7bdea663ed Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 10 Jun 2014 14:25:23 +0200
Subject: [PATCH 69/97] navigator: don't say triplet is valid in ALTCTL

---
 src/modules/navigator/navigator_main.cpp | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 241d1d8a44..da06f826a6 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -381,6 +381,14 @@ Navigator::task_main()
 			_loiter.reset();
 		}
 
+		/* if nothing is running, set position setpoint triplet invalid */
+		if (_navigation_mode == nullptr) {
+			_pos_sp_triplet.previous.valid = false;
+			_pos_sp_triplet.current.valid = false;
+			_pos_sp_triplet.next.valid = false;
+			_update_triplet = true;
+		}
+
 		if (_update_triplet ) {
 			publish_position_setpoint_triplet();
 			_update_triplet = false;

From fd1f1c81efb384e46a5767555a58061082612c9f Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 10 Jun 2014 17:24:04 +0200
Subject: [PATCH 70/97] navigator: added parameter for acceptance radius for
 take-off mission items

---
 src/modules/navigator/mission_block.cpp  | 14 ++++++++++----
 src/modules/navigator/navigator.h        |  2 ++
 src/modules/navigator/navigator_main.cpp |  3 ++-
 src/modules/navigator/navigator_params.c | 11 +++++++++++
 4 files changed, 25 insertions(+), 5 deletions(-)

diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 861aed8139..08576750c9 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -102,13 +102,19 @@ MissionBlock::is_mission_item_reached()
 							  _navigator_priv->get_global_position()->alt,
 				&dist_xy, &dist_z);
 
-		if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
-
-			/* require only altitude for takeoff */
-			if (_navigator_priv->get_global_position()->alt > altitude_amsl - _mission_item.acceptance_radius) {
+		if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator_priv->get_vstatus()->is_rotary_wing) {
+			/* require only altitude for takeoff for multicopter */
+			if (_navigator_priv->get_global_position()->alt >
+				altitude_amsl - _navigator_priv->get_takeoff_acceptance_radius()) {
+				_waypoint_position_reached = true;
+			}
+		} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
+			/* for takeoff mission items use the parameter for the takeoff acceptance radius */
+			if (dist >= 0.0f && dist <= _navigator_priv->get_takeoff_acceptance_radius()) {
 				_waypoint_position_reached = true;
 			}
 		} else {
+			/* for normal mission items used their acceptance radius */
 			if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) {
 				_waypoint_position_reached = true;
 			}
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 296294a918..fe7485f566 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -109,6 +109,7 @@ public:
 	Geofence&	get_geofence() { return _geofence; }
 	bool		get_is_in_loiter() { return _is_in_loiter; }
 	float		get_loiter_radius() { return _param_loiter_radius.get(); }
+	float		get_takeoff_acceptance_radius() { return _param_takeoff_acceptance_radius.get(); }
 	int		get_mavlink_fd() { return _mavlink_fd; }
 
 private:
@@ -156,6 +157,7 @@ private:
 	bool		_update_triplet;		/**< flags if position SP triplet needs to be published */
 
 	control::BlockParamFloat _param_loiter_radius;	/**< loiter radius for fixedwing */
+	control::BlockParamFloat _param_takeoff_acceptance_radius;	/**< acceptance for takeoff */
 	/**
 	 * Retrieve global position
 	 */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index da06f826a6..dc85738134 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -122,7 +122,8 @@ Navigator::Navigator() :
 	_loiter(this, "LOI"),
 	_rtl(this, "RTL"),
 	_update_triplet(false),
-	_param_loiter_radius(this, "LOITER_RAD")
+	_param_loiter_radius(this, "LOITER_RAD"),
+	_param_takeoff_acceptance_radius(this, "TF_ACC_RAD")
 {
 	updateParams();
 }
diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
index 9235f3046b..ca31adecc6 100644
--- a/src/modules/navigator/navigator_params.c
+++ b/src/modules/navigator/navigator_params.c
@@ -53,3 +53,14 @@
  * @group Mission
  */
 PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
+
+/**
+ * Takeoff Acceptance Radius (FW only)
+ *
+ * Acceptance radius for fixedwing.
+ *
+ * @unit meters
+ * @min 1.0
+ * @group Mission
+ */
+PARAM_DEFINE_FLOAT(NAV_TF_ACC_RAD, 25.0f);

From 6f24afd68a840698a9a77b46bc8f1ca81cc1f5b9 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 10 Jun 2014 17:33:12 +0200
Subject: [PATCH 71/97] navigator: always listen to new current mission index
 and to new missions

---
 src/modules/navigator/mission.cpp | 14 ++++++++++++++
 1 file changed, 14 insertions(+)

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index db606e6fac..33a1399b1e 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -84,6 +84,19 @@ void
 Mission::reset()
 {
 	_first_run = true;
+
+	/* check anyway if missions have changed so that feedback to groundstation is given */
+	bool onboard_updated;
+	orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
+	if (onboard_updated) {
+		update_onboard_mission();
+	}
+
+	bool offboard_updated;
+	orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);
+	if (offboard_updated) {
+		update_offboard_mission();
+	}
 }
 
 bool
@@ -185,6 +198,7 @@ Mission::update_offboard_mission()
 		_offboard_mission.current_index = 0;
 		_current_offboard_mission_index = 0;
 	}
+	report_current_offboard_mission_item();
 }
 
 

From d9a64bb58720300417f190b8a8b610ab2966a11f Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 11 Jun 2014 23:16:22 +0200
Subject: [PATCH 72/97] navigator: don't give up after DO_JUMPS

---
 src/modules/navigator/mission.cpp | 43 +++++++++++++++++--------------
 1 file changed, 23 insertions(+), 20 deletions(-)

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 33a1399b1e..a551df9a24 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -391,30 +391,33 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio
 		/* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */
 		if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) {
 
-			if (new_mission_item->do_jump_current_count >= new_mission_item->do_jump_repeat_count) {
+			/* do DO_JUMP as many times as requested */
+			if (new_mission_item->do_jump_current_count < new_mission_item->do_jump_repeat_count) {
+
+				/* only raise the repeat count if this is for the current mission item
+				* but not for the next mission item */
+				if (is_current) {
+					(new_mission_item->do_jump_current_count)++;
+					/* save repeat count */
+					if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET,
+						new_mission_item, len) != len) {
+						/* not supposed to happen unless the datamanager can't access the
+						 * dataman */
+						mavlink_log_critical(_navigator->get_mavlink_fd(),
+								"#audio: ERROR DO JUMP waypoint could not be written");
+						return false;
+					}
+				}
+				/* set new mission item index and repeat
+				* we don't have to validate here, if it's invalid, we should realize this later .*/
+				*mission_index = new_mission_item->do_jump_mission_index;
+			} else {
 				mavlink_log_info(_navigator->get_mavlink_fd(),
 						 "#audio: DO JUMP repetitions completed");
-				return false;
+				/* no more DO_JUMPS, therefore just try to continue with next mission item */
+				(*mission_index)++;
 			}
 
-			/* only raise the repeat count if this is for the current mission item
-			 * but not for the next mission item */
-			if (is_current) {
-				(new_mission_item->do_jump_current_count)++;
-
-				/* save repeat count */
-				if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET,
-					     new_mission_item, len) != len) {
-					/* not supposed to happen unless the datamanager can't access the dataman */
-					mavlink_log_critical(_navigator->get_mavlink_fd(),
-							     "#audio: ERROR DO JUMP waypoint could not be written");
-					return false;
-				}
-			}
-			/* set new mission item index and repeat
-			 * we don't have to validate here, if it's invalid, we should realize this later .*/
-			*mission_index = new_mission_item->do_jump_mission_index;
-
 		} else {
 			/* if it's not a DO_JUMP, then we were successful */
 			return true;

From d48a8bc073a3aa5f515c582a1c2c3cae58a8d783 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 12 Jun 2014 19:09:18 +0200
Subject: [PATCH 73/97] navigator: renamed the different RTL states

---
 src/modules/commander/commander.cpp           | 10 ++---
 .../commander/state_machine_helper.cpp        |  8 ++--
 src/modules/mavlink/mavlink_messages.cpp      |  4 +-
 src/modules/navigator/navigator_main.cpp      |  4 +-
 src/modules/uORB/topics/vehicle_status.h      | 37 ++++++++++---------
 5 files changed, 32 insertions(+), 31 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index bb75b2af07..e992706ac3 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1300,7 +1300,7 @@ int commander_thread_main(int argc, char *argv[])
 
 					/* if we have a global position, we can switch to RTL, if not, we can try to land */
 					if (status.condition_global_position_valid) {
-						status.failsafe_state = FAILSAFE_STATE_RTL_RC;
+						status.failsafe_state = FAILSAFE_STATE_RC_LOSS;
 					} else {
 						status.failsafe_state = FAILSAFE_STATE_LAND;
 					}
@@ -1313,10 +1313,10 @@ int commander_thread_main(int argc, char *argv[])
 
 		/* hack to detect if we finished a mission after we lost RC, so that we can trigger RTL now */
 		if (status.rc_signal_lost && status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION &&
-		    mission_result.mission_finished && status.failsafe_state != FAILSAFE_STATE_RTL_RC) {
+		    mission_result.mission_finished && status.failsafe_state != FAILSAFE_STATE_RC_LOSS) {
 			/* if we have a global position, we can switch to RTL, if not, we can try to land */
 			if (status.condition_global_position_valid) {
-				status.failsafe_state = FAILSAFE_STATE_RTL_RC;
+				status.failsafe_state = FAILSAFE_STATE_RC_LOSS;
 				mavlink_log_info(mavlink_fd, "#audio: RTL after Mission is finished");
 			} else {
 				/* this probably doesn't make sense since we are in mission and have global position */
@@ -1719,8 +1719,8 @@ set_control_mode()
 	case NAVIGATION_STATE_AUTO_MISSION:
 	case NAVIGATION_STATE_AUTO_LOITER:
 	case NAVIGATION_STATE_AUTO_RTL:
-	case NAVIGATION_STATE_AUTO_RTL_RC:
-	case NAVIGATION_STATE_AUTO_RTL_DL:
+	case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS:
+	case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS:
 		control_mode.flag_control_manual_enabled = false;
 		control_mode.flag_control_auto_enabled = true;
 		control_mode.flag_control_rates_enabled = true;
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 2144800790..c52e618ef1 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -406,12 +406,12 @@ void set_nav_state(struct vehicle_status_s *status)
 		}
 		break;
 
-	case FAILSAFE_STATE_RTL_RC:
-		status->set_nav_state = NAVIGATION_STATE_AUTO_RTL_RC;
+	case FAILSAFE_STATE_RC_LOSS:
+		status->set_nav_state = NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS;
 		break;
 
-	case FAILSAFE_STATE_RTL_DL:
-		status->set_nav_state = NAVIGATION_STATE_AUTO_RTL_DL;
+	case FAILSAFE_STATE_DL_LOSS:
+		status->set_nav_state = NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS;
 		break;
 
 	case FAILSAFE_STATE_LAND:
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index e7d2ba515a..b58fb4cb8a 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -161,8 +161,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 			break;
 
 		case NAVIGATION_STATE_AUTO_RTL:
-		case NAVIGATION_STATE_AUTO_RTL_RC:
-		case NAVIGATION_STATE_AUTO_RTL_DL:
+		case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS:
+		case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS:
 			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
 			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
 					      | MAV_MODE_FLAG_GUIDED_ENABLED;
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index dc85738134..20cff5c282 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -351,8 +351,8 @@ Navigator::task_main()
 				_navigation_mode = &_loiter;
 				break;
 			case NAVIGATION_STATE_AUTO_RTL:
-			case NAVIGATION_STATE_AUTO_RTL_RC:
-			case NAVIGATION_STATE_AUTO_RTL_DL:
+			case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS:
+			case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS:
 				_navigation_mode = &_rtl;
 				break;
 			case NAVIGATION_STATE_LAND:
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 259d7329e8..9390ff717f 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -1,10 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *           @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
- *           @author Thomas Gubler <thomasgubler@student.ethz.ch>
- *           @author Julian Oes <joes@student.ethz.ch>
+ *   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
@@ -45,6 +41,11 @@
  * All apps should write to subsystem_info:
  *
  *  (any app) --> subsystem_info (published) --> (commander app state machine)  --> vehicle_status --> (mavlink app)
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <julian@oes.ch>
  */
 
 #ifndef VEHICLE_STATUS_H_
@@ -90,25 +91,25 @@ typedef enum {
 
 typedef enum {
 	FAILSAFE_STATE_NORMAL = 0,		/**< Normal operation */
-	FAILSAFE_STATE_RTL_RC,			/**< Return To Launch on remote control loss */
-	FAILSAFE_STATE_RTL_DL,			/**< Return To Launch on datalink loss */
+	FAILSAFE_STATE_RC_LOSS,			/**< Return To Launch on remote control loss */
+	FAILSAFE_STATE_DL_LOSS,			/**< Return To Launch on datalink loss */
 	FAILSAFE_STATE_LAND,			/**< Land as safe as possible */
 	FAILSAFE_STATE_TERMINATION,		/**< Disable motors and use parachute, can't be recovered */
 	FAILSAFE_STATE_MAX,
 } failsafe_state_t;
 
 typedef enum {
-	NAVIGATION_STATE_MANUAL = 0,
-	NAVIGATION_STATE_ACRO,
-	NAVIGATION_STATE_ALTCTL,
-	NAVIGATION_STATE_POSCTL,
-	NAVIGATION_STATE_AUTO_MISSION,
-	NAVIGATION_STATE_AUTO_LOITER,
-	NAVIGATION_STATE_AUTO_RTL,
-	NAVIGATION_STATE_AUTO_RTL_RC,
-	NAVIGATION_STATE_AUTO_RTL_DL,
-	NAVIGATION_STATE_LAND,
-	NAVIGATION_STATE_TERMINATION,
+	NAVIGATION_STATE_MANUAL = 0,		/**< Manual mode */
+	NAVIGATION_STATE_ACRO,			/**< Acro mode */
+	NAVIGATION_STATE_ALTCTL,		/**< Altitude control mode */
+	NAVIGATION_STATE_POSCTL,		/**< Position control mode */
+	NAVIGATION_STATE_AUTO_MISSION,		/**< Auto mission mode */
+	NAVIGATION_STATE_AUTO_LOITER,		/**< Auto loiter mode */
+	NAVIGATION_STATE_AUTO_RTL,		/**< Auto RTL mode */
+	NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS,	/**< Auto failsafe on RC loss mode */
+	NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS,	/**< Auto failsafe on datalink loss mode */
+	NAVIGATION_STATE_LAND,			/**< Land mode */
+	NAVIGATION_STATE_TERMINATION,		/**< Termination mode */
 } navigation_state_t;
 
 enum VEHICLE_MODE_FLAG {

From 3b39a8a789e347e318375ebe18c583eabec0501c Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 12 Jun 2014 19:37:25 +0200
Subject: [PATCH 74/97] navigator: rename update and reset calls to on_active
 and on_inactive

---
 src/modules/navigator/mission.cpp        |  6 +++---
 src/modules/navigator/mission.h          |  4 ++--
 src/modules/navigator/navigator_main.cpp | 12 ++++++------
 src/modules/navigator/navigator_mode.cpp |  6 +++---
 src/modules/navigator/navigator_mode.h   |  4 ++--
 src/modules/navigator/rtl.cpp            |  6 +++---
 src/modules/navigator/rtl.h              |  4 ++--
 7 files changed, 21 insertions(+), 21 deletions(-)

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index a551df9a24..9244063b19 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -72,7 +72,7 @@ Mission::Mission(Navigator *navigator, const char *name) :
 	/* load initial params */
 	updateParams();
 	/* set initial mission items */
-	reset();
+	on_inactive();
 
 }
 
@@ -81,7 +81,7 @@ Mission::~Mission()
 }
 
 void
-Mission::reset()
+Mission::on_inactive()
 {
 	_first_run = true;
 
@@ -100,7 +100,7 @@ Mission::reset()
 }
 
 bool
-Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet)
+Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
 {
 	bool updated = false;
 
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index a3dd09ecd8..322aaf96af 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -78,12 +78,12 @@ public:
 	/**
 	 * This function is called while the mode is inactive
 	 */
-	virtual void reset();
+	virtual void on_inactive();
 
 	/**
 	 * This function is called while the mode is active
 	 */
-	virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet);
+	virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet);
 
 private:
 	/**
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 20cff5c282..a3c190c7f2 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -365,21 +365,21 @@ Navigator::task_main()
 
 		/* TODO: make list of modes and loop through it */
 		if (_navigation_mode == &_mission) {
-			_update_triplet = _mission.update(&_pos_sp_triplet);
+			_update_triplet = _mission.on_active(&_pos_sp_triplet);
 		} else {
-			_mission.reset();
+			_mission.on_inactive();
 		}
 
 		if (_navigation_mode == &_rtl) {
-			_update_triplet = _rtl.update(&_pos_sp_triplet);
+			_update_triplet = _rtl.on_active(&_pos_sp_triplet);
 		} else {
-			_rtl.reset();
+			_rtl.on_inactive();
 		}
 
 		if (_navigation_mode == &_loiter) {
-			_update_triplet = _loiter.update(&_pos_sp_triplet);
+			_update_triplet = _loiter.on_active(&_pos_sp_triplet);
 		} else {
-			_loiter.reset();
+			_loiter.on_inactive();
 		}
 
 		/* if nothing is running, set position setpoint triplet invalid */
diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp
index c968291901..25e767c2b3 100644
--- a/src/modules/navigator/navigator_mode.cpp
+++ b/src/modules/navigator/navigator_mode.cpp
@@ -48,7 +48,7 @@ NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) :
 	/* load initial params */
 	updateParams();
 	/* set initial mission items */
-	reset();
+	on_inactive();
 }
 
 NavigatorMode::~NavigatorMode()
@@ -56,13 +56,13 @@ NavigatorMode::~NavigatorMode()
 }
 
 void
-NavigatorMode::reset()
+NavigatorMode::on_inactive()
 {
 	_first_run = true;
 }
 
 bool
-NavigatorMode::update(struct position_setpoint_triplet_s *pos_sp_triplet)
+NavigatorMode::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
 {
 	pos_sp_triplet->current.valid = false;
 	_first_run = false;
diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h
index 0844bb94d4..cbb53d91b9 100644
--- a/src/modules/navigator/navigator_mode.h
+++ b/src/modules/navigator/navigator_mode.h
@@ -68,7 +68,7 @@ public:
 	/**
 	 * This function is called while the mode is inactive
 	 */
-	virtual void reset();
+	virtual void on_inactive();
 
 	/**
 	 * This function is called while the mode is active
@@ -76,7 +76,7 @@ public:
 	 * @param position setpoint triplet to set
 	 * @return true if position setpoint triplet has been changed
 	 */
-	virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet);
+	virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet);
 
 protected:
 	Navigator *_navigator;
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index 8888c6b625..c1b1d3f097 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -64,7 +64,7 @@ RTL::RTL(Navigator *navigator, const char *name) :
 	/* load initial params */
 	updateParams();
 	/* initial reset */
-	reset();
+	on_inactive();
 }
 
 RTL::~RTL()
@@ -72,14 +72,14 @@ RTL::~RTL()
 }
 
 void
-RTL::reset()
+RTL::on_inactive()
 {
 	_first_run = true;
 	_rtl_state = RTL_STATE_NONE;
 }
 
 bool
-RTL::update(struct position_setpoint_triplet_s *pos_sp_triplet)
+RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
 {
 	bool updated = false;
 
diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h
index 9836f5064e..bcccf74548 100644
--- a/src/modules/navigator/rtl.h
+++ b/src/modules/navigator/rtl.h
@@ -70,7 +70,7 @@ public:
 	/**
 	 * This function is called while the mode is inactive
 	 */
-	void reset();
+	void on_inactive();
 
 	/**
 	 * This function is called while the mode is active
@@ -78,7 +78,7 @@ public:
 	 * @param position setpoint triplet that needs to be set
 	 * @return true if updated
 	 */
-	bool update(position_setpoint_triplet_s *pos_sp_triplet);
+	bool on_active(position_setpoint_triplet_s *pos_sp_triplet);
 
 
 private:

From 9bb8b12f43de3aa4e7f24c516e7e8e7e9e6c196d Mon Sep 17 00:00:00 2001
From: Andrew Chambers <achamber@gmail.com>
Date: Thu, 12 Jun 2014 15:50:06 -0700
Subject: [PATCH 75/97] Using a vector to store navigation modes

---
 src/modules/navigator/navigator.h        |  4 ++++
 src/modules/navigator/navigator_main.cpp | 29 ++++++++++--------------
 2 files changed, 16 insertions(+), 17 deletions(-)

diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index fe7485f566..dfa939c38a 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -40,6 +40,8 @@
 #ifndef NAVIGATOR_H
 #define NAVIGATOR_H
 
+#include <vector>
+
 #include <systemlib/perf_counter.h>
 
 #include <controllib/blocks.hpp>
@@ -153,6 +155,8 @@ private:
 	Loiter		_loiter;			/**< class that handles loiter */
 	RTL 		_rtl;				/**< class that handles RTL */
 
+	std::vector<NavigatorMode*> _navigation_mode_vector;
+
 	bool		_is_in_loiter;			/**< flags if current position SP can be used to loiter */
 	bool		_update_triplet;		/**< flags if position SP triplet needs to be published */
 
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index a3c190c7f2..ac32d4b22d 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -125,6 +125,11 @@ Navigator::Navigator() :
 	_param_loiter_radius(this, "LOITER_RAD"),
 	_param_takeoff_acceptance_radius(this, "TF_ACC_RAD")
 {
+	/* Create a list of our possible navigation types */
+	_navigation_mode_vector.push_back(&_mission);
+	_navigation_mode_vector.push_back(&_loiter);
+	_navigation_mode_vector.push_back(&_rtl);
+
 	updateParams();
 }
 
@@ -363,23 +368,13 @@ Navigator::task_main()
 				break;
 		}
 
-		/* TODO: make list of modes and loop through it */
-		if (_navigation_mode == &_mission) {
-			_update_triplet = _mission.on_active(&_pos_sp_triplet);
-		} else {
-			_mission.on_inactive();
-		}
-
-		if (_navigation_mode == &_rtl) {
-			_update_triplet = _rtl.on_active(&_pos_sp_triplet);
-		} else {
-			_rtl.on_inactive();
-		}
-
-		if (_navigation_mode == &_loiter) {
-			_update_triplet = _loiter.on_active(&_pos_sp_triplet);
-		} else {
-			_loiter.on_inactive();
+		/* iterate through navigation modes and set active/inactive for each */
+		for(unsigned int i = 0; i < _navigation_mode_vector.size(); i++) {
+			if (_navigation_mode == _navigation_mode_vector[i]) {
+				_update_triplet = _navigation_mode_vector[i]->on_active(&_pos_sp_triplet);
+			} else {
+				_navigation_mode_vector[i]->on_inactive();
+			}
 		}
 
 		/* if nothing is running, set position setpoint triplet invalid */

From e53c2ab98504cf398a48f5051383796a1ad4b85e Mon Sep 17 00:00:00 2001
From: Andrew Chambers <achamber@gmail.com>
Date: Thu, 12 Jun 2014 16:22:04 -0700
Subject: [PATCH 76/97] Switched to using c-type arrays

---
 src/modules/navigator/navigator.h        |  6 +++---
 src/modules/navigator/navigator_main.cpp | 14 +++++++-------
 2 files changed, 10 insertions(+), 10 deletions(-)

diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index dfa939c38a..929de02598 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -40,8 +40,6 @@
 #ifndef NAVIGATOR_H
 #define NAVIGATOR_H
 
-#include <vector>
-
 #include <systemlib/perf_counter.h>
 
 #include <controllib/blocks.hpp>
@@ -60,6 +58,8 @@
 #include "rtl.h"
 #include "geofence.h"
 
+#define NAVIGATOR_MODE_ARRAY_SIZE 3
+
 class Navigator : public control::SuperBlock
 {
 public:
@@ -155,7 +155,7 @@ private:
 	Loiter		_loiter;			/**< class that handles loiter */
 	RTL 		_rtl;				/**< class that handles RTL */
 
-	std::vector<NavigatorMode*> _navigation_mode_vector;
+	NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE];	/**< array of navigation modes */
 
 	bool		_is_in_loiter;			/**< flags if current position SP can be used to loiter */
 	bool		_update_triplet;		/**< flags if position SP triplet needs to be published */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index ac32d4b22d..b91af8e4c0 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -126,9 +126,9 @@ Navigator::Navigator() :
 	_param_takeoff_acceptance_radius(this, "TF_ACC_RAD")
 {
 	/* Create a list of our possible navigation types */
-	_navigation_mode_vector.push_back(&_mission);
-	_navigation_mode_vector.push_back(&_loiter);
-	_navigation_mode_vector.push_back(&_rtl);
+	_navigation_mode_array[0] = &_mission;
+	_navigation_mode_array[1] = &_loiter;
+	_navigation_mode_array[2] = &_rtl;
 
 	updateParams();
 }
@@ -369,11 +369,11 @@ Navigator::task_main()
 		}
 
 		/* iterate through navigation modes and set active/inactive for each */
-		for(unsigned int i = 0; i < _navigation_mode_vector.size(); i++) {
-			if (_navigation_mode == _navigation_mode_vector[i]) {
-				_update_triplet = _navigation_mode_vector[i]->on_active(&_pos_sp_triplet);
+		for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
+			if (_navigation_mode == _navigation_mode_array[i]) {
+				_update_triplet = _navigation_mode_array[i]->on_active(&_pos_sp_triplet);
 			} else {
-				_navigation_mode_vector[i]->on_inactive();
+				_navigation_mode_array[i]->on_inactive();
 			}
 		}
 

From 51f05612ebf056a46d0838fed0dc562d3e5289ad Mon Sep 17 00:00:00 2001
From: Andrew Chambers <achamber@gmail.com>
Date: Thu, 12 Jun 2014 16:33:15 -0700
Subject: [PATCH 77/97] Added some comments

---
 src/modules/navigator/navigator.h | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 929de02598..dadd155271 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -58,6 +58,10 @@
 #include "rtl.h"
 #include "geofence.h"
 
+/**
+ * Number of navigation modes that need on_active/on_inactive calls
+ * Currently: mission, loiter, and rtl
+ */
 #define NAVIGATOR_MODE_ARRAY_SIZE 3
 
 class Navigator : public control::SuperBlock

From e14d4751423e21a9ce052e1f6760f929d3694515 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Fri, 13 Jun 2014 14:31:19 +0200
Subject: [PATCH 78/97] navigator: update Loiter navigator mode to use new
 method names

---
 src/modules/navigator/loiter.cpp | 6 +++---
 src/modules/navigator/loiter.h   | 4 ++--
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp
index 035d035e16..8bbb79d5ee 100644
--- a/src/modules/navigator/loiter.cpp
+++ b/src/modules/navigator/loiter.cpp
@@ -59,7 +59,7 @@ Loiter::Loiter(Navigator *navigator, const char *name) :
 	/* load initial params */
 	updateParams();
 	/* initial reset */
-	reset();
+	on_inactive();
 }
 
 Loiter::~Loiter()
@@ -67,14 +67,14 @@ Loiter::~Loiter()
 }
 
 bool
-Loiter::update(struct position_setpoint_triplet_s *pos_sp_triplet)
+Loiter::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
 {
 	/* set loiter item, don't reuse an existing position setpoint */
 	return set_loiter_item(false, pos_sp_triplet);;
 }
 
 void
-Loiter::reset()
+Loiter::on_inactive()
 {
 }
 
diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h
index a83b53f438..685ae61410 100644
--- a/src/modules/navigator/loiter.h
+++ b/src/modules/navigator/loiter.h
@@ -63,12 +63,12 @@ public:
 	/**
 	 * This function is called while the mode is inactive
 	 */
-	bool update(struct position_setpoint_triplet_s *pos_sp_triplet);
+	virtual void on_inactive();
 
 	/**
 	 * This function is called while the mode is active
 	 */
-	void reset();
+	virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet);
 };
 
 #endif

From 9772aa5814eb2f0d864e89814f5be2cf9f136eda Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sun, 15 Jun 2014 11:59:45 +0200
Subject: [PATCH 79/97] mavlink: set current DO_JUMP repetitions to 0 initially

---
 src/modules/mavlink/mavlink_main.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 3a656ae4da..d7cf45351c 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -900,6 +900,7 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item
 		break;
 	case MAV_CMD_DO_JUMP:
 		mission_item->do_jump_mission_index = mavlink_mission_item->param1;
+		mission_item->do_jump_current_count = 0;
 		mission_item->do_jump_repeat_count = mavlink_mission_item->param2;
 		break;
 	default:

From 91f0b9eee41a8446c0a5ec455fbe3853c5c3eee3 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Mon, 16 Jun 2014 17:32:58 +0200
Subject: [PATCH 80/97] mavlink: store last heartbeat time in telemetry_status
 topic

---
 src/modules/mavlink/mavlink_receiver.cpp   | 18 ++++++++++++++++++
 src/modules/uORB/topics/telemetry_status.h |  1 +
 2 files changed, 19 insertions(+)

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 33358b7b67..32c5e51ddb 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -106,6 +106,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_telemetry_status_pub(-1),
 	_rc_pub(-1),
 	_manual_pub(-1),
+	_telemetry_heartbeat_time(0),
 	_hil_frames(0),
 	_old_timestamp(0),
 	_hil_local_proj_inited(0),
@@ -150,6 +151,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_manual_control(msg);
 		break;
 
+	case MAVLINK_MSG_ID_HEARTBEAT:
+		handle_message_heartbeat(msg);
+		break;
+
 	default:
 		break;
 	}
@@ -411,6 +416,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 	memset(&tstatus, 0, sizeof(tstatus));
 
 	tstatus.timestamp = hrt_absolute_time();
+	tstatus.heartbeat_time = _telemetry_heartbeat_time;
 	tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
 	tstatus.rssi = rstatus.rssi;
 	tstatus.remote_rssi = rstatus.remrssi;
@@ -451,6 +457,18 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 	}
 }
 
+void
+MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
+{
+	mavlink_heartbeat_t hb;
+	mavlink_msg_heartbeat_decode(msg, &hb);
+
+	/* ignore own heartbeats, accept only heartbeats from GCS */
+	if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) {
+		_telemetry_heartbeat_time = hrt_absolute_time();
+	}
+}
+
 void
 MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 {
diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h
index 76693c46ef..e9e00d76cb 100644
--- a/src/modules/uORB/topics/telemetry_status.h
+++ b/src/modules/uORB/topics/telemetry_status.h
@@ -57,6 +57,7 @@ enum TELEMETRY_STATUS_RADIO_TYPE {
 
 struct telemetry_status_s {
 	uint64_t timestamp;
+	uint64_t heartbeat_time;	/**< Time of last received heartbeat from remote system */
 	enum TELEMETRY_STATUS_RADIO_TYPE type;	/**< type of the radio hardware     */
 	uint8_t rssi;				/**< local signal strength                      */
 	uint8_t remote_rssi;			/**< remote signal strength                     */

From e0ed0625f81841194b4bd9b26c7e047a1f68a1fc Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Mon, 16 Jun 2014 17:34:21 +0200
Subject: [PATCH 81/97] commander: failsafe_state removed, replaced with bool
 failsafe, navigation state and failsafe determined directly from main state
 and conditions

---
 src/modules/commander/commander.cpp           |  90 +++++++--------
 .../commander/state_machine_helper.cpp        | 106 +++++++++++-------
 src/modules/commander/state_machine_helper.h  |   2 -
 src/modules/gpio_led/gpio_led.c               |   2 +-
 src/modules/mavlink/mavlink_messages.cpp      |   4 +-
 src/modules/mavlink/mavlink_receiver.h        |   2 +
 src/modules/navigator/navigator_main.cpp      |   4 +-
 src/modules/sdlog2/sdlog2.c                   |   2 +-
 src/modules/uORB/topics/vehicle_status.h      |  25 ++---
 9 files changed, 122 insertions(+), 115 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index e992706ac3..43683f833d 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -77,6 +77,7 @@
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/safety.h>
 #include <uORB/topics/mission_result.h>
+#include <uORB/topics/telemetry_status.h>
 
 #include <drivers/drv_led.h>
 #include <drivers/drv_hrt.h>
@@ -122,6 +123,7 @@ extern struct system_load_s system_load;
 #define POSITION_TIMEOUT		(600 * 1000)		/**< consider the local or global position estimate invalid after 600ms */
 #define FAILSAFE_DEFAULT_TIMEOUT	(3 * 1000 * 1000)	/**< hysteresis time - the failsafe will trigger after 3 seconds in this state */
 #define RC_TIMEOUT			500000
+#define DL_TIMEOUT			5 * 1000* 1000
 #define DIFFPRESS_TIMEOUT		2000000
 
 #define PRINT_INTERVAL	5000000
@@ -495,12 +497,12 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			unsigned int mav_goto = cmd->param1;
 
 			if (mav_goto == 0) {	// MAV_GOTO_DO_HOLD
-				status->set_nav_state = NAVIGATION_STATE_AUTO_LOITER;
+				status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
 				mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd");
 				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else if (mav_goto == 1) {	// MAV_GOTO_DO_CONTINUE
-				status->set_nav_state = NAVIGATION_STATE_AUTO_MISSION;
+				status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
 				mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd");
 				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 
@@ -639,13 +641,6 @@ int commander_thread_main(int argc, char *argv[])
 	arming_states_str[5] = "REBOOT";
 	arming_states_str[6] = "IN_AIR_RESTORE";
 
-	char *failsafe_states_str[FAILSAFE_STATE_MAX];
-	failsafe_states_str[0] = "NORMAL";
-	failsafe_states_str[1] = "RTL_RC";
-	failsafe_states_str[2] = "RTL_DL";
-	failsafe_states_str[3] = "LAND";
-	failsafe_states_str[4] = "TERMINATION";
-
 	/* pthread for slow low prio thread */
 	pthread_t commander_low_prio_thread;
 
@@ -666,10 +661,10 @@ int commander_thread_main(int argc, char *argv[])
 	// We want to accept RC inputs as default
 	status.rc_input_blocked = false;
 	status.main_state = MAIN_STATE_MANUAL;
-	status.set_nav_state = NAVIGATION_STATE_MANUAL;
+	status.nav_state = NAVIGATION_STATE_MANUAL;
 	status.arming_state = ARMING_STATE_INIT;
 	status.hil_state = HIL_STATE_OFF;
-	status.failsafe_state = FAILSAFE_STATE_NORMAL;
+	status.failsafe = false;
 
 	/* neither manual nor offboard control commands have been received */
 	status.offboard_control_signal_found_once = false;
@@ -678,6 +673,7 @@ int commander_thread_main(int argc, char *argv[])
 	/* mark all signals lost as long as they haven't been found */
 	status.rc_signal_lost = true;
 	status.offboard_control_signal_lost = true;
+	status.data_link_lost = true;
 
 	/* set battery warning flag */
 	status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
@@ -771,6 +767,11 @@ int commander_thread_main(int argc, char *argv[])
 	struct offboard_control_setpoint_s sp_offboard;
 	memset(&sp_offboard, 0, sizeof(sp_offboard));
 
+	/* Subscribe to telemetry status */
+	int telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
+	struct telemetry_status_s telemetry;
+	memset(&telemetry, 0, sizeof(telemetry));
+
 	/* Subscribe to global position */
 	int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 	struct vehicle_global_position_s global_position;
@@ -908,6 +909,12 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
 		}
 
+		orb_check(telemetry_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(telemetry_status), telemetry_sub, &telemetry);
+		}
+
 		orb_check(sensor_sub, &updated);
 
 		if (updated) {
@@ -1186,7 +1193,7 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
 		}
 
-		/* start RC input check */
+		/* RC input check */
 		if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
 			/* handle the case where RC signal was regained */
 			if (!status.rc_signal_found_once) {
@@ -1198,9 +1205,6 @@ int commander_thread_main(int argc, char *argv[])
 				if (status.rc_signal_lost) {
 					mavlink_log_critical(mavlink_fd, "#audio: RC signal regained");
 					status_changed = true;
-
-					status.failsafe_state = FAILSAFE_STATE_NORMAL;
-					failsafe_state_changed = true;
 				}
 			}
 
@@ -1271,12 +1275,6 @@ int commander_thread_main(int argc, char *argv[])
 				mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied");
 			}
 
-
-			// if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
-			//	/* recover from failsafe */
-			//	(void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
-			// }
-
 			/* evaluate the main state machine according to mode switches */
 			transition_result_t main_res = set_main_state_rc(&status, &sp_man);
 
@@ -1295,34 +1293,24 @@ int commander_thread_main(int argc, char *argv[])
 				mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST");
 				status.rc_signal_lost = true;
 				status_changed = true;
-
-				if (!(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.mission_finished)) {
-
-					/* if we have a global position, we can switch to RTL, if not, we can try to land */
-					if (status.condition_global_position_valid) {
-						status.failsafe_state = FAILSAFE_STATE_RC_LOSS;
-					} else {
-						status.failsafe_state = FAILSAFE_STATE_LAND;
-					}
-					failsafe_state_changed = true;
-				} else {
-					mavlink_log_info(mavlink_fd, "#audio: no RTL during Mission");
-				}
 			}
 		}
 
-		/* hack to detect if we finished a mission after we lost RC, so that we can trigger RTL now */
-		if (status.rc_signal_lost && status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION &&
-		    mission_result.mission_finished && status.failsafe_state != FAILSAFE_STATE_RC_LOSS) {
-			/* if we have a global position, we can switch to RTL, if not, we can try to land */
-			if (status.condition_global_position_valid) {
-				status.failsafe_state = FAILSAFE_STATE_RC_LOSS;
-				mavlink_log_info(mavlink_fd, "#audio: RTL after Mission is finished");
-			} else {
-				/* this probably doesn't make sense since we are in mission and have global position */
-				status.failsafe_state = FAILSAFE_STATE_LAND;
+		/* data link check */
+		if (hrt_abstime() < telemetry.heartbeat_time + DL_TIMEOUT) {
+			/* handle the case where RC signal was regained */
+			if (status.data_link_lost) {
+				mavlink_log_critical(mavlink_fd, "#audio: data link regained");
+				status.data_link_lost = false;
+				status_changed = true;
+			}
+
+		} else {
+			if (!status.data_link_lost) {
+				mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST");
+				status.data_link_lost = false;
+				status_changed = true;
 			}
-			failsafe_state_changed = true;
 		}
 
 		/* handle commands last, as the system needs to be updated to handle them */
@@ -1377,7 +1365,7 @@ int commander_thread_main(int argc, char *argv[])
 
 		was_armed = armed.armed;
 
-		/* now set nav state according to failsafe and main state */
+		/* now set navigation state according to failsafe and main state */
 		set_nav_state(&status);
 
 		if (main_state_changed) {
@@ -1388,7 +1376,7 @@ int commander_thread_main(int argc, char *argv[])
 
 		if (failsafe_state_changed) {
 			status_changed = true;
-			mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %s", failsafe_states_str[status.failsafe_state]);
+			mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %i", status.failsafe);
 			failsafe_state_changed = false;
 		}
 
@@ -1415,7 +1403,7 @@ int commander_thread_main(int argc, char *argv[])
 			/* play tune on battery critical */
 			set_tune(TONE_BATTERY_WARNING_FAST_TUNE);
 
-		} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe_state != FAILSAFE_STATE_NORMAL) {
+		} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe) {
 			/* play tune on battery warning or failsafe */
 			set_tune(TONE_BATTERY_WARNING_SLOW_TUNE);
 
@@ -1519,7 +1507,7 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
 
 		if (set_normal_color) {
 			/* set color */
-			if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe_state != FAILSAFE_STATE_NORMAL) {
+			if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe) {
 				rgbled_set_color(RGBLED_COLOR_AMBER);
 				/* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */
 
@@ -1667,7 +1655,7 @@ set_control_mode()
 	control_mode.flag_external_manual_override_ok = !status.is_rotary_wing;
 	control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON;
 
-	switch (status.set_nav_state) {
+	switch (status.nav_state) {
 	case NAVIGATION_STATE_MANUAL:
 		control_mode.flag_control_manual_enabled = true;
 		control_mode.flag_control_auto_enabled = false;
@@ -1719,8 +1707,6 @@ set_control_mode()
 	case NAVIGATION_STATE_AUTO_MISSION:
 	case NAVIGATION_STATE_AUTO_LOITER:
 	case NAVIGATION_STATE_AUTO_RTL:
-	case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS:
-	case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS:
 		control_mode.flag_control_manual_enabled = false;
 		control_mode.flag_control_auto_enabled = true;
 		control_mode.flag_control_rates_enabled = true;
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index c52e618ef1..df718ff99f 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -354,7 +354,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
 		}
 	}
 
-	if (ret = TRANSITION_CHANGED) {
+	if (ret == TRANSITION_CHANGED) {
 		current_status->hil_state = new_state;
 		current_status->timestamp = hrt_absolute_time();
 		// XXX also set lockdown here
@@ -363,67 +363,95 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
 	return ret;
 }
 
-
 /**
  * Check failsafe and main status and set navigation status for navigator accordingly
  */
 void set_nav_state(struct vehicle_status_s *status)
 {
-	switch (status->failsafe_state) {
-	case FAILSAFE_STATE_NORMAL:
-		/* evaluate main state to decide in normal (non-failsafe) mode */
-		switch (status->main_state) {
-		case MAIN_STATE_MANUAL:
-			status->set_nav_state = NAVIGATION_STATE_MANUAL;
-			break;
+	status->failsafe = false;
 
-		case MAIN_STATE_ALTCTL:
-			status->set_nav_state = NAVIGATION_STATE_ALTCTL;
-			break;
+	/* evaluate main state to decide in normal (non-failsafe) mode */
+	switch (status->main_state) {
+	case MAIN_STATE_ACRO:
+	case MAIN_STATE_MANUAL:
+	case MAIN_STATE_ALTCTL:
+	case MAIN_STATE_POSCTL:
+		/* require RC for all manual modes */
+		if (status->rc_signal_lost) {
+			status->failsafe = true;
 
-		case MAIN_STATE_POSCTL:
-			status->set_nav_state = NAVIGATION_STATE_POSCTL;
-			break;
+		} else {
+			switch (status->main_state) {
+			case MAIN_STATE_ACRO:
+				status->nav_state = NAVIGATION_STATE_ACRO;
+				break;
 
-		case MAIN_STATE_AUTO_MISSION:
-			status->set_nav_state = NAVIGATION_STATE_AUTO_MISSION;
-			break;
+			case MAIN_STATE_MANUAL:
+				status->nav_state = NAVIGATION_STATE_MANUAL;
+				break;
 
-		case MAIN_STATE_AUTO_LOITER:
-			status->set_nav_state = NAVIGATION_STATE_AUTO_LOITER;
-			break;
+			case MAIN_STATE_ALTCTL:
+				status->nav_state = NAVIGATION_STATE_ALTCTL;
+				break;
 
-		case MAIN_STATE_AUTO_RTL:
-			status->set_nav_state = NAVIGATION_STATE_AUTO_RTL;
-			break;
+			case MAIN_STATE_POSCTL:
+				status->nav_state = NAVIGATION_STATE_POSCTL;
+				break;
 
-		case MAIN_STATE_ACRO:
-			status->set_nav_state = NAVIGATION_STATE_ACRO;
-			break;
-
-		default:
-			break;
+			default:
+				status->nav_state = NAVIGATION_STATE_MANUAL;
+				break;
+			}
 		}
 		break;
 
-	case FAILSAFE_STATE_RC_LOSS:
-		status->set_nav_state = NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS;
+	case MAIN_STATE_AUTO_MISSION:
+		/* require data link and global position */
+		if (status->data_link_lost || !status->condition_global_position_valid) {
+			status->failsafe = true;
+
+		} else {
+			status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
+		}
 		break;
 
-	case FAILSAFE_STATE_DL_LOSS:
-		status->set_nav_state = NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS;
+	case MAIN_STATE_AUTO_LOITER:
+		/* require data link and local position */
+		if (status->data_link_lost || !status->condition_local_position_valid) {
+			status->failsafe = true;
+
+		} else {
+			status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
+		}
 		break;
 
-	case FAILSAFE_STATE_LAND:
-		status->set_nav_state = NAVIGATION_STATE_LAND;
-		break;
+	case MAIN_STATE_AUTO_RTL:
+		/* require global position and home */
+		if (!status->condition_global_position_valid || !status->condition_home_position_valid) {
+			status->failsafe = true;
 
-	case FAILSAFE_STATE_TERMINATION:
-		status->set_nav_state = NAVIGATION_STATE_TERMINATION;
+		} else {
+			status->nav_state = NAVIGATION_STATE_AUTO_RTL;
+		}
 		break;
 
 	default:
 		break;
 	}
+
+	if (status->failsafe) {
+		if (status->condition_global_position_valid && status->condition_home_position_valid) {
+			status->nav_state = NAVIGATION_STATE_AUTO_RTL;
+
+		} else if (status->condition_local_position_valid) {
+			status->nav_state = NAVIGATION_STATE_LAND;
+
+		} else if (status->condition_local_altitude_valid) {
+			status->nav_state = NAVIGATION_STATE_DESCEND;
+
+		} else {
+			status->nav_state = NAVIGATION_STATE_TERMINATION;
+		}
+	}
 }
 
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index 594bf23a1d..0622717642 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -63,8 +63,6 @@ transition_result_t arming_state_transition(struct vehicle_status_s *current_sta
 
 transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state);
 
-transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state);
-
 transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd);
 
 void set_nav_state(struct vehicle_status_s *status);
diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c
index 6dfd22fdff..839a7890bb 100644
--- a/src/modules/gpio_led/gpio_led.c
+++ b/src/modules/gpio_led/gpio_led.c
@@ -264,7 +264,7 @@ void gpio_led_cycle(FAR void *arg)
 		pattern = 0x2A;	// *_*_*_ fast blink (armed, error)
 
 	} else if (priv->status.arming_state == ARMING_STATE_ARMED) {
-		if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE && priv->status.failsafe_state == FAILSAFE_STATE_NORMAL) {
+		if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE && !priv->status.failsafe) {
 			pattern = 0x3f;	// ****** solid (armed)
 
 		} else {
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index b58fb4cb8a..d7f56b63d3 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -118,7 +118,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 	union px4_custom_mode custom_mode;
 	custom_mode.data = 0;
 
-	switch (status->set_nav_state) {
+	switch (status->nav_state) {
 
 		case NAVIGATION_STATE_MANUAL:
 			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
@@ -161,8 +161,6 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 			break;
 
 		case NAVIGATION_STATE_AUTO_RTL:
-		case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS:
-		case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS:
 			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
 			                      | MAV_MODE_FLAG_STABILIZE_ENABLED
 					      | MAV_MODE_FLAG_GUIDED_ENABLED;
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 9ab84b58a3..ab3dc81c6b 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -112,6 +112,7 @@ private:
 	void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg);
 	void handle_message_radio_status(mavlink_message_t *msg);
 	void handle_message_manual_control(mavlink_message_t *msg);
+	void handle_message_heartbeat(mavlink_message_t *msg);
 	void handle_message_hil_sensor(mavlink_message_t *msg);
 	void handle_message_hil_gps(mavlink_message_t *msg);
 	void handle_message_hil_state_quaternion(mavlink_message_t *msg);
@@ -138,6 +139,7 @@ private:
 	orb_advert_t _telemetry_status_pub;
 	orb_advert_t _rc_pub;
 	orb_advert_t _manual_pub;
+	hrt_abstime _telemetry_heartbeat_time;
 	int _hil_frames;
 	uint64_t _old_timestamp;
 	bool _hil_local_proj_inited;
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index a3c190c7f2..70da5393f1 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -336,7 +336,7 @@ Navigator::task_main()
 		}
 
 		/* Do stuff according to navigation state set by commander */
-		switch (_vstatus.set_nav_state) {
+		switch (_vstatus.nav_state) {
 			case NAVIGATION_STATE_MANUAL:
 			case NAVIGATION_STATE_ACRO:
 			case NAVIGATION_STATE_ALTCTL:
@@ -351,8 +351,6 @@ Navigator::task_main()
 				_navigation_mode = &_loiter;
 				break;
 			case NAVIGATION_STATE_AUTO_RTL:
-			case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS:
-			case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS:
 				_navigation_mode = &_rtl;
 				break;
 			case NAVIGATION_STATE_LAND:
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 53cd6a7970..19872bbd08 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1117,7 +1117,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.msg_type = LOG_STAT_MSG;
 			log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state;
 			log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state;
-			log_msg.body.log_STAT.failsafe_state = (uint8_t) buf_status.failsafe_state;
+			log_msg.body.log_STAT.failsafe_state = (uint8_t) buf_status.failsafe;
 			log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
 			log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning;
 			log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed;
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 9390ff717f..898c176fb2 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -59,7 +59,9 @@
  * @addtogroup topics @{
  */
 
-/* main state machine */
+/**
+ * Main state, i.e. what user wants. Controlled by RC or from ground station via telemetry link.
+ */
 typedef enum {
 	MAIN_STATE_MANUAL = 0,
 	MAIN_STATE_ALTCTL,
@@ -89,15 +91,9 @@ typedef enum {
 	HIL_STATE_ON
 } hil_state_t;
 
-typedef enum {
-	FAILSAFE_STATE_NORMAL = 0,		/**< Normal operation */
-	FAILSAFE_STATE_RC_LOSS,			/**< Return To Launch on remote control loss */
-	FAILSAFE_STATE_DL_LOSS,			/**< Return To Launch on datalink loss */
-	FAILSAFE_STATE_LAND,			/**< Land as safe as possible */
-	FAILSAFE_STATE_TERMINATION,		/**< Disable motors and use parachute, can't be recovered */
-	FAILSAFE_STATE_MAX,
-} failsafe_state_t;
-
+/**
+ * Navigation state, i.e. "what should vehicle do".
+ */
 typedef enum {
 	NAVIGATION_STATE_MANUAL = 0,		/**< Manual mode */
 	NAVIGATION_STATE_ACRO,			/**< Acro mode */
@@ -106,9 +102,8 @@ typedef enum {
 	NAVIGATION_STATE_AUTO_MISSION,		/**< Auto mission mode */
 	NAVIGATION_STATE_AUTO_LOITER,		/**< Auto loiter mode */
 	NAVIGATION_STATE_AUTO_RTL,		/**< Auto RTL mode */
-	NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS,	/**< Auto failsafe on RC loss mode */
-	NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS,	/**< Auto failsafe on datalink loss mode */
 	NAVIGATION_STATE_LAND,			/**< Land mode */
+	NAVIGATION_STATE_DESCEND,			/**< Descend mode (no position control) */
 	NAVIGATION_STATE_TERMINATION,		/**< Termination mode */
 } navigation_state_t;
 
@@ -171,10 +166,10 @@ struct vehicle_status_s {
 	uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
 
 	main_state_t main_state;		    	/**< main state machine */
-	navigation_state_t set_nav_state;		/**< set navigation state machine to specified value */
+	navigation_state_t nav_state;		/**< set navigation state machine to specified value */
 	arming_state_t arming_state;			/**< current arming state */
 	hil_state_t hil_state;				/**< current hil state */
-	failsafe_state_t failsafe_state;		/**< current failsafe state */
+	bool failsafe;					/**< true if system is in failsafe state */
 
 	int32_t system_type;				/**< system type, inspired by MAVLink's VEHICLE_TYPE enum */
 	int32_t	system_id;				/**< system id, inspired by MAVLink's system ID field */
@@ -199,6 +194,8 @@ struct vehicle_status_s {
 	bool rc_signal_lost;				/**< true if RC reception lost */
 	bool rc_input_blocked;				/**< set if RC input should be ignored */
 
+	bool data_link_lost;						/**< datalink to GCS lost */
+
 	bool offboard_control_signal_found_once;
 	bool offboard_control_signal_lost;
 	bool offboard_control_signal_weak;

From 55e5f747de013fb386727b41cc67bd019c129c31 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Tue, 17 Jun 2014 13:19:50 +0200
Subject: [PATCH 82/97] commander: modes display fixes, don't activate failsafe
 while disarmed

---
 src/modules/commander/commander.cpp           | 56 +++++++++++++------
 .../commander/state_machine_helper.cpp        | 32 ++++++++---
 src/modules/commander/state_machine_helper.h  |  2 +-
 src/modules/uORB/topics/vehicle_status.h      |  3 +-
 4 files changed, 66 insertions(+), 27 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 43683f833d..f8dcec8cb8 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -624,22 +624,34 @@ int commander_thread_main(int argc, char *argv[])
 	warnx("starting");
 
 	char *main_states_str[MAIN_STATE_MAX];
-	main_states_str[0] = "MANUAL";
-	main_states_str[1] = "ALTCTL";
-	main_states_str[2] = "POSCTL";
-	main_states_str[3] = "AUTO_MISSION";
-	main_states_str[4] = "AUTO_LOITER";
-	main_states_str[5] = "AUTO_RTL";
-	main_states_str[6] = "ACRO";
+	main_states_str[MAIN_STATE_MANUAL]			= "MANUAL";
+	main_states_str[MAIN_STATE_ALTCTL]			= "ALTCTL";
+	main_states_str[MAIN_STATE_POSCTL]			= "POSCTL";
+	main_states_str[MAIN_STATE_AUTO_MISSION]	= "AUTO_MISSION";
+	main_states_str[MAIN_STATE_AUTO_LOITER]		= "AUTO_LOITER";
+	main_states_str[MAIN_STATE_AUTO_RTL]		= "AUTO_RTL";
+	main_states_str[MAIN_STATE_ACRO]			= "ACRO";
 
 	char *arming_states_str[ARMING_STATE_MAX];
-	arming_states_str[0] = "INIT";
-	arming_states_str[1] = "STANDBY";
-	arming_states_str[2] = "ARMED";
-	arming_states_str[3] = "ARMED_ERROR";
-	arming_states_str[4] = "STANDBY_ERROR";
-	arming_states_str[5] = "REBOOT";
-	arming_states_str[6] = "IN_AIR_RESTORE";
+	arming_states_str[ARMING_STATE_INIT]			= "INIT";
+	arming_states_str[ARMING_STATE_STANDBY]			= "STANDBY";
+	arming_states_str[ARMING_STATE_ARMED]			= "ARMED";
+	arming_states_str[ARMING_STATE_ARMED_ERROR]		= "ARMED_ERROR";
+	arming_states_str[ARMING_STATE_STANDBY_ERROR]	= "STANDBY_ERROR";
+	arming_states_str[ARMING_STATE_REBOOT]			= "REBOOT";
+	arming_states_str[ARMING_STATE_IN_AIR_RESTORE]	= "IN_AIR_RESTORE";
+
+	char *nav_states_str[NAVIGATION_STATE_MAX];
+	nav_states_str[NAVIGATION_STATE_MANUAL]		= "MANUAL";
+	nav_states_str[NAVIGATION_STATE_ALTCTL]		= "ALTCTL";
+	nav_states_str[NAVIGATION_STATE_POSCTL]		= "POSCTL";
+	nav_states_str[NAVIGATION_STATE_AUTO_MISSION]	= "AUTO_MISSION";
+	nav_states_str[NAVIGATION_STATE_AUTO_LOITER]	= "AUTO_LOITER";
+	nav_states_str[NAVIGATION_STATE_AUTO_RTL]		= "AUTO_RTL";
+	nav_states_str[NAVIGATION_STATE_ACRO]			= "ACRO";
+	nav_states_str[NAVIGATION_STATE_LAND]			= "LAND";
+	nav_states_str[NAVIGATION_STATE_DESCEND]		= "DESCEND";
+	nav_states_str[NAVIGATION_STATE_TERMINATION]	= "TERMINATION";
 
 	/* pthread for slow low prio thread */
 	pthread_t commander_low_prio_thread;
@@ -844,7 +856,7 @@ int commander_thread_main(int argc, char *argv[])
 	/* check which state machines for changes, clear "changed" flag */
 	bool arming_state_changed = false;
 	bool main_state_changed = false;
-	bool failsafe_state_changed = false;
+	bool failsafe_old = false;
 
 	while (!thread_should_exit) {
 
@@ -1366,18 +1378,26 @@ int commander_thread_main(int argc, char *argv[])
 		was_armed = armed.armed;
 
 		/* now set navigation state according to failsafe and main state */
-		set_nav_state(&status);
+		bool nav_state_changed = set_nav_state(&status);
 
+		// TODO handle mode changes by commands
 		if (main_state_changed) {
 			status_changed = true;
+			warnx("main state: %s", main_states_str[status.main_state]);
 			mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]);
 			main_state_changed = false;
 		}
 
-		if (failsafe_state_changed) {
+		if (status.failsafe != failsafe_old) {
 			status_changed = true;
 			mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %i", status.failsafe);
-			failsafe_state_changed = false;
+			failsafe_old = status.failsafe;
+		}
+
+		if (nav_state_changed) {
+			status_changed = true;
+			warnx("nav state: %s", nav_states_str[status.nav_state]);
+			mavlink_log_info(mavlink_fd, "[cmd] nav state: %s", nav_states_str[status.nav_state]);
 		}
 
 		/* publish states (armed, control mode, vehicle status) at least with 5 Hz */
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index df718ff99f..c0d7309495 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -366,8 +366,11 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
 /**
  * Check failsafe and main status and set navigation status for navigator accordingly
  */
-void set_nav_state(struct vehicle_status_s *status)
+bool set_nav_state(struct vehicle_status_s *status)
 {
+	navigation_state_t nav_state_old = status->nav_state;
+
+	bool armed = (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR);
 	status->failsafe = false;
 
 	/* evaluate main state to decide in normal (non-failsafe) mode */
@@ -377,7 +380,7 @@ void set_nav_state(struct vehicle_status_s *status)
 	case MAIN_STATE_ALTCTL:
 	case MAIN_STATE_POSCTL:
 		/* require RC for all manual modes */
-		if (status->rc_signal_lost) {
+		if (status->rc_signal_lost && armed) {
 			status->failsafe = true;
 
 		} else {
@@ -407,31 +410,44 @@ void set_nav_state(struct vehicle_status_s *status)
 
 	case MAIN_STATE_AUTO_MISSION:
 		/* require data link and global position */
-		if (status->data_link_lost || !status->condition_global_position_valid) {
+		if ((status->data_link_lost || !status->condition_global_position_valid) && armed) {
 			status->failsafe = true;
 
 		} else {
-			status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
+			if (armed) {
+				status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
+
+			} else {
+				// TODO which mode should we set when disarmed?
+				status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
+			}
 		}
 		break;
 
 	case MAIN_STATE_AUTO_LOITER:
 		/* require data link and local position */
-		if (status->data_link_lost || !status->condition_local_position_valid) {
+		if ((status->data_link_lost || !status->condition_local_position_valid) && armed) {
 			status->failsafe = true;
 
 		} else {
+			// TODO which mode should we set when disarmed?
 			status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
 		}
 		break;
 
 	case MAIN_STATE_AUTO_RTL:
 		/* require global position and home */
-		if (!status->condition_global_position_valid || !status->condition_home_position_valid) {
+		if ((!status->condition_global_position_valid || !status->condition_home_position_valid) && armed) {
 			status->failsafe = true;
 
 		} else {
-			status->nav_state = NAVIGATION_STATE_AUTO_RTL;
+			if (armed) {
+				status->nav_state = NAVIGATION_STATE_AUTO_RTL;
+
+			} else {
+				// TODO which mode should we set when disarmed?
+				status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
+			}
 		}
 		break;
 
@@ -453,5 +469,7 @@ void set_nav_state(struct vehicle_status_s *status)
 			status->nav_state = NAVIGATION_STATE_TERMINATION;
 		}
 	}
+
+	return status->nav_state != nav_state_old;
 }
 
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index 0622717642..cffbc9b8f7 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -65,6 +65,6 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state
 
 transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd);
 
-void set_nav_state(struct vehicle_status_s *status);
+bool set_nav_state(struct vehicle_status_s *status);
 
 #endif /* STATE_MACHINE_HELPER_H_ */
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 898c176fb2..8519387958 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -96,15 +96,16 @@ typedef enum {
  */
 typedef enum {
 	NAVIGATION_STATE_MANUAL = 0,		/**< Manual mode */
-	NAVIGATION_STATE_ACRO,			/**< Acro mode */
 	NAVIGATION_STATE_ALTCTL,		/**< Altitude control mode */
 	NAVIGATION_STATE_POSCTL,		/**< Position control mode */
 	NAVIGATION_STATE_AUTO_MISSION,		/**< Auto mission mode */
 	NAVIGATION_STATE_AUTO_LOITER,		/**< Auto loiter mode */
 	NAVIGATION_STATE_AUTO_RTL,		/**< Auto RTL mode */
+	NAVIGATION_STATE_ACRO,			/**< Acro mode */
 	NAVIGATION_STATE_LAND,			/**< Land mode */
 	NAVIGATION_STATE_DESCEND,			/**< Descend mode (no position control) */
 	NAVIGATION_STATE_TERMINATION,		/**< Termination mode */
+	NAVIGATION_STATE_MAX,
 } navigation_state_t;
 
 enum VEHICLE_MODE_FLAG {

From e24925c743330bc3c6c0a24ba913a9c5fab5e07d Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 18 Jun 2014 19:01:41 +0200
Subject: [PATCH 83/97] commander: added some failsafe logic

---
 src/modules/commander/commander.cpp           |  39 +++--
 src/modules/commander/commander_params.c      |  13 +-
 .../commander/state_machine_helper.cpp        | 134 +++++++++++++-----
 src/modules/commander/state_machine_helper.h  |   2 +-
 src/modules/navigator/navigator_main.cpp      |   4 +
 src/modules/uORB/topics/vehicle_status.h      |   2 +
 6 files changed, 143 insertions(+), 51 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index f8dcec8cb8..5954dcbb1e 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -619,6 +619,7 @@ int commander_thread_main(int argc, char *argv[])
 	param_t _param_component_id = param_find("MAV_COMP_ID");
 	param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT");
 	param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN");
+	param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN");
 
 	/* welcome user */
 	warnx("starting");
@@ -627,9 +628,9 @@ int commander_thread_main(int argc, char *argv[])
 	main_states_str[MAIN_STATE_MANUAL]			= "MANUAL";
 	main_states_str[MAIN_STATE_ALTCTL]			= "ALTCTL";
 	main_states_str[MAIN_STATE_POSCTL]			= "POSCTL";
-	main_states_str[MAIN_STATE_AUTO_MISSION]	= "AUTO_MISSION";
-	main_states_str[MAIN_STATE_AUTO_LOITER]		= "AUTO_LOITER";
-	main_states_str[MAIN_STATE_AUTO_RTL]		= "AUTO_RTL";
+	main_states_str[MAIN_STATE_AUTO_MISSION]		= "AUTO_MISSION";
+	main_states_str[MAIN_STATE_AUTO_LOITER]			= "AUTO_LOITER";
+	main_states_str[MAIN_STATE_AUTO_RTL]			= "AUTO_RTL";
 	main_states_str[MAIN_STATE_ACRO]			= "ACRO";
 
 	char *arming_states_str[ARMING_STATE_MAX];
@@ -637,21 +638,23 @@ int commander_thread_main(int argc, char *argv[])
 	arming_states_str[ARMING_STATE_STANDBY]			= "STANDBY";
 	arming_states_str[ARMING_STATE_ARMED]			= "ARMED";
 	arming_states_str[ARMING_STATE_ARMED_ERROR]		= "ARMED_ERROR";
-	arming_states_str[ARMING_STATE_STANDBY_ERROR]	= "STANDBY_ERROR";
+	arming_states_str[ARMING_STATE_STANDBY_ERROR]		= "STANDBY_ERROR";
 	arming_states_str[ARMING_STATE_REBOOT]			= "REBOOT";
-	arming_states_str[ARMING_STATE_IN_AIR_RESTORE]	= "IN_AIR_RESTORE";
+	arming_states_str[ARMING_STATE_IN_AIR_RESTORE]		= "IN_AIR_RESTORE";
 
 	char *nav_states_str[NAVIGATION_STATE_MAX];
-	nav_states_str[NAVIGATION_STATE_MANUAL]		= "MANUAL";
-	nav_states_str[NAVIGATION_STATE_ALTCTL]		= "ALTCTL";
-	nav_states_str[NAVIGATION_STATE_POSCTL]		= "POSCTL";
-	nav_states_str[NAVIGATION_STATE_AUTO_MISSION]	= "AUTO_MISSION";
-	nav_states_str[NAVIGATION_STATE_AUTO_LOITER]	= "AUTO_LOITER";
+	nav_states_str[NAVIGATION_STATE_MANUAL]			= "MANUAL";
+	nav_states_str[NAVIGATION_STATE_ALTCTL]			= "ALTCTL";
+	nav_states_str[NAVIGATION_STATE_POSCTL]			= "POSCTL";
+	nav_states_str[NAVIGATION_STATE_AUTO_MISSION]		= "AUTO_MISSION";
+	nav_states_str[NAVIGATION_STATE_AUTO_LOITER]		= "AUTO_LOITER";
 	nav_states_str[NAVIGATION_STATE_AUTO_RTL]		= "AUTO_RTL";
+	nav_states_str[NAVIGATION_STATE_AUTO_FS_RC_LOSS]	= "AUTO_FS_RC_LOSS";
+	nav_states_str[NAVIGATION_STATE_AUTO_FS_DL_LOSS]	= "AUTO_FS_DL_LOSS";
 	nav_states_str[NAVIGATION_STATE_ACRO]			= "ACRO";
 	nav_states_str[NAVIGATION_STATE_LAND]			= "LAND";
 	nav_states_str[NAVIGATION_STATE_DESCEND]		= "DESCEND";
-	nav_states_str[NAVIGATION_STATE_TERMINATION]	= "TERMINATION";
+	nav_states_str[NAVIGATION_STATE_TERMINATION]		= "TERMINATION";
 
 	/* pthread for slow low prio thread */
 	pthread_t commander_low_prio_thread;
@@ -853,6 +856,8 @@ int commander_thread_main(int argc, char *argv[])
 
 	transition_result_t arming_ret;
 
+	int32_t datalink_loss_enabled = false;
+
 	/* check which state machines for changes, clear "changed" flag */
 	bool arming_state_changed = false;
 	bool main_state_changed = false;
@@ -907,6 +912,7 @@ int commander_thread_main(int argc, char *argv[])
 			/* navigation parameters */
 			param_get(_param_takeoff_alt, &takeoff_alt);
 			param_get(_param_enable_parachute, &parachute_enabled);
+			param_get(_param_enable_datalink_loss, &datalink_loss_enabled);
 		}
 
 		orb_check(sp_man_sub, &updated);
@@ -1309,8 +1315,8 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 		/* data link check */
-		if (hrt_abstime() < telemetry.heartbeat_time + DL_TIMEOUT) {
-			/* handle the case where RC signal was regained */
+		if (hrt_absolute_time() < telemetry.heartbeat_time + DL_TIMEOUT) {
+			/* handle the case where data link was regained */
 			if (status.data_link_lost) {
 				mavlink_log_critical(mavlink_fd, "#audio: data link regained");
 				status.data_link_lost = false;
@@ -1320,7 +1326,7 @@ int commander_thread_main(int argc, char *argv[])
 		} else {
 			if (!status.data_link_lost) {
 				mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST");
-				status.data_link_lost = false;
+				status.data_link_lost = true;
 				status_changed = true;
 			}
 		}
@@ -1378,7 +1384,8 @@ int commander_thread_main(int argc, char *argv[])
 		was_armed = armed.armed;
 
 		/* now set navigation state according to failsafe and main state */
-		bool nav_state_changed = set_nav_state(&status);
+		bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled,
+						       mission_result.mission_finished);
 
 		// TODO handle mode changes by commands
 		if (main_state_changed) {
@@ -1727,6 +1734,8 @@ set_control_mode()
 	case NAVIGATION_STATE_AUTO_MISSION:
 	case NAVIGATION_STATE_AUTO_LOITER:
 	case NAVIGATION_STATE_AUTO_RTL:
+	case NAVIGATION_STATE_AUTO_FS_RC_LOSS:
+	case NAVIGATION_STATE_AUTO_FS_DL_LOSS:
 		control_mode.flag_control_manual_enabled = false;
 		control_mode.flag_control_auto_enabled = true;
 		control_mode.flag_control_rates_enabled = true;
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index 80ca68f21f..4750f9d5cb 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -39,7 +39,7 @@
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
  * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
+ * @author Julian Oes <julian@oes.ch>
  */
 
 #include <nuttx/config.h>
@@ -84,3 +84,14 @@ PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
  * @group Battery Calibration
  */
 PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
+
+/**
+ * Datalink loss mode enabled.
+ *
+ * Set to 1 to enable actions triggered when the datalink is lost.
+ *
+ * @group commander
+ * @min 0
+ * @max 1
+ */
+PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0);
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index c0d7309495..f0fe13921e 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -366,7 +366,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
 /**
  * Check failsafe and main status and set navigation status for navigator accordingly
  */
-bool set_nav_state(struct vehicle_status_s *status)
+bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished)
 {
 	navigation_state_t nav_state_old = status->nav_state;
 
@@ -383,6 +383,16 @@ bool set_nav_state(struct vehicle_status_s *status)
 		if (status->rc_signal_lost && armed) {
 			status->failsafe = true;
 
+			if (status->condition_global_position_valid && status->condition_home_position_valid) {
+				status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS;
+			} else if (status->condition_local_position_valid) {
+				status->nav_state = NAVIGATION_STATE_LAND;
+			} else if (status->condition_local_altitude_valid) {
+				status->nav_state = NAVIGATION_STATE_DESCEND;
+			} else {
+				status->nav_state = NAVIGATION_STATE_TERMINATION;
+			}
+
 		} else {
 			switch (status->main_state) {
 			case MAIN_STATE_ACRO:
@@ -409,45 +419,116 @@ bool set_nav_state(struct vehicle_status_s *status)
 		break;
 
 	case MAIN_STATE_AUTO_MISSION:
-		/* require data link and global position */
-		if ((status->data_link_lost || !status->condition_global_position_valid) && armed) {
+		/* go into failsafe
+		 * - if either the datalink is enabled and lost as well as RC is lost
+		 * - if there is no datalink and the mission is finished */
+		if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) ||
+		    (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) {
 			status->failsafe = true;
 
-		} else {
-			if (armed) {
-				status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
-
+			if (status->condition_global_position_valid && status->condition_home_position_valid) {
+				status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS;
+			} else if (status->condition_local_position_valid) {
+				status->nav_state = NAVIGATION_STATE_LAND;
+			} else if (status->condition_local_altitude_valid) {
+				status->nav_state = NAVIGATION_STATE_DESCEND;
 			} else {
-				// TODO which mode should we set when disarmed?
-				status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
+				status->nav_state = NAVIGATION_STATE_TERMINATION;
 			}
+
+		/* also go into failsafe if just datalink is lost */
+		} else if (status->data_link_lost && data_link_loss_enabled) {
+			status->failsafe = true;
+
+			if (status->condition_global_position_valid && status->condition_home_position_valid) {
+				status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS;
+			} else if (status->condition_local_position_valid) {
+				status->nav_state = NAVIGATION_STATE_LAND;
+			} else if (status->condition_local_altitude_valid) {
+				status->nav_state = NAVIGATION_STATE_DESCEND;
+			} else {
+				status->nav_state = NAVIGATION_STATE_TERMINATION;
+			}
+
+		/* don't bother if RC is lost and mission is not yet finished */
+		} else if (status->rc_signal_lost) {
+
+			/* this mode is ok, we don't need RC for missions */
+			status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
+		} else {
+			/* everything is perfect */
+			status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
 		}
 		break;
 
 	case MAIN_STATE_AUTO_LOITER:
-		/* require data link and local position */
-		if ((status->data_link_lost || !status->condition_local_position_valid) && armed) {
+		/* go into failsafe if datalink and RC is lost */
+		if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) {
 			status->failsafe = true;
 
+			if (status->condition_global_position_valid && status->condition_home_position_valid) {
+				status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS;
+			} else if (status->condition_local_position_valid) {
+				status->nav_state = NAVIGATION_STATE_LAND;
+			} else if (status->condition_local_altitude_valid) {
+				status->nav_state = NAVIGATION_STATE_DESCEND;
+			} else {
+				status->nav_state = NAVIGATION_STATE_TERMINATION;
+			}
+
+		/* also go into failsafe if just datalink is lost */
+		} else if (status->data_link_lost && data_link_loss_enabled) {
+			status->failsafe = true;
+
+			if (status->condition_global_position_valid && status->condition_home_position_valid) {
+				status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS;
+			} else if (status->condition_local_position_valid) {
+				status->nav_state = NAVIGATION_STATE_LAND;
+			} else if (status->condition_local_altitude_valid) {
+				status->nav_state = NAVIGATION_STATE_DESCEND;
+			} else {
+				status->nav_state = NAVIGATION_STATE_TERMINATION;
+			}
+
+		/* go into failsafe if RC is lost and datalink loss is not set up */
+		} else if (status->rc_signal_lost && !data_link_loss_enabled) {
+			status->failsafe = true;
+
+			if (status->condition_global_position_valid && status->condition_home_position_valid) {
+				status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS;
+			} else if (status->condition_local_position_valid) {
+				status->nav_state = NAVIGATION_STATE_LAND;
+			} else if (status->condition_local_altitude_valid) {
+				status->nav_state = NAVIGATION_STATE_DESCEND;
+			} else {
+				status->nav_state = NAVIGATION_STATE_TERMINATION;
+			}
+
+		/* don't bother if RC is lost if datalink is connected */
+		} else if (status->rc_signal_lost) {
+
+			/* this mode is ok, we don't need RC for loitering */
+			status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
 		} else {
-			// TODO which mode should we set when disarmed?
+			/* everything is perfect */
 			status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
 		}
 		break;
 
 	case MAIN_STATE_AUTO_RTL:
 		/* require global position and home */
-		if ((!status->condition_global_position_valid || !status->condition_home_position_valid) && armed) {
+		if ((!status->condition_global_position_valid || !status->condition_home_position_valid)) {
 			status->failsafe = true;
 
-		} else {
-			if (armed) {
-				status->nav_state = NAVIGATION_STATE_AUTO_RTL;
-
+			if (status->condition_local_position_valid) {
+				status->nav_state = NAVIGATION_STATE_LAND;
+			} else if (status->condition_local_altitude_valid) {
+				status->nav_state = NAVIGATION_STATE_DESCEND;
 			} else {
-				// TODO which mode should we set when disarmed?
-				status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
+				status->nav_state = NAVIGATION_STATE_TERMINATION;
 			}
+		} else {
+			status->nav_state = NAVIGATION_STATE_AUTO_RTL;
 		}
 		break;
 
@@ -455,21 +536,6 @@ bool set_nav_state(struct vehicle_status_s *status)
 		break;
 	}
 
-	if (status->failsafe) {
-		if (status->condition_global_position_valid && status->condition_home_position_valid) {
-			status->nav_state = NAVIGATION_STATE_AUTO_RTL;
-
-		} else if (status->condition_local_position_valid) {
-			status->nav_state = NAVIGATION_STATE_LAND;
-
-		} else if (status->condition_local_altitude_valid) {
-			status->nav_state = NAVIGATION_STATE_DESCEND;
-
-		} else {
-			status->nav_state = NAVIGATION_STATE_TERMINATION;
-		}
-	}
-
 	return status->nav_state != nav_state_old;
 }
 
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index cffbc9b8f7..2e076cdae3 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -65,6 +65,6 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state
 
 transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd);
 
-bool set_nav_state(struct vehicle_status_s *status);
+bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished);
 
 #endif /* STATE_MACHINE_HELPER_H_ */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 70da5393f1..50bcfd58db 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -351,8 +351,12 @@ Navigator::task_main()
 				_navigation_mode = &_loiter;
 				break;
 			case NAVIGATION_STATE_AUTO_RTL:
+			case NAVIGATION_STATE_AUTO_FS_RC_LOSS:
 				_navigation_mode = &_rtl;
 				break;
+			case NAVIGATION_STATE_AUTO_FS_DL_LOSS:
+				_navigation_mode = &_rtl; /* TODO: change this to something else */
+				break;
 			case NAVIGATION_STATE_LAND:
 			case NAVIGATION_STATE_TERMINATION:
 			default:
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 8519387958..16652fddfe 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -101,6 +101,8 @@ typedef enum {
 	NAVIGATION_STATE_AUTO_MISSION,		/**< Auto mission mode */
 	NAVIGATION_STATE_AUTO_LOITER,		/**< Auto loiter mode */
 	NAVIGATION_STATE_AUTO_RTL,		/**< Auto RTL mode */
+	NAVIGATION_STATE_AUTO_FS_RC_LOSS,	/**< Auto failsafe mode on RC loss */
+	NAVIGATION_STATE_AUTO_FS_DL_LOSS,	/**< Auto failsafe mode on DL loss */
 	NAVIGATION_STATE_ACRO,			/**< Acro mode */
 	NAVIGATION_STATE_LAND,			/**< Land mode */
 	NAVIGATION_STATE_DESCEND,			/**< Descend mode (no position control) */

From 94e004955df3f467b7e67b3fac0d968b9a68e091 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 19 Jun 2014 09:33:28 +0200
Subject: [PATCH 84/97] mavlink: publish telemtry status without radio status

---
 src/modules/mavlink/mavlink_receiver.cpp | 22 ++++++++++++++++++++++
 src/modules/mavlink/mavlink_receiver.h   |  1 +
 2 files changed, 23 insertions(+)

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 32c5e51ddb..7a6922bfa8 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -107,6 +107,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_rc_pub(-1),
 	_manual_pub(-1),
 	_telemetry_heartbeat_time(0),
+	_radio_status_available(false),
 	_hil_frames(0),
 	_old_timestamp(0),
 	_hil_local_proj_inited(0),
@@ -432,6 +433,9 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 	} else {
 		orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
 	}
+
+	/* this means that heartbeats alone won't be published to the radio status no more */
+	_radio_status_available = true;
 }
 
 void
@@ -467,6 +471,24 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
 	if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) {
 		_telemetry_heartbeat_time = hrt_absolute_time();
 	}
+
+	/* if no radio status messages arrive, lets at least publish that heartbeats were received */
+	if (!_radio_status_available) {
+
+		struct telemetry_status_s tstatus;
+		memset(&tstatus, 0, sizeof(tstatus));
+
+		tstatus.timestamp = _telemetry_heartbeat_time;
+		tstatus.heartbeat_time = _telemetry_heartbeat_time;
+		tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
+
+		if (_telemetry_status_pub < 0) {
+			_telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
+
+		} else {
+			orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
+		}
+	}
 }
 
 void
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index ab3dc81c6b..cd1dab3658 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -140,6 +140,7 @@ private:
 	orb_advert_t _rc_pub;
 	orb_advert_t _manual_pub;
 	hrt_abstime _telemetry_heartbeat_time;
+	bool _radio_status_available;
 	int _hil_frames;
 	uint64_t _old_timestamp;
 	bool _hil_local_proj_inited;

From 62faa2ee5155253779d3b93c5a280e8918585f6e Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 19 Jun 2014 10:26:56 +0200
Subject: [PATCH 85/97] commander/navigator: renamed FS modes to RTL and RTGS
 (return to ground station)

---
 src/modules/commander/commander.cpp            |  6 ++----
 src/modules/commander/px4_custom_mode.h        |  1 +
 src/modules/commander/state_machine_helper.cpp | 12 ++++++------
 src/modules/mavlink/mavlink_messages.cpp       |  8 ++++++++
 src/modules/navigator/navigator_main.cpp       |  3 +--
 src/modules/uORB/topics/vehicle_status.h       |  5 ++---
 6 files changed, 20 insertions(+), 15 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 5954dcbb1e..b957ad9b5f 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -649,8 +649,7 @@ int commander_thread_main(int argc, char *argv[])
 	nav_states_str[NAVIGATION_STATE_AUTO_MISSION]		= "AUTO_MISSION";
 	nav_states_str[NAVIGATION_STATE_AUTO_LOITER]		= "AUTO_LOITER";
 	nav_states_str[NAVIGATION_STATE_AUTO_RTL]		= "AUTO_RTL";
-	nav_states_str[NAVIGATION_STATE_AUTO_FS_RC_LOSS]	= "AUTO_FS_RC_LOSS";
-	nav_states_str[NAVIGATION_STATE_AUTO_FS_DL_LOSS]	= "AUTO_FS_DL_LOSS";
+	nav_states_str[NAVIGATION_STATE_AUTO_RTGS]		= "AUTO_RTGS";
 	nav_states_str[NAVIGATION_STATE_ACRO]			= "ACRO";
 	nav_states_str[NAVIGATION_STATE_LAND]			= "LAND";
 	nav_states_str[NAVIGATION_STATE_DESCEND]		= "DESCEND";
@@ -1734,8 +1733,7 @@ set_control_mode()
 	case NAVIGATION_STATE_AUTO_MISSION:
 	case NAVIGATION_STATE_AUTO_LOITER:
 	case NAVIGATION_STATE_AUTO_RTL:
-	case NAVIGATION_STATE_AUTO_FS_RC_LOSS:
-	case NAVIGATION_STATE_AUTO_FS_DL_LOSS:
+	case NAVIGATION_STATE_AUTO_RTGS:
 		control_mode.flag_control_manual_enabled = false;
 		control_mode.flag_control_auto_enabled = true;
 		control_mode.flag_control_rates_enabled = true;
diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h
index e0f8dc95dc..7f5f93801f 100644
--- a/src/modules/commander/px4_custom_mode.h
+++ b/src/modules/commander/px4_custom_mode.h
@@ -25,6 +25,7 @@ enum PX4_CUSTOM_SUB_MODE_AUTO {
 	PX4_CUSTOM_SUB_MODE_AUTO_MISSION,
 	PX4_CUSTOM_SUB_MODE_AUTO_RTL,
 	PX4_CUSTOM_SUB_MODE_AUTO_LAND,
+	PX4_CUSTOM_SUB_MODE_AUTO_RTGS
 };
 
 union px4_custom_mode {
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index f0fe13921e..74885abf6a 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -384,7 +384,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 			status->failsafe = true;
 
 			if (status->condition_global_position_valid && status->condition_home_position_valid) {
-				status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS;
+				status->nav_state = NAVIGATION_STATE_AUTO_RTL;
 			} else if (status->condition_local_position_valid) {
 				status->nav_state = NAVIGATION_STATE_LAND;
 			} else if (status->condition_local_altitude_valid) {
@@ -427,7 +427,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 			status->failsafe = true;
 
 			if (status->condition_global_position_valid && status->condition_home_position_valid) {
-				status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS;
+				status->nav_state = NAVIGATION_STATE_AUTO_RTL;
 			} else if (status->condition_local_position_valid) {
 				status->nav_state = NAVIGATION_STATE_LAND;
 			} else if (status->condition_local_altitude_valid) {
@@ -441,7 +441,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 			status->failsafe = true;
 
 			if (status->condition_global_position_valid && status->condition_home_position_valid) {
-				status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS;
+				status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
 			} else if (status->condition_local_position_valid) {
 				status->nav_state = NAVIGATION_STATE_LAND;
 			} else if (status->condition_local_altitude_valid) {
@@ -467,7 +467,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 			status->failsafe = true;
 
 			if (status->condition_global_position_valid && status->condition_home_position_valid) {
-				status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS;
+				status->nav_state = NAVIGATION_STATE_AUTO_RTL;
 			} else if (status->condition_local_position_valid) {
 				status->nav_state = NAVIGATION_STATE_LAND;
 			} else if (status->condition_local_altitude_valid) {
@@ -481,7 +481,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 			status->failsafe = true;
 
 			if (status->condition_global_position_valid && status->condition_home_position_valid) {
-				status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS;
+				status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
 			} else if (status->condition_local_position_valid) {
 				status->nav_state = NAVIGATION_STATE_LAND;
 			} else if (status->condition_local_altitude_valid) {
@@ -495,7 +495,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 			status->failsafe = true;
 
 			if (status->condition_global_position_valid && status->condition_home_position_valid) {
-				status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS;
+				status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
 			} else if (status->condition_local_position_valid) {
 				status->nav_state = NAVIGATION_STATE_LAND;
 			} else if (status->condition_local_altitude_valid) {
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index d7f56b63d3..1fd8b1a27a 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -176,6 +176,14 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
 			break;
 
+		case NAVIGATION_STATE_AUTO_RTGS:
+			*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_RTGS;
+			break;
+
 		case NAVIGATION_STATE_TERMINATION:
 			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 50bcfd58db..661d3d744a 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -351,10 +351,9 @@ Navigator::task_main()
 				_navigation_mode = &_loiter;
 				break;
 			case NAVIGATION_STATE_AUTO_RTL:
-			case NAVIGATION_STATE_AUTO_FS_RC_LOSS:
 				_navigation_mode = &_rtl;
 				break;
-			case NAVIGATION_STATE_AUTO_FS_DL_LOSS:
+			case NAVIGATION_STATE_AUTO_RTGS:
 				_navigation_mode = &_rtl; /* TODO: change this to something else */
 				break;
 			case NAVIGATION_STATE_LAND:
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 16652fddfe..5dc46dacbb 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -100,9 +100,8 @@ typedef enum {
 	NAVIGATION_STATE_POSCTL,		/**< Position control mode */
 	NAVIGATION_STATE_AUTO_MISSION,		/**< Auto mission mode */
 	NAVIGATION_STATE_AUTO_LOITER,		/**< Auto loiter mode */
-	NAVIGATION_STATE_AUTO_RTL,		/**< Auto RTL mode */
-	NAVIGATION_STATE_AUTO_FS_RC_LOSS,	/**< Auto failsafe mode on RC loss */
-	NAVIGATION_STATE_AUTO_FS_DL_LOSS,	/**< Auto failsafe mode on DL loss */
+	NAVIGATION_STATE_AUTO_RTL,		/**< Auto return to launch mode */
+	NAVIGATION_STATE_AUTO_RTGS,		/**< Auto return to groundstation on data link loss */
 	NAVIGATION_STATE_ACRO,			/**< Acro mode */
 	NAVIGATION_STATE_LAND,			/**< Land mode */
 	NAVIGATION_STATE_DESCEND,			/**< Descend mode (no position control) */

From e0c78e51e3a5768014c73bed5cd087830d602227 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 19 Jun 2014 14:14:24 +0200
Subject: [PATCH 86/97] mavlink: only publish telemetry status from GCS

---
 src/modules/mavlink/mavlink_receiver.cpp | 24 ++++++++++++------------
 1 file changed, 12 insertions(+), 12 deletions(-)

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 7a6922bfa8..baa6571a88 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -470,23 +470,23 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
 	/* ignore own heartbeats, accept only heartbeats from GCS */
 	if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) {
 		_telemetry_heartbeat_time = hrt_absolute_time();
-	}
 
-	/* if no radio status messages arrive, lets at least publish that heartbeats were received */
-	if (!_radio_status_available) {
+		/* if no radio status messages arrive, lets at least publish that heartbeats were received */
+		if (!_radio_status_available) {
 
-		struct telemetry_status_s tstatus;
-		memset(&tstatus, 0, sizeof(tstatus));
+			struct telemetry_status_s tstatus;
+			memset(&tstatus, 0, sizeof(tstatus));
 
-		tstatus.timestamp = _telemetry_heartbeat_time;
-		tstatus.heartbeat_time = _telemetry_heartbeat_time;
-		tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
+			tstatus.timestamp = _telemetry_heartbeat_time;
+			tstatus.heartbeat_time = _telemetry_heartbeat_time;
+			tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
 
-		if (_telemetry_status_pub < 0) {
-			_telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
+			if (_telemetry_status_pub < 0) {
+				_telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
 
-		} else {
-			orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
+			} else {
+				orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
+			}
 		}
 	}
 }

From 9d3d5a30af0781b975d17c5d73796f593c78b6e4 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Wed, 25 Jun 2014 17:13:47 +0200
Subject: [PATCH 87/97] navigator: move set_previous_pos_sp to MissionBlock
 class

---
 src/modules/navigator/mission.cpp       | 12 +-----------
 src/modules/navigator/mission.h         |  6 ------
 src/modules/navigator/mission_block.cpp |  9 +++++++++
 src/modules/navigator/mission_block.h   |  5 +++++
 4 files changed, 15 insertions(+), 17 deletions(-)

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 9244063b19..becaa09e83 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -223,7 +223,7 @@ Mission::advance_mission()
 void
 Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
 {
-	set_previous_pos_setpoint(&pos_sp_triplet->current, &pos_sp_triplet->previous);
+	set_previous_pos_setpoint(pos_sp_triplet);
 
 	/* try setting onboard mission item */
 	if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) {
@@ -261,16 +261,6 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
 	}
 }
 
-void
-Mission::set_previous_pos_setpoint(const struct position_setpoint_s *current_pos_sp,
-		                   struct position_setpoint_s *previous_pos_sp)
-{
-	/* reuse current setpoint as previous setpoint */
-	if (current_pos_sp->valid) {
-		memcpy(previous_pos_sp, current_pos_sp, sizeof(struct position_setpoint_s));
-	}
-}
-
 bool
 Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
 {
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index 322aaf96af..842c8c9ee8 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -106,12 +106,6 @@ private:
 	 */
 	void set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet);
 
-	/**
-	 * Set previous position setpoint
-	 */
-	void set_previous_pos_setpoint(const struct position_setpoint_s *current_pos_sp,
-			               struct position_setpoint_s *previous_pos_sp);
-
 	/**
 	 * Try to set the current position setpoint from an onboard mission item
 	 * @return true if mission item successfully set
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 08576750c9..a8eec92ca6 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -194,6 +194,15 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 	}
 }
 
+void
+MissionBlock::set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet)
+{
+    /* reuse current setpoint as previous setpoint */
+    if (pos_sp_triplet->current.valid) {
+        memcpy(&pos_sp_triplet->previous, &pos_sp_triplet->current, sizeof(struct position_setpoint_s));
+    }
+}
+
 bool
 MissionBlock::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet)
 {
diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h
index 47e6fe81fe..a740deea48 100644
--- a/src/modules/navigator/mission_block.h
+++ b/src/modules/navigator/mission_block.h
@@ -82,6 +82,11 @@ public:
 	 */
 	void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp);
 
+    /**
+     * Set previous position setpoint to current setpoint
+     */
+	void set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet);
+
 	/**
 	 * Set a loiter item, if possible reuse the position setpoint, otherwise take the current position
 	 *

From bdf1b9274c369ddfd52bcb92952f2467abd7c26f Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Wed, 25 Jun 2014 17:56:11 +0200
Subject: [PATCH 88/97] commander: modes fallback and reject messages fixed

---
 src/modules/commander/commander.cpp | 30 +++++++++++++++++++++++++----
 1 file changed, 26 insertions(+), 4 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index b957ad9b5f..ec4273fc33 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1613,10 +1613,10 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
 				break;	// changed successfully or already in this state
 			}
 
-			// else fallback to ALTCTL
 			print_reject_mode(status, "POSCTL");
 		}
 
+        // fallback to ALTCTL
 		res = main_state_transition(status, MAIN_STATE_ALTCTL);
 
 		if (res != TRANSITION_DENIED) {
@@ -1627,7 +1627,7 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
 			print_reject_mode(status, "ALTCTL");
 		}
 
-		// else fallback to MANUAL
+		// fallback to MANUAL
 		res = main_state_transition(status, MAIN_STATE_MANUAL);
 		// TRANSITION_DENIED is not possible here
 		break;
@@ -1639,28 +1639,50 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
 			if (res != TRANSITION_DENIED) {
 				break;	// changed successfully or already in this state
 			}
+
+            print_reject_mode(status, "AUTO_RTL");
+
+            // fallback to LOITER if home position not set
+            res = main_state_transition(status, MAIN_STATE_AUTO_LOITER);
+
+            if (res != TRANSITION_DENIED) {
+                break;  // changed successfully or already in this state
+            }
+
 		} else if (sp_man->loiter_switch == SWITCH_POS_ON) {
 			res = main_state_transition(status, MAIN_STATE_AUTO_LOITER);
 
 			if (res != TRANSITION_DENIED) {
 				break;	// changed successfully or already in this state
 			}
+
+            print_reject_mode(status, "AUTO_LOITER");
+
 		} else {
 			res = main_state_transition(status, MAIN_STATE_AUTO_MISSION);
 
 			if (res != TRANSITION_DENIED) {
 				break;	// changed successfully or already in this state
 			}
+
+            print_reject_mode(status, "AUTO_MISSION");
 		}
 
-		// else fallback to ALTCTL (POSCTL likely will not work too)
+        // fallback to POSCTL
+        res = main_state_transition(status, MAIN_STATE_POSCTL);
+
+        if (res != TRANSITION_DENIED) {
+            break;  // changed successfully or already in this state
+        }
+
+		// fallback to ALTCTL
 		res = main_state_transition(status, MAIN_STATE_ALTCTL);
 
 		if (res != TRANSITION_DENIED) {
 			break;	// changed successfully or already in this state
 		}
 
-		// else fallback to MANUAL
+		// fallback to MANUAL
 		res = main_state_transition(status, MAIN_STATE_MANUAL);
 		// TRANSITION_DENIED is not possible here
 		break;

From 39454ca99d984698983404b2c82bbd61f93be3fc Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Wed, 25 Jun 2014 17:56:30 +0200
Subject: [PATCH 89/97] navigator: RTL return altitude fixed

---
 src/modules/navigator/rtl.cpp | 59 ++++++++++++++++++-----------------
 1 file changed, 31 insertions(+), 28 deletions(-)

diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index c1b1d3f097..46194ed52a 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -44,6 +44,7 @@
 
 #include <mavlink/mavlink_log.h>
 #include <systemlib/err.h>
+#include <geo/geo.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/mission.h>
@@ -112,21 +113,29 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		if (_navigator->get_vstatus()->condition_landed) {
 			_rtl_state = RTL_STATE_FINISHED;
 			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
+
 		/* if lower than return altitude, climb up first */
 		} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
 			   + _param_return_alt.get()) {
 			_rtl_state = RTL_STATE_CLIMB;
+
 		/* otherwise go straight to return */
 		} else {
+		    /* set altitude setpoint to current altitude */
 			_rtl_state = RTL_STATE_RETURN;
+	        _mission_item.altitude_is_relative = false;
+	        _mission_item.altitude = _navigator->get_global_position()->alt;
 		}
 	}
 
-	/* if switching directly to return state, set altitude setpoint to current altitude */
-	if (_rtl_state == RTL_STATE_RETURN) {
-		_mission_item.altitude_is_relative = false;
-		_mission_item.altitude = _navigator->get_global_position()->alt;
-	}
+    if (_rtl_state == RTL_STATE_FINISHED) {
+        /* RTL finished, nothing to do */
+        pos_sp_triplet->current.valid = false;
+        pos_sp_triplet->next.valid = false;
+        return;
+    }
+
+    set_previous_pos_setpoint(pos_sp_triplet);
 
 	switch (_rtl_state) {
 	case RTL_STATE_CLIMB: {
@@ -157,17 +166,20 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 
 		_mission_item.lat = _navigator->get_home_position()->lat;
 		_mission_item.lon = _navigator->get_home_position()->lon;
+		 // don't change altitude
 
-		/* TODO: add this again */
-		// don't change altitude
-		// if (_pos_sp_triplet.previous.valid) {
-		// 	/* if previous setpoint is valid then use it to calculate heading to home */
-		// 	_mission_item.yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, _mission_item.lat, _mission_item.lon);
+		 if (pos_sp_triplet->previous.valid) {
+		 	/* if previous setpoint is valid then use it to calculate heading to home */
+		 	_mission_item.yaw = get_bearing_to_next_waypoint(
+		 	        pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon,
+		 	        _mission_item.lat, _mission_item.lon);
 
-		// } else {
-		// 	/* else use current position */
-		// 	_mission_item.yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, _mission_item.lat, _mission_item.lon);
-		// }
+		 } else {
+		 	/* else use current position */
+		 	_mission_item.yaw = get_bearing_to_next_waypoint(
+		 	        _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
+		 	        _mission_item.lat, _mission_item.lon);
+		 }
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
@@ -228,24 +240,15 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		break;
 	}
 
-	case RTL_STATE_FINISHED: {
-		/* nothing to do, report fail */
-	}
-
 	default:
 		break;
 	}
 
-	if (_rtl_state == RTL_STATE_FINISHED) {
-		pos_sp_triplet->current.valid = false;
-		pos_sp_triplet->next.valid = false;
-	} else {
-		/* if not finished, convert mission item to current position setpoint and make it valid */
-		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
-		reset_mission_item_reached();
-		pos_sp_triplet->current.valid = true;
-		pos_sp_triplet->next.valid = false;
-	}
+    /* convert mission item to current position setpoint and make it valid */
+    mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+    reset_mission_item_reached();
+    pos_sp_triplet->current.valid = true;
+    pos_sp_triplet->next.valid = false;
 }
 
 void

From c5a5604ae975294347f1571258a633c11ef61261 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Thu, 26 Jun 2014 00:17:25 +0200
Subject: [PATCH 90/97] navigator: loiter fixes

---
 src/modules/navigator/loiter.cpp         |  3 +-
 src/modules/navigator/mission.cpp        |  8 ++---
 src/modules/navigator/mission_block.cpp  | 40 +++++++++++++-----------
 src/modules/navigator/mission_block.h    |  3 +-
 src/modules/navigator/navigator.h        |  6 ++--
 src/modules/navigator/navigator_main.cpp |  4 +--
 src/modules/navigator/rtl.cpp            |  9 +-----
 7 files changed, 34 insertions(+), 39 deletions(-)

diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp
index 8bbb79d5ee..f5eb1e7a5a 100644
--- a/src/modules/navigator/loiter.cpp
+++ b/src/modules/navigator/loiter.cpp
@@ -70,11 +70,10 @@ bool
 Loiter::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
 {
 	/* set loiter item, don't reuse an existing position setpoint */
-	return set_loiter_item(false, pos_sp_triplet);;
+	return set_loiter_item(pos_sp_triplet);
 }
 
 void
 Loiter::on_inactive()
 {
 }
-
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index becaa09e83..c09f617446 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -233,7 +233,7 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
 					"#audio: onboard mission running");
 		}
 		_mission_type = MISSION_TYPE_ONBOARD;
-		_navigator->set_is_in_loiter(false);
+		_navigator->set_can_loiter_at_sp(false);
 
 	/* try setting offboard mission item */
 	} else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) {
@@ -243,7 +243,7 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
 					"#audio: offboard mission running");
 		}
 		_mission_type = MISSION_TYPE_OFFBOARD;
-		_navigator->set_is_in_loiter(false);
+		_navigator->set_can_loiter_at_sp(false);
 	} else {
 		if (_mission_type != MISSION_TYPE_NONE) {
 			mavlink_log_info(_navigator->get_mavlink_fd(),
@@ -253,9 +253,9 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
 					"#audio: no mission available");
 		}
 		_mission_type = MISSION_TYPE_NONE;
+        _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached);
 
-		bool use_current_pos_sp = pos_sp_triplet->current.valid && _waypoint_position_reached;
-		set_loiter_item(use_current_pos_sp, pos_sp_triplet);
+		set_loiter_item(pos_sp_triplet);
 		reset_mission_item_reached();
 		report_mission_finished();
 	}
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index a8eec92ca6..41d81ad9b0 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -204,31 +204,35 @@ MissionBlock::set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_
 }
 
 bool
-MissionBlock::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet)
+MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet)
 {
-	if (_navigator_priv->get_is_in_loiter()) {
-		/* already loitering, bail out */
-		return false;
-	}
-
-	if (reuse_current_pos_sp && pos_sp_triplet->current.valid) {
-		/* leave position setpoint as is */
-	} else {
+    /* don't change setpoint if 'can_loiter_at_sp' flag set */
+	if (!(_navigator_priv->get_can_loiter_at_sp() && pos_sp_triplet->current.valid)) {
 		/* use current position */
 		pos_sp_triplet->current.lat = _navigator_priv->get_global_position()->lat;
 		pos_sp_triplet->current.lon = _navigator_priv->get_global_position()->lon;
 		pos_sp_triplet->current.alt = _navigator_priv->get_global_position()->alt;
 		pos_sp_triplet->current.yaw = NAN;	/* NAN means to use current yaw */
+
+	    _navigator_priv->set_can_loiter_at_sp(true);
 	}
-	pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER;
-	pos_sp_triplet->current.loiter_radius = _navigator_priv->get_loiter_radius();
-	pos_sp_triplet->current.loiter_direction = 1;
 
-	pos_sp_triplet->previous.valid = false;
-	pos_sp_triplet->current.valid = true;
-	pos_sp_triplet->next.valid = false;
+    if (pos_sp_triplet->current.type != SETPOINT_TYPE_LOITER
+            || pos_sp_triplet->current.loiter_radius != _navigator_priv->get_loiter_radius()
+            || pos_sp_triplet->current.loiter_direction != 1
+            || pos_sp_triplet->previous.valid
+            || !pos_sp_triplet->current.valid
+            || pos_sp_triplet->next.valid) {
+        /* position setpoint triplet should be updated */
+        pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER;
+        pos_sp_triplet->current.loiter_radius = _navigator_priv->get_loiter_radius();
+        pos_sp_triplet->current.loiter_direction = 1;
 
-	_navigator_priv->set_is_in_loiter(true);
-	return true;
+        pos_sp_triplet->previous.valid = false;
+        pos_sp_triplet->current.valid = true;
+        pos_sp_triplet->next.valid = false;
+        return true;
+    }
+
+    return false;
 }
-
diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h
index a740deea48..8726964fa3 100644
--- a/src/modules/navigator/mission_block.h
+++ b/src/modules/navigator/mission_block.h
@@ -90,11 +90,10 @@ public:
 	/**
 	 * Set a loiter item, if possible reuse the position setpoint, otherwise take the current position
 	 *
-	 * @param true if the current position setpoint should be re-used
 	 * @param the position setpoint triplet to set
 	 * @return true if setpoint has changed
 	 */
-	bool set_loiter_item(const bool reuse_current_pos_sp, position_setpoint_triplet_s *pos_sp_triplet);
+	bool set_loiter_item(position_setpoint_triplet_s *pos_sp_triplet);
 
 	bool _waypoint_position_reached;
 	bool _waypoint_yaw_reached;
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index dadd155271..4709f71965 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -102,7 +102,7 @@ public:
 	/**
 	 * Setters
 	 */
-	void		set_is_in_loiter(bool is_in_loiter) { _is_in_loiter = is_in_loiter; }
+	void		set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; }
 
 	/**
 	 * Getters
@@ -113,7 +113,7 @@ public:
 	int		get_onboard_mission_sub() { return _onboard_mission_sub; }
 	int		get_offboard_mission_sub() { return _offboard_mission_sub; }
 	Geofence&	get_geofence() { return _geofence; }
-	bool		get_is_in_loiter() { return _is_in_loiter; }
+	bool		get_can_loiter_at_sp() { return _can_loiter_at_sp; }
 	float		get_loiter_radius() { return _param_loiter_radius.get(); }
 	float		get_takeoff_acceptance_radius() { return _param_takeoff_acceptance_radius.get(); }
 	int		get_mavlink_fd() { return _mavlink_fd; }
@@ -161,7 +161,7 @@ private:
 
 	NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE];	/**< array of navigation modes */
 
-	bool		_is_in_loiter;			/**< flags if current position SP can be used to loiter */
+	bool		_can_loiter_at_sp;			/**< flags if current position SP can be used to loiter */
 	bool		_update_triplet;		/**< flags if position SP triplet needs to be published */
 
 	control::BlockParamFloat _param_loiter_radius;	/**< loiter radius for fixedwing */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index d762b8a9dc..f39300590c 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -347,7 +347,7 @@ Navigator::task_main()
 			case NAVIGATION_STATE_ALTCTL:
 			case NAVIGATION_STATE_POSCTL:
 				_navigation_mode = nullptr;
-				_is_in_loiter = false;
+				_can_loiter_at_sp = false;
 				break;
 			case NAVIGATION_STATE_AUTO_MISSION:
 				_navigation_mode = &_mission;
@@ -365,7 +365,7 @@ Navigator::task_main()
 			case NAVIGATION_STATE_TERMINATION:
 			default:
 				_navigation_mode = nullptr;
-				_is_in_loiter = false;
+				_can_loiter_at_sp = false;
 				break;
 		}
 
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index 46194ed52a..411f8c5272 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -136,6 +136,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
     }
 
     set_previous_pos_setpoint(pos_sp_triplet);
+    _navigator->set_can_loiter_at_sp(false);
 
 	switch (_rtl_state) {
 	case RTL_STATE_CLIMB: {
@@ -156,8 +157,6 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.autocontinue = true;
 		_mission_item.origin = ORIGIN_ONBOARD;
 
-		_navigator->set_is_in_loiter(false);
-
 		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: climb to %d meters above home",
 			(int)(climb_alt - _navigator->get_home_position()->alt));
 		break;
@@ -189,8 +188,6 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.autocontinue = true;
 		_mission_item.origin = ORIGIN_ONBOARD;
 
-		_navigator->set_is_in_loiter(false);
-
 		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: return at %d meters above home",
 			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
 		break;
@@ -211,8 +208,6 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.autocontinue = _param_land_delay.get() > -0.001f;
 		_mission_item.origin = ORIGIN_ONBOARD;
 
-		_navigator->set_is_in_loiter(true);
-
 		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home",
 			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
 		break;
@@ -234,8 +229,6 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.autocontinue = true;
 		_mission_item.origin = ORIGIN_ONBOARD;
 
-		_navigator->set_is_in_loiter(false);
-
 		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: land at home");
 		break;
 	}

From 63e14c73bab8f5b14b5259ba249f84a8edb0ee05 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Thu, 26 Jun 2014 12:18:19 +0200
Subject: [PATCH 91/97] navigator: don't reset RTL state on loiter

---
 src/modules/navigator/rtl.cpp | 58 +++++++++++++++++++----------------
 1 file changed, 31 insertions(+), 27 deletions(-)

diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index 411f8c5272..a2eec85893 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -76,7 +76,11 @@ void
 RTL::on_inactive()
 {
 	_first_run = true;
-	_rtl_state = RTL_STATE_NONE;
+
+	/* reset RTL state only if setpoint moved */
+	if (!_navigator->get_can_loiter_at_sp()) {
+	    _rtl_state = RTL_STATE_NONE;
+	}
 }
 
 bool
@@ -85,14 +89,35 @@ RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
 	bool updated = false;
 
 	if (_first_run) {
+        _first_run = false;
+
+	    /* decide where to enter the RTL procedure when we switch into it */
+	    if (_rtl_state == RTL_STATE_NONE) {
+	        /* for safety reasons don't go into RTL if landed */
+	        if (_navigator->get_vstatus()->condition_landed) {
+	            _rtl_state = RTL_STATE_FINISHED;
+	            mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
+
+	        /* if lower than return altitude, climb up first */
+	        } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
+	               + _param_return_alt.get()) {
+	            _rtl_state = RTL_STATE_CLIMB;
+
+	        /* otherwise go straight to return */
+	        } else {
+	            /* set altitude setpoint to current altitude */
+	            _rtl_state = RTL_STATE_RETURN;
+	            _mission_item.altitude_is_relative = false;
+	            _mission_item.altitude = _navigator->get_global_position()->alt;
+	        }
+	    }
+
 		set_rtl_item(pos_sp_triplet);
+
 		updated = true;
-		_first_run = false;
 	}
 
-	if ((_rtl_state == RTL_STATE_CLIMB
-	     || _rtl_state == RTL_STATE_RETURN)
-	    && is_mission_item_reached()) {
+	if ((_rtl_state != RTL_STATE_FINISHED) && is_mission_item_reached()) {
 		advance_rtl();
 		set_rtl_item(pos_sp_triplet);
 		updated = true;
@@ -107,31 +132,10 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 	/* make sure we have the latest params */
 	updateParams();
 
-	/* decide where to enter the RTL procedure when we switch into it */
-	if (_rtl_state == RTL_STATE_NONE) {
-		/* for safety reasons don't go into RTL if landed */
-		if (_navigator->get_vstatus()->condition_landed) {
-			_rtl_state = RTL_STATE_FINISHED;
-			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
-
-		/* if lower than return altitude, climb up first */
-		} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
-			   + _param_return_alt.get()) {
-			_rtl_state = RTL_STATE_CLIMB;
-
-		/* otherwise go straight to return */
-		} else {
-		    /* set altitude setpoint to current altitude */
-			_rtl_state = RTL_STATE_RETURN;
-	        _mission_item.altitude_is_relative = false;
-	        _mission_item.altitude = _navigator->get_global_position()->alt;
-		}
-	}
-
     if (_rtl_state == RTL_STATE_FINISHED) {
         /* RTL finished, nothing to do */
-        pos_sp_triplet->current.valid = false;
         pos_sp_triplet->next.valid = false;
+        mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed");
         return;
     }
 

From 6e5aafb3a753f83b1db936e94de09144ee115c0d Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Thu, 26 Jun 2014 13:44:41 +0200
Subject: [PATCH 92/97] navigator: minor formatting fix

---
 src/modules/navigator/navigator_main.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index f39300590c..493e86d4a7 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -386,7 +386,7 @@ Navigator::task_main()
 			_update_triplet = true;
 		}
 
-		if (_update_triplet ) {
+		if (_update_triplet) {
 			publish_position_setpoint_triplet();
 			_update_triplet = false;
 		}

From 8e8798a5225d333596c9018ec703da0c6787493d Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Thu, 26 Jun 2014 23:39:17 +0200
Subject: [PATCH 93/97] navigator: spaces/tabs fixed, old commented code
 removed

---
 src/modules/navigator/navigator_main.cpp | 322 -----------------------
 src/modules/navigator/rtl.cpp            |  66 ++---
 2 files changed, 33 insertions(+), 355 deletions(-)

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 493e86d4a7..e22c4d72df 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -446,329 +446,7 @@ Navigator::status()
 		warnx("Geofence not set");
 	}
 }
-#if 0
-bool
-Navigator::start_none_on_ground()
-{
-	reset_reached();
 
-	_pos_sp_triplet.previous.valid = false;
-	_pos_sp_triplet.current.valid = false;
-	_pos_sp_triplet.next.valid = false;
-
-	_update_triplet = true;
-	return true;
-}
-
-bool
-Navigator::start_none_in_air()
-{
-	reset_reached();
-
-	_pos_sp_triplet.previous.valid = false;
-	_pos_sp_triplet.current.valid = false;
-	_pos_sp_triplet.next.valid = false;
-
-	_update_triplet = true;
-	return true;
-}
-
-bool
-Navigator::start_auto_on_ground()
-{
-	reset_reached();
-
-	_pos_sp_triplet.previous.valid = false;
-	_pos_sp_triplet.current.valid = true;
-	_pos_sp_triplet.next.valid = false;
-
-	_pos_sp_triplet.current.type = SETPOINT_TYPE_IDLE;
-
-	_update_triplet = true;
-	return true;
-}
-
-bool
-Navigator::start_loiter()
-{
-	/* if no existing item available, use current position */
-	if (!(_pos_sp_triplet.current.valid && _waypoint_position_reached)) {
-
-		_pos_sp_triplet.current.lat = _global_pos.lat;
-		_pos_sp_triplet.current.lon = _global_pos.lon;
-		_pos_sp_triplet.current.yaw = NAN;	// NAN means to use current yaw
-		_pos_sp_triplet.current.alt = _global_pos.alt;
-	}
-	_pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER;
-	_pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius;
-	_pos_sp_triplet.current.loiter_direction = 1;
-	_pos_sp_triplet.previous.valid = false;
-	_pos_sp_triplet.current.valid = true;
-	_pos_sp_triplet.next.valid = false;
-
-	mavlink_log_info(_mavlink_fd, "#audio: loiter at current altitude");
-
-	_update_triplet = true;
-	return true;
-}
-
-bool
-Navigator::start_mission()
-{
-	/* start fresh */
-	_pos_sp_triplet.previous.valid = false;
-	_pos_sp_triplet.current.valid = false;
-	_pos_sp_triplet.next.valid = false;
-
-	return set_mission_items();
-}
-
-bool
-Navigator::advance_mission()
-{
-	/* tell mission to move by one */
-	_mission.move_to_next();
-
-	/* now try to set the new mission items, if it fails, it will dispatch loiter */
-	return set_mission_items();
-}
-
-bool
-Navigator::set_mission_items()
-{
-	if (_pos_sp_triplet.current.valid) {
-		memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
-		_pos_sp_triplet.previous.valid = true;
-	}
-
-	bool onboard;
-	int index;
-
-	/* if we fail to set the current mission, continue to loiter */
-	if (!_mission.get_current_mission_item(&_mission_item, &onboard, &index)) {
-
-		return false;
-	}
-
-	/* if we got an RTL mission item, switch to RTL mode and give up */
-	if (_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
-		return false;
-	}
-
-	_mission_item_valid = true;
-
-	/* convert the current mission item and set it valid */
-	mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
-	_pos_sp_triplet.current.valid = true;
-
-
-	mission_item_s next_mission_item;
-
-	bool last_wp = false;
-	/* now try to set the next mission item as well, if there is no more next
-	 * this means we're heading to the last waypoint */
-	if (_mission.get_next_mission_item(&next_mission_item)) {
-		/* convert the next mission item and set it valid */
-		mission_item_to_position_setpoint(&next_mission_item, &_pos_sp_triplet.next);
-		_pos_sp_triplet.next.valid = true;
-	} else {
-		last_wp = true;
-	}
-
-	/* notify user about what happened */
-	mavlink_log_info(_mavlink_fd, "#audio: heading to %s%swaypoint %d",
-		(last_wp ? "last " : "" ), (onboard ? "onboard " : ""), index);
-
-	_update_triplet = true;
-
-	reset_reached();
-
-	return true;
-}
-
-bool
-Navigator::start_rtl()
-{
-	if (_rtl.get_current_rtl_item(&_global_pos, &_mission_item)) {
-
-		_mission_item_valid = true;
-
-		mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
-		_pos_sp_triplet.current.valid = true;
-
-		reset_reached();
-
-		_update_triplet = true;
-		return true;
-	}
-
-	/* if RTL doesn't work, fallback to loiter */
-	return false;
-}
-
-bool
-Navigator::advance_rtl()
-{
-	/* tell mission to move by one */
-	_rtl.move_to_next();
-
-	/* now try to set the new mission items, if it fails, it will dispatch loiter */
-	if (_rtl.get_current_rtl_item(&_global_pos, &_mission_item)) {
-
-		_mission_item_valid = true;
-
-		mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
-		_pos_sp_triplet.current.valid = true;
-
-		reset_reached();
-
-		_update_triplet = true;
-		return true;
-	}
-
-	return false;
-}
-
-bool
-Navigator::start_land()
-{
-	/* TODO: verify/test */
-
-	reset_reached();
-
-	/* this state can be requested by commander even if no global position available,
-	 * in his case controller must perform landing without position control */
-
-	memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
-
-	_mission_item.lat = _global_pos.lat;
-	_mission_item.lon = _global_pos.lon;
-	_mission_item.altitude_is_relative = false;
-	_mission_item.altitude = _global_pos.alt;
-	_mission_item.yaw = NAN;
-	_mission_item.loiter_radius = _parameters.loiter_radius;
-	_mission_item.loiter_direction = 1;
-	_mission_item.nav_cmd = NAV_CMD_LAND;
-	_mission_item.acceptance_radius = _parameters.acceptance_radius;
-	_mission_item.time_inside = 0.0f;
-	_mission_item.pitch_min = 0.0f;
-	_mission_item.autocontinue = true;
-	_mission_item.origin = ORIGIN_ONBOARD;
-
-	_mission_item_valid = true;
-
-	mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
-
-	_pos_sp_triplet.next.valid = false;
-
-	_update_triplet = true;
-	return true;
-}
-bool
-Navigator::check_mission_item_reached()
-{
-	/* only check if there is actually a mission item to check */
-	if (!_mission_item_valid) {
-		return false;
-	}
-
-	if (_mission_item.nav_cmd == NAV_CMD_LAND) {
-		return _vstatus.condition_landed;
-	}
-
-	/* XXX TODO count turns */
-	if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
-	     _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
-	    _mission_item.loiter_radius > 0.01f) {
-
-	// 	return false;
-	// }
-
-	uint64_t now = hrt_absolute_time();
-
-	if (!_waypoint_position_reached) {
-		float acceptance_radius;
-
-		if (_mission_item.nav_cmd == NAV_CMD_WAYPOINT && _mission_item.acceptance_radius > 0.01f) {
-			acceptance_radius = _mission_item.acceptance_radius;
-
-		} else {
-			acceptance_radius = _parameters.acceptance_radius;
-		}
-
-		if (_do_takeoff) {
-			/* require only altitude for takeoff */
-			if (_global_pos.alt > _pos_sp_triplet.current.alt - acceptance_radius) {
-				_waypoint_position_reached = true;
-			}
-		} else {
-			float dist = -1.0f;
-			float dist_xy = -1.0f;
-			float dist_z = -1.0f;
-
-			/* calculate AMSL altitude for this waypoint */
-			float wp_alt_amsl = _mission_item.altitude;
-
-			if (_mission_item.altitude_is_relative)
-				wp_alt_amsl += _home_pos.alt;
-
-			dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl,
-					(double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt,
-					&dist_xy, &dist_z);
-
-			if (dist >= 0.0f && dist <= acceptance_radius) {
-				_waypoint_position_reached = true;
-			}
-		}
-	}
-
-	if (_waypoint_position_reached && !_waypoint_yaw_reached) {
-
-		/* TODO: removed takeoff, why? */
-		if (_vstatus.is_rotary_wing && isfinite(_mission_item.yaw)) {
-
-			/* check yaw if defined only for rotary wing except takeoff */
-			float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw);
-
-			if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */
-				_waypoint_yaw_reached = true;
-			}
-
-		} else {
-			_waypoint_yaw_reached = true;
-		}
-	}
-
-	/* check if the current waypoint was reached */
-	if (_waypoint_position_reached && _waypoint_yaw_reached) {
-
-		if (_time_first_inside_orbit == 0) {
-			_time_first_inside_orbit = now;
-
-			if (_mission_item.time_inside > 0.01f) {
-				mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs",
-					(double)_mission_item.time_inside);
-			}
-		}
-
-		/* check if the MAV was long enough inside the waypoint orbit */
-		if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item.time_inside * 1e6)
-		    || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
-			return true;
-		}
-	}
-	return false;
-}
-
-void
-Navigator::reset_reached()
-{
-	_time_first_inside_orbit = 0;
-	_waypoint_position_reached = false;
-	_waypoint_yaw_reached = false;
-
-}
-#endif
 void
 Navigator::publish_position_setpoint_triplet()
 {
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index a2eec85893..c5bb06c3b7 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -79,7 +79,7 @@ RTL::on_inactive()
 
 	/* reset RTL state only if setpoint moved */
 	if (!_navigator->get_can_loiter_at_sp()) {
-	    _rtl_state = RTL_STATE_NONE;
+		_rtl_state = RTL_STATE_NONE;
 	}
 }
 
@@ -89,28 +89,28 @@ RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
 	bool updated = false;
 
 	if (_first_run) {
-        _first_run = false;
+		_first_run = false;
 
-	    /* decide where to enter the RTL procedure when we switch into it */
-	    if (_rtl_state == RTL_STATE_NONE) {
-	        /* for safety reasons don't go into RTL if landed */
-	        if (_navigator->get_vstatus()->condition_landed) {
-	            _rtl_state = RTL_STATE_FINISHED;
-	            mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
+		/* decide where to enter the RTL procedure when we switch into it */
+		if (_rtl_state == RTL_STATE_NONE) {
+			/* for safety reasons don't go into RTL if landed */
+			if (_navigator->get_vstatus()->condition_landed) {
+				_rtl_state = RTL_STATE_FINISHED;
+				mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
 
-	        /* if lower than return altitude, climb up first */
-	        } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
-	               + _param_return_alt.get()) {
-	            _rtl_state = RTL_STATE_CLIMB;
+			/* if lower than return altitude, climb up first */
+			} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
+				   + _param_return_alt.get()) {
+				_rtl_state = RTL_STATE_CLIMB;
 
-	        /* otherwise go straight to return */
-	        } else {
-	            /* set altitude setpoint to current altitude */
-	            _rtl_state = RTL_STATE_RETURN;
-	            _mission_item.altitude_is_relative = false;
-	            _mission_item.altitude = _navigator->get_global_position()->alt;
-	        }
-	    }
+			/* otherwise go straight to return */
+			} else {
+				/* set altitude setpoint to current altitude */
+				_rtl_state = RTL_STATE_RETURN;
+				_mission_item.altitude_is_relative = false;
+				_mission_item.altitude = _navigator->get_global_position()->alt;
+			}
+		}
 
 		set_rtl_item(pos_sp_triplet);
 
@@ -132,15 +132,15 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 	/* make sure we have the latest params */
 	updateParams();
 
-    if (_rtl_state == RTL_STATE_FINISHED) {
-        /* RTL finished, nothing to do */
-        pos_sp_triplet->next.valid = false;
-        mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed");
-        return;
-    }
+	if (_rtl_state == RTL_STATE_FINISHED) {
+		/* RTL finished, nothing to do */
+		pos_sp_triplet->next.valid = false;
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed");
+		return;
+	}
 
-    set_previous_pos_setpoint(pos_sp_triplet);
-    _navigator->set_can_loiter_at_sp(false);
+	set_previous_pos_setpoint(pos_sp_triplet);
+	_navigator->set_can_loiter_at_sp(false);
 
 	switch (_rtl_state) {
 	case RTL_STATE_CLIMB: {
@@ -241,11 +241,11 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		break;
 	}
 
-    /* convert mission item to current position setpoint and make it valid */
-    mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
-    reset_mission_item_reached();
-    pos_sp_triplet->current.valid = true;
-    pos_sp_triplet->next.valid = false;
+	/* convert mission item to current position setpoint and make it valid */
+	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+	reset_mission_item_reached();
+	pos_sp_triplet->current.valid = true;
+	pos_sp_triplet->next.valid = false;
 }
 
 void

From 9ae44291b169a90945f090487e3757db9fc8c075 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Fri, 27 Jun 2014 00:27:08 +0200
Subject: [PATCH 94/97] navigator: added NAV_CMD_IDLE, added RTL_STATE_LOITER
 and RTL_STATE_LANDED instead of FINISHED

---
 src/modules/navigator/mission_block.cpp | 25 +++++---
 src/modules/navigator/rtl.cpp           | 84 ++++++++++++++++++-------
 src/modules/navigator/rtl.h             |  3 +-
 src/modules/uORB/topics/mission.h       |  1 +
 4 files changed, 82 insertions(+), 31 deletions(-)

diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 41d81ad9b0..5e545706d1 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -69,10 +69,10 @@ MissionBlock::~MissionBlock()
 bool
 MissionBlock::is_mission_item_reached()
 {
-	/* don't check landed WPs */
 	if (_mission_item.nav_cmd == NAV_CMD_LAND) {
-		return false;
+		return _navigator_priv->get_vstatus()->condition_landed;
 	}
+
 	/* TODO: count turns */
 #if 0
 	if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
@@ -178,19 +178,28 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 	sp->loiter_direction = item->loiter_direction;
 	sp->pitch_min = item->pitch_min;
 
-	if (item->nav_cmd == NAV_CMD_TAKEOFF) {
+	switch (item->nav_cmd) {
+	case NAV_CMD_IDLE:
+		sp->type = SETPOINT_TYPE_IDLE;
+		break;
+
+	case NAV_CMD_TAKEOFF:
 		sp->type = SETPOINT_TYPE_TAKEOFF;
+		break;
 
-	} else if (item->nav_cmd == NAV_CMD_LAND) {
+	case NAV_CMD_LAND:
 		sp->type = SETPOINT_TYPE_LAND;
+		break;
 
-	} else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
-		   item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
-		   item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
+	case NAV_CMD_LOITER_TIME_LIMIT:
+	case NAV_CMD_LOITER_TURN_COUNT:
+	case NAV_CMD_LOITER_UNLIMITED:
 		sp->type = SETPOINT_TYPE_LOITER;
+		break;
 
-	} else {
+	default:
 		sp->type = SETPOINT_TYPE_POSITION;
+		break;
 	}
 }
 
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index c5bb06c3b7..b7961a260c 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -53,6 +53,8 @@
 #include "navigator.h"
 #include "rtl.h"
 
+#define DELAY_SIGMA	0.01f
+
 RTL::RTL(Navigator *navigator, const char *name) :
 	NavigatorMode(navigator, name),
 	MissionBlock(navigator),
@@ -95,7 +97,7 @@ RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
 		if (_rtl_state == RTL_STATE_NONE) {
 			/* for safety reasons don't go into RTL if landed */
 			if (_navigator->get_vstatus()->condition_landed) {
-				_rtl_state = RTL_STATE_FINISHED;
+				_rtl_state = RTL_STATE_LANDED;
 				mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
 
 			/* if lower than return altitude, climb up first */
@@ -113,11 +115,9 @@ RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
 		}
 
 		set_rtl_item(pos_sp_triplet);
-
 		updated = true;
-	}
 
-	if ((_rtl_state != RTL_STATE_FINISHED) && is_mission_item_reached()) {
+	} else if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
 		advance_rtl();
 		set_rtl_item(pos_sp_triplet);
 		updated = true;
@@ -132,19 +132,11 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 	/* make sure we have the latest params */
 	updateParams();
 
-	if (_rtl_state == RTL_STATE_FINISHED) {
-		/* RTL finished, nothing to do */
-		pos_sp_triplet->next.valid = false;
-		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed");
-		return;
-	}
-
 	set_previous_pos_setpoint(pos_sp_triplet);
 	_navigator->set_can_loiter_at_sp(false);
 
 	switch (_rtl_state) {
 	case RTL_STATE_CLIMB: {
-
 		float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get();
 
 		_mission_item.lat = _navigator->get_global_position()->lat;
@@ -165,8 +157,8 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 			(int)(climb_alt - _navigator->get_home_position()->alt));
 		break;
 	}
-	case RTL_STATE_RETURN: {
 
+	case RTL_STATE_RETURN: {
 		_mission_item.lat = _navigator->get_home_position()->lat;
 		_mission_item.lon = _navigator->get_home_position()->lon;
 		 // don't change altitude
@@ -196,8 +188,8 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
 		break;
 	}
-	case RTL_STATE_DESCEND: {
 
+	case RTL_STATE_DESCEND: {
 		_mission_item.lat = _navigator->get_home_position()->lat;
 		_mission_item.lon = _navigator->get_home_position()->lon;
 		_mission_item.altitude_is_relative = false;
@@ -207,9 +199,9 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
 		_mission_item.acceptance_radius = _param_acceptance_radius.get();
-		_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
+		_mission_item.time_inside = 0.0f;
 		_mission_item.pitch_min = 0.0f;
-		_mission_item.autocontinue = _param_land_delay.get() > -0.001f;
+		_mission_item.autocontinue = false;
 		_mission_item.origin = ORIGIN_ONBOARD;
 
 		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home",
@@ -217,8 +209,35 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		break;
 	}
 
-	case RTL_STATE_LAND: {
+	case RTL_STATE_LOITER: {
+		bool autoland = _param_land_delay.get() > -DELAY_SIGMA;
 
+		_mission_item.lat = _navigator->get_home_position()->lat;
+		_mission_item.lon = _navigator->get_home_position()->lon;
+		_mission_item.altitude_is_relative = false;
+		_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();
+		_mission_item.yaw = NAN;
+		_mission_item.loiter_radius = _navigator->get_loiter_radius();
+		_mission_item.loiter_direction = 1;
+		_mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;
+		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = autoland;
+		_mission_item.origin = ORIGIN_ONBOARD;
+
+		_navigator->set_can_loiter_at_sp(true);
+
+		if (autoland) {
+			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: loiter %.1fs", _mission_item.time_inside);
+
+		} else {
+			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, loiter");
+		}
+		break;
+	}
+
+	case RTL_STATE_LAND: {
 		_mission_item.lat = _navigator->get_home_position()->lat;
 		_mission_item.lon = _navigator->get_home_position()->lon;
 		_mission_item.altitude_is_relative = false;
@@ -237,6 +256,25 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		break;
 	}
 
+	case RTL_STATE_LANDED: {
+		_mission_item.lat = _navigator->get_home_position()->lat;
+		_mission_item.lon = _navigator->get_home_position()->lon;
+		_mission_item.altitude_is_relative = false;
+		_mission_item.altitude = _navigator->get_home_position()->alt;
+		_mission_item.yaw = NAN;
+		_mission_item.loiter_radius = _navigator->get_loiter_radius();
+		_mission_item.loiter_direction = 1;
+		_mission_item.nav_cmd = NAV_CMD_IDLE;
+		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.time_inside = 0.0f;
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = true;
+		_mission_item.origin = ORIGIN_ONBOARD;
+
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, landed");
+		break;
+	}
+
 	default:
 		break;
 	}
@@ -262,18 +300,20 @@ RTL::advance_rtl()
 
 	case RTL_STATE_DESCEND:
 		/* only go to land if autoland is enabled */
-		if (_param_land_delay.get() < 0) {
-			_rtl_state = RTL_STATE_FINISHED;
+		if (_param_land_delay.get() < -DELAY_SIGMA || _param_land_delay.get() > DELAY_SIGMA) {
+			_rtl_state = RTL_STATE_LOITER;
+
 		} else {
 			_rtl_state = RTL_STATE_LAND;
 		}
 		break;
 
-	case RTL_STATE_LAND:
-		_rtl_state = RTL_STATE_FINISHED;
+	case RTL_STATE_LOITER:
+		_rtl_state = RTL_STATE_LAND;
 		break;
 
-	case RTL_STATE_FINISHED:
+	case RTL_STATE_LAND:
+		_rtl_state = RTL_STATE_LANDED;
 		break;
 
 	default:
diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h
index bcccf74548..c4286b391f 100644
--- a/src/modules/navigator/rtl.h
+++ b/src/modules/navigator/rtl.h
@@ -97,8 +97,9 @@ private:
 		RTL_STATE_CLIMB,
 		RTL_STATE_RETURN,
 		RTL_STATE_DESCEND,
+		RTL_STATE_LOITER,
 		RTL_STATE_LAND,
-		RTL_STATE_FINISHED,
+		RTL_STATE_LANDED,
 	} _rtl_state;
 
 	control::BlockParamFloat _param_return_alt;
diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h
index d25db3a772..d9dd61df12 100644
--- a/src/modules/uORB/topics/mission.h
+++ b/src/modules/uORB/topics/mission.h
@@ -50,6 +50,7 @@
 
 /* compatible to mavlink MAV_CMD */
 enum NAV_CMD {
+	NAV_CMD_IDLE=0,
 	NAV_CMD_WAYPOINT=16,
 	NAV_CMD_LOITER_UNLIMITED=17,
 	NAV_CMD_LOITER_TURN_COUNT=18,

From 1bae18377a444279f91b75281ffde5da70cc6086 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Fri, 27 Jun 2014 00:29:00 +0200
Subject: [PATCH 95/97] navigator: is_mission_item_reached() for LOITER items
 fixed

---
 src/modules/navigator/mission_block.cpp | 9 ++-------
 1 file changed, 2 insertions(+), 7 deletions(-)

diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 5e545706d1..8ef7be4496 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -74,15 +74,10 @@ MissionBlock::is_mission_item_reached()
 	}
 
 	/* TODO: count turns */
-#if 0
-	if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
-	     _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
-	     _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
-	    _mission_item.loiter_radius > 0.01f) {
-
+	if ((/*_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||*/
+	     _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED)) {
 		return false;
 	}
-#endif
 
 	hrt_abstime now = hrt_absolute_time();
 

From 52eb49ba0bd1ea5a05845350f1b3c46f0b059a39 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Fri, 27 Jun 2014 11:09:49 +0200
Subject: [PATCH 96/97] navigator: use common "acceptance radius" parameter for
 all modes

---
 src/modules/navigator/mission_block.cpp  |  4 ++--
 src/modules/navigator/navigator.h        |  4 ++--
 src/modules/navigator/navigator_main.cpp |  2 +-
 src/modules/navigator/navigator_params.c |  6 +++---
 src/modules/navigator/rtl.cpp            | 15 +++++++--------
 src/modules/navigator/rtl.h              |  1 -
 src/modules/navigator/rtl_params.c       | 12 ------------
 7 files changed, 15 insertions(+), 29 deletions(-)

diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 8ef7be4496..4f177d76ea 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -100,12 +100,12 @@ MissionBlock::is_mission_item_reached()
 		if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator_priv->get_vstatus()->is_rotary_wing) {
 			/* require only altitude for takeoff for multicopter */
 			if (_navigator_priv->get_global_position()->alt >
-				altitude_amsl - _navigator_priv->get_takeoff_acceptance_radius()) {
+				altitude_amsl - _navigator_priv->get_acceptance_radius()) {
 				_waypoint_position_reached = true;
 			}
 		} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
 			/* for takeoff mission items use the parameter for the takeoff acceptance radius */
-			if (dist >= 0.0f && dist <= _navigator_priv->get_takeoff_acceptance_radius()) {
+			if (dist >= 0.0f && dist <= _navigator_priv->get_acceptance_radius()) {
 				_waypoint_position_reached = true;
 			}
 		} else {
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 4709f71965..184ecd365f 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -115,7 +115,7 @@ public:
 	Geofence&	get_geofence() { return _geofence; }
 	bool		get_can_loiter_at_sp() { return _can_loiter_at_sp; }
 	float		get_loiter_radius() { return _param_loiter_radius.get(); }
-	float		get_takeoff_acceptance_radius() { return _param_takeoff_acceptance_radius.get(); }
+	float		get_acceptance_radius() { return _param_acceptance_radius.get(); }
 	int		get_mavlink_fd() { return _mavlink_fd; }
 
 private:
@@ -165,7 +165,7 @@ private:
 	bool		_update_triplet;		/**< flags if position SP triplet needs to be published */
 
 	control::BlockParamFloat _param_loiter_radius;	/**< loiter radius for fixedwing */
-	control::BlockParamFloat _param_takeoff_acceptance_radius;	/**< acceptance for takeoff */
+	control::BlockParamFloat _param_acceptance_radius;	/**< acceptance for takeoff */
 	/**
 	 * Retrieve global position
 	 */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index e22c4d72df..546602741f 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -123,7 +123,7 @@ Navigator::Navigator() :
 	_rtl(this, "RTL"),
 	_update_triplet(false),
 	_param_loiter_radius(this, "LOITER_RAD"),
-	_param_takeoff_acceptance_radius(this, "TF_ACC_RAD")
+	_param_acceptance_radius(this, "ACC_RAD")
 {
 	/* Create a list of our possible navigation types */
 	_navigation_mode_array[0] = &_mission;
diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
index ca31adecc6..084afe3401 100644
--- a/src/modules/navigator/navigator_params.c
+++ b/src/modules/navigator/navigator_params.c
@@ -55,12 +55,12 @@
 PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
 
 /**
- * Takeoff Acceptance Radius (FW only)
+ * Acceptance Radius
  *
- * Acceptance radius for fixedwing.
+ * Default acceptance radius, overridden by acceptance radius of waypoint if set.
  *
  * @unit meters
  * @min 1.0
  * @group Mission
  */
-PARAM_DEFINE_FLOAT(NAV_TF_ACC_RAD, 25.0f);
+PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 25.0f);
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index b7961a260c..af2ff308ce 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -61,8 +61,7 @@ RTL::RTL(Navigator *navigator, const char *name) :
 	_rtl_state(RTL_STATE_NONE),
 	_param_return_alt(this, "RETURN_ALT"),
 	_param_descend_alt(this, "DESCEND_ALT"),
-	_param_land_delay(this, "LAND_DELAY"),
-	_param_acceptance_radius(this, "ACCEPT_RAD")
+	_param_land_delay(this, "LAND_DELAY")
 {
 	/* load initial params */
 	updateParams();
@@ -147,7 +146,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 		_mission_item.time_inside = 0.0f;
 		_mission_item.pitch_min = 0.0f;
 		_mission_item.autocontinue = true;
@@ -178,7 +177,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 		_mission_item.time_inside = 0.0f;
 		_mission_item.pitch_min = 0.0f;
 		_mission_item.autocontinue = true;
@@ -198,7 +197,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
-		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 		_mission_item.time_inside = 0.0f;
 		_mission_item.pitch_min = 0.0f;
 		_mission_item.autocontinue = false;
@@ -220,7 +219,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;
-		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 		_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
 		_mission_item.pitch_min = 0.0f;
 		_mission_item.autocontinue = autoland;
@@ -246,7 +245,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = NAV_CMD_LAND;
-		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 		_mission_item.time_inside = 0.0f;
 		_mission_item.pitch_min = 0.0f;
 		_mission_item.autocontinue = true;
@@ -265,7 +264,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = NAV_CMD_IDLE;
-		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 		_mission_item.time_inside = 0.0f;
 		_mission_item.pitch_min = 0.0f;
 		_mission_item.autocontinue = true;
diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h
index c4286b391f..90299c3fad 100644
--- a/src/modules/navigator/rtl.h
+++ b/src/modules/navigator/rtl.h
@@ -105,7 +105,6 @@ private:
 	control::BlockParamFloat _param_return_alt;
 	control::BlockParamFloat _param_descend_alt;
 	control::BlockParamFloat _param_land_delay;
-	control::BlockParamFloat _param_acceptance_radius;
 };
 
 #endif
diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c
index 63724f4612..bfe6ce7e18 100644
--- a/src/modules/navigator/rtl_params.c
+++ b/src/modules/navigator/rtl_params.c
@@ -96,15 +96,3 @@ PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20);
  * @group RTL
  */
 PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f);
-
-/**
- * RTL acceptance radius
- *
- * Acceptance radius for waypoints set for RTL
- *
- * @unit meters
- * @min 1
- * @max
- * @group RTL
- */
-PARAM_DEFINE_FLOAT(RTL_ACCEPT_RAD, 25.0f);

From affc312411b7634fa13bab6da8889de90f964ce8 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Fri, 27 Jun 2014 11:34:19 +0200
Subject: [PATCH 97/97] navigator: make MissionBlock subclass of NavigatorMode

---
 src/modules/navigator/loiter.cpp        |  3 +-
 src/modules/navigator/loiter.h          |  2 +-
 src/modules/navigator/mission.cpp       |  3 +-
 src/modules/navigator/mission.h         |  2 +-
 src/modules/navigator/mission_block.cpp | 44 ++++++++++++-------------
 src/modules/navigator/mission_block.h   | 11 +++----
 src/modules/navigator/rtl.cpp           |  3 +-
 src/modules/navigator/rtl.h             |  2 +-
 8 files changed, 32 insertions(+), 38 deletions(-)

diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp
index f5eb1e7a5a..542483fb15 100644
--- a/src/modules/navigator/loiter.cpp
+++ b/src/modules/navigator/loiter.cpp
@@ -53,8 +53,7 @@
 #include "loiter.h"
 
 Loiter::Loiter(Navigator *navigator, const char *name) :
-	NavigatorMode(navigator, name),
-	MissionBlock(navigator)
+	MissionBlock(navigator, name)
 {
 	/* load initial params */
 	updateParams();
diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h
index 685ae61410..65ff5c31ee 100644
--- a/src/modules/navigator/loiter.h
+++ b/src/modules/navigator/loiter.h
@@ -47,7 +47,7 @@
 #include "navigator_mode.h"
 #include "mission_block.h"
 
-class Loiter : public NavigatorMode, MissionBlock
+class Loiter : public MissionBlock
 {
 public:
 	/**
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index c09f617446..72255103bd 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -58,8 +58,7 @@
 #include "mission.h"
 
 Mission::Mission(Navigator *navigator, const char *name) :
-	NavigatorMode(navigator, name),
-	MissionBlock(navigator),
+	MissionBlock(navigator, name),
 	_param_onboard_enabled(this, "ONBOARD_EN"),
 	_onboard_mission({0}),
 	_offboard_mission({0}),
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index 842c8c9ee8..6e47619467 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -62,7 +62,7 @@
 
 class Navigator;
 
-class Mission : public NavigatorMode, MissionBlock
+class Mission : public MissionBlock
 {
 public:
 	/**
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 4f177d76ea..9b8d3d9c70 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -52,13 +52,13 @@
 #include "mission_block.h"
 
 
-MissionBlock::MissionBlock(Navigator *navigator) :
+MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
+	NavigatorMode(navigator, name),
 	_waypoint_position_reached(false),
 	_waypoint_yaw_reached(false),
 	_time_first_inside_orbit(0),
 	_mission_item({0}),
-	_mission_item_valid(false),
-	_navigator_priv(navigator)
+	_mission_item_valid(false)
 {
 }
 
@@ -70,7 +70,7 @@ bool
 MissionBlock::is_mission_item_reached()
 {
 	if (_mission_item.nav_cmd == NAV_CMD_LAND) {
-		return _navigator_priv->get_vstatus()->condition_landed;
+		return _navigator->get_vstatus()->condition_landed;
 	}
 
 	/* TODO: count turns */
@@ -88,24 +88,24 @@ MissionBlock::is_mission_item_reached()
 		float dist_z = -1.0f;
 
 		float altitude_amsl = _mission_item.altitude_is_relative
-				      ? _mission_item.altitude + _navigator_priv->get_home_position()->alt
+				      ? _mission_item.altitude + _navigator->get_home_position()->alt
 			              : _mission_item.altitude;
 
 		dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl,
-				                          _navigator_priv->get_global_position()->lat,
-							  _navigator_priv->get_global_position()->lon,
-							  _navigator_priv->get_global_position()->alt,
+				                          _navigator->get_global_position()->lat,
+							  _navigator->get_global_position()->lon,
+							  _navigator->get_global_position()->alt,
 				&dist_xy, &dist_z);
 
-		if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator_priv->get_vstatus()->is_rotary_wing) {
+		if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator->get_vstatus()->is_rotary_wing) {
 			/* require only altitude for takeoff for multicopter */
-			if (_navigator_priv->get_global_position()->alt >
-				altitude_amsl - _navigator_priv->get_acceptance_radius()) {
+			if (_navigator->get_global_position()->alt >
+				altitude_amsl - _navigator->get_acceptance_radius()) {
 				_waypoint_position_reached = true;
 			}
 		} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
 			/* for takeoff mission items use the parameter for the takeoff acceptance radius */
-			if (dist >= 0.0f && dist <= _navigator_priv->get_acceptance_radius()) {
+			if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) {
 				_waypoint_position_reached = true;
 			}
 		} else {
@@ -119,10 +119,10 @@ MissionBlock::is_mission_item_reached()
 	if (_waypoint_position_reached && !_waypoint_yaw_reached) {
 
 		/* TODO: removed takeoff, why? */
-		if (_navigator_priv->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) {
+		if (_navigator->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) {
 
 			/* check yaw if defined only for rotary wing except takeoff */
-			float yaw_err = _wrap_pi(_mission_item.yaw - _navigator_priv->get_global_position()->yaw);
+			float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw);
 
 			if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */
 				_waypoint_yaw_reached = true;
@@ -167,7 +167,7 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 	sp->valid = true;
 	sp->lat = item->lat;
 	sp->lon = item->lon;
-	sp->alt = item->altitude_is_relative ? item->altitude + _navigator_priv->get_home_position()->alt : item->altitude;
+	sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude;
 	sp->yaw = item->yaw;
 	sp->loiter_radius = item->loiter_radius;
 	sp->loiter_direction = item->loiter_direction;
@@ -211,25 +211,25 @@ bool
 MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet)
 {
     /* don't change setpoint if 'can_loiter_at_sp' flag set */
-	if (!(_navigator_priv->get_can_loiter_at_sp() && pos_sp_triplet->current.valid)) {
+	if (!(_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid)) {
 		/* use current position */
-		pos_sp_triplet->current.lat = _navigator_priv->get_global_position()->lat;
-		pos_sp_triplet->current.lon = _navigator_priv->get_global_position()->lon;
-		pos_sp_triplet->current.alt = _navigator_priv->get_global_position()->alt;
+		pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
+		pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
+		pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
 		pos_sp_triplet->current.yaw = NAN;	/* NAN means to use current yaw */
 
-	    _navigator_priv->set_can_loiter_at_sp(true);
+	    _navigator->set_can_loiter_at_sp(true);
 	}
 
     if (pos_sp_triplet->current.type != SETPOINT_TYPE_LOITER
-            || pos_sp_triplet->current.loiter_radius != _navigator_priv->get_loiter_radius()
+            || pos_sp_triplet->current.loiter_radius != _navigator->get_loiter_radius()
             || pos_sp_triplet->current.loiter_direction != 1
             || pos_sp_triplet->previous.valid
             || !pos_sp_triplet->current.valid
             || pos_sp_triplet->next.valid) {
         /* position setpoint triplet should be updated */
         pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER;
-        pos_sp_triplet->current.loiter_radius = _navigator_priv->get_loiter_radius();
+        pos_sp_triplet->current.loiter_radius = _navigator->get_loiter_radius();
         pos_sp_triplet->current.loiter_direction = 1;
 
         pos_sp_triplet->previous.valid = false;
diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h
index 8726964fa3..f990027522 100644
--- a/src/modules/navigator/mission_block.h
+++ b/src/modules/navigator/mission_block.h
@@ -47,17 +47,17 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 
+#include "navigator_mode.h"
+
 class Navigator;
 
-class MissionBlock
+class MissionBlock : public NavigatorMode
 {
 public:
 	/**
 	 * Constructor
-	 *
-	 * @param pointer to parent class
 	 */
-	MissionBlock(Navigator *navigator);
+	MissionBlock(Navigator *navigator, const char *name);
 
 	/**
 	 * Destructor
@@ -101,9 +101,6 @@ public:
 
 	mission_item_s _mission_item;
 	bool _mission_item_valid;
-
-private:
-	Navigator *_navigator_priv;
 };
 
 #endif
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index af2ff308ce..043f773a43 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -56,8 +56,7 @@
 #define DELAY_SIGMA	0.01f
 
 RTL::RTL(Navigator *navigator, const char *name) :
-	NavigatorMode(navigator, name),
-	MissionBlock(navigator),
+	MissionBlock(navigator, name),
 	_rtl_state(RTL_STATE_NONE),
 	_param_return_alt(this, "RETURN_ALT"),
 	_param_descend_alt(this, "DESCEND_ALT"),
diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h
index 90299c3fad..b4b729e898 100644
--- a/src/modules/navigator/rtl.h
+++ b/src/modules/navigator/rtl.h
@@ -54,7 +54,7 @@
 
 class Navigator;
 
-class RTL : public NavigatorMode, MissionBlock
+class RTL : public MissionBlock
 {
 public:
 	/**